捷聯(lián)慣導(dǎo)互補濾波姿態(tài)融合算法設(shè)計

杜 瑾,趙華超,鄭 哲,王 祥,司迎利

(中國空空導(dǎo)彈研究院,河南 洛陽 471000)

捷聯(lián)式慣導(dǎo)是當(dāng)今慣性導(dǎo)航的主流發(fā)展方向。捷聯(lián)算法的核心是捷聯(lián)姿態(tài)算法,姿態(tài)角的解算精度直接影響到整個系統(tǒng)的精度。由于本身成本低、功耗小,MEMS器件被廣泛應(yīng)用于姿態(tài)測量領(lǐng)域[1]。由于MEMS陀螺存在漂移,長時間工作有累積誤差,需要借助其他傳感器的輔助才能完成姿態(tài)測量。常用的輔助傳感器有加速度計、磁阻傳感器等[2-3]。加速度計和磁阻傳感器在靜態(tài)和無外磁場干擾情況下均能表現(xiàn)出良好的測量性能,然而,在動態(tài)和有磁場干擾的情況下,這兩種傳感器的測量精度會受到嚴(yán)重影響[4]。

可以看出,單獨使用某一種傳感器不能獲得精確可靠的姿態(tài)信息,必須利用姿態(tài)融合算法對不同傳感器的數(shù)據(jù)進(jìn)行融合,提高系統(tǒng)的精度和抗干擾能力。常用的融合算法有擴展卡爾曼濾波(EKF),無跡卡爾曼濾波(UKF),它們均從時域出發(fā),對系統(tǒng)噪聲精確建模[5-6]。這兩種方法計算量大,對MCU主頻要求高且模型建立比較復(fù)雜[7]。互補濾波(CF)與卡爾曼濾波相比,具有簡單方便,計算量小,易于工程實現(xiàn)等優(yōu)點[8]。

本文提出一種基于方向余弦矩陣(DCM)的多軸互補濾波姿態(tài)融合算法,利用加速度計和磁阻傳感器良好的靜態(tài)性能補償MEMS陀螺儀的漂移誤差。設(shè)計了靜態(tài)試驗與動態(tài)試驗,分別驗證了本算法的性能。

1 坐標(biāo)系描述

定義2個坐標(biāo)系:載體系(b系)與導(dǎo)航系(n系)。導(dǎo)航坐標(biāo)系(n系)采用北,東,地坐標(biāo)系,符合右手法則。載體坐標(biāo)系固聯(lián)于運載體,xb,yb,zb分別代表了運載體的前進(jìn)方向,右側(cè)和正下方。

2 方向余弦矩陣(DCM)

有許多方法可以描述運動坐標(biāo)系相對于參考坐標(biāo)系的方位關(guān)系,例如歐拉角法,四元數(shù)法和方向余弦矩陣(DCM)[9]。用DCM求解姿態(tài)角與四元數(shù)法相比更加直觀,可避免歐拉角法中大角度運動時的奇異性,實現(xiàn)全姿態(tài)工作[10]。從載體系到導(dǎo)航系的轉(zhuǎn)化可以通過三次旋轉(zhuǎn)實現(xiàn),依次繞z,x,y得到航向角ψ,滾動角φ和俯仰角θ。通過三次旋轉(zhuǎn),n系和b系的關(guān)系可以通過一個矩陣表達(dá)(見式(1)),即DCM。

(1)

DCM具有正交性和單位性兩大特征:矩陣中任意一對行或一對列向量都是相互垂直的,且內(nèi)積為零[11]。DCM是正交矩陣;DCM的任意列向量或行向量都是單位向量。

3 DCM的更新與姿態(tài)角的提取

當(dāng)時間間隔t趨向于無窮小時,航向,俯仰,滾轉(zhuǎn)角度的變化dψ、dθ、dφ趨向于0,此時,轉(zhuǎn)動與次序無關(guān)。DCM隨時間更新的關(guān)系式如下:

(2)

當(dāng)dt很小時,得到DCM的導(dǎo)數(shù):

(3)

DCM矩陣的更新過程如圖1所示。

圖1 DCM矩陣更新流程圖

得到DCM后,很容易推導(dǎo)出俯仰角,航向角,滾轉(zhuǎn)角的表達(dá)式:

每年國家及省級財政撥付資金用于蔬菜產(chǎn)業(yè)的發(fā)展以及從政策上對農(nóng)村農(nóng)民減免和補貼，但是部分地區(qū)還處于比較落后的貧困村，農(nóng)民自身無資金發(fā)展蔬菜產(chǎn)業(yè)，縣級財政也是捉襟見肘，無法配套及自籌剩余款項，對于落后的民族地區(qū)，以及薄弱的農(nóng)業(yè)基礎(chǔ)條件，無法起到很好的效果。而且金融信貸體系的不完善以及鄉(xiāng)鎮(zhèn)農(nóng)技站資金欠缺和技術(shù)人員不足，很難適應(yīng)目前的新型農(nóng)民要求。在蔬菜種植的鐵蓋鄉(xiāng)、龍羊峽鎮(zhèn)等地區(qū)，農(nóng)業(yè)綜合服務(wù)中心沒能起到爭取省州縣資金，依靠等、“喂”的消極觀念，人員配備大多數(shù)是非專業(yè)人員。

(4)

為了保證解算出的姿態(tài)角的單值性,必須對3個姿態(tài)角的范圍做出限定。具體角度取舍過程不再贅述。

4 互補濾波姿態(tài)融合

由于正交性和單位性的約束,DCM的行,列向量都是單位向量,且任意一對行或列向量都是相互垂直的。然而,在DCM不斷更新的過程中,由于數(shù)值計算舍入誤差的累積,MEMS陀螺儀存在漂移等各種原因,導(dǎo)致DCM的邊長增長或縮短,不再是等長的單位向量,使得原本正交的矩陣傾斜,如圖2所示。

圖2 DCM非正交型變示意圖

DCM的型變勢必會影響到姿態(tài)角的解算精度,對于同一確定的向量,用不同坐標(biāo)系表示時,它的模值和方向一定是相同的。正如前面所說,DCM存在偏差,所以該已知向量經(jīng)過旋轉(zhuǎn)得到的在新坐標(biāo)系下的向量一定也存在誤差,通過計算這個偏差來修正旋轉(zhuǎn)矩陣,從而達(dá)到修正姿態(tài)角的目的。

本文選取兩個方向基準(zhǔn)向量:一個是通過加速度計測得的重力場向量,另一個是通過電子磁羅盤測得的航向向量。方向基準(zhǔn)向量與通過DCM矩陣轉(zhuǎn)化得到的向量表示的是理論上同一方向的向量,它們的外積定義為我們所求取的誤差向量。該外積的模值恰好與兩個向量所呈夾角的大小成正比。

4.1 利用重力場計算俯仰角、滾轉(zhuǎn)角的誤差向量

令[vx,vy,vz]T為重力場向量[0,0,1]T在b系下的投影向量。由DCM可推出:

(5)

這里求出的重力場在b系下的投影[vx,vy,vz]T,就是我們在上文中提出的方向基準(zhǔn)向量。現(xiàn)在,需要求出與該基準(zhǔn)向量相關(guān)且同向的另一個向量,與基準(zhǔn)向量作叉乘,求出誤差向量。

(6)

式中,離心加速度的計算公式如下:

(7)

(8)

(9)

4.2 利用地磁場計算航向角的誤差向量

地球表面的磁場是從地球磁南極出發(fā),回到磁北極。測量的地磁場水平分量來計算載體與磁北極之間的夾角[12-13]。在北東地坐標(biāo)系下,令x軸對準(zhǔn)北向,顯然,y軸沒有分量,磁場強度為零。設(shè)x,z軸的磁場為bx,bz,與上文中用重力場作為基準(zhǔn)向量修正加速度計的方法類似,這里,我們選取n系磁阻傳感器的理論值[bx0bz]T為基準(zhǔn)向量,計算誤差向量來修正航向角。

當(dāng)載體處于任意姿態(tài)時,通過坐標(biāo)轉(zhuǎn)換矩陣,磁阻傳感器在b系下的輸出[mx,my,mz]T轉(zhuǎn)換到水平坐標(biāo)系,轉(zhuǎn)換過程如下:

在n系XOZ平面上,磁阻傳感器測得的向量模值一定相同,所以有:

(10)

Z軸上,兩種方式得到的向量大小相等,有bz=hz?，F(xiàn)在,我們得到了n系磁阻傳感器的理論值[bx0bz]T,由該向量再反推回b系得到向量[mbx,mby,mbz]T,計算過程如下:

(11)

至此,用來修正航向角的誤差向量可以由[mx,my,mzT]和[mbx,mby,mbz]T的外積算出。記該誤差向量為erroryaw,其表達(dá)式為:

(12)

綜上,總的誤差向量為e=[ex,ey,ez]T,其大小為:

e=errorpitch,roll+erroryaw

(13)

4.3 多軸互補濾波器設(shè)計與融合策略

一階低通濾波器和一階高通濾波器的傳遞函數(shù)模型分別為:

(14)

(15)

可以看出,互補濾波器能夠有效濾除低頻和高頻噪聲的干擾,得到接近真實值的重構(gòu)信號。

以誤差向量errorpitch,roll,erroryaw作為濾波器的輸入,輸出陀螺儀角速率測量值的修正量。輸入的誤差向量為e=[ex,ey,ez]T,經(jīng)過PI調(diào)節(jié)系統(tǒng)輸出可表示為:

(16)

修正后的陀螺角速率ω的表達(dá)式為:ω=ωg+Ku(k),ωg為陀螺儀測量到的角速率,Kp為比例環(huán)節(jié)系數(shù),其大小決定了濾波器的截止頻率和響應(yīng)速度,Ki為積分環(huán)節(jié)系數(shù),積分環(huán)節(jié)的加入有利于消除系統(tǒng)的穩(wěn)態(tài)誤差。顯然,這種互補濾波的形式與一般互補濾波不同,其中加入了比例積分環(huán)節(jié),整個系統(tǒng)是一個二階系統(tǒng),常稱其為顯式互補濾波[15]ECF(Explicit Complementary Filter)。ECF濾波器的結(jié)構(gòu)示意圖如圖3所示。

合適濾波器參數(shù)的選取就等價于確定濾波器的截止頻率。截止頻率取的低,濾波結(jié)果依賴陀螺多一點;取得高,依賴加計、磁阻傳感器多一點。由于實際應(yīng)用中由于震動等因素的影響,高頻噪聲干擾較多,互補濾波器截止頻率應(yīng)在合理范圍內(nèi)盡量選取較小的值,有效去除加速度計和磁阻傳感器的噪聲影響。具體參數(shù)需根據(jù)實際應(yīng)用中的噪聲水平確定。

圖3 ECF濾波器結(jié)構(gòu)示意圖

5 實驗與分析

5.1 轉(zhuǎn)臺靜態(tài)實驗

轉(zhuǎn)臺靜態(tài)實驗是為了驗證在姿態(tài)測量系統(tǒng)穩(wěn)定放置的靜態(tài)條件下,姿態(tài)角的解算精度。本文利用高精度三軸位置速率轉(zhuǎn)臺為靜態(tài)實驗的平臺,通過設(shè)定程序,控制轉(zhuǎn)臺轉(zhuǎn)動,并在不同位置穩(wěn)定,使系統(tǒng)采集到不同角度下穩(wěn)定狀態(tài)時的傳感器輸出。以轉(zhuǎn)臺實時反饋的角度信息為參考值,通過將本算法解算得到的姿態(tài)角信息與其相比較,驗證算法的精度及可靠性。靜態(tài)實驗的實物圖如圖4所示。

圖4 靜態(tài)試驗實物圖

實驗分兩步進(jìn)行,第1步將系統(tǒng)安裝在轉(zhuǎn)臺上,令轉(zhuǎn)臺轉(zhuǎn)動,使俯仰角分別穩(wěn)定在0°,30°,45°,60°,75°,0°,-30°,-60°,0°。第2步將轉(zhuǎn)臺三軸回零,使系統(tǒng)滾轉(zhuǎn)角分別穩(wěn)定在相同的角度,將傳感器數(shù)據(jù)存儲在系統(tǒng)內(nèi)部FLASH中。由于轉(zhuǎn)臺為鐵質(zhì),地磁傳感器受到影響,所以沒有進(jìn)行航向角測試。利用本文提出的基于互補濾波的姿態(tài)算法(圖5中ECF解算)與轉(zhuǎn)臺反饋的角度值相比對,將后者視為參考基準(zhǔn),對比圖如圖5所示。

根據(jù)國軍標(biāo)GJB 729-1989,靜態(tài)實驗的姿態(tài)角精度評定標(biāo)準(zhǔn)如下,即

(17)

式中,RMS為姿態(tài)角誤差的平均平方均方根值,ΔEi,j為第i次實驗第j個采樣時刻的姿態(tài)角誤差,mi為第i次實驗的數(shù)據(jù)采樣點數(shù),n為有效實驗的次數(shù)。由以上公式計算得到的RMSpitch為0.114 8°,RMSroll為0.085 9°,可見當(dāng)載體處于平穩(wěn)放置狀態(tài)下時,姿態(tài)角解算精度高,姿態(tài)角誤差小于0.2°。

圖5 姿態(tài)角解算系統(tǒng)靜態(tài)實驗

5.2 系統(tǒng)動態(tài)跑車試驗

為了驗證在動態(tài)條件下的姿態(tài)角解算精度,以及靜態(tài)實驗中未能驗證的航向角精度,本文設(shè)計了以高精度母慣導(dǎo)為參考基準(zhǔn)的動態(tài)跑車試驗。由于所用陀螺儀精度有限,所以將高精度母慣導(dǎo)輸出的三軸姿態(tài)角傳遞給航姿系統(tǒng),進(jìn)行初始對準(zhǔn),作為航姿系統(tǒng)的初始姿態(tài)角。將系統(tǒng)與高精度母慣導(dǎo)并排安裝在與車體固聯(lián)的測試工裝上,開動試驗車,兩套系統(tǒng)同時采集車體的運動信息,試驗車開車到停止的間隔時間約為180 s。跑車試驗現(xiàn)場圖如圖6所示。

圖6 車載實驗實物圖

將測試時間內(nèi)試驗系統(tǒng)輸出的三軸姿態(tài)角信息與母慣導(dǎo)輸出的姿態(tài)角相比較,對比圖如圖7所示,試驗系統(tǒng)與母慣導(dǎo)之間的姿態(tài)角誤差如圖8所示。

圖7 姿態(tài)角對比圖

從圖7中姿態(tài)角參考值與本算法的計算結(jié)果相對比可以看出,由于MEMS陀螺儀漂移導(dǎo)致姿態(tài)角發(fā)散的情況得到了很好的抑制。在跑車實驗的動態(tài)環(huán)境下,本文提出的ECF算法得到的姿態(tài)角與高精度母慣導(dǎo)提供的姿態(tài)信息吻合度很高,并且具備對動態(tài)變化的實時跟蹤性能。從圖8姿態(tài)角誤差曲線可以看出,由于航向角需要地磁傳感器提供信息,本身容易受到干擾,航向角誤差在三軸姿態(tài)角中誤差最大,達(dá)到4°左右,但后期航向角誤差發(fā)散得到抑制,誤差值穩(wěn)定。滾轉(zhuǎn)角與俯仰角誤差相對較小,滾轉(zhuǎn)角誤差小于2.3°,俯仰角誤差小于3.1°,誤差曲線沒有發(fā)散的趨勢。由以上實驗數(shù)據(jù)可算得航向角、滾轉(zhuǎn)角、俯仰角的均方根誤差(RMSE)分別為3.268 2°、1.325 7°、1.595 4°。

圖8 姿態(tài)角誤差曲線

6 結(jié)論

為了滿足低成本、高性能的載體姿態(tài)測量系統(tǒng)的迫切需求,針對MEMS陀螺儀漂移導(dǎo)致姿態(tài)無法準(zhǔn)確測量的難題,提出了基于方向余弦矩陣更新的互補濾波姿態(tài)融合方法(ECF)。該濾波算法既保持了經(jīng)典互補濾波的簡潔形式,又在原理上運用了自動控制中的PI調(diào)節(jié)。具有運行計算量小、占用內(nèi)存空間少、穩(wěn)定性好等突出優(yōu)點。從濾波效果上來看,選取合適的參數(shù)可使本算法在靜態(tài)及動態(tài)的應(yīng)用環(huán)境中均取得不錯的效果。因此本文提出的姿態(tài)融和算法具有較高的工程應(yīng)用價值。