彭宏偉，梁列全，溫梓呈，周 璇，童桂超

（1.上海創(chuàng)感傳感技術(shù)有限公司，上海 200125；2.廣東財(cái)經(jīng)大學(xué) 信息學(xué)院，廣東 廣州 510320；3.華南理工大學(xué) 機(jī)械與汽車工程學(xué)院，廣東 廣州 510641）

0 引 言

組合導(dǎo)航是提高導(dǎo)航系統(tǒng)整體性能的有效技術(shù)手段。INS/GPS組合導(dǎo)航是當(dāng)前最常見的組合導(dǎo)航技術(shù)，由GPS導(dǎo)航系統(tǒng)和慣性導(dǎo)航系統(tǒng)（Inertial Navigation System, INS）集成，它將兩種導(dǎo)航技術(shù)進(jìn)行優(yōu)勢互補(bǔ)，在民用和軍用領(lǐng)域得到廣泛應(yīng)用。

INS/GPS組合導(dǎo)航精度的提高近年來引起了諸多研究人員的廣泛關(guān)注。然而，GPS短期失效環(huán)境下，低成本的慣性導(dǎo)航器件測量誤差會迅速累積，導(dǎo)致導(dǎo)航精度迅速下降。改進(jìn)濾波算法能夠在一定程度上改進(jìn)導(dǎo)航精度，但時間復(fù)雜度較高，且難以保證GPS失效時的導(dǎo)航精度，常用算法包括粒子濾波算法、自適應(yīng)濾波算法、人工智能算法、人工神經(jīng)網(wǎng)絡(luò)算法、模糊邏輯算法等。另一種方法為通過選擇易于集成的傳感器為系統(tǒng)提供輔助導(dǎo)航信息，以改善GPS衛(wèi)星信號斷續(xù)情況下的定位精度，提高系統(tǒng)穩(wěn)定性。例如，Du S等采用里程計(jì)數(shù)據(jù)改善組合導(dǎo)航精度；黃劍雄等人對視覺/慣導(dǎo)無人機(jī)組合導(dǎo)航算法展開研究，提出了自適應(yīng)漸消卡爾曼濾波算法以降低視覺位姿信息滯后帶來的影響；徐士厚等利用電子羅盤和里程儀的輸出對INS/GPS數(shù)據(jù)進(jìn)行修正；郭美玲等使用輪速傳感器、加速度計(jì)、橫擺角傳感器、方向盤傳感器和慣性傳感器構(gòu)成組合導(dǎo)航系統(tǒng)；蔣長輝等引入雷達(dá)和氣壓計(jì)的速度數(shù)據(jù)和高度數(shù)據(jù)修正INS/GPS的累積誤差，這些方法均在不同程度上提高了系統(tǒng)的定位精度。

地磁是地球的固有屬性，地磁信息分布廣泛，獲取方便，同時，地磁信息是具有大小和方向的向量，可以視為導(dǎo)航信息的可靠來源。現(xiàn)有的地磁檢測傳感器分別基于3種不同的電磁效應(yīng)，即磁感式（Magneto-Inductive）傳感器、霍爾傳感器和磁阻式（Magneto-Resistive）傳感器?；魻柕卮艂鞲衅骱蚆R地磁傳感器均存在精度不足、功耗較高、溫度特性不穩(wěn)定等問題。而搭載MI元件的傳感器在精度、溫度特性、磁滯性能等方面比另外兩種傳感器表現(xiàn)更優(yōu)秀。磁感式地磁傳感器具有霍爾地磁傳感器1萬倍以上的靈敏度。為此，本文選擇MI地磁傳感器RM3100作為地磁檢測傳感器。

MI/GPS/INS組合導(dǎo)航裝置在多種傳感器數(shù)據(jù)采集過程中可能會引入各種干擾噪聲，直接影響導(dǎo)航系統(tǒng)的測量精度。為此，本文提出了MI/GPS/INS組合導(dǎo)航方法，以提高GPS間歇性失效時的導(dǎo)航定位精度。

1 MI/GPS/INS組合導(dǎo)航裝置硬件設(shè)計(jì)

圖1 硬件結(jié)構(gòu)框架

2 組合導(dǎo)航方法

MI/GPS/INS組合導(dǎo)航方法指利用MI傳感器修正慣性導(dǎo)航裝置的積分誤差，以提高GPS失效時的導(dǎo)航精度。組合導(dǎo)航方法流程如圖2所示，分為數(shù)據(jù)采集、數(shù)據(jù)濾波、數(shù)據(jù)校正、四元數(shù)解算、數(shù)據(jù)融合等步驟，最終可輸出的參數(shù)包括姿態(tài)歐拉角與軌跡坐標(biāo)兩部分。

圖2 組合導(dǎo)航方法流程

2.1 數(shù)據(jù)采集

本裝置采集的數(shù)據(jù)包括GPS數(shù)據(jù)、加速度計(jì)數(shù)據(jù)、陀螺儀數(shù)據(jù)和地磁數(shù)據(jù)。GPS數(shù)據(jù)采集頻率與慣性導(dǎo)航、地磁數(shù)據(jù)的采集頻率難以同步，GPS采集頻率較低。當(dāng)GPS采集間隔或失效時，地磁數(shù)據(jù)作為慣性導(dǎo)航數(shù)據(jù)的輔助數(shù)據(jù)，輸出慣性導(dǎo)航數(shù)據(jù)，當(dāng)新的GPS數(shù)據(jù)采集完成時，融合慣性導(dǎo)航數(shù)據(jù)和GPS數(shù)據(jù)輸出軌跡坐標(biāo)。

2.2 數(shù)據(jù)濾波

環(huán)境溫度變化和裝置運(yùn)動對加速度計(jì)的干擾較大，經(jīng)對比實(shí)驗(yàn)發(fā)現(xiàn)，滑動窗口平均濾波算法與卡爾曼濾波算法相比，加速度計(jì)數(shù)據(jù)的濾波效果相差不大，選用計(jì)算量較小的滑動窗口平均濾波算法改善加速度計(jì)和陀螺儀采集的數(shù)據(jù)。

地磁傳感器誤差主要來自地磁場的隨機(jī)干擾，且磁場方向和大小不確定，一階卡爾曼濾波算法對高頻和隨機(jī)干擾噪聲的濾波效果較好，且響應(yīng)速度較快。因此，地磁傳感器數(shù)據(jù)采用卡爾曼濾波算法降低干擾。

2.3 數(shù)據(jù)校正

加速度計(jì)在靜置狀態(tài)時存在重力加速度；地磁傳感器對工作環(huán)境的硬磁干擾十分敏感，且地磁傳感器、慣性測量模塊（加速度計(jì)和陀螺儀）的安裝位置存在一定距離，地磁傳感器的三軸坐標(biāo)系與慣性測量單元坐標(biāo)系的零點(diǎn)不在同一點(diǎn)上，因此，需要對地磁傳感器、加速度計(jì)以及陀螺儀進(jìn)行零點(diǎn)偏移校正，校正數(shù)值通過初始標(biāo)定試驗(yàn)得到。

（1）加速度計(jì)初始標(biāo)定方法

1.3 標(biāo)準(zhǔn)溶液的配制 根據(jù)需求，分別吸取適量的各腹瀉性貝類毒素標(biāo)準(zhǔn)溶液于5 mL棕色容量瓶中，用50%甲醇水溶液稀釋并定容，配制成標(biāo)準(zhǔn)儲備液。用空白基質(zhì)液稀釋儲備液配制成各個不同濃度的標(biāo)準(zhǔn)工作液。各類標(biāo)準(zhǔn)溶液避光保存于-12 ℃。

加速度計(jì)用于測量載體的加速度。理論上，三軸加速度計(jì)采集的數(shù)據(jù)在空間上組成圓球，而圓球的球心為坐標(biāo)軸的零點(diǎn)，球半徑為重力加速度的數(shù)值9.8 m/s。加速度計(jì)零點(diǎn)漂移通過基于六位取樣法的單軸零點(diǎn)校正算法進(jìn)行校準(zhǔn)。校正所用算法如式（1）所示：

式中：，，為靈敏度；，，為測量值；，，為偏移量。

（2）陀螺儀初始標(biāo)定方法

靜止條件下，陀螺儀理論上三軸輸出均為0，實(shí)際測量會有偏移，通過平均偏移參數(shù)校正法進(jìn)行校正。

（3）地磁傳感器初始標(biāo)定方法

根據(jù)地磁傳感器原理，理論上磁場數(shù)據(jù)同樣在空間上構(gòu)成球心在零點(diǎn)、球半徑為的球。但實(shí)際上，存在軟硬磁干擾以及傳感器加工誤差，得出的磁場數(shù)據(jù)組成橢球，因此通過校正橢球成為球形，就能完成地磁傳感器的零點(diǎn)校正。地磁傳感器的初始標(biāo)定方法采用空間八字環(huán)繞法獲取，采用各軸數(shù)據(jù)最大、最小值的平均校正三軸數(shù)據(jù)。地磁傳感器校正前后對比如圖3所示。

圖3 地磁傳感器數(shù)據(jù)修正前后對比圖

2.4 四元數(shù)解算

2.4.1 四元數(shù)法

載體的姿態(tài)解算是實(shí)現(xiàn)慣性導(dǎo)航的核心技術(shù)之一，通過姿態(tài)解算可以通過得到的載體姿態(tài)和導(dǎo)航參數(shù)計(jì)算所需參數(shù)。

目前常用的坐標(biāo)系包括載體坐標(biāo)系（b系）、WGS84坐標(biāo)系（GPS坐標(biāo)）和東北天坐標(biāo)系（n系）。載體坐標(biāo)系主要反映載體方向；慣性導(dǎo)航傳感器數(shù)據(jù)一般是基于載體的坐標(biāo)系。不同坐標(biāo)系之間的數(shù)據(jù)必須轉(zhuǎn)換到統(tǒng)一坐標(biāo)系下才能融合。

描述載體坐標(biāo)相對地理坐標(biāo)系方位關(guān)系的常用表示方法主要有3種，分別是歐拉角法、方向余弦法以及四元數(shù)法。歐拉角法又稱為三參數(shù)法，該方法由于不能用于全姿態(tài)飛行運(yùn)載體而難以廣泛用于工程實(shí)踐，且實(shí)時計(jì)算困難。方向余弦法又稱為九參數(shù)法，該方法避免了“奇點(diǎn)”現(xiàn)象的出現(xiàn)，但方程計(jì)算量大，效率較低。四元數(shù)法又稱為四參數(shù)法，其由1個實(shí)數(shù)和3個復(fù)數(shù)組成，數(shù)學(xué)形式可以寫成=+i+j+k，其中 i=j=k=-1，，，，為實(shí)數(shù)。對于任意在載體坐標(biāo)系下的三維向量，可以通過四元數(shù)法將其轉(zhuǎn)換為地理坐標(biāo)系下的三維向量，如式（2）所示：

2.4.2 四元數(shù)解算

將上述方程寫成矩陣的形式：

四元數(shù)更新：

求出每個時刻的角速度ω，ω，ω就可以更新四元數(shù)。

2.4.3 加速度計(jì)與地磁傳感器誤差計(jì)算

陀螺儀可以測量三軸角速度，但陀螺儀依賴積分，而且存在漂移，所以不能只用陀螺儀測量的數(shù)據(jù)作為四元數(shù)更新的依據(jù)。本裝置通過加速度計(jì)以及地磁傳感器的數(shù)據(jù)融合校正陀螺儀的角速度，實(shí)現(xiàn)精確的姿態(tài)表示。加速度計(jì)和地磁傳感器的誤差計(jì)算包括數(shù)據(jù)歸一化、坐標(biāo)變換以及誤差計(jì)算。加速度計(jì)利用地球重力加速度來修正誤差。載體僅受重力時，重力加速度為g，而對于加速度計(jì)所測量的三軸加速度，通過坐標(biāo)變換可以獲得合加速度，但由于加速度計(jì)的測量誤差，導(dǎo)致測量值與實(shí)際值存在偏差，因此通過四元數(shù)姿態(tài)值估算加速度分量。地磁傳感器的誤差校正方法原理：地磁線方向從地理南極指向地理北極，地理坐標(biāo)系下地磁矢量可以分解至軸和軸，軸方向（東西方向）分量為0。從地磁傳感器獲得的磁場數(shù)據(jù)通過坐標(biāo)變換換算為地理坐標(biāo)系下的磁場矢量，這兩組數(shù)據(jù)代表相同磁場，但由于測量誤差，這兩個磁場值不能完全重合，因此利用向量積描述該誤差，并且通過反饋誤差不斷減小該誤差，使得傳感器數(shù)據(jù)不斷逼近實(shí)際值，實(shí)現(xiàn)補(bǔ)償矯正。

2.4.4 誤差數(shù)據(jù)融合與角速度數(shù)據(jù)修正

加速度計(jì)數(shù)據(jù)在載體坐標(biāo)系下測得，地磁傳感器數(shù)據(jù)在地理坐標(biāo)系下測得，上述2個誤差向量的叉乘可以求出向量之間的夾角，即歐拉角。然后利用加速度計(jì)和地磁傳感器測量誤差的叉乘值修正四元數(shù)解算的陀螺儀角速度。

2.5 組合導(dǎo)航

載體軌跡通過計(jì)算每個采樣時間點(diǎn)的坐標(biāo)位置，經(jīng)連接后形成軌跡。坐標(biāo)位置數(shù)據(jù)來源于兩方面，一是加速度測量得到的地理坐標(biāo)系下的加速度，根據(jù)牛頓第二定律，通過積分可以得到位移，但由于加速度計(jì)自身的精度問題，使得載體自帶加速度，在兩次積分的作用下，隨著時間的推移，位移誤差累積導(dǎo)致加速度輸出不可靠，故論文采用四元數(shù)姿態(tài)矩陣在線更新、修正加速度累積誤差；二是GPS的衛(wèi)星定位信息，可以提供絕對位置信息，但GPS相對刷新速度較慢，且當(dāng)GPS間歇性失效時，缺乏定位信息。因此，根據(jù)不同情況對兩組數(shù)據(jù)進(jìn)行融合，提高定位精度。

慣性導(dǎo)航坐標(biāo)位置更新步驟：加速度各軸分量的坐標(biāo)變換、加速度積分、速度積分、慣性導(dǎo)航數(shù)據(jù)更新；GPS坐標(biāo)更新步驟：GPS數(shù)據(jù)采集、坐標(biāo)系變換、GPS數(shù)據(jù)更新。

當(dāng)GPS采樣時間到后，判斷GPS導(dǎo)航信息是否正常，若GPS導(dǎo)航信息不正常，則輸出慣性導(dǎo)航軌跡；若GPS導(dǎo)航信息正常，則采用二階互補(bǔ)濾波對GPS位移數(shù)據(jù)和慣性位移數(shù)據(jù)進(jìn)行加權(quán)融合，并通過兩組位移的差值作為反饋，調(diào)整位移大小，最終輸出組合導(dǎo)航軌跡。

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

論文設(shè)計(jì)了室內(nèi)和室外兩種導(dǎo)航實(shí)驗(yàn)，室外導(dǎo)航實(shí)驗(yàn)和室內(nèi)導(dǎo)航實(shí)驗(yàn)分別用于驗(yàn)證GPS信號正常和失效時，論文提出的組合導(dǎo)航方法的導(dǎo)航精度是否優(yōu)于慣性組合導(dǎo)航的精度。

3.1 室外導(dǎo)航實(shí)驗(yàn)

室外導(dǎo)航實(shí)驗(yàn)進(jìn)行了2組，分別選擇某大學(xué)內(nèi)部教學(xué)樓外某路徑的2個方向進(jìn)行，每組實(shí)驗(yàn)分為2次，一次僅采用GPS導(dǎo)航，另一次采用論文的組合導(dǎo)航裝置。第一次實(shí)驗(yàn)的路徑長度為200 m，實(shí)驗(yàn)沿路徑的西邊緣向南行走，軌跡如圖4（a）所示；第二次行走實(shí)驗(yàn)的路徑長為150 m，實(shí)驗(yàn)沿路徑的北邊緣向東行走，軌跡如圖4（b）所示。紅色線為僅采用GPS導(dǎo)航的行走軌跡，藍(lán)色線為組合導(dǎo)航軌跡。

圖4 室外導(dǎo)航實(shí)驗(yàn)軌跡對比圖

從圖4可以看出，論文研制的組合導(dǎo)航方法的導(dǎo)航軌跡比GPS導(dǎo)航更平滑，更接近實(shí)際的行走軌跡。因此，當(dāng)GPS導(dǎo)航有效時，論文研制的組合導(dǎo)航裝置導(dǎo)航精度高于單一的GPS導(dǎo)航。

3.2 室內(nèi)導(dǎo)航實(shí)驗(yàn)

室內(nèi)導(dǎo)航實(shí)驗(yàn)主要驗(yàn)證GPS失效情況下的導(dǎo)航效果，分別選擇教學(xué)樓室內(nèi)走廊150 m和半室內(nèi)走廊200 m進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)1在室內(nèi)走廊進(jìn)行，室內(nèi)為全封閉環(huán)境，GPS信號微弱，室內(nèi)走廊邊線用黑色線標(biāo)出，紅色代表實(shí)驗(yàn)時真實(shí)行走軌跡，藍(lán)色代表組合導(dǎo)航裝置的輸出軌跡。室內(nèi)走廊寬度2.35 m，實(shí)驗(yàn)時步行速度大約為1.2 m/s，實(shí)驗(yàn)時間約為125 s。行走先向南，而后轉(zhuǎn)向西，如圖5中藍(lán)色箭頭所示。實(shí)驗(yàn)2的室外走廊是一條連接不同教學(xué)樓的連廊，屬于半室外環(huán)境，GPS信號時斷時續(xù)。

圖5 室內(nèi)組合導(dǎo)航軌跡與真實(shí)行走軌跡對比圖

從圖5可以看出，無論是GPS完全失效環(huán)境還是部分失效環(huán)境，組合導(dǎo)航的藍(lán)色輸出軌跡都能準(zhǔn)確跟蹤真實(shí)的紅色行走軌跡，驗(yàn)證了論文研制的組合導(dǎo)航裝置的有效性。

4 結(jié) 語

本文設(shè)計(jì)的磁感式地磁輔助組合導(dǎo)航方法包括數(shù)據(jù)濾波、數(shù)據(jù)標(biāo)定、四元數(shù)解算以及數(shù)據(jù)融合算法等，與其他類型地磁輔助導(dǎo)航方式相比，本方法無需地磁圖先驗(yàn)信息，且原理簡單、計(jì)算量小，有效解決了系統(tǒng)的零點(diǎn)漂移、環(huán)境干擾和誤差累積等問題。實(shí)驗(yàn)結(jié)果表明，該方法有效融合了多傳感器信息，無需地磁圖先驗(yàn)信息，在GPS短期失效或斷續(xù)連接時的導(dǎo)航精確度較高，有效抑制了常規(guī)慣性導(dǎo)航誤差。