基于卡爾曼濾波法的船載慣性測量單元設(shè)計*

孔令哲

(中國電子科技集團(tuán)公司第三十九研究所 陜西省天線與控制技術(shù)重點實驗室，西安 710065)

0 引言

衛(wèi)星技術(shù)的進(jìn)步和成本降低將讓未來衛(wèi)星帶寬資源越來越豐富，加上廉價大數(shù)據(jù)以及物聯(lián)網(wǎng)的廣泛應(yīng)用，基于機載、車載和船載動中通設(shè)備的需求量日愈劇增。慣性測量單元(IMU)作為動中通設(shè)備的核心部件，是決定動中通設(shè)備跟蹤精度和市場競爭力的關(guān)鍵[1]。但常規(guī)姿態(tài)測量單元價格昂貴，簡易姿態(tài)測量單元精度不足，需要通過合理的算法彌補彼此的不足[2]。本文將介紹我所在民用船載動中通領(lǐng)域如何運用常規(guī)的微電子(MEMS)傳感器測量數(shù)據(jù)構(gòu)建慣性測量單元(IMU)并通過卡爾曼濾波法與四元數(shù)獲得穩(wěn)定、可靠的歐拉角姿態(tài)信息。此方法簡單易行，通過數(shù)據(jù)融合提升了常規(guī)的微電子傳感器對歐拉角姿態(tài)信息測量的精度，可廣泛的應(yīng)用于各個行業(yè)，尤其為動中通系列產(chǎn)品及類似設(shè)備的民用化、普遍化提供了可能。

1 姿態(tài)信息與歐拉角

在介紹姿態(tài)板構(gòu)成前首先要說明一下什么是姿態(tài)。此處的姿態(tài)源于飛行器姿態(tài)[3]，它著力描述的是研究對象體軸相對于地面的角位置，通常用偏航角、俯仰角與橫滾角三個角度表示。偏航角(yaw)，研究對象機體縱軸在水平面上的投影與該面上參數(shù)線之間的夾角；俯仰角(pitch)，研究對象機體縱軸與水平面的夾角；滾轉(zhuǎn)角(roll)，研究對象對稱平面與通過其機體縱軸的鉛垂平面間的夾角。

為了更為形象的表述，本方案選用了歐拉角(Eulerian angles)描述姿態(tài)。對于在三維空間里的一個參考系，任何研究對象坐標(biāo)系的取向都可以用一組有序的歐拉角來表現(xiàn)[4]。參考系是靜止不動的，而研究對象坐標(biāo)系則固定于剛體，隨著剛體的旋轉(zhuǎn)而相對參考系旋轉(zhuǎn)。任何研究對象坐標(biāo)系均可通過右手笛卡爾坐標(biāo)系(Cartesian coordinates)描述，因此，在參考系內(nèi)，研究對象坐標(biāo)系的兩個表征向量確定后，此研究對象坐標(biāo)系即可確定，反之亦然[5]。

2 MEMS姿態(tài)信息

姿態(tài)信息指的是研究對象坐標(biāo)系相對于參考系的歐拉角信息。在非標(biāo)定的前提下，為迅速有效的獲得此信息，首先我們得選擇可供微電子傳感器直接測量的參考系兩個表征向量[6]。于是，重力加速度與磁場成為了最佳的選擇。依據(jù)右手笛卡爾坐標(biāo)系法則，我們規(guī)定重力加速度的指向為參考系Z軸反方向，水平面上磁場北指向為參考系X軸方向，則此參考系為磁場指北坐標(biāo)系。在慣性測量單元三個標(biāo)志向量X、Y與Z上分別安裝測量重力加速度與磁場的微電子傳感器，即可獲取重力加速度與磁場在此測量坐標(biāo)系下的測量向量(gxgygz)與(magxmagymagz)，歸一量化后為(GxGyGz)與(MagxMagyMagz)，利用向量叉乘獲得第三個單位表征向量(SxSySz)。由此構(gòu)成姿態(tài)單位標(biāo)準(zhǔn)矩陣Mc，

(1)

此矩陣即可描述研究對象坐標(biāo)系與參考坐標(biāo)系間的關(guān)系。

鑒于歐拉角所描述的旋轉(zhuǎn)矩陣Me同樣是單位標(biāo)準(zhǔn)矩陣，因此可獲得聯(lián)立關(guān)系式。

(2)

由此，可通過微電子傳感器獲得的Mc矩陣求得偏航角(Y)、俯仰角(P)與滾轉(zhuǎn)角(R)三個旋轉(zhuǎn)歐拉角，繼而獲得姿態(tài)信息。

但僅依靠此方法獲得的姿態(tài)信息精度完全取決于微電子測量單元的測量精度，在震蕩、抖動與電磁干擾等諸多不利環(huán)境下，如若想獲得高精度姿態(tài)信息，單單靠提升微電子傳感器的測量精度與抗干擾性成本極大且效果欠佳。因此，需尋求一類濾波方案使測量結(jié)果精度高、實時性好，卡爾曼濾波法成了最佳選擇。

3 卡爾曼濾波法

卡爾曼濾波(Kalman filtering)是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法[7-8]。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程。其實時性好、精度高，普遍應(yīng)用于航天航空領(lǐng)域，是姿態(tài)信息濾波法的不二選擇。但歐拉角并非線性系統(tǒng)，廣義上不存在系統(tǒng)狀態(tài)方程。因此，使用卡爾曼濾波法前需引入另一個描述旋轉(zhuǎn)的數(shù)學(xué)模型，即四元數(shù)。

3.1 四元數(shù)

四元數(shù)(quaternions)也是一種重要的描述姿態(tài)的方法[9]。早在19世紀(jì)，愛爾蘭數(shù)學(xué)家哈密頓(Hamilton，1805—1865)在1843年首先在數(shù)學(xué)中引入了四元數(shù)的概念，并建立了四元數(shù)理論，后來Klein等又做了進(jìn)一步的研究，但該理論一直未得到實際應(yīng)用。自20世紀(jì)60年代以來，隨著航空航天領(lǐng)域中飛行器姿態(tài)控制系統(tǒng)研究的迅速發(fā)展，才開始廣泛采用四元數(shù)法。用四元數(shù)法來描述姿態(tài)只需要四個參數(shù)，并且沒有奇異點。

四元數(shù)是具有四個元素的超復(fù)數(shù)，它可以描述一個坐標(biāo)系或一個矢量相對于某一坐標(biāo)系的旋轉(zhuǎn)，定義為：

(3)

(4)

(5)

旋轉(zhuǎn)矩陣與歐拉角的關(guān)系是

(6)

歐拉角與四元數(shù)的關(guān)系是

(7)

由此可見旋轉(zhuǎn)矩陣、歐拉角與四元數(shù)都可以描述同一個目標(biāo)姿態(tài)，且彼此可以相互轉(zhuǎn)換。相比而言，旋轉(zhuǎn)矩陣普遍存在于空間姿態(tài)轉(zhuǎn)換運算中，但有九個參量[10]，雖然彼此有限制但仍不宜于描述；歐拉角由三個參量表述，形象直觀地描述了姿態(tài)的旋轉(zhuǎn)，但由于三角函數(shù)非線性，因此在數(shù)學(xué)應(yīng)用上存在局限性；四元數(shù)由四個參量組成，描述姿態(tài)既不直觀，其運算過程又復(fù)雜，但它存在狀態(tài)方程。因此，通過四元數(shù)狀態(tài)方程，借助卡爾曼濾波是可以實現(xiàn)姿態(tài)信息濾波的。

3.2 卡爾曼濾波方案

卡爾曼濾波法的實質(zhì)是通過多渠道測量，在預(yù)先知曉各測量方案的噪音狀態(tài)情況下獲得概率意義上的測量數(shù)據(jù)[11]。因此，為配合卡爾曼濾波法，需構(gòu)建有別于直接測量的其它測量姿態(tài)通道。在慣性測量單元三個標(biāo)志向量X、Y與Z上分別安裝角速度計、加速度計，實現(xiàn)一步預(yù)測，結(jié)合卡爾曼濾波法，通過姿態(tài)旋轉(zhuǎn)四元數(shù)狀態(tài)方程，對比預(yù)估姿態(tài)與實際測量姿態(tài)獲得概率意義上的低噪音、高精度姿態(tài)信息，從而獲得快速、準(zhǔn)確的濾波結(jié)果。過程可分為五步：

第一步：狀態(tài)預(yù)測

設(shè)在Tk-1時刻，即在沒有姿態(tài)測量輸出時，由姿態(tài)旋轉(zhuǎn)四元數(shù)狀態(tài)方程運算獲得四元數(shù)姿態(tài)信息預(yù)估值：

(8)

第二步：一步預(yù)測誤差方差陣

(9)

其中，Qk-1為四元素旋轉(zhuǎn)矩陣干擾參數(shù)陣，與安裝精度與微電子測速單元的測量精度有關(guān)，可預(yù)先測得[12]，Pk-1為Tk-1時刻的預(yù)測誤差方差陣，Pk，k-1為下一時刻的預(yù)測誤差方差陣。

第三步：濾波增益矩陣

Kk=Pk，k-1HkT[HkPk，k-1HkT]-1

(10)

其中，Hk為觀測陣，此算法如選用姿態(tài)矩陣則為單位對角陣。

第四步：狀態(tài)估計

在Tk時刻，由姿態(tài)測量輸出與預(yù)估值，通過增益矩陣運算，獲得概率意義上的真值：

(11)

第五步：估計誤差方差陣

(12)

其中，I為單位對角陣。

由此五步，可及時準(zhǔn)確的獲得經(jīng)卡爾曼濾波法處理的以四元數(shù)形式表述的姿態(tài)信息，從而獲得旋轉(zhuǎn)矩陣與姿態(tài)歐拉角信息。

3.3 基于卡爾曼濾波的慣性測量實際效果

以某型號產(chǎn)品實測數(shù)據(jù)為例，如圖1所示。

(a)角速度數(shù)據(jù) (b)加速度數(shù)據(jù)

此運動由三軸測試轉(zhuǎn)臺實現(xiàn)，其運動過程與空間姿態(tài)可知。由于噪音、溫漂等因素干擾，測量信息模糊。

如僅通過三軸加速度計以常規(guī)測量方式獲得姿態(tài)信息，如圖2所示。

(a)橫搖角度 (b)縱搖角度 (c)偏搖角度

由圖2可知，測量的橫搖、縱搖雖然逼近理論值，但噪音大，如用于姿態(tài)標(biāo)校則將引起控制信息震蕩，另外偏航運動不能通過三軸加速度計信息測得，因此，常規(guī)測量方式獲得姿態(tài)信息不可直接應(yīng)用。

如僅通過三軸角速度計以積分方式獲得姿態(tài)信息，如圖3所示。

(a) 橫搖角度 (b) 縱搖角度 (c) 偏搖角度

由圖可知，積分的橫搖、縱搖、偏航角度雖然連續(xù)平滑，但由于殘差、溫漂、離散處理等不可避免因素影響，在運動過程中，角度均有可能偏離理論值甚至發(fā)散。因此，積分姿態(tài)信息不可長時間無修正使用。

使用卡爾曼濾波法將測量的姿態(tài)信息與積分的姿態(tài)信息綜合處理，獲得統(tǒng)計的姿態(tài)信息如圖4所示。

(a) 橫搖角度 (b) 縱搖角度 (c) 偏搖角度

由圖可知，通過卡爾曼濾波法獲得統(tǒng)計的橫搖、縱搖角度不但相對連續(xù)平滑，而且可長時間逼近理論值，較為及時、準(zhǔn)確的反映了真實的姿態(tài)信息，滿足動中通姿態(tài)標(biāo)定及速度前饋的需求。

4 結(jié)論

基于微電子器件的慣性測量單元，其價格低廉、構(gòu)成簡單、易于實現(xiàn)，配合上卡爾曼濾波法，可消除大部分的噪音干擾與累積誤差，進(jìn)一步提升了其測量精度與性能，最大限度的發(fā)掘了測量芯片的能力，使其可滿足多數(shù)動中通設(shè)備姿態(tài)測量的需求[13-15]，尤其是在船載動中通領(lǐng)域，其測量精度與實時性基本可滿足各類民用船載動中通天線姿態(tài)標(biāo)定及速度前饋需求。