張 靜,李維剛,張駿虎,聶 鵬,張傳莊

(1.中北大學 軟件學院，太原 030051; 2.山西江淮重工有限責任公司，山西 晉城 048026)

0 引言

MEMS慣性測量單元(MIMU)，采用MEMS制造工藝制作，借助內(nèi)置的MEMS加速度傳感器和MEMS陀螺儀，測量來自3個方向的線性加速度和旋轉角速率，通過解算MIMU的角速率和加速度輸出，得到載體的姿態(tài)信息[1]。MIMU以其功耗低、重量輕、低成本和小體積使得它廣泛應用在智能駕駛、機器人控制、 平臺穩(wěn)定、飛行器姿態(tài)控制等民用、商用級軍事領域。其數(shù)字化和高可靠性的特點使其有漸漸代替其他中低精度慣性測量單元的趨勢。

但是受制于制造極限和體積的約束，MEMS慣性測量單元中檢測電路電子元件性能隨溫度發(fā)生漂移、交叉軸存在耦合誤差，嚴重影響系統(tǒng)的輸出精度[2]。捷聯(lián)慣性導航系統(tǒng)的精度主要取決于慣性器件的精度、導航算法的精度、導航計算機的處理能力及載體外界環(huán)境。因此本文研究對解算算法進行優(yōu)化來提升MIMU的工作精度具有非常重要的實用意義。

MEMS慣性導航系統(tǒng)自主獲取載體完整運動信息進行導航時，需要確定載體的指向軸與真北的方向，以提供系統(tǒng)初始對準過程的航向角。對于中低精度光纖陀螺和MEMS陀螺儀而言，其精度無法精確測量到地球自轉角速度，在靜置狀態(tài)下，尋北精度相對較低，甚至不能尋北。要實現(xiàn)導航任務就必須外接絕對航向的傳感器，如GPS、磁傳感器，來解決系統(tǒng)初始對準問題。目前，慣性導航定位系統(tǒng)中常用的尋北方式主要有3種：1)基于單軸旋轉調(diào)制的MEMS慣性導航系統(tǒng)及其尋北方法，解析地球自轉角速度與地球重力加速度兩個不共線的矢量，利用MEMS慣性系統(tǒng)進行自主、快速、準確尋北[3]；2)采用兩個或更多天線以構成基線向量，用于測量航向和俯仰角，采用不在一條線上的3個天線組合能測量航向、俯仰和橫滾角;3)磁力計尋北，通過利用磁力計測得磁場強度進而得到所需的載體方位角信息。因此在MEMS慣性測量組合中基本都集成了經(jīng)過內(nèi)部誤差、應用安裝誤差以及羅差補償后的三軸磁力計用于尋北[4]。

MIMU工作過程中，陀螺儀存在低頻積分漂移累積的問題，加速度計受高頻振動影響較大，磁力計測量得到的磁場信息除了包含地磁導航所需的地磁場信息，還有載體自身存在的線圈、鐵磁物質(zhì)等產(chǎn)生的載體磁場，干擾磁力計的測量精度，造成航向角漂移。因此，要建立了各自慣性器件輸出誤差模型,發(fā)揮MIMU系統(tǒng)中各傳感器各自的優(yōu)勢，利用Kalman濾波對基于MIMU的導航系統(tǒng)進行信息融合處理[5]。

本文設計了一種基于卡爾曼濾波的四元數(shù)姿態(tài)解算算法來解決MEMS慣性測量單元器件特性導致的解算精度較低的問題。本文設計的四階卡爾曼濾波器復雜度相對解四元數(shù)微分方程組并不高，實驗證明該算法可以有效提高MIMU姿態(tài)解算精度，明顯改善測量儀器精度低、數(shù)據(jù)不穩(wěn)定的狀況，具有較強的工程實用價值。

1 MIMU姿態(tài)解算原理

由三軸MEMS陀螺儀、三軸MEMS加速度傳感器和三軸磁力計組成的MIMU固連在載體上，MIMU姿態(tài)角解算原理如圖1所示。

MEMS陀螺儀輸出的旋轉角速度信號經(jīng)過數(shù)字濾波、信號采集后轉化為數(shù)字信號，通過四元數(shù)姿態(tài)解算后，獲得載體運載體的姿態(tài)角。這種姿態(tài)角獲取方法是由陀螺儀測得的角速率，能保證系統(tǒng)的動態(tài)性能，不受載體的工作狀態(tài)和環(huán)境影響，但隨時間積累姿態(tài)角誤差變大；三軸MEMS加速度計輸出的加速度信號經(jīng)過數(shù)字濾波、信號采集后，選用方向余弦矩陣在載體坐標系和導航坐標系之間進行變換，得到俯仰角和橫滾角；三軸磁力計輸出的磁場信號同樣經(jīng)過數(shù)字濾波、信號采集后，根據(jù)方向余弦矩陣，得到地球磁場矢量在x、y軸上的分量，計算后得到載體相對地磁北極的航向角，考慮地理北極和地磁北極夾角，可修正得到載體相對地理北極的航向角。由三軸加速度計和磁強計計算得到的姿態(tài)角，適合運載體靜態(tài)時的測量，測量誤差不隨時間累計，但動態(tài)性能差，尤其是加速度信號噪聲較大。

因此，本文結合兩種獲取姿態(tài)角優(yōu)勢，既能保證系統(tǒng)動態(tài)時的性能，又對補償靜態(tài)時陀螺儀的零點漂移。將陀螺儀獲取到的姿態(tài)角與角速度計和磁力計獲得的姿態(tài)角做差，得到姿態(tài)角誤差。利用設計的卡爾曼濾波器對姿態(tài)誤差角進行優(yōu)化估計，得到姿態(tài)角誤差角的最優(yōu)估計值。對陀螺儀得到的姿態(tài)角進行優(yōu)化校正，從而得到較高精度的姿態(tài)角信息。

1.1 坐標系建立

在捷聯(lián)慣性導航系統(tǒng)中，進行姿態(tài)解算的目的是利用陀螺儀角速率積分和加速度計瞬時值確定載體坐標系(b系)與導航坐標系(n系)的三次轉角即航向角ψ、俯仰角θ和橫滾角γ，為慣性導航系統(tǒng)的速度解算、位置解算時提供姿態(tài)矩陣。姿態(tài)矩陣的實時計算和更新是慣性導航系統(tǒng)的主要計算內(nèi)容。其解算的實時性、解算的精度直接決定著影響著慣性導航的精度及任務的成敗[6]。航向角ψ、俯仰角θ和橫滾角γ三個姿態(tài)角定義如圖2所示。

圖2 載體姿態(tài)角定義

捷聯(lián)慣性導航系統(tǒng)中慣性儀表直接固連在載體上，可以直接給出軸向的線加速度和角速度。通過其建立的“數(shù)學平臺”，將空間每個矢量在導航坐標系下進行分解，計算坐標系之間的相對位置關系，獲得轉換到導航坐標系的參量[7]，進行導航解算，進而得到運載體的速度和位置。

載體坐標系(簡稱b系)是一個正交坐標系，采用右手定則確立的三維笛卡爾坐標系, 記作OXbYbZb[8]。與載體固連，隨載體的轉動一起運動[9]，其原點與載體的重心重合，Xb軸沿載體的俯仰軸向右、Yb軸沿載體的橫滾軸向前、Zb軸沿載體的偏航軸向上。

導航坐標系(簡稱n系)，一種站內(nèi)坐標系，與地理坐標系軸向完全平行，只是原點不同。采用傳統(tǒng)的“東北天”地理坐標系，記作OXnYnZn。文中均以地理坐標系作導航坐標系，進行導航解算時的參考坐標系，導航坐標系原點位于載體的質(zhì)心，始終取觀測點為原點。建立的載體坐標系和導航坐標系如圖3所示。

圖3 坐標系示意圖

1.2 姿態(tài)更新算法

捷聯(lián)慣性導航對導航算法的實時性、精確性要求較高。導航算法誤差包括姿態(tài)算法誤差、速度算法誤差、位置算法誤差。其中姿態(tài)算法的精度最為關鍵，直接決定比力積分的精度，進而影響速度、位置解算的精度。姿態(tài)算法本身引入的誤差應控制在慣性器件引入誤差的5%內(nèi)。

載體運動中高頻成分產(chǎn)生不可交換性誤差，對姿態(tài)解算精度影響很大，需采用執(zhí)行頻率高的姿態(tài)算法才能精確消除誤差；而高階姿態(tài)算法具有較高的精度，需要更多的計算量，對計算機處理能力提出較高的要求。因此在姿態(tài)算法的選擇上必須兼顧實時性和精確性。相對于方向余弦矩陣方法和歐拉角法，四元數(shù)方法以其非奇異性、簡便性和計算有效性成為姿態(tài)更新的主要方法，能在高頻角速率環(huán)境下姿態(tài)更新精度更高。本文選用四元數(shù)方法對MEMS陀螺儀輸出的動態(tài)旋轉角速度信號進行實時姿態(tài)解算。

1.2.1 陀螺儀數(shù)據(jù)姿態(tài)解算

1.2.1.1 四元數(shù)初始化

選“東-北-天(E-N-U)”地理坐標系(e系)作為捷聯(lián)慣導系統(tǒng)的導航參考坐標系，重新記為n系。

(1)

1.2.1.2 四元數(shù)更新

四元數(shù)微分方程為:

(2)

寫成矩陣形式為:

(3)

即:

(4)

要進行姿態(tài)矩陣的實時計算和更新，必須實時求解四元數(shù)微分方程，這里采用龍格-庫塔法(Runge-Kutta)[9]。

X(t+T)=X(t)+hf(X(t),ω(t))

式中，h為采樣周期[10]。根據(jù)上文，四元數(shù)的更新方程可表示為：

(5)

式中，ωX，ωY，ωZ表示陀螺儀在規(guī)定周期內(nèi)完成的3個軸向的角速度采樣，T是采樣周期。由于四元數(shù)是一個四維單位向量，在完成四元數(shù)的更新后需要將其進行單位化處理，計算公式如下：

(6)

1.2.1.3 四元數(shù)轉換為姿態(tài)矩陣與姿態(tài)角

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

(7)

(8)

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

1.2.2 角速度計和磁力計數(shù)據(jù)姿態(tài)解算

捷聯(lián)慣性系統(tǒng)載體坐標系和導航(地理)坐標系相互重合時，MEMS加速度計的測量值為An=[0 0g]T，磁力計的測量值為：Mn=[MN0MD]T。

若載體處于處在任意一個位置時，MEMS加速度計的測量值為：Ab=[axayaz]T；磁力計的測量值為：Mb=[MxMyMz]T。

依據(jù)方向余弦矩陣，b系與n系的變換關系有：

(9)

(10)

由上式可解算出俯仰角θ和橫滾角γ：

(11)

將式代入得到地磁場矢量在x、y方向的分量：

(12)

從而計算出載體相對地磁北極的航向角：

(13)

故航向角φ=φm+Δφ，其中Δφ為地磁北極與地理北極的夾角。

1.3 卡爾曼濾波器構建

慣性測量過程中存在隨機干擾的問題，在觀測的信號中常常夾雜著隨機噪聲。這就需要采用最優(yōu)估計理論來得到所需的狀態(tài)變量[12-14]，這里采用卡爾曼濾波器(Kalman filter)進行狀態(tài)估計，該算法的原理如圖4所示，X0代表初始狀態(tài)值，P0代表初始協(xié)方差矩陣，一般選取為單位矩陣I。

圖4 卡爾曼濾波算法原理圖

使用該算法時，系統(tǒng)狀態(tài)方程中的參量選為姿態(tài)角偏差和陀螺儀的零漂誤差；測量方程由加速度計與磁強計輸出數(shù)據(jù)解算的姿態(tài)角和四元數(shù)法計算出的姿態(tài)角的差值構成。

建立的狀態(tài)方程如下：

(14)

狀態(tài)轉移矩陣:

(15)

噪音驅動矩陣:

測量方程為：

Z(t)=H(t)X(t)+V(t)

其中:H(t)=(I3×303×3)為測量矩陣，測量噪聲V(t)為零均值的高斯白噪聲，它的協(xié)方差矩陣為R(t)。

一般地，卡爾曼濾波器在計算機上運行，對狀態(tài)方程和測量方程采取離散化處理后，為了表述方便，這里狀態(tài)和測量方程的變量表示不變。卡爾曼濾波器可以看作狀態(tài)變量在由觀測生成的線性空間的映射，采用時域狀態(tài)空間法，對系統(tǒng)可觀測信號進行測量，以觀測量作為卡爾曼濾波器的輸入量，狀態(tài)量作為輸出量，根據(jù)觀測量修正狀態(tài)估計量進行迭代從而達到最優(yōu)估計，實現(xiàn)對系統(tǒng)的狀態(tài)進行估計的目的[15]。

2 實驗與測試

2.1 實驗條件

為了驗證算法對MEMS慣性測量單元姿態(tài)解算精度補償?shù)男Ч?，使用姿態(tài)精度為0.05°的三軸無磁轉臺對MIMU進行靜態(tài)姿態(tài)測試。利用安裝夾具將MEMS慣性測量單元固定在無磁轉臺上。

使用位置控制精度為0.1°的多功能三軸轉臺進行動態(tài)姿態(tài)測試。通過對比算法補償前后的姿態(tài)精度得出實驗結論。

被測MIMU如圖5所示，其主要器件包括：3個單軸MEMS陀螺儀CRG20、三軸MEMS加表ADIS16210、三軸MEMS磁強計選用HMC5883L磁力計，該芯片具有體積小、精度高、自帶數(shù)字接口的優(yōu)點。HMC5883L內(nèi)置偏差校準裝置，可以使得羅盤精度在1～2°。

圖5 被測MIMU實物圖

2.2 測試步驟

1)將被測MIMU安裝在無磁轉臺臺面中心，保證其軸向與轉臺轉動軸平行。

2)接通慣組電源，對被測MIMU完成預熱。

3)以固定步長分別轉動無磁轉臺3個軸向，在每一個靜止角位置采集被測器件原始數(shù)據(jù)，利用USB-TTL將數(shù)據(jù)發(fā)送至上位機進行采集并保存。

4)將被測MIMU安裝在慣性測量單元測試標定系統(tǒng)臺面中心，系統(tǒng)調(diào)至搖擺模式，采集被測器件原始數(shù)據(jù)并保存。

5)對原始數(shù)據(jù)進行處理并繪圖，取最大姿態(tài)誤差作姿態(tài)精度。

3 結果分析

在傳統(tǒng)的姿態(tài)解算方法中，加速度計并不參與航向角的解算，航向角的測量值完全依賴陀螺儀積分得到。受到陀螺儀的低頻積分噪聲干擾以及梯形近似積分引入的計算誤差影響，航向角漂移嚴重。因此，航向角的測量精度可以很好的反映器件姿態(tài)精度。

以靜態(tài)航向角和動態(tài)航向角的補償前后解算結果繪圖，得到圖6和圖7。

圖6 航向軸靜態(tài)姿態(tài)角

圖7 航向軸動態(tài)姿態(tài)角

圖6可以看出，經(jīng)過卡爾曼濾波對四元數(shù)的補償，四元數(shù)解算得到的航向角精度得到了明顯提高，在10°范圍內(nèi)并未發(fā)生明顯漂移。

從圖7可以看出，在動態(tài)測試過程中，經(jīng)過補償?shù)暮较蚪钦摲逯稻吔谵D臺搖擺幅度，具有良好的動態(tài)跟隨效應。相較于補償之前解算精度有了明顯改善。

4 結束語

綜上所述，通過卡爾曼濾波器對四元數(shù)的修正，MIMU的姿態(tài)解算精度得到了顯著提升。采用基于卡爾曼濾波的四元數(shù)姿態(tài)解算算法能夠增強MIMU的動態(tài)跟隨性能、提升姿態(tài)解算精度。通過實驗證明了該算法的可行性，在工程實踐中具有一定的應用價值。