无路标环境下遥操作机器人SLAM系统

曹卫华，吴净斌，吴 敏，陈 鑫

自K.Goldbergz在Mercury Project中提出基于Internet的网络机器人后［1］，随着网络和无线通信技术的发展，结合无线网络通信设计具有本地自主的探索移动机器人具有非常重要的意义，其在军事、安防、家政等领域具有很好的应用前景.

对于探索机器人而言，SLAM(simultaneous localization and mapping)是一项非常重要的功能［2-5］.目前多位学者提出一些SLAM算法，将其应用于探索机器人中，其中基于EKF(extended Kalman filter)的 SLAM 算法［4，6-8］、基于EM(expectation maximization)的SLAM算法［9-14］、基于粒子滤波器的FastSLAM算法［15-16］成为SLAM研究的主流方向.这些算法都有较大的计算量且复杂的优化过程，对机器人的控制器要求高，这对于目前以嵌入式系统为控制器的机器人而言就显得过于复杂.

实时定位是SLAM的基础，室内环境下可使用里程计结合电子罗盘或人为设定路标的方式.在有路标条件下的定位研究已经取得了很多成功实例［17-18］，但在未知环境中设置路标不具有实用性.一些学者提出，在未知环境下，从机器人携带的激光测距数据中提取直线段信息或直线段之间的交点，将其作为路标［19-20］;但其寻找路标过程却增加了SLAM算法的复杂度，同时也增加了控制器的负担，不适宜应用于嵌入式系统.

针对SLAM算法的复杂性和实时性问题，设计了多控制器协作及多传感器信息融合的移动机器人硬件结构，开发了快速地图构建及探索方法.该方法包括:多传感器信息融合的实时定位及路径规划和基于网络地图构建方法.基于无线网络的未知环境下探索机器人系统可分为移动机器人子系统(以下简称机器人)和机器人远程状态监视子系统(以下简称监视系统)2个部分.远程访问者通过Web登录机器人系统，并下载机器人上的Java Applet监视系统的操作界面程序，对机器人进行访问.

1 机器人SLAM和路径规划

未知环境下探索机器人在实现SLAM过程中，需要经过实时定位、局部路径规划、地图绘制3个阶段.机器人通过基于里程计和电子罗盘的实时定位方式，获得当前机器人的实时坐标.机器人基于当前坐标与预设定的目标点，通过多传感器信息融合技术完成实时路径规划，并引导机器人向目标点移动.同时将所获得的环境信息，通过无线网络通信的方式传输给监视系统完成地图绘制.

1.1 实时定位

在常规的机器人控制中，机器人往往需要以机器人的朝向角为X轴建立局部坐标系，机器人在进行局部到全局坐标系变换时需要建立位姿转换矩阵.本文机器人通过电子罗盘，可以自身维持一个不变方向的局部坐标系，实现局部坐标系与全局坐标系朝向保持一致.这样简化了坐标变换过程，计算量也相对减少，有利于将有限的控制器资源合理利用.

机器人采用里程计结合电子罗盘信息的定位方式.其定位过程如下:机器人得到目标点后，以自身为原点，东西方向为X轴，南北方向为Y轴，建立坐标系AOB，计算出目标点坐标.然后再以目标点为原点建立全局坐标系XOY，经过坐标变换得出机器人在全局坐标系中的坐标.当机器人移动时，电机反馈脉冲并计数，可实时计算出机器人走过的里程.电子罗盘以30 Hz的频率输出角度信息，可实时获知机器人移动朝向的角度.由此，便可实时计算出机器人在全局坐标系中的坐标，如图1所示.

为便于控制及简化计算，机器人通过原地转向运动和直线运动的组合实现向任意目标点移动.设初始时刻t0机器人的坐标为(xt0，yt0)，直线运动状态下每经过Δt时段通过电机的光电编码盘脉冲反馈可得机器人移动的距离 ΔL，则 ΔL1，ΔL2，…，ΔLn分别表示机器人在 Δt1，Δt2，…，Δtn时段走过的距离.通过电子罗盘角度信息反馈可实时获得机器人朝向角θbot，则经过T时段，机器人走过距离为在 tn时刻机器人到达(xtn，ytn)，假设任意一个T时段，机器人都处于直线运动状态下，则

式中:0≤θbot≤2π.

图1 系统坐标系示意图Fig.1 Coordinate systems referred in the calculation

1.2 局部路径规划

机器人根据设定，每遇到障碍物时就重新作路径规划，因而需要利用激光测距重新扫描周围障碍物.本文提出一种改进的 VFH(vector field histogram)局部路径规划算法［21］，该算法是基于多传感器信息融合的路径寻优方法.在规划路径之前需要定义一个与机器人朝向角相关的通过代价值，表示机器人沿该方向移动所付出的代价.在每个运动周期前，机器人自身旋转，扫描其周围360°的范围，并每隔5°计算出一个代价值.机器人根据代价最小原则选择运动方向.设待定运动方向为θ，则方向选择算法表示为

式中:C(α)为机器人在α方向上的代价函数，其计算方式为

式中:wj为影响机器人路径规划的优化分量权值，且满足为机器人相关信息，下标j代表不同的优化分量，包括:F1(q，α)为红外传感器测距值;F2(q，α)为激光传感器反馈测距值;F3(q，α)为机器人朝向与机器人和目标点连线的夹角值，其经过实时计算得出.

基于红外传感器测距值F1(q，α)可以确定机器人何时完成避障的动作.机器人在移动过程中，F1(q，α)反映其侧身的红外传感器实时检测右侧障碍物延伸状况.当检测到侧身障碍物消失，F1(q，α)值减小，机器人趋于目标点方向直接转向.基于激光测距值F2(q，α)可以测量障碍物距离信息指引机器人选择路径.基于机器人朝向与机器人和目标点连线的夹角值F3(q，α)可以确定机器人朝目标点移动的方向.因此，通过权值wj将各优化分量映射为惟一的代价值，也就综合考虑了以上信息对机器人的影响，实现实时路径规划.

图2(a)展示了该方法的一个实例.在该环境中，机器人对周围障碍物进行扫描并计算代价值.图2(b)所示为机器人扫描0°～180°时综合代价的直方图.根据综合代价，机器人运动角度在40°～55°和80°～95°的代价最小.机器人便可根据式(2)得到下一步运动的方向角.

图2 局部路径规划实例说明图Fig.2 Example of local path planning

1.3 实时地图构建

监视系统接收并显示机器人在定位与路径规划的相关信息，且同时重构机器人所经过环境的障碍物模型.实时绘图与定位都需要机器人利用激光测量周围障碍物的距离信息.距离信息的测量使用激光和摄像头相结合的技术.测量时，激光直射到障碍物上，摄像头截取当前图像，并通过对图像进行二值化，定位在图像上由激光直射到障碍物上产生的亮斑，之后根据相似三角原理计算出亮斑离摄像头的距离，即机器人离障碍物的距离.如图3所示，可知D为激光摄像头与障碍物之间的距离.并且D可由

计算得出.式中:h为激光发生器到摄像头的距离，θls为摄像头和亮斑连线与激光发生器的夹角，pto为亮斑在纵轴方向上离图像中心的像素值距离，e为角度对应像素变换率.

图3 基于图像处理的激光测距示意图Fig.3 Laser ranging with image processing

与TOF(time of flight)激光测距法相比，该方法的优点为短距离测距效果好.测距时激光是静态照射，只需处理获得的图像，此过程不存在激光反射速度过快、感受器来不及感受的问题.所以此方法能测出离机器人距离较小的障碍物.与超声波测距法相比，该方法的优点为单向性好，很长距离内光束发散很小，具有小角度分辨率.

整个绘图过程可划分为多个绘图周期.一个周期的基本步骤是:1)当机器人探测到前方一定距离内的一个障碍物时，以自身为中心建立局部坐标系AOB，通过激光测距和电子罗盘测量出相对于自身在半径为R范围内的障碍物信息，包括障碍物离机器人的距离及方向;2)根据障碍物的距离和方向信息进行局部路径规划，同时将该扫描周期得到的障碍物距离和方向信息上传至监视系统;3)监视系统将每个周期所上传的信息还原成直观的特征图.机器人到达目标点的过程中，已经历了多个绘图周期，将这些绘图周期按顺序整合在一张地图中，便渐进地将机器人所遍历的环境绘制成一张环境模型地图.

在第3)步绘图过程中，以机器人出发点为参考原点，可利用机器人反馈信息，提取障碍物的直线特征与点特征，并显示在绘图窗口.

2 实验结果与分析

为验证以上提出的未知环境探索技术，使用基于嵌入式系统的自主式移动机器人完成对预设目标的探索及实时路径规划，通过实验测试实时定位、路径规划及远程地图构建方法的有效性.

2.1 实验平台

本文所述的机器人采用ARM9体系的嵌入式处理器作为主控制器，并移植了具有较高实时性的linux(2.6.14内核)作为其操作系统，配备摄像头、无线网卡、激光发生器、红外测距传感器和电子罗盘.机器人平面长宽分别为0.2 m、0.16 m，其整体硬件结构如图4所示.监视系统通过无线网络与机器人建立连接，并负责向机器人传送控制者设定的目标点，显示机器人传送上来的视频信息和机器人的运行参数，且可利用相关传感器采集的信息绘制机器人运行轨迹和环境地图.机器人负责在未知环境中移动到目标点过程的探索，并将传感器采集的相关信息发送给监视系统.机器人在探索过程中需要具备:无线网卡实现网络通信，摄像头实现视频监视，摄像头、激光、电子罗盘实现扫描测距和路径规划，里程计和电子罗盘实现自身定位.

图4 机器人硬件结构Fig.4 Robot hardware

机器人需要一个多控制器协作与多传感器信息融合的结构实现所需的功能.可将其为3个模块:嵌入式系统为上位机的控制与决策模块;单片机为控制传感器信息融合传递的传感器模块;单片机为控制电机转动并对电机做速度反馈检测的驱动模块.控制与决策模块通过串口分别与传感器模块和驱动模块进行交互.整个硬件控制系统结构如图5所示.

图5 机器人总体结构图Fig.5 Configuration of the robot system

2.2 激光测距实验

激光测距效果直接影响实时地图绘制的精确度，首先对激光测距效果进行测试.结合激光的图像处理测距法，对图像进行二值化处理后得到只有亮斑的图片，将亮斑的位置放大处理，可清晰看到亮点，如图6所示.由此，便可得出亮点在图片的像素点坐标，经过比例运算，可得亮斑与发生器的距离，即机器人与障碍物的距离.

图6 图像处理效果图Fig.6 Results of image processing

表1中显示所测距离与实际距离的比较.可以发现，在机器人与障碍物相距0.74 m之内时，该测距方法具有较高精度，但个别测量数据精度略有跳动;当实际距离超过0.9 m的时候，误差增大.究其原因，在0.74 m之内时，机器人运动造成激光与摄像头轻微晃动，而使测量精度产生跳动.而当实际距离超过0.9 m时，由于图像上的激光亮斑移动变缓，图像处理精度下降，使其测距误差增大.

表1 测距数据对比Table 1 Data comparison

在实际应用中，现有激光测距精度不影响机器人的路径规划及监视系统的地图绘制.因为，当机器人进行测距扫描时，其移动方向上的障碍物已经处在0.74 m之内，即使某障碍物位于机器人前进方向的0.74 m之外，随着机器人移动，障碍物距离缩短，扫描的精度也随之提高，从而保证了地图绘制的准确度.

2.3 机器人运行实验

监视系统通过无线网络与机器人建立连接，其操作界面包括绘图区、视频区、数据参数区.如图7所示.将机器人置于如图8所示的一个人工实验环境中，该环境为2 m×3.2 m的矩形场地，场地中间有2块突起的隔板，将场地划分为3个较小空间，3个小空间由一直道连通.

将机器人置于场地西侧空间内，在绘图区通过鼠标点击空白区域，给机器人设定一个目标点，经过计算，机器人将目标点映射到实际场地东侧空间内，这样机器人便获知其与目标点之间的距离.机器人在起始点扫描并感知周围环境后，开始朝目标点移动.当机器人到达第1道隔板前，如图9(a)所示，扫描周期被激活，机器人进行扫描并感知周围环境.监视系统的绘图区实时将机器人获取的环境信息绘制成抽象图形.机器人在第1道隔板前完成周围环境扫描后，决策并向南移动.当机器人左侧红外传感器感知到隔板消失，机器人启动扫描并转向目标点移动.当机器人越过第1道隔板，机器人再一次启动扫描，并向目标点移动，如图9(b)所示.绘图区的地图根据机器人逐步反馈的信息生长.最终，机器人到达目标点，绘图区绘制出完整的地图及轨迹，如图9(d)所示.

图7 监视系统界面Fig.7 Interface of the monitoring system

图8 实验环境示意图Fig.8 Experimental environment

图9 机器人轨迹图及环境地图Fig.9 Traces of the robot and map building

实验完成时，机器人认为已准确到达目标点.而经过实际测量发现，机器人到达的目标点与实际设定的目标点之间存在一定的偏差，这是由光电编码盘和电子罗盘的精度误差造成的.经过多次实验测量表明，机器人在实验场地环境内，每米大约产生1.5 cm的误差，在无路标且环境未知的条件下，该误差范围是可接受的.实验表明，该机器人系统设计开发是有效的.

3 结束语

本文重点为设计一套具有高自主性的网络遥操作机器人系统，并根据机器人硬件结构设计一套在未知环境下、无路标实现SLAM的算法.算法从设计上很好满足了ARM9体系处理器的特点，结合机器人所携带的传感器，使定位与地图绘制过程都具有较好的实时性.

实验表明，机器人激光测距效果满足路径规划及实时地图绘制的要求，电子罗盘与里程计定位法的误差处于可接受范围内.机器人的局部路径规划算法能有效避开障碍物，并选择可行的路径.监视系统根据机器人传回的扫描信息，很好地完成绘图任务.

下一步的研究将着手提高机器人在SLAM过程中的精度，以及研究网络遥操作机器人系统实现多机器人协作绘图的可能性，并进一步将二维静态环境扩展为三维动态环境建图.

［1］GOLDBERG K，MASCHA M，GENTNER S，et al.Desktop teleoperation via the world wide web［C］//Proceedings of the 1995 IEEE International Conference on Robotics and Automation.Nagoya，Japan:1995:654-659.

［2］LEONARD J，DURRANT-WHYTE H.Simultaneous map building and localization for an autonomous mobile robot［C］//Proceedings of the IEEE International Workshop on Intelligent Robots and Systems. Osaka， Japan:1991:1442-1447.

［3］DURRANT-WHYTE H，BAILEY T.Simultaneous localization and mapping(SLAM):part I the essential algorithms［J］.Robotics and Automation Magazine，2006，13(2):99-110.

［4］SMITH R，SELF M，CHESSEMAN P.Estimating uncertain spatial relationships in robotics［C］//Proceedings of the IEEE International Conference on Robotics and Automation.Piscataway，USA:IEEE，1987:850.

［5］SMITH R，CHEESEMAN P.On the representation and estimation of spatial uncertainty［J］.The International Journal of Robotics Research，1986，5(4):56-68.

［6］CSORBA M.Simultaneous localization and map building［D］.Oxford:University of Oxford，1997.

［7］DISSANAYAKE M W M G，NEWMAN P，CLARK S，et al.A solution to the simultaneous localization and map building(SLAM)problem［J］.IEEE Transactions on Robotics and Automation，2001，17(3):229-241.

［8］CASTELLANOS J A，TARDOS J D.Mobile robot localization and map building:a multisensor fusion approach［J］.Automatica，2003，39(6):1120-1121.

［9］THRUN S，BURGARD W，FOX D.A probabilistic approach to concurrent mapping and localization for mobile robots［J］.Machine Learning，1998，31(1/3):29-53.

［10］DEMPSTER A，LAIRD A，RUBIN D.Maximum likelihood from incomplete data via the EM algorithm［J］.Journal of the Royal Statistical Society:Series B(Methodological)，1997，39(1):1-38.

［11］BURGARD W，FOX D，JANS H，et al.Sonar-based mapping of large-scale mobile robot environments using EM［C］//Proceedings of the 16th International Conference on Machine Learning.Bled，Slovenia:Morgan Kaufmann，1999:67-76.

［12］THRUN S，BURGARD W，FOX D.A real-time algorithm for mobile robot mapping with application to multi-robot and 3D mapping［C］//Proceedings of IEEE InternationalConference on Robotics and Automation.San Francisco，USA:IEEE，2000:321-328.

［13］THRUN S，BEETZ M，BURGARD W，et al.Probabilistic algorithms and the interactive museum tour-guided robot Minerva［J］.Journal of Robotics Research，2000，19(11):972-1000.

［14］HANHEL D，SCHULZ D，BURGARD W.Mapping building with mobile robots in populated environments［C］//Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems.Lausanne，Switzerland:IEEE，2002:496-501.

［15］MONTEMERLO M，THRUN S，KOLLER D，et al.FastSLAM:a factored solution to the simultaneous location and mapping problem［C］//Proceedings of the AAAI National Conference on Artificial Intelligence.Edmonton，Canada:AAAI，2002:593-598.

［16］MONTEMERLO M，THRUN S，KOLLER D，et al.FastSLAM 2.0:an improved particle filtering algorithm for simultaneous localization and mapping that provably converges［C］//Proceedings of the International Conferenceon ArtificialIntelligence. Acapulco， Mexico:AAAI，2003:1151-1156.

［17］ARRAS K O，TOMATIS N，JENSEN B T，et al.Multisensor on-the-fly localization:precision and reliability for applications［J］.Robotics and Autonomous System，2001，34(2/3):131-143.

［18］JENSFELT P，KRISTENSEN S.Active global localization for a mobile robot using multiple hypothesis tracking［J］.IEEE Transactions on Robotics and Automation，2001，17(5):748-760.

［19］BORGES G A，ALDON M J.A split-and-merge segmentation algorithm for line extraction in 2-D range images［C］//Proceedings of International Conference on Pattern Recognition.Barcelona，Spain:IEEE，2000:441-444.

［20］JENSFELT P，CHRISTENSEN H.Laser based position acquisition and tracking in an indoor environment［C］//Proceedings of the International Symposium on Robotics and Automation. Saltillo， Coahuila， Mexico:IEEE，1998:331-338.

［21］ULRICH I，BORENSTEIN J.VFH+:reliable obstacle avoidance for fast mobile robots［C］//Proceedings of the 1998 IEEE International Conference on Robotics and Automation.Leuven，Belgium:IEEE，1998:1572-1577.