微型航姿系統(tǒng)抗加速度干擾姿態(tài)估計(jì)研究

桂林電子科技大學(xué) 電子工程與自動(dòng)化學(xué)院，廣西桂林 541004

一、引言

小型無人機(jī)采用的航姿參考系統(tǒng)（Attitude and Heading Reference System，AHRS）利用3軸加速度計(jì)、3軸陀螺儀和3軸磁強(qiáng)計(jì)分別測(cè)量重力場(chǎng)、地磁場(chǎng)和載體角速度矢量，并由此估計(jì)載體姿態(tài)（包括航向角、俯仰角與橫滾角）[1-4]。然而，3軸加速度計(jì)的讀數(shù)實(shí)際上是重力加速度與載體運(yùn)動(dòng)加速度的矢量和。因此，當(dāng)無人機(jī)處于加速狀態(tài)時(shí)，重力觀測(cè)值會(huì)受到運(yùn)動(dòng)加速度的干擾，進(jìn)而影響AHRS航姿精度。

以AHRS中最常用的EKF（擴(kuò)展卡爾曼濾波）姿態(tài)估計(jì)算法[1-4]為基礎(chǔ)，本文對(duì)抑制運(yùn)動(dòng)加速度干擾的方法進(jìn)行了理論分析，并討論了航姿誤差的收斂性及其改進(jìn)措施。通過仿真和實(shí)驗(yàn)對(duì)各種方法進(jìn)行了對(duì)比，結(jié)果表明對(duì)運(yùn)動(dòng)加速度進(jìn)行外部估計(jì)所需計(jì)算量最小，且補(bǔ)償效果最佳，適合實(shí)際應(yīng)用。

二、算法分析

1、EKF姿態(tài)算法

采用EKF進(jìn)行姿態(tài)估計(jì)，首先需選擇合適的姿態(tài)表示方式來構(gòu)造狀態(tài)向量。除了常用的歐拉角（包括航向角、俯仰角和橫滾角）、姿態(tài)四元素及方向余弦矩陣（DCM）外，采用乘性姿態(tài)誤差可最大限度地降低EKF狀態(tài)向量的維數(shù)[5]。以乘性姿態(tài)誤差及陀螺儀零漂作為狀態(tài)變量，EKF的狀態(tài)方程可寫為：

其中，α —乘性姿態(tài)誤差；

δb—陀螺儀零漂；

wα—乘性姿態(tài)誤差的狀態(tài)噪聲；

wb—陀螺儀零漂的狀態(tài)噪聲；

Ω×—角速度 ω 的叉乘矩陣（即 Ω×α=ω × α）。

EKF的量測(cè)方程如下：

vg—重力的量測(cè)噪聲；

vh—磁場(chǎng)的量測(cè)噪聲。

2、運(yùn)動(dòng)加速度估計(jì)及補(bǔ)償方法

可將運(yùn)動(dòng)加速度作為EKF狀態(tài)向量的一部分，由EKF進(jìn)行估計(jì)[6-8]；也可將運(yùn)動(dòng)加速度的估計(jì)置于EKF外部[9,10]。由此可將運(yùn)動(dòng)加速度的估計(jì)與補(bǔ)償方法分為內(nèi)部估計(jì)法和外部估計(jì)法。

記運(yùn)動(dòng)加速度為a，將a作為狀態(tài)變量，則內(nèi)部估計(jì)法的EKF狀態(tài)方程為：

其中，wacc—a的狀態(tài)噪聲。

狀態(tài)向量中加入運(yùn)動(dòng)加速度后，EKF的量測(cè)方程變?yōu)椋?/p>

另一方面，外部估計(jì)法不改變EKF結(jié)構(gòu)設(shè)計(jì)，即仍采用式（1）與式（2）實(shí)現(xiàn)EKF算法。

在k時(shí)刻，利用上一時(shí)刻的運(yùn)動(dòng)加速度估計(jì)值對(duì)加速度計(jì)讀數(shù)（比力）進(jìn)行修正，得到當(dāng)前時(shí)刻的重力觀測(cè)值

綜上所述，外部估計(jì)法通過不斷地交替利用式（5）與式（6），即可實(shí)現(xiàn)運(yùn)動(dòng)加速度的估計(jì)與補(bǔ)償。

3、討論

內(nèi)部估計(jì)法由于將運(yùn)動(dòng)加速度納入EKF狀態(tài)向量中，使得濾波器維數(shù)增加，因而所需存儲(chǔ)量和計(jì)算量都會(huì)隨之顯著增大。相比之下，外部估計(jì)法只需計(jì)算式（5）與式（6），對(duì)存儲(chǔ)量與計(jì)算量的影響很小。因此，外部估計(jì)法比內(nèi)部估計(jì)法更簡(jiǎn)便易行。

內(nèi)部估計(jì)法和外部估計(jì)法實(shí)際上都將運(yùn)動(dòng)加速度序列｛ak｝作為白噪聲驅(qū)動(dòng)的隨機(jī)游走過程處理，即任意相鄰時(shí)刻的運(yùn)動(dòng)加速度改變量為正態(tài)隨機(jī)變量：

在動(dòng)態(tài)條件下，載體的運(yùn)動(dòng)加速度在不斷變化，因此估計(jì)算法本身必須能很好地跟蹤運(yùn)動(dòng)加速度的變化。這是影響運(yùn)動(dòng)加速度估計(jì)與補(bǔ)償效果的關(guān)鍵因素之一。

對(duì)于內(nèi)部估計(jì)法，式（3）中的wacc即反映了運(yùn)動(dòng)加速度變化的劇烈程度。因此，wacc的協(xié)方差取值必須足夠大，才能保證運(yùn)動(dòng)加速度的估計(jì)值能夠較好地跟蹤載體運(yùn)動(dòng)狀態(tài)的實(shí)際變化。

對(duì)于外部估計(jì)法，采用式（6）修正加速度計(jì)讀數(shù)時(shí)，實(shí)際上使得運(yùn)動(dòng)加速度的估計(jì)誤差疊加在重力的量測(cè)噪聲之上。因此，外部估計(jì)法的效果取決于EKF算法本身在量測(cè)噪聲作用下的穩(wěn)定性與收斂性。

一個(gè)值得重視的問題是，不論內(nèi)部估計(jì)法或外部估計(jì)法，當(dāng)運(yùn)動(dòng)加速度估計(jì)值存在誤差時(shí)，該誤差將持續(xù)影響重力觀測(cè)值，并進(jìn)而導(dǎo)致姿態(tài)誤差的累積和發(fā)散。

應(yīng)當(dāng)注意，ρ的取值應(yīng)小于1而接近于1。若ρ取值過小，雖可使的誤差快速衰減，但也會(huì)使得運(yùn)動(dòng)加速度的補(bǔ)償效果變差，甚至完全失去補(bǔ)償效果。

三、仿真驗(yàn)證

1、仿真條件

仿真中，設(shè)載體沿x軸做變速直線運(yùn)動(dòng)，加速度變化規(guī)律如圖1所示。

AHRS各傳感器采樣頻率均為50Hz，其中.加速度計(jì)噪聲標(biāo)準(zhǔn)差設(shè)為0.01m/s2，磁強(qiáng)計(jì)噪聲標(biāo)準(zhǔn)差為0.1μT，陀螺儀噪聲標(biāo)準(zhǔn)差為0.05°/s，陀螺儀零漂為一階隨機(jī)游走過程（驅(qū)動(dòng)噪聲標(biāo)準(zhǔn)差為0.05°/s2）。

重力矢量取為：g=(0 0 9.8)T(m/s2)

地磁矢量取為：h=(40 0 30)T(μT)

按式（1）構(gòu)造EKF算法，對(duì)運(yùn)動(dòng)加速度不作補(bǔ)償，仿真得到的航姿誤差變化曲線如圖2所示。其中，航向角、俯仰角、橫滾角的均方誤差（RMSE）依次為0.05°、1.23°和0.03°，可見沿x軸的運(yùn)動(dòng)加速度主要影響俯仰角精度。

2、內(nèi)部估計(jì)法仿真

表1給出了不同參數(shù)設(shè)定下，內(nèi)部估計(jì)法的仿真結(jié)果，包括航向角、俯仰角與橫滾角的均方誤差。

表1 內(nèi)部估計(jì)法航姿誤差（RMSE）

由表1可見，當(dāng)運(yùn)動(dòng)加速度協(xié)方差σacc取0.01m/s2且未采用遺忘因子（即ρ=1）時(shí)，航姿誤差很大，表明內(nèi)部估計(jì)法此時(shí)起不到補(bǔ)償運(yùn)動(dòng)加速度干擾的作用。對(duì)應(yīng)的航姿誤差曲線如圖3所示。

如前所述，造成這一現(xiàn)象的原因有二：過小的σacc使得EKF無法跟隨載體運(yùn)動(dòng)狀態(tài)的變化，導(dǎo)致運(yùn)動(dòng)加速度估計(jì)值出現(xiàn)較大誤差；運(yùn)動(dòng)加速度的估計(jì)誤差會(huì)隨時(shí)間而累積和發(fā)散。

適當(dāng)增大σacc并引入遺忘因子ρ有利于改善補(bǔ)償效果。但由表1可見，過大的σacc（1m/s2）或過小的ρ（0.9）均會(huì)使得航姿誤差不減反增。ρ=0.95且σacc=0.1 m/s2時(shí)的補(bǔ)償效果最好，此時(shí)的航姿誤差變化曲線如圖4所示。

圖4所示航姿誤差與無補(bǔ)償?shù)那樾危磮D2）相比，雖然俯仰角的誤差得到了較好的抑制，然而航向角和橫滾角的均方誤差卻超過1°，說明內(nèi)部估計(jì)法的補(bǔ)償效果并不十分理想。

3、外部估計(jì)法仿真

表2為外部估計(jì)法仿真結(jié)果。與表1對(duì)比可知，外部估計(jì)法的航姿誤差總體上明顯優(yōu)于內(nèi)部估計(jì)法。

表2 外部估計(jì)法航姿誤差（RMSE）

圖5和圖6分別為ρ=1和ρ=0.95時(shí)的航姿誤差曲線，可見引入遺忘因子有助于避免誤差的累積。

四、轉(zhuǎn)臺(tái)實(shí)驗(yàn)

采用基于MPU9250傳感器的AHRS進(jìn)行實(shí)驗(yàn)驗(yàn)證。MPU9250本身由3軸加速度計(jì)、3軸磁強(qiáng)計(jì)和3軸陀螺儀構(gòu)成，故又稱為9軸傳感器。

將AHRS固定于速率轉(zhuǎn)臺(tái)上，使其y軸指向轉(zhuǎn)臺(tái)軸心，x軸沿切線方向，且x、y軸均在水平面內(nèi)。轉(zhuǎn)臺(tái)繞豎直軸轉(zhuǎn)動(dòng)，AHRS與轉(zhuǎn)軸的距離約為0.5m。

實(shí)驗(yàn)中，轉(zhuǎn)臺(tái)首先由靜止加速至120°/s，角加速度設(shè)定為10°/s2（即加速過程歷時(shí)12s），而后保持120°/s勻速旋轉(zhuǎn)12s，再以-10°/s2的角加速度減速至靜止。在此過程中，x軸與y軸的加速度計(jì)讀數(shù)分別對(duì)應(yīng)圓周運(yùn)動(dòng)的切向與法向加速度，如圖7所示。

傳感器采樣數(shù)據(jù)分別采用無補(bǔ)償EKF算法、內(nèi)部估計(jì)法以及外部估計(jì)法進(jìn)行航姿解算后，航姿誤差（均方根）如表3所示。由于實(shí)驗(yàn)中法向加速度較大、切向加速度相對(duì)較小，因而橫滾角受影響最大，其次是航向角（因求解航向角需先計(jì)算地磁場(chǎng)水平分量），未補(bǔ)償運(yùn)動(dòng)加速度時(shí)這兩者的均方誤差都達(dá)到4°以上。分別采用內(nèi)部估計(jì)法和外部估計(jì)法對(duì)運(yùn)動(dòng)加速度進(jìn)行估計(jì)和補(bǔ)償后，航姿誤差得到明顯改善。

表3 AHRS實(shí)測(cè)數(shù)據(jù)濾波結(jié)果

盡管內(nèi)部估計(jì)法與外部估計(jì)法補(bǔ)償效果十分相近，但后者的計(jì)算量遠(yuǎn)小于前者，因而更為便捷實(shí)用。

五、結(jié)束語(yǔ)

本文對(duì)EKF姿態(tài)估計(jì)算法中的兩種運(yùn)動(dòng)加速度補(bǔ)償方法——內(nèi)部估計(jì)法和外部估計(jì)法進(jìn)行了分析和驗(yàn)證，通過參數(shù)調(diào)節(jié)改善了補(bǔ)償效果并保證了誤差的收斂性。仿真和實(shí)驗(yàn)結(jié)果表明，外部估計(jì)法性能更為穩(wěn)定，能很好地消除運(yùn)動(dòng)加速度對(duì)航姿精度的不利影響，且其所需存儲(chǔ)量和計(jì)算量遠(yuǎn)小于內(nèi)部估計(jì)法，具有很好的實(shí)用性。