融合动态障碍物运动信息的路径规划算法

封 硕，吉现友，程 博，韩启东，于少伟

1.长安大学 工程机械学院，西安 710064

2.长安大学 运输工程学院，西安 710064

移动机器人路径规划的相关工作种类繁多，具体可分为两大类，全局路径规划和局部路径规划。全局路径规划算法中，A*算法因其快速的计算速度广泛应用于机器人的路径划化问题。对于路径不发生变化且路径中不存在动态障碍物的情况，机器人只需严格遵循全局规划的路径即可。然而，当环境是动态的且随时可能发生变化时需要借助局部路径规划，以保证机器人及时避开障碍物。其中，局部路径规划算法主要有人工势场法、动态窗口法等。动态窗口法通过在速度空间中采样多组速度，并模拟在这些速度组下机器人在一定时间内的移动轨迹，通过评价函数对这些轨迹进行打分，从而选择出最优轨迹的速度组，驱使机器人移动。由于动态窗口法可以直接获得机器人在接下来一定时间内的速度信息[1]。因此，可以通过获取动态障碍物的运动信息，并且与机器人的预测信息进行融合判断，从而实现机器人在运动时及时避开障碍物，避免发生碰撞。

对于A*算法的改进，文献[2-6]分别从不同角度对传统A*算法进行了优化处理，主要成果可归类为从问题的各个方向上减少了传统贪心算法带来的路径转折点过多、转折角度过大等问题。国内路径研究人员针对障碍物位置不发生变化时的路径规划算法做出了大量研究，但是对于复杂动态环境下的机器人路径规划与导航研究比较少，比如：文献[7]针对机器人在不同类型障碍物环境下的路径规划问题，提出基于神经网络的改进粒子群优化算法，可以应用于静态和动态障碍物环境，但是该方法需要对神经网络进行初始化，无法做到及时处理。文献[8]针对人工势场法在路径规划中出现的目标点不可达、转折次数多及路线较长的问题，提出了一种动态环境下移动机器人全局路径规划的改进A*势场算法。文献[9]针对在局部动态路径规划中人工势场法出现的局部最小值和目标不可达等问题，提出一种改进的人工势场法。文献[10]提出了一种适用于局部动态环境的路径规划的多重A*算法，并结合反应性和慎思型避障控制策略的优点进行实时避障。文献[11]对现有路径规划算法进行了分类，并根据每种算法的特点将不同算法相互结合运用，以解决动态环境下机器人的路径规划问题。文献[12]为了实现AGV 实时动态避障，将A*和DWA 两种算法融合，进行在线实时规划路径，设计了一种基于全局最优路径的圆滑路径曲线。经过仿真，提出的算法在路径长度、机器人平均转折角度、运行时间等都大大减少。文献[13]针对动态环境下UGV 的正常工作，提出了动态障碍物窗口法和动态障碍物树窗口法，确保了UGV 在工作中可以安全的避开障碍物和环境安全。为了解决利用动态窗口法进行机器人避障时可能会撞到一些机器人轨迹附近，但不在轨迹上的障碍物的问题，文献[14]提出了一种区域动态窗口法，通过对传统动态窗口法的评价函数进行修改，最终得到一个可以安全避开轨迹附近障碍物的算法。为了提高机器人运动的智能性，文献[15-16]提出了一种行人特征模型以识别机器人导航中行人的特征，从而及时避免与行人发生碰撞。

上述文献对传统A*算法和动态窗口法从不同角度进行了优化，但是未考虑机器人在复杂多变的、高度动态的环境下机器人的路径规划问题。针对以上问题，提出了一种融合机器人与障碍物运动信息的改进动态窗口法来解决机器人在动态环境下的局部路径规划问题，并且与优化A*算法相结合来实现全局最优路径规划。通过实验验证，该算法与传统的动态窗口法相比，具有更好的动态避障能力、路径更加符合避让规则，同时避免了陷入局部最优解的问题。

1 优化A＊算法

1.1 传统A＊算法

A*算法是一种快速、有效求解最短路径的启发式搜索算法。其主要思想为，从起点开始对周围的八个点进行搜索，计算周围点到当前点的实际代价值以及周围点到目标点的估计代价值，通过选择总代价值最小的点作为下一个节点，直到终点。其代价函数如下：

式中，F(n)是节点n的代价函数，G(n)是在状态空间中从初始节点到第n个节点的实际代价，H(n)是从节点n到目标节点的估计代价。其中H(n)可分为有曼哈顿距离、欧氏距离和切比雪夫距离，本文算法重点比较所规划路径的长度，故采取欧氏距离度量节点之间的代价，欧氏距离计算公式如下：

式中，(xs,ys)为当前点的位置坐标，(xg,yg)为目标点的位置坐标。

1.2 优化A＊算法

传统的A*算法只能对当前点周围的八个点进行搜索，从而导致利用A*算法求解的路径中存在大量的冗余转折点。为了解决该问题，本文提出一种转折点剔除算法，将传统A*算法求解的路径冗余点剔除，从而得到一条长度较短、更平滑的最优路径（如图1）。

图1中红色曲线为优化后的路径，蓝色表示传统A*算法路径，黑色栅格表示障碍物。

其优化的具体步骤如下：

（1）获取传统A*算法求解的全局路径点集合P。用Pi表示第i个路径点。新建一个只包含起始点和终点的集合Pn。

（2）令i=1，m=2。判断第i个路径点Pi与Pi+m路径点之间是否存在障碍物。

（3）若存在障碍物，则将路径点Pi加入到Pn集合，令i=i+1。继续判断第i个路径点Pi与Pi+m路径点之间是否存在障碍物。若存在则返回第（3）步，若不存在则返回第（4）步。

（4）若不存在，则剔除Pi与Pi+m中间的路径点，令m=m+1。判断第i个路径点Pi与Pi+m路径点之间是否存在障碍物。若存在则返回第（3）步，若不存在则返回第（4）步。直到终点。

（5）依次连接集合Pn中的路径点，得到一条剔除冗余转折点的最优路径。

2 优化动态窗口法

2.1 传统动态窗口法

动态窗口法是机器人局部路径规划中常用的一种路径算法。该算法采样多组速度（包括：线速度、加速度、角速度、角加速度），并模拟出这些速度下机器人下一段时间内的位置移动轨迹。然后，通过评价函数对这些轨迹进行评价处理，选择最优速度组，驱使机器人移动。传统动态窗口法的评价函数主要分为三部分，具体表达式如下：

其中，heading(v,w)是方位角评价函数，用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差距。velocity(v,w)是速度评价函数，用来评价当前轨迹的速度大小。dist(v,w)为距离评价函数，表示机器人在当前轨迹上与最近的障碍物之间的距离。如果在这条轨迹上没有障碍物，则设定为一个常数。α、β、γ为权重参数，ρ为归一化参数。

在传统动态窗口法中主要存在以下缺陷：

（1）由于缺少全局规划，机器人一旦面临“L”型、“C”型等半封闭障碍物区间时，会导致评价函数失灵，无法选择正确路径。

（2）在障碍物发生移动的动态环境下，机器人只有在距离障碍物很近时才能触发判断函数，但是由于机器人与障碍物皆在移动，处于评价函数的机制问题，机器人无法做到安全、及时的避障。

针对动态窗口法在复杂动态环境中存在的两大问题，提出了一种融合障碍物运动信息的改进算法。

2.2 优化动态窗口法

2.2.1 优化动态避障功能

传统的动态窗口法在障碍物固定不动的环境下具有很好的鲁棒性，可实现机器人的局部避障。但是当环境中存在高度动态的障碍物时，传统的动态窗口法将不在具备可信度，无法做到及时避障、且容易陷入局部最优解。但是，传统动态窗口法的评价函数结构清晰，具有很好的可扩展性。因此本文通过对传统评价函数进行扩展，在原来的基础上新增一项可动态避障的评价函数(dym_dist(v,w))。

通过对dym_dist(v,w)函数的定义，使得机器人在感知到障碍物时可以根据速度差和距离大小的综合判断来选择下一时刻的速度组。经扩展后的优化动态窗口法的评价函数如式（5）所示：

式中，δ为权重参数。

对于动态环境下的移动障碍物，由于不同的障碍物其速度不同，因此其单位时间内可运动的距离也有所不同，为了实现更加智能的避障，将不同速度的障碍物赋予其不同大小的危险区域供机器人参考（如图2）。速度大的障碍物，其在单位时间内的运动距离大，从而危险区域更大，反之则更小。

对于图2 中的安全行驶决策，在机器人行驶过程中，原则上不应该影响到动态障碍物（行人、车辆等动态因素）的移动，要自主的根据障碍物移动信息进行预测、判断，选择路径。

主要算法流程步骤为：

步骤1 根据机器人预测轨迹与障碍物危险区域，判断两者是否即将相遇。如果是，则进行步骤2。如果没有，则按照传统动态窗口法运动。

步骤2 计算每一个速度组下机器人的预测位置，将预测位置触及障碍物危险区域的速度组抛弃。

步骤3 对剩余的速度组进行安全驶离判断。计算机器人当前移动方向与障碍物到机器人连线方向的叉乘在坐标轴Z轴方向的正负。如果为负，则机器人应该往左行驶，反之则向右行驶。

步骤4 根据机器人的移动方向，给定新增评价函数的权重参数。对于符合安全驶离移动方向的预测轨迹，dym_dist(v,w)取正值，反之，取负值。

步骤5 计算所有轨迹的评分，选择最大评分轨迹驱使机器人移动。

其中，步骤3的具体算法如图2所示，假设机器人预测轨迹已经进入障碍物危险区域，此时需要判断机器人如何行驶。用W表示Pnr与Pr的叉乘在坐标轴z方向的正负（式（6）），如果为负则机器人向左行驶（图2左图），反之向右行驶（图2右图）。

式中，Pr表示机器人速度方向、Po表示障碍物速度方向、Pnr表示障碍物与机器人连线方向。

图2 中，灰色圆形表示机器人，红色圆形表示障碍物，绿色圆形表示危险区域，红色表示障碍物运动方向，蓝色表示机器人运动方向，黑色表示障碍物与机器人连线方向。

2.2.2 优化局部最优缺陷

针对动态窗口法易陷入局部最优和成功率低等缺点，将动态窗口法与优化A*算法做融合，进一步优化动态窗口法的评价函数，从而继续对路径进行局部优化。按照各个路径点之间的相对距离大小，对A*算法计算出的路径点进行插值，使得只有I个路径点的原始路径增加到J(J＞I)。然后，设置一个阈值M，计算机器人起始位置到所有路径点的距离，找到距离大于M的最近路径点，设置为临时目标点。同时，从路径点集合中剔除距离小于M的路径点。执行优化DWA 算法，计算机器人当前位置到剩余路径点的距离，找到距离大于M的最近路径点，替换原来的临时目标点，直到终点。

式中，Heading( )v,w为机器人与临时目标点之间的方位角函数，用来评价机器人在当前设定的采样速度下，达到模拟轨迹末端时的朝向和临时目标之间的角度差。

最终得到机器人在复杂动态环境下的基于全局最优路径的动态避障路径规划的流程图（如图3所示）。

3 实验分析

为了验证算法的可行性，在MATLAB 中对该算法进行实验验证。采用栅格法将环境划分为24×24 的栅格地图，其中黑色表示障碍物，白色表示非障碍物。实验中用到的机器人速度信息如表1 所示。算法参数如表2、表3所示。

表1 移动机器人机械特性Table 1 Mechanical characteristic of mobile robot

表2 动态窗口法的基本参数Table 2 Basic parameters of dynamic window approach

表3 评价函数参数Table 3 Evaluation function parameter

以（4，4）为起点、（20，18）为终点，分别采用传统A*算法和优化A*算法进行路径规划，如图4所示，其中红色曲线表示传统A*算法，绿色曲线表示优化A*算法。从图4中可以得到，传统A*算法的转折点有9个，路径长度为22.728 m。优化A*算法仅有5个，路径长度为21.367 m，相较于传统算法优化后的路径在转折点上减少了4个，路径长度减少了1.361（5.99%）m，且路径更加平滑。

如图5所示，以（4，4）为起点，（12，12）为终点，在其间加入L 型障碍物，通过传统动态窗口法进行路径规划。从图5 可以得到传统的动态窗口法在进行路径规划时容易陷入局部最优解，特别是当导航路线中存在U、L型等凹形障碍物时尤为明显。同时，对于环境中存在动态障碍物时，由于障碍物在实时移动，仅通过传统动态窗口法进行路径规划容易出现碰撞等危险，因为只有当障碍物距离机器人很近时才会进行避障。图6 为仅采用传统动态窗口法面对动态障碍物时的路径规划路线，由图可知当机器人开始准备避障时，已经与障碍物发生碰撞。

对于存在动态障碍物的环境，将优化A*算法与优化动态窗口法进行融合，利用优化A*算法得到的全局最优路径作为动态窗口法的临时目标路径指示，如图7所示，红色曲线表示优化A*算法求解的全局最优路径，红色圆形表示动态障碍物，绿色圆形表示危险区域，蓝色箭头表示障碍物移动方向。优化动态窗口法以临时目标点（temp goal）作为行驶目标，驱使机器人尽可能地按照全局最优路径行驶，避免陷入局部最优。由图8、9、10、11、12可知，机器人在预测轨迹触及障碍物危险区域时开始执行远离决策，通过预先设置的评价函数，将机器人速度信息与障碍物速度信息进行分析，从而决定机器人在下一时刻的运动状态，安全规避动态障碍物。其中，从图8、10可知，当机器人遇到动态障碍物时，机器人一致性的向左行驶，从而在不阻碍障碍物运动的条件下安全的避开障碍物，同时尽可能地按照全局最优路径进行行驶，最终得到一条既考虑全局最优路径也考虑动态障碍物的实时避障路径（如图12）。图13为机器人在面临动态障碍物时的速度变化图。从图13中可知，每当新的障碍物出现在预测范围内时，机器人速度会适当降低，但是在确定正确的行驶方向后又迅速回到最大值，以保证机器人的运动速度。

针对优化的动态窗口法和传统动态窗口法，进行多次仿真实验，得到其碰撞次数、行驶时间、路径长度、平均速度的对比表。由表4可知，优化算法的碰撞次数降低了95%，行驶时间降低了46%，路径长度降低了37%，平均速度提高了13%。总的来说，优化算法可以大幅度减少与动态障碍物的碰撞次数，同时提升机器人运动速度、减少路径长度。

表4 优化算法的性能提升Table 4 Performance improvement of optimization algorithm单位：%

4 结束语

针对传统动态窗口法在面对动态障碍物时容易出现碰撞，环境复杂时容易陷入局部最优等问题，以及传统A*算法在路径上存在冗余转折点的问题，提出了一种融合动态障碍物运动信息的路径规划算法。通过与传统A*算法作比较可以得到，剔除冗余转折点后的优化算法在路径上更加平滑、长度更短。与传统动态窗口法做比较可以得到，优化后的算法在面对动态障碍物时，通过采用快速驶离决策可以在不干涉障碍物移动的前提下安全有效地避开动态障碍物。同时，通过将优化A*算法的最优路径作为局部路径规划的临时目标，解决了动态窗口法容易陷入局部最优解的问题。最终得到一种可以既考虑全局最优路径也考虑动态障碍物的实时避障路径规划算法。