基于誤差四元數(shù)的單兵導(dǎo)航系統(tǒng)算法*

于 飛，白紅美，葉 攀，高 偉，，趙 博

(1.哈爾濱工程大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱150001;2.哈爾濱工業(yè)大學(xué)鞍山研究院，遼寧 鞍山114000)

0 引言

單兵導(dǎo)航系統(tǒng)的一大難題在于如何獲得準(zhǔn)確的航向信息，為解決航向估計(jì)問題，減小單兵定位誤差，文獻(xiàn)［1］利用地圖匹配輔助微型慣性測(cè)量單元(MIMU)的算法對(duì)單兵進(jìn)行定位。此算法需針對(duì)已知環(huán)境，且計(jì)算量較大。文獻(xiàn)［2］采用啟發(fā)式偏移消除算法對(duì)單兵姿態(tài)進(jìn)行修正，但要求單兵處于直行運(yùn)動(dòng)狀態(tài)。

在磁干擾較弱的環(huán)境，考慮到MEMS陀螺和磁力計(jì)互補(bǔ)的性質(zhì)，本文提出基于誤差四元數(shù)的多傳感器信息融合算法。通過磁力計(jì)標(biāo)定技術(shù)補(bǔ)償外界磁干擾，并以MEMS加速度計(jì)和磁力計(jì)輸出為姿態(tài)基準(zhǔn)，利用卡爾曼濾波器對(duì)誤差四元數(shù)和陀螺漂移進(jìn)行估計(jì)，以減小單兵定位誤差。

1 磁力計(jì)標(biāo)定

每次使用前利用標(biāo)定實(shí)驗(yàn)對(duì)磁力計(jì)進(jìn)行標(biāo)定可有效補(bǔ)償外界磁干擾的影響［3］。標(biāo)定前后的磁場(chǎng)強(qiáng)度如圖1。

圖1 標(biāo)定前后的磁場(chǎng)強(qiáng)度Fig 1 Magnetic field intensity before and after calibration

由圖1可以看出，標(biāo)定前后的磁力計(jì)輸出存在明顯差距，通過標(biāo)定能夠有效補(bǔ)償外界磁場(chǎng)的影響。

2 多傳感器數(shù)據(jù)融合算法

基于誤差四元數(shù)的數(shù)據(jù)融合算法流程圖如圖2。

圖2 數(shù)據(jù)融合算法流程圖Fig 2 Flow chart of data fusion algorithm

首先利用MEMS陀螺獲得更新四元數(shù)，磁力計(jì)和加速度計(jì)獲得姿態(tài)基準(zhǔn);然后以誤差四元數(shù)和速度誤差為觀測(cè)量實(shí)現(xiàn)濾波更新;最后利用估計(jì)結(jié)果修正導(dǎo)航參數(shù)，得到修正后輸出。

2.1 狀態(tài)模型分析

以誤差四元數(shù)和陀螺漂移為狀態(tài)向量

其中，δqi(i=1，2，3，4)為四元數(shù)元素，εj(j=x，y，z)為陀螺漂移。

非線性系統(tǒng)模型為

其中，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣，G為噪聲驅(qū)動(dòng)矩陣，v為系統(tǒng)噪聲。

定義誤差向量δx為

其中，x(t)為濾波估計(jì)值，xr(t)為向量的真實(shí)值。在真值處進(jìn)行一階泰勒展開［4］，可得到關(guān)于誤差向量的模型為

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

由式(4)和式(5)得到誤差四元數(shù)的微分方程

對(duì)陀螺漂移進(jìn)行一階高斯馬爾科夫建模

其中，εj為陀螺漂移，βj為相關(guān)時(shí)間的倒數(shù)，σ2j為陀螺信號(hào)的協(xié)方差，v為零均值高斯白噪聲。

由式(6)和式(7)得到系統(tǒng)狀態(tài)模型

其中，F(xiàn)w和Fb分別為與w和βj相關(guān)的矩陣

2.2 量測(cè)模型分析

量測(cè)更新過程由MEMS加速度計(jì)、陀螺和磁力計(jì)共同完成。姿態(tài)基準(zhǔn)測(cè)量由MEMS加速度計(jì)、磁力計(jì)獲得。陀螺主要用于濾波更新階段。由陀螺輸出可得到旋轉(zhuǎn)四元數(shù)［5］q(w)

其中，k為某一時(shí)刻，Δt為采樣間隔。

在磁干擾較弱的地方，可由標(biāo)定后的磁力計(jì)輸出得到航向值［6］。將載體系的磁力計(jì)測(cè)量值Bbk投影到導(dǎo)航系

其中，ψ為航向角，Md為磁偏角。

加速度計(jì)主要用于獲得水平姿態(tài)角［7］。利用三軸MEMS加速度輸出計(jì)算得到橫搖角θ和縱搖角γ

在得到歐拉角后，可根據(jù)式(15)求解捷聯(lián)矩陣，再利用捷聯(lián)矩陣和四元數(shù)的關(guān)系式(16)得到四元數(shù)q(ε)。

以q(w)與q(ε)之差為觀測(cè)量，實(shí)現(xiàn)量測(cè)更新過程。量測(cè)模型為

3 場(chǎng)地實(shí)驗(yàn)

采用荷蘭Xsens公司的MTi—G型IMU進(jìn)行實(shí)驗(yàn)。場(chǎng)地為哈爾濱工程大學(xué)籃球場(chǎng)。首先將MIMU安裝在鞋上，并通過USB與PC連接;然后對(duì)磁力計(jì)進(jìn)行標(biāo)定，并采集靜態(tài)數(shù)據(jù)以獲得導(dǎo)航參數(shù)的初值;最后選取行走軌跡，行走時(shí)間約15 min，路程約80 m。利用谷歌地圖可得到運(yùn)動(dòng)軌跡如圖3所示。其中，五角星標(biāo)記的曲線代表運(yùn)動(dòng)軌跡，三角代表開始點(diǎn)。

圖3 運(yùn)動(dòng)軌跡Fig 3 Trajectory

磁力計(jì)輸出航向和濾波后的航向?qū)Ρ热鐖D4。

圖4 航向?qū)Ρ葓DFig 4 Heading comparison diagram

由圖4可以看出:磁力計(jì)輸出航向與濾波后的航向存在較大差距。在130～160 s之間磁力計(jì)輸出較為準(zhǔn)確，由于外界磁干擾的影響，其他時(shí)間的磁力計(jì)航向波動(dòng)較大。濾波后的航向更能夠反映單兵真實(shí)運(yùn)動(dòng)軌跡。

基于誤差四元數(shù)的數(shù)據(jù)融合算法得到的單兵運(yùn)動(dòng)軌跡如圖5。

圖5 單兵運(yùn)動(dòng)軌跡Fig 5 Personal trajectory

由圖5和圖3對(duì)比可以看出:圖5中的運(yùn)動(dòng)軌跡與實(shí)驗(yàn)軌跡相符，此算法能夠完成單兵定位功能。運(yùn)動(dòng)過程中的單兵位置誤差如表1所示。

表1 單兵位置誤差Tab 1 Personal location error

由表1可以看出:運(yùn)動(dòng)過程中的最大誤差為2.194 m，約占總步行距離的2.74%;平均誤差為1.269 m，約占總距離的1.58%;返回起始點(diǎn)時(shí)的誤差約為2.184 m，約占總距離的2.73%，故其全局誤差小于總步行距離的3%。

4 結(jié)論

本文所提的基于誤差四元數(shù)的數(shù)據(jù)融合算法充分利用了MEMS傳感器的信息，實(shí)驗(yàn)證明該算法能夠?qū)伪M(jìn)行準(zhǔn)確定位。在無外界信號(hào)下，當(dāng)步行距離為80 m時(shí)，該算法的全局誤差不超過總距離的3%，可以滿足單兵的自主定位要求。

［1］Aggarwal P，Thomas D，Ojeda L，et al.Map matching and heuristic elimination of gyro drift for personal navigation systems in GPS-denied conditions［J］.Measurement Science and Technology，2011，22(2):025205.

［2］Borenstein J，Ojeda L.Heuristic drift elimination for personnel tracking systems［J］.Journal of Navigation，2010，63(4):591-606.

［3］Renaudin V，Afzal M H，Lachapelle G.New method for magnetometers-based orientation estimation［C］∥Position Location and Navigation Symposium(PLANS)，2010 IEEE/ION，IEEE，2010:348-356.

［4］孫紅星，李德仁.非線性系統(tǒng)中卡爾曼濾波的一種新線性化方法［J］.武漢大學(xué)學(xué)報(bào):信息科學(xué)版，2004，29(4):346-348.

［5］田亞軍，周 剛，胡軍照，等.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)算法實(shí)現(xiàn)及工程應(yīng)用［J］.四川兵工學(xué)報(bào)，2011，32(1):10-12.

［6］Afzal M H，Renaudin V，Lachapelle G.Magnetic field-based heading estimation for pedestrian navigation environments［C］∥2011 International Conference on Indoor Positioning and Indoor Navigation(IPIN)，IEEE，2011:1-10.

［7］Luinge H J，Veltink P H.Measuring orientation of human body segments sing miniature gyroscopes and accelerometers［J］.Medical and Biological Engineering and Computing，2005，43(2):273-282.