高 寧,李 杰,馮凱強,許廷金,高詩堯,李炳臻

(中北大學(xué)電子測試技術(shù)重點實驗室,太原 030051)

在廣泛的導(dǎo)航應(yīng)用領(lǐng)域中,精確的姿態(tài)估計是必不可少的,如無人機導(dǎo)航、車輛導(dǎo)航、室內(nèi)導(dǎo)航、人體姿態(tài)控制等[1-4]。由于體積小、成本低、功耗小等優(yōu)點,MEMS器件被廣泛應(yīng)用于姿態(tài)測量領(lǐng)域[5]。然而,由于MEMS陀螺儀存在漂移,長時間工作有累積誤差;利用加速度計和磁力計來估計姿態(tài)角會受載體線性加速度和磁場干擾的影響[6-7]。因此,單獨使用某一種傳感器不能獲得精確可靠的姿態(tài)信息,必須利用姿態(tài)融合算法對不同傳感器的數(shù)據(jù)進(jìn)行融合,以提高系統(tǒng)的精度和抗干擾能力[8-10]。

至今,已有許多國內(nèi)外學(xué)者相繼研究出了不同的數(shù)據(jù)融合算法用于姿態(tài)測量領(lǐng)域。Sabatini等[11]人提出了一種基于四元數(shù)的擴展卡爾曼濾波器(EKF),將旋轉(zhuǎn)四元數(shù)以及加速度計和磁力計誤差作為狀態(tài)矢量,同時引入了一種自適應(yīng)測量噪聲協(xié)方差矩陣構(gòu)造方法,以減小載體線性加速度和磁場干擾的影響。但是EKF存在觀測方程在線性化時會引入線性誤差等缺點。為此,Yun等[12]人提出了一種基于Gauss-Newton的外部觀測方法,有效解決了觀測方程的線性化問題。但其計算量大,低成本處理器難以實現(xiàn)。為了降低計算量,Mahony等[13]人提出了一種用于無人機姿態(tài)估計的顯式互補濾波器(ECF)。Madgwick等[14]人使用梯度下降算法優(yōu)化提出了固定增益互補濾波器。這些互補濾波算法利用加速度計和磁力計的測量數(shù)據(jù)來補償陀螺儀的累積誤差,但是由于磁力計的測量值極易受周圍磁場干擾的影響,給航向角估計帶來誤差的同時也會給水平角帶來不確定的影響。

針對上述問題,本文提出了一種雙級互補濾波姿態(tài)融合算法。該算法利用加速度計和磁力計的測量值分步對MEMS陀螺儀估計的四元數(shù)進(jìn)行補償,避免了磁場干擾情況下航向角誤差對水平角測量的影響,同時引入了載體非重力加速度誤差和磁干擾誤差自適應(yīng)補償方案,提高了姿態(tài)測量精度。并且算法實現(xiàn)過程簡單,計算量小,降低了對處理器運算速度的要求。最后,基于現(xiàn)有的慣性測量系統(tǒng)對所提算法進(jìn)行了實驗評估。

1 基于四元數(shù)的姿態(tài)確定分析

取載體的重心為坐標(biāo)系原點,3個坐標(biāo)軸分別與載體的縱軸、橫軸和豎軸相重合,定義為載體坐標(biāo)系b,與之相對應(yīng)的絕對坐標(biāo)系通常稱為導(dǎo)航坐標(biāo)系n。導(dǎo)航坐標(biāo)系n向載體坐標(biāo)系b的轉(zhuǎn)換可以通過四元數(shù)法或Euler角法實現(xiàn),四元數(shù)法由于可以避免Euler角的奇異問題而得到了廣泛的應(yīng)用[15]。

載體坐標(biāo)系和導(dǎo)航坐標(biāo)系之間的轉(zhuǎn)換關(guān)系可以表示為:

(1)

用四元數(shù)q=q0+q1i+q2j+q3k表示坐標(biāo)系的轉(zhuǎn)換,則旋轉(zhuǎn)矩陣可表示為:

(2)

(3)

式中:Ω(ω)表示載體坐標(biāo)系相對于導(dǎo)航坐標(biāo)系角速度在載體坐標(biāo)系上分量的四元數(shù)。矩陣形式可以表示為:

(4)

通常假設(shè)在時間間隔(t,t+ΔT)內(nèi),角速度ω是一個固定值,其中ΔT是旋轉(zhuǎn)四元數(shù)更新時間間隔。因此可以獲得式(3)對應(yīng)的離散時間模型:

(5)

利用陀螺儀輸出的角速度數(shù)據(jù),結(jié)合式(5)對四元數(shù)進(jìn)行更新,從而獲得旋轉(zhuǎn)矩陣。

2 雙級互補濾波姿態(tài)融合算法

首先,利用上述更新的四元數(shù)獲得重力矢量和磁場矢量在載體坐標(biāo)系下的計算值:

(6)

而加速度計和磁力計的實際測量矢量為:

(7)

2.1 一級互補濾波算法設(shè)計

第一級互補濾波利用加速度計的測量數(shù)據(jù)對陀螺儀估計的四元數(shù)進(jìn)行補償修正,輸出準(zhǔn)確的水平姿態(tài)。如圖1所示,通過將k時刻由式(5)計算的四元數(shù)qk繞向量na旋轉(zhuǎn)角度Δθa來補償水平角誤差。因此,相應(yīng)的誤差四元數(shù)qae和修正四元數(shù)qa可以由下式獲得:

(8)

圖1 利用加速度計測量值補償估計的重力矢量

在靜止?fàn)顟B(tài)下,加速度計通過測量由于重力引起的加速度,可以準(zhǔn)確地計算出載體的水平姿態(tài)角。但是,當(dāng)載體存在線性加速度時,加速度計測得的加速度矢量的大小和方向與重力矢量存在偏差,利用加速度計輸出值計算載體姿態(tài)角將會出現(xiàn)比較大的誤差。然而,陀螺儀的測量值不會受線性加速度的影響,因此在這種情況下應(yīng)當(dāng)把陀螺儀數(shù)據(jù)作為估計的主要來源,用來進(jìn)行相對準(zhǔn)確的姿態(tài)估計。為了解決這一問題,本文在式(8)中引入自適應(yīng)增益系數(shù)μa來減少載體非重力加速度誤差對姿態(tài)測量精度的影響。

首先,定義載體非重力加速度誤差ea由下式獲得:

(9)

式中:‖gb‖為加速度計測得的加速度矢量模值,g為當(dāng)?shù)刂亓铀俣取V波增益系數(shù)μa與載體非重力加速度誤差ea之間的關(guān)系由下式表示:

μa=f(ea)

(10)

式中:f為分段連續(xù)函數(shù),如圖2所示。當(dāng)載體線性加速度很小且非重力加速度誤差不大于事先設(shè)定的閾值xa,濾波增益系數(shù)隨著非重力加速度誤差的增加而線性減小。如果載體存在較大的線性加速度,非重力加速度誤差大于閾值,那么濾波增益系數(shù)等于零。誤差閾值 的大小可通過衡量慣性器件各自的精度、載體的動態(tài)強度等條件,由試驗獲得最優(yōu)值。

圖2 濾波增益系數(shù)與非重力加速度誤差的關(guān)系

2.2 二級互補濾波算法設(shè)計

第二級互補濾波利用磁力計的測量數(shù)據(jù)對上一級獲得的四元數(shù)進(jìn)行補償,來修正航向角。在第一級工作的基礎(chǔ)上,利用磁力計測量值求得磁場在導(dǎo)航坐標(biāo)系下的估計矢量:

(11)

忽略垂直分量,得

(12)

如圖3所示,通過將qa繞nm旋轉(zhuǎn)小角Δθm來補償偏航角誤差,因此,相應(yīng)的誤差四元數(shù)qme和補償四元數(shù)qm可以由下式獲得:

(13)

式中:

mn=(0 1 0)為導(dǎo)航坐標(biāo)系下忽略垂直分量的地磁參考矢量。濾波增益系數(shù)μm的獲取辦法與上述μa獲取辦法類似,在此不再贅述。

圖3 利用磁力計測量值補償估計的磁場矢量

由上述可以看出,航向角誤差四元數(shù)的旋轉(zhuǎn)軸nm與坐標(biāo)軸z軸重合,也就是說第二級補償?shù)乃脑獢?shù)為上一級獲得的四元數(shù)僅繞z軸旋轉(zhuǎn)獲得,即使在磁場受到干擾的情況下也不會影響水平角的估計。

2.3 算法總體設(shè)計

根據(jù)上面的分析,設(shè)計的雙級互補濾波算法流程圖如圖4所示。

圖4 雙級互補濾波姿態(tài)測量算法流程圖

3 實驗驗證

利用實驗室現(xiàn)有的慣性測量系統(tǒng)對算法進(jìn)行驗證。慣性測量單元包含三個單軸CRM100 MEMS陀螺儀、三個單軸MS9000 MEMS加速度計、一個三軸HMC1043L AMR磁力計和一個采集存儲單元。內(nèi)部微處理器實時采集并存儲傳感器測量的原始數(shù)據(jù)。本文采用MATLAB設(shè)計所提濾波器,并對原始數(shù)據(jù)進(jìn)行處理,來獲得載體的姿態(tài)信息。設(shè)計了2種實驗方式來驗證算法的有效性:磁干擾靜態(tài)試驗;跑車動態(tài)試驗。第一組試驗用來驗證算法能否有效避免磁干擾對水平姿態(tài)的影響;第二組試驗用來驗證是否能有效消除載體線性加速度的影響。數(shù)據(jù)處理時算法中的誤差閾值取為xa=xm=0.2。

圖5 磁干擾實驗姿態(tài)角對比圖

3.1 磁干擾靜態(tài)實驗

將測量單元水平靜止放置,拿一根鐵棒在不同時間段內(nèi)靠近測量單元,以此來模擬磁場受到干擾的情況。將采集的數(shù)據(jù)用不同算法處理結(jié)果如圖5所示。

在圖5中可以看出,慣性測量單元分別在5 s～13 s 和22 s～29 s兩個時間段內(nèi)受到了磁場的干擾。利用Mahony算法解算出來的航向角在磁干擾的情況下產(chǎn)生了15°左右的誤差,同時也給俯仰/滾動角帶來了不同程度的影響。相比于Mahony算法,雙級互補濾波姿態(tài)測量算法在磁場受到干擾的情況下,有效降低了磁干擾對航向角的影響,同時俯仰/滾動角完全不受航向角誤差的影響,性能明顯優(yōu)于Mahony算法。在沒有磁干擾的情況下,所提算法可以輸出穩(wěn)定可靠的三維姿態(tài)角信息,誤差均小于0.2°。

3.2 車載動態(tài)實驗

為驗證算法實時性以及能否有效消除載體線性加速度的影響,設(shè)計了跑車動態(tài)實驗。采用加拿大NovAtel公司高精度光纖組合導(dǎo)航系統(tǒng)span-lci為參考基準(zhǔn),考核算法的姿態(tài)精度。將系統(tǒng)與高精度母慣導(dǎo)捷聯(lián)安裝在與車體固聯(lián)的測試工裝上進(jìn)行跑車實驗,兩套系統(tǒng)同時采集車體的運動信息。跑車現(xiàn)場圖如圖6所示。

采集到的加速度矢量的模值如圖7所示,從圖中可以看出,載體存在線性加速度。將融合濾波后輸出的三軸姿態(tài)角信息與母慣導(dǎo)輸出的姿態(tài)角相比較,對比圖如圖8所示。實驗系統(tǒng)與母慣導(dǎo)之間的姿態(tài)角誤差如圖9所示。

圖7 跑車試驗加速度矢量模值

從圖8中可以看出,本文所提雙級互補濾波融合算法得到的姿態(tài)角與高精度母慣導(dǎo)提供的姿態(tài)信息吻合度較高,并且具備對動態(tài)變化的實時跟蹤性能。對比Mahony算法,本文所提算法由于引入的自適應(yīng)濾波增益而有效得減小了載體線性加速度對姿態(tài)測量精度的影響,精度得到了明顯的提升。從圖9姿態(tài)角誤差曲線可以看出,由于磁力計容易受到干擾,航向角誤差在三維姿態(tài)角中誤差最大,達(dá)到5°左右。俯仰角與滾動角誤差相對較小,俯仰角誤差小于2°,滾動角誤差小于3°,誤差曲線沒有發(fā)散趨勢。

圖8 三維姿態(tài)角對比圖

圖9 三維姿態(tài)角誤差曲線

4 結(jié)論

本文采用雙級互補濾波算法對MARG傳感器的數(shù)據(jù)進(jìn)行融合,實現(xiàn)了三自由度的姿態(tài)測量。與傳統(tǒng)的Mahony互補濾波算法相比較,采用雙級互補濾波融合算法將水平與航向姿態(tài)分級融合修正,消除了磁干擾情況下航向角誤差對水平角測量的影響。同時,針對載體非重力加速度誤差和磁干擾誤差引入兩個獨立的濾波增益系數(shù),可分別由加速度計與磁力計不同頻段的噪聲自適應(yīng)調(diào)節(jié),大大提高了姿態(tài)角測量精度。基于慣性測量單元的靜態(tài)、動態(tài)實驗結(jié)果表明:本文所提算法能夠有效的解決磁場干擾和載體線性加速度對姿態(tài)測量的影響,顯著提高了三維姿態(tài)角的測量精度。