王錄, 劉明雍, 王夢凡, 郭嬌嬌, 雷罡, 張小件

(西北工業(yè)大學(xué) 航海學(xué)院, 陜西 西安 710072)

隨著微機電系統(tǒng)(MEMS)技術(shù)的高速發(fā)展,MEMS傳感器(陀螺儀、加速度計和磁力計)在移動設(shè)備(智能手機、無人智能系統(tǒng)等)領(lǐng)域的使用越來越普及。雖然MEMS傳感器的成本越來越低、集成度越來越高、體積越來越小[1],但是存在輸出噪聲過大、零點漂移無法完全消除、角速率輸出易受干擾等問題,因此可以采用多個傳感器通過信息融合算法[2]實現(xiàn)高精度的姿態(tài)解算。

針對AHRS傳感器在姿態(tài)解算過程中,輸出姿態(tài)存在噪聲較大及累積誤差等問題,文獻(xiàn)[3]提出了一種基于四元數(shù)的互補濾波算法,對于由MEMS陀螺儀解算的具有較大累積誤差的姿態(tài)四元數(shù),首先利用重力加速度信息對載體姿態(tài)的橫滾及俯仰分量進(jìn)行修正,其次利用地磁場信息對載體航向分量進(jìn)行修正。文獻(xiàn)[4]設(shè)計了一種基于四元數(shù)的擴展卡爾曼濾波算法,該算法分別利用陀螺儀輸出數(shù)據(jù)與加速度計輸出數(shù)據(jù)構(gòu)建狀態(tài)轉(zhuǎn)移矩陣與量測矩陣,不需求解雅克比矩陣,降低了構(gòu)建基于姿態(tài)矩陣的擴展卡爾曼濾波算法的復(fù)雜性。文獻(xiàn)[5]針對文獻(xiàn)[4]中量測噪聲協(xié)方差矩陣出現(xiàn)的奇異,加入了一個對角矩陣,有效克服了文獻(xiàn)[4]所提出的算法在工程應(yīng)用中的局限性。文獻(xiàn)[6-7]提出的基于姿態(tài)矩陣的擴展卡爾曼算法利用重力加速度與地磁場矢量構(gòu)建非線性量測方程,需要求解雅克比矩陣來獲得量測矩陣,大大增加了該算法的運算量。文獻(xiàn)[8]提出了一種通過利用擴展卡爾曼濾波器進(jìn)行數(shù)據(jù)融合以獲得姿態(tài)的準(zhǔn)確輸出,考慮加速度對系統(tǒng)精度的影響,解決了四元數(shù)協(xié)方差奇異性問題,其中的地磁場分量通過GPS測得的經(jīng)緯度和世界地磁場模型求出。文獻(xiàn)[9]提出了一種自適應(yīng)卡爾曼濾波算法,該算法自適應(yīng)估計加速度計量測噪聲協(xié)方差矩陣,用于補償載體在運動過程中產(chǎn)生的線性加速度誤差。

本文針對:①當(dāng)只利用加速度數(shù)據(jù)時,載體航向分量依然存在累積誤差;②當(dāng)加入磁力計數(shù)據(jù)時,由于環(huán)境對地磁場數(shù)據(jù)的影響極大[10],因此解算出的載體姿態(tài)對環(huán)境敏感,而且先驗地磁場矢量未知等問題,提出了一種自適應(yīng)的擴展卡爾曼濾波算法。首先由最小方差原則,利用加速度計數(shù)據(jù)對載體姿態(tài)的橫滾及俯仰分量進(jìn)行準(zhǔn)確估計;其次在上述估計的基礎(chǔ)之上,該算法結(jié)合磁力計數(shù)據(jù)對載體姿態(tài)的航向分量進(jìn)行了修正。優(yōu)點是在消除累積誤差的基礎(chǔ)上,能盡量降低姿態(tài)輸出噪聲,而且周圍環(huán)境的磁擾動對載體橫滾及俯仰分量不產(chǎn)生任何影響,僅對航向分量有一定的影響。

1 AHRS問題提出

本文提出的基于自適應(yīng)擴展卡爾曼濾波器的AHRS姿態(tài)解算算法是在擴展卡爾曼濾波算法的基礎(chǔ)之上,加入了實測地磁場信息,從而不斷修正載體姿態(tài)的航向累積誤差,如圖1所示。

圖1 AHRS姿態(tài)解算結(jié)構(gòu)框圖

1.1 AHRS動力學(xué)模型

描述AHRS動力學(xué)模型的方法有歐拉角法、方向余弦法、四元數(shù)法等。本文中,使用四元數(shù)微分方程表示AHRS動態(tài)模型[11]如下

(1)

即

利用四元數(shù)微分方程的比卡求解法,使(1)式離散化可得

(2)

式中

1.2 加速度計觀測模型

圖2為重力加速度在全局坐標(biāo)系G與載體坐標(biāo)系B中的示意圖,且規(guī)定重力加速度矢量為單位矢量,即

‖g‖=‖a‖=1

式中：g為在全局坐標(biāo)系G中測量的重力加速度；a為在載體坐標(biāo)系B中測量的重力加速度。

圖2 重力加速度示意圖

如果把單位矢量g與a看作零標(biāo)量的四元數(shù),則g與a間的變換關(guān)系可用四元數(shù)乘法表示為

g=q(tk)?a?q*(tk)

(3)

經(jīng)過變換可得

M′(a)q(tk)-M(g)q(tk)=0

(4)

式中,M(g)與M′(a)表示由零標(biāo)量的四元數(shù)g與a構(gòu)成的矩陣,參見文獻(xiàn)[11]。

2 自適應(yīng)擴展卡爾曼濾波器設(shè)計

本文所提出的自適應(yīng)擴展卡爾曼濾波算法的數(shù)學(xué)描述如(5)式、(6)式所示,其算法流程框圖如圖3所示。

圖3 自適應(yīng)擴展卡爾曼濾波算法流程框圖

狀態(tài)方程為

Xk=Φk/k-1αXk-1+Wk-1

(5)

量測方程為

Zk=HkXk+Vk

(6)

式中：Φk/k-1為tk-1～tk時刻的一步轉(zhuǎn)移矩陣;Hk表示量測矩陣;Vk和Wk分別表示激勵噪聲、量測噪聲。

要求Wk和Vk滿足[12]:

(7)

式中：Qk表示系統(tǒng)噪聲方差矩陣；Rk表示量測噪聲方差矩陣。

2.1 狀態(tài)轉(zhuǎn)移矩陣與量測矩陣

選取姿態(tài)四元數(shù)為狀態(tài)變量,則由(2)式可知,系統(tǒng)狀態(tài)方程的狀態(tài)轉(zhuǎn)移矩陣為

(8)

在(4)式中,令矩陣

(9)

向量

Zk=0

(10)

則(4)式可改寫為

Zk=Hkq(tk)

(11)

比較(6)式與(11)式,則系統(tǒng)量測矩陣為(9)式所示。

2.2 系統(tǒng)噪聲協(xié)方差矩陣

當(dāng)MEMS 陀螺儀不存在噪聲、零漂等因素的影響時,離散四元數(shù)微分方程為

(12)

或者

(13)

式中,ω°表示理想狀態(tài)下,陀螺儀輸出的角速率。

由于陀螺儀在微小時間間隔Δt內(nèi),其真實的角增量Δθ°無法測量。因此假設(shè)在第k個時間間隔內(nèi),陀螺儀的實際輸出與真實角增量的誤差為δθk,則有

Δθk/k-1=Δθ°k/k-1+δθk/k-1

(14)

相應(yīng)的,實際狀態(tài)轉(zhuǎn)移矩陣與真實狀態(tài)轉(zhuǎn)移矩陣的誤差為ΔΦk/k-1,即有

(15)

把(15)式代入(13)式可得

(16)

由文獻(xiàn)[4]可知

δΘk/k-1q(tk-1)=Ξ(q(tk-1))δθk/k-1

(17)

式中

設(shè)陀螺儀的輸出噪聲為ngyro,則有

(18)

則系統(tǒng)噪聲協(xié)方差矩陣為

(19)

式中,Σgyro為陀螺儀噪聲協(xié)方差矩陣。

2.3 量測噪聲協(xié)方差矩陣

量測噪聲主要由加速度計產(chǎn)生。設(shè)加速度計的輸出噪聲為nacc,則有

nacc=a-a°

(20)

式中,a°表示載體坐標(biāo)系下加速度計的真實值。

實際量測矩陣與理想量測矩陣誤差為

(21)

把(21)式代入(11)式可得

0=Hkq(tk)-ΔHkq(tk)

(22)

類似(17)式,-ΔHkq(tk)變形為

(23)

則量測噪聲協(xié)方差矩陣為

(24)

式中，Σacc為加速度計噪聲協(xié)方差矩陣。

(24)式得到的量測噪聲協(xié)方差矩陣Rk在應(yīng)用過程中很有可能是奇異的,為了避免矩陣的奇異,需要對該量測噪聲協(xié)方差矩陣Rk進(jìn)行改進(jìn)[5],即

(25)

2.4 自適應(yīng)矩陣因子

(26)

式中,矢量l表示在包含航向誤差的四元數(shù)坐標(biāo)變換下的地磁場矢量。假設(shè)真實的先驗地磁場矢量為

(27)

則有

(28)

(29)

把(29)式代入(28)式可得

(30)

(31)

由此可知,自適應(yīng)矩陣為

(32)

3 實驗驗證分析

3.1 ARHS傳感器

本文使用AH-100B AHRS傳感器對上述自適應(yīng)擴展卡爾曼濾波算法進(jìn)行了有效性驗證。該AH-100B傳感器集成了三軸陀螺、三軸加速度、三軸磁場計,輸出數(shù)據(jù)分別為ω,a和m。傳感器及特性參數(shù)分別如圖4和表1所示。

表1 AH-100B AHRS傳感器特性參數(shù)

圖4 AH-100B AHRS傳感器

3.2 靜態(tài)姿態(tài)測試

自適應(yīng)擴展卡爾曼濾波算法的初始狀態(tài)由文獻(xiàn)[1]中的算法給出,均方誤差初始矩陣為單位矩陣。在算法中,假設(shè)三軸陀螺儀輸出數(shù)據(jù)的方差相等,且

(33)

三軸加速度計輸出數(shù)據(jù)的方差相等,且

(34)

參數(shù)β選取為0.03。

完成參數(shù)設(shè)置后,在ROS平臺下進(jìn)行了實驗,并使用rqt-plot軟件繪制出載體姿態(tài)四元數(shù)輸出波形,如圖5所示。

圖5 姿態(tài)四元數(shù)輸出波形

在圖5中,姿態(tài)四元數(shù)的分量w在0.993～0.994之間變化,分量x在-0.004～-0.002之間波動,分量y在0.005～0.007之間波動,分量z在-0.114～-0.110之間波動。

通過上述分析可知,雖然姿態(tài)四元數(shù)的分量z所包含的噪聲比其他3個分量大,但是基本保持在同一水平線上,因此有效消除了陀螺儀姿態(tài)解算過程中的累積誤差。

3.3 動態(tài)姿態(tài)測試

為了驗證該自適應(yīng)擴展卡爾曼濾波算法在系統(tǒng)運動過程中的有效性,分別向搭載AH-100B航姿參考系統(tǒng)的ROS機器人平臺輸入三軸正交的抖動脈沖,如圖6所示。

圖6 AHRS中輸入抖動脈沖

圖7為分別向俯仰軸、橫滾軸及偏航向軸輸入正負(fù)抖動脈沖,載體姿態(tài)歐拉角的變化曲線。該圖中,當(dāng)系統(tǒng)輸入正抖動脈沖時,載體橫滾角、俯仰角及航向角都可以收斂到輸入抖動脈沖之前的姿態(tài);當(dāng)系統(tǒng)輸入負(fù)抖動脈沖時,3個姿態(tài)角同樣可以收斂到輸入抖動脈沖之前的姿態(tài)。因此該自適應(yīng)擴展卡爾曼濾波算法具有良好的動態(tài)性能。

3.4 不同算法性能比較

本文對上述文獻(xiàn)中提出的算法與自適應(yīng)擴展卡爾曼濾波算法進(jìn)行了比較,如圖8所示。

圖8a)、b)中,自適應(yīng)擴展卡爾曼濾波算法、互補濾波算法與擴展卡爾曼濾波算法的俯仰角及橫滾角輸出波形圖變化幅度較小,且比較平直。由于自適應(yīng)擴展卡爾曼濾波與擴展卡爾曼濾波都是通過重力加速度信息對系統(tǒng)俯仰角及橫滾角進(jìn)行修正,因此兩者的俯仰角及橫滾角變化曲線重合。相較于基于四元數(shù)的互補濾波算法,自適應(yīng)擴展卡爾曼濾波算法與擴展卡爾曼濾波算法中包含的噪聲較小。

圖8c)中,互補濾波器的航向角變化曲線在5°～10°范圍內(nèi)變化,精度大約為4°。擴展卡爾曼濾波算法的航向角變化曲線隨著時間的增加逐漸偏離初始航向角,具有較大的累積誤差。相較于上述2種算法,自適應(yīng)擴展卡爾曼濾波算法的航向角變化約為0.5°。綜上所述,自適應(yīng)卡爾曼濾波算法有效解決了載體姿態(tài)的累積誤差較大的問題,同時提高了姿態(tài)解算精度。

圖7 輸入抖動脈沖的姿態(tài)歐拉角變化曲線

圖8 不同算法姿態(tài)歐拉角輸出波形

4 結(jié) 論

針對AHRS傳感器中的MEMS陀螺儀在姿態(tài)解算過程中,輸出姿態(tài)存在噪聲較大及累積誤差等問題,本文提出了一種自適應(yīng)卡爾曼濾波算法。該算法融合了地球物理場(重力場,地磁場)信息,不但能夠消除陀螺儀在姿態(tài)解算過程中的累積誤差,也可以保證解算姿態(tài)的精度。并在ROS機器人平臺上進(jìn)行了驗證,試驗結(jié)果表明了該算法的有效性。