基于卡爾曼濾波的MIMU姿態(tài)解算算法研究

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

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

0 引言

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

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

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

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

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

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

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

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

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

1.1 坐標(biāo)系建立

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

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

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

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

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

圖3 坐標(biāo)系示意圖

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

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

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

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

1.2.1.1 四元數(shù)初始化

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

(1)

1.2.1.2 四元數(shù)更新

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

(2)

寫成矩陣形式為:

(3)

即:

(4)

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

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

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

(5)

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

(6)

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

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

(7)

(8)

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

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

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

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

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

(9)

(10)

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

(11)

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

(12)

從而計(jì)算出載體相對(duì)地磁北極的航向角：

(13)

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

1.3 卡爾曼濾波器構(gòu)建

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

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

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

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

(14)

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

(15)

噪音驅(qū)動(dòng)矩陣:

測(cè)量方程為：

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

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

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

2 實(shí)驗(yàn)與測(cè)試

2.1 實(shí)驗(yàn)條件

為了驗(yàn)證算法對(duì)MEMS慣性測(cè)量單元姿態(tài)解算精度補(bǔ)償?shù)男Ч褂米藨B(tài)精度為0.05°的三軸無磁轉(zhuǎn)臺(tái)對(duì)MIMU進(jìn)行靜態(tài)姿態(tài)測(cè)試。利用安裝夾具將MEMS慣性測(cè)量單元固定在無磁轉(zhuǎn)臺(tái)上。

使用位置控制精度為0.1°的多功能三軸轉(zhuǎn)臺(tái)進(jìn)行動(dòng)態(tài)姿態(tài)測(cè)試。通過對(duì)比算法補(bǔ)償前后的姿態(tài)精度得出實(shí)驗(yàn)結(jié)論。

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

圖5 被測(cè)MIMU實(shí)物圖

2.2 測(cè)試步驟

1)將被測(cè)MIMU安裝在無磁轉(zhuǎn)臺(tái)臺(tái)面中心，保證其軸向與轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)軸平行。

2)接通慣組電源，對(duì)被測(cè)MIMU完成預(yù)熱。

3)以固定步長(zhǎng)分別轉(zhuǎn)動(dòng)無磁轉(zhuǎn)臺(tái)3個(gè)軸向，在每一個(gè)靜止角位置采集被測(cè)器件原始數(shù)據(jù)，利用USB-TTL將數(shù)據(jù)發(fā)送至上位機(jī)進(jìn)行采集并保存。

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

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

3 結(jié)果分析

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

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

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

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

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

從圖7可以看出，在動(dòng)態(tài)測(cè)試過程中，經(jīng)過補(bǔ)償?shù)暮较蚪钦?fù)峰值均趨近于轉(zhuǎn)臺(tái)搖擺幅度，具有良好的動(dòng)態(tài)跟隨效應(yīng)。相較于補(bǔ)償之前解算精度有了明顯改善。

4 結(jié)束語

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