融合改进A*与DWA算法的移动机器人路径规划

庞永旭,袁德成

(沈阳化工大学信息工程学院,辽宁 沈阳 110142)

0 引 言

随着科技的日益发展，移动机器人与人们的生活交集日渐增多。路径规划[1]作为当前移动机器人研究领域的前沿课题之一，其目的是在复杂的环境中躲避障碍，找寻一条能够实现从起始点无碰撞快速到达目标点的最佳路径。为解决路径规划中出现的问题，该领域内的学者进行了大量研究，提出了各种行之有效的算法。其中全局路径规划[2]算法有传统的A*[3]算法、Dijkstra[4]算法；智能仿生的蚁群算法[5]、粒子群算法[6]、遗传算法[7]等；局部路径规划算法有动态窗口法[8-9](DWA)和人工势场法[10]等。王小红等人[11]基于A*算法成功优化其评价函数，提高了搜索效率，并且通过关键点选取策略去除冗余节点。槐创锋等人[12]通过扩大传统的A*算法搜索邻域，将传统的3×3邻域分别扩展为5×5邻域与7×7邻域，优化了搜索角度，显著提高了其搜索效率。张超超等人[13]通过定向加权的方法改进A*算法，成功解决了机器人运动轨迹穿过障碍物的问题，但是仍然存在机器人运动轨迹与障碍物顶点相交的问题。张丹红等人[14]提出了一种将A*算法与蚁群算法相融合的算法，此算法去除了冗余节点，使路径更平滑，但是缺乏动态实时避障的功能，而且计算复杂度较大。徐保来等人[15]改进了动态窗口法，虽然能够实现移动机器人的实时避障，轨迹却无法满足全局最优。传统的路径规划算法在面对复杂多变的障碍环境时已然力不从心。程传奇等人[16]、吴飞龙等人[17]分别提出了2种基于A*与动态窗口的融合算法。根据上述路径规划问题，本文提出一种新的融合算法。通过引入障碍物占比优化并设计A*算法的启发函数，提高搜索效率，通过优化子节点的选择方式解决移动机器人路径与障碍物顶点相交的问题，通过删除轨迹中的冗余节点实现路径平滑度的优化，并且融合动态窗口法，在满足全局最优的情况下实现动态实时避障。

1 A*算法的改进

A*算法是一种能够实现全局路径规划的启发式搜索算法，可以在静态网络环境中根据评价函数来搜索最佳路径。但是传统的A*算法在复杂环境下存在以下问题：1)冗余节点多，搜索效率低；2)规划后路径的轨迹与障碍物顶点相交，安全性低；3)路径拐点多，平滑度低。因此本文通过对传统A*算法的评价函数做出改进，对子节点选择方式和路径平滑度进行优化以解决上述问题。

1.1 传统A*算法的基本原理

传统A*算法搜索原理是将起始点加入open列表，将该点作为父节点加入close列表，搜索其相邻的可到达节点加入open列表，依据评价函数计算open列表中节点的代价，选取代价最低的节点作为下一个父节点并放入close列表，再次搜索父节点可到达节点并计算其代价，依次循环，直到父节点为目标点的位置。

传统A*算法的评价函数为：

f(n)=g(n)+h(n)

(1)

式(1)中，n代表当前节点，f(n)表示的是移动机器人在当前节点的评价函数，用来选取最优路径，g(n)代表移动机器人从起始点到当前点的实际代价值[18-20]，h(n)为启发函数，代表从当前点n到目标点的估计代价值[21-22]。一般情况下，h(n)小于从当前点n到目标点的实际代价值，当h(n)值为0时，此时只有g(n)起作用，算法即为Dijkstra算法。h(n)的估计值比实际代价值小时，算法搜索时间会因搜索空间的增大而增加。h(n)估计值比实际代价值大时，算法搜索速度会增加，但是算法可能无法搜索到最短路径。

不难看出h(n)对于搜索效率有很大的影响，h(n)有几种常见的表现形式：1)曼哈顿距离；2)切比雪夫距离；3)欧几里得距离。其几种表现形式如下。

h(n)=abs(nx-gx)+abs(ny-gy)

(2)

h(n)=max[abs(nx-gx),abs(ny-gy)]

(3)

h(n)=sqrt[(nx-gx)2+(ny-gy)2]

(4)

在本文中h(n)采用的是欧几里得距离，并且通过引入障碍物占比优化改进启发函数。

1.2 启发函数的改进

当栅格地图中的障碍物即障碍栅格适度增加时，地图中从起始点到达目标点的路径的选择也会相应增多，优化后过程中容易陷入局部最优，导致搜索效率降低，依据传统A*算法的基本原理可看出，评价函数影响着A*算法的搜索效率，因此对评价函数中的启发函数进行改进。

1.2.1 引入障碍物占比P

通过引入障碍物占比P来影响启发函数h(n)在不同环境下的权重。假设一局部区域中障碍物的数量为a，所有栅格数量为A，设当前点的坐标为(nx，ny)，目标点的坐标为(gx，gy)，则A可以由式(5)表示。

A=[1+abs(nx-gx)]×[1+abs(ny-gy)]

(5)

障碍物占比P表示在当前节点与目标节点所构成的局部区域中，障碍物在所有栅格中占的比重。则P如式(6)所示。

P=a/A

(6)

1.2.2 优化启发函数

局部区域内障碍物较多时，需要确保最优路径的精度，避免陷入局部最优，此时应该减少h(n)的权重，降低搜索速度；局部区域内障碍物较少时，如果按照传统A*算法的启发函数h(n)寻找最优路径，会导致搜索节点过多，搜索效率降低，此时应该加大h(n)的权重。如式(7)所示，改进后的评价函数提高了A*算清的适应性。

f(n)=g(n)+h(n)-lgP×h(n)

(7)

1.2.3 子节点的优化

传统的A*算法在寻找最优路径时，会出现最优路径与障碍栅格顶点相交的情况，为解决这一问题，本文优化子节点的选择方式。如图1所示，中间节点为父节点，父节点周围相邻的8个邻域为8个子节点。根据8个子节点与父节点的位置关系，将子节点1、3、5、7归类为A类子节点，将父节点上下方向上的子节点2、6归类为B类子节点，将父节点左右方向上的子节点4、8归类为C类子节点。

图1 子节点图

根据A、B、C这3类子节点，定义了以下子节点优化选择方式，如表1所示。

表1 子节点选择规则

1.2.4 优化路径平滑度

传统的A*算法在路径规划过程中会出现冗余节点，影响移动机器人的正常工作。冗余节点包括共线节点和多余拐点，如图2所示，X1→X2→X3→X4→X5→X6→X7→X8路径存在较多冗余节点，为传统A*算法所寻路径。本文通过从起始点到目标点删除冗余节点，然后从目标点反向到起始点再次删除多余节点进行路径平滑度优化。

图2 删除冗余节点图

1)删除共线节点，从目标点方向在X2开始，沿着传统A*算法的路径判断当前节点与前一节点是否共线，若共线，则前一节点被认为需要删除的冗余节点。优化后轨迹变为X1→X5→X6→X7→X8。

2 动态窗口法

动态窗口法(DWA)是一种解决移动机器人局部避障时的常用算法。此算法是指在速度空间中，依据移动机器人的运动模型，对多组速度进行采样，分析并预测出一段时间内在各组速度下移动机器人的移动轨迹，然后根据算法的评价函数选出最优轨迹所对应的速度，根据最优轨迹对应的速度驱动移动机器人进行局部路径规划。

2.1 移动机器人的运动模型

DWA对窗口区域内移动机器人的线速度和角速度进行采样，因此第一步需要对移动机器人进行运动学建模。假设在一段时间内(Δt)移动机器人做匀速直线运动。式(8)表示移动机器人在该段时间内的运动学模型。

(8)

2.2 移动机器人速度采样

在移动机器人速度组空间中，理论上有无穷多组速度集[23](v,ω)，但是移动机器人容易受到自身的硬件、工作环境的制约，因此获得机器人运动模型后需要根据实际情况对采样的速度集范围进行约束。

1)移动机器人受自身条件制约的最大、最小速度范围可表示为：

Vm={(v,ω)|v∈[vmin,vmax]∩ω∈[ωmin,ωmax]}

(9)

2)移动机器人受到自身电机的影响，存在加速减速约束，在动态窗口显示内，移动机器人受加减速影响的最大、最小速度范围为：

(10)

式中，vt代表移动机器人的当前线速度，ωt代表机器人当前角的速度，va、ωa为移动机器人的最大加减速度，Δt为模拟预测时间。

3)移动机器人制动距离约束。实现动态避障主要依赖制动距离的约束。动态窗口法在选择速度和轨迹评价的过程中寻找障碍物，为保证移动机器人工作时的可靠性与安全性，需要其在与障碍物发生碰撞前，在最大减速度条件下，把速度减到0。约束公式如式(11)所示。

(11)

式(11)中dist(v，ω)表示此速度下该轨迹与障碍物之间的最短距离。根据上述3种速度约束可得移动机器人所取速度集为满足上述3种约束条件的速度矢量空间交集，如图3所示。

图3 速度采样示意图

2.3 评价函数

动态窗口法中的评价函数用于选取最优轨迹。轨迹评判的准则为：准确避开障碍物，耗时最短靠近目标点。设计评价函数为：

G(v,ω)=σ(α·head(v,ω))+β·dist(v,ω)+γ·vel(v,ω)

(12)

式中head(v，ω)为方位角评价函数[24]，表示目标位置与预测轨迹终点位置方向的方位角偏差大小；dist(v,ω)为距离评价函数；vel(v，ω)为速度评价函数；α、β、γ为3项加权系数，σ为归一化平滑系数。

3 算法融合

改进后的A*算法本质上仍然是一种全局路径规划算法，在错综复杂的环境下无法进行动态避障，路径也不够平滑。而动态窗口法作为一种解决移动机器人局部避障时的常用算法，能够动态实时规划路径，从而弥补改进A*算法的不足。而且传统的动态窗口法缺少全局路径规划的指引，在障碍环境复杂时容易陷入局部最优，对此提取改进后A*算法的全局路径节点，设计一种融合全局路径信息的评价函数。

G(v,ω)=σ(α·Head(v,ω))+β·dist(v,ω)+γ·vel(v,ω)+λDist(v,ω)

(13)

式中Head(v，ω)为方位角评价函数，表示模拟预测路径终点位置方向与全局路径节点的方位角偏差，依照改进后A*算法的全局路径节点顺序设置当前目标节点，在抵达目标点时更新，依次动态设置目标节点。dist(v,ω)、Dist(v,ω)为距离评价函数，分别表示全局已知障碍物与模拟轨迹的最短距离和局部未知障碍物与模拟轨迹的最短距离。本文采用改进后的A*算法做全局路径规划，然后采用融合全局路径信息的DWA进行局部路径规划，最终实现移动机器人动态避障，寻找最优路径，融合算法流程如图4所示。

图4 融合算法流程图

4 实验仿真

本文在MATLAB下进行仿真实验，从而验证改进算法及融合算法的有效性。

4.1 A*算法优化后仿真结果

上述操作实现了搜索效率的提高，删除了多余节点并且优化了路径平滑度，本文在MATLAB下进行不同环境的仿真，分别为环境1(简易环境)、环境2(复杂环境)，其中仿真环境2的尺寸相比环境1扩大了4倍。通过图5、图6可看出简易环境下优化后的A*算法有障碍物信息的引导，搜索空间减少，搜索时间更快，全局路径效果更佳；通过图7、图8可看出复杂环境下的改进A*算法路径长度上短于传统A*算法，避免了路径与障碍物顶点相切，安全系数提高，路径精度高于传统A*算法。表2为不同环境下路径规划后的指标，改进后的A*算法优于传统算法。

图5 环境1下传统A*算法仿真

图6 环境1下改进A*算法仿真

图7 环境2下传统A*算法仿真

图8 环境2下改进A*算法仿真

表2 路径规划对比

4.2 融合算法仿真结果

图9 避障过程仿真图1

图10 避障过程仿真图2

图11 改进A*算法仿真图

图12 融合算法仿真图

58 结束语

本文提出了一种融合改进A*算法与动态窗口法的融合算法，以提升移动机器人在不同复杂度环境下工作的灵活性，提高其工作效率。与传统算法相比，改进后的A*算法解决了轨迹与障碍栅格顶点相交的问题，路径节点减少，路径更加平滑，引入障碍物占比，提高了不同环境下的灵活性与搜索效率。并采用动态窗口算法进行局部避障，结合全局路径规划信息的指引规划出最优路径，弥补了A*算法无法完成动态避障的缺点。通过MATLAB仿真对比实验，表明了本文所提出融合算法的有效性。未来预计从以下几个方面进行研究：1)针对复杂动态环境下多数目移动机器人相互协调，多目标移动机器人路径规划问题进行探索；2)针对未知动态环境，结合机器学习进行移动机器人路径规划的进一步研究。