智能系统：面向救援任务的地面移动机器人路径规划

第4卷第5期 智能系统学报 VoL.4 No.5 2009年10月 CAAI Transactions on Intelligent Systems 0ct.2009 doi:10.3969/j.i8sn.16734785.2009.05.005 面向救援任务的地面移动机器人路径规划 秦世引，高书征 (北京航空航天大学自动化科学与电气工程学院，北京100191) 摘要：由于救援机器人处于复杂环境中，面向紧急救援任务的实时性要求较高，因此，救援机器人的路径规划技术 在整个救援过程中发挥着十分重要的作用.针对复杂环境条件，用多边形表示障碍物，设计了一种基于障碍物编码 的遗传算法，进行路径规划.与以往的基于顶点编码的方法相比，该方法对障碍物复杂不规则的情况适应性更强，同 时也缩减了解搜索空间，提高了算法的效率，增强了路径规划的实时性.通过异构多机器人联合救援模拟实验验证 和分析，表明所提出的方法能够使机器人避让复杂环境中不同类型的障碍物，实现高效实时的路径规划，可操作性 强，可以推广应用于实际救援系统中. 关键词：路径规划：障碍物编码：机器人救援：遗传算法 中图分类号：TP242.6文献标识码：A文章编号：16734785(2009)050414-07 Path planning for mobile rescue robots in disaster areas with complex environments QIN Shi-yin,GAO Shu-zheng (School of Automation Science and Electrical Engineering,Beihang University,Beijing 100191,China) Abstract:Rescue robots often work in complex environments and their effectiveness can determine whether victims will survive.This makes research on robot path planning of great significance.In this paper,obstacles in complex environments were modeled as polygons.Based on the encoded polygon,a new path planning algorithm was pro- posed using genetic algorithms.Compared with previous vertex-based encoding methods,the proposed encoding method can more easily adapt to the irregular shapes of obstacles.The solution space is thus considerably reduced, improving the efficiency and real-time performance of the path planning algorithm.Experimental results demonstra- ted that the algorithm can effectively guide a robot around obstacles in complex environments,producing acceptable paths for robots covering difficult terrain in rescues.The algorithm can be extended to practical rescue systems in- volving multiple robots. Keywords:path planning;obstacles encoding;robot rescue;genetic algorithm 近年来多发的自然灾害如雪灾、火灾、水灾、地 境感知和目标识别的可靠性和准确性.其中地面机 震、瘟疫以及人为的恐怖活动都威胁着人们生命财 器人负责直接的搜寻和救援，空中飞行机器人在灾 产的安全.采用救援机器人辅助甚至代替救援人员 害环境中可以不受地面状况的影响、灵活性高、“视 完成对幸存者的搜救任务，能够大大提高救援的实 野”广、加快搜索速度，在搜救行动中更多地从事现 施效力和执行效率，减少人员伤亡1.】.这种异构机 场勘查和为地面救援机器人探路导航等工作.它利 器人救援系统由多种类型的机器人协作完成任务， 用所得到的环境信息快速进行路径规划，并遥控地 地面救援机器人和空中飞行机器相结合进行立体化 面救援机器人按照规划的路线行进. 侦察、搜寻和救援，以扩大搜索和救援范围，提高环 机器人路径规划3)本质上是一个NP完全问 题，难以求得精确解.传统的图搜索法、栅格法等搜 收稿日期：200905-29. 索效率较低，路径优化结果[46]也不尽理想.遗传算 基金项目：国家自然科学基金资助项目(60875072)：国家高技术研究 发展计划(863)资助项目(2006A404Z207):教育部博士点 法是模拟生物在自然环境中的遗传和进化过程而形 基金资助项目(20060006018)：中澳国际合作资助项目 (2007DFA11530). 成的一种自适应全局优化概率搜索算法，它对复杂 通信作者：秦世引.E-mail:qy@buaa.ed血.cm. 的非线性问题具有良好的适用性.本文在用多边形

第5期 秦世引，等：面向救援任务的地面移动机器人路径规划 ·415· 表示障碍物的基础上，提出了一种新的基于障碍物 的接触，造成碰撞或者阻挡[7，o] 编码的方法，和以往类似方法[7，别相比，进一步缩小 2)对圆形障碍物用正多边形表示，可以用正六 了搜索的解空间，提高了搜索速度.通过异构多机器 边形、正八边形，边数越多越精确，然而边数越多，在 人联合救援模拟实验，验证了此方法非常适用于复 判定是否能穿越障碍物的过程中随之造成的计算负 杂环境中的障碍物建模，能够提高基于遗传算法的 担也会增加； 路径规划效率， 3)如图2(c)所示，当障碍物某一顶点处的锐角 1基于障碍物的编码 度数特别小时，其两侧外轮廓线的焦点将远离实际 顶点P,那么按照P的位置来规划路径，必定产生错 1.1环境建模 误的结果.因此，可以将P点视为用2个顶点表示的 如图1中，空中飞行机器人采集的图像和地面 一段很短距离的线段，使建模更合理，如图中虚线圈 搜救机器人通过超声、红外等传感器获得的信息进 区域所示； 行融合，得到环境中障碍物的精确位置，由空中机器 4)凹多边形内角和大于180°的点是不可能到 人规划好路径，将控制指令通过无线网络发送给地 达的，否则必然不是最短路径.对凹多边形通过增加 面救援机器人，这将大幅度提高救援系统的工作效 连线，将凹多边形转化成多个相互连接的凸多边形 率.显而易见，只有准确建模复杂环境，快速地规划 以方便最短路径的计算，以及判定所规划的路径是 出地面救援机器人的行进路线，才能够快速实施救 否穿越障碍物 援，将灾情导致的损失和伤亡降到最低.目前环境建 模方法包括可视图法、切线图法、Voronoi图法、拓扑 法、惩罚函数法、栅格法等)].前4种方法都是采用 基于图论的思想，将目标、机器人及其工作空间用一 个连接图表示，将路径规划问题转化为在图上寻找 (影胀特碍物 (》圆形诗碍物建模 一条从起始节点到目标节点的路线.惩罚函数法将 路径规划这个有约束的问题（受到障碍物的限制） 转化为一个无约束最优化问题，再求解就可得出解 答.栅格法用网格描述机器人的工作环境，根据栅格 的可信度值可确定出障碍物的分布，此时通过避障 规划就可得到无碰路径.文献[10]提到了根据机器 c尖角弹碍物建损 (d)阿障码物建模 人尺寸膨胀障碍物的方法，文献[78]用到一种用 图2障碍物轮廓的多边形拟合 多边形表示障碍物的形式，将路径限制在障碍物顶 Fig.2 Modeling obstacles with polygon 点的连线上，大大缩小了搜索的解空间， 经过上述处理，从指定起始点到目标点的最短 距离问题的最优解必然是通过多边形某些顶点连起 环境佳贺 来的路线.这样不仅限定了可行解空间，提高搜索算 路径规划 法效率，而且可以简化路径编码方式，通用性更强。 1.2染色体编码方式 信总反磷 采用多边形表示障碍物的建模方法，文献[8] 将路径编码成定长为环境中所有障碍物顶点个数之 和的十进制染色体串.与基于顶点的编码方式不同， 图1异构多机器人救援系统 Fig.1 Rescue system of heterogeneous robots 本文采用基于障碍物的编码方式.首先对M个由多 边形表示的障碍物编号，令所有障碍物顶点个数之 本文同样采用多边形表示障碍物，并且完善了 和为N,其顶点按一个方向（顺时针或逆时针）依次 对不同形状障碍物处理的规则，如图2所示.具体处 编号.然后对路径经过每个凸多边形的顶点用4位 理方法如下： 十进制数表示，如： 1)如图2外轮廓线所示，考虑到实际机器人尺 寸，对障碍物用多边形表示时留出一定的安全距离 Sde Nwot 使得机器人沿障碍物运动时不会与障碍物发生直接

·416 智能系统学报 第4卷 其中：S表示障碍物序号，该障碍物是第几个 要的，它是引导遗传算法向问题最优解逼近的关键 被经历的；S表示顶点序号，从该障碍物第几个顶 因素.对于具有约束性能指标的最优问题，可以取性 点开始；N表示经过的顶点个数；D表示顶点排列 能指标或性能指标的变换作为适应度函数[].本文 的方向，为0（顺时针）或者1（逆时针）.种群个体均 讨论的问题是求一条最短路径，约束条件是路径不 为长度为M×4的十进制染色体串.每条染色体中 能穿越障碍物.为了方便赌盘选择操作，将适应度函 M个S位为1~M的随机排列，第M:=(i=1,2, 数定义为最大化问题： ·,M)个障碍物的顶点个数为N:,路径经过障碍物 maxF(p）=A-dm(p）-rp(p). 的起始顶点S为1~N:的随机数.N表示经过的 式中：A为适当大的实数值(A的取值保证F(P)为 顶点个数，为1，)的整数，脑止路径对某个摩 非负的实数)，dm(p)= ∑d(m,m1)表示路径 碍物的环绕.顶点排列的方向D有50%的概率为 总长，d(m:,m+1)表示两相邻节点m:、m+1之间的 0或者1. 欧氏距离，p(p)表示该路径穿过障碍物的次数，通 作为对比，引用文献[8]中的环境图（图3），其 过依次判断障碍物顶点是否在路径两侧即可得到.， 中编号1~6的方块表示障碍物，编号1~24的圆点 取一个比较大的正实数表示对穿过障碍物的不可行 表示障碍物的顶点.对图中一条从起始位置到目标 路径的惩罚，实验中取值5000，远大于可行路径长 位置的可行路径，基于顶点和基于障碍物的编码对 度以使路径尽快进化为可行解.称dm（p)+rp(p) 比如图4所示.这种编码方式将路径中经过的所有 为路径的等效距离，之所以称其为等效距离，是因为 不规则障碍物顶点的信息都用4位整数表达，在障 它包含了对路径穿过障碍物的惩罚.在用多边形表 碍物顶点很多时减小了染色体编码的长度，更重要 示障碍物的方法完成环境建模后，首先计算出各顶 的是可以有效防止路径重复经过某一个障碍物的 点之间的等效距离矩阵，以后计算个体适应度时，依 “迂回”情况发生，大大缩小解的搜索空间。 次从等效距离矩阵中取出路径相邻顶点的等效距离 值，这种建立查询表的方式可以避免对两顶点间线 8 段等效距离的重复计算，节省了时间。 2.2遗传操作 2.2.1选择 采用传统的赌盘选择]方法，若种群中有N 个个体，第个个体的适应度为f,则它被选择进入 56789 下一代的概率为，“这样适应度大的个体更 图3编码后的障碍物 有可能进人下一代，同时进化中采用最优个体保留 Fig.3 Encoded obstacles in the environment 策略。 2.2.2个体交叉 基于顶点编 :00 21000000000 00034000560 从父代中随机选择2个个体，以障碍物为单位 分成2部分，交换彼此的第2部分生成2个新个体 基于障碍物编码 新个体中可能会出现2个障碍物序号相同的情况， :20x X o xxx o xx x o xep a2▣： 因此要进行1次障碍物序号的排序，将障碍物序号 重新整合成1~M的整数排列，而且每个整数只出 图4基于顶点和基于障碍物的编码对比 Fig.4 Comparison between vertex-based encoding and 现1次 obstacle-based encoding 2.2.3变异 相对于传统方法中的插入节点、删除节点、变换 2 基于遗传算法的路径规划 节点u3,这里变成Ser、Ne、D位的变异.S等概 2.1适应度函数 率取（1，N)的随机数，N等装率取(1，)的整 适应度函数[)的选择在遗传算法中是相当重 数，D变异为(1-D),具体操作如图5所示

第5期 秦世引，等：面向救援任务的地面移动机器人路径规划 ·417· 表13种算法参数 Table 1 Parameters of the three algorithms 顶点编码 顶点编码 障碍物 参数 简单算法 改进算法 编码算法 种群规模 30 30 30 (a)个体父叉 操作算 个体交叉 个体交叉 个体交叉 子概率概率20% 概率20% 概率20% 变异 插人节点 变换起始顶点 概率20% 概率20% 概率20% 改变起始点 删除节点 变换经过顶点 概率20% 个数概率20% 改变圆 。点个复 变换节点 变换顶点排列 概率20% 方向概率20% }改变项点个数 图6是对3种算法分别实验100次后取均值得 到的算法效果对比图.在实验中，基于顶点编码简单 改变经讨 算法、基于顶点编码改进算法、基于障碍物编码算 上顶点力向 法最后搜索到全局最优解的概率分别是65%、 100%和95%. 成变经过顶点方向 司出班可行鲜平均代数 图5遗传操作 口现最优解平均代数 Fig.5 Operators of the genetic algorithm 最优路径平均长度 81.54 3实验和分析 3.1算法效率比较 22.1 18.34 94102903822608 3.1.1四边形障碍物 顶点编妈简中算法顶点编码改进算法障碍物菊码算法 种群规模直接影响遗传算法的收敛性，由于图3 图63种算法性能比较 中障碍物形状比较简单，本文采用30个个体进行仿 Fig.6 Performance comparison of the three algorithms 真实验，当最优个体连续100代没有变化时终止算 通过对上面实验结果的分析可以发现，对简单 法.文献[8]旨在表述一种新的路径编码机制，对环境 的环境图，顶点编码改进算法不仅收敛性比顶点编 图3只采用了简单的交叉变异操作，但是实验表明插 码简单算法有显著改善，而且寻找到全局最优解的 入节点、删除节点、变换节点3]等操作算子可以显 能力大大提高.相比而言，基于障碍物编码算法，在 著提高遗传算法寻优能力和收敛速度.在同样的多边 很少代数就出现了可行解和局部最优解，主要原因 形表示障碍物建模的情况下，本文对基于顶点编码的 是这种编码方式在大大缩小了解的搜索空间.但是 简单（只有交叉变异操作）算法、基于顶点编码的改进 最后算法寻找到全局最优解的概率比顶点编码改进 算法（包括个体交叉、插入节点、删除节点、交换节点 算法略低一些，因为当各个障碍物顶点数N:≤4时， 操作)和基于障碍物编码的方法进行对比仿真实验。 基于障碍物编码的染色体长度并不比基于顶点编码 为了使得顶点编码的算法与障碍物编码算法能够相 算法短，而且编码格式的限制使其寻找全局最优的 互比较，本文为顶点编码算法增加了插入节点、删除 能力有所减弱， 节点以及变化节点的操作，以求更符合实际的对比效 3.1.2多边形障碍物 果.算法参数设定如表1所示。 当障碍物形状比较复杂或者用多边形表示圆形

·418 智能系统学报 第4卷 障碍物时，为了获得较高精度而增加正多边形边数， 变化和某一次实验中最优个体路径等效距离长度变 此时不仅个体编码长度大大增加，而且整个解搜索 化对比图分别如图8和图9所示.实验中对穿过障 空间也大大增加.对这种顶点数N≥4的情况也进 碍物的不可行路径的惩罚r取5000，可见种群初始 行了实验，机器人环境图如图7所示.需要说明的 化时基于障碍物编码方式的算法更容易产生适应度 是，这里影响算法效率的主要参数是多边形的顶点 高的个体 数，而与多边形是否规则无关，为了方便布置，采用 4.0x10 了正八边形.这次实验将种群规模设置为50，操作 一顶点编码简单算法 3.5 顶点编码攻进算法 算子概率参数不变，如表1所示，种群迭代200代结 3.0 障碍物编码算法 2.5 束程序. 2.0 9 1.5 8 2 D 1.0 0.5 191 0- 102030405060708090100 种群代数 图9最优个体路径等效距离长度对比 Fig.9 Comparative results of equivalent distance of the op- timum individual contrast 3.2救援过程路径规划模拟实验 6 在采用广角相机、计算机和ASR(ability storm 图7八边形障碍物环境示例 robot)机器人等搭建的异构多机器人联合救援模拟 Fig.7 An example of environment with octagon obstacles 实验平台（图10）上进行了实验.计算机和广角相机 810 模拟空中机器人采集地面图像，完成环境建模和路 一顶点编码简单算法 径规划，通过无线网络将控制指令发送给地面救援 一顶点编码改进算法 6 碎碍物编码算法 机器人，使地面救援机器人沿着规划出的路径前进， 5 并精确定位到救援目标位置。 4 3 电缆 2 K冈图像采集卡 广角摄像头 0 102030405060708090100 天线 种群代数 无线网络 图8种群路径等效距离平均长度对比 无线路中器 Fig.8 Comparative results of average equivalent distance of 移动机器人 population contrast 使携式标准网线移动机器人 计算机 驱动轮 当种群进化200代后，最优解路径可行的概率 模拟空巾机器人移动机器人 分别为12%、65%、100%.主要原因在于基于顶点 导向轮 线通 各类 辟碍物待救援 的编码方式路径经常会出现多次经历同一障碍物的 讯模块传感器 口标 “迂回”现象，而且在没有任何先验信息的情况下随 图10多机器人联合救援模拟实验平台 机生成的路径穿过障碍物的概率更高；基于障碍物 Fig.10 Rescue simulation platform of multi-robots 的编码方式本质上是利用了同一障碍物的顶点集合 实验中关键参数如表2所示.对广角摄像头采 作为先验信息，能产生平均适应度更高的随机个体， 集的畸变图像[5]，如图11所示，文中采用三次多项 并且特殊的编码方式大大减少了解的搜索空间.前 式拟合图像坐标和世界坐标的对应关系，通过32组 100代种群路径等效距离dm（p)+Tp(p)平均长度 特征点求解多项式的系数，实现图像坐标到世界坐

第5期 秦世引，等：面向救援任务的地面移动机器人路径规划 ·419· 标的映射.还利用投影关系对机器人高度引起的误 解和最优解出现的时间大大缩短.在模拟实验中，10 差进行了补偿，实现了对机器人的精确定位，并通过 组机器人路径规划平均时间t<0.2s,已经低于反 基于图像[16]的视觉反馈控制门提高机器人导航的 馈控制的时间周期，使实时系统的在线路径规划成 精度， 为可能 表2模拟救援实验系统参数 Table 2 Parameters of the rescue simulation system 参数项 参数值 计算机配置 Core2 Duo CPU2.1GB,内存2GB 无线网卡 802.11b,54Mbps 相机分辨率 640 pixel×480 pixel 编程语言 Visual C++ 机器人半径 0.24m 图12机器人按照规划的路径在救援实验环境中运动 机器人运动区域 4.8m×2.4m Fig.12 The robot moves along the planned path in the res- 机器人定位精度 误差<0.03m cue simulation environment 机器人行进速度 0.06m/s 每秒处理图像数 5帧 4结束语 本文针对面向救援任务的机器人路径规划问 题，设计了一种基于障碍物编码的遗传算法.较之以 往基于顶点编码的算法，该算法可以初始化产生适 应度更高的个体，而且通过缩减解搜索空间加快了 遗传算法的搜索速度，缩短了可行解和最优解的搜 索时间，使在线实时的操作成为可能.通过模拟实验 证明了这种方法的可行性和有效性，可以推广应用 到下一步将要研究的实际的救援系统中. 图11。模拟救援实验环境 参考文献： Fig.11 Environment of the rescue simulation 采用基于障碍物编码的遗传算法进行路径规划。 [1]CASPER J L,MICIRE M,MURPHY RR.Issues in intel- 种群规模取50，交叉变异参数与表1相同.对图11环 ligent robots for search and rescue[C]//Proceedings of 境放置了不同的障碍物，改变其位置，并指定不同的 SPE.0 rlando,USA,2000,4024:292-302. [2]MATSUNO F,TADOKORO S.Rescue robots and systems 起始终止点，做了10组实验.图11是某一次实验中 in Japan[C]//Proc of IEEE Int Conf on Robotics and Bio- 间时刻置顶相机采集到的图像，图12是监控程序画 mimetics.Shenyang,China,2004:12-20. 的对应的路径规划与导航控制图，已经转化到标准直 [3]邬再新，李艳宏，刘涛.多移动机器人路径规划技术 角坐标系下，O为原点，横轴为x轴，纵轴为y轴，黑色 的研究现状与展望[J].机械，2008(1)：13-16 区域为障碍物区，图像四周深灰色区域为不可行区， WU Zaixin,LI Yanhong,LIU Tao.Research progress and 黑色周围的浅灰色区为考虑到机器人尺寸而做的安 future development on path planning for multi-robot[J]. 全限制区，白色部分为安全区.S为机器人起始点，D Machinery,2008(1):13-16. 为目标点.规划出的路径在图12中用细线表示，机器 [4]QUINLAN S,KHATIB O.Elastic bands:connecting path 人行进效果如图12中黑色粗线所示， planning and control [C]//IEEE International Conference 由于基于障碍物编码的方法初始化产生性状更 on Robotics and Automation.Atlanta,USA,1993,2:802- 加优良的种群，并且大大减少了解的搜索空间，可行 807

·420 智能系统学报 第4卷 [5]STENTZ A.Optimal and efficient path planning for partial- USA:IEEE Computer Society,2006:637-642. ly-known environments[C]//Proceedings of the IEEE Inter- [14]MAHJOUBI H,BAHRAMI F,LUCAS C.Path planning national Conference on Robotics and Automation.Los Alam- in an environment with static and dynamic obstacles using itos,USA:IEEE Computer Society,1994,4:3310-3317. genetic algorithm:a simplified search space approach [6]黄彦文，曹其新.RoboCup比赛环境下足球机器人路径 [C]//IEEE Congress on Evolutionary Computation.Van- 规划研究[J].智能系统学报，2007,2(4)：5257. couver,Canada,2006:2483-2489. HUANG Yanwen,CAO Qixin.Path planning for robot soc- [15]刘宏鼎，秦世引.双目轮式移动机器人的运动目标识 cer in the RoboCup environment[J].CAAI Transactions on 别与跟踪[J].智能系统学报，2007,2(3)：19-24. Intelligent Systems,2007,2(4):52-57. LIU Hongding,QIN Shiyin.Recognition and tracking of [7]WANG Y,SILLITOE I,MULVANEY D J.Mobile robot moving targets for binocular wheel moving robotst[J]. path planning in dynamic environments[C]//IEEE Inter- CAAI Transactions on Intelligent Systems,2007,2(3): national Conference on Robotics and Automation.Roma, 19-24. 1ay,2007:71-76. [16]GONZALEZ R C,WOODS R E.Digital image processing [8]蔡自兴，彭志红.一种新的路径编码机制在移动机器人 [M].2nd ed.Upper Saddle River,USA:Prentice Hall, 路径规划中的应用[J].机器人，2001(3)：230-233. 2002:420-500. CAI Zixing,PENG Zhihong.The application of a novel path [17]方勇纯.机器人视觉伺服研究综述[J].智能系统学 encoding mechanism in path planning for a mobile robot 报，2008,3(2)：109-114. [J].Robot,2001(3):230-233. FANG Yongchun.A survey of robot visual servoing[J]. [9]戴博，肖晓明，蔡自兴.移动机器人路径规划技术的 CAAI Transactions on Intelligent Systems,2008,3(2): 研究现状与展望[J].控制工程，2005(3)：198-202. 109-114. DAI Bo,XIAO Xiaoming,CAI Zixing.Current status and 作者简介： future development of mobile robot path planning technology 秦世引，男，1955年生，教授，博士 [J].Control Engineering of China,2005(3):198-202. 生导师，主要研究方向为复杂系统的智 [10]WISE K D,BOWYER A.A survey of global configuration- 能控制、图像处理与模式识别等。作为 space mapping techniques for a single robot in a static en- 负责人主持完成（或在研）国家攀登计 vironment[J].The International Joumnal of Robotics Re- 划项目的子项目、国家“973”项目的子 search,2000,19(8):762-779. 课题、国家“863”项目、国家自然科学基金项目、国防科技预 [11]HU Y R,YANG S X,XU LZ,et al.A knowledge based 研基金项目、武器装备预研基金项目等18项.1999年获全 genetic algorithm for path planning of a mobile robot envi- 国优秀科技图书奖暨科技进步奖（科技著作）一等奖，1999 ronments [C]//IEEE International Conference Robotics 年获国家第五届工程设计优秀软件金奖.发表学术论文130 and Automation.Shenyang,China,2004:767-772. 余篇，出版学术著作1部，研究生教材1部，译著2部. [12]王小平，曹立明.遗传算法一理论，应用与软件实现 高书征，男，1985年生，硕士研究 [M].西安：西安交通大学出版社，2002：25-28. 生，主要研究方向为移动机器人路径规 [13]LI Q,ZHANG W,YIN Y,et al.An improved genetic al- 划与视觉导航控制， gorithm of optimum path planning for mobile robots[C]/ Proceedings of the Sixth International Conference on Intelli- gent Systems Design and Applications.Washington DC