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

段敏 趙凌 周瑩

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

關(guān)鍵詞：擴展卡爾曼濾波;四旋翼無人機;姿態(tài)估計

中圖分類號：TP368;V279? ? ? ? ? ? 文獻(xiàn)標(biāo)識碼：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? 引? 言

四旋翼無人機因其成本低廉、維護(hù)操作簡單、可替代人力完成特殊任務(wù)等優(yōu)點，近年來被廣泛應(yīng)用于軍用和民用領(lǐng)域[1，2]。在四旋翼無人機飛控中，獲取穩(wěn)定可靠、高精度的姿態(tài)信息是首要目標(biāo)和任務(wù)，決定著后續(xù)姿態(tài)控制的精度，在四旋翼無人機的相關(guān)研究中至關(guān)重要。

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

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

1? 姿態(tài)估計數(shù)學(xué)模型

1.1? 相關(guān)坐標(biāo)系

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

1.2? 姿態(tài)表示

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

2? 傳感器原始數(shù)據(jù)誤差校準(zhǔn)

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

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

3? 基于EKF的姿態(tài)估計方法

3.1? 確定EKF濾波器狀態(tài)量和觀測量

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

3.2? 確定初始狀態(tài)值和初始誤差協(xié)方差

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

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

將初始姿態(tài)角轉(zhuǎn)換成四元數(shù)，加上初始陀螺儀輸出，就完成了初始狀態(tài)的求解。

初始誤差協(xié)方差（PQ和Pω可調(diào)）：P（0）=diag{PQ，PQ，PQ，PQ，PQ，Pω，Pω，Pω}

3.3? 濾波迭代過程

3.3.1? 基于陀螺儀的狀態(tài)預(yù)測

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

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

其中：

由式（8）可推出狀態(tài)預(yù)測方程：

式中W是系統(tǒng)噪聲。這種預(yù)測的誤差協(xié)方差P（k）也可由上一時刻的值推算得到：

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

其中Q是系統(tǒng)噪聲的協(xié)方差矩陣。F是從 k-1時刻到? k時刻的狀態(tài)轉(zhuǎn)移矩陣，通過求偏導(dǎo)的雅克比矩陣獲得：

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

單純依靠陀螺儀更新狀態(tài)值，長期結(jié)果可能會發(fā)散，通常需要加速度計和磁力計數(shù)據(jù)作為觀測量，對（1）中預(yù)測值進(jìn)行修正。

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

在理想狀態(tài)下，水平面內(nèi)磁場強度應(yīng)該相等，此時磁力計輸出應(yīng)為其中，，則在b系下的磁力計理想輸出值：

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

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

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

利用新息和卡爾曼增益，對狀態(tài)預(yù)測值和誤差協(xié)方差預(yù)測值進(jìn)行修正：

不斷重復(fù)3.3.1～3.3.2小節(jié)內(nèi)容，形成迭代，完成每一個時刻的狀態(tài)預(yù)測和校正。

4? 實驗結(jié)果與分析

4.1? 傳感器數(shù)據(jù)采集實驗

為了驗證2節(jié)中誤差補償?shù)挠行裕O(shè)計實驗采取傳感器原始數(shù)據(jù)，使校準(zhǔn)后的陀螺儀、加速度計、磁力計依次繞X軸、Y軸、Z軸從0°旋轉(zhuǎn)至±90°，采樣頻率為500 Hz，記錄并保存數(shù)據(jù)文件，在MATLAB上顯示波形，如圖3所示。

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

4.2? 姿態(tài)角解算實驗

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

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

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

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

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

5? 結(jié)? 論

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

參考文獻(xiàn)：

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

[2] 黃愛鳳，鄧克緒.民用無人機發(fā)展現(xiàn)狀及關(guān)鍵技術(shù) [C]//航空航天科技創(chuàng)新與長三角經(jīng)濟轉(zhuǎn)型發(fā)展分論壇，南京江蘇省航空航天學(xué)會，2012：29-35.

[3] 秦永元.慣性導(dǎo)航 [M].北京：科學(xué)出版社，2006.

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

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

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

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

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

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

[10] 秦永元.卡爾曼濾波與組合導(dǎo)航原理 [M].西安：西北工業(yè)大學(xué)出版社，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] 劉勝，張紅梅.最優(yōu)估計理論 [M].北京：科學(xué)出版社，2011：129-130.

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

[14] 楊越.基于捷聯(lián)慣導(dǎo)系統(tǒng)的小型無人機姿態(tài)解算及控制算法研究 [D].西安：西安電子科技大學(xué)，2017.

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

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