基于導航信息雙向融合的行人/移動機器人協同導航方法

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

（1.南京師范大學 電氣與自動化工程學院，南京 210042；2.南京航空航天大學 自動化學院，南京 210016）

基于導航信息雙向融合的行人/移動機器人協同導航方法

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

（1.南京師范大學 電氣與自動化工程學院，南京 210042；2.南京航空航天大學 自動化學院，南京 210016）

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

人機一體化智能系統；協同導航；信息雙向融合；磁航向誤差；零速修正

人機一體化智能系統（簡稱人機智能系統）是一類人與智能機器共同感知、共同認知、共同決策，實現人與智能機器在決策層面上進行融合，產生更高行動效率的智能系統，該系統在軍事行動、工程作業、殘疾人行動輔助等領域具有很高的應用價值。人機智能系統中各類信息的交互與融合是建立人機耦合協同作業機制的重要前提[1]。

利用人體與移動機器人可構建人機一體化智能導航系統，該系統由安裝于人體的行人導航系統與移動機器人上安裝的組合導航系統組成，其中行人導航系統采用足部微慣性/衛星組合導航系統，移動機器人導航系統采用光纖捷聯慣性/衛星緊組合導航系統。在衛星導航系統可用的環境中，兩者的導航系統可獨立工作，但在進入存在電磁干擾的未知環境時，衛星導航系統將會受到嚴重影響甚至失效，單獨采用慣性系統將無法滿足人機智能系統的導航定位性能需求。

針對該問題本文研究了一種基于行人導航系統與移動機器人慣性導航系統的信息雙向融合協同導航方法。該方法利用兩種導航系統不同的誤差特性，構建信息雙向融合濾波器，同步修正兩者的導航信息誤差，即利用移動機器人光纖捷聯慣導系統較高的傳感器精度完成對行人導航系統磁航向誤差的實時修正，同時利用行人導航系統較高的單步位移精度修正機器人捷聯慣導系統的誤差，從而實現同時提高兩類載體導航系統的定位與航向精度。

1 基于中高精度慣性系統的行人導航系統磁航向誤差實時修正方法

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

地球磁場在空間中構成一個三維磁矢量,基于對該磁矢量進行檢測與分解，即可求得運動載體當前的磁航向信息。然而，實際應用中磁傳感器所測量的信號，是地磁場與環境干擾磁場源所共同形成的磁場。由于環境中干擾磁場源的位置、數量、類型等往往無法確定，且隨著載體的移動呈現不可預測的特點，因此對各類干擾源產生的磁場進行建模與分析是十分復雜的，但若可對環境中某一位置地磁場與其他磁場源所產生的磁場進行辨識與分離，則可有效修正由環境干擾源所導致的磁航向誤差[2]。

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

忽略地磁場異常等現象且暫時忽略磁偏角，在某地域內的地磁場矢量可視為已知常量，表示為mage，并將磁偏角表示為eθ；在存在磁場干擾的環境中，將磁傳感器所測量的環境中總磁場矢量表示為magh，類似地將帶有誤差的磁偏角表示為hθ。

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

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

設三軸磁傳感器測量的地磁場mage在載體坐標系下的投影為，而測量總磁場矢量magh在載體坐標系下的投影為，其中

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

通過相同方法也可以求得dθ：

因此即可求得該地理位置上的磁航向角修正值Δθ=θd-θe。由物理含義可知，Δθ與載體的姿態無關，只與載體位置與磁場干擾源的相對位置有關。因此，可通過該特性實現高精度慣性系統對低精度慣性系統的航向修正。

1.2 基于中高精度慣性系統的行人導航系統磁航向誤差修正方法

基于上節所分析的磁航向角修正值，利用人機一體化智能系統中移動機器人慣性導航系統中較高精度的傳感器實時修正行人導航系統的磁航向誤差。具體實現方法如下：

1）在磁場環境純凈的區域，將與捷聯慣性傳感器同軸安裝且經過標定的中高精度三軸磁傳感器搭載于移動機器人上進行硬、軟磁誤差補償，補償方法采用基于橢圓約束的多位置最小二乘擬合方法[3]，采用捷聯慣導系統來測量機器人水平姿態，從而結合磁傳感器的輸出解算磁航向，并利用當地地理位置信息修正磁偏角；

2）行人與移動機器人同步進入電磁干擾環境中，利用機器人中高精度捷聯慣導系統輸出的航向角為航向基準eθ，并以其磁傳感器輸出與水平姿態數據解算hθ，從而求解Δθ，并將該值實時通過數據通訊傳遞給行人導航系統，實現行人導航系統的航向修正。

2 行人導航系統與移動機器人的信息雙向融合協同導航方法

2.1 行人導航系統與移動機器人的協同導航機理

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

圖3 信息雙向融合協同導航原理圖Fig.3 Principle of information bidirectional fusion cooperative navigation

與此同時，移動機器人導航系統工作于捷聯慣性模式，其系統由三軸正交的陀螺儀和加速度計組成慣性測量組件與三軸正交的磁傳感器構成。由于需要保持移動機器人捷聯慣導系統的高精度，采用行人導航系統零速修正后的位置信息來實時修正移動機器人捷聯慣導的系統級誤差與慣性傳感器誤差，從而構成行人導航系統與移動機器人的協同導航。

行人導航系統與移動機器人的信息雙向融合協同導航基于工作原理如圖3所示。圖3中，行人與移動機器人同步行進，行人導航系統與機器人導航系統之間通過藍牙、UWB等無線通訊方式進行數據交換。研究中將無線通訊視為理想狀態，即暫不將數據傳輸導致的導航定位誤差作為本文的研究內容。

2.2 基于最優估計理論的信息雙向融合方法

1）行人導航系統與傳感器誤差建模與修正

行人導航系統與傳感器誤差可在初始對準與零速修正中得到修正。由于初始對準時間較短，而零速修正采用閉環校正的方式，系統與慣性傳感器誤差積累均為小量，因此可采用捷聯慣性導航系統的線性誤差模型，同時采用線性卡爾曼濾波完成對準與零速修正。

行人導航系統誤差模型由平臺誤差角模型、速度誤差模型、定位誤差以及慣性傳感器誤差模型組成。其中平臺誤差角方程為：

速度誤差模型如下：

定位誤差模型為：

經粗對準后MEMS慣性傳感器的誤差可分別近似為隨機常值與白噪聲的結合，導航過程中的慣性器件隨機常值誤差模型[6]。

在足部運動過程中，采用移動機器人導航系統提供的航向誤差為觀測量構建觀測方程，即采用航向匹配的方法完成行人導航系統的誤差估計與補償。由于捷聯慣性系統姿態、航向誤差與平臺誤差角之間存在如下轉換關系：

航向誤差作為觀測量本質上是對平臺誤差角的直接觀測，人體足部周期性運動情況下航向誤差對于速度誤差與位置誤差也存在一定的間接觀測性。

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

2）移動機器人慣性導航系統的誤差建模與修正

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

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

即狀態方程擴展為18階。觀測方程的形式如下：

其中觀測系數矩陣H擴展為3× 21階矩陣，其中。

3 環境中磁場干擾的導航定位實驗

采用XSENS MTi-10慣性/地磁測量組件與某型光纖捷聯慣性系統進行靜態測試，該組件的基本性能如表1所示。

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

基于導航信息雙向融合的行人/移動機器人導航定位實驗過程如下：

行進路線為圍繞一座樓宇的矩形路線，在該路線上行走一圈，總行進距離約240 m，歷時約540 s，行進結束時回到出發點。在不存在電磁干擾的環境中完成行人導航系統與移動機器人導航系統的初始對準與磁傳感器標定。清除行進環境中的電磁干擾物體，行人與移動機器人同步完成行進一次；在行進路線中放置若干電磁干擾源，行人與移動機器人再次按該路線同步完成行進一次。

采用如下導航解算方案驗證雙向信息融合方法的有效性：

1） 無磁場干擾環境中行人導航系統與機器人導航系統分別單獨工作；

2） 電磁干擾環境中行人導航系統與機器人導航系統分別單獨工作；

3） 電磁干擾環境中行人導航系統與機器人導航系統實施信息雙向融合工作機制。實驗結果如圖5與表2所示。

圖5 矩形路線實驗中的導航定位結果Fig.5 The positioning result in rectangle route experiment

表2 移動機器人導航定位誤差Tab.1 Navigation and positioning error of mobile robot

由圖5可知，以實驗地點地理測繪信息為定位基準 ，無電磁干擾環境中的行人導航方案在實驗結束時刻定位誤差約為6 m，約為行進距離的2.5%，如圖中實線所示；電磁干擾環境中行人導航方案定位誤差隨行進距離逐漸增大，行進結束時約為18 m，約為行進距離的7.5%，且在磁場源附近方位誤差可達35°左右，如圖中點劃線所示；電磁干擾環境中采用協同導航方案，行人導航系統解算的運動軌跡與無電磁干擾條件下較為接近，最大航向誤差約為5°，行進結束時定位誤差約為8 m，約為行進距離的3.3%，如圖中虛線所示。

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

4 結 論

本文以在GNSS失效的電磁干擾環境中導航定位為研究背景，研究了一種基于導航信息雙向融合的人機一體化智能系統協同導航方案。該方案采用移動機器人中高精度的慣性系統，實時修正行人導航系統的磁航向誤差，大幅抑制磁場干擾對行人導航系統定位精度的影響；移動機器人導航系統由于可實時通過雙向信息融合實現位置匹配來進行系統誤差的修正，在衛星導航系統失效的環境中，其定位誤差相對于捷聯慣性工作模式有明顯的提高。

綜上，在電磁干擾較嚴重的環境中，采用基于雙向信息融合方法的協同導航是一種適用于人機一體化智能系統的有效的導航定位方法，具有重要的理論研究與工程應用價值。

（References）：

[1]盧永錦，張學寧，王亮.一種基于操控信息的人機耦合方法[J].系統仿真學報，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慣性技術的鞋式個人導航系統[J].中國慣性技術學報，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]錢偉行，朱欣華，蘇巖.基于足部微慣性/地磁測量組件的個人導航方法[J].中國慣性技術學報，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]許睿，孫永榮，陳武，劉建業.一種基于抗差濾波的行人導航算法研究[J].系統工程與電子技術，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

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

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