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

段敏 趙凌 周瑩

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

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

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

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

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

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

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

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

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

1.2? 姿態(tài)表示

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

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

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

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

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

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

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

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

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

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

將初始姿態(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ù)測(cè)

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

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

其中：

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

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

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

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

3.3.2? 基于加速度計(jì)/磁力計(jì)的量測(cè)修正

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

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

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

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

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

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

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

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

4? 實(shí)驗(yàn)結(jié)果與分析

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

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

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

4.2? 姿態(tài)角解算實(shí)驗(yàn)

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

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

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

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

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

5? 結(jié)? 論

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

參考文獻(xiàn)：

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

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

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

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

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

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

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

[8] 王曉初，盧琛.四旋翼姿態(tài)解算算法的對(duì)比與研究 [J].制造業(yè)自動(dòng)化，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)估計(jì)理論 [M].北京：科學(xué)出版社，2011：129-130.

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

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

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

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