基于自適應(yīng)無跡卡爾曼濾波的四旋翼無人機(jī)姿態(tài)解算

劉康安， 張偉偉， 肖永超， 葉 沐

(1.上海工程技術(shù)大學(xué)，上海 201000； 2.清華大學(xué)，北京 100000)

0 引言

無人機(jī)的姿態(tài)估計(jì)是其穩(wěn)定飛行的基礎(chǔ)，姿態(tài)解算的精度直接關(guān)系到姿態(tài)控制的穩(wěn)定性。近年來，隨著微機(jī)電系統(tǒng)(MEMS) 技術(shù)的快速發(fā)展，低成本、低功耗、體積小的MEMS慣性傳感器廣泛應(yīng)用于四旋翼小型無人機(jī)的姿態(tài)解算[1]。其中，具有優(yōu)越靜態(tài)特性的加速度計(jì)在靜止?fàn)顟B(tài)下有較高的測(cè)量精度，具有良好動(dòng)態(tài)響應(yīng)特性的陀螺儀在短時(shí)間運(yùn)動(dòng)狀態(tài)下有較高的測(cè)量精度，具有長(zhǎng)時(shí)間測(cè)試穩(wěn)定性的磁力計(jì)容易受到周圍環(huán)境中的外部擾動(dòng)磁源的干擾，但陀螺儀不受磁場(chǎng)源的干擾[2]。所以，使用單一的傳感器解算姿態(tài)角往往精度較低，難以滿足平穩(wěn)飛行的要求。而采用多傳感器數(shù)據(jù)融合的方法獲得更加精確的姿態(tài)輸出是當(dāng)前無人機(jī)領(lǐng)域的研究難點(diǎn)和熱點(diǎn)。

目前常用的航向和姿態(tài)信息融合算法有互補(bǔ)濾波(Complementary Filter，CF)[3]、卡爾曼濾波(Kalman Filter,KF)和梯度下降法[4]等。其中，互補(bǔ)濾波算法相對(duì)容易實(shí)現(xiàn)，計(jì)算成本低。然而，其輸出的姿態(tài)和導(dǎo)航的精度略低于卡爾曼濾波估計(jì)的精度，其高、低通濾波器轉(zhuǎn)接頻率難以確定，在動(dòng)態(tài)環(huán)境中測(cè)量精度較差。LEFFERTS等[5]使用卡爾曼濾波算法進(jìn)行姿態(tài)信息融合，建模簡(jiǎn)單、濾波速度快，但其只適用于線性高斯噪聲系統(tǒng)，忽略了非線性因素給濾波帶來的影響。針對(duì)多旋翼飛行器等非線性系統(tǒng)，文獻(xiàn)[6]提出一種基于擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)算法的飛行器姿態(tài)信息融合方法，但在姿態(tài)確定過程中，系統(tǒng)方程的線性化步驟采用雅可比矩陣更新狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣，穩(wěn)定性差、估計(jì)偏差大。面對(duì)EKF算法存在的問題，文獻(xiàn)[7-9]提出使用無跡卡爾曼濾波(Unscented Kalman Filter，UKF)算法進(jìn)行無人機(jī)姿態(tài)解算，減小了EKF算法線性化帶來的誤差，且無需計(jì)算雅可比矩陣，具有更快的收斂速度。在UKF算法中，基于無跡變換生成的sigma-point近似值更新方程[10]，憑借其更高的姿態(tài)求解精度和強(qiáng)魯棒性，尤其適合于高度非線性運(yùn)動(dòng)學(xué)模型的姿態(tài)估計(jì)。然而，根據(jù)經(jīng)驗(yàn)選擇的UKF算法關(guān)鍵參數(shù)——過程噪聲協(xié)方差Q對(duì)復(fù)雜工作環(huán)境下的估計(jì)精度有很大影響。文獻(xiàn)[11]通過求解系統(tǒng)誤差協(xié)方差之間的比率對(duì)過程噪聲協(xié)方差矩陣進(jìn)行在線調(diào)整。

綜合考慮姿態(tài)解算精度、計(jì)算速度和算法實(shí)時(shí)性的需求，提出一種優(yōu)化的自適應(yīng)無跡卡爾曼濾波(Adaptive Unscented Kalman Filter，AUKF)算法。采用梯度下降算法對(duì)過程噪聲協(xié)方差Q進(jìn)行調(diào)優(yōu)，梯度下降算法具有全局收斂速度快、運(yùn)算速度快、實(shí)現(xiàn)簡(jiǎn)單等優(yōu)點(diǎn)，可以用來調(diào)整過程噪聲協(xié)方差Q，提高UKF算法的姿態(tài)解算精度和性能。

1 無人機(jī)姿態(tài)建模

1.1 建立坐標(biāo)系

無人機(jī)姿態(tài)測(cè)量需要建立符合右手定則的2個(gè)坐標(biāo)系和3個(gè)姿態(tài)角，分別是導(dǎo)航坐標(biāo)系、機(jī)體坐標(biāo)系、偏航角(ψ)、橫滾角(φ)和俯仰角(θ)。導(dǎo)航坐標(biāo)系n是相對(duì)大地水平面定義的正交坐標(biāo)，坐標(biāo)軸Xn,Yn,Zn分別指向地理上的北、 東、地方向。機(jī)體坐標(biāo)系b固定在無人機(jī)上，坐標(biāo)軸Xb,Yb,Zb分別指向無人機(jī)的前方、右側(cè)以及底部。導(dǎo)航坐標(biāo)系到機(jī)體坐標(biāo)系的坐標(biāo)變換矩陣可由式(1)的方向余弦矩陣表示，即

(1)

1.2 系統(tǒng)模型建立

借助四元數(shù)法表述航向和姿態(tài)變化的過程因具有無奇點(diǎn)以及計(jì)算量小等優(yōu)點(diǎn)而被廣泛應(yīng)用。劉興川等[12]提出了一種基于四元數(shù)的MARG傳感器姿態(tài)測(cè)量算法，實(shí)現(xiàn)三自由度的姿態(tài)測(cè)量，顯著提高姿態(tài)測(cè)量精度。本文選用四元數(shù)進(jìn)行姿態(tài)解算，將陀螺儀得到的數(shù)據(jù)進(jìn)行四元數(shù)更新后得到的值當(dāng)作預(yù)測(cè)值，觀測(cè)值為加速度計(jì)和磁力計(jì)的測(cè)量值。

1.2.1 基于加速度計(jì)的姿態(tài)估計(jì)

加速度計(jì)輸出的是載體的加速度，用加速度計(jì)計(jì)算姿態(tài)角時(shí)，坐標(biāo)系n下的重力加速度為gn=(00g)T，加速度計(jì)在坐標(biāo)系b下的輸出為ab=(axayaz)T，其轉(zhuǎn)換關(guān)系可以表示為

(2)

通過上式得到的加速度，可以得出基于加速度計(jì)的俯仰角和橫滾角分別為

(3)

1.2.2 基于磁力計(jì)的姿態(tài)估計(jì)

(4)

因此，地磁矢量在水平方向上的分量為

(5)

則偏航角可表示為

(6)

ψmagn=ψm+α

(7)

式中，α為磁偏角。

1.2.3 基于陀螺儀的姿態(tài)估計(jì)

陀螺儀輸出的是敏感載波角加速度?；谒脑獢?shù)的陀螺儀角速度動(dòng)態(tài)模型更新可表示為

(8)

則式(8)的矩陣形式為

(9)

采用四元數(shù)微分方程的皮卡逐次逼近法對(duì)式(9)求解，其離散形式為

(10)

用四元數(shù)表示從機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系的坐標(biāo)變換矩陣可寫作

(11)

因此，無人機(jī)姿態(tài)角可以用四元數(shù)形式表示為

(12)

2 基于自適應(yīng)無跡卡爾曼濾波器的姿態(tài)解算

在本章中，提出一種優(yōu)化的自適應(yīng)無跡卡爾曼濾波器來解算無人機(jī)的姿態(tài)角。為了提高算法的性能，采用梯度下降算法對(duì)無跡卡爾曼濾波器的關(guān)鍵參數(shù)進(jìn)行了調(diào)整，即過程噪聲協(xié)方差Q。圖1給出了基于梯度下降算法優(yōu)化的自適應(yīng)無跡卡爾曼濾波(AUKF)姿態(tài)估計(jì)的全過程。

圖1 基于AUKF的姿態(tài)解算流程圖Fig.1 Flow chart of attitude calculation based on AUKF

2.1 梯度下降算法

作為求解無約束優(yōu)化問題的常用方法，梯度下降算法具有實(shí)現(xiàn)簡(jiǎn)單的優(yōu)點(diǎn)。梯度下降法的基本思想是選擇一個(gè)合適的初值x(0),迭代更新x的值，最小化目標(biāo)函數(shù)直到收斂。梯度下降的輸入是目標(biāo)函數(shù)f(x)，梯度函數(shù)g(x)=▽f(x)和計(jì)算精度ε;輸出是f(x)的最小值x*。梯度下降的過程可以詳細(xì)總結(jié)如下。

1) 初始關(guān)鍵參數(shù)x(0)和k。

2) 計(jì)算目標(biāo)函數(shù)f(x(k))。

4) 設(shè)變量x(k+1)=x(k)+λkpk，更新函數(shù)f(x(k+1))。如果||f(x(k+1))-f(x(k))||<ε或||x(k+1)-x(k)||<ε，停止迭代，并讓x*=x(k)。否則，令k=k+1，然后回到步驟3)去尋找另一個(gè)更優(yōu)的最小值。

5) 當(dāng)目標(biāo)函數(shù)為凸函數(shù)時(shí)，梯度下降的解為全局最優(yōu)解。

2.2 傳統(tǒng)UKF設(shè)計(jì)

2.2.1 狀態(tài)方程

狀態(tài)預(yù)測(cè)是基于之前的最優(yōu)估計(jì)，且無跡卡爾曼濾波器的離散時(shí)間非線性動(dòng)態(tài)狀態(tài)方程為

xk=Ak-1xk-1+ωk-1

(13)

式中:xk和xk-1分別為時(shí)刻k的先驗(yàn)估計(jì)和時(shí)刻k-1的后驗(yàn)估計(jì);ωk是協(xié)方差Q的過程噪聲，簡(jiǎn)化為獨(dú)立高斯白噪聲。Ak是狀態(tài)轉(zhuǎn)移矩陣，可以得到

(14)

2.2.2 量測(cè)方程

校正狀態(tài)是細(xì)化測(cè)量估計(jì)的關(guān)鍵步驟。量測(cè)方程表示為

zk=H(xk)+vk

(15)

式中:vk是具有噪聲協(xié)方差R的獨(dú)立高斯測(cè)量噪聲，Hk(xk)為輸出函數(shù)，其算式為

(16)

2.2.3 無跡變換

計(jì)算2n+1個(gè)sigma點(diǎn)，即

(17)

計(jì)算sigma點(diǎn)對(duì)應(yīng)的權(quán)值,即

(18)

2.3 過程噪聲協(xié)方差

由于無人機(jī)與周圍環(huán)境的耦合作用，在無人機(jī)飛行和懸停過程中，機(jī)體會(huì)產(chǎn)生隨機(jī)振動(dòng)和加速度，導(dǎo)致過程噪聲協(xié)方差Q難以確定。

(19)

(20)

2.4 基于梯度下降的無跡卡爾曼濾波

基于卡爾曼線性濾波框架的UKF不是傳統(tǒng)的線性化非線性函數(shù)的方法，而是利用無跡變換傳播均值和協(xié)方差。與擴(kuò)展卡爾曼濾波相比，無需推導(dǎo)雅可比矩陣的UKF可以使?fàn)顟B(tài)變量近似于非線性函數(shù)的概率密度分布，具有更強(qiáng)的魯棒性和精確性。但過程噪聲協(xié)方差Q在一定程度上影響濾波性能，可以采用梯度下降法對(duì)無跡卡爾曼濾波進(jìn)行改進(jìn)?；谔荻认陆邓惴▋?yōu)化的無跡卡爾曼濾波流程如下。

(21)

式中，i=0,2，…，2n。

2) 根據(jù)步驟1)得到的sigma點(diǎn)，通過式(13)狀態(tài)方程計(jì)算系統(tǒng)狀態(tài)變量的預(yù)測(cè)值和協(xié)方差矩陣為

(22)

3) 對(duì)步驟2)的預(yù)測(cè)值再次進(jìn)行UT變換，計(jì)算出新的sigma點(diǎn)，即

(23)

式中，i=0,2，…，2n。

4) 根據(jù)步驟3)得到的新sigma點(diǎn)，通過式(15)中的觀測(cè)值計(jì)算出觀測(cè)變量的預(yù)測(cè)值和協(xié)方差矩陣可

表示為

(24)

式中,i=0,2，…，2n。

5) 無跡卡爾曼濾波增益矩陣為

(25)

6) 無跡卡爾曼濾波系統(tǒng)的狀態(tài)更新和協(xié)方差更新算式為

(26)

(27)

式中:L(·)為損失函數(shù)；RMSE(·)為估計(jì)結(jié)果的均方根誤差函數(shù)；x′為姿態(tài)估計(jì)系統(tǒng)的真狀態(tài)值。

3 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證本文所提AUKF算法的精確性和有效性，搭建了基于pixhawk的飛行實(shí)驗(yàn)平臺(tái)并在室外進(jìn)行無人機(jī)動(dòng)態(tài)飛行實(shí)驗(yàn)。然后，將實(shí)驗(yàn)結(jié)果與LPMS-IG1P高精度慣性測(cè)量單元讀取的數(shù)據(jù)進(jìn)行對(duì)比，驗(yàn)證了所設(shè)計(jì)系統(tǒng)的優(yōu)異性能和實(shí)用性。本文設(shè)計(jì)的姿態(tài)測(cè)量系統(tǒng)由MPU9250三軸陀螺儀，MPU9250三軸加速度計(jì)和LSM3030C三軸磁力計(jì)組成，選用STM32MP157作為微處理器，通過I2C總線與上位機(jī)進(jìn)行通信。

實(shí)驗(yàn)設(shè)置采樣頻率為10 Hz，選取連續(xù)60 s內(nèi)的姿態(tài)測(cè)量數(shù)據(jù)，圖2(a)、圖3(a)和圖4(a)為實(shí)驗(yàn)過程中高精度慣導(dǎo)讀取的實(shí)際橫滾角、俯仰角以及偏航角。本文分別采用常規(guī)卡爾曼濾波(KF)、無跡卡爾曼濾波(UKF)和本文所提的自適應(yīng)無跡卡爾曼濾波(AUKF)算法進(jìn)行姿態(tài)解算，并將3種算法解算出的橫滾角、俯仰角和偏航角數(shù)據(jù)與實(shí)際角度進(jìn)行對(duì)比，得到如圖2(b)、圖3(b)和圖4(b)所示的3種算法的估計(jì)誤差。本文采用均方根誤差(RMSE)來評(píng)估算法的估計(jì)精度，3種算法的均方根誤差對(duì)比見表1。

圖2 橫滾角姿態(tài)變化及測(cè)量誤差Fig.2 Attitude variation and measurement error of roll angle

圖3 俯仰角姿態(tài)變化及測(cè)量誤差Fig.3 Attitude variation and measurement error of pitch angle

圖4 偏航角姿態(tài)變化及測(cè)量誤差Fig.4 Attitude variation and measurement error of yaw angle

表1 3種算法的均方根誤差Table 1 RMSE of three algorithms (°)

從圖2～4中可以看出，基于常規(guī)卡爾曼濾波算法的姿態(tài)解算誤差最大，橫滾角最大誤差為2.1°，俯仰角最大誤差為2.5°，偏航角最大誤差為3.8°。與常規(guī)卡爾曼濾波算法相比，無跡卡爾曼濾波算法姿態(tài)解算精度更高，其橫滾角最大誤差為1.4°，俯仰角最大誤差為1.6°，偏航角最大誤差為2.9°，但由于噪聲協(xié)方差Q被設(shè)定為經(jīng)驗(yàn)值，導(dǎo)致其估計(jì)誤差仍然較大。而本文提出的自適應(yīng)無跡卡爾曼濾波算法通過梯度下降來調(diào)整Q，大大提高了UKF的估計(jì)性能，因此其姿態(tài)解算效果最好，估計(jì)誤差明顯小于UKF和KF算法，橫滾角和俯仰角最大誤差分別小于1.1°和1.3°，偏航角最大誤差優(yōu)于2.2°。表1給出了動(dòng)態(tài)飛行實(shí)驗(yàn)中3種算法獲得的俯仰角、橫滾角和偏航角的均方根誤差，本文算法獲得的3種姿態(tài)角的均方根誤差最小，UKF算法的均方根誤差略大于本文算法，KF算法的均方根誤差最大。綜合來看，本文算法具有更好的濾波效果，滿足無人機(jī)姿態(tài)解算的實(shí)用性要求。

4 結(jié)束語

針對(duì)四旋翼無人機(jī)姿態(tài)解算問題，本文提出了一種優(yōu)化自適應(yīng)無跡卡爾曼濾波的姿態(tài)解算算法。該算法采用梯度下降算法對(duì)過程噪聲協(xié)方差進(jìn)行在線調(diào)整，優(yōu)化狀態(tài)預(yù)測(cè)估計(jì)，有效降低了姿態(tài)解算的誤差。在姿態(tài)建模中以四元數(shù)作為狀態(tài)量，消除了Euler角的奇異問題，同時(shí)減少了運(yùn)算量。為了驗(yàn)證所提算法的姿態(tài)解算精度，做了室外飛行實(shí)驗(yàn)并進(jìn)行了分析和比較。實(shí)驗(yàn)分析結(jié)果表明，本文算法與KF和傳統(tǒng)的UKF算法相比具有較高的精度，在動(dòng)態(tài)飛行環(huán)境下橫滾角和俯仰角均方根誤差小于0.5°，具有一定的工程應(yīng)用價(jià)值。