桂林電子科技大學 電子工程與自動化學院，廣西桂林 541004

一、引言

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

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

二、算法分析

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

采用EKF進行姿態(tài)估計，首先需選擇合適的姿態(tài)表示方式來構造狀態(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的量測方程如下：

vg—重力的量測噪聲；

vh—磁場的量測噪聲。

2、運動加速度估計及補償方法

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

記運動加速度為a，將a作為狀態(tài)變量，則內部估計法的EKF狀態(tài)方程為：

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

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

另一方面，外部估計法不改變EKF結構設計，即仍采用式（1）與式（2）實現(xiàn)EKF算法。

在k時刻，利用上一時刻的運動加速度估計值對加速度計讀數(shù)（比力）進行修正，得到當前時刻的重力觀測值

綜上所述，外部估計法通過不斷地交替利用式（5）與式（6），即可實現(xiàn)運動加速度的估計與補償。

3、討論

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

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

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

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

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

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

應當注意，ρ的取值應小于1而接近于1。若ρ取值過小，雖可使的誤差快速衰減，但也會使得運動加速度的補償效果變差，甚至完全失去補償效果。

三、仿真驗證

1、仿真條件

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

AHRS各傳感器采樣頻率均為50Hz，其中.加速度計噪聲標準差設為0.01m/s2，磁強計噪聲標準差為0.1μT，陀螺儀噪聲標準差為0.05°/s，陀螺儀零漂為一階隨機游走過程（驅動噪聲標準差為0.05°/s2）。

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

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

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

2、內部估計法仿真

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

表1 內部估計法航姿誤差（RMSE）

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

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

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

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

3、外部估計法仿真

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

表2 外部估計法航姿誤差（RMSE）

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

四、轉臺實驗

采用基于MPU9250傳感器的AHRS進行實驗驗證。MPU9250本身由3軸加速度計、3軸磁強計和3軸陀螺儀構成，故又稱為9軸傳感器。

將AHRS固定于速率轉臺上，使其y軸指向轉臺軸心，x軸沿切線方向，且x、y軸均在水平面內。轉臺繞豎直軸轉動，AHRS與轉軸的距離約為0.5m。

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

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

表3 AHRS實測數(shù)據濾波結果

盡管內部估計法與外部估計法補償效果十分相近，但后者的計算量遠小于前者，因而更為便捷實用。

五、結束語

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