一种全局与局部融合的动态路径规划方法

张洁，张林

（商洛学院数学与计算机应用学院，陕西商洛 726000）

机器人移动过程中自主导航和避障应用研究的一个关键问题是如何提高移动机器人自主学习的能力，使其能够在未知环境下通过自主学习从起点移动到达指定目标点而不发生碰撞。

文献[1]针对多台移动式起重机缆索并联机器人的定位、避障规划与控制问题，采用三维网格图方法构建环境图模型，并将相对定位与绝对定位方法相结合，提出了一种基于网格的人工势场协同定位方法，对CPRMCS系统进行了全局路径规划。文献[2]针对非完整约束轮式机器人的非线性跟踪与避障控制问题，引入了扩展状态观测器估计轮式移动机器人的未知扰动和速度信息，并为复杂环境下的目标跟踪和避障，设计了一种非线性控制器，使机器人在障碍物检测区域内有效避障。文献[3]针对移动机器人路径规划优化的问题，给出一个基于布谷鸟搜索算法设计的一种新的元启发式方法，该方法在机器人与目标和障碍物之间建立了满足机器人避障条件的函数关系，可用于移动机器人在未知或部分已知环境下的路径规划。此外，常见的针对机器人路径规划的方法还包括基于生物启发式[4]的遗传算法、蚁群算法及细菌势场方法，基于模型的动态窗口法和人工势场法，基于节点的Dijkstra算法、A*算法及D*算法，基于采样的快速搜索随机树方法和Voronoi图方法[5]。全局路径规划解决了机器人要去哪里的问题。定位完成后，机器人将自己的位置[6]设为起点，目标点设为终点。从起点到终点的全局路径规划过程中有三个基本要求：一是准确到达目标点；二是航行过程中不能与障碍物发生碰撞[7]；三是选择最快到达目的地的路径。传统的Dijkstra算法虽然是最短路径算法，但属于贪心算法，需要遍历每个连接节点。节点越多，耗时越长。

本文将Dijkstra算法和动态窗口方法相结合，提出一种新的机器人混合路径规划方法。

1 算法原理

Dijkstra算法使用了贪心算法模式求解最短路径。该算法解决关于有向图中单个源点到其他节点的最短路径问题[8]，其主要思路是每次迭代时选择的下一个节点是标记点之外距离源点最近的节点。在机器人导航领域，这个算法将一个节点固定为起始点，从而寻找到目标点的最佳路径。

解决机器人导航过程中如何到达目的地的问题用全局路径规划，最优路径可以由机器人根据全局地图计算出，但很多不确定因素在实际移动过程中存在。如全局地图与局部实际情况不符，某个地方多出行人或障碍物等情况[9]。局部路径规划非常重要，否则如果只按照全局地图执行导航任务，如同盲人过马路一样危险。局部路径规划主要是对局部未知障碍物进行避开障碍，避障后返回已计算的全局路径的值。

本算法的核心思想是将基于局部路径规划的动态窗口法和最短路径算法的全局规划算法结合起来使用，算法首先根据全局规划方法规划一个最短行驶路径，在机器人移动过程中使用动态窗口法不断进行局部规划，并根据局部规划和全局规划后的最优解进行实际的路径规划。

1.1 算法1（全局规划算法）

输入：起始点pt

输出：最短路径dj

假设算法路径网中每个节点都有标号（dt,Pt），dt是从出发点s到点t的最短路径长度；Pt表示从s到t的最短路径中t点的前一个点。求解从出发点s到点t的最短的路径算法的基本过程为：

第一步，初始化。出发点设置为Ps=null ds=0；所有其他点di=∞，pi未定义标记起始点s，记k=s，其他所有点设为未标记。

第二步，检验从所有已标记的点k到其他的直接连接的未标记的点j的距离，并设置：

其中，表示 w（k，j）从 k到 j的路径长度。

第三步，选取下一个节点。从所有未标记的点中选取路径最小的点i，点i被选为最短路径中的一点，并设为已标记的点。

第四步，找到点i的前一节点。从已经标记的点集合中找到直接连接到点i的节点，并标记。

标记点i。若所有的节点已标记，则算法结束。否则，记k=i，转到第二步继续。

1.2 算法2（局部规划算法）

动态窗口法（DWA方法）是基于机器人运动学和动力学[10]的局部路径规划算法，核心思想是将路径规划问题转化成速度矢量空间上的约束优化问题。本算法中输入参数包括：当前状态、模型参数、目标点、评价函数的参数、障碍物位置和障碍物半径。输出参数为：控制量和轨迹集合。由于机器人运动可以分解为直线速度ν和旋转速度 ω，所以 Vr(ν,ω)集合便是机器人的速度矢量空间，在此空间中实现对机器人运动控制的命令搜索。速度矢量空间Vr由速度矢量集合Vs、速度动态窗口Va和许可速度矢量Vd的交集构成。

式(2)中的Vd是许可速度，表示机器人的加速度能力不导致碰撞的速度，可表示为：

式(3)中，distant(ν,ω)表示机器人障碍物的最小距离，aν，aω为机器人平移的加速度和旋转加速度。

为了从速度矢量空间中得到机器人在k+1时刻的最优值，需要提前设定DWA的标准目标函数：

heading(ν,ω)函数可以对机器人 k+1 时刻朝向目标点的程度作出评估，当机器人与目标点的夹角为0°时，此函数取最大值：

distan(ν,ω)函数可以对机器人 k+1 时刻自身位置和目标点的距离作出评估，函数值越大说明与目标点距离越大，发生碰撞的几率越小；函数值越小说明离障碍物越近，发生碰撞几率越大。distant(ν,ω)定义如式(6)所示：

νel(ν,ω)函数是机器人速度的大小，其定义如式(7)所示：

其中ν是机器人平均速度，νmax是在νr机器人的最大速度。

1.3 算法3（混合规划算法）

本算法的输入参数分别为算法1和算法2的输出参数。输出参数是优化过的运动方向和时速。算法首先会每隔一个时间间隔检测局部规划算法中计算出的局部移动速度和方向是否和全局的一致，如果一致算法会休眠一个时间周期后重新获取新的输入参数。如果不一致，就会计算全局路径中可达的抽样节点到当前位置的距离dn及抽样节点到目标节点的距离dj。然后计算dn与dj相加后的计算值，选择计算值最小的一条路径继续前进，一个时间周期后重新再检测输入参数直到到达目的地为止。具体流程如图1所示。

图1 算法3的计算流程

2 仿真实验与结果分析

2.1 实验环境

仿真平台使用matlab进行验证，机器人状态包括坐标（x,y）、航向角、速度、角速度。机器人初始位置设为（0,0），目标点位置为（10,10），共设置 10个障碍物，坐标点分别为（0,2），（2,4），（2,5），（4,2），（5,4），（5,6），（5,9），（8,8），（8,9），（7,9），用于冲突判定的障碍物半径R=0.5，模拟区域范围为[-1 11，-1 11]。评价函数参数[11]分别由航向得分比重、距离得分比重、速度得分比重、向前模拟轨迹的时间衡量，模拟实验结果包括走过的轨迹及导航时间。

DWA输入参数有返回控制量及轨迹，机器人移动到下一个时刻的状态量根据当前速度和角速度推导[12]。由坐标上两点之间的距离确定机器人是否到达目标点，记录机器人走过的所有位置坐标，作为机器人移动的历史数据。

将本文提出的融合方法与DWA方法进行实验比对。

2.2 实验结果分析

实验结果表明，在保证其他约束条件一致的情况下，两种算法都能成功实现移动机器人由起始点至目标点的导航，且图2机器人起始状态、图3中间状态和图4终止状态图表明，两种算法的移动轨迹基本一致，但根据表1和表2的导航用时数据，两种算法的导航用时有所不同。在保持障碍物数量及位置不变及障碍物半径相同的条件下，8次实验中融合算法的导航用时比只使用DWA算法的导航用时平均减少了5.467 s，减少比例约为12.3%。

图2 机器人初始状态

图3 机器人中间状态

图4 机器人终止状态

表1 机器人8次导航用时

表2 机器人8次导航平均用时

3 结论

针对机器人在移动过程中的路径规划的问题，本文给出了一种融合Dijkstra方法和动态窗口法的混合路径规划方法。基于动态窗口算法，构造了一种考虑全局最优路径的评价函数，在保证路径规划的全局最优性的同时，可避免动态窗口法陷入局部最优的问题。相比较传统Dijkstra算法，本文提出的融合路径规划的方法可实现机器人在实时避开障碍的同时更加节省时间。但因为实验中障碍物分布不复杂，地图较为简单，以及较为理想的条件设置的障碍物半径都相同，所以本文给出的方法在实际应用中还可进一步的扩展优化。