第5卷第3期 智能系统学报 Vol.5 No.3 2010年6月 CAAI Transactions on Intelligent Systems Jun.2010 doi:10.3969/i.issn.1673-4785.2010.03.009 智能空间中的服务机器人路径规划 薛英花2，田国会2，吴皓，吉艳青 (1.山东大学控制科学与工程学院，山东济南250061；2.山东财政学院计算机信息工程学院，山东济南250014) 摘要：为了加深服务机器人对环境的理解，实现安全高效的智能空间导航，建立了一种信息更为丰富的环境模 型一危险度地图；并针对智能空间环境部分未知的特点，设计了分层的路径规划方法.静态规划层根据已知环境 信息，采用改进的粒子群优化算法规划初始最优路径，动态规划层利用基于动态危险度地图的改进A*算法进行避 障.该方法克服了常规算法只追求路径最短的缺点，增加了对路径危险度的评价，规划出的路径既安全又较短；且该 方法实现简单，实时性好.仿真结果验证了该方案的可行性。 关键词：智能空间；服务机器人；路径规划：危险度地图：粒子群优化算法；A*算法 中图分类号：TP24文献标识码：A文章编号：16734785(2010)030260-06 Path planning for service robots in an intelligent space XUE Ying-hua2,TIAN Guo-hui',WU Hao',JI Yan-qing (1.School of Control Science and Engineering,Shandong University,Ji'nan 250061,China;2.School of Computer and Information Engineering,Shandong Finance Institute,Ji'nan 250014,China) Abstract:A service robot must be capable of deeply understand its environment,so that it may safely navigate in- telligent spaces with high performance.In order for this to be possible,a new environmental model,or danger de- gree map (DDM),was created to provide enriched environmental information for service robots.As the intelligent space is partly unknown,the layered path planning method was used.A modified particle swarm optimization (PSO)algorithm based on a known environment was introduced to get a static optimized path.The dynamic layer used a modified A algorithm for avoidance of dynamic obstacles on the basis of the dynamic DDM.The proposed method adds evaluation of the degree of danger in a path to overcome the disadvantages of conventional methods, which follow the shortest path and ignore safety.The new method is simple and meets the real-time requirements of robot navigation.The resulting dynamic path is not only safe enough but also comparatively short.Simulation re- sults demonstrated the feasibility of the method. Keywords:intelligent space;service robot;path planning;danger degree map;particle swarm optimization;A algorithm 1996年日本东京大学的Hashimoto实验室提出 种服务（如物品抓取、目标跟踪）的基本环节之一， 了“智能空间”的概念.此后，智能空间作为一种新 定义为按照某一性能指标搜索一条从起始状态到目 的技术手段，在国内外都得到了广泛研究4.将智 标状态的最优或近似最优的无碰路径.路径规划可 能空间技术应用于服务机器人系统，可以有效地解 分为静态路径规划和动态路径规划2类6$).全局 决许多机器人靠自身无法解决或难于解决的问题， 规划需要环境中的全部先验信息，而局部规划则强 使得服务机器人应用变得轻松可行5] 调避碰行为，虽实时性好，但是由于缺乏规划，有时 路径规划是服务机器人顺利完成智能空间中各 会产生局部极值点或振荡，无法保证机器人顺利地 到达目标点.智能空间的环境是部分未知的，一方 收稿日期：2009-12-15. 基金项目：国家“863”计划重点资助项目(2006AA040206);国家 面，智能空间中的整体布局已知，如沙发、茶几、电视 “863”计划资助项目(2009AA04Z220). 机柜的摆放等；另一方面，环境中存在着不可预知的 通信作者：薛英花.E-mail:yhua_xue@yahoo..com.cm 动态障碍物，如人、另外的机器人等.单独使用全局
第3期 薛英花，等：智能空间中的服务机器人路径规划 ·261· 规划和局部规划都不能满足智能空间中路径规划的 由图2可知，危险度地图包含了比栅格地图更 安全性、实时性和高效性要求。 丰富的环境信息，除了能表示障碍物的有无外，还能 本文首先分析了栅格地图的不足，建立了环境 表示任意位置的危险程度，为机器人路径规划提供 信息更加丰富的危险度地图；然后采用了分层的路 了更有效的环境表示. 径规划方法，既有效利用了已知环境信息，又实现了 实时避障.本文充分利用了智能空间的多传感器和 通信网络，从多角度获取动态障碍物的信息，使服务 机器人可以对环境做出快速反映，安全及时地完成 任务。 1环境模型的建立 栅格法是一种常用的环境建模方法，由于其表 示直观、实现简单而得到了广泛应用].图1为根 图2危险度地图 据环境中障碍物的疏密建立的栅格地图 Fig.2 Danger dergree map 2改进的PS0静态路径规划 粒子群优化算法(particle swarm optimization, PS0)是一种进化计算技术，具有个体数目少、计算 简单、鲁棒性好等优点.PS0初始化为一群随机粒 子，然后通过迭代寻找最优解.粒子根据下面的 公式来完成自己的速度和位置的进化： V:(t+1)=o×V:(e)+c1×r×(peti-X,(t)+ 图1栅格地图 c2×r×(ge-X:(t), Fig.1 Grids map X:(t+1)=X(t)+V:(t+1). 栅格地图只能表示某处障碍物的有无，不能提 式中：V:(t)是粒子i在第t代的速度，X(t)是粒子i 供更为详细的其他环境信息，如该栅格距离障碍物 在第t代的位置，粒子的速度V和位置X均为m维 的远近等.为了能提供更充分的环境信息，采用危险 向量，Pct:为粒子本身个体极值，g为全局极值，r 度地图来表示环境].危险度地图是在栅格地图的 是介于(0,1)之间的随机数，c1和c2是学习因子，w 基础上，充分考虑周围障碍物与栅格的距离和障碍 为惯性因子 物的疏密程度而建立的能充分体现该处危险程度的 地图. 设栅格地图的行数为T,列数为c.栅格的取值 用g(1≤i≤r,1≤j≤c)表示，危险度用d表示.若 g=0,说明该栅格本身就是障碍物，是机器人不可 通过区域，因此将其危险度置为最大；若=1，说 明该栅格为自由栅格，这时应根据其周围障碍物的 多少和距离远近来计算该栅格的危险度，如式(1)》 所示： 图3路径规划建模 4,= ∑Raiam,）X Fig.3 Model of path planning 定义n个m维粒子，粒子每一维位置分量x √(s-1m1)2+(s-ln1)2.(1) (1≤i≤n,1≤j≤m)分别对应图3中垂直于SG的直 式中：s为窗口尺寸因子，实际窗口大小为(2+1)× 线y,则一个粒子就对应一条路径P. (2+1);Riem,w为点(i+mJj+n)处的障碍物标志，当 在图3中，定义起始点S为Po,目标点G为 g+m+=0,即该点为障碍物时，R(m=1,否则取0. Pm+1,则路径P的长度和危险度可分别用式(2)、 (3)表示：
262 智能系统学报 第5卷 为21.其中，避障的行为采用基于动态危险度地图的 Lp =Ism 1+ (2) =0 加权A*算法实现，该方法实现简单、实时性强， m+1 Dp ds dn+d (3) 3.1跟踪静态路径的行为 若在智能空间中未发现动态障碍物，由改进的 式中：+1为点乃与点乃+1间的距离；d,为路径点 粒子群算法规划出的静态路径就是一条最优路径， 的危险度；ds为起始点S的危险度；dc为目标点G 机器人只需跟踪这条静态路径即可，目前路径跟踪 的危险度；D,越大，表示该路径的危险度越高， 的方法主要是根据静态路径划分出一些局部目标 取路径长度和路径危险度的加权和作为粒子的 点，使机器人能够沿着已经规划好的路径前进.因 适应度函数，则第i个粒子的适应度函数F表示为 此，根据上一节改进的粒子群优化算法得到机器人 F=0Lm+0aDm=01】 静态路径，将各路径点也就是各粒子的最优位置作 0 为路径跟踪的局部目标点.该方法简单易行，能迅速 式中：w,和0是Ln和D的加权因子，为大于等于 得到局部目标点，很好地满足了自主机器人导航的 零的实数，.+)表示粒子i第j维和第j+1维间的 实时性要求. 长度，d表示粒子i第j维的危险度.该适应度函数 3.2避障行为 可充分反映路径的长度和危险程度，使粒子在迭代 设定安全距离P,当机器人检测到有动态障碍物 过程中能自动避开危险度高的区域，选择一条既安 进入其探测区域时，计算机器人与动态障碍物之间的 全又长度较短的路径， 距离p.当p<p,时表示障碍物已进入机器人周围的危 3 动态路径规划 险区域，转入避障行为，否则继续跟踪静态路径 如前所述，智能空间可获得动态障碍物的位置、 智能空间为机器人动态路径规划提供了丰富的 大小等信息，将这些信息添加到已有的静态地图中 信息.由于机器人本身携带超声等距离传感器，因此 去，就可生成动态地图，再按照第1节所述的方法， 能够检测出进入其探测区域的障碍物，如图4中的 建立包含动态障碍物的危险度地图，即动态危险度 沙发和另一机器人（或人）.结合已知的先验地图 地图，如图5所示.由于动态障碍物的位置可能会随 (即全局地图)，可知沙发为固定障碍物，不做任何 时发生变化，因此智能空间和机器人必须实时监控 处理，而另一障碍物在先验地图中并不存在，故为动 动态障碍物的状态，定时刷新动态危险度地图，以保 态障碍物.另外，文中假定障碍物为圆形.由于智能 持对动态障碍物的跟踪。 空间中的动态障碍物一般为人或其他机器人，所以 该假设不失一般性，单独依靠超声只能检测机器人 与障碍物的距离，无法得到障碍物的大小等信息；利 用智能空间中的顶棚摄像头可识别出动态障碍物， 再作一个包含动态障碍物的最小外切圆作为动态障 碍物在二维地图中的覆盖范围，并把相关数据通过 动态障碍物 智能空间网络系统传递给机器人，这样机器人就可 获得比较完备的动态障碍物信息， 图5动态危险度地图 Fig.5 Dynamic danger degree map 在动态危险度地图的基础上，采用改进的加权 A*算法进行避障.取评价函数为 f.(n)wag(n)+wph(n) 超声探 即用实际耗散g(n)与启发函数h(n)的加权和作为评 机器 测区域 价函数.式中：权值心和0。在搜索过程中可动态调 整，n(n=1,2,…,8)是机器人周围8个栅格节点中的 图4智能空间中的动态障碍物检测 某一个，g(n)是从当前节点（即机器人的当前位置）到 Fig.4 Dynamic obstacle detection in intelligent space 节点n的实际代价，h(n)是从节点n到路径终点的估 智能空间中的动态路径规划包含3个基本行为， 计代价，h(m)体现了搜索的启发信息B, 即跟踪静态路径的行为、避障的行为和目标制导的行 为了在避障的同时能自主选择危险度低的路
第3期 薛英花，等：智能空间中的服务机器人路径规划 ·263 径，在g(n)中加入了表示路径节点危险度的信息， 碍物，若有则重新规划后续路径，否则转4)； 即取机器人当前节点到节点n的路径长度1n和路 4)判断原静态路径是否逐渐趋近于当前路径 径危险度d.的加权和作为g(n),即g(n)=wln+ 点（即偏离程度会越来越小），若是，则继续跟踪静 w:0n·图6为机器人周围的局部动态危险度地图，机 态路径，否则重新规划后续路径. 器人左上方节点的危险度为255，即该处有障碍物， 寻回静态路径和重新规划后续路径仍采用改进 由于此处的危险度远远高出其他位置，故此节点不 的PS0算法，因为需要规划的路径相对较短，故粒 会被选中，无障碍物区域的危险度为0~1（已进行 子数和粒子维数都很少，所需时间也大大缩短 归一化). 4实验仿真结果 255 0.2873 0.2325 硬件环境为ntel(R)Core(TM)2E4700@ 2.60GHz、RAM4.0GB,软件环境为Microsoft Win-- 0.2873 Robot 0.1897 dows Vista Home Premium,Matlab 7.4.0(R2007a). 参数设置为：n=30,m=19,01=1.0,0a=0.3 0.2325 0.1897 0.1513 4.1静态路径规划仿真 图7为静态路径规划仿真结果.其中图7(a)为 图6局部动态危险度地图 常规PS0算法的路径规划结果，图7(b)为本文方 Fig.6 Local dynamic danger degree map 法得到的路径，经过多次实验得到其性能对比如表 取h(n)为n节点到路径终点G的欧氏距离，即 1所示. h(n)=√(，-c)+(yn-yc) 式中：（x,yn)是栅格n的中心点坐标，(xc,yc)是 路径终点G的坐标 这样，总的评价函数为 f.(n)=wg(wl +wad)+ 【a)常规PS0算法 (b)改进PSO算法 h(-%c)+(yn -yc). (4) 由式(4)可知，当一个节点的危险度越小，与机 器人当前位置越近，与终点距离越短，那么整个的启 发函数就越小，此节点就更容易选中.这样就可以保 证在进行下一个节点选取的时候，选择一个相对于 其他节点既安全又路径较短的栅格节点， (c)智能空间环境 (d)陷阱环境 3.3目标制导行为 图7静态路径规划仿真 机器人避障时，通常会偏离原始静态路径，目标 Fig.7 Static path planning diagram 制导行为是为了使机器人能够以最小的代价到达目 的地.一般来说，当机器人偏离静态路径不远时，可以 表1常规PS0算法与本文算法性能对比 Table 1 Performance comparison of conventional PSO and 引导机器人寻回原来静态路径，从而继续跟踪静态路 modified algorithm 径；当机器人已经远远偏离静态路径时，继续跟踪静 态路径可能比重新规划的路径所需代价更高，这时机 算法 路径长度路径危险度算法耗时/s 器人应当重新规划路径，以便迅速到达目的地， 常规PS0193.24 5.655 15.740322 综合考虑了机器人可能遇到的各种情况，制定 本文算法200.87 0-0.6 7.267456 的目标制导策略如下： 由表1可知，利用改进的粒子群优化算法得到 1)计算剩余路径的长度l1ae,若lse小于设定 的路径虽然比常规PS0算法略长，但是路径的危险 的阈值。，则重新规划后续路径，否则转2)； 度却大大降低，且本文算法耗时不到常规算法的 2)判断当前路径点与静态路径点的偏离程度 1/2,极大提高了算法的收敛速度.由图7(c)和(d) epe,若departure小于阈值L1,则继续跟踪静态路径， 可知该算法对一般智能空间环境和陷阱环境亦能得 否则转3)； 到最优路径. 3)判断当前路径点与静态路径点间是否有障
·264 智能系统学报 第5卷 4.2动态路径规划仿真 图8(a)为机器人避障结束后选择寻回原始静 图8为动态路径规划的仿真结果，其中动态障 态路径策略时获得的动态路径.机器人开始时跟踪 碍物处于t=10时的位置，即若不进行避障，服务机 静态路径前进；t=5时机器人探测到有动态障碍物 器人沿静态路径行进时将与动态障碍物发生碰撞， 进入其危险区域，启动避障行为；t=11时动态障碍 静态路径 物离开机器人的危险区域，避障结束，开始寻找原始 路径；t=15时找到原始静态路径，此后一直跟踪原 始静态路径直到目标点.表2为该奔向目标策略下 的性能。 图8(b)为机器人避障结束后选择重新规划后 续路径策略获得的动态路径.机器人开始时跟踪静 障碍物移动路线 动态路径 态路径；t=5时机器人转入避障行为；t=11时避 (a)避障后寻回原始路径(b)避障后重新规划路径 障结束，此时机器人根据目标制导策略，选择重新规 图8动态路径规划仿真 划后续路径并沿新规划的路径到达目标点.表3为 Fig.8 Dynamic path planning diagram 该策略下的性能。 表2寻回静态路径策略 Table 2 Strategy of looking for the static path after avoiding 路径长度 路径危险度 算法耗时/s 避障时间/s 寻回静态路径时间/s 静态路径167.8865 0.4293 6.111823 动态路径170.8371 0.0763 0.177595 0.565577 表3 重新规划后续路径策略 Table 3 Strategy of planning the remaining path after avoiding 路径长度路径危险度 算法耗时/s 避障时间/s重新规划路径时间/s 静态路径173.5816 0.8205 6.438510 动态路径174.3069 0.0388 0.179185 2.375652 由表2和表3可知，不同的奔向目标策略都可 4)动态路径规划安全可靠且路径长度较短， 以得到最优路径，且避障迅速，在避障结束后能快速 因此，本文设计的智能空间中的动态路径规划 灵活地进行后续路径规划；另外，与静态路径相比， 方案是完全可行的.当然，本文的工作也有一些不足 动态路径的长度有所增加，但是路径的危险度反而 之处，如尚未考虑智能空间中有多个障碍物的情况 有所降低，这主要是由于本文的动态避障算法更注 和机器人的实际运行速度等问题，这些问题将在今 重安全性 后的研究中进一步解决， 5结束语 参考文献： 安全性是路径规划中最为重要的一个问题.针 [1]BROOKS R A.The intelligent room project[C]//Proceed- 对智能空间环境的特点，提出一种层次化路径规划 ings of the Second International Conference on Cognitive 方法，克服了常规路径规划算法对安全性重视不够 Technology.Fukushima,Japan,1997:271-278. [2]JOHANSON B.WINOGRAD T,FOX A.Interactive work- 的缺点.本文设计的智能空间中的路径规划有以下 4个特点： spaces[J].Computer,2003,36(4):99-101. [3]NIITSUMA M,HASHIMOTO H.Spatial memory as an aid 1)能充分利用智能空间中的丰富信息，及时快 system for human activity in intelligent space[J].IEEE 速地获取动态障碍物的完备资料； Transactions on Industrial Electronics,2007,54(2):1122- 2)实时性好，能迅速避开动态障碍物； 1131. 3)避障结束后机器人能根据当前状况灵活快 [4]SHI Yuanchun,XIE Weikai,XU Guangyou.The smart 速地选择不同的目标制导策略； classroom:merging technologies for seamless tele-education
第3期 薛英花，等：智能空间中的服务机器人路径规划 .265. [J].Pervasive Computing,2003,2(2):47-55. 方法[J].机器人，2003,25(1)：18-21,43. 「5]田国会，李晓磊，赵守鹏，等.家庭服务机器人智能空间 PIAO Songhao,HONG Bingrong.A path planning ap- 技术研究与进展[J].山东大学学报：工学版，2007,37 proach to mobile robot under dynamic environment[J]. (5):53-59. Robot,2003,25(1):18-21,43. TIAN Guohui,LI Xiaolei,ZHAO Shoupeng,et al.Re- [13]RUSSELL S,NORVIG P.Artificial intelligence:a modem search and development of intelligent space technology for a approach[M].2nd ed.Beijing:Pearson Education Asia home service robot[J].Journal of Shandong University: Limited and Posts Telecom Press,2004:76-105 Engineering Science,2007,37(5):53-59. 作者简介： [6]席裕庚，张纯刚.一类动态不确定环境下机器人的滚动 薛英花，女，1974年生，博士研究生、 路径规划J].自动化学报，2002,28(2)：161-174. 讲师.主要研究方向为智能空间、机器人 XI Yugeng,ZHANG Chungang.Rolling path planning of 路径规划、机器人物品搜寻与管理.先后 mobile robot in a kind of dynamic uncertain environment 参加了国家自然科学基金、国家“863”计 [J].Acta Automatica Sinica,2002,28 (2):161-174. 划等多项项目，发表学术论文10余篇. [7]TOMPKINS P,STENTZ A,WETTERGREEN D.Mission- level path planning and re-planning for rover exploration [J].Robotics and Autonomous Systems,2006,54(2): 田国会，男，1969年生，教授、博士 174-183. 生导师.山东大学重要岗位教授、控制 [8]MORA M C,TORNERO J.Path planning and trajectory 科学与工程学院副院长、中国人工智能 generation using multi-rate predictive artificial potential 学会理事、山东省自动化学会理事.主 fields C]//Proceedings of the IEEE/RSJ International 要研究方向为服务机器人、智能空间、 Conference on Intelligent Robots and Systems.Nice, 多机器人系统的协调与协作、现代物流 France,2008:2990-2995. 系统的优化与调度等.作为课题负责人或执行负责人已完成 [9]LI Jigong,FENG Yiwei,GUO Ge.Real-time path planning 国家自然科学基金项目、国家“863”计划CMS主题基金项 based on certainty grids map in complex environments 目、国防预研项目、中国博士后科学基金项目、山东省自然科 [C]//Proceedings of the IEEE International Conference on 学基金项目等12项.获山东省科技进步二等奖、山东省教委 Integration Technology.Shenzhen,China,2007:525-529. 科技进步（自然科学理论）一等奖各1项.发表学术论文80 [10]XUE Yinghua,TIAN Guohui,HUANG Bin.Optimal robot 余篇。 path planning based on danger degree map[C]//Proceed- ings of the IEEE International Conference on Automation 吴皓，女，1972年生，博士研究 and Logistics.Shenyang,China,2009:1040-1045. 生、副教授.主要研究方向为多机器人 [11]EBERHART R C,SHI Y.Particle swarm optimization: 系统的协调与协作、智能空间中的机器 developments,applications and resources C]//Proceed- 人定位导航.先后参加了国家自然科学 ings Congress on Evolutionary Computation.Piscataway, 基金项目、国家“863”计划等多项项目， USA,2001:81-86. 发表学术论文10余篇. [12]朴松吴，洪炳熔。一种动态环境下移动机器人的路径规划
