信息双向融合的机器人协同导航方法*

任仁凯, 钱伟行， 彭　晨， 祝燕华， 谢　非

(1.南京师范大学 电气与自动化工程学院，江苏 南京 210042;2.东南大学 仪器科学与工程学院，江苏 南京 210096)



信息双向融合的机器人协同导航方法*

任仁凯1, 钱伟行1， 彭晨1， 祝燕华2， 谢非1

(1.南京师范大学 电气与自动化工程学院，江苏 南京 210042;2.东南大学 仪器科学与工程学院，江苏 南京 210096)

根据双足机器人足部惯性导航系统与轮式机器人航位递推/同步定位与建图(SLAM)导航系统不同的误差发散特性构建信息双向融合滤波器，即利用双足机器人导航系统中的误差零速修正(ZUPT)方法减少轮式机器人导航系统误差，并利用轮式机器人导航系统中的SLAM算法在一定环境条件下修正双足机器人的定位误差，从而同步提高两类机器人导航系统的定位与航向精度。实验结果表明:导航系统定位误差约为行进距离的2.3%，在室内等卫星导航系统失效的环境中可有效提高机器人群体导航系统的综合导航性能。

双足机器人； 轮式机器人； 信息双向融合； 同步定位与建图； 零速修正

0　引　言

移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统，适用于复杂的工作任务及多变的工作环境，在军事行动、工程作业等领域具有很高的应用价值[1,2]。其中，双足机器人和轮式机器人是两类较典型的移动机器人。

应用于导航领域时，移动机器人多采用全球导航卫星系统(global navigation satellite system，GNSS)。但在室内等GNSS失效环境中，惯性导航以其自主性强、更新率高、导航信息完整、系统便于安装等特点，成为移动机器人导航定位的主要手段。目前，双足机器人采用足部惯性导航方式[3,4]，轮式机器人采用车辆航位递推(vehicle dead recko-ning，VDR)与同步定位与建图(simultaneous localization and mapping,SLAM)相结合的导航方式[5,6]，均能取得较好的导航效果。然而，双足机器人导航系统误差随路程发散，长距离行进时精度较低；轮式机器人导航系统采用的VDR算法误差随时间发散较快，影响SLAM算法发挥作用。

本文研究了一种基于信息双向融合的机器人协同导航方法。该方法利用两种导航系统不同的误差发散特性，通过无线通信方式进行信息交互，构建信息双向融合滤波器，从而同步修正其导航信息误差，同时提高两者的定位与航向精度。

1　机器人协同导航机理分析

1.1基于零速修正的双足机器人导航机理

双足机器人导航系统中，将惯性/地磁测量单元(inertial magnetic measurement unit，IMMU)安装在机器人足部，与其内部微型导航计算机通过有线或无线通信方式完成数据传输，其工作流程如图1所示。导航开始前要进行磁传感器误差标定与补偿[3]，以及系统初始对准[4]。足部惯性导航方法通过对IMMU组件上的惯性测量单元(inertial measurement unit，IMU)信息进行捷联惯导解算获得机器人足部的姿态、速度和位置信息，利用磁传感器确定运动的航向信息，并采用基于步态相位检测的零速修正(ZUPT)方法，实时修正导航系统级误差和惯性传感器的随机误差。

图1　双足机器人导航系统工作流程Fig 1　Working process of navigation system of biped robot

首先定义加速度计三个正交轴的输出为fx,fy,fz，再定义如下变量

(1)

式中各变量定义详见文献[4]。通过上述的检测方式，可在复杂的足部运动中有效提高步态相位检测的准确性。由于双足机器人足部运动的特点，IMMU组件在某些时间间隔里是保持静止的，从而可采用零速修正的方式估计导航系统级误差以及陀螺仪与加速度计的部分惯性器件漂移误差。

1.2基于VDR与SLAM的轮式机器人导航机理

轮式机器人导航系工作流程如图2所示。

图2　轮式机器人导航系统工作流程Fig 2　Working process of navigation system of wheeled robot

根据VDR算法过程中系统测量输出和导航坐标系之间的关系，在不考虑导航系统水平姿态变化的情况下，只要获得了各参量初始值，即可实现后续的VDR推算过程。然而，单独的VDR定位误差随时间迅速累积，导致系统性能下降，可用SLAM算法进行定位误差补偿。

SLAM算法是机器人在自身位置不确定的未知环境中，通过对外部环境进行识别并加以分析，然后创建地图，并利用地图进行定位和导航[7]。首先，对视觉传感器采集的图像进行处理与分析，识别特征点并定位，将轮式机器人的视觉投影与空间物体的几何关系转换为机器人相对特征点的距离和角度。然后，利用加权最小二乘拟合方法和尺度不变特征变换算法分别从激光信息与视觉信息中提取直线和点特征，进行特征级的信息融合[5]。最后，运用SLAM算法，整体步骤按照预测、观测、数据关联、更新、地图构建的递推算法进行同步定位与建图，从而识别环境目标，并进行平滑运动[6]。

轮式机器人采用视觉和激光传感器信息融合的SLAM算法减少定位误差，当重复经过某处路标时，即可进行误差校正，提高导航系统的定位精度。

1.3机器人的协同导航机理

基于以上两节分析可知，双足机器人采用IMMU足部安装的导航定位方案，通过步态相位检测对导航系统进行间断性的误差零速修正，其误差随路程发散，在一定路程内可保持较高的定位精度；轮式机器人导航系统采用VDR算法导航，运用视觉和激光传感器信息融合的SLAM算法进行误差校正，其误差随时间发散，在一定时间内可保持较高的定位精度。

由于采用步态相位检测和零速修正后导航系统定位精度高于VDR算法，可将双足机器人导航系统零速修正后的定位信息实时修正轮式机器人的导航系统误差。此外，双足机器人行走时晃动较大，而负载小，轮式机器人行驶稳定，负载较大，满足SLAM算法对载体稳定性较高的要求。因此，将视觉、激光等传感器安装在轮式机器人上，经SLAM辅助后的定位信息也可对双足机器人导航系统进行误差校正。

双足机器人与轮式机器人的信息双向融合协同导航原理如图3所示。双足机器人与轮式机器人同步行进，两者导航系统通过无线通信方式进行数据交换。本研究将无线通讯视为理想状态，即暂不将数据传输导致的导航定位误差作为本文的研究内容。

图3　信息双向融合协同导航原理Fig 3　Principle of information bidirectional fusion cooperative navigation

2　基于最优估计理论的信息双向融合方法

2.1双足机器人导航系统误差建模与修正

双足机器人导航系统零速修正的误差模型由平台误差角模型、速度误差模型、定位误差模型以及惯性传感器误差模型组成[3]。

双足机器人行进过程中，采用SLAM提供的定位信息完成误差估计与补偿。SLAM算法中，系统的状态向量可表示为

(2)

式中xv(k)=[xvx(k),xvy(k),xvθ(k)]T表示机器人在k时刻的位姿，M=[m1,m2,…,ml]，mi=[xi,yi]T，i=1,2,…,l，表示路标集合，其中,l为路标的标记序号[8]。

当检测到重复经过某一路标时，其定位误差即校正为上一时刻经过该路标时的数据。检测到同一路标的次数越多，定位就越精确。通过SLAM算法对双足机器人的误差修正，提高定位精度，有效抑制其误差随路程的发散。

2.2轮式机器人导航系统误差建模与修正

轮式机器人导航系统误差模型共有9维，由VDR算法的基本导航参数误差模型和惯性传感器误差模型组成。

基本导航参数误差包括航向误差、东向速度误差、北向速度误差、经度误差和纬度误差，状态建模方程为

(3)

(4)

根据以上误差微分方程，可获得系统误差状态方程如下

(5)

在行进过程中，轮式机器人VDR算法输出的定位信息和双足机器人导航系统提供的定位信息分别可表示为

(6)

式中λWR,LWR分别为轮式机器人提供的经度和纬度，λBR,LBR分别为双足机器人提供的经度和纬度。

定义导航系统的观测矢量为

=H(t)X(t)+NBR(t)

(7)

则有观测方程

Z=HX+V

(8)

式中由于一定行进距离内双足机器人定位误差NeBR和NnBR远小于轮式机器人定位误差δλ和δL，可忽略，并在观测方程中加入小量白噪声作为量测噪声。

3　机器人协同导航系统定位性能实验与分析

3.1信息双向融合的导航定位实验

对双足机器人和轮式机器人上安装的IMMU组件分别进行静态测试，基本性能如表1所示。

表1　IMMU组件基本性能参数

实验前，在不存在电磁干扰的环境中完成导航系统的初始对准与磁传感器标定，标定后水平姿态精度在0.5°左右，磁航向精度在1°左右。实验中，双足机器人与轮式机器人同步行进，两者导航系统通过蓝牙无线通信方式进行数据交换。图4所示为机器人进行实验时的行进路线地图，总行进路程约280m，历时约370s，沿途设有路标1和路标2，其中，起始位置到路标1约100m，路标1到路标2约40m，路标2到返回位置约20m，路标1到终止位置约60m。

图4　行进路线地图Fig 4　Map of walking route

采用三种不同方案验证协同导航方法的有效性,实验结果如图5所示。

图5　不同工作方案下移动机器人导航定位误差Fig 5　Navigation and positioning error of mobile robot under different working schemes

由图5可知：以实验地点地理测绘信息为定位基准，双足机器人单独工作方案在实验结束时刻定位误差约为9.6m，约为行进路程的3.4 %；轮式机器人单独工作方案在实验结束时刻定位误差约为16.8m，约为行进路程的6.0 %；移动机器人信息双向融合的协同导航工作方案在实验结束时刻定位误差约为6.4m，约为行进路程的2.3 %。

3.2实验结果分析

分析上述实验结果，采用信息双向融合的协同导航方法，可有效提高移动机器人群体导航系统的定位精度。这种导航方式下的导航定位误差主要来源于以下几个方面：

1)姿态与航向误差，主要由IMMU组件的器件误差、硬软磁误差经标定补偿后的残差等方面造成；

2)零速修正误差，主要由惯性导航系统模型误差、惯性器件模型误差以及步态相位的误检造成；

3)SLAM与VDR导航定位误差，主要由视觉与激光传感器的检测误差、信息融合的延迟以及VDR算法误差等造成。

4　结　论

本文以在室内等GNSS失效环境中移动机器人导航定位方法为研究背景，研究了一种基于信息双向融合的机器人群体协同导航方案。该方案以同步行进的双足机器人和轮式机器人为研究对象，采用基于双足机器人步态相位检测的误差零速修正方法为轮式机器人提供较精确的定位信息；并采用轮式机器人导航系统中SLAM算法提供的定位信息实时修正双足机器人定位误差，抑制其随路程的发散。由于可实时通过双向信息融合实现位置匹配来进行系统误差的修正，在GNSS失效环境中，移动机器人群体的定位精度相对于单独工作模式有明显的提高，具有一定的理论研究与工程应用价值。

[1]StrömbackP,RantakokkoJ,WirkanderSL,etal.Foot-mountedinertialnavigationandcooperativesensorfusionforindoorpositioning[C]∥Proceedingsofthe2010InternationalTechnicalMee-tingofTheInstituteofNavigation，2010:89-98.

[2]CalusdianJ.Apersonalnavigationsystembasedoninertialandmagneticfieldmeasurements[D].Monterey：NavalPostgraduateSchool，2010.

[3]张金亮,秦永元,梅春波.基于MEMS惯性技术的鞋式个人导航系统[J].中国惯性技术学报,2011,19(3):253-256.

[4]钱伟行,朱欣华,苏岩.基于足部微惯性/地磁测量组件的个人导航方法[J].中国惯性技术学报,2012,20(5):567-572.

[5]杜钊君,吴怀宇.基于激光测距与双目视觉信息融合的移动机器人SLAM研究[J].计算机测量与控制,2013,21(1):180-183.

[6]弋英民,刘丁.基于单目视觉的轮式机器人同步定位与地图构建[J].仪器仪表学报,2010,31(1):117-121.

[7]SmithR,SelfM,CheesemanP.Autonomousrobotvehicles[M].NewYork：Springer，1990:167-193.

[8]张文玲,朱明清,陈宗海.基于强跟踪UKF的自适应SLAM算法[J].机器人,2010,32(2):190-195.

钱伟行，通讯作者，E—mail：61192@njnu.edu.cn。

Robot cooperative navigation method based on information bidirectional fusion*

REN Ren-kai1, QIAN Wei-xing1, PENG Chen1, ZHU Yan-hua2, XIE Fei1

(1.School of Electrical and Automation Engineering,Nanjing Normal University,Nanjing 210042,China; 2.School of Instrument Science & Engineering,Southeast University,Nanjing 210096,China)

The method builds information bidirectional fusion filters on the basis of different error divergence of the foot-mounted inertial navigation system and dead reckoning/simultaneous localization and mapping(SLAM)navigation system.The zero-velocity update(ZUPT) of biped robot is used to reduce the error of navigation system,while the SLAM algorithm of wheeled robot is used to correct the positioning error,which both improve the positioning and heading precision of two navigation systems synchronously.Experimental results show that,the positioning error of navigation system is about 2.3 % of the walking distance, which can effectively improve the navigation performance of the robot group in the environment without GNSS.

biped robot; wheeled robot; information bidirectional fusion; simultaneous localization and mapping(SLAM); zero-velocity update(ZUPT)

2015—10—23

国家自然科学基金资助项目(61304227)；江苏省自然科学基金资助项目(BK20141453，BK20130628)；江苏省高校自然科学基金资助项目(13KJB590001)

U 666.1

A

1000—9787(2016)08—0040—04

任仁凯(1991-)，男，江苏南京人，硕士研究生，主要研究方向为惯性导航和智能家居设备。

DOI:10.13873/J.1000—9787(2016)08—0040—04