孔令哲

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

0 引言

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

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

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

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

2 MEMS姿態(tài)信息

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

(1)

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

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

(2)

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

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

3 卡爾曼濾波法

卡爾曼濾波(Kalman filtering)是一種利用線性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法[7-8]。由于觀測(cè)數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作是濾波過(guò)程。其實(shí)時(shí)性好、精度高，普遍應(yīng)用于航天航空領(lǐng)域，是姿態(tài)信息濾波法的不二選擇。但歐拉角并非線性系統(tǒng)，廣義上不存在系統(tǒng)狀態(tài)方程。因此，使用卡爾曼濾波法前需引入另一個(gè)描述旋轉(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ù)理論，后來(lái)Klein等又做了進(jìn)一步的研究，但該理論一直未得到實(shí)際應(yīng)用。自20世紀(jì)60年代以來(lái)，隨著航空航天領(lǐng)域中飛行器姿態(tài)控制系統(tǒng)研究的迅速發(fā)展，才開始廣泛采用四元數(shù)法。用四元數(shù)法來(lái)描述姿態(tài)只需要四個(gè)參數(shù)，并且沒(méi)有奇異點(diǎn)。

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

(3)

(4)

(5)

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

(6)

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

(7)

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

3.2 卡爾曼濾波方案

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

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

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

(8)

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

(9)

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

第三步：濾波增益矩陣

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

(10)

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

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

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

(11)

第五步：估計(jì)誤差方差陣

(12)

其中，I為單位對(duì)角陣。

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

3.3 基于卡爾曼濾波的慣性測(cè)量實(shí)際效果

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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