基于擴展卡爾曼濾波的四旋翼無人機姿態估計方法

段敏 趙凌 周瑩

摘? 要：為提高四旋翼無人機姿態參數獲取的準確性，確保后續姿態控制精度，采用STM32F407微控制器以及多傳感器構成姿態測量系統。對各傳感器原始誤差進行校準，應用擴展卡爾曼濾波（EKF）進行基于陀螺儀的狀態預測和基于加速度計/磁力計的測量校正，融合信息并估計出3姿態角，與3自由度姿態算法驗證系統測量出的姿態角真實值對比，3個角度的平均誤差為0.7°，相對于基于單一陀螺儀積分和基于加速度計/磁力計的姿態解算，誤差分別下降了3.034°和0.174°，該方法可有效提高EKF估計精度。

關鍵詞：擴展卡爾曼濾波;四旋翼無人機;姿態估計

中圖分類號：TP368;V279? ? ? ? ? ? 文獻標識碼：A文章編號：2096-4706（2022）04-0007-05

Attitude Estimation Method for Quad-rotor UAV Based on Extended Kalman Filter

DUAN Min， ZHAO Ling， ZHOU Ying

（The College of Post and Telecommunication of WIT， Wuhan? 430073， China）

Abstract： In order to improve the accuracy of attitude parameters acquisition of quad-rotor UAV and ensure the subsequent attitude control accuracy， STM32F407 microcontroller and multi-sensor are used to form an attitude measurement system. The original error of each sensor is calibrated. The extended Kalman filter （EKF） is used for gyro-based state prediction and accelerometer/magnetometer-based measurement correction. The information is fused and the three attitude angles are estimated. Compared with the real value of the attitude angle measured by the 3-DOF attitude algorithm verification system， the average error of the three angles is 0.7°， compared with the attitude solution based on single gyro integral and the attitude solution based on accelerometer/magnetometer， the errors are reduced by 3.034° and 0.174° respectively. This method can effectively improve the accuracy of EKF estimation.

Keywords： extended Kalman filter; quad-rotor UAV; attitude estimation

0? 引? 言

四旋翼無人機因其成本低廉、維護操作簡單、可替代人力完成特殊任務等優點，近年來被廣泛應用于軍用和民用領域[1，2]。在四旋翼無人機飛控中，獲取穩定可靠、高精度的姿態信息是首要目標和任務，決定著后續姿態控制的精度，在四旋翼無人機的相關研究中至關重要。

無人機依靠姿態測量系統獲取實時姿態。四旋翼無人機應其體積、成本和載荷的限制，通常使用微型化、高集成化的微慣性測量單元（MEMS-IMU）來獲取姿態，IMU不受外界環境影響，可隨時隨地為無人機提供狀態信息。但是IMU中的陀螺儀存在時間積分誤差，長期穩定性差的缺點;IMU中的加速度計受運動加速度影響，動態精度低[3]。一些姿態測量系統里會加入磁力計輔助測量，但磁力計也會受磁場影響，不能完全信賴[4]。最終，需要算法融合不同傳感器的數據，并估計出最佳的姿態角。常用于數據融合、姿態估計的濾波算法有：互補濾波[5]、梯度下降法[6]、卡爾曼濾波（KF）[7]等，文獻[8]簡單對比了這幾種算法，互補濾波和梯度下降法算法簡單，適合用于處理性能受限的飛行器，在硬件性能滿足的條件下，大多選用KF[9];KF效果優越、應用廣泛，但不適用于包括無人機飛控系統在內的非線性系統[10]。針對此局限，有許多基于KF的擴展算法，如無跡卡爾曼濾波（UKF）[11]、粒子濾波（PF）[12]、擴展卡爾曼濾波（EKF）[13]，前兩者都有計算量大、實時性差的問題，而EKF數據存儲量小、計算量相對較小，綜上考慮，EKF是小成本四旋翼無人機數據融合、姿態估計的最佳選擇[14]。文獻[15]采用了EKF算法直接融合MPU9250中陀螺儀、加速度計、磁力計9項原始數據，通過實驗證明EKF估計精度確實優于互補濾波和梯度下降法。傳感器由于自身制作工藝缺陷以及環境影響，原始數據存在一定誤差，直接融合對EKF估計精度有影響。本文在文獻[15]的基礎上，對各傳感器原始數據誤差進行了初步校準，提高EKF輸入信息的精度，保證融合的有效性。

采用基于ARM Cortex-M4內核的STM32F407作為核心微控制器，3軸加速度計、3軸陀螺儀和3軸磁力計構成姿態測量系統，其中陀螺儀/加速度計采用芯片MPU6050，磁力計采用HMC5883L。對各傳感器原始誤差進行初步校準后采用EKF融合，利用角速度信息進行基于模型的狀態預測，利用加速度和磁強度信息進行基于觀測量的量測修正，估計出無人機的3個姿態角供后續姿態控制使用，結構圖如圖1所示。D64BE70C-2D9A-4AA2-9F93-49CB47C251EB

1? 姿態估計數學模型

1.1? 相關坐標系

在研究四旋翼無人機的飛行狀態時，主要用到導航坐標系（n系）和機體坐標系（b系）。n系是求解導航參數時的參考坐標系，選用地理的北東地（NED）方向作為n系;b系與飛機固連，隨機體運動而運動，x軸在飛行器對稱平面內，沿飛行器設計軸線指向機首，y軸垂直于對稱平面，指向機身右弦，z軸在對稱平面內垂直于另外兩軸，指向機身下方。n系和b系示意圖如圖2所示。

1.2? 姿態表示

通常用歐拉角、旋轉變換矩陣、四元數3種方式來描述無人機的姿態?；跉W拉角的姿態解算存在萬向節死鎖問題;旋轉變換矩陣有9個向量，計算量大。因此，本文選擇用四元數Q（q0，q1，q2，q3）來描述姿態，用它表示姿態角（俯仰角θ，偏航角ψ，橫滾角?）如式（1）所示，表示從b系到n系的旋轉變換矩陣如式（2）：

2? 傳感器原始數據誤差校準

考慮陀螺儀、加速度計、磁力計都存在的3項確定性誤差：零偏誤差、刻度因子誤差、安裝誤差，建立如式（3）所示的誤差校準模型，當式中x為a，ω，m時，式（3）分別為加速度計、陀螺儀、磁力計的誤差模型。

x是校準后的準確數據，x′是原始數據，bx是零偏誤差，x′-bx補償零偏，Sx補償刻度因子誤差，Rx補償安裝誤差。求出3維向量bx及3維矩陣SxRx即完成了校準。對加速度計使用6面校準法，依次使加速度計±X，±Y，±Z軸指向地向，理論上，只有沿著地向的軸輸出為g，其他兩軸輸出均為0，記錄實際輸出代入式（3）可求得校準參數。對陀螺儀使用3軸轉臺法，將陀螺儀固定在高精度轉臺，以確定角速率旋轉，測量實際輸出，代入式（3）即可求出參數。對磁力計使用簡單標定法，各軸向零偏等于該軸最大最小輸出的平均數，SxRx為各軸向最大最小輸出差值的比值。使用上述方法，本文測得各傳感器校準參數為：

3? 基于EKF的姿態估計方法

3.1? 確定EKF濾波器狀態量和觀測量

姿態四元數和3軸陀螺儀輸出組成7維狀態量，3軸加速度計、3軸磁力計、3軸陀螺儀輸出以及四元數組成13維觀測量，分別如式（4）和式（5）所示（上標b表示是在b系下的輸出值）：

3.2? 確定初始狀態值和初始誤差協方差

利用加速度計在靜態時的輸出可以計算初始俯仰角和初始橫滾角，如式（6）：

利用磁力計輸出可以求初始偏航角，先左乘得到n下的輸出mn，再計算偏航角：

將初始姿態角轉換成四元數，加上初始陀螺儀輸出，就完成了初始狀態的求解。

初始誤差協方差（PQ和Pω可調）：P（0）=diag{PQ，PQ，PQ，PQ，PQ，Pω，Pω，Pω}

3.3? 濾波迭代過程

3.3.1? 基于陀螺儀的狀態預測

在每個時鐘采樣周期T里，已知k-1時刻的四元數Qk-1，利用陀螺儀輸出ΩωQk-1可以對k時刻的四元數進行預測，如式（8）所示：

Q（k）=Q（k-1）+T/2Ωω（k-1）Q（k-1）（8）

其中：

由式（8）可推出狀態預測方程：

式中W是系統噪聲。這種預測的誤差協方差P（k）也可由上一時刻的值推算得到：

P（k）- =F*P（k-1）*FT+Q? ? （10）

其中Q是系統噪聲的協方差矩陣。F是從 k-1時刻到? k時刻的狀態轉移矩陣，通過求偏導的雅克比矩陣獲得：

3.3.2? 基于加速度計/磁力計的量測修正

單純依靠陀螺儀更新狀態值，長期結果可能會發散，通常需要加速度計和磁力計數據作為觀測量，對（1）中預測值進行修正。

在理想狀態下，飛行器能夠達到和n系XOY面水平的狀態，此時的加速度計輸出在歸一化后為（0，0，1），則在b系下的加速度計理想輸出為：

在理想狀態下，水平面內磁場強度應該相等，此時磁力計輸出應為其中，，則在b系下的磁力計理想輸出值：

依據式（12）和式（13）可以建立量測方程：

當k時刻傳感器真實測量值Z（k）到達時，計算新息和卡爾曼增益如式（15）和式（16）所示：

式（16）中R是量測噪聲協方差矩陣，H是量測矩陣，通過求偏導的雅克比矩陣獲得：

利用新息和卡爾曼增益，對狀態預測值和誤差協方差預測值進行修正：

不斷重復3.3.1～3.3.2小節內容，形成迭代，完成每一個時刻的狀態預測和校正。

4? 實驗結果與分析

4.1? 傳感器數據采集實驗

為了驗證2節中誤差補償的有效性，設計實驗采取傳感器原始數據，使校準后的陀螺儀、加速度計、磁力計依次繞X軸、Y軸、Z軸從0°旋轉至±90°，采樣頻率為500 Hz，記錄并保存數據文件，在MATLAB上顯示波形，如圖3所示。

理論上，陀螺儀在繞某一軸轉動時，另兩軸應輸出為0;加速度計在旋轉過程中只有指向地向的軸輸出為1，另兩軸輸出為0;磁力計的3軸在轉到相同位置時有相同的輸出，從輸出波形來看，傳感器數據準確，誤差補償有效。

4.2? 姿態角解算實驗

在Keil5的軟件編譯環境下調試代碼，加載程序至飛控板。采用3自由度姿態算法驗證系統G-TZ-4002測量真實姿態角，將加載有EKF姿態解算程序的飛控板置于驗證平臺上，平臺和飛控板分別通過總線和無線數傳將數據發送到地面站，地面站顯示姿態角隨時間變化的曲線。

驗證平臺傳輸波特率為115200Baud，解算頻率為200 Hz，IMU更新頻率為1 000 Hz，磁力計更新頻率為200 Hz。經過調試后的Ra=0.2、Rω=0.1、Rm=20。D64BE70C-2D9A-4AA2-9F93-49CB47C251EB

俯仰角、橫滾角、偏航角隨時間變化的部分曲線圖分別如圖4、圖5、圖6所示。

由圖4可知，靜態時，EKF估計的俯仰角保持在-12°，幾乎呈直線，穩定性優秀;動態時，EKF估計誤差保持在1°以內，總體精度優異。圖5靜態時，EKF估計的橫滾角保持在26°，非常穩定;動態時，最大誤差2.5°。由圖6可知，靜態時，偏航角估計值維持在-1°，無明顯波動，穩定性非常好;動態時，誤差保持在2°以內?？傮w來說EKF估計的3個姿態角曲線都非常平滑，代表EKF姿態估計具有優異的穩定性和準確度。

將本文中EKF估計的姿態角均方誤差（MSE）與基于單一陀螺儀積分以及基于加速度計/磁力計解算的進行對比，列出表1。從表中可以看出，基于單一陀螺儀積分的解算誤差較為明顯，最大接近10°?；诩铀俣扔?磁力計的解算誤差相對較小，但是其數據是在靜態條件下獲得，如果是動態，誤差會更大?；贓KF估計的姿態角誤差明顯小于前面兩者，與基于陀螺儀的解算相比，3個角度的均方誤差平均下降了3.034°;與基于加速度計/磁力計的解算相比，下降了0.174°。上述數據證明基于EKF的姿態估計確實提高了姿態數據的精度，效果優異。另外，加入了文獻[15]中的EKF估計結果進行對比，本文的基于傳感器校準的EKF估計誤差下降了0.619°，證明本文中對傳感器的校準提高了EKF輸入數據精度，保證了估計的有效性和精度。

5? 結? 論

本文首先對各姿態傳感器原始數據進行了校準，然后使用擴展卡爾曼濾波（EKF）融合了校準后的數據并估計出姿態角。將估計出的姿態角與真實值對比，發現誤差能控制在2.1°以內（3角度平均值），與基于單一傳感器（陀螺儀、加速度計/磁力計）的姿態解算對比，誤差分別下降了3.034°和0.174°，說明本文的EKF融合效果較好。與文獻[15]中的EKF對比，本文的估計誤差下降了0.619°，證明本文中對傳感器的校準有效，基于傳感器校準的EKF姿態估計能提高精度，效果優異。

參考文獻：

[1] 陳彥強，張淑瑞，張永富.軍用無人機發展現狀和趨勢 [C]//中國國際無人駕駛航空器系統大會.北京：出版社不詳，2016：1-5.

[2] 黃愛鳳，鄧克緒.民用無人機發展現狀及關鍵技術 [C]//航空航天科技創新與長三角經濟轉型發展分論壇，南京江蘇省航空航天學會，2012：29-35.

[3] 秦永元.慣性導航 [M].北京：科學出版社，2006.

[4] 潘佳虹.四旋翼無人機的姿態估計與控制研究 [D].杭州：杭州電子科技大學，2016.

[5] 萬曉鳳，康利平，余運俊，等.互補濾波算法在四旋翼飛行器姿態解算中的應用 [J].測控技術，2015，34（2）：8-11.

[6] 劉青文，郭劍東，浦黃忠，等.基于梯度下降法的四旋翼無人機姿態估計系統 [J].電光與控制，2018，25（5）：17-21.

[7] 杭成，朱海霞，許毅立，等.卡爾曼濾波在四旋翼飛行器姿態解算中的應用 [J].江蘇科技信息，2016（17）：67-68.

[8] 王曉初，盧琛.四旋翼姿態解算算法的對比與研究 [J].制造業自動化，2015，37（2）：120-122+138.

[9] 林慶峰，諶利，奚海蛟.多旋翼無人飛行器嵌入式飛控開發指南 [M].北京：清華大學出版社，2017.

[10] 秦永元.卡爾曼濾波與組合導航原理 [M].西安：西北工業大學出版社，2012.

[11] JULIER S J，UHLMAN J K，DURRANT H F. A new Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators [J].IEEE Transactions on Automatic Control，2000，45 （3）：477-482.

[12] 劉勝，張紅梅.最優估計理論 [M].北京：科學出版社，2011：129-130.

[13] 楊兆，沈作軍.基于擴展卡爾曼濾波的小型固定翼無人機姿態估計方法分析 [J].航空科學技術，2017，28（11）：15-21.

[14] 楊越.基于捷聯慣導系統的小型無人機姿態解算及控制算法研究 [D].西安：西安電子科技大學，2017.

[15] 石川，林達，張果，等.基于QEKF的四旋翼飛行器姿態估計 [J].現代雷達，2018，40（11）：49-52+56.

作者簡介：段敏（1994.08—），女，漢族，湖北黃岡人，助教，碩士，主要研究方向：信號處理與智能控制。D64BE70C-2D9A-4AA2-9F93-49CB47C251EB