基于非線性互補濾波算法的四旋翼飛行器姿態信息融合處理

孫菁宇 高國偉 潘宏生 毛瑞燕

1.北京信息科技大學傳感器重點實驗室，北京 100192；2.北京國科艦航傳感技術有限公司，北京 100101

一、引言

近年來，隨著新材料、微機電(MEMS)、微慣導(MIMU)以及飛行控制等技術的逐漸成熟及個人航拍和航模運動的興起，低成本飛行器迅速進入大眾視線，無人機市場呈現出爆炸式增長，其中尤以四旋翼飛行器最為主流[1]。

四旋翼在布局形式上屬于非共軸式碟形飛行器，四個旋槳按照不同的方向旋轉不僅抵消了反扭力矩，并且可以通過調節四個旋翼的轉速實現各個方向的飛行控制。其結構的簡單可靠，體積小、重量輕，成本低，機動性強等特點使其在許多領域發揮出越來越不可替代的作用[2-3]，因此，研究其基本原理并在現有的功能基礎上進行改進和優化具有重要意義。

姿態的準確獲取是飛行器實現穩定飛行的前提，而姿態解算的精度和速度將直接影響到飛控算法的穩定性和可靠性。隨著人們對姿態解算的精度要求以及機載硬件的計算效率越來越高，姿態數據融合的精細化仍是當下研究的一個熱點。在成本控制上，由于在消費級無人機領域高精度慣性導航器件不具有普遍使用性，目前常用的MEMS慣導器件中，陀螺儀會帶來零點漂移和累計積分誤差，加速度計則容易受到噪聲或震動的干擾[4]。

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

因此，本文在互補濾波的基礎上，通過對方向余弦矩陣做重規范化處理[13]，使方向余弦矩陣保持正交，修正數據誤差積累；應用四元數方程進行姿態更新，并增加一個PI反饋環節實時修正誤差，構建了一種改進型互補濾波算法對姿態數據進行融合處理，實現姿態信息的準確解算，滿足了四旋翼飛行器飛行控制要求。

二、姿態的描述

1、坐標系的定義

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

2、姿態的數學表示

飛行器飛行過程中，機體坐標系經過頻繁的旋轉變換生成新的機體坐標系，旋轉一般常用歐拉角、方向余弦矩陣或四元數來表示。歐拉角表示最為直觀但會存在萬向節死鎖現象，不能實現對全部姿態的表示，方向余弦矩陣則有效規避了這個問題，然而方向余弦姿態矩陣有9個元素，所以解算時，實際需要計算9個聯合微分方程[15]，計算量很大。四元數法是一種全局的姿態描述方法，即避免了萬向節死鎖現象，而且計算量小，速度快。由于姿態控制算法的輸入參數必須是歐拉角，因此，本文采用四元數來保存機體的姿態，用矩陣處理向量間的變換，最后轉化為歐拉角做進一步的數據處理。圖1的三次旋轉過程可用歐拉角描述的方向余弦矩陣表示為：

定義四元數：

于是式（1）可用四元數表示為：

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

聯立四元數矩陣和方向余弦矩陣，可得用四元數表示的姿態角：

三、互補濾波算法的原理及實現過程

加速度計靜態特性非常好，能夠長期穩定的輸出測量數據，但動態特性較差，即在低頻段動態響應特性好，在高頻段動態響應特性差；陀螺儀雖然動態特性較好，在短期內可提供高動態的姿態數據，但是存在溫度漂移和零點漂移，在低頻段動態響應特性差而高頻段動態響應特性好。互補濾波就是利用兩者在頻域上的動態響應特性的互補關系，對姿態數據進行融合，進而提高系統的測量精度和動態性能。其原理圖如圖2所示：

設θ為姿態角，可得融合后的姿態角估計值為：

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

θa—加速度計測量角度值；

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

式（6）經過反拉普拉斯變換可得到其時域形式為：

根據上式并通過對kp值的調整，最終實現對陀螺儀和加速度計得到的姿態數據的互補濾波處理。

四、互補濾波算法的改進

1、對方向余弦矩陣的進一步探究

方向余弦矩陣描述的是一個坐標系相對于另一個坐標系的方位信息，方向余弦矩陣的列是其中一個坐標系的單位向量在另一個坐標系中的投影，坐標系中的向量可以通過左乘轉換到另外一個坐標系中，將另外一個坐標系中的向量左乘這個旋轉矩陣的轉置矩陣可以完成相反方向的變換。式（1）中，矩陣的三列是用來將機體的三個向量轉換到地球參考坐標系。矩陣三行是用來將地球參考坐標系的三個向量轉換到機體參考系。根據相乘的順序，依次疊加兩個旋轉矩陣，所得到的結果與這兩個矩陣相乘所得到的矩陣代表的旋轉是相同的。

陀螺儀是四旋翼飛行器方位信息的主要來源，通過聯合飛行器的非線性微分動力學方程，將飛行器的方位對旋轉速率的時間變化率及當前飛行器的位置結合起來。準確的陀螺儀姿態信息進行準確的符號積分可得到準確的旋轉矩陣，然而，數值積分會引進數值誤差，無法得到與符號積分相同的結果，即使有了準確的陀螺信號依然會引入下面這兩類數值誤差：

（1）積分誤差

數值積分采用一定的時間步長和一定采樣率的數據，根據所使用的數值積分方法對采樣數據做特定的假設，然而，一般采用的方法假設每個時間步長內的旋轉速度是恒定不變的，因而會引入正比于旋轉加速度的誤差。

（2）量化誤差

無論是用什么方法表示測量值這些表示都是有限的，從模數轉換開始到執行的工程中，都會存在無法保留計算結果所有位數的計算，因此量化誤差會一直累積。

旋轉矩陣的基本特征就是矩陣的正交性，也就是說如果兩個向量在某個坐標系中是正交的，那么它們不管旋轉變換到任何坐標系中都還是正交的，并且不同參考系下同一向量的模值相等。然而數值誤差累積會破壞這一特征。比如旋轉矩陣的行列都是單位向量，它們的模值都等于1，但是數值誤差會導致它們的長度變小或變大，長時間的累積下去，它們的長度將會變為0或∞。另一方面，旋轉矩陣的行列本應該互相垂直，但是數值誤差的累積會改變這一狀態，如圖3所示。

2、互補濾波算法的改進

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

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

算法的具體實現如下：

（1）首先將加速度計測得的三維向量規范化處理，即：

（2）根據公式（3）中的方向余弦矩陣的四元數表示形式，將機體坐標系轉換到地理坐標系下，得到機體坐標系下重力加速度的三位向量；

進而修正陀螺的零偏誤差；

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

（5）將更新后的四元數積分規范化處理，并根據式（5）求出姿態角估計值。

部分程序實現如下：

五、實驗測試

為驗證改進后算法的正確性和可行性，搭建了以STM32為核心主控芯片，MPU6050為慣性測量單元的四旋翼飛行器硬件平臺對算法進行測試，其中MPU6050以軟件I2C的形式與主控芯片相連，實時姿態信息和解算后的姿態角信息通過串口通信形式由計算機端接收。為更加直觀的觀察算法效果，編寫了基于經典互補濾波算法的一階互補濾波算法、二階互補濾波算法，將直接轉換、一階互補濾波算法解算、二階互補濾波算法解算和改進后的互補濾波算法解算四種方式解算出的姿態角在靜態環境下進行測試對比，測試結果如圖5所示。

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

為驗證算法在動態環境下有效性，在四旋翼飛行器硬件平臺上分別對幾種解算方式做動態測試，測試結果對比如圖6所示。

由圖6可以看出，經典互補濾波算法和改進后的互補濾波算法都能有效的濾除一些高頻信號干擾量，一階互補濾波和二階互補濾波都有不同程度的滯后，二階互補濾波相比一階互補濾波曲線更為平滑，然而曲線也相對滯后，特別是在四旋翼飛行器快速運動時，二階互補濾波出現了嚴重的滯后現象，而改進后的互補濾波算法不僅姿態測量曲線更為平滑，并且因為加入了預測校正算法，表現出了較傳統互補濾波更好的實時跟隨性，解算效果良好。

六、結束語

姿態信息的精確獲取是四旋翼飛行器平穩、安全飛行的先決條件，本文研究了四旋翼飛行器姿態信息的融合處理方法，在結合傳統互補濾波融合的基礎上，采用一種改進型互補濾波融合思路，并編寫了不同的融合算法程序作為濾波對比，搭建了硬件測試平臺在靜態和動態環境下對幾種不同的姿態解算方法進行測試對比。對比結果表明，改進后的互補濾波融合算法靜態誤差小，動態曲線過渡平滑，實時跟蹤效果較為理想。