基于非線性互補(bǔ)濾波算法的四旋翼飛行器姿態(tài)信息融合處理

孫菁宇 高國(guó)偉 潘宏生 毛瑞燕

1.北京信息科技大學(xué)傳感器重點(diǎn)實(shí)驗(yàn)室，北京 100192；2.北京國(guó)科艦航傳感技術(shù)有限公司，北京 100101

一、引言

近年來(lái)，隨著新材料、微機(jī)電(MEMS)、微慣導(dǎo)(MIMU)以及飛行控制等技術(shù)的逐漸成熟及個(gè)人航拍和航模運(yùn)動(dòng)的興起，低成本飛行器迅速進(jìn)入大眾視線，無(wú)人機(jī)市場(chǎng)呈現(xiàn)出爆炸式增長(zhǎng)，其中尤以四旋翼飛行器最為主流[1]。

四旋翼在布局形式上屬于非共軸式碟形飛行器，四個(gè)旋槳按照不同的方向旋轉(zhuǎn)不僅抵消了反扭力矩，并且可以通過(guò)調(diào)節(jié)四個(gè)旋翼的轉(zhuǎn)速實(shí)現(xiàn)各個(gè)方向的飛行控制。其結(jié)構(gòu)的簡(jiǎn)單可靠，體積小、重量輕，成本低，機(jī)動(dòng)性強(qiáng)等特點(diǎn)使其在許多領(lǐng)域發(fā)揮出越來(lái)越不可替代的作用[2-3]，因此，研究其基本原理并在現(xiàn)有的功能基礎(chǔ)上進(jìn)行改進(jìn)和優(yōu)化具有重要意義。

姿態(tài)的準(zhǔn)確獲取是飛行器實(shí)現(xiàn)穩(wěn)定飛行的前提，而姿態(tài)解算的精度和速度將直接影響到飛控算法的穩(wěn)定性和可靠性。隨著人們對(duì)姿態(tài)解算的精度要求以及機(jī)載硬件的計(jì)算效率越來(lái)越高，姿態(tài)數(shù)據(jù)融合的精細(xì)化仍是當(dāng)下研究的一個(gè)熱點(diǎn)。在成本控制上，由于在消費(fèi)級(jí)無(wú)人機(jī)領(lǐng)域高精度慣性導(dǎo)航器件不具有普遍使用性，目前常用的MEMS慣導(dǎo)器件中，陀螺儀會(huì)帶來(lái)零點(diǎn)漂移和累計(jì)積分誤差，加速度計(jì)則容易受到噪聲或震動(dòng)的干擾[4]。

針對(duì)這一問(wèn)題，文獻(xiàn)[5-7]采用Kalman濾波對(duì)測(cè)量數(shù)據(jù)進(jìn)行融合[5-7]；為解決系統(tǒng)的非線性問(wèn)題，文獻(xiàn)[8-9]采用擴(kuò)展卡爾曼濾波EKF算法實(shí)現(xiàn)姿態(tài)數(shù)據(jù)融合[8-9]。然而EKF難以建立穩(wěn)定可靠的更新方程，而且計(jì)算量較大，對(duì)處理器要求很高，過(guò)長(zhǎng)的計(jì)算時(shí)間將導(dǎo)致姿態(tài)角更新時(shí)間過(guò)長(zhǎng)，不適合微小型四旋翼飛行器應(yīng)用平臺(tái)?；パa(bǔ)濾波算法簡(jiǎn)單可靠，對(duì)傳感器的精度要求較低，是一種從頻域角度對(duì)數(shù)據(jù)進(jìn)行處理的姿態(tài)融合算法。文獻(xiàn)[10-12]分別給出了歐拉角、方向余弦形式下的互補(bǔ)濾波算法[10-12]，雖然能滿(mǎn)足飛行器飛行控制的要求，但由于會(huì)出現(xiàn)歐拉角法存在奇點(diǎn)、方向余弦法計(jì)算量大等問(wèn)題，在噪聲較大時(shí)，難以取得較好的濾波效果，使得姿態(tài)解算誤差較大。

因此，本文在互補(bǔ)濾波的基礎(chǔ)上，通過(guò)對(duì)方向余弦矩陣做重規(guī)范化處理[13]，使方向余弦矩陣保持正交，修正數(shù)據(jù)誤差積累；應(yīng)用四元數(shù)方程進(jìn)行姿態(tài)更新，并增加一個(gè)PI反饋環(huán)節(jié)實(shí)時(shí)修正誤差，構(gòu)建了一種改進(jìn)型互補(bǔ)濾波算法對(duì)姿態(tài)數(shù)據(jù)進(jìn)行融合處理，實(shí)現(xiàn)姿態(tài)信息的準(zhǔn)確解算，滿(mǎn)足了四旋翼飛行器飛行控制要求。

二、姿態(tài)的描述

1、坐標(biāo)系的定義

為了方便描述四旋翼飛行器的姿態(tài)信息，需建立相應(yīng)的三維空間坐標(biāo)系，分別為：地理坐標(biāo)系On，原點(diǎn)為慣性平臺(tái)在大地上的投影；機(jī)體坐標(biāo)系Ob，原點(diǎn)為機(jī)體的重心。地理坐標(biāo)系采用“東-北-天”坐標(biāo)指向[14]，On系和Ob系的角度旋轉(zhuǎn)變換如圖1所示：XnYnZn繞Zn軸旋轉(zhuǎn)ψ得到X1Y1Z1；然后繞X1軸旋轉(zhuǎn)θ角得到X2Y2Z2；最后繞Y2旋轉(zhuǎn)γ得到機(jī)體坐標(biāo)系XbYbZb。

2、姿態(tài)的數(shù)學(xué)表示

飛行器飛行過(guò)程中，機(jī)體坐標(biāo)系經(jīng)過(guò)頻繁的旋轉(zhuǎn)變換生成新的機(jī)體坐標(biāo)系，旋轉(zhuǎn)一般常用歐拉角、方向余弦矩陣或四元數(shù)來(lái)表示。歐拉角表示最為直觀但會(huì)存在萬(wàn)向節(jié)死鎖現(xiàn)象，不能實(shí)現(xiàn)對(duì)全部姿態(tài)的表示，方向余弦矩陣則有效規(guī)避了這個(gè)問(wèn)題，然而方向余弦姿態(tài)矩陣有9個(gè)元素，所以解算時(shí)，實(shí)際需要計(jì)算9個(gè)聯(lián)合微分方程[15]，計(jì)算量很大。四元數(shù)法是一種全局的姿態(tài)描述方法，即避免了萬(wàn)向節(jié)死鎖現(xiàn)象，而且計(jì)算量小，速度快。由于姿態(tài)控制算法的輸入?yún)?shù)必須是歐拉角，因此，本文采用四元數(shù)來(lái)保存機(jī)體的姿態(tài)，用矩陣處理向量間的變換，最后轉(zhuǎn)化為歐拉角做進(jìn)一步的數(shù)據(jù)處理。圖1的三次旋轉(zhuǎn)過(guò)程可用歐拉角描述的方向余弦矩陣表示為：

定義四元數(shù)：

于是式（1）可用四元數(shù)表示為：

四元數(shù)的更新可由一階龍格庫(kù)塔法求解四元數(shù)微分方程得到：

聯(lián)立四元數(shù)矩陣和方向余弦矩陣，可得用四元數(shù)表示的姿態(tài)角：

三、互補(bǔ)濾波算法的原理及實(shí)現(xiàn)過(guò)程

加速度計(jì)靜態(tài)特性非常好，能夠長(zhǎng)期穩(wěn)定的輸出測(cè)量數(shù)據(jù)，但動(dòng)態(tài)特性較差，即在低頻段動(dòng)態(tài)響應(yīng)特性好，在高頻段動(dòng)態(tài)響應(yīng)特性差；陀螺儀雖然動(dòng)態(tài)特性較好，在短期內(nèi)可提供高動(dòng)態(tài)的姿態(tài)數(shù)據(jù)，但是存在溫度漂移和零點(diǎn)漂移，在低頻段動(dòng)態(tài)響應(yīng)特性差而高頻段動(dòng)態(tài)響應(yīng)特性好?；パa(bǔ)濾波就是利用兩者在頻域上的動(dòng)態(tài)響應(yīng)特性的互補(bǔ)關(guān)系，對(duì)姿態(tài)數(shù)據(jù)進(jìn)行融合，進(jìn)而提高系統(tǒng)的測(cè)量精度和動(dòng)態(tài)性能。其原理圖如圖2所示：

設(shè)θ為姿態(tài)角，可得融合后的姿態(tài)角估計(jì)值為：

其中，ωg—陀螺儀測(cè)量角速度值；

θa—加速度計(jì)測(cè)量角度值；

高通濾波器很好的抑制了陀螺儀的低頻漂移，而低通濾波器很好的抑制了加速度計(jì)的噪聲[16]。

式（6）經(jīng)過(guò)反拉普拉斯變換可得到其時(shí)域形式為：

根據(jù)上式并通過(guò)對(duì)kp值的調(diào)整，最終實(shí)現(xiàn)對(duì)陀螺儀和加速度計(jì)得到的姿態(tài)數(shù)據(jù)的互補(bǔ)濾波處理。

四、互補(bǔ)濾波算法的改進(jìn)

1、對(duì)方向余弦矩陣的進(jìn)一步探究

方向余弦矩陣描述的是一個(gè)坐標(biāo)系相對(duì)于另一個(gè)坐標(biāo)系的方位信息，方向余弦矩陣的列是其中一個(gè)坐標(biāo)系的單位向量在另一個(gè)坐標(biāo)系中的投影，坐標(biāo)系中的向量可以通過(guò)左乘轉(zhuǎn)換到另外一個(gè)坐標(biāo)系中，將另外一個(gè)坐標(biāo)系中的向量左乘這個(gè)旋轉(zhuǎn)矩陣的轉(zhuǎn)置矩陣可以完成相反方向的變換。式（1）中，矩陣的三列是用來(lái)將機(jī)體的三個(gè)向量轉(zhuǎn)換到地球參考坐標(biāo)系。矩陣三行是用來(lái)將地球參考坐標(biāo)系的三個(gè)向量轉(zhuǎn)換到機(jī)體參考系。根據(jù)相乘的順序，依次疊加兩個(gè)旋轉(zhuǎn)矩陣，所得到的結(jié)果與這兩個(gè)矩陣相乘所得到的矩陣代表的旋轉(zhuǎn)是相同的。

陀螺儀是四旋翼飛行器方位信息的主要來(lái)源，通過(guò)聯(lián)合飛行器的非線性微分動(dòng)力學(xué)方程，將飛行器的方位對(duì)旋轉(zhuǎn)速率的時(shí)間變化率及當(dāng)前飛行器的位置結(jié)合起來(lái)。準(zhǔn)確的陀螺儀姿態(tài)信息進(jìn)行準(zhǔn)確的符號(hào)積分可得到準(zhǔn)確的旋轉(zhuǎn)矩陣，然而，數(shù)值積分會(huì)引進(jìn)數(shù)值誤差，無(wú)法得到與符號(hào)積分相同的結(jié)果，即使有了準(zhǔn)確的陀螺信號(hào)依然會(huì)引入下面這兩類(lèi)數(shù)值誤差：

（1）積分誤差

數(shù)值積分采用一定的時(shí)間步長(zhǎng)和一定采樣率的數(shù)據(jù)，根據(jù)所使用的數(shù)值積分方法對(duì)采樣數(shù)據(jù)做特定的假設(shè)，然而，一般采用的方法假設(shè)每個(gè)時(shí)間步長(zhǎng)內(nèi)的旋轉(zhuǎn)速度是恒定不變的，因而會(huì)引入正比于旋轉(zhuǎn)加速度的誤差。

（2）量化誤差

無(wú)論是用什么方法表示測(cè)量值這些表示都是有限的，從模數(shù)轉(zhuǎn)換開(kāi)始到執(zhí)行的工程中，都會(huì)存在無(wú)法保留計(jì)算結(jié)果所有位數(shù)的計(jì)算，因此量化誤差會(huì)一直累積。

旋轉(zhuǎn)矩陣的基本特征就是矩陣的正交性，也就是說(shuō)如果兩個(gè)向量在某個(gè)坐標(biāo)系中是正交的，那么它們不管旋轉(zhuǎn)變換到任何坐標(biāo)系中都還是正交的，并且不同參考系下同一向量的模值相等。然而數(shù)值誤差累積會(huì)破壞這一特征。比如旋轉(zhuǎn)矩陣的行列都是單位向量，它們的模值都等于1，但是數(shù)值誤差會(huì)導(dǎo)致它們的長(zhǎng)度變小或變大，長(zhǎng)時(shí)間的累積下去，它們的長(zhǎng)度將會(huì)變?yōu)?或∞。另一方面，旋轉(zhuǎn)矩陣的行列本應(yīng)該互相垂直，但是數(shù)值誤差的累積會(huì)改變這一狀態(tài)，如圖3所示。

2、互補(bǔ)濾波算法的改進(jìn)

由上文的分析可知，積分產(chǎn)生的數(shù)值誤差的累積會(huì)逐漸破壞方向余弦矩陣所必須滿(mǎn)足的正交約束，所幸數(shù)值誤差的累積比較緩慢，因此，可以將方向余弦矩陣做重規(guī)范化處理，使其重新滿(mǎn)足矩陣正交性；向量間的誤差，可以用向量的叉積來(lái)表示，利用叉積誤差即可修正陀螺儀產(chǎn)生的積分誤差。

根據(jù)互補(bǔ)濾波算法的原理可知：互補(bǔ)濾波算法最后通過(guò)調(diào)節(jié)陀螺儀和加速度計(jì)兩者的權(quán)重來(lái)進(jìn)行數(shù)據(jù)的融合，但介于四旋翼飛行器飛行過(guò)程中的高動(dòng)態(tài)性，固定的參數(shù)必然不能得到在所有情況下的最優(yōu)估計(jì)值，因此，本文在互補(bǔ)濾波原理的基礎(chǔ)上，增加了一個(gè)PI反饋控制環(huán)節(jié)，不僅提高了系統(tǒng)的型別，使低通阻帶的衰減速度更快，另外，針對(duì)不同的運(yùn)動(dòng)狀態(tài)下，動(dòng)態(tài)調(diào)整補(bǔ)償系數(shù)，即：正常運(yùn)動(dòng)狀態(tài)下，適當(dāng)加大加速度計(jì)的數(shù)值權(quán)重；當(dāng)飛行器姿態(tài)做高動(dòng)態(tài)變化時(shí)，適當(dāng)降低加速度計(jì)的權(quán)重，此時(shí)主要依靠陀螺儀來(lái)進(jìn)行姿態(tài)的測(cè)量，使其能夠根據(jù)四旋翼飛行器的不同運(yùn)動(dòng)狀態(tài)做出合適的參數(shù)選擇。改進(jìn)后的算法如圖4所示。

算法的具體實(shí)現(xiàn)如下：

（1）首先將加速度計(jì)測(cè)得的三維向量規(guī)范化處理，即：

（2）根據(jù)公式（3）中的方向余弦矩陣的四元數(shù)表示形式，將機(jī)體坐標(biāo)系轉(zhuǎn)換到地理坐標(biāo)系下，得到機(jī)體坐標(biāo)系下重力加速度的三位向量；

進(jìn)而修正陀螺的零偏誤差；

（4）代入四元數(shù)更新方程（4）中，得出角速度微分四元數(shù)；

（5）將更新后的四元數(shù)積分規(guī)范化處理，并根據(jù)式（5）求出姿態(tài)角估計(jì)值。

部分程序?qū)崿F(xiàn)如下：

五、實(shí)驗(yàn)測(cè)試

為驗(yàn)證改進(jìn)后算法的正確性和可行性，搭建了以STM32為核心主控芯片，MPU6050為慣性測(cè)量單元的四旋翼飛行器硬件平臺(tái)對(duì)算法進(jìn)行測(cè)試，其中MPU6050以軟件I2C的形式與主控芯片相連，實(shí)時(shí)姿態(tài)信息和解算后的姿態(tài)角信息通過(guò)串口通信形式由計(jì)算機(jī)端接收。為更加直觀的觀察算法效果，編寫(xiě)了基于經(jīng)典互補(bǔ)濾波算法的一階互補(bǔ)濾波算法、二階互補(bǔ)濾波算法，將直接轉(zhuǎn)換、一階互補(bǔ)濾波算法解算、二階互補(bǔ)濾波算法解算和改進(jìn)后的互補(bǔ)濾波算法解算四種方式解算出的姿態(tài)角在靜態(tài)環(huán)境下進(jìn)行測(cè)試對(duì)比，測(cè)試結(jié)果如圖5所示。

由圖5可以看出，由加速度計(jì)直接解算出的角度存在著很大的噪聲干擾，一階和二階互補(bǔ)濾波算法解算之后效果好了很多，濾除了大量的尖刺，但波動(dòng)依然較大；陀螺儀解算出的角度經(jīng)測(cè)試在一分鐘之內(nèi)漂移了將近1°；而改進(jìn)后的互補(bǔ)濾波算法表現(xiàn)出了非常好的效果，靜態(tài)估計(jì)誤差基本控制在了0.05°以?xún)?nèi)。

為驗(yàn)證算法在動(dòng)態(tài)環(huán)境下有效性，在四旋翼飛行器硬件平臺(tái)上分別對(duì)幾種解算方式做動(dòng)態(tài)測(cè)試，測(cè)試結(jié)果對(duì)比如圖6所示。

由圖6可以看出，經(jīng)典互補(bǔ)濾波算法和改進(jìn)后的互補(bǔ)濾波算法都能有效的濾除一些高頻信號(hào)干擾量，一階互補(bǔ)濾波和二階互補(bǔ)濾波都有不同程度的滯后，二階互補(bǔ)濾波相比一階互補(bǔ)濾波曲線更為平滑，然而曲線也相對(duì)滯后，特別是在四旋翼飛行器快速運(yùn)動(dòng)時(shí)，二階互補(bǔ)濾波出現(xiàn)了嚴(yán)重的滯后現(xiàn)象，而改進(jìn)后的互補(bǔ)濾波算法不僅姿態(tài)測(cè)量曲線更為平滑，并且因?yàn)榧尤肓祟A(yù)測(cè)校正算法，表現(xiàn)出了較傳統(tǒng)互補(bǔ)濾波更好的實(shí)時(shí)跟隨性，解算效果良好。

六、結(jié)束語(yǔ)

姿態(tài)信息的精確獲取是四旋翼飛行器平穩(wěn)、安全飛行的先決條件，本文研究了四旋翼飛行器姿態(tài)信息的融合處理方法，在結(jié)合傳統(tǒng)互補(bǔ)濾波融合的基礎(chǔ)上，采用一種改進(jìn)型互補(bǔ)濾波融合思路，并編寫(xiě)了不同的融合算法程序作為濾波對(duì)比，搭建了硬件測(cè)試平臺(tái)在靜態(tài)和動(dòng)態(tài)環(huán)境下對(duì)幾種不同的姿態(tài)解算方法進(jìn)行測(cè)試對(duì)比。對(duì)比結(jié)果表明，改進(jìn)后的互補(bǔ)濾波融合算法靜態(tài)誤差小，動(dòng)態(tài)曲線過(guò)渡平滑，實(shí)時(shí)跟蹤效果較為理想。