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

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

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

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

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

1 AHRS問題提出

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

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

1.1 AHRS動(dòng)力學(xué)模型

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

(1)

即

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

(2)

式中

1.2 加速度計(jì)觀測模型

圖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)擴(kuò)展卡爾曼濾波器設(shè)計(jì)

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

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

狀態(tài)方程為

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

(5)

量測方程為

Zk=HkXk+Vk

(6)

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

要求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í),離散四元數(shù)微分方程為

(12)

或者

(13)

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

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

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

(14)

相應(yīng)的,實(shí)際狀態(tài)轉(zhuǎn)移矩陣與真實(shí)狀態(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é)方差矩陣

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

nacc=a-a°

(20)

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

實(shí)際量測矩陣與理想量測矩陣誤差為

(21)

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

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

(22)

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

(23)

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

(24)

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

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

(25)

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

(26)

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

(27)

則有

(28)

(29)

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

(30)

(31)

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

(32)

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

3.1 ARHS傳感器

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

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

圖4 AH-100B AHRS傳感器

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

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

(33)

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

(34)

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

完成參數(shù)設(shè)置后,在ROS平臺下進(jìn)行了實(shí)驗(yà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之間波動(dòng),分量y在0.005～0.007之間波動(dòng),分量z在-0.114～-0.110之間波動(dòng)。

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

3.3 動(dòng)態(tài)姿態(tài)測試

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

圖6 AHRS中輸入抖動(dòng)脈沖

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

3.4 不同算法性能比較

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

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

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

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

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

4 結(jié) 論

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