基于改進(jìn)自適應(yīng)擴(kuò)展卡爾曼濾波的高精度姿態(tài)解算

吳葉麗,行鴻彥,侯天浩,王海峰,李 瑾

(南京信息工程大學(xué)電子與信息工程學(xué)院,江蘇 南京 210044)

0 引言

高精度姿態(tài)角度解算對(duì)移動(dòng)目標(biāo)自主定位導(dǎo)航有著重要的應(yīng)用價(jià)值[1-3]。姿態(tài)角度可以通過陀螺儀傳感器[4]的積分得到,但是所測(cè)數(shù)據(jù)在長(zhǎng)時(shí)間積分環(huán)境下容易產(chǎn)生累積誤差,單個(gè)的傳感器采集到的數(shù)據(jù)信息較為單一,不具有可靠性,并且進(jìn)行姿態(tài)解算的過程中,傳感器采集的數(shù)據(jù)中會(huì)存在不確定性的系統(tǒng)噪聲和測(cè)量噪聲,導(dǎo)致得到的位姿結(jié)果產(chǎn)生較大的偏差。而采用陀螺儀傳感器、加速度傳感器和地磁計(jì)對(duì)姿態(tài)變換情況共同進(jìn)行跟蹤可有效解決上述問題,確保采集的數(shù)據(jù)具有充分性和可靠性,通過將三個(gè)傳感器的數(shù)據(jù)進(jìn)行信息融合,實(shí)現(xiàn)較為準(zhǔn)確的姿態(tài)估算。

國(guó)內(nèi)外學(xué)者對(duì)多個(gè)應(yīng)用背景下的姿態(tài)解算進(jìn)行了大量研究[5-7]。文獻(xiàn)[8]提出一種結(jié)合共軛梯度法和互補(bǔ)濾波的混合濾波算法,實(shí)現(xiàn)慣性傳感器的姿態(tài)解算,但沒有充分考慮傳感器數(shù)據(jù)的影響、算法迭代復(fù)雜且計(jì)算量較大。文獻(xiàn)[9]通過對(duì)多個(gè)慣性傳感器進(jìn)行空間陣列布局,并設(shè)計(jì)了基于BP神經(jīng)網(wǎng)絡(luò)的誤差補(bǔ)償模型,減少姿態(tài)的誤差,一定程度上提高了姿態(tài)解算的準(zhǔn)確性,但該方法姿態(tài)解算過程中增加了較多計(jì)算參數(shù),解算實(shí)現(xiàn)步驟較為復(fù)雜。文獻(xiàn)[10]提出了一種基于優(yōu)化自適應(yīng)無跡卡爾曼濾波的姿態(tài)解算方法,通過對(duì)陀螺儀數(shù)據(jù)和加速度數(shù)據(jù)進(jìn)行誤差預(yù)處理,再通過所提算法實(shí)現(xiàn)姿態(tài)角度解算,但在濾波過程中沒有考慮誤差的迭代更新過程。文獻(xiàn)[11]提出了一種基于Q學(xué)習(xí)的方法,可以自動(dòng)調(diào)整過程和測(cè)量噪聲協(xié)方差矩陣的值,解決了迭代過程中的誤差影響,降低了姿態(tài)解算誤差,但同時(shí)更新過程和測(cè)量噪聲協(xié)方差矩陣和容易造成姿態(tài)發(fā)散且計(jì)算量增加。文獻(xiàn)[12]將互補(bǔ)濾波器與EKF算法相結(jié)合,提出了一種改進(jìn)的EKF濾波算法實(shí)現(xiàn)對(duì)無人機(jī)的姿態(tài)解算,一定程度上提高了精度和實(shí)時(shí)性,但是該方法需要進(jìn)行兩步濾波過程,計(jì)算量大且容易導(dǎo)致結(jié)果發(fā)散,仍需要對(duì)解算精度進(jìn)行提高。

為解決已有方法的姿態(tài)解算精度較低和計(jì)算復(fù)雜等問題,本文通過設(shè)置權(quán)重值對(duì)測(cè)量噪聲協(xié)方差矩陣R的估計(jì)公式進(jìn)行改進(jìn),提出一種改進(jìn)加權(quán)自適應(yīng)的擴(kuò)展卡爾曼濾波算法應(yīng)用于姿態(tài)解算。

1 優(yōu)化自適應(yīng)的擴(kuò)展卡爾曼濾波算法

傳統(tǒng)的EKF模型要求過程噪聲和測(cè)量噪聲的均值為零且統(tǒng)計(jì)特性已知的白噪聲。但在動(dòng)態(tài)實(shí)踐中無法滿足上述要求,也無法準(zhǔn)確地對(duì)噪聲的統(tǒng)計(jì)特性進(jìn)行描述。Sage-Husa自適應(yīng)濾波方法可有效顧及實(shí)際過程中的測(cè)量信息和動(dòng)態(tài)信息,可以不以動(dòng)力學(xué)模型噪聲先驗(yàn)協(xié)方差矩陣為基礎(chǔ),就能估計(jì)出狀態(tài)向量和測(cè)量向量的協(xié)方差矩陣,可有效進(jìn)行濾波。

1.1 Sage-Husa自適應(yīng)濾波算法

Sage-Husa濾波利用開窗法估計(jì)出此刻的測(cè)量噪聲協(xié)方差矩陣或過程噪聲協(xié)方差矩陣,使過程噪聲協(xié)方差矩陣和測(cè)量噪聲協(xié)方差矩陣自適應(yīng)于此時(shí)的狀態(tài)信息和測(cè)量信息,結(jié)合EKF濾波進(jìn)行解算,進(jìn)而獲得最優(yōu)的狀態(tài)估計(jì)值。

EKF的非線性系統(tǒng)表達(dá)式為

(1)

對(duì)于非線性的EKF系統(tǒng)線性化后其系統(tǒng)方程為

(2)

式(2)中,X(k)為狀態(tài)向量;Z(k)為觀測(cè)向量;A為狀態(tài)轉(zhuǎn)移矩陣,是狀態(tài)向量非線性函數(shù)泰勒級(jí)數(shù)展開式的一階項(xiàng);H為觀測(cè)矩陣;w(k)為過程噪聲;v(k)為測(cè)量噪聲。

由上,基于Sage-Husa的時(shí)變?cè)肼暯y(tǒng)計(jì)估計(jì)器[13]為

q(k)=(1-d(k))q(k-1)+
d(k)(X(k|k)-AX(k-1|k-1)),

(3)

Q(k)=(1-d(k))Q(k-1)+d(k)(K(k)ε(k)·
ε(k)TK(k)T+P(k|k)-AP(k-1|k-1)AT),

(4)

r(k)=(1-d(k))r(k-1)+d(k)(Z(k)-
HX(k|k-1)),

(5)

R(k)=(1-d(k))R(k-1)+d(k)(ε(k)ε(k)T-
HP(k|k-1)HT),

(6)

但傳統(tǒng)的Sage-Husa自適應(yīng)濾波方法存在一些缺點(diǎn):

1) 由文獻(xiàn)[14]可知,在實(shí)踐過程中不能同時(shí)估計(jì)出系統(tǒng)中的Q和R,只能在一方已知的條件下,估計(jì)出另一個(gè);

2) 由式(3)-式(6)可知,d(k)和b的數(shù)值選取,直接影響了系統(tǒng)的自適應(yīng)程度,傳統(tǒng)的Sage-Husa自適應(yīng)方法,使用單一的d(k)數(shù)值對(duì)不同傳感器測(cè)得的數(shù)據(jù)進(jìn)行噪聲估計(jì),不能滿足實(shí)際的要求,容易產(chǎn)生較大的誤差;

3) 由式(4)、式(6)可知,系統(tǒng)的噪聲方差矩陣Q或R可能失去半正定性和正定性發(fā)生濾波發(fā)散,導(dǎo)致系統(tǒng)不具有穩(wěn)定性和收斂性。因此,本文需要針對(duì)上述問題對(duì)傳統(tǒng)的Sage-Husa自適應(yīng)濾波方法進(jìn)行改進(jìn)。

1.2 優(yōu)化加權(quán)自適應(yīng)的擴(kuò)展卡爾曼濾波算法

針對(duì)在實(shí)踐過程中不能同時(shí)估計(jì)出系統(tǒng)中的Q和R的問題,本文實(shí)現(xiàn)姿態(tài)解算的系統(tǒng)的過程噪聲較小并且變化較為穩(wěn)定,而噪聲通常受到復(fù)雜的環(huán)境影響,使得實(shí)際過程中的測(cè)量噪聲會(huì)發(fā)生較大的變化。因此,本文僅對(duì)系統(tǒng)的測(cè)量噪聲協(xié)方差R進(jìn)行估計(jì),對(duì)過程噪聲協(xié)方差矩陣Q設(shè)置為已知定值,并且對(duì)測(cè)量噪聲協(xié)方差矩陣進(jìn)行改進(jìn),提高系統(tǒng)的穩(wěn)定性和準(zhǔn)確性。由此,本文提出一種改進(jìn)的加權(quán)Sage-Husa自適應(yīng)擴(kuò)展卡爾曼濾波算法。算法流程如下:

狀態(tài)預(yù)測(cè)方程:

X(k|k-1)=f(X(k-1|k-1))。

(7)

預(yù)測(cè)協(xié)方差方程:

P(k|k-1)=AP(k-1|k-1)AT+Q。

(8)

更新增益方程:

K(k)=P(k|k-1)HT[HP(k|k-1)HT+R]-1。

(9)

計(jì)算殘差:

e(k)=Z(k)-g(X(k|k-1))。

(10)

更新狀態(tài)估計(jì)方程:

X(k|k)=X(k|k-1)+K(k)e(k)。

(11)

更新協(xié)方差矩陣方程:

P(k|k)=(I-K(k)H)P(k|k-1)。

(12)

對(duì)不同的傳感器的噪聲統(tǒng)計(jì)特性的置信程度進(jìn)行判斷,對(duì)不同傳感器測(cè)量所得的數(shù)據(jù)賦不同的權(quán)重值,可以有效解決單一的d(k)數(shù)值對(duì)不同傳感器測(cè)得的數(shù)據(jù)進(jìn)行噪聲估計(jì)而導(dǎo)致產(chǎn)生誤差的問題。

濾波出現(xiàn)收斂趨勢(shì)時(shí),誤差協(xié)方差矩陣P逐漸減小至0,可忽略不計(jì),對(duì)傳統(tǒng)的測(cè)量噪聲協(xié)方差矩陣R的估計(jì)公式進(jìn)行改進(jìn),保留殘差e(k),同時(shí)使其滿足傳統(tǒng)R的變化趨勢(shì),保證了矩陣R的正定性,防止發(fā)生濾波發(fā)散的情況,使R隨著噪聲和系統(tǒng)真實(shí)情況的變換進(jìn)行估計(jì)。

改進(jìn)的加權(quán)測(cè)量噪聲協(xié)方差矩陣更新方程:

R(k)=(I-w(k)D(k))R(k-1)+w(k)D(k)·
|((I-HK(k-1))e(k)e(k)T(I-HK(k-1))T)|,

(13)

綜上所述,改進(jìn)算法結(jié)構(gòu)簡(jiǎn)單、計(jì)算效率高、穩(wěn)定性好、靈活性高,更適用于系統(tǒng)求解姿態(tài)角度信息。

2 基于優(yōu)化加權(quán)自適應(yīng)擴(kuò)展卡爾曼濾波的姿態(tài)解算

姿態(tài)角一般由陀螺儀、加速度計(jì)傳感器的測(cè)量數(shù)據(jù)解算得到[15],陀螺儀可由所測(cè)角速度經(jīng)積分得到三個(gè)姿態(tài)角:俯仰角、航向角和橫滾角,其不易受到外界環(huán)境的干擾,且短時(shí)內(nèi)的測(cè)量值較為準(zhǔn)確但單一的陀螺儀測(cè)得數(shù)據(jù)在長(zhǎng)時(shí)間積分環(huán)境下容易產(chǎn)生累積誤差,而加速度計(jì)和磁力計(jì)長(zhǎng)時(shí)間測(cè)量的數(shù)據(jù)比較穩(wěn)定、準(zhǔn)確。因此,本文利用提出的WAEKF方法將陀螺儀、加速度計(jì)和地磁計(jì)的測(cè)量數(shù)據(jù)進(jìn)行了融合,實(shí)現(xiàn)對(duì)陀螺儀實(shí)踐過程中產(chǎn)生的隨機(jī)偏移誤差的修正,在EKF算法的基礎(chǔ)上,加入自適應(yīng)過程,控制系統(tǒng)的殘差矩陣對(duì)系統(tǒng)的影響,進(jìn)而得到姿態(tài)角的最優(yōu)估計(jì)。其實(shí)現(xiàn)原理如圖1所示。

圖1 基于改進(jìn)方法的姿態(tài)角度估計(jì)原理圖Fig.1 Schematic diagram of attitude angle estimation based on the improved method

2.1 基于慣性導(dǎo)航的姿態(tài)解算原理

姿態(tài)解算的方法主要有歐拉法、方向余弦法和四元數(shù)法等。歐拉法的計(jì)算速度較慢,并且計(jì)算過程中矩陣會(huì)產(chǎn)生“奇點(diǎn)”,造成萬(wàn)向節(jié)死鎖的現(xiàn)象[16],實(shí)時(shí)計(jì)算困難,難以用于實(shí)踐。方向余弦法可有效避免歐拉法出現(xiàn)的“奇點(diǎn)”現(xiàn)象,但計(jì)算量較大、較為復(fù)雜。因此本文在姿態(tài)融合過程中使用四元數(shù)法,可克服歐拉法的缺點(diǎn)并且方法簡(jiǎn)單、計(jì)算量較小。最終將四元數(shù)轉(zhuǎn)換為歐拉角,得到姿態(tài)角信息。

若載體坐標(biāo)系為B系,地理坐標(biāo)系為N系,兩者可通過旋轉(zhuǎn)矩陣進(jìn)行轉(zhuǎn)換,由四元數(shù)表示的從地理坐標(biāo)系旋轉(zhuǎn)到載體坐標(biāo)系的旋轉(zhuǎn)矩陣[17]為

(14)

則由旋轉(zhuǎn)矩陣解算出目標(biāo)載體的歐拉角,即俯仰角γ、橫滾角θ和航向角φ為

(15)

2.2 基于慣導(dǎo)和改進(jìn)方法的姿態(tài)解算

基于慣導(dǎo)系統(tǒng)和四元數(shù)法構(gòu)建狀態(tài)方程和測(cè)量方程,將四元數(shù)和陀螺儀角速度偏移量作為狀態(tài)方程的變量,將加速度計(jì)和地磁計(jì)測(cè)得的數(shù)據(jù)作為測(cè)量信息,加入自適應(yīng)因子考慮系統(tǒng)的測(cè)量噪聲對(duì)系統(tǒng)精度的影響,提高系統(tǒng)姿態(tài)角最優(yōu)估計(jì)的準(zhǔn)確性。

2.2.1建立狀態(tài)方程

使用一階龍格-庫(kù)塔法對(duì)陀螺儀的角速度值進(jìn)行更新處理,實(shí)現(xiàn)四元數(shù)值的更新,則由龍格-庫(kù)塔法得到的離散模型[18]為

(16)

式(16)中,q(k),q(k-1)分別為k,k-1時(shí)刻的四元數(shù);T為系統(tǒng)采樣時(shí)間間隔;ΩB為載體坐標(biāo)系相對(duì)于地理坐標(biāo)系的角速度在載體坐標(biāo)系上的分量。

陀螺儀的偏移量模型可表示為

(18)

則,以四元數(shù)和陀螺儀數(shù)據(jù)偏移量作為狀態(tài)變量,進(jìn)而建立的狀態(tài)方程為

(19)

2.2.2建立測(cè)量方程

本文將加速度計(jì)和地磁計(jì)得到的數(shù)據(jù)作為測(cè)量方程,對(duì)狀態(tài)向量實(shí)現(xiàn)補(bǔ)償估計(jì),設(shè)加速度計(jì)數(shù)據(jù)為aB,地磁計(jì)所測(cè)數(shù)據(jù)為mB,與地理坐標(biāo)系中的加速度和磁場(chǎng)mN關(guān)系[19]可表示為

(20)

通過地磁和加速度計(jì)解算所得的姿態(tài)角經(jīng)過求解得到初始四元數(shù)的數(shù)值,進(jìn)而實(shí)現(xiàn)四元數(shù)的更新優(yōu)化,由式(20)求解可得由地磁計(jì)和加速度計(jì)數(shù)值表示的姿態(tài)角的表達(dá)方式為

(21)

選取加速度計(jì)和地磁計(jì)的數(shù)據(jù)向量作為測(cè)量信息,因此本系統(tǒng)的測(cè)量方程表達(dá)式為

(22)

2.2.3姿態(tài)估計(jì)的實(shí)現(xiàn)步驟

本文改進(jìn)WAEKF方法可以更新時(shí)變?cè)肼暰仃噮?shù)R,因此,通過本文方法實(shí)現(xiàn)姿態(tài)角度解算,可有效解決姿態(tài)解算過程中的誤差累積問題,提高姿態(tài)解算精度。將地磁計(jì)的測(cè)量數(shù)據(jù)、加速度傳感器的測(cè)量數(shù)據(jù)和陀螺儀傳感器的測(cè)量數(shù)據(jù)經(jīng)本文方法解算處理后,得到最優(yōu)姿態(tài)。

算法具體的實(shí)現(xiàn)步驟如下:

1) 建立系統(tǒng)的狀態(tài)方程(19)和測(cè)量方程(22),并對(duì)系統(tǒng)進(jìn)行初始化設(shè)置;

2) 根據(jù)式(7)、式(8)實(shí)現(xiàn)時(shí)間更新,得到預(yù)測(cè)狀態(tài)變量X(k|k-1)和預(yù)測(cè)協(xié)方差矩陣P(k|k-1);

3) 根據(jù)式(9)更新濾波增益,利用增益K(k)更新k時(shí)刻的最優(yōu)狀態(tài)估計(jì)X(k|k)和協(xié)方差矩陣P(k|k);

4) 重復(fù)上述步驟2)至3),實(shí)現(xiàn)四元數(shù)的最優(yōu)估計(jì),得到最優(yōu)姿態(tài)角度信息。

本文算法通過使用改進(jìn)的噪聲統(tǒng)計(jì)估計(jì)器,能夠有效避免濾波發(fā)散,并且能夠?qū)崟r(shí)地估計(jì)和修正系統(tǒng)的量測(cè)噪聲,實(shí)現(xiàn)系統(tǒng)的最優(yōu)估計(jì)。

3 實(shí)驗(yàn)與結(jié)果分析

基于MPU9250搭建移動(dòng)小車實(shí)驗(yàn)平臺(tái),將慣性傳感器在二維水平面內(nèi)進(jìn)行移動(dòng),利用算法實(shí)現(xiàn)移動(dòng)小車的姿態(tài)融合解算,獲取更加準(zhǔn)確的姿態(tài)信息。基于本文的WAEKF方法實(shí)現(xiàn)陀螺儀數(shù)據(jù)、加速度數(shù)據(jù)和地磁計(jì)數(shù)據(jù)的姿態(tài)解算,并將實(shí)驗(yàn)的解算結(jié)果與傳統(tǒng)AEKF、EM-AEKF和互補(bǔ)濾波方法進(jìn)行對(duì)比分析,驗(yàn)證本文方法求解姿態(tài)角具有優(yōu)越性。

為直觀體現(xiàn)改進(jìn)方法的姿態(tài)解算實(shí)驗(yàn)效果,本文選取均方根誤差(root mean square error,RMSE)和平均絕對(duì)誤差(mean absolute error,MAE)對(duì)姿態(tài)角度誤差情況進(jìn)行衡量評(píng)估[20]。RMSE和MAE表達(dá)式如下:

(23)

(24)

式(23)、式(24)中,N表示所測(cè)數(shù)據(jù)個(gè)數(shù),x(t)表示系統(tǒng)的解算估計(jì)值,y(t)表示系統(tǒng)的真實(shí)數(shù)據(jù)。RMSE可以反映出數(shù)據(jù)值與真實(shí)值之間的擬合程度,RMSE的值越小,表明處理后的實(shí)驗(yàn)數(shù)據(jù)具有更高的精確性。MAE可凸顯實(shí)驗(yàn)值與真實(shí)值之間的數(shù)值差距,通過將所有的誤差絕對(duì)值相加,可以有效避免因數(shù)據(jù)值比真實(shí)值局部過高或過低而抵銷誤差,因此較適合用于評(píng)估數(shù)據(jù)的整體誤差。

3.1 與傳統(tǒng)AEKF算法的誤差對(duì)比

將慣性傳感器在二維水平面內(nèi)進(jìn)行直線運(yùn)動(dòng)。使用AEKF濾波器和本文的WAEKF方法對(duì)慣性測(cè)量單元數(shù)據(jù)進(jìn)行處理,并將實(shí)驗(yàn)的解算誤差結(jié)果進(jìn)行比較分析,采樣周期為0.02 s。

3.1.1傳統(tǒng)自適應(yīng)擴(kuò)展卡爾曼濾波的角度解算誤差實(shí)驗(yàn)分析

采用基于傳統(tǒng)的Sage-Husa自適應(yīng)擴(kuò)展卡爾曼濾波算法對(duì)傳感器的數(shù)據(jù)進(jìn)行融合,分別得到俯仰角、航向角和橫滾角的姿態(tài)角度信息,并將其與真實(shí)值之間的誤差情況進(jìn)行表現(xiàn),顯示出傳統(tǒng)算法的融合效果,效果如圖2所示。

圖2 基于AEKF姿態(tài)解算角度誤差曲線Fig.2 Angle error curve calculated based on AEKF attitude

由圖2可看出,基于AEKF的姿態(tài)解算結(jié)果存在較大誤差值。解算的橫滾角誤差極值差約為40°,在數(shù)列值為85 s附近出現(xiàn)誤差極大值,高出真實(shí)值33°,誤差變化較為明顯?；贏EKF姿態(tài)解算的俯仰角誤差波動(dòng)幅度值約為28°,誤差極大值高出實(shí)際值18°,誤差變化分布的較為明顯?；贏EKF解算的航向角的誤差值在75～85 s區(qū)間外整體變化較為平穩(wěn),但在75～85 s區(qū)間內(nèi)出現(xiàn)了異常尖峰誤差,誤差極值大于200°,對(duì)解算結(jié)果造成了較大影響。因此,傳統(tǒng)的AEKF對(duì)慣性傳感器數(shù)據(jù)進(jìn)行濾波解算后,姿態(tài)角度的誤差變化情況都較為明顯,誤差值變化區(qū)間較大并且都出現(xiàn)了一段異常尖峰值,因此仍需要對(duì)傳統(tǒng)的AEKF濾波算法進(jìn)行改進(jìn)。

3.1.2優(yōu)化加權(quán)自適應(yīng)擴(kuò)展卡爾曼濾波的角度解算誤差實(shí)驗(yàn)分析

在吸收Sage-Husa濾波和基于傳感器置信程度進(jìn)行加權(quán)的基礎(chǔ)上,本文提出一種新的WAEKF方法,用于改善傳統(tǒng)的AEKF濾波算法的不足,增加濾波融合的靈活性和有效性。

由圖3可知,基于WAEKF方法解算的姿態(tài)角誤差明顯小于AEKF的姿態(tài)解算結(jié)果,能夠更加準(zhǔn)確、有效地實(shí)現(xiàn)姿態(tài)解算。WAEKF方法解算的橫滾角的誤差整體變化較為平緩,無異常尖峰值段,相較于AEKF算法的誤差變化范圍減小約33°,橫滾角誤差變化程度有所控制。本文方法解算的俯仰角誤差波動(dòng)幅度值約為13°,誤差極大值高出實(shí)際值8°,相較于AEKF算法誤差極大值減小約10°。基于本文方法得到的航向角誤差波動(dòng)幅度值約為13°,遠(yuǎn)小于基于AEKF算法的航向角誤差數(shù)值范圍,并且整體誤差值偏小。

圖3 基于WAEKF姿態(tài)解算角度誤差曲線Fig.3 Angle error curve calculated based on WAEKF attitude

因此,基于WAEKF方法解算的姿態(tài)角度信息相較于傳統(tǒng)的AEKF算法的解算結(jié)果得到了有效地改善,尤其是航向角角度值變化更加準(zhǔn)確,說明基于WAEKF方法的姿態(tài)解能夠依靠改進(jìn)的測(cè)量噪聲協(xié)方差矩陣自適應(yīng)濾除噪聲,減小誤差,提高姿態(tài)解算精度。

3.2 姿態(tài)角度對(duì)比分析

為證明本文方法對(duì)姿態(tài)角度解算具有高效性,將慣性傳感器在二維水平面內(nèi)進(jìn)行直線運(yùn)動(dòng)。利用互補(bǔ)濾波、基于EM-AEKF和本文的WAEKF方法分別對(duì)傳感器數(shù)據(jù)進(jìn)行處理,直觀對(duì)比不同方法融合得到的角度變化情況,結(jié)果如圖4-圖6所示。w(k)取(0.5,0.95,0.95,0.95,0.99,0.99)。

圖4 不同方法解算的俯仰角角度變化圖Fig.4 Elevation angle variation figure calculated by different methods

由圖4可知,互補(bǔ)濾波方法解算后在時(shí)間序列200 s后得到的俯仰角隨著時(shí)間的推移,信號(hào)發(fā)生嚴(yán)重漂移,誤差逐漸增大,因此互補(bǔ)濾波方法較不適合俯仰角的融合解算?；贓M-AEKF方法解算的俯仰角在時(shí)間序列為0～50 s內(nèi)誤差較大,誤差波動(dòng)幅度值約為30°,因此該方法解算的俯仰角誤差依然較大?；赪AEKF方法融合解算后的俯仰角整體波動(dòng)最為平緩,誤差值相較于互補(bǔ)濾波和基于EM-AEKF方法最小。

由圖5可知,基于互補(bǔ)濾波方法解算的橫滾角在時(shí)間為100 s后逐漸下降低至-55°,互補(bǔ)濾波方法解算的橫滾角誤差較大?；贓M-AEKF方法解算的橫滾角在時(shí)間為0～50 s內(nèi)誤差較大,誤差極值約為-50°左右,且隨著時(shí)間的增加,橫滾角波動(dòng)較為明顯,因此該方法解算的橫滾角存在明顯失真。基于WAEKF方法解算的橫滾角整體波動(dòng)穩(wěn)定,誤差波動(dòng)幅度值約為20°,解算的橫滾角誤差遠(yuǎn)小于互補(bǔ)濾波和EM-AEKF方法。

圖5 不同方法解算的橫滾角角度變化圖Fig.5 Changes of roll angle calculated by different methods

由圖6可知,基于互補(bǔ)濾波方法在整個(gè)時(shí)間序列內(nèi)解算得到航向角的波形呈上升趨勢(shì),隨著數(shù)據(jù)的疊加,誤差逐漸增大,最大值偏離40°,因此互補(bǔ)濾波方法較為不適合本文數(shù)據(jù)下的航向角融合。基于EM-AEKF方法解算的航向角整體變化較為穩(wěn)定,但0～8 s之間存在較大異常值,因此該方法仍待改進(jìn)?；赪AEKF方法解算得到的航向角角度值在-109°～-94°之間,解算的航向角誤差相對(duì)最小,角度變化具有穩(wěn)定性和準(zhǔn)確性,因此可以用于實(shí)際應(yīng)用中求解航向角。

圖6 不同方法解算的航向角角度變化圖Fig.6 Variation of course angle calculated by different methods

綜上所述,基于互補(bǔ)濾波方法解算的俯仰角、橫滾角和航向角都存在明顯的漂移現(xiàn)象;基于EM-AEKF方法解算的俯仰角、橫滾角和航向角都存在較大的異常值。本文的WAEKF方法可有效解決互補(bǔ)濾波方法姿態(tài)解算存在漂移和EM-AEKF方法姿態(tài)解算存在異常值導(dǎo)致誤差較大的問題,WAEKF方法解算的俯仰角、橫滾角和航向角波形變化都較為平穩(wěn)且誤差較小,能夠提高姿態(tài)解算精度,較適用于實(shí)踐應(yīng)用。因此,WAEKF方法具有優(yōu)勢(shì)性和準(zhǔn)確性。

本文為對(duì)比四種方法下姿態(tài)角的解算效果,使用MAE對(duì)其誤差進(jìn)行衡量,并通過記錄計(jì)算時(shí)間衡量四種方法的實(shí)現(xiàn)效率,結(jié)果如表1所示。

表1 不同方法航向角解算誤差對(duì)比表Tab.1 Comparison table of heading angle calculation errors of different methods

從表1可知,本文的WAEKF方法解算得到的橫滾角的RMSE和MAE值明顯小于其他三種方法,EM-AEKF和互補(bǔ)濾波方法解算的橫滾角誤差較大。WAEKF方法解算的俯仰角的RMSE和MAE明顯小于EM-AEKF和互補(bǔ)濾波方法,略小于AEKF方法?；赪AEKF方法解算得到的航向角的RMSE和MAE值明顯小于其他三種方法,且相較于AEKF方法,解算得到的航向角的RMSE和MAE分別降低了84.23%,78.28%。

因此,本文的WAEKF方法相較于互補(bǔ)濾波、AEKF和EM-AEKF方法的姿態(tài)解算效果最好,誤差最小,能夠自適應(yīng)濾除噪聲影響,姿態(tài)解算精度最高。

為驗(yàn)證本文方法的姿態(tài)解算具有適用性,在上述相同實(shí)驗(yàn)條件下,將慣性傳感器在水平二維平面內(nèi)進(jìn)行多角度變化運(yùn)動(dòng),由于本文主要考慮載體在二維水平面內(nèi)的移動(dòng),此時(shí)只有航向角發(fā)生顯著變化,不同方法下的航向角解算結(jié)果如圖7所示。

圖7 多角度變化運(yùn)動(dòng)下航向角對(duì)比圖Fig.7 Comparison of heading angles under multi angle changing motion

由圖7可知,基于互補(bǔ)濾波方法解算的航向角誤差最大,發(fā)生明顯漂移。AEKF解算的航向角存在尖峰值,并且在4.4 s后解算的航向角存在明顯漂移誤差?；贓M-AEKF方法和本文的WAEKF方法解算的航向角值較為接近真實(shí)值,但在0.5～3.85 s之間,基于EM-AEKF方法解算的航向角略小于真實(shí)值,且波形存在異常尖峰值。基于WAEKF方法解算在多角度變化運(yùn)動(dòng)背景下解算航向角的效果最佳,解算的航向角波形變化平穩(wěn)且不存在異常尖峰值,同樣克服了互補(bǔ)濾波、AEKF及EM-AEKF方法解算的航向角存在異常尖峰和漂移等問題,在多角度變化運(yùn)動(dòng)背景下同樣適用。

綜上所述,可知基于WAEKF方法姿態(tài)解算精度整體優(yōu)于其他三種方法,姿態(tài)解算時(shí)能夠自適應(yīng)濾除噪聲影響,解算角度精度較高,具有優(yōu)越性和可行性。

4 結(jié)論

本文對(duì)測(cè)量噪聲協(xié)方差矩陣的估計(jì)公式進(jìn)行了改進(jìn),設(shè)置權(quán)重值對(duì)測(cè)量噪聲協(xié)方差矩陣進(jìn)行調(diào)節(jié),提出一種基于優(yōu)化加權(quán)自適應(yīng)的擴(kuò)展卡爾曼濾波方法實(shí)現(xiàn)姿態(tài)解算,解決了濾波發(fā)散的問題,提高姿態(tài)角度解算的精度。利用本文方法實(shí)現(xiàn)慣性傳感器測(cè)量數(shù)據(jù)的姿態(tài)解算,驗(yàn)證本文方法的有效性,對(duì)陀螺儀數(shù)據(jù)采用基于四元數(shù)的龍格-庫(kù)塔法求得姿態(tài)運(yùn)動(dòng)模型,建立狀態(tài)方程,將地磁計(jì)和加速度計(jì)的數(shù)據(jù)作為測(cè)量信息,利用優(yōu)化加權(quán)的自適應(yīng)因子調(diào)節(jié)測(cè)量噪聲協(xié)方差矩陣,控制測(cè)量殘差的影響,經(jīng)過不斷遞歸更新能夠自適應(yīng)濾除測(cè)量噪聲,估算出最優(yōu)的姿態(tài)角。實(shí)驗(yàn)表明,相較于傳統(tǒng)的自適應(yīng)擴(kuò)展卡爾曼濾波算法,本文方法解算姿態(tài)的誤差得到了較大的改善。