基于機動加速度補償的高動態無人機MEMS姿態實時解算

郭忠誠，管練武，吉彩妮

中航貴州飛機有限責任公司

針對高動態無人機機動加速和發動機高頻振動導致MEMS姿態測量精度降低問題，本文研究MEMS加速度計和陀螺儀組成的低成本小型姿態測量系統，提出一種自適應Mahony互補濾波算法，以補償機動加速度及動態整定互補濾波器PI參數，對傳感器數據進行融合、補償和修正。該方法利用加速度計三軸輸出模值，以及對水平計算加速度與當地重力加速度的比較，判斷機動加速度并進行補償，然后通過自適應Mahony互補濾波算法完成姿態實時解算，最后經無人機試驗驗證得出，所采用的算法收斂性和平滑性較好，降低了MEMS的誤差影響，可對無人機姿態進行有效跟蹤和測量，為低成本小型姿態測量系統的開發和應用提供實用的方法。

姿態角是用來描述無人機空間指向的重要信息，在導航應用中對姿態信息進行分析顯得至關重要。姿態測量系統（Attitude Measurement System，AMS）利用陀螺儀、加速度計等慣性傳感器和微處理器等測量飛行器的姿態角。一般而言，在高動態及復雜環境下，姿態測量系統采用機械陀螺儀、光纖陀螺儀或激光陀螺儀獲取姿態信息，這種姿態測量系統精度和穩定性高，但成本高、體積大、功耗高且維護復雜，不能滿足現代無人機姿態測量系統在低成本、小型化、低功耗、免維護等方面的要求。近年來，隨著微機電系統（Micro-Electro-Mechanical System，MEMS）技術的發展，MEMS慣性測量單元（Inertial Measurement Unit，IMU）在控制領域的應用日趨廣泛，尤其是對無人機系統的姿態測量。因此，近年來基于MEMS慣性測量單元的姿態測量系統常用于實時測量和分析高動態無人機的姿態。

MEMS慣性測量單元利用加速度計和陀螺儀測量數據，加速度計主要測量機體坐標系下的線性加速度，陀螺儀主要測量機體坐標系下的角加速度。陀螺儀經過積分得到姿態角，短時間內精度高，但陀螺儀自身存在漂移，隨著時間的積累誤差逐漸增加，因此長時間的精度較差。與之相反，機體高頻振動等因素引發的噪聲，對加速度計的影響較大，短期精度較低，但其測量誤差不會隨時間的積累而增加。利用兩者的互補關系，采用較優的算法對姿態數據進行融合，能提高姿態的測量精度和動態響應。姿態解算的主流數據融合算法包括卡爾曼濾波和互補濾波，與卡爾曼濾波相比，互補濾波的計算量較小、復雜度較低，多應用于低成本、實時的姿態測量系統。但是，傳統互補濾波中，PI參數在滑行、起飛、加速、減速、轉彎和降落等各種動作中均為固定值，不能根據無人機運動幅度和頻率變化調整PI參數。因此，在無人機整個飛行過程中，傳統互補濾波算法的動態適應性和測量精度均較差，更糟糕的是，很難將無人機速度變化引起的機動加速度與重力分量分開，尤其是在滑行和拐彎飛行過程中，無人機大幅度機動，未補償的機動加速度會引起較大姿態測量誤差。

針對無人機大幅度機動以及發動機高頻振動導致無人機實時姿態測量精度降低問題，本文利用MEMS慣性測量單元構建無人機的姿態測量系統，提出一種對機動加速度進行補償的自適應Mahony互補濾波算法，以補償機動加速度以及動態整定互補濾波器PI參數，對傳感器數據進行融合、補償和修正，通過開展無人機地面高速滑行試驗和空中飛行試驗，驗證所提算法的可行性和有效性。

坐標系定義

機體坐標系（b系）——ObXbYbZb

機體坐標系的三軸分別與無人機固定連接，其原點O定位在無人機的重心，ObXb軸沿無人機縱軸向前，ObYb軸沿無人機橫軸向右，ObZb軸沿無人機豎直軸向上。

導航坐標系（n系）——OnXnYnZn

導航坐標系取當地地理坐標系，其原點O定位在無人機的重心，OnXn軸指向地理東向，OnYn軸指向地理北向，OnZn軸垂直于當地旋轉橢球面指向天向。

b系相對于n系的方位關系用航向角Ψ、橫滾角γ和俯仰角θ描述。具體而言，航向角是無人機縱軸在當地水平面上的投影線與當地地理北向的夾角，俯仰角是無人機縱軸與水平面之間的夾角，橫滾角是無人機橫軸與水平面之間的夾角。

坐標系轉換

由于MEMS慣性測量單元固定在無人機上，各傳感器基于機體坐標系測量數據，因此在姿態解算時，需要將機體坐標系的數據轉換到導航坐標系，在此給出依次繞Z軸、Y軸、X軸旋轉所產生的轉換矩陣，用Cbn表示如下：

MEMS慣性測量單元姿態解算

MEMS慣性測量單元的基本原理框圖如圖1所示，MEMS慣性測量單元在初始靜態條件下由加速度計感知地球重力分量計算水平姿態角，即俯仰角和橫滾角信息；在動態條件下主要采用陀螺儀進行水平姿態角變化量計算。此外，無人機在動態條件下首先對機動加速度進行補償，然后利用基于重力的自適應Mahony互補濾波算法，對陀螺的漂移進行實時跟蹤校正，保障系統動態測量精度，最終輸出無人機姿態角、三軸加速度和三軸角速度等數字信息。

圖1 MEMS慣性測量單元基本原理框圖。

MEMS慣性測量單元采用的數據融合算法是基于重力的自適應Mahony互補濾波算法，它能綜合加速度計和陀螺儀各自的頻率響應優勢，從頻率角度對兩個傳感器數據進行融合，以減少測量和估計誤差。與此同時，為降低無人機加速起飛、爬升、拐彎、降落等大機動狀態對測量誤差的影響，需對無人機機動引起的加速度進行補償修正，保證MEMS慣性測量單元的測量精度。

機動加速度補償

其中β1和β2分別為無人機在縱軸和橫軸的機動加速度補償系數，該系數大小由所采用的慣性傳感器精度和無人機機動情況共同決定。

自適應Mahony互補濾波算法

考慮到低成本姿態測量系統的計算能力和精度要求，自適應Mahony互補濾波常作為低成本姿態測量系統中的姿態解算算法。本文對MEMS慣性測量單元采集的數據進行融合，用高通濾波器處理陀螺儀測量信號的同時，采用低通濾波器平滑加速度計測量信號，并在互補濾波器中加入自適應PI調節，以形成增強型互補濾波器。濾波器設計如（3）式所示。

（1）利用經過補償后的三軸加速度計輸出模值|?|的大小，確定調節比例參數kp和ki的值；

（2）將?歸一化；

（3）將導航坐標系下的重力向量轉換到機體坐標系得到向量d；

（4）計算誤差向量e；

（5）通過濾波器上一時刻的積分項項In?1和這一時刻的誤差向量e計算出這一時刻的積分項；

（6）計算信息δ；

（7）得到補償后的陀螺儀值w′；

（8）利用上一時刻的四元數qn?1和補償后的陀螺儀值w′更新四元數；

（9）將四元數qn歸一化，并將四元數轉成姿態角θ和γ。重復上述步驟，即可實時解算無人機姿態角。

無人機試驗驗證及分析

將MEMS慣性測量單元裝載于無人機航電設備艙，通過地面高速滑行試驗和空中飛行試驗，驗證低成本MEMS慣性測量單元的實際性能。測試設備為低成本MEMS慣性測量單元構成的姿態測量系統，參考測試設備為較高精度的捷聯慣性導航系統（INS），通過對比兩者的實時姿態測量，分析MEMS慣性測量單元的姿態測量精度。

無人機高速滑行試驗驗證

在地面共進行3次高速滑行試驗，驗證MEMS慣性測量單元的姿態測量精度，MEMS慣性測量單元及捷聯慣性導航系統在無人機高速滑行時，其姿態測量值如圖2所示，姿態測量誤差分析如圖3所示，MEMS慣性測量單元姿態誤差統計結果如表1所示，俯仰角和橫滾角在高速滑行狀態下誤差均方根值分別為0.181°和0.116°。試驗結果表明，在無人機高速滑行及發動機振動情況下，本文提出的方法可行并有效。

表1 無人機高速滑行時MEMS慣性測量單元姿態誤差統計結果。

圖2 無人機發動機轉速變化時姿態測量值。

圖3 無人機滑行速度變化時姿態測量誤差。

無人機飛行試驗驗證

無人機飛行試驗涉及高速滑行、起飛、爬升、平飛、拐彎、下降、著陸等過程，并驗證MEMS慣性測量單元的姿態測量精度。整個飛行過程超過30min，其飛行軌跡如圖4藍色曲線所示。

圖4 無人機飛行軌跡。

無人機在飛行過程中，MEMS慣性測量單元的俯仰角和橫滾角測量值如圖5和圖6上半部分所示，圖5和圖6下半部分分別表示俯仰角和橫滾角測量誤差，MEMS慣性測量單元姿態誤差統計結果如表2所示，其中俯仰角和橫滾角誤差均方根值分別為0.879° 和0.867°，最大誤差主要出現在無人機拐彎時。試驗結果表明，無人機飛行狀態下MEMS慣性測量單元姿態測量的跟隨性較好，誤差較平滑，所采用的算法具有較好效果。

圖5 無人機飛行時俯仰角跟隨性測量及誤差。

圖6 無人機飛行時橫滾角跟隨性測量及誤差。

表2 無人機飛行時MEMS慣性測量單元姿態誤差統計結果。

利用低成本MEMS慣性測量單元為高動態無人機構建姿態測量系統，是工程實踐應用中最具挑戰性的研究工作之一。試驗結果表明，本文提出的自適應Mahony互補濾波算法，提高了MEMS慣性測量單元姿態實時解算的精度，可以應用于高動態無人機相關姿態測量。