基于加速度分離算法的姿態(tài)測量方法研究*

王向軍,張朝陽,劉 峰

(1.天津大學精密測試技術(shù)及儀器國家重點實驗室,天津 300072;2.天津大學微光機電系統(tǒng)技術(shù)教育部重點實驗室,天津 300072)

姿態(tài)測量是實現(xiàn)精確慣性制導的核心技術(shù)[1,2],合適的姿態(tài)測量方法不僅是成像探測器求解視線角速率的基礎(chǔ),而且保證了慣性導航的精度和可靠性[3]。MEMS傳感器具有體積小、重量輕、成本低、易集成等眾多突出優(yōu)點,基于MEMS傳感器的姿態(tài)解算算法,在精確制導和姿態(tài)控制領(lǐng)域得到廣泛的應(yīng)用。在MEMS器件中,陀螺儀具有很高的精度,短時間內(nèi)可以得到可靠的姿態(tài)信息,但會產(chǎn)生隨時間累積的積分誤差;在重力作用下加速度計的低頻特性比較穩(wěn)定,通過與陀螺儀數(shù)據(jù)融合可以消除陀螺儀的積分誤差。然而,加速度計不僅容易受到外部振動干擾產(chǎn)生高頻噪聲,還會受到運動產(chǎn)生的加速度的影響,特別是在工作狀態(tài)為變加速運動時,姿態(tài)解算精度受到嚴重影響。

針對加速度計的測量誤差對成像探測器姿態(tài)測量精度的影響,傳統(tǒng)的方法是對加速度計測量數(shù)據(jù)低通濾波,這些方法有效抑制了振動噪聲,但是忽略了運動產(chǎn)生的加速度對姿態(tài)解算的影響。文獻[4]提出了專家系統(tǒng)判斷載體運動狀態(tài),降低了運動加速度對姿態(tài)解算精度的影響,但對于持續(xù)運動的載體會抑制加速度的修正作用,導致姿態(tài)解算誤差的積累和發(fā)散。文獻[5]提出了利用GPS修正加速度信息并融合陀螺儀參數(shù)完成姿態(tài)更新,但對GPS的依賴影響了微小型姿態(tài)測量系統(tǒng)的體積和成本。論文[6]提出靜止時只采用加速度計進行姿態(tài)解算,該方法對于間歇性運動的載體如機器人姿態(tài)測量有很好的效果,對加速運動下的載體效果有限。論文[7]提出利用空速計對加速度計測量數(shù)據(jù)進行補償,由于空速計精度的限制,導致最終姿態(tài)解算精度有限。

為解決以上問題,本文提出了針對成像探測器飛行規(guī)律的運動加速度分離算法,并根據(jù)此算法實現(xiàn)了基于卡爾曼濾波器的姿態(tài)解算。最后,在自主研制的集成有姿態(tài)測量模塊的微型成像探測器上,進行姿態(tài)測量方法的可行性分析和精度評估。飛行仿真實驗結(jié)果表明,該算法可以有效的分離出運動加速度,大幅提高了變加速運動狀態(tài)下姿態(tài)測量精度和抗干擾能力。

1 姿態(tài)測量系統(tǒng)的總體結(jié)構(gòu)

姿態(tài)測量系統(tǒng)以集成電路的形式嵌入在微型成像探測器中。系統(tǒng)可以分為信息采集、數(shù)據(jù)處理以及結(jié)果輸出三部分。由加速度計和陀螺儀進行信息采集并提供初始參數(shù),MCU實現(xiàn)對MEMS傳感器數(shù)據(jù)的誤差補償和姿態(tài)解算,并以歐拉角的形式通過無線傳輸輸出姿態(tài)信息。姿態(tài)測量系統(tǒng)的總體結(jié)構(gòu)框圖如1所示。

圖1 姿態(tài)測量系統(tǒng)結(jié)構(gòu)框圖

2 橢球擬合算法補償加速度計誤差

由于MEMS傳感器的安裝誤差、垂直誤差以及量測誤差等因素,需要對MEMS傳感器進行誤差標定補償。陀螺儀短時間精度較高,直接采用簡單的建模分析法即可取得較好效果[8],本文主要介紹利用橢球擬合法對加速度計采樣數(shù)據(jù)進行誤差補償。該方法利用重力場完成加速度計的誤差標定,穩(wěn)定的重力矢量場和大量的采樣數(shù)據(jù)保證了標定結(jié)果具有較高的精度,而且不需要借助外部設(shè)備或基準,具有簡便易行等優(yōu)點[9-11]。

2.1 橢球擬合算法的原理

當加速度計靜止時,由于只受重力作用,各軸上的測量參數(shù)在空間坐標系上理想分布為球心在原點、半徑為g的球面上。由于加速度計的刻度誤差和軸間偏置誤差等因素影響,加速度計所測數(shù)據(jù)會分布在近似橢球的表面上。在各種姿態(tài)下采樣加速度計的測量數(shù)據(jù)并進行基于最小二乘法的橢球擬合,求出球心偏置位置和橢球的各個軸長,即可得到測量值與理想值之間的對應(yīng)關(guān)系,實現(xiàn)誤差補償。橢球最小二乘擬合算法的采樣點空間分布如圖2所示。

圖2 橢球擬合算法的采樣點空間分布

橢球最小二乘擬合法如式(1)所示:

a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1

(1)

式中(x,y,z)表示加速度計測量數(shù)據(jù),a1～9表示待估計值。為表示方便令:

(m1,m2,m3,m4,m5,m6,m7,m8,m9)=

(x2,y2,z2,xy,xz,yz,x,y,z)

(2)

對于多組測量數(shù)據(jù),mij表示第i組的mj,由于每一組測量數(shù)據(jù)都有測量誤差,故有相應(yīng)的測量殘差方程組如式(3)所示:

(3)

按照最小二乘法原理,待求的aj應(yīng)滿足殘差平和方最小,得到式(4):

(4)

分別對aj求偏導,且令其為零推導得正規(guī)方程組:

(5)

式中:mi表示列向量[m1i,m2i…mni]T,[mimj]表示兩列向量的內(nèi)積。根據(jù)正規(guī)方程組可以直接得出待估計值a1～a9。

2.2 橢球擬合法精度分析

在不同姿態(tài)下進行加速度計采樣,得到使用橢球最小二乘擬合算法前后的加速度計初始誤差曲線圖,如圖3所示。

圖3 使用橢球擬合法前后初始誤差曲線

比較橢球擬合法前后的初始誤差數(shù)據(jù)可以發(fā)現(xiàn),使用橢球擬合法可以將加速度計初始誤差由0.2 m/s2降低到0.05 m/s2以內(nèi),說明該算法可以有效對加速度計測量數(shù)據(jù)進行誤差補償,提高了加速度計測量數(shù)據(jù)的初始精度。

3 自適應(yīng)運動加速度分離算法

在微小型姿態(tài)測量系統(tǒng)中,為保證精度 MEMS傳感器的坐標系與載體坐標系的對應(yīng)軸平行或垂直。當成像探測器高速飛行時,沿著前進的方向(橫滾軸)受運動影響會產(chǎn)生明顯變化的加速度,探測器的前進方向上產(chǎn)生的運動加速度僅對加速度計的特定軸有影響。載體坐標系和MEMS傳感器坐標系的相對位置關(guān)系如圖4所示。

圖4 載體坐標系和MEMS傳感器坐標系

其中XZY表示載體坐標系,xyz表示MPU6050安裝坐標系。從圖中可以看到,沿載體坐標系的X軸運動,會在加速度計坐標系的z軸中產(chǎn)生運動加速度分量,而不會影響其他兩軸的加速度分量。利用重力加速度恒定這一條件,可以由其他兩軸上的重力加速度分量推導出運動方向上的重力加速度分量,進一步計算出運動加速度,實現(xiàn)加速度分離和姿態(tài)信息的解算。

在自適應(yīng)加速度分離算法中,首先設(shè)定一個加速度閾值,假如加速度計測得數(shù)據(jù)與重力加速度的相差超出該閾值,表示運動產(chǎn)生的加速度對加速度計測量數(shù)據(jù)產(chǎn)生了一定的影響,此時進行加速度計分離算法。當運動產(chǎn)生的加速度影響較小時,判定條件為:

(6)

閾值ε根據(jù)載體的運動情況而定。加速度轉(zhuǎn)換關(guān)系由載體坐標系XYZ和加速度計傳感器坐標系xyz的相對位置關(guān)系而定,根據(jù)轉(zhuǎn)換矩陣直接求出載體坐標系的三軸加速度對陀螺儀參數(shù)進行校準。以圖4中建立的坐標系為例,加速度轉(zhuǎn)換公式為:

(7)

當運動產(chǎn)生的加速度影響較大時,需要加速度分離算法分離出重力加速度分量與陀螺儀參數(shù)進行融合進行校準。由探測器的飛行規(guī)律可知,運動加速度主要作用于載體的前進方向X,導致加速度計傳感器的z軸方向測量數(shù)據(jù)出現(xiàn)偏差,而加速度計的x軸和y軸和運動方向垂直,測量值為重力加速度的分量且不受運動影響。如式(8)所示,利用x軸和y軸的加速度計測量數(shù)據(jù)可以對z軸的重力加速度分量進行估計,獲得重力加速度三軸分量,并可進一步獲得運動加速度。式中aX的方向可以結(jié)合當前姿態(tài)角獲得。

(8)

自適應(yīng)加速度分離算法可以保證探測器在變加速運動狀態(tài)下,能采用有效的加速度數(shù)據(jù)作為觀測矩陣進行卡爾曼濾波,保證了姿態(tài)測量系統(tǒng)的精度和魯棒性。

4 基于四元數(shù)的擴展卡爾曼濾波

姿態(tài)解算的方法主要有歐拉角法、方向余弦法和四元數(shù)法[12]。歐拉角法只需求解3個微分方程,具有描述直觀、易于實現(xiàn)等優(yōu)點,但是姿態(tài)矩陣會出現(xiàn)奇異點,導致萬向節(jié)死鎖現(xiàn)象,不能對目標進行全姿態(tài)解算。方向余弦法需要對運動目標的9個參數(shù)進行解算,計算量較大,因此實際中很少應(yīng)用。四元數(shù)法可以表示剛體的定軸轉(zhuǎn)動,避免了歐拉角萬向節(jié)死鎖現(xiàn)象,而且與方向余弦法相比計算量更小,所以得到廣泛應(yīng)用。在姿態(tài)信息融合的過程中需要使用濾波器保證姿態(tài)解算的精度,常用的濾波器有互補濾波器和卡爾曼濾波器及其改進算法?；パa濾波器具有結(jié)構(gòu)簡單和實時性強等優(yōu)點,但精度不如卡爾曼濾波器[13,14]?？柭鼮V波器具有精度高、追蹤效果好以及抗噪聲能力強等優(yōu)點,擴展卡爾曼濾波推廣了卡爾曼濾波器在非線性領(lǐng)域的應(yīng)用。隨著人們對MEMS傳感器姿態(tài)解算精度要求的提高,以及芯片性能的增加,卡爾曼濾波器的重要性越來越大。

本文基于四元數(shù)姿態(tài)表示,使用陀螺儀角速度更新四元數(shù)作為卡爾曼濾波的狀態(tài)矩陣,結(jié)合自適應(yīng)加速度分離算法得到的重力加速度分量作為卡爾曼濾波的觀測矩陣,通過改進的擴展卡爾曼濾波器,對運動狀態(tài)下的微慣性導航系統(tǒng)進行高精度姿態(tài)解算。四元數(shù)可以通過陀螺儀參數(shù)直接進行更新,四元數(shù)更新的微分方程為:

(9)

式中:ωxyz表示經(jīng)過誤差補償后的陀螺儀三軸角速度,q0～3表示四元數(shù)。對四元數(shù)進行實時更新,并根據(jù)四元數(shù)和歐拉角的旋轉(zhuǎn)矩陣對應(yīng)關(guān)系,可以獲得實時歐拉角信息。

卡爾曼濾波過程的迭代公式分別為狀態(tài)矩陣的預(yù)測、協(xié)方差預(yù)測、濾波增益方程、狀態(tài)矩陣更新方程以及協(xié)方差更新方程,如式(10)所示:

(10)

式中:狀態(tài)量X為四元數(shù)向量,Ak-1表示狀態(tài)轉(zhuǎn)移矩陣,由四元數(shù)微分方程的雅可比矩陣確定;Qk-1為觀測量的協(xié)方差矩陣,可以對加速度計采樣信號分析得出;P為協(xié)方差矩陣,具有自收斂性,K為卡爾曼增益;Hk為觀測矩陣,由重力加速度g通過四元數(shù)旋轉(zhuǎn)矩陣在加速度計三軸上的投影和加速度計測量數(shù)據(jù)關(guān)系確定。

5 飛行試驗仿真及結(jié)果分析

基于自主研制的微小型成像探測器,驗證上述慣性導航系統(tǒng)的有效性。該探測器目前集成了圖像采集、目標智能識別跟蹤以及姿態(tài)測量等多個模塊。在姿態(tài)測量系統(tǒng)中,主控芯片為采用C8051F383,該芯片尺寸微小,主頻達到24 MHz,能夠完成姿態(tài)解算、數(shù)據(jù)通信以及視線角速率的計算等功能。姿態(tài)芯片采用MPU6050,包含三軸加速度和三軸陀螺儀,在必要時可以外接磁力計進行輔助校準。仿真實驗以SBG公司的高精度AHRS器件Ellipse2-A作為姿態(tài)參考器件,俯仰和橫滾角精度達到0.1°,航向角精度達到0.8°,輸出速率達到200 Hz,而且具有低噪聲的加速度計和陀螺儀,對運動加速度有很好的抗干擾能力。圖5為探測器姿態(tài)測量模塊和Ellipse2-A實物圖。

圖5 姿態(tài)測量模塊和AHRS Ellipse2-A

圖6 普通卡爾曼濾波姿態(tài)誤差曲線

將探測器和Ellipse2-A固定在一起,進行姿態(tài)解算精度的分析和比較。通過串口將各自的姿態(tài)解算結(jié)果發(fā)送到上位機,使用MATLAB搭建姿態(tài)解算評估系統(tǒng)上位機,不僅可以保證探測器和Ellipse2-A同時采樣,還可以對姿態(tài)解算結(jié)果和精度進行直觀的實時顯示和即時評價。

為保證仿真實驗時探測器的運動規(guī)律和實際工作時相同,對探測器施加外力使其沿飛行方向(橫滾軸方向)做變加速往返運動,比較不同姿態(tài)解算算法對探測器姿態(tài)解算精度的影響。如圖6為存在一定運動加速度情況下,未使用加速度分離算法,僅使用普通卡爾曼濾波進行姿態(tài)解算的三軸姿態(tài)角誤差曲線。

從圖6中可以發(fā)現(xiàn),當探測器沿橫滾角方向做變加速運動時,運動加速度對探測器姿態(tài)角中的俯仰角有很大影響。

圖7為在相同的運動加速度情況下,應(yīng)用本文提出的基于加速度分離算法的改進卡爾曼濾波進行姿態(tài)解算的姿態(tài)誤差曲線。

圖7 應(yīng)用加速度分離算法的姿態(tài)誤差曲線

從圖7中可以發(fā)現(xiàn),本文算法在橫滾角和偏航角解算上和原算法接近,保留了原有算法的精度,橫滾角和航向角誤差在±1°以內(nèi);在最易受到運動加速度影響的俯仰角方面,該算法改善效果明顯,最大誤差從±4°減小到±1°以內(nèi),誤差降低了70%以上。本文提出的基于加速度分離算法的姿態(tài)測量系統(tǒng)可以有效的抑制運動加速度對姿態(tài)解算的影響,提高了運動狀態(tài)下姿態(tài)解算的精度。

6 結(jié)束語

隨著人們對姿態(tài)測量系統(tǒng)高精度和小型化的期待,基于MEMS傳感器的微小型姿態(tài)測量系統(tǒng)及相應(yīng)姿態(tài)解算算法成為重要研究方向。傳統(tǒng)的姿態(tài)測量系統(tǒng)只是在靜止情況下有較好的效果,針對運動條件下的姿態(tài)解算往往只能舍棄加速度計數(shù)據(jù)或降低其信任度,并沒有從根本上實現(xiàn)運動狀態(tài)下的數(shù)據(jù)有效融合。本文首先對MEMS傳感器建立誤差模型,然后針對成像探測器飛行規(guī)律提出了運動狀態(tài)下加速度分離算法,結(jié)合擴展卡爾曼濾波實現(xiàn)了高精度姿態(tài)解算,并在自主設(shè)計的微小型成像探測器上進行模擬飛行試驗。實驗結(jié)果表明,該姿態(tài)測量系統(tǒng)在運動狀況下可以抑制運動加速度的干擾,對比未使用加速度分離算法精度得到大幅提高,該姿態(tài)測量方法滿足了設(shè)計要求。