多传感器信息融合伺服的移动机器人快速跟随

周 霞，何建忠，王宝磊

（上海理工大学 光电信息与计算机工程学院，上海200093）

0 引 言

移动机器人拥有丰富的外部感知设备，可以作为信息融合技术研究的理想实验平台，数据融合技术能够对原始信息进行整合处理，为机器人快速跟随提供理想的伺服信息［1］，因此，研究多传感器信息融合伺服的移动机器人快速跟随问题重点在于将上述两方面内容的优势进行结合［2］。实际研究中，文献 ［3］利用蒙特卡罗粒子滤波方法将多传感器信息进行融合，对移动机器人快速精确定位进行伺服，保证了快速性和精确性；文献 ［4］利用扩展卡尔曼滤波方法将里程计和超声波信息进行融合，应用于机器人定位研究，消除了里程计的累计误差；文献 ［5－6］分别利用自适应加权融合算法和智能推理方法对红外传感器信息和超声波信息进行融合，并且应用于移动机器人避障研究，从实验仿真的角度验证了算法的有效性和实用性。上述文献在取得研究进展的同时，也表现出了一定的局限性，主要是信息融合的层次单一，仅进行特征级融合或像素级融合，当传感器种类、数量增多时就会产生决策上的冲突，不能满足机器人复杂任务的要求。本论文结合快速跟随任务的需要，从像素级、特征级、决策级3个层次对采集信息进行融合，涉及的移动机器人外部感知设备包括双目摄像机、单目云台摄像机、激光雷达、声纳、红外传感器、麦克风、光电编码器等。

1 移动机器人传感器外部世界感知

1.1 双目摄像机视觉导航

移动机器人视觉导航包括目标检测、坐标定位和滤波预测三部分。

目标检测模块的实现是通过对目标的特征颜色识别进行的，常用色彩空间有RGB、HIS、YUV等色彩空间。RGB空间以红色R、绿色G、蓝色B等3种基色为坐标轴。YUV空间由亮度Y和两个色度信号U、V为坐标轴。两空间的转换关系如下

如果目标非纯色，则可以通过实验的方法对目标特征色进行标定。本文目标特征色为偏浅红色，采用实验法，目标特征色YUV空间表示如下

通过以上工作即可完成对目标特征色的检测，进而对图像进行二值化处理，求取目标几何中心。

目标定位模块的实现是通过双目视觉三维测量原理完成的。

对图1假定左目摄像机坐标系O－XYZ为惯性坐标系，O1－X1Y1为左目相机图像坐标系，f1为左目摄像机焦距，Or－XrYr为右目摄像机图像坐标系，fr为右目摄像机焦距，当空间中某一点在左右图像坐标系坐标分别为 （x1，y1）和（xr，yr）时，惯性坐标系中坐标位置可由式 （3）算出［7］

其中，左右目摄像机相对位置平移向量为旋转矩阵当双目摄像机采用图2所示规则放置f＝fr＝f1时，式 （3）可化简为式 （4）

式中：d——左右目设想投影中心连线的距离。

本文利用卡尔曼滤波技术进行预测，卡尔曼滤波由状态方程、量测方程以及预测递推公式组成，分别如式（5）式 （7）所示

式中：x（k）、y（k）——机器人在惯性系下x方向和y方向的位移，——相应速度，T ——采样周期，w（k）——随机误差

式中：x1（k）、y1（k）——信号测量值，vx1（k）、vy1（k）——随机噪声

式中：Qk、Rk——w（k）、v（k）的方差，Ak、Ck表示式 （5）、式 （6）状态矩阵。

1.2 移动机器人避障设计

移动机器人可用于避障设计的传感器包括激光雷达、超声波传感器以及红外传感器。鉴于3种传感器性能，本文避障模块设计中将红外传感器屏蔽，主要利用激光雷达和超声波传感器［8］。

激光雷达适合于近距离避障，超声波适合远距离避障，因此本文对两种传感器返回的数据按照自适应的方法进行融合，即返回数据距离远则对超声波数据加以较大的权重，返回距离近则对激光雷达数据加以较大的权重。激光雷达避障策略采用VHF方法［9］，即以栅格表示环境进行避障。该方法首先通过空间变换将惯性坐标系下环境信息变换到以机器人为圆心的极坐标系中建立栅格地图，然后根据需要将栅格地图进行分层，这里分层从内到外应至少包括三层，即禁区层、影响层和无关层，当机器人进入禁区层时，应该立即停止并按原路线返回至安全位置，机器人进入影响层时应按照事先定好的避障轨迹进行移动，机器人在无关层不考虑避障，本文地图分层如图3所示。

图3 激光雷达栅格地图

在避障轨迹选择上，本文借鉴势场法［10］的思想，将机器人的跟随目标视为引力，将栅格地图中的障碍物视为斥力，其加权合力方向即为机器人避障轨迹方向，这里加权系数根据障碍物位于栅格地图的所在层进行自适应调整，越往内层斥力加权系数越大，越往外层引力系数越小。超声波传感器避障原理较为简单，但是由于超声波在一定情况下会由于特殊障碍物表面使发射光不能正常返回相应的传感器，因此需要进行合适的建模与处理。本文是与激光雷达配合使用，且主要用于远距离避障，因此采用处理较为快速的中轴线传感模型及容忍函数的滤波方法。实际避障中超声波传感器反馈结果如图4所示。

图4 超声波传感器反馈

1.3 机器人自定位模块设计

机器人自定位模块是通过位于机器人轮部的光电编码器反馈信息处理实现的，反馈信息包括左轮速度v1和右轮速度vr，在两轮距离d已知情况下，机器人旋转角速度，前向速度在一个采样周期内机器人导航角变化为Δθ＝w t，则第i次采样后机器人导航角为θi＝θi－1＋Δθ。将机器人前向速度变换到惯性系下因此，一个周期内机器人在惯性坐标系下x方向和y方向位移变化量分别为第i次采样后机器人x 方向和y方向坐标值为完成初步的机器人自定位。假设机器人速度不会在短时间内发生剧烈变化，系统误差服从均值为0的正态分布，采用滑动平均滤波方案，取得了理想的机器人自定位结果。

1.4 人机交互模块设计

人机交互模块包括人机语音交互和人机手势交互，主要用于机器人跟随的开始、停止、以及紧急处理等需要人对机器人进行干涉的情况。

人机语音交互采用微软公司提供的Speech SDK进行开发设计，利用命令识别模式将需要识别的语音信号进行存储，加上识别标签后对使用者进行声音学习训练，训练完成即可以较高精度识别使用者发出来的语音命令并产生交互决定。人机手势交互是指噪音较大时人通过手势对机器人发出干涉信号，本文手势识别采取基于特征的识别方法，首先利用单目云台摄像机对手势进行取样，然后根据肤色对取样照片进行二值化处理，计算手势面积与轮廓周长比，利用这个比值的不同完成对不同手势的识别，进而根据识别结果产生相应的交互决定。

2 跟随策略及系统集成

2.1 机器人跟随策略

机器人上电开机后，使用者首先利用人机交互模块相向机器人发送跟随指令，机器人接到指令后开始跟随，否则保持原有状态继续等待。机器人进入跟随状态后，同时利用双目摄像导航模块对目标进行定位和避障模块进行避障，如果需要避障则产生避障动作，否则正常导航。

需要特别注意的是当跟随目标停止运动并且机器人已经靠近目标时，导航模块会给出信息使机器人继续跟随，而避障模块则误认为是障碍物采取避障，因此产生动作冲突，本文对这种情况给出的特殊处理是让机器人原地等待。另外机器人在跟随过程中，机器人会实时检测优先级更高的人机交互模块，响应使用者的干涉指令。机器人自定位模块则通过显示终端对机器人移动路线进行显示，机器人跟随整体策略如图5所示。

2.2 系统集成

图5 机器人快速跟随整体策略

由于本文跟随用的机器人机械主体及硬件部分是提前设计好的，因此系统集成主要指软硬件结合的集成和软件自身的设计优化。在前文介绍的传感器模块中，都用相应的数据采集卡与主控机进行通信，而主机中的信息处理是通过软件完成的，因此需要设计合理的机器人软硬件接口，构建一个硬件采集数据与软件信息处理的中介。软件开发语言C＋＋的COM中IUnknown接口为此提供了方便，利用它可以方便的构造出相应的机器人软硬件接口，本文机器人使用设计的是可裁剪的IMTObject接口。这个接口负责对其它接口进行统一管理与协调，其他接口按照需要在这个接口基础上进行继承衍生，主要设备接口 （IMTDEVICE）包括运动Motion接口 （包括电机和编码器）、视觉Image接口、声音Voice接口、超声UltraSonic接口和激光雷达Laser接口，整体接口关系示意图如图6所示。

图6 机器人接口关系

由于机器人是一种高度自主的智能设备，因此在软件设计时首先要构建一个良好的框架对软件进行支撑，使其能正确有效的调节管理机器人感知、分析、决策、执行等各模块的关系，即软件需要满足开放性和模块化的要求。结合前文介绍的软硬件接口，本文软件框架采用分布式智能系统的多智能体结构 （MAS）。具体设计时，以 Microsoft Visual Studio 2003为开发工具。为保证模块间独立运行减少耦合影响，使用多线程技术进行并行处理，本文软件开设的线程一共有6个，分别为机器人视觉导航线程、激光雷达线程、超声波线程、运动线程、人机交互线程和MFC用户显示线程。另外，为加快软件开发速度，保证软件开发质量，本文还是用了开源的OPENCV库、微软Speech SDK 5.1语音识别库和 XML技术。

3 实验验证

实验环境选择普通实验办公场所，引导者按照参考跟随地图要求引导机器人完成快速跟随任务，实验环境参考跟随地图如图7所示。实验开始时，机器人位于起始点A点开机上电，引导者站在与机器人一定距离B点向机器人通过语音发送跟随指令，机器人开始靠近引导者，这个过程中，AB间会不断有干扰者穿插经过。机器人避开干扰，到达引导者所在的B点时，干扰者开始发出声音干扰引导者的语音指令，引导者向机器人发出手势指令使机器人原地等待并且前进至远距离C点，机器人开始对引导者进行远距离跟随。远距离跟随的下一任务为经过障碍区并自动避障，完成避障后机器人进入D点开始在引导者引导下巡航至终点E。

图7 实验环境参考跟随地图

实验中，AB段、BC段和DE段对应的传感器信息融合包括双目视觉导航模块的像素级信息融合、双目视觉导航模块与人机交互模块以及机器人自定位模块的决策级融合；CD段对应的传感器信息融合除以上两种融合外，还包括机器人避障模块中激光雷达与超声波传感器返回数据的特征级融合。

实验效果可以从图8、图9、图10所示的曲线图加以分析。从图8可以直观的看出，机器人能够基本在引导者引导下按照参考地图完成跟随任务。综合分析图9和图10中，在图7参考跟随地图的AB段，机器人从A点前进至B点，这个过程中，机器人与引导者的距离持续减小到安全距离；机器人接到引导者的手势指令后，暂停前进，与引导者的距离不断增大；图7参考跟随地图的BC段对应实际机器人远距离跟随过程，此时机器人速度及与引导者距离的性状与第AB相类似；图7CD段对应机器人的避障过程，这个过程中机器人由于障碍物的限制，速度变化很快，与引导者距离相对适中；图7DE段为机器人巡航回到终点，性状与AB、BC段相类似。另外，整个机器人快速跟随过程用时132s，途径距离约20m，能够快速顺利进行人机有交互、巡航、避障，完成跟随任务。

4 结束语

本文讨论了移动机器人快速跟随中传感器信息融合伺服的应用问题，根据传感器信息采集处理原理，给出了合适的跟随策略以及软硬件集成方案，通过实验验证此方案能够很好地解决信息融合单一的问题，在进行特征级融合或像素级融合时，能够很好地解决传感器种类、数量增多时产生的决策上的冲突，移动机器人能够快速顺利的完成复杂任务。本文所述方案应用于2011年全国服务机器人大赛快速跟随比赛中也取得了良好的效果。实验和实践结果都表明了本文方案的有效性和可行性。

：

［1］ZHAO Xiaochuan，LUO Qingsheng.School of aerospace science and engineering Beijing institute of technology ［J］.Proceeding of the 7th World Congress on Intelligent Control and Automation，2008，22 （4）：5019－5023.

［2］ZHOU Fang，HAN Liyan.A survey of multi－sensor information fusion technology ［J］.Journal of Telemetry，Tracking and Command，2006，27 （3）：1－7 （in Chinese）. ［周芳，韩立岩.多传感器信息融合技术综述 ［J］.遥测遥控，2006，27（3）：1－7.］

［3］ZHANG Xuexi，YANG Yimin.Fast and accurate self－localization of mobile robot based on multi－sensor ［J］.Control Theory＆ Applications，2011，28 （3）：443－448 （in Chinese）.［张学习，杨宜民.基于多传感器信息融合的移动机器人快速精确自定位 ［J］.控制理论与应用，2011，28 （3）：443－448.］

［4］CHEN Xiaoning，HUANG Yuqing，YANG Jia.Application of multi－sensor information fusion in localization of mobile robot［J］.Transducer and Microsystem Technology，2008，27（6）：110－113.［陈小宁，黄玉清，杨佳.多传感器信息融合在移动机器人定位中的应用 ［J］.传感器与微系统，2008，27（6）：110－113.］

［5］WANG Yanping. Application of multi－sensor information fusion technology in obstacles detection of mobile robot ［J］.Electronic Engineer，2009，35 （1）：55－57 （in Chinese）. ［王艳平.多传感器信息融合技术在移动机器人障碍物探测中的应用 ［J］.信息化研究，2009，35 （1）：55－57.］

［6］XIAN Baojin，CHEN Songtao.Intelligent autonomous robot multisensor information fusion and application ［J］.Journal of Astronautic Metrology and Measurement，2010，30 （2）：41－44 （in Chinese）.［咸宝金，陈松涛.智能移动机器人多传感器信息融合及应用研究 ［J］.宇航计测技术，2010，30 （2）：41－44.］

［7］ZHANG Guangjun.Machine vision ［M］.Beijing：Science Press，2005 （in Chinese）.［张广军.机器视觉 ［M］.北京：学出版社，2005.］

［8］YU Jinxia，WANG Lu，CAI Zixing.Self－localization technologies of mobile robot in unknown environment ［M］.Beijing：Publishing House of Electronics Industry，2011 （in Chinese）.［于金霞，王璐，蔡自兴.未知环境中移动机器人自定位技术［M］.北京：电子工业出版社，2011.］

［9］CAI Zixing，ZHENG Minjie，ZOU Xiaobing.Real－time obstacle avoidance for mobile robots strategy based on laser radar［J］.Journal of Central South University，2006，37 （2）：324－329（in Chinese）.［蔡自兴，郑敏杰，邹小兵.基于激光雷达的移动机器人实时避障策略 ［J］.中南大学报，2006，37（2）：324－329.］

［10］LI Yunchong，HE Kezhong.A novel obstacle avoidance and navigation method for outdoor mobile robot based on laser radar［J］.Robot，2006，28 （3）：275－278 （in Chinese）.［李云翀，何克忠.基于激光雷达的室外移动机器人避障与导航新方法 ［J］.机器人，2006，28 （3）：275－278.］