基于INS/GPS/磁力計(jì)的全組合導(dǎo)航*

梅玲玉， 馬 磊， 張 濤

(西南交通大學(xué) 電氣學(xué)院，四川 成都610031)

0 引 言

隨著傳感器技術(shù)的迅猛發(fā)展，各種面向復(fù)雜應(yīng)用背景的多傳感器組合導(dǎo)航系統(tǒng)也隨之大量涌現(xiàn)。目前應(yīng)用最為廣泛的是由慣性導(dǎo)航系統(tǒng)[1](inertial navigation system,INS)和全球定位系統(tǒng)[2](global positioning system,GPS)構(gòu)成的組合導(dǎo)航系統(tǒng)[3～5]，如 INS/GPS,INS/北斗等。INS/GPS組合導(dǎo)航系統(tǒng)，具有良好的互補(bǔ)性，克服了純慣性導(dǎo)航系統(tǒng)誤差隨時(shí)間累積的缺點(diǎn)，同時(shí)在GPS信號(hào)受到遮擋時(shí)慣性導(dǎo)航系統(tǒng)短期內(nèi)能提供較高精度的導(dǎo)航信息。然而該系統(tǒng)存在著載體航向角誤差可觀測(cè)性較差的問(wèn)題。地磁導(dǎo)航具有完全自主、抗干擾能力強(qiáng)、無(wú)累積誤差的優(yōu)點(diǎn)，可以作為慣導(dǎo)的輔助實(shí)現(xiàn)快速對(duì)準(zhǔn)，同時(shí)提供實(shí)時(shí)的航向參考，從而彌補(bǔ)慣性測(cè)量單元(inertial measurement unit,IMU)與GPS組合導(dǎo)航系統(tǒng)的不足[6]。

本文首先對(duì)傳感器數(shù)據(jù)進(jìn)行預(yù)處理，其次推導(dǎo)基于誤差四元數(shù)的定姿系統(tǒng)方程和量測(cè)方程，利用加速度、磁力計(jì)修正陀螺姿態(tài)信息；給出定位系統(tǒng)方程和量測(cè)方程，利用GPS修正陀螺位置、速度信息。最后在移動(dòng)平臺(tái)上搭建組合導(dǎo)航系統(tǒng)，進(jìn)行實(shí)驗(yàn)驗(yàn)證。

1 數(shù)據(jù)預(yù)處理

磁力計(jì)和陀螺儀傳感器作為組合導(dǎo)航中的關(guān)鍵器件[7]，不可避免地存在各種誤差，為提高器件的可靠性、降低器件對(duì)導(dǎo)航精度的影響，需對(duì)傳感器數(shù)據(jù)進(jìn)行預(yù)處理。

1.1 磁力計(jì)數(shù)據(jù)處理

在某一地區(qū)，地磁強(qiáng)度可視為恒定的向量。因此,在只有地磁場(chǎng)的作用下，磁力計(jì)測(cè)量值的水平分量為圓形。但實(shí)際環(huán)境中存在各種磁干擾[8]，水平分量形成的圓被拉成圓心偏移的橢圓。環(huán)境干擾具體可表示為

(1)

式中K,Φp,Φs為與軟磁有關(guān)的誤差，bx，by分別為和硬磁材料有關(guān)的誤差。

傳統(tǒng)的擬合多采用最小二乘法求解橢圓參數(shù)估計(jì)值。該方法至少需要在橢圓上均勻分布的5個(gè)點(diǎn)，同時(shí)隨機(jī)誤差將嚴(yán)重影響到估計(jì)值的計(jì)算。一種解決方法是盡可能多地采集橢圓上的數(shù)據(jù)點(diǎn)，但數(shù)據(jù)點(diǎn)的增加將導(dǎo)致計(jì)算量的增大，并要求更多的存儲(chǔ)空間，不利于整個(gè)導(dǎo)航系統(tǒng)的實(shí)現(xiàn)。因此需要一種在線校正方法，本文采用遞推最小二乘法實(shí)現(xiàn)羅差補(bǔ)償,最小二乘法的遞推公式如下為[9]

(2)

(3)

式中B為與磁場(chǎng)強(qiáng)度有關(guān)的參數(shù)。求解出橢圓參數(shù)后，根據(jù)式(1)即可得到誤差系數(shù)，從而對(duì)磁力計(jì)進(jìn)行校正。

1.2 陀螺儀數(shù)據(jù)處理

陀螺儀的預(yù)處理一般采用時(shí)間序列分析法[10]，建立自回歸滑動(dòng)平均模型(auto regressive moving average model,ARMA)

yk-φ1yk-1-Lφpyk-p=ak-θ1ak-1-L-θqak-q

(4)

式中yi為時(shí)間序列;φi,ai分別為自回歸系數(shù)和滑動(dòng)平均系數(shù)。得到陀螺儀輸出噪聲的ARMA模型后，進(jìn)行隨機(jī)噪聲的預(yù)判并在實(shí)時(shí)數(shù)據(jù)中抑制該部分噪聲。隨機(jī)噪聲序列看作由一白噪聲信號(hào)經(jīng)過(guò)一系列線性變換得到的，符合卡爾曼濾波條件。將式(4)擴(kuò)展成卡爾曼濾波方程，即

Y=Xφ+Aθ

(5)

式中 取X=[yk-1yk-p]T。

2 組合導(dǎo)航系統(tǒng)

本文研究了GPS/INS/磁力計(jì)全組合導(dǎo)航系統(tǒng)，分別就定姿系統(tǒng)和定位系統(tǒng)建立系統(tǒng)方程和量測(cè)方程，采用卡爾曼濾波器[11]進(jìn)行信息融合，整體框架如圖1所示。

圖1 組合導(dǎo)航系統(tǒng)整體框圖

2.1 定姿系統(tǒng)

傳統(tǒng)的GPS/INS組合導(dǎo)航系統(tǒng)中，將平臺(tái)誤差角近似等同于姿態(tài)誤差角進(jìn)行姿態(tài)校正，從而導(dǎo)致較大的模型誤差。本文直接以誤差四元數(shù)為狀態(tài)量，建立誤差四元數(shù)模型進(jìn)行信息融合。

2.1.1 系統(tǒng)方程

2.1.2 觀測(cè)方程

在某一地區(qū)，重力加速度和地磁強(qiáng)度可視為恒定的向量。由加速度計(jì)和磁力計(jì)數(shù)據(jù)可得到三軸姿態(tài)

(6)

(7)

2.2 定位系統(tǒng)

2.2.1 系統(tǒng)方程

根據(jù)捷聯(lián)慣導(dǎo)解算方程，可推導(dǎo)出位置誤差、速度誤差、平臺(tái)失準(zhǔn)角誤差方程，本文直接給出各方程為

(8)

式中F11～F34參見(jiàn)文獻(xiàn)[1]。本文僅考慮傳感器的常值零偏和隨機(jī)游走誤差，則陀螺儀和加速度的誤差可寫(xiě)為

(9)

式中bg,εg,ba,εa分別為陀螺儀和加速度計(jì)的零偏誤差和隨機(jī)游走誤差。常值零偏的特性可知

(10)

將式(9)、式(10)代入式(8)，可得系統(tǒng)方程

(11)

2.2.2 量測(cè)方程

將慣性導(dǎo)航系統(tǒng)解算的位置、速度信息與GPS測(cè)量得到的位置、速度信息的差值作為系統(tǒng)的量測(cè)信息?？紤]組合導(dǎo)航應(yīng)用于移動(dòng)平臺(tái)上，暫不考慮載體的高度信息。

位置量測(cè)方程如下

(12)

式中xI,yI為慣性導(dǎo)航解算得到的東、北向位置信息;xG,yG為GPS測(cè)量得到的東向北向位置信息;Hp=I2×2，Vp=[npxnpy]T。同理可以得到，速度量測(cè)方程

(13)

式中vxI,vyI為慣性導(dǎo)航解算得到的東、北向速度;vxG,vyG為GPS測(cè)量得到的東、北向速度;Hp=I2×2，Vv=[nvxnvy]T。合并位置、速度量測(cè)方程得到定位系統(tǒng)量測(cè)方程

(14)

3 實(shí)驗(yàn)與結(jié)果分析

核心處理器采用STM32，IMU采用ADIS16405，輸出頻率為81.9 Hz，GPS采用諾瓦泰公司的OEM615板卡，輸出頻率為1 Hz。實(shí)驗(yàn)過(guò)程中，將IMU和GPS搭載在差分移動(dòng)平臺(tái)上，測(cè)試區(qū)域?yàn)?65 m×70 m的方形區(qū)域，移動(dòng)平臺(tái)以一定的速度沿實(shí)驗(yàn)路線運(yùn)動(dòng)。實(shí)驗(yàn)路線標(biāo)記軌跡如圖2。

圖2 實(shí)驗(yàn)路線

跑車實(shí)驗(yàn)前，差分移動(dòng)平臺(tái)勻速自轉(zhuǎn)1周，得到磁力計(jì)校正參數(shù)。圖3中，實(shí)線為校正前的數(shù)據(jù)，虛線為校正后的數(shù)據(jù)。校正后的磁力計(jì)x軸,y軸軌跡為圓心在原點(diǎn)的圓，航向角趨于直線，對(duì)應(yīng)差分移動(dòng)平臺(tái)勻速自轉(zhuǎn)。校正前后航向誤差最大可達(dá)23.9°?？梢钥闯?，遞推最小二乘法較好地修正了環(huán)境誤差。

圖3 最小二乘法處理對(duì)比

實(shí)驗(yàn)姿態(tài)結(jié)果如圖4，實(shí)線為濾波前的觀測(cè)姿態(tài)角，即加速度計(jì)、磁力計(jì)計(jì)算出的姿態(tài)，虛線為卡爾曼濾波后的姿態(tài)角。由于采用了反饋校正，本文不再給出慣性導(dǎo)航的姿態(tài)信息?？梢钥闯?，濾波后橫滾角和俯仰角控制在4°以內(nèi)?？柭鼮V波法解決了單慣導(dǎo)系統(tǒng)漂移的問(wèn)題，同時(shí)精度與單獨(dú)的加計(jì)、磁力計(jì)解算相比也得到了較大改善。

圖4 卡爾曼濾波姿態(tài)對(duì)比

實(shí)驗(yàn)定位結(jié)果如圖5所示,實(shí)線為GPS測(cè)量的位置信息，虛線為卡爾曼濾波后的位置信息。GPS軌跡突出部分的數(shù)據(jù)明顯異常，可看作GPS失效點(diǎn)?？梢钥闯觯M合導(dǎo)航系統(tǒng)抑制了陀螺單獨(dú)解算的發(fā)散，同時(shí)在GPS失效時(shí)，仍能很好地工作。

圖5 卡爾曼濾波位置對(duì)比

速度測(cè)量結(jié)果如圖6所示，實(shí)線為GPS測(cè)量的速度信息，虛線為濾波后的速度信息。由于移動(dòng)平臺(tái)的速度較慢，導(dǎo)致速度對(duì)比不夠明顯，濾波前后速度的標(biāo)準(zhǔn)差如下：GPS測(cè)量值東向速度標(biāo)準(zhǔn)差為0.146 068 58 m/s，北向速度標(biāo)準(zhǔn)差為0.084 437 32 m/s；卡爾曼濾波東向速度標(biāo)準(zhǔn)差為0.145 171 12 m/s，北向速度方差為0.084 024 41 m/s。該結(jié)果表明組合導(dǎo)航系統(tǒng)的性能有所改善。

圖6 卡爾曼濾波速度對(duì)比

4 結(jié)束語(yǔ)

本文建立了多傳感器信息融合的組合導(dǎo)航系統(tǒng)，針對(duì)定姿、定位系統(tǒng)分別建模并進(jìn)行了實(shí)驗(yàn)驗(yàn)證。最終結(jié)果表明組合導(dǎo)航系統(tǒng)相比慣性導(dǎo)航和GPS定位得到了有效改善。但該模型中將GPS的誤差看作白噪聲處理，可進(jìn)一步對(duì)GPS進(jìn)行誤差建模，進(jìn)行GPS/INS緊組合研究。