动态环境下矢量化路径规划算法＊

徐啟蕾，唐功友，盖绍婷，杨 雪

（1.中国海洋大学信息科学与工程学院，山东 青岛266100；2.青岛科技大学自动化与电子工程学院，山东 青岛266042）

路径规划问题是机器人自主导航研究的一个重要问题。机器人从初始位置到目标位置的路径规划要求实现：（1）具有可达性和无碰撞；（2）规划的线路应满足某种指标的最优（如时间最优、能耗最优等）。

针对完全未知环境下路径规划，常用方法有人工势场法[1－3]、模糊逻辑算法[4－5]、BUG系列算法[6－10]等。人工势场法结构简单，便于控制，能得到平滑的路径轨迹，多种研究方法可避免其陷入局部最优，但该类方法对狭窄空间的路径规划及不规则障碍物的处理方面还有待研究。模糊逻辑算法模拟生物的避障思想，对机器人定位精度、环境检测精度不敏感，具有较好的实时性和稳定性，但规划结果、运算量受模糊隶属函数和模糊控制规则的影响十分严重。Bug系列算法中，机器人利用触觉或视觉传感器数据，按朝向目标前进、沿障碍物边界绕行障碍物和停止3种运动状态行进，此类算法简单，但在选取最优障碍物绕行方向时运算量较大，且所有的Bug系列算法均未考虑机器人行进的安全半径，仅是贴合障碍物边缘行进。

对于移动障碍物，文献[11]提出轨道变形算法，根据移动障碍物信息实时修改已知环境下的预定路线，当移动障碍超过算法约束时，则需要由1个带时间约束的实时动态规划器[12]重新规划路径。此方法可以解决已知环境下的移动障碍物，但对未知的动态环境无能为力。

本文针对完全未知动态环境，提出了矢量化路径描述方法，并将此方法与Bug系列算法思想相结合，研究全方位移动式机器人的运动规划及其避障策略。机器人根据其感知的局部信息独立规划并更新其路径，实现可达、无碰撞的路径规划。起初根据初始位置和目标位置生成初始路径，路径以矢量形式进行描述及保存。机器人根据规划路径进行移动，先根据路径转过相应的角度，其次沿直线向前运动至路径规定距离，然后依规划路径进行下一轮角度旋转和直线运动。运动过程中以一定的间隔对环境信息采样、分析，不断根据局部障碍物的位置及其移动速度和方向信息，在一定范围内产生路径中间点，根据设定的规则选择可行的中间点，并将中间点插入原有路径以实现路径实时更新，达到避障的目的。提出的矢量化路径描述方法是以一组首尾相连的矢量来表示路径，通过该种方式来记录和保存路径可降低路径存储的空间需求，并在一定程度上简化对机器人的控制。路径规划过程中，考虑到机器人行进过程中与障碍物间的安全距离，增强了算法的实用性和安全性。

1 问题描述

1.1 规划问题描述

将全方位移动式机器人简化为1个质点，考虑其在二维状态空间R2内从初始位置qstart到目标位置qgoal的运动规划问题。

假设机器人配备的测距设备可扫描机器人前方360°的以最大测距Rsensor为半径的圆形范围。在行进过程中机器人每走过一定距离就会对周围环境进行检测，且在前一段路径结束，转向进入下一段路径后必须立即进行环境检测。在检测到障碍物时，机器人将在一定时间间隔tsensor内停止运动，并不断检测障碍物，以计算出障碍物的移动速度及方向。假设障碍物沿直线匀速移动，且障碍物的移动速度vo满足，n≥5，以保证在机器人停驻检测障碍物时不会发生碰撞，且机器人有足够时间和距离绕过障碍物。然后综合当前行进路径向量以及检测到的障碍物所在方位及其移动方向和速度来判断该障碍物是否会阻挡当前行进路线。一旦发现该障碍物阻挡当前路径，则计算中间插入点，并更新规划路径以实现避障。

1.2 矢量化路径描述

本文提出以一组有序的矢量来表示任意路径规划结果，该方法称为矢量化路径描述方法。

令Pi表示任意第i次规划的路径，且有

式（1）中各个向量依次首尾相接，其中αik为构成路径Pi的第k个向量，αik以向量αi（k－1）的终点为起点，记作αik＝ （θik，‖αik‖ ），式（1）中θik是向量αik与向量αi（k－1）的夹角，取顺时针方向为正，逆时针方向为负，－π≤θik≤π，‖αik‖为向量的模，即机器人在转动θik方向后应行进的距离。

机器人首先根据初始位置和目标位置创建初始路径，然后按照路径行进，并在行进过程中，不断检测环境中障碍物信息，并根据检测到的障碍物信息和中间点生成法则来生成中间点，最后插入中间点、实时更新路径，从而实时规划出无碰撞路径。其初始路径P0由以起始位置为起点、目标位置为终点构成的向量组成，有

其中：以机器人的初始方向为基准，转向目标位置所转过的角度记为θ01，起始位置到目标位置间的直线距离记为‖α01‖。

机器人沿初始路径开始移动，并不断对周边环境进行检测，一旦发现有障碍物，则需要增加路径中间点以更新规划路径，从而实现避障。

图1给出了机器人按照矢量化描述的路径移动、并插入中间点更新路径的过程。机器人首先按照路径矢量组Pi行进，在沿αik运动的途中，检测到障碍物Obstacle阻挡路线，则在路径矢量组Pi上添加点qinsert（i＋1）得到更新路线后的路线矢量组Pi＋1，则有

因α（i＋1）n是机器人在αin上行进过的路线，所以θ（i＋1）n＝θin，‖α（i＋1）n‖和‖αin‖－‖α（i＋1）n‖已知。又根据规则1可知‖α（i＋1）（n＋1）‖和θ（i＋1）（n＋1），则由余弦定理可得

2 规划算法

2.1 障碍物检测

机器人按规划路径行进过程中，需要不断根据检测到的环境信息，判断前方是否有障碍物阻挡或将要阻挡当前路径。图2给出了障碍物检测的4种情况。图中浅色矩形为第一次检测到障碍物时障碍物所在位置；深色矩形为经过tsensor时间后，障碍物所在位置；q表示机器人当前所在位置；大圆形表示检测范围Rsensor；小圆形表示安全距离Rsafe；pi表示检测到的障碍物点，p1是第一次检测到的障碍物点，p2和p3分别是tsensor时间后检测到的左右2个边缘的障碍物点；φi表示当前路径间与q到pi之间的连线的夹角，以当前路径方向为起始方向，逆时针方向为正，顺时针方向为负。

情况（a）时，经过时间tsensor后，无法检测到障碍物，此时判定该障碍物无法阻挡当前路径，机器人仍沿原来路径行进。

情况（b），（c）和（d）时，须按下列不等式组进行判断：

若满足不等式组（9），则认为此障碍物正向远离障碍物的方向移动，机器人可沿原路径行进；否则，则认为障碍物正朝向阻挡路径方向移动，或保持不动，需计算中间插入点更新路径。

图2 障碍物检测示意图Fig.2 Obstacle detection

2.2 插入点生成

当检测到有障碍物阻挡当前路径时，首先计算出最大插入点距离dmax，然后按照本文提出的下列规则计算中间插入点。

规则1 取tsensor时间后检测到的机器人与障碍物间最长距离为dmax，Rsafe≤dmax≤Rsensor，以dmax为向量长度，机器人当前位置为向量起始点，以机器人当前行进方向为基准，按角度θ做向量，若该向量无障碍阻挡，则将该向量添加到新的路径中，且该向量的终点为插入的中间点；若有障碍物阻挡，则取θ＝θ＋Δθ，Δθ为规定的角度值，重复上述步骤做向量；若直至θ＝2π时仍未有向量满足上述要求，则取dmax＝α·dmax，0＜α＜1，重新以角度θ做向量，重复以上过程，其中

2.3 更新路径

当计算出中间插入点之后，需要根据1.2章节中给出的路径更新计算方法，计算出更新后的路径，然后令机器人按照更新后的路径继续行进。

3 仿真实验及分析

为验证算法，在配置为i5－M430（2.27GHz）、2G内存的笔记本电脑上以C＃语言编写仿真软件进行实验。图4是对不同地图进行仿真所得到的结果。每幅地图大小均为1 024×768像素，以1像素为1长度单位，模拟未知的实际环境。初始位置到目标位置的距离称为初始距离。取机器人测距范围Rsensor＝300，安全半径Rsafe＝20。图中“●”是初始位置，“█”是目标位置，“□”表示检测到障碍物，原地等待tsensor的位置。实线是机器人的行进路径。图中最浅色实体为第一次检测到障碍物时障碍物所在位置，由浅入深的几个实体依次分别为机器人等待tsensor时间后检测到的障碍物所在位置。为了使图看起来更加清晰、简洁，对于那些已经被绕行过、不会对当前行进路径造成阻挡、或已离开移动智能体感知范围的障碍物，图中将不再加以显示。

图3 不同地图仿真结果Fig.3 Simulation results of different maps

从图3的仿真结果中可以看出，本文提出的方法可以预测出移动障碍物的运动趋势，实现提前根据其运动方向选择是否绕行障碍物、或在绕行障碍物时选择与障碍物运动方向相反的方向绕行，最终使机器人自主地从初始位置安全、无碰撞的到达目标位置。

表1给出了实验数据。假设机器人移动及距离检测的时间为零，表中总耗时仅为计算中间点及规划路径的耗用时间。结果证明本算法实时性可满足要求。

表1 不同地图规划结果对比Table 1 Result data of different maps

4 结语

本文提出了矢量化路径描述方法，以一组有序的首尾相接的矢量对路径进行描述与存储，该方式易于实现路径的执行，并可降低对路径存储空间的要求，尤其适合大范围、复杂动态环境下机器人运动路径规划。文中将矢量化路径描述方法与Bug思想相结合，解决未知动态环境下机器人实时在线路径规划问题。规划过程中充分考虑了机器人与障碍物间的安全距离，提高了算法的实用性及安全性。提出的中间点选择策略只需进行少量简单计算，有利于机器人实时根据障碍物移动信息更新路径。经仿真试验证明，本文提出的方法可实现完全未知动态环境下机器人实时路径规划。

[1] Masoud Ahmad A.Decentralized self－organizing potential fieldbased control for individually motivated mobile agents in a cluttered environment：A vector－harmonic potential field approach[J].IEEE Trans on Systems，Man，and Cybernetics（TSMC），2007，37（3）：372－390.

[2]Mabrouk M H，McInnes C R.Solving the potential field local minimum problem using internal agent states[J].Robotics and Autonomous Systems，2008，56（12）：1050－1060.

[3]Zhang Tao，Zhu Yi，Song Jingyan.Real－time motion planning for mobile robots by means of artificial potential field method in unknown environment[J].Industrial Robot：An International Journal，2010，37（4）：384－400.

[4]Tarokh Mahmoud.Hybrid intelligent path planning for articulated rovers in rough terrain [J].Fuzzy Sets and Systems，2008，159（21）：2927－2937.

[5]Min Byung Cheol，Lee Moon－Su，Kim Donghan.Fuzzy logic path planner and motion controller by evolutionary programming for mobile robots[J].International Journal of Fuzzy System，2009，11（3）：154－163.

[6]Lumelsky V J，Stepanov A A.Path－Planning strategies for a point mobile automaton moving amidst unknown obstacles for arbitrary shape[J].Algorithnica，1987，2（3）：403－430.

[7]Lumelsky V J，Mukhopadhyay S，Sun K.Dynamic path planning in sensor－based terrain acquisition [J].IEEE Trans on Robotics and Automation，1990，6（4）：462－472.

[8]Kamon I，Rimon E，Rivlin E.TangentBug：A range－sensor－based navigation algorithm [J].The Inteernational Journal of Robotics Research，1998，17（9）：934－953.

[9]Buniamin N，Wan N W A J，Sariff N，et al.A simple local path planning algorithm for autonomous mobile robots [J].International Journal of Systems Applications，Engineering and Development，2011，5（2）：151－159.

[10]Guruprasad K R.Egressbug：a real time path planning algorithm for a mobile robot in an unknown environment[J].Lecture Notes in Computer Science，2012，7135：228－236.

[11]Delsart V，Fraichard T.Navigating Dynamic Environments Using Trajectory Deformation[C].France：IEEE－RSJ Int Conf on Intelligent Robots and Systems，2008：226－233.

[12]Delsart V，Fraichard T，Martinez L.Real－time Trajectory Generation for Car－like Vehicles Navigating Dynamic Environments[C].Kobe：IEEE Int Conf on Robotics and Automation，2009：3401－3406.