基于四元數(shù)互補濾波算法的車載MIMU

李孟委，徐文武，張鵬

(中北大學 儀器與電子學院，山西 太原 030051)

0 引 言

微慣性測量單元(MIMU)是一種由多個MEMS慣性傳感器作為姿態(tài)測量組件，可獨立工作、不依賴外界數(shù)據的姿態(tài)檢測導航系統(tǒng)。相比于傳統(tǒng)基于光纖陀螺、激光陀螺等大型慣性傳感器設計的慣性測量單元(IMU)，MIMU體積小、功耗低，移動性與操作性更強。隨著慣性技術的發(fā)展，MIMU的精度得到大幅度提升，已廣泛應用于車載導航系統(tǒng)。陶子涵等[1]采用MEMS慣性測量器件設計的姿態(tài)測量系統(tǒng)，精度為俯仰軸0.35°，橫滾軸0.36°，航向軸0.51°，滿足車載MIMU使用精度。車載MIMU內置歐拉角、四元數(shù)等姿態(tài)解算算法,對慣性傳感器的原始數(shù)據進行整合處理;利用互補濾波、卡爾曼濾波算法對系統(tǒng)輸出的姿態(tài)進行耦合，提升姿態(tài)解算精度，為車輛提供高精度實時三軸姿態(tài)變化信息。

車載MIMU中的MEMS慣性傳感器存在易漂移、單個器件測量精度低以及易受外界環(huán)境干擾的缺點。在求解載體姿態(tài)信息的過程中，MEMS陀螺儀通過角速度積分計算載體姿態(tài)變化，MEMS加速度計通過采集重力加速度分量獲取載體姿態(tài)變化信息，兩者均可獨立地進行姿態(tài)解算。然而，在車載MIMU運行過程中，因MEMS慣性器件本身特性問題,會導致系統(tǒng)解算出的姿態(tài)數(shù)據不準確：MEMS陀螺儀存在積分漂移累積的問題，導致輸出不準確姿態(tài)數(shù)據；MEMS加速度計存在因外部環(huán)境引起的振動誤差，導致系統(tǒng)的姿態(tài)解算精度大幅度降低[2]；MEMS三軸磁力計因外界磁場干擾，導致航向角漂移。這三者易受外界溫度、電磁干擾等影響，從而導致輸出姿態(tài)不準。因此，MIMU應結合MEMS傳感器各自的優(yōu)勢，對多傳感器輸出的數(shù)據進行融合處理。

本文在車載MIMU四元數(shù)姿態(tài)解算算法的基礎上加入互補濾波算法，用以解決因慣性器件本身特性導致的解算精度較低的問題。四元數(shù)的使用可以有效降低計算的復雜度和難度,其旋轉特性可以解決車載MIMU姿態(tài)解算中存在的奇異性問題。在慣性測量儀器的精度較低的情況下,使用互補濾波算法可以有效改善測量儀器精度低、數(shù)據不穩(wěn)定的狀況。

1 姿態(tài)解算

1.1 建立坐標系

當描述一個三維空間剛體轉動時，需要選用3個獨立的角度表示具有一個固定點剛體的相對位置。為了計算車輛的實時姿態(tài)變換關系與輸出姿態(tài)角的表達式，建立相應的載體坐標系與導航坐標系，通過坐標系之間的角度變換公式描述載體姿態(tài)。導航坐標系采用傳統(tǒng)的“東北天”地理坐標系，載體坐標系采用通過右手定則確立的三維笛卡爾坐標系[3]。

求解車載MIMU運動過程中姿態(tài)角變換需要引入姿態(tài)變換矩陣。姿態(tài)變換矩陣是根據載體旋轉順序確定的變換矩陣的連乘，是一種多個運動復合后的數(shù)學表達式。通過姿態(tài)變換矩陣可以很好地描述載體的運動狀態(tài)[4]。建立如圖1所示的坐標系，其中n系為導航坐標系，b系為載體坐標系。載體按照Zn-X1-Y2的順序依次旋轉。

圖1 導航坐標系與載體坐標系

各次基本旋轉對應的變換矩陣為

(1)

(2)

(3)

將式(1)～(3)連乘，可得到用歐拉角表示的任意兩個坐標之間的方向余弦矩陣：

式(4)可簡化為

(5)

姿態(tài)變換矩陣為單位正交陣[5]，根據其特性可得

(6)

由此可求出姿態(tài)角(歐拉角)：

θ=sin-1(-T31)；

(7)

式中，γ，θ，ψ分別為車載MIMU經過歐拉角姿態(tài)解算算法計算后輸出的橫滾角、俯仰角與航向角。

1.2 四元數(shù)法

利用歐拉角求解載體姿態(tài)的算法思想簡單、編程難度低。但是，從上面的歐拉角推理過程可以看出，其計算過程存在大量的三角函數(shù)運算，對主控器負擔較重，影響MIMU整體性能，算法計算量較大，而且在橫滾角與航向角的求解公式中存在正切函數(shù)，而正切函數(shù)的特性使±90°位置會出現(xiàn)無窮大解，導致奇點問題出現(xiàn)，影響車載MIMU姿態(tài)解算精度。為了實現(xiàn)車載MIMU全方位的姿態(tài)解算，避免解算過程中出現(xiàn)奇點問題，本文引入四元數(shù)姿態(tài)解算算法[6]。

四元數(shù)法通過4個變量實時更新計算方法，代替大量的三角函數(shù)運算表示載體姿態(tài)角的變化，具有計算量小、算法流程簡單、計算過程中解算無奇點等優(yōu)點，能使車載MIMU在運行過程中實現(xiàn)全方位完全的姿態(tài)解算[7]。

四元數(shù)算法的運算步驟如下。

(1)采集加速度計與陀螺儀的原始數(shù)據，通過方向余弦矩陣思想，求解出歐拉角的初始值。根據陀螺儀的數(shù)據推算理論上的加速度計值。將四元數(shù)的4個數(shù)值換算為方向余弦矩陣的第3列元素，代表載體坐標系的值，即可得到加速度計對于陀螺儀的修正量；將修正量再進行積分，即可得到當前時間段內的累積誤差。最后用陀螺儀的數(shù)據減去誤差量，即可得到修正后的、較為準確的陀螺儀數(shù)據，代入四元數(shù)微分方程中，對最終運算結果的準確性起到很好的作用。

(2)四元數(shù)初始化。初始化方程如下：

(8)

通過式(8)即可得到初始的四元數(shù)量。

(3)四元數(shù)的更新及規(guī)范化。有了初始化的四元數(shù)量，再結合姿態(tài)數(shù)據采樣周期及采樣數(shù)據，即可對四元數(shù)進行更新，從而實時更新數(shù)據，實現(xiàn)姿態(tài)解算[8]。更新方程可表示為

(9)

式中，ωX，ωY，ωZ分別為陀螺儀在規(guī)定周期內完成的3個軸向角速度采樣。在各個采樣周期中，角速度是變量，將采樣值與四元數(shù)初始值代入式(9)，就可以實現(xiàn)四元數(shù)的不斷更新。由于四元數(shù)是一個四維單位向量，在完成四元數(shù)的更新后，需要將其進行單位化處理，計算公式為

(10)

(4)四元數(shù)轉換為歐拉角。四元數(shù)運算過程中也需要經過姿態(tài)變換矩陣，即用四元數(shù)常量代替三角函數(shù)運算，求解姿態(tài)角。在完成四元數(shù)矩陣的更新之后，需要將得到的四元數(shù)變量轉換為可以表示載體運動的方向余弦陣表達式，再轉換成歐拉角[9]。由四元數(shù)變量組成的方向余弦陣如式(11)所示。

(11)

至此，得到了只用四元數(shù)中的4個變量表示的載體姿態(tài)角公式：

(12)

1.3 互補濾波算法

MEMS陀螺儀與MEMS加速度計由于制作工藝以及自身屬性的原因，輸出的原始數(shù)據易受溫度、電磁干擾、振動等外界因素影響。MEMS陀螺儀容易產生因積分漂移累加所導致的低頻誤差；MEMS加速度計在受到外界振動時易產生高頻振動誤差。若不對這些誤差進行相關處理，將導致MIMU輸出姿態(tài)數(shù)據紊亂，可信度較低。為此，引入互補濾波算法，對慣性器件工作過程中產生的高低頻誤差進行處理[10]。

互補濾波算法可針對車載MIMU系統(tǒng)的具體誤差組成情況設置相慶的濾波器結構。本文所設計的互補濾波算法內置高通濾波器與低通濾波器，分別用來處理MEMS陀螺儀與MEMS加速度計所產生的低頻誤差與高頻誤差[11]。利用互補濾波算法進行姿態(tài)解算，互補濾波算法原理框圖如圖2所示。

圖2 互補濾波算法流程圖

為分析互補濾波算法的結構組成，利用MEMS陀螺儀與MEMS加速度計分別進行獨立的姿態(tài)解算[12]。設x為所得姿態(tài)角，

(13)

式中：x1，u1分別為MEMS加速度計在系統(tǒng)工作中解算的姿態(tài)角與因外界振動產生的高頻噪聲誤差；x2，u2分別為MEMS陀螺儀在系統(tǒng)工作中解算的姿態(tài)角與自身積分漂移累加產生的低頻噪聲誤差。

(14)

式(14)為姿態(tài)角x的連續(xù)表達式，F(xiàn)H(s)，F(xiàn)L(s)為濾波器傳遞函數(shù)。為了實現(xiàn)互補濾波算法的編程，需要利用差分變換法將其變換為離散表達式[13]。建立如式(15)所示方程，

X2(k-1)]+fX1(k)，

(15)

式中，X1(k)，X2(k)分別為MEMS加速度計、MEMS陀螺儀在k時刻解算的角度值。

由式(14)可知，互補濾波器的結構設計簡單、計算量小，對高低頻誤差信號均有抑制作用[14]。根據上述算法流程進行程序編寫工作，其編程思想可以描述為

angle=(1-A)×(angle_last+gyro×dt)+
A×acc，

(16)

式中：angle為當前融合而成的姿態(tài)角；angle_last為上一次姿態(tài)融合結果值；gyro為當前陀螺儀測量的角速度；dt為積分時間；acc為加速度計解算出的姿態(tài)角。

由式(16)可以看出，互補濾波算法通過調節(jié)MEMS陀螺儀與MEMS加速度計單獨解算姿態(tài)角數(shù)據的信任系數(shù)A，實現(xiàn)解算數(shù)據融合濾波。信任系數(shù)A可根據調試系統(tǒng)具體情況，對MEMS陀螺儀原始數(shù)據與MEMS加速度計原始數(shù)據進行加權平均處理，其中0≤A≤1。從算法原理角度出發(fā)，根據所選慣性傳感器本身的精度選擇理論置信度。信任系數(shù)A對應在參數(shù)設置上應該接近于1。這樣設置信任系數(shù)A的意義是MIMU姿態(tài)解算時所用的原始數(shù)據大部分來自MEMS加速度計[15]。根據所選慣性傳感器的測量精度與漂移特性，經過反復比較與計算，最終選擇信任系數(shù)A為0.98。

2 試 驗

2.1 車載MIMU系統(tǒng)組成

車載MIMU整機采用鋁合金六面體作為安裝框架，確保各個電路嚴格正交，通過物理方法減小因MEMS慣性器件正交耦合導致的姿態(tài)信息解算誤差。車載MIMU工作結構如圖3所示。

圖3 車載MIMU工作結構

MEMS慣性器件主要包括：3個單軸MEMS陀螺儀CRG20、三軸MEMS加表ADIS16210、三軸MEMS磁強計HMC5789。信號處理電路采用ADS1274作為處理器，將采集的原始數(shù)據進行模數(shù)轉換以便于控制器更好地處理數(shù)據。

2.2 測試步驟

首先對俯仰軸進行測試：車載MIMU與無磁轉臺內框通過安裝夾具捷聯(lián)在一起;以10°為步長轉動無磁轉臺，由于機械限位，俯仰軸的轉動范圍為(-90°，90°);通過藍牙實時傳輸3個電機的碼盤位置，當車載MIMU數(shù)據穩(wěn)定后靜止采集數(shù)據10 s;對所采集的數(shù)據進行存儲與分析。橫滾軸、航向軸的測試方法與俯仰軸測試方法相同，由于機械限位，橫滾軸測試范圍為0～180°。

3 結果分析

將車載MIMU放置于高精度無磁轉臺上，由無磁轉臺提供高精度姿態(tài)參考信息。無磁轉臺的姿態(tài)精度為0.05°，通過對比車載MIMU解算姿態(tài)與無磁轉臺姿態(tài)，可以獲取車載MIMU的姿態(tài)精度。圖4為車載MIMU測試環(huán)境與實物圖。利用MATLAB分析上述方法采集的橫滾軸、俯仰軸以及航向軸的姿態(tài)角數(shù)據,得到圖5～7所示的姿態(tài)角曲線。

圖4 車載MIMU測試環(huán)境與實物圖

圖5 橫滾軸姿態(tài)角曲線Fig.5 Attitude angle curves of rollers

圖6 俯仰軸姿態(tài)角曲線Fig.6 Attitude angle curves of pitchaxis

圖7 航向軸姿態(tài)角曲線Fig.7 Attitude angle curves of heading axis

從圖5～7可以看出，基于四元數(shù)互補濾波算法的車載MIMU在運動過程中所得三軸姿態(tài)角與高精度無磁轉臺提供的參考姿態(tài)角基本一致，動態(tài)跟隨效應較好。

為了具體分析車載MIMU 3個軸向姿態(tài)解算精度，分3次采集車載MIMU三軸姿態(tài)誤差角數(shù)據。利用MATLAB分析采集到的俯仰軸與橫滾軸的誤差角數(shù)據，得到圖8所示的誤差圖。

由圖8可知，車載MIMU收到高精度無磁轉臺相同的位置姿態(tài)信號后，俯仰軸、橫滾軸以及航向軸均有誤差產生，其中俯仰軸姿態(tài)誤差角在±0.23°內波動；橫滾軸姿態(tài)誤差角在±0.26°內波動；航向軸姿態(tài)誤差角在±0.51°內波動。經過3次對車載MIMU三軸姿態(tài)誤差角數(shù)據的采集與分析試驗，可以得到車載MIMU實時解算的俯仰軸精度，為0.23°，橫滾軸精度為0.26°，航向軸精度為0.51°的結論。

圖8 車載MIMU姿態(tài)角誤差圖

本文所設計的車載MIMU與文獻[1]中的MIMU姿態(tài)解算精度對比情況為：俯仰軸精度提升34%，橫滾軸精度提升27%，航向軸精度相等。采用此方案車載MIMU的俯仰軸與橫滾軸姿態(tài)解算精度提升較大，后期可使用文獻[16]中提出的有限時間一致無跡Kalman濾波器方案，實現(xiàn)車載MIMU姿態(tài)解算精度的提升。

4 結 語

通過采用四元數(shù)互補濾波算法，本文所設計的車載MIMU實時解算得到的俯仰軸精度為0.23°，橫滾軸精度為0.26°，航向軸精度為0.51°。采用四元數(shù)互補濾波算法能夠增強車載MIMU的動態(tài)跟隨性能、提升姿態(tài)解算精度。通過試驗驗證了四元數(shù)互補濾波算法的可行性，滿足車載導航系統(tǒng)慣性組件的要求。