一种动态不确定环境下的机器人路径规划算法

武心安，孙 尧，莫宏伟

(哈尔滨工程大学自动化学院，哈尔滨150001，wxashow@126.com)

目前，未知动态环境下机器人的实时路径规划的研究工作主要集中在对可用环境信息的分析处理和在此基础上的机器人动作规划上.对环境信息的分析和处理主要有:根据机器人和障碍物的安全距离直接规划机器人的运动动作［1-2］，在预定运动集合中选择机器人的运动动作［3-4］，运用关键特征信息规划机器人的运动动作［5］3种.在未知动态不确定环境下，机器人所在位置到目标点之间的路径是由连续的运动动作构成的.有很多文献描述了此类方法，例如基于栅格的A*算法［6］、voronoi diagrams and visibility graphs算法［7］、人工势场法［8］.有些算法只是应用全局的方法并计算静态障碍物，有些算法则由于面临未考虑到的环境从而使得机器人陷入死区.为了选择合理的运动动作，文献［9］将启发式函数引入进来.启发式函数通过损失函数从可能的动作集中选择最优化动作，实现机器人运动动作的连续性和最优性.

本文提出了一种新的实时路径规划算法，在环境信息中提取用于规划的双安全边缘信息，减少环境信息的处理量，并运用启发式函数规划机器人的运动动作.

1 实时环境信息描述

1.1 搜索双安全边缘信息

1.1.1 搜索传感器边缘

传感器边缘是根据传感器信息直接搜索到的障碍物边缘，根据探测到的障碍物位置可以判断其距离和方向.如图1(a)所示，o点为机器人所在质点(传感器也视为和机器人质点重合的质点)，机器人质点为圆心的自身安全区域，半径为rs;传感器簇的最大探测角度是π，最大探测半径为Rmax，障碍物为ob1和ob2;移动机器人与障碍物边缘点的距离为di.将传感器探测范围分为180个等份，相邻之间角度为1°，该半圆的直径与机器人轴线垂直.根据判定规则参数SEi，可以快速搜索到传感器边缘点集{a，b，c，d}.其中SEi= Rmax-di，i∈［0，180］.判定存在障碍物边缘点的原则为:若SEi≥2rs，则存在传感器边缘点Pi;判定传感器边缘点方向的原则为:假设从机器人左侧开始搜索传感器边缘点，若min{SEi-1，SEi+1}= SEi-1，则Pi的方向相对于机器人自身为左，记为L，若min{SEi-1，SEi+1}=SEi+1，则Pi的方向相对于机器人自身为右，记为R.如图1(b)所示为某一时刻SE的状态曲线，根据传感器边缘点判定规则可以得到传感器边缘点集{a，b，c，d}及相应的方向集{R，L，R，L}.在图1(a)中可以直观地看到传感器边缘点所处情况，它们即为当前环境信息下障碍物的传感器边缘点.

图1 搜索传感器边缘点

1.1.2 搜索机器人双安全边缘

在上节中搜索的边缘点是基于传感器的障碍物边缘点，也就是把机器人看成是一个质点.但是，实际的机器人有自己特定的外形和动力学特性，简化成质点的结果会导致路径规划的失败，因此，将机器人自身的安全区域和动力学影响范围融入到环境信息中是十分必要的.本文将结合传感器边缘点和机器人自身的安全区域与动力学影响范围，将当前时刻的环境信息抽象为双安全边缘信息，为下一步规划运动动作做好前提工作.

机器人双安全边缘定义:在搜索到传感器边缘点的前提下，结合机器人自身的安全区域与动力学影响范围，从传感器边缘点开始，按照其方向搜索障碍物上的点，使得通过这一点向机器人安全区域的切线同时又与障碍物相切，障碍物和机器人处在切线的异侧，这些点称之为双安全边缘点集，这些切线为双安全边缘集.

如图2(b)所示为某一时刻SE的状态曲线，根据双安全边缘点的定义可以搜索到双安全边缘点集{a'，b'，c'，d'}和相应的方向集{R，L，R，L}.在图2(a)中可以直观地看到双安全边缘点情况，它们就是本文用来规划机器人运动动作的关键环境信息.

成功搜索到机器人双安全边缘的意义在于有效地分析和处理了某时刻传感器所探测到的环境信息，简化了环境信息，提高了实时性能，并兼顾了移动机器人自身的安全区域和动力学影响范围，为能更加高效地规划机器人的运动动作做准备.

图2 搜索双安全边缘点

1.2 双安全边缘相关的物理量

如图3所示，以双安全边缘点b'和c'为例. ob'oc'为机器人质点到双安全边缘点的距离，m和n为移动机器人自身安全圆上的点，om=on=rs. mb'和nc'为双安全边缘点到机器人自身安全圆的切线长度，mb'=(ob'2-om2)1/2，nc'=(oc'2-on2)1/2.op平行mb'，oq平行nc'，向量p和q为当前信息下移动机器人的备选规划航向，mb'和nc'为当前信息下的备选安全距离.向量p和q之间的夹角为α，此α角用来判断机器人在当前航向下能否在两个障碍物之间通过.

图3 双安全边缘的安全距离和方向

1.3 双安全边缘的类型

在对复杂的环境信息分析和处理并搜索到了用于规划机器人运动动作的双安全边缘后，对这些双安全边缘的类型进行划分，分别为单双边缘、多双边缘和无双边缘情况，见表1.

1.3.1 单双边缘

如表1中a所示，目标点g和机器人安全圆上的点m连线为mg，只要mg与障碍物信息有交点k，则移动机器人必须要绕开障碍物ob.根据当前环境信息搜索到安全边缘点b'，安全边缘线mb'，op平行mb'，则移动机器人的运动方向为∠s=∠op，安全航行距离为de-e=mb'.

表1 双安全边缘类型

1.3.2 多双边缘

如表1中b所示，目标点g和机器人安全圆上的点m2和n1连线为m2g和n1g，只要m2g和n1g与障碍物信息有交点k1和k2，则移动机器人必须要绕开障碍物ob2.根据当前环境信息搜索到双安全点集{a，b，c，d，e，f}，与障碍物ob2相关的安全边缘集{m1b，n1c，m2d，n2e}，可行的路线集为{p1，q1，p2，q2}，α1和α2分别为向量p1和q1以及p2和q2的夹角.

1.3.3 无双边缘

根据机器人的当前传感器信息搜索不到双安全边缘.一种情况是没有探测到障碍物，则机器人可以在所探测的区域内向任何方向运动;另一种情况是障碍物探测范围部分或者全部被遮盖.如表1中c所示部分遮盖情况，此情况下没有传感器边缘点，搜索不到双安全边缘点，则机器人进入游弋状态进行搜索双安全边缘点.游弋原则为:若传感器探测范围)cd部分被遮盖，则选取与障碍物交接点集{a，b}，将点集合{a，b}按照双安全边缘点处理方法处理得到方向集为{oa'，ob'}，并用启发式算法选取方向，即方向oa'，进行搜索双安全边缘，直到搜索到双安全双边缘为止;若传感器探测范围)cd全部被遮盖，则选取传感器端点集{c，d}，可行方向集为{oc，od}，并用启发式算法选取方向，即方向oc，进行搜索双安全边缘，直到搜索到双安全双边缘为止.

2 双安全边缘算法设计

2.1 局部双安全子目标的选取

选取局部子目标的目的是确定机器人当前状态下的安全运动方向和距离，本文确定子目标点的方法是跟踪双安全边缘点，采用启发式函数来完成局部子目标S的选择.单双安全边缘点情况下机器人对运行的方向和安全距离没有选择，局部子目标即为双安全边缘点，只需按照当前的安全距离和安全方向进行运动动作的规划.所以，启发式函数的应用，主要是针对多双安全边缘和无双安全边缘情况，根据从环境信息中提取的双安全边缘信息，定义启发式函数形式如式(1)所示:

其中:i∈{1，2，…，n}，j∈{1，2，…，m}.

为达到优化路径的目的，选取的子目标点必须要满足函数f(si)的最小化.其中λ(αj)为双安全边缘点阈值参数(开关函数).它是判断在当前环境信息下，机器人是否有足够的空间在两障碍物之间穿越，能穿越，其值为1，否则为0.g(si)为机器人在当前环境信息下可移动的安全距离函数.它是当前环境信息下备选安全双边缘线的长度，可以由机器人质点到安全边缘点的距离di(o-e)和机器人的自身安全圆半径rs求得.h(si)为机器人在当前环境信息下备选安全子目标点到目标点的距离，可以由备选子目标点和目标点的位置求得.通过启发式函数即可选择用于规划机器人运动动作的最优双安全边缘点，即为局部子目标点.

2.2 对新障碍物和动态障碍物的探测

根据双安全边缘的搜索原理可知，只需跟踪双安全边缘点数量和位置变化，就能准确地判断障碍物的出现或消失.当双安全边缘点数量和位置变化时，首先需要判断此障碍物是否是动态障碍物，即判断连续两时刻机器人同一位置双安全边缘是否变化，若无变化则为静态障碍物，按照规划好的路径运动;若有变化则为动态障碍物，需要预测双安全边缘的运动规律来近似动态障碍物的运动规律，并对机器人的运动方向和安全距离进行调整.

机器人通过跟踪双安全边缘点数量和位置变化探测到新障碍物后，需要在原地探测至少两个时刻{t1，t2}的传感器信息，并分别进行机器人双安全边缘的搜索，通过两时刻的双安全边缘信息确定机器人安全航向和安全航行距离.如图4所示，在t1时刻，移动机器人根据当前环境信息搜索

到的双安全边缘点为 a't1，安全航行距离为d(e-e)t1=mt1a't1，安全运动方向为∠st1=∠opt1;同理在t2时刻可搜索到安全双边缘点为a't2，安全航行距离为 d(e-e)t2=mt2a't2，安全运动方向为∠st2=∠opt2.于是可以估计出一侧双安全边缘点在t=t1-t2内的运动距离^se、速度^ve和运动方向∠^ve，分别为

同理可以估计出另一侧双安全边缘点的运动规律.运用这种方法可以对动态障碍物状态进行实时监控，从而提高对动态障碍物运动规律预测的精度.本文通过对双安全边缘的跟踪来近似动态障碍物的运动规律.在此基础上就可对移动机器人和动态障碍物在未来时刻的相对位置进行预测.

图4 探测新的障碍物和动态障碍物

2.3 对动态障碍物的躲避算法

如图4所示，机器人在当前环境信息下的安全航行距离和方向分别为d(e-e)t1和∠st1，动态障碍物(双安全边缘)的运动速度和方向分别为^ve和∠^ve，则可进行对动态障碍物运动规律的预测及指导机器人采取相应的躲避方法.令 lse= mt1a't1为机器人靠近动态障碍物一侧的安全边界向量，lve=a't1a't2为动态障碍物双安全边缘点运动向量，d⊥为两个向量的距离，如图4所示.

根据具体情况分为如下4种处理方法:

(a)若lse平行lve，且d⊥≥rs，则动态障碍物不会影响机器人当前的安全运动方向和安全运动距离.

(b)若lse平行lve，且d⊥＜rs，则根据机器人安全边界和双安全边缘点的相对速度计算是否能相碰.若能相碰，则需要预测双安全边缘点在未来时刻的位置，重新确定机器人运动的安全方向和安全距离.

(c)若lse不平行lve，且两向量预测不会相交，则动态障碍物不会影响机器人当前的安全运动方向和安全运动距离.

(d)若lse不平行lve，且两向量预测会相交，则预测交点时刻移动机器人能否通过交点，若能通过，则视为动态障碍物不会影响机器人当前的安全运动方向和安全运动距离.若不能通过，则根据动态障碍物两侧安全双边缘点的运动规律重新制定移动机器人的航向和安全距离.

3 算法流程

图5所示为移动机器人安全双边缘点路径规划算法决策树模型.可以看出算法针对动态不确定环境下静态和动态障碍物进行分类描述和处理，最终规划出每一时刻的机器人运动动作，其中原则1、原则2和原则3分别为第2.2、1.3和2.3叙述的原则，具体算法流程如下:

图5 DSE算法决策树

Step1 根据当前状态传感器信息按照原则1检测是否有新障碍物出现并判断障碍物是静态还是动态障碍物.若静态障碍物，转到step2;若动态障碍物，转到step3.

Step2 根据原则2判断双安全边缘点所属类型，转到step4.

Step3 根据原则3判断动态障碍物所属4种类型分别处理后，根据原则2判断双安全边缘点所属类型，转到step4.

Step4 3种双安全边缘点类型及规划方法:

1)按照单双安全边缘方法规划出安全运动方向和安全距离后，转到step5;

2)按照多双安全边缘方法规划出安全运动方向和安全距离后，转到step5;

3)按照无双安全边缘方法规划出安全运动方向和安全距离后，转到step5.

Step5.机器人按照step4规划的机器人运动动作运动，若子目标点是目标点，则规划结束，否则转到step1.

4 算法在不同环境下的仿真

图6是对本文提出的算法在U型(死区)环境信息和复杂静态环境下进行的仿真结果.仿真图中黑色圆点为机器人每一时刻所处位置，在每一时刻机器人通过双安全边缘算法规划当前时刻的安全方向和距离，圆点之间的连线即为机器人的实际路径.在图6(a)中，机器人首先进入小U型环境并摆脱U型环境，然后进入连续的狭长U型环境，最终到达目标点.图6(b)中，机器人处在复杂环境下，通过传感器探测到的局部环境信息最终到达目标点.

图6 静态环境下的仿真结果

表2是对本文提出的算法在含有动态障碍物的未知环境下进行的仿真结果，机器人在起始点到达目标点过程中，需要面临3个动态障碍物，它们的运动速度和方向分别为v1，v2，v3和相对地图向左，向右和向上.其中(1)～(3)分别是对3个不同相对位置的动态障碍物进行的避碰，(a)～(d)为机器人与动态障碍物之间4个相对位置状态.可以看到在(1)中，机器人在探测并且预测了动态障碍物的运动规律后，采取躲避策略成功避开动态障碍物;在(2)中，机器人在探测并且预测了动态障碍物的运动规律后，采取跟随策略，直到有新的空间出现，从而脱离动态障碍物;在(3)中，机器人在探测并且预测了动态障碍物的运动规律后，计算出机器人无需采取任何措施，直接到达目标点.从仿真结果可以看出，本文提出的动态不确定环境下的路径规划算法在复杂的静态和动态环境下都能够成功的完成规划任务，体现了此算法的强实时、强适应和较优化的特点.

表2 动态环境下的仿真结果

5 结论

1)本文提出的双安全边缘算法分析和处理传感器探测的有限实时环境信息，在保证机器人运动安全的前提下，搜索可用于规划机器人运动动作的双安全边缘点信息，简化了复杂的环境信息，降低了运算量，并结合了具有优化路径能力的启发式思想，增强了在动态不确定环境下的适应性和实时性.

2)仿真证实了本文算法的可行性.该方法中局部子目标的选取，也为同步地图创建和定位中机器人运动方式提供了新的方案.

［1］AZARM K，SCHMIDT G.Integrated mobile robot motion planning and execution in changing indoor environment［C］//IEEE International conference on Intelligent Robot and System.Munich:IEEE，1994:298-305.

［2］WILLMS A R，YANG S X.An efficient dynamic system for real-time robot-path planning［J］.IEEE Transactions on System，Man and Cybernetics，Part B(Cybernetics)，2006，36(4):755-766.

［3］FEITEN W，BAUER R，LAWITZKY G.Robust obstacle avoidance in unknown and crmped environment［C］//IEEE Int Conference on Robotics and Automation.San Diego:IEEE，1994:2412-2417.

［4］FOX D，BURGARD W.The dynamic window approach to collision avoidance［J］.IEEE Robotics＆Automation Magazine，1997，4(1):23-33.

［5］WU L，HORI Y.Real-time collision-free path planning for robot manipulator based on octree model［C］//9thIEEE International Worshop on Advanced Motion Control.Istanbul:IEEE，2006:284-288.

［6］KOENING S，LIHACHEV M.Fast replanning for navigation in unknown terrain［J］.IEEE Transactions on Robotics，2005，21(3):354-363.

［7］HAMMOURI O M，MUSTAFA M.Voronoi Path Planning Technique for Recovering Communication in UAVs［C］// IEEE/ACS International Conference in Computer Systems and Applications.Doha:IEEE，2008:403-406.

［8］JUAN A F，JAVIER G.Hierarchical Graph Search for Mobile Robot Path Planning［C］//Proceeding of the 1998 IEEE International Conference on Robotics＆Automation.Leuven:IEEE，1998:656-661.

［9］CHA Y Y.Navigation of a free-ranging mobile robotusing heuristic local path-planning algorithm［J］.Robotics and Computer-Integrated Manufacturing，1997，13 (2):145-156.