基于加速度分離算法的姿態測量方法研究*

王向軍,張朝陽,劉 峰

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

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

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

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

1 姿態測量系統的總體結構

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

圖1 姿態測量系統結構框圖

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

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

2.1 橢球擬合算法的原理

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

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

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

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

(1)

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

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

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

(2)

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

(3)

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

(4)

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

(5)

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

2.2 橢球擬合法精度分析

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

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

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

3 自適應運動加速度分離算法

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

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

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

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

(6)

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

(7)

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

(8)

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

4 基于四元數的擴展卡爾曼濾波

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

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

(9)

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

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

(10)

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

5 飛行試驗仿真及結果分析

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

圖5 姿態測量模塊和AHRS Ellipse2-A

圖6 普通卡爾曼濾波姿態誤差曲線

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

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

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

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

圖7 應用加速度分離算法的姿態誤差曲線

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

6 結束語

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