基于导航信息双向融合的行人/移动机器人协同导航方法

钱伟行，彭 晨，田恩刚，李荣冰

（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