錢偉行，彭 晨，田恩剛，李榮冰

（1.南京師范大學(xué) 電氣與自動(dòng)化工程學(xué)院，南京 210042；2.南京航空航天大學(xué) 自動(dòng)化學(xué)院，南京 210016）

基于導(dǎo)航信息雙向融合的行人/移動(dòng)機(jī)器人協(xié)同導(dǎo)航方法

錢偉行1，彭 晨1，田恩剛1，李榮冰2

（1.南京師范大學(xué) 電氣與自動(dòng)化工程學(xué)院，南京 210042；2.南京航空航天大學(xué) 自動(dòng)化學(xué)院，南京 210016）

為提高衛(wèi)星導(dǎo)航系統(tǒng)失效且環(huán)境存在電磁干擾的情況下行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)的定位性能，研究了一種基于人機(jī)一體化智能系統(tǒng)的信息雙向融合協(xié)同導(dǎo)航方法。該方法利用行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)不同的誤差特性，構(gòu)建信息雙向融合濾波器同步修正兩者的導(dǎo)航信息誤差，即利用移動(dòng)機(jī)器人捷聯(lián)慣導(dǎo)系統(tǒng)較高的傳感器精度完成對(duì)行人導(dǎo)航系統(tǒng)磁航向誤差的實(shí)時(shí)修正，并利用行人導(dǎo)航系統(tǒng)較高的位移精度修正機(jī)器人慣導(dǎo)系統(tǒng)誤差，實(shí)現(xiàn)同步提高兩套導(dǎo)航系統(tǒng)的定位與航向精度。導(dǎo)航定位實(shí)驗(yàn)表明，該方法可有效提高人機(jī)一體化智能系統(tǒng)的導(dǎo)航定位精度，行人導(dǎo)航系統(tǒng)定位誤差約為行進(jìn)距離的3.3%，移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)的誤差積累速度降低為單獨(dú)工作時(shí)的1/3。本文所提出的方法利用了人機(jī)一體化智能系統(tǒng)的結(jié)構(gòu)特征，在衛(wèi)星導(dǎo)航系統(tǒng)失效的電磁干擾環(huán)境中有效提高了一體化系統(tǒng)的綜合導(dǎo)航定位性能，具有較高的理論研究與工程應(yīng)用價(jià)值。

人機(jī)一體化智能系統(tǒng)；協(xié)同導(dǎo)航；信息雙向融合；磁航向誤差；零速修正

人機(jī)一體化智能系統(tǒng)（簡(jiǎn)稱人機(jī)智能系統(tǒng)）是一類人與智能機(jī)器共同感知、共同認(rèn)知、共同決策，實(shí)現(xiàn)人與智能機(jī)器在決策層面上進(jìn)行融合，產(chǎn)生更高行動(dòng)效率的智能系統(tǒng)，該系統(tǒng)在軍事行動(dòng)、工程作業(yè)、殘疾人行動(dòng)輔助等領(lǐng)域具有很高的應(yīng)用價(jià)值。人機(jī)智能系統(tǒng)中各類信息的交互與融合是建立人機(jī)耦合協(xié)同作業(yè)機(jī)制的重要前提[1]。

利用人體與移動(dòng)機(jī)器人可構(gòu)建人機(jī)一體化智能導(dǎo)航系統(tǒng)，該系統(tǒng)由安裝于人體的行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人上安裝的組合導(dǎo)航系統(tǒng)組成，其中行人導(dǎo)航系統(tǒng)采用足部微慣性/衛(wèi)星組合導(dǎo)航系統(tǒng)，移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)采用光纖捷聯(lián)慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)。在衛(wèi)星導(dǎo)航系統(tǒng)可用的環(huán)境中，兩者的導(dǎo)航系統(tǒng)可獨(dú)立工作，但在進(jìn)入存在電磁干擾的未知環(huán)境時(shí)，衛(wèi)星導(dǎo)航系統(tǒng)將會(huì)受到嚴(yán)重影響甚至失效，單獨(dú)采用慣性系統(tǒng)將無(wú)法滿足人機(jī)智能系統(tǒng)的導(dǎo)航定位性能需求。

針對(duì)該問(wèn)題本文研究了一種基于行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人慣性導(dǎo)航系統(tǒng)的信息雙向融合協(xié)同導(dǎo)航方法。該方法利用兩種導(dǎo)航系統(tǒng)不同的誤差特性，構(gòu)建信息雙向融合濾波器，同步修正兩者的導(dǎo)航信息誤差，即利用移動(dòng)機(jī)器人光纖捷聯(lián)慣導(dǎo)系統(tǒng)較高的傳感器精度完成對(duì)行人導(dǎo)航系統(tǒng)磁航向誤差的實(shí)時(shí)修正，同時(shí)利用行人導(dǎo)航系統(tǒng)較高的單步位移精度修正機(jī)器人捷聯(lián)慣導(dǎo)系統(tǒng)的誤差，從而實(shí)現(xiàn)同時(shí)提高兩類載體導(dǎo)航系統(tǒng)的定位與航向精度。

1 基于中高精度慣性系統(tǒng)的行人導(dǎo)航系統(tǒng)磁航向誤差實(shí)時(shí)修正方法

1.1 磁航向誤差修正的基本原理

地球磁場(chǎng)在空間中構(gòu)成一個(gè)三維磁矢量,基于對(duì)該磁矢量進(jìn)行檢測(cè)與分解，即可求得運(yùn)動(dòng)載體當(dāng)前的磁航向信息。然而，實(shí)際應(yīng)用中磁傳感器所測(cè)量的信號(hào)，是地磁場(chǎng)與環(huán)境干擾磁場(chǎng)源所共同形成的磁場(chǎng)。由于環(huán)境中干擾磁場(chǎng)源的位置、數(shù)量、類型等往往無(wú)法確定，且隨著載體的移動(dòng)呈現(xiàn)不可預(yù)測(cè)的特點(diǎn)，因此對(duì)各類干擾源產(chǎn)生的磁場(chǎng)進(jìn)行建模與分析是十分復(fù)雜的，但若可對(duì)環(huán)境中某一位置地磁場(chǎng)與其他磁場(chǎng)源所產(chǎn)生的磁場(chǎng)進(jìn)行辨識(shí)與分離，則可有效修正由環(huán)境干擾源所導(dǎo)致的磁航向誤差[2]。

磁航向的常規(guī)解算方法是將與載體固聯(lián)的磁傳感器檢測(cè)的地磁場(chǎng)矢量投影在水平面上，通過(guò)水平分量之間的三角函數(shù)關(guān)系求解載體磁航向。因此，通過(guò)分析磁場(chǎng)矢量在水平面上的投影可進(jìn)行磁場(chǎng)的辨識(shí)方法的研究。未知環(huán)境中的磁場(chǎng)分布示意圖如圖1所示。圖中mage表示地磁場(chǎng)矢量在水平面上的投影，magd表示環(huán)境干擾磁場(chǎng)源所產(chǎn)生的磁場(chǎng)矢量和在水平面上的投影，magh表示環(huán)境中某位置總磁場(chǎng)矢量的水平分量，即mage與magd的矢量和。

忽略地磁場(chǎng)異常等現(xiàn)象且暫時(shí)忽略磁偏角，在某地域內(nèi)的地磁場(chǎng)矢量可視為已知常量，表示為mage，并將磁偏角表示為eθ；在存在磁場(chǎng)干擾的環(huán)境中，將磁傳感器所測(cè)量的環(huán)境中總磁場(chǎng)矢量表示為magh，類似地將帶有誤差的磁偏角表示為hθ。

圖1 未知環(huán)境中磁傳感器所敏感的磁矢量示意圖Fig.1 Schematic diagram of magnetic vector magnetic sensor detected in unknown environment

圖2 地磁場(chǎng)與環(huán)境磁源產(chǎn)生磁場(chǎng)的水平分量示意圖Fig.2 Schematic diagram of the horizontal component of the magnetic field produced by geomagnetic field and environmental magnetic source

設(shè)三軸磁傳感器測(cè)量的地磁場(chǎng)mage在載體坐標(biāo)系下的投影為，而測(cè)量總磁場(chǎng)矢量magh在載體坐標(biāo)系下的投影為，其中

若已知與磁傳感器固聯(lián)載體的橫滾角與俯仰角分別為γ與θ，則可將投影到水平面與垂直的方向上。

通過(guò)相同方法也可以求得dθ：

因此即可求得該地理位置上的磁航向角修正值Δθ=θd-θe。由物理含義可知，Δθ與載體的姿態(tài)無(wú)關(guān)，只與載體位置與磁場(chǎng)干擾源的相對(duì)位置有關(guān)。因此，可通過(guò)該特性實(shí)現(xiàn)高精度慣性系統(tǒng)對(duì)低精度慣性系統(tǒng)的航向修正。

1.2 基于中高精度慣性系統(tǒng)的行人導(dǎo)航系統(tǒng)磁航向誤差修正方法

基于上節(jié)所分析的磁航向角修正值，利用人機(jī)一體化智能系統(tǒng)中移動(dòng)機(jī)器人慣性導(dǎo)航系統(tǒng)中較高精度的傳感器實(shí)時(shí)修正行人導(dǎo)航系統(tǒng)的磁航向誤差。具體實(shí)現(xiàn)方法如下：

1）在磁場(chǎng)環(huán)境純凈的區(qū)域，將與捷聯(lián)慣性傳感器同軸安裝且經(jīng)過(guò)標(biāo)定的中高精度三軸磁傳感器搭載于移動(dòng)機(jī)器人上進(jìn)行硬、軟磁誤差補(bǔ)償，補(bǔ)償方法采用基于橢圓約束的多位置最小二乘擬合方法[3]，采用捷聯(lián)慣導(dǎo)系統(tǒng)來(lái)測(cè)量機(jī)器人水平姿態(tài)，從而結(jié)合磁傳感器的輸出解算磁航向，并利用當(dāng)?shù)氐乩砦恢眯畔⑿拚牌牵?/p>

2）行人與移動(dòng)機(jī)器人同步進(jìn)入電磁干擾環(huán)境中，利用機(jī)器人中高精度捷聯(lián)慣導(dǎo)系統(tǒng)輸出的航向角為航向基準(zhǔn)eθ，并以其磁傳感器輸出與水平姿態(tài)數(shù)據(jù)解算hθ，從而求解Δθ，并將該值實(shí)時(shí)通過(guò)數(shù)據(jù)通訊傳遞給行人導(dǎo)航系統(tǒng)，實(shí)現(xiàn)行人導(dǎo)航系統(tǒng)的航向修正。

2 行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人的信息雙向融合協(xié)同導(dǎo)航方法

2.1 行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人的協(xié)同導(dǎo)航機(jī)理

行人導(dǎo)航系統(tǒng)采用基于微慣性/地磁測(cè)量組件（IMMU）足部安裝的個(gè)人導(dǎo)航定位方案，經(jīng)干凈磁環(huán)境中磁傳感器誤差標(biāo)定與補(bǔ)償后進(jìn)行導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)，系統(tǒng)進(jìn)入導(dǎo)航工作狀態(tài)，通過(guò)人體步態(tài)相位檢測(cè)對(duì)導(dǎo)航系統(tǒng)進(jìn)行間斷性的零速修正（Zero Velocity Update，簡(jiǎn)稱ZUPT），估計(jì)導(dǎo)航系統(tǒng)級(jí)誤差以及陀螺儀與加速度計(jì)的部分漂移誤差[4]。在衛(wèi)星導(dǎo)航系統(tǒng)信號(hào)衰減或失效的情況下在一定時(shí)間內(nèi)保持較高的導(dǎo)航精度，但該方法無(wú)法有效地估計(jì)與修正航向誤差，因此磁航向的精度對(duì)于行人導(dǎo)航系統(tǒng)的性能具有至關(guān)重要的作用[5]。因此，可基于第1節(jié)中所述的磁航向誤差實(shí)時(shí)修正方法，采用提高行人導(dǎo)航系統(tǒng)的航向精度。

圖3 信息雙向融合協(xié)同導(dǎo)航原理圖Fig.3 Principle of information bidirectional fusion cooperative navigation

與此同時(shí)，移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)工作于捷聯(lián)慣性模式，其系統(tǒng)由三軸正交的陀螺儀和加速度計(jì)組成慣性測(cè)量組件與三軸正交的磁傳感器構(gòu)成。由于需要保持移動(dòng)機(jī)器人捷聯(lián)慣導(dǎo)系統(tǒng)的高精度，采用行人導(dǎo)航系統(tǒng)零速修正后的位置信息來(lái)實(shí)時(shí)修正移動(dòng)機(jī)器人捷聯(lián)慣導(dǎo)的系統(tǒng)級(jí)誤差與慣性傳感器誤差，從而構(gòu)成行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人的協(xié)同導(dǎo)航。

行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人的信息雙向融合協(xié)同導(dǎo)航基于工作原理如圖3所示。圖3中，行人與移動(dòng)機(jī)器人同步行進(jìn)，行人導(dǎo)航系統(tǒng)與機(jī)器人導(dǎo)航系統(tǒng)之間通過(guò)藍(lán)牙、UWB等無(wú)線通訊方式進(jìn)行數(shù)據(jù)交換。研究中將無(wú)線通訊視為理想狀態(tài)，即暫不將數(shù)據(jù)傳輸導(dǎo)致的導(dǎo)航定位誤差作為本文的研究?jī)?nèi)容。

2.2 基于最優(yōu)估計(jì)理論的信息雙向融合方法

1）行人導(dǎo)航系統(tǒng)與傳感器誤差建模與修正

行人導(dǎo)航系統(tǒng)與傳感器誤差可在初始對(duì)準(zhǔn)與零速修正中得到修正。由于初始對(duì)準(zhǔn)時(shí)間較短，而零速修正采用閉環(huán)校正的方式，系統(tǒng)與慣性傳感器誤差積累均為小量，因此可采用捷聯(lián)慣性導(dǎo)航系統(tǒng)的線性誤差模型，同時(shí)采用線性卡爾曼濾波完成對(duì)準(zhǔn)與零速修正。

行人導(dǎo)航系統(tǒng)誤差模型由平臺(tái)誤差角模型、速度誤差模型、定位誤差以及慣性傳感器誤差模型組成。其中平臺(tái)誤差角方程為：

速度誤差模型如下：

定位誤差模型為：

經(jīng)粗對(duì)準(zhǔn)后MEMS慣性傳感器的誤差可分別近似為隨機(jī)常值與白噪聲的結(jié)合，導(dǎo)航過(guò)程中的慣性器件隨機(jī)常值誤差模型[6]。

在足部運(yùn)動(dòng)過(guò)程中，采用移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)提供的航向誤差為觀測(cè)量構(gòu)建觀測(cè)方程，即采用航向匹配的方法完成行人導(dǎo)航系統(tǒng)的誤差估計(jì)與補(bǔ)償。由于捷聯(lián)慣性系統(tǒng)姿態(tài)、航向誤差與平臺(tái)誤差角之間存在如下轉(zhuǎn)換關(guān)系：

航向誤差作為觀測(cè)量本質(zhì)上是對(duì)平臺(tái)誤差角的直接觀測(cè)，人體足部周期性運(yùn)動(dòng)情況下航向誤差對(duì)于速度誤差與位置誤差也存在一定的間接觀測(cè)性。

在足部著地的過(guò)程中，采用航向與速度同時(shí)匹配的觀測(cè)方程。公式(5)～(8)中各物理量詳見(jiàn)有關(guān)參考文獻(xiàn)，本文不再詳述。

2）移動(dòng)機(jī)器人慣性導(dǎo)航系統(tǒng)的誤差建模與修正

為移動(dòng)機(jī)器人定位精度需求，通常安裝中高精度的光纖IMU構(gòu)成捷聯(lián)系統(tǒng)，該系統(tǒng)誤差模型同樣采用公式(5)～(7)，并采用隨機(jī)常值、一階馬爾科夫過(guò)程以及白噪聲相結(jié)合的慣性器件誤差模型。

行人導(dǎo)航系統(tǒng)的零速修正使其誤差積累速度由時(shí)間的三次函數(shù)轉(zhuǎn)變隨位移的線性函數(shù)，且人體行走過(guò)程中雖然步長(zhǎng)不斷變化，但總試圖維持比較平穩(wěn)的步速，因此在行進(jìn)速度不突變情況下基于零速修正的行人導(dǎo)航系統(tǒng)誤差特性也可近似為隨時(shí)間的線性函數(shù)[7-8]。將行人導(dǎo)航系統(tǒng)的位置信息作為觀測(cè)量，通過(guò)觀測(cè)量擴(kuò)充法實(shí)現(xiàn)基于抗差濾波的系統(tǒng)誤差估計(jì)與修正[9]，此時(shí)系統(tǒng)方程中將增加如下3階方程組：

即狀態(tài)方程擴(kuò)展為18階。觀測(cè)方程的形式如下：

其中觀測(cè)系數(shù)矩陣H擴(kuò)展為3× 21階矩陣，其中。

3 環(huán)境中磁場(chǎng)干擾的導(dǎo)航定位實(shí)驗(yàn)

采用XSENS MTi-10慣性/地磁測(cè)量組件與某型光纖捷聯(lián)慣性系統(tǒng)進(jìn)行靜態(tài)測(cè)試，該組件的基本性能如表1所示。

表1 Mti-10組件與光纖慣性系統(tǒng)的傳感器基本性能參數(shù)Tab.1 Essential performance parameters of inertial sensors in MTi-10 unit and optical inertial system

基于導(dǎo)航信息雙向融合的行人/移動(dòng)機(jī)器人導(dǎo)航定位實(shí)驗(yàn)過(guò)程如下：

行進(jìn)路線為圍繞一座樓宇的矩形路線，在該路線上行走一圈，總行進(jìn)距離約240 m，歷時(shí)約540 s，行進(jìn)結(jié)束時(shí)回到出發(fā)點(diǎn)。在不存在電磁干擾的環(huán)境中完成行人導(dǎo)航系統(tǒng)與移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)與磁傳感器標(biāo)定。清除行進(jìn)環(huán)境中的電磁干擾物體，行人與移動(dòng)機(jī)器人同步完成行進(jìn)一次；在行進(jìn)路線中放置若干電磁干擾源，行人與移動(dòng)機(jī)器人再次按該路線同步完成行進(jìn)一次。

采用如下導(dǎo)航解算方案驗(yàn)證雙向信息融合方法的有效性：

1） 無(wú)磁場(chǎng)干擾環(huán)境中行人導(dǎo)航系統(tǒng)與機(jī)器人導(dǎo)航系統(tǒng)分別單獨(dú)工作；

2） 電磁干擾環(huán)境中行人導(dǎo)航系統(tǒng)與機(jī)器人導(dǎo)航系統(tǒng)分別單獨(dú)工作；

3） 電磁干擾環(huán)境中行人導(dǎo)航系統(tǒng)與機(jī)器人導(dǎo)航系統(tǒng)實(shí)施信息雙向融合工作機(jī)制。實(shí)驗(yàn)結(jié)果如圖5與表2所示。

圖5 矩形路線實(shí)驗(yàn)中的導(dǎo)航定位結(jié)果Fig.5 The positioning result in rectangle route experiment

表2 移動(dòng)機(jī)器人導(dǎo)航定位誤差Tab.1 Navigation and positioning error of mobile robot

由圖5可知，以實(shí)驗(yàn)地點(diǎn)地理測(cè)繪信息為定位基準(zhǔn) ，無(wú)電磁干擾環(huán)境中的行人導(dǎo)航方案在實(shí)驗(yàn)結(jié)束時(shí)刻定位誤差約為6 m，約為行進(jìn)距離的2.5%，如圖中實(shí)線所示；電磁干擾環(huán)境中行人導(dǎo)航方案定位誤差隨行進(jìn)距離逐漸增大，行進(jìn)結(jié)束時(shí)約為18 m，約為行進(jìn)距離的7.5%，且在磁場(chǎng)源附近方位誤差可達(dá)35°左右，如圖中點(diǎn)劃線所示；電磁干擾環(huán)境中采用協(xié)同導(dǎo)航方案，行人導(dǎo)航系統(tǒng)解算的運(yùn)動(dòng)軌跡與無(wú)電磁干擾條件下較為接近，最大航向誤差約為5°，行進(jìn)結(jié)束時(shí)定位誤差約為8 m，約為行進(jìn)距離的3.3%，如圖中虛線所示。

由表2可知，移動(dòng)機(jī)器人慣性導(dǎo)航系統(tǒng)在電磁干擾環(huán)境中受到一定程度的影響，但由于采用中高精度的慣性傳感器慣導(dǎo)系統(tǒng)短時(shí)間內(nèi)仍可正常工作；采用本文提出的信息雙向融合方案時(shí)，系統(tǒng)定位精度得到了大幅的提高，誤差積累速度為單獨(dú)工作時(shí)的1/3左右。

4 結(jié) 論

本文以在GNSS失效的電磁干擾環(huán)境中導(dǎo)航定位為研究背景，研究了一種基于導(dǎo)航信息雙向融合的人機(jī)一體化智能系統(tǒng)協(xié)同導(dǎo)航方案。該方案采用移動(dòng)機(jī)器人中高精度的慣性系統(tǒng)，實(shí)時(shí)修正行人導(dǎo)航系統(tǒng)的磁航向誤差，大幅抑制磁場(chǎng)干擾對(duì)行人導(dǎo)航系統(tǒng)定位精度的影響；移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)由于可實(shí)時(shí)通過(guò)雙向信息融合實(shí)現(xiàn)位置匹配來(lái)進(jìn)行系統(tǒng)誤差的修正，在衛(wèi)星導(dǎo)航系統(tǒng)失效的環(huán)境中，其定位誤差相對(duì)于捷聯(lián)慣性工作模式有明顯的提高。

綜上，在電磁干擾較嚴(yán)重的環(huán)境中，采用基于雙向信息融合方法的協(xié)同導(dǎo)航是一種適用于人機(jī)一體化智能系統(tǒng)的有效的導(dǎo)航定位方法，具有重要的理論研究與工程應(yīng)用價(jià)值。

（References）：

[1]盧永錦，張學(xué)寧，王亮.一種基于操控信息的人機(jī)耦合方法[J].系統(tǒng)仿真學(xué)報(bào)，2008，20(16)：4360-4367.LU Yong-jin,ZHANG Xue-ning,WANG Liang.Method of human-machine coupling on operation signals[J].Journal of System Simulation,2008,20(16):4360-4367.

[2]Calusdian J.A personal navigation system based on inertial and magnetic field measurements[D].Monterey California,USA:Naval Postgraduate School,2010.

[3]Str?mb?ck P,Rantakokko J,Wirkander S L.Foot-mounted inertial navigation and cooperative sensor fusion for indoor positioning[C]//Proceedings of the 2010 International Technical Meeting of The Institute of Navigation.2010:89-98.

[4]Skog I,Handel P,Nilsson J O,et al.Zero-velocity detection — an algorithm evaluation[J].IEEE Trans.on Biomedical Engineering,2010,57(11):2657- 2666.

[5]張金亮，秦永元，梅春波.基于MEMS慣性技術(shù)的鞋式個(gè)人導(dǎo)航系統(tǒng)[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2011，19(3)：253-256.ZHANG Jin-liang,QIN Yong-yuan,MEI Chun-bo.shoe-mounted personal navigation system based on MEMS inertial technology[J].Journal of Chinese Inertial Technology,2011,19(3):253-256.

[6]Mohd-Yasin F,Nagel D J,Korman C E.Noise in MEMS [J].Measurement Science and Technology,2010,21(1):1-22.

[7]錢偉行，朱欣華，蘇巖.基于足部微慣性/地磁測(cè)量組件的個(gè)人導(dǎo)航方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2012，20(5)：567-572.QIAN Wei-xing,ZHU Xin-hua,SU Yan.Personal navigation method based on foot-mounted MEMS inertial/ magnetic measurement unit[J].Journal of Chinese Inertial Technology,2012,20(5):567-572.

[8]Wu Xi,Xu Qian,Jing Zhanrong,Fu Renqi.Research on attitude measurement system of the individual soldier virtual training system[C]// ICIEA 2009:326-331.

[9]許睿，孫永榮，陳武，劉建業(yè).一種基于抗差濾波的行人導(dǎo)航算法研究[J].系統(tǒng)工程與電子技術(shù)，2010，30(7)：1506-1508.XU Rui1,SUN Yong-rong,CHEN Wu,et al.Method of pedestrian navigation based on robust filter[J].Systems Engineering and Electronics,2010,30(7):1506-1508.

Pedestrian/mobile robot cooperative navigation method based on navigation information bidirectional fusion

QIAN Wei-xing1,PENG Chen1,TIAN En-gang1,LI Rong-bing2
(1.School of Electrical and Automation Engineering,Nanjing Normal University，Nanjing 210042,China;2.College of Automation,Nanjing University of Aeronautics and Astronautics,Nanjing 210016,China)

In order to improve the positioning precision of pedestrian navigation system and mobile robot navigation system in eletromagnetic interference environment,an information bidirectional fusion cooperative navigation method based on human-machine intelligent system is studied.The method utilizes the different error characteristics of pedestrian navigation system and mobile robot strapdown inertial navigation system(SINS) to build information bidirectional fusion filters for correcting the errors of two navigation systems synchronously.The method makes use of the high-accuracy sensors of mobile robot SINS to complete real-time modify of magnetic heading error in pedestrian navigation system,and use the high displacement accuracy of pedestrian navigation system to correct inertial navigation system errors in robot to improve the positioning and heading accuracy of two navigation systems synchronously.Experiments results show that the positioning error of pedestrian navigation system is about 3.3% of the walking distance,and the system error accumulation rate of mobile robot SINS is reduced to 1/3 of the rate when working alone.The studied method can effectively improve the navigation and positioning accuracy of the human-machine intelligent system in eletromagnetic interference environment,and has high theoretical research and engineering application value.

human-machine intelligent system;cooperative navigation;information bidirectional fusion;magnetic azimuth error;zero-velocity update

U666.1

：A

1005-6734(2014)01-0074-05

10.13695/j.cnki.12-1222/o3.2014.01.015

2013-08-11；

：2013-11-28

國(guó)家自然科學(xué)基金（61304227，61273057，60904091）；江蘇省高校自然科學(xué)基金（13KJB590001，12KJB470011）

錢偉行（1981—），男，講師，博士，從事慣性與組合導(dǎo)航技術(shù)研究。E-mail：61192@njnu.edu.cn