融合改进A*算法和DWA算法的全局动态路径规划

董晓东，李 刚，宗长富，李永明，李云龙，李 祥

（1.辽宁工业大学汽车与交通工程学院，辽宁 锦州 121001；2.吉林大学汽车仿真与控制国家重点实验室，长春 130022）

0 引言

无人驾驶技术是指利用机器人替代人类在工业、交通、物流、医疗、军事等领域中完成各种工作任务。实现无人驾驶最重要的环节之一就是路径规划，目标是通过搜索寻找最佳路径，连接起点和终点［1］。目前，无人驾驶路径规划研究主要分为2类：一类是基于车辆动力学模型，采用运动学优化的方法进行路径规划；另一类是基于全局环境建模和动态障碍物避障的方法进行路径规划［2］。在整个控制系统中，路径规划环节承上启下，是无人驾驶技术的核心［3］。

路径规划算法根据适用的场景分类为全局路径规划和局部路径规划［4］。全局路径规划一般用来生成全局参考轨迹，如传统Dijkstra算法［5］、A*算法［6］、D*算法［7］、快速扩展随机树RRT算法、蚁群算法、粒子群算法、遗传算法等［8］；局部路径规划一般是在参考路径的引导下，规划出未来短暂时间段内可通行的局部路径，常见的有动态窗口算法、人工势场法、TEB算法等。

虽然A*算法具有较好的求解效率而被用于全局寻优，但该寻优路线仍面临着拐点过多、节点冗余、与障碍间距离过于接近等问题。同时，如果应用动态窗口法（DWA）进行路径规划可以使无人驾驶汽车具备良好的避障能力，并生成平滑的路径，无需额外优化。但是，该方法可能会陷入局部最优解，无法按全局最优路径到达目标点。随着上述问题造成的无人驾驶汽车路径规划局限性越来越大，有的专家学者对传统A*算法进行了相应改进优化，克服了上述问题衍生出了各种新的路径规划方法，但需要联合全局与局部2种方法［9］；Guruji等［10］提出了一种名为时效A*的改进算法，该算法通过使用斜率检测的方式来减少相邻节点的搜索次数，从而提高了算法的效率；姚进鑫等［11］提出了一种将跳跃点搜索与动态窗口法相融合的路径规划算法，该方法能较好地克服最优A*方法在曲线转弯处存在的曲率不连续性问题，改善了轨迹的光滑度和整体优化；合肥工业大学的贾屿等［12］将A*算法与跳点策略相结合，给出了一种新的优化策略，该策略在提高寻优速度和路径安全性方面取得了显著的进展；重庆理工大学的尹婉秋等［13］基于A*算法，将人工势场法和障碍的位置信息相结合，利用粒子群算法自适应地选取参数，解决了传统A*算法“先移后避”的难题，降低了A*算法的计算复杂度；重庆理工大学的李长庚等［14］引入一种双向交叉搜索策略和基于指数衰减的启发函数权重方法，解决了现有A*算法存在搜索时间长、转弯角度大和曲线不连续等问题；辽宁工业大学的白云龙等［15］提出一种通过扩展搜索邻域的方法来提高A*算法的规划效率，并将人工势场法融入其中来避障，最后通过3次B样条曲线平滑路径中出现的尖锐节点；海南大学的田晃等［16］提出了一种基于多目标节点的改进动态窗口算法，在提高路径平滑度的同时，相对于传统算法具有更好的局部避障能力；张振等［17］提出了一种实时路径规划方法，该方法将改良的A*算法与动态窗口法相结合，将经过优化的关键点作为临时目标点，能够生成一条基于全局最优的光滑曲线路径；郭翰卿等［18］在传统A*算法的评价函数中引入安全估值，通过运用改进的动态窗口评价函数，成功将安全性A*算法与动态窗口法相融合，实现了在全局路径行进中的动态避障。

路径规划算法常用于机器人领域，无人驾驶的路径规划算法受其启发［19］。在无人驾驶系统中，路径规划算法作为其关键技术之一，引起了国内外学者的广泛关注，并呈现出百家争鸣的局面［20］。虽然大部分研究人员对现有的A*算法、DWA算法作了一定的修改，但是仍然不能彻底解决实际规划中的某些问题。基于此，本文中对A*算法进行了改进，使其可以寻找到最优的全局路线节点，同时利用动态窗口方法对邻近节点进行了局部规划。该方法可使无人车在全局路径中遭遇任意的障碍时合理绕开障碍，返回到全局路径，这样不仅可以保证路径的整体最优性，而且可以有效地克服各种随机障碍对规划路径的影响。

1 算法基础

1.1 传统A*算法

A*算法被广泛应用于优化路径的搜索，因为它具有高效的搜索效率和快速的规划速度［21］。A*算法基本思想是从起始栅格点开始，寻找与起点相邻的子栅格点。每一次都会从周围的子栅格点中选择一个评估函数值最小的点作为下一步的搜索节点，即当前节点。然后，在当前节点的基础上，生成与其邻接的子栅格点，并以目标节点为终点进行下一步的搜索。

A*算法的评价函数对检索空间的大小有影响，而检索空间的大小、访问量反过来又对A*算法的运算速度有影响。所以，A*算法评价函数的优劣对其寻优效果及寻优效率有着很大的影响。

设f（n）为A*算法评价函数，公式如下：

式中：f（n）为起始位置到当前位置n之间总和的代价函数；h（n）为当前节点位置n到目的地位置之间估计路径代价的启发函数［22］。

启发函数的选择会影响A*算法的效率和准确性。如果没有障碍物阻挡，那么用欧几里得距离作为启发函数，就能使得估计路径和实际最短路径一致，这样算法就能快速而精确地找到目标，不会浪费时间在多余的节点上。但是在实际情况中，往往有很多障碍物使得实际路径比估计路径要长，这时候算法就会产生很多冗余节点，增大了搜索空间，降低了搜索速度［23］。如果能够调整启发函数的值，使其接近于实际路径的值，就能够有效地提高搜索效率。由于不同的地图环境中障碍物的分布不同，如果能够对障碍物进行量化，并根据量化数据调节启发函数，就能够提高算法的效率和适应性。

针对传统A*算法在路径规划中存在的缺陷，本文对其进行了改进和优化。具体方法如下：

1）通过栅格法对环境进行建模，并根据式（9）计算障碍率P。同时，设定无人驾驶汽车的起始节点和目标节点，并建立Open列表和Close列表。将起始节点添加到Open列表中，而Close列表为空。

2）算法开始遍历起始节点周围的节点，并根据优化后的子节点选择方式生成可选子节点。随后，将起始节点从Open列表中移到Close列表中，若Open列表为空，表示不存在待搜索节点，算法将终止，表明无可行的路径。

3）若Open列表不为空且存在待搜索节点，则分别计算各子节点的f（n）值，并选取具有最小f（n）值的节点作为下一个路径节点，同时将该节点从Open列表中移到Close列表中。

4）判断该扩展节点是否为目标节点。若为目标节点，则从目标节点开始逆向搜索其父节点，并对路径进行双向平滑度优化。最后，输出规划路径。若非目标节点，则继续扩展其周围节点并计算子节点的f（n）值，将不在Close列表中的周围子节点添加到Open列表中。

5）重复步骤4），直至扩展到目标节点并输出最终规划路径。

改进A*算法的流程如图1所示。

图1 改进A*算法流程

1.2 动态窗口（DWA）算法

动态窗口（DWA）法是一种局部动态路径规划算法，它通过对无人驾驶汽车的位置变化进行简化，仅考虑线速度和角速度控制。它将避障问题简化为空间中的运动约束问题，从而可以根据运动约束条件选择最佳的局部路径［24］。假设一辆无人驾驶汽车在时间段（t1－t2）内运动，那么它对应的位姿变化信息如下：

式中：xt2和yt2分别为t2处无人驾驶汽车的x、y坐标位置；θt2为t2时刻无人驾驶汽车的航向角；v为无人驾驶汽车的线速度；ω为无人驾驶汽车的角速度；Δt为（t1－t2）的时间间隔。

一旦在预测时间内对速度空间进行了采样，就可以使用评价函数来评估预测轨迹的优劣。DWA算法的评价函数通常需要根据实际情况进行设置，主要包括目标距离、速度、障碍物和方位角的代价。本文中根据式（5）设置DWA算法的评价函数，即总成本越小，路径规划过程越好。

式中：C（v，ω）为对速度采样的总成本；D（v，ω）为目标点位置与速度取样空间之间的距离成本；S（v，ω）为速度采样中的速度成本；O（v，ω）为速度采样空间和障碍物之间的距离成本；α，β，λ分别表示目标距离、速度、障碍物距离的单位成本增益。

各代价函数D（v，ω）、S（v，ω）、O（v，ω）的具体函数表示为：

式中：（xt，yt）为预测节点的（x，y）坐标位置；（xd，yd）为目标点的（x，y）坐标位置；vmax为无人驾驶汽车最大速度；vt为预测速度空间的速度；（xoi，yoi）为第i个障碍物的（x，y）坐标位置。

2 改进传统A*算法

2.1 量化栅格地图障碍物信息

栅格地图是一种用网格单元表示实际地图的方法，每个网格单元可以表示一个区域是否有障碍物。有障碍物的网格单元叫做障碍格，障碍格的多少会影响地图上可供选择的路径的数量，也会使得最短路径的长度大于起点和终点之间的直线距离。为了估计最短路径的长度，需要定义障碍率P，它可以反映地图环境的复杂程度。障碍率P是指当前点和终点构成的矩形局部环境中，障碍格的数量占该局部地图总格数的比例。假设当前位置与目的地位置组成的栅格地图内的障碍栅格个数为N，初始位置坐标（xs，ys）、目的地位置坐标（xg，yg），其表达式如式（9）所示。

2.2 改进A*算法的评价函数

评价函数是A*算法的核心，为了提高A*算法的搜索效率和精度，本文中针对A*算法的评价函数进行了优化，提升搜索速度和准确度。评价函数包括代价函数和启发函数2部分，其中启发函数是决定搜索效果的重要因素。启发函数的值要根据地图情况中的障碍物密度进行变化，障碍物密度越高，启发函数越低，搜索范围越广，搜索精确度越好。障碍物密度由障碍物个数和起点、终点的位置共同确定，它使得A*算法的搜索范围具有变化性。通过将障碍物密度纳入评价函数中，根据障碍物密度自适应调节代价函数和启发函数的比重，实现评价函数的自动调节，从而保证路径规划的灵活性、快速性和最优性。

式中：g（n）为初始位置（xg，yg）到当前位置（xn，yn）之间累计距离的代价函数；h（n）为当前位置（xn，yn）到目的地位置（xg，yg）之间的启发函数。

2.3 优化搜索点选取策略

A*算法在搜索过程中，需要维护一个开放列表，其中包含了所有当前点周围可达的子节点。每次选择评价函数值最小的节点作为下一个路径节点。在生成子节点时，需要判断子节点是否位于障碍栅格上。如果是，将不会将该子节点加入开放列表。这样做的结果会导致规划出的路径可能沿着障碍物的边缘走，甚至还可能穿过2个相邻障碍物的顶点。这样就会增加无人驾驶汽车与障碍物碰撞的风险，如图2所示。

图2 碰撞风险示例

如图3所示，展示了父节点及其8个子节点之间的位置关系。根据子节点相对于父节点的位置，将其分为3种类型。第Ⅰ类是位于父节点正上方和正下方的子节点2和6，第Ⅱ类是位于父节点正左方和正右方的子节点4和8，第Ⅲ类是位于父节点斜对角线上的子节点1、3、5和7。

图3 节点移动方向示意图

为了解决传统A*算法可能导致路径穿过障碍物顶点的问题，本文中提出了一种新的搜索点选取策略，根据子节点和障碍物的位置关系，排除一些不优的子节点。以图3为例，为了防止路径穿过障碍物顶点，假设某个子节点是障碍物，那么子节点选取规则如下：

1）如果障碍子节点在父节点的正上方或正下方（第Ⅰ类位置），则舍弃该障碍子节点左右两侧的子节点（如子节点1、3或子节点5、7）。

2）如果障碍子节点在父节点的正左方或正右方（第Ⅱ类位置），则舍弃该障碍子节点上下两侧的子节点（如子节点1、7或子节点3、5）。

3）如果障碍子节点在父节点的斜对角线上（第Ⅲ类位置），则不做任何处理。通过这种优化方式，改进A*算法可以有效地规避风险路径，不会让路径斜穿过障碍物顶点，从而减少无人驾驶汽车与障碍物碰撞的可能性。图2中的风险路径经过优化后变成了图4中的安全路径。

图4 优化后的安全路径

2.4 路径平滑度优化

本文中针对栅格地图中路径的转折和长度问题，设计了一种双向节点判断的路径平滑度优化方法。该方法的核心思想是，对于路径中的任意2个不相邻的节点，如果它们之间的连线长度小于它们之间的规划路径长度，并且连线不与障碍物相交，那么就可以删除它们之间的所有节点。这样可以有效地减少路径的转折和长度，提高路径的平滑度。此外，该算法还考虑了防碰撞的要求，通过设置安全距离和计算障碍点到连线的垂直距离和纵轴距离，以及利用单元障碍栅格的外接圆半径，判断路径是否安全，避免与障碍物发生碰撞。

以图5所示的栅格地图路径规划轨迹为例，采用A*算法规划的路径由若干栅格节点组成，图中优化前的路径为（S→n1→n2→n3→n4→n5→n6→n7→n8→n9→n10→G），该路径存在冗余节点、过多转折和节点仅能在栅格中心等问题，不利于无人驾驶汽车路径跟随。

图5 栅格地图路径规划轨迹

针对以上问题本文中对A*算法得到的路径节点进行优化处理，包括双向删除冗余节点、障碍距离判断和双向平滑度优化，使得路径节点可以在栅格中任意位置选择，减少了路径的转折和长度，提高了路径的平滑度。具体优化步骤如下：

步骤1删掉路径节点中同一直线上的中间点，只保留起点、拐点和终点。这样可以简化路径，减少不必要的节点，处理后路径为S→n7→n8→n9→G。

步骤2 从起始点S方向，在保留的2拐点ni、nj之间每隔k步长取一节点。这样可以在2拐点之间生成一系列候选节点，用于优化路径。

步骤3判断2点之间路径是否有障碍物。如果有，就不选择当前节点为路径节点，如果没有，就继续下一步。

步骤4计算路径旁障碍物与路径的距离，并使用大小关系来判断是否选择当前节点作为路径节点。这样可以保证路径不会太靠近障碍物，避免碰撞危险，处理后的路径为S→n11→G。

步骤5反向取点后，按照与步骤2、3、4相同的判断方式，从目的地点G方向进行判断。这样可以优化路径，使其变得更短或更平滑。处理后的优化路径为S→n12→G。

步骤6输出优化路径Path。

3 融合算法

根据前文所述，改进后的A*算法在全局信息已知的情况下，能够找到最优的全局路径。然而，当存在未知障碍物的环境中，依赖改进后的A*算法单独进行路径规划并不有效。DWA算法可以实现路径的平滑和实时避障，但很容易陷入局部最优，成功规划路径的概率较低。因此，本文中提出了一种融合改进A*算法和DWA算法的方法。通过巧妙地结合这2种算法，可以得到一种既能保留全局最优性，又能适应局部实时变化的路径规划算法。

3.1 优化DWA算法评价函数

根据无人车的速度范围和运动模型，在仿真中使用了多个轨迹，并使用估计函数进行筛选，以选择最优轨迹［25］。针对传统动态窗口法容易出现局部最优的问题，对动态窗口算法的估计函数进行了改进。新的评估函数结合了2.2节改进的A*算法提供的全局轨迹信息，确保最终的局部轨迹以全局最优轨迹为基础。其中DWA算法的基本参数设定如下：最大速度限制设置为1 m／s；最大角速度限制定为20（°）／s；速度分辨率设置为0.01 m／s；角速度分辨率设置为1（°）／s；加速度限制为0.2 m／s2；角加速度限制为50（°）／s2；评价函数的各参数分别为：α＝0.1，β＝0.05，γ＝0.2，预测时间的周期设定为3.0 s。改进后的估计函数如式（17）所示：

式中：PHead（v，ω）为轨迹终点方向与当前目标点之间的角度差；dist（v，ω）为轨迹与最近障碍物之间的距离；vel（v，ω）为当前速度评价函数；σ为平滑函数；α、β、Υ为加权系数。

3.2 算法融合策略

为了同时实现全局路径优化和随机避障目标，本文中提出了一种结合改进A*算法和动态窗口法的路径规划方法。改进A*算法利用了启发式搜索算法来在复杂环境中快速找到全局最优路径。动态窗口法是一种局部避障算法，能够根据被控对象的运动状态和环境信息，实时生成适应的速度空间。利用改进的A*算法进行全局路径规划后，获得全局最优节点序列后，可以使用动态窗口法在相邻节点之间进行局部路径规划。本文中将这2种算法结合起来，所采用的最终融合算法流程如图6所示。

图6 融合算法流程

4 仿真验证

4.1 改进A*算法实验仿真分析

为了验证本文中改进A*算法的有效性，在栅格地图环境信息相同的前提下，分别用改进后的A*算法、传统A*算法、蚁群算法、Dijkstra算法进行路径规划仿真对比实验，并通过改变对应栅格地图信息进一步仿真验证。栅格地图模型大小分别为20×20、30×30和50×50。在栅格地图环境中，黑色区域表示障碍，白色区域为可移动区域。计算机配置为：Windows 11操作系统，处理器为AMD Ryzen 7 5800H with Radeon Graphics，主频为3 201 MHz，运行内存为16 G。

1）实验1：栅格地图20×20，障碍物覆盖率p＝20%。路径规划轨迹如图7所示。

图7 实验1的4种算法路径规划轨迹

2）实验2：栅格地图30×30，障碍物覆盖率p＝13%。路径规划轨迹如图8所示。

图8 实验2的4种算法路径规划轨迹

3）实验3：栅格地图30×30，障碍物覆盖率p＝25%。路径规划轨迹如图9所示。

图9 实验3的4种算法路径规划轨迹

4）实验4：栅格地图50×50，障碍物覆盖率p＝25%。路径规划轨迹如图10所示。

图10 实验4的4种算法路径规划轨迹

本文中改进A*算法在评价函数中增加了环境信息的引导，搜索空间对比4组仿真实验比传统A*算法分别降低29.2%、45.83%、61.17%、60.36%；相比Dijkstra算法分别降低72.8%、83.18%、84.23%、88.37%。同时增加子节点优化规则及路径平滑度优化，使改进A*算法搜索的路径转折次数更少，转折角度更小，在保证与障碍物保持在安全距离之外的同时，规划出的路径有效避免了可能会沿着障碍物的边缘走，甚至穿过2个障碍物相邻的顶点的情况。从图8—图10可知，随着栅格地图扩大和障碍物的增多，蚁群算法所规划路径由于信息素高低的影响，出现折返路径、规划时间长、路径折点多等局限性。Dijkstra算法与传统A*算法规划的路径均一直朝着目标点方向，没有考虑障碍物对路径的影响，选择了局部路径最短的路线，出现多次转折，且不能避免规划出的路径可能会沿着障碍物的边缘走，甚至穿过2个障碍物相邻的顶点的情况。4种算法性能数对比如表1所示。

表1 4种算法性能数

4.2 融合算法实验仿真分析

通过在栅格地图中添加不同的随机障碍物，分别设置3组改进A*和DWA算法融合算法在障碍物覆盖率p＝20%的20×20栅格地图中进行路径规划，进而对融合算法的随机避障能力进行分析验证。

1）实验1：栅格地图20×20，障碍物覆盖率p＝20%，无障碍物。路径规划轨迹如图11所示。

图11 无障碍物路径规划轨迹

2）实验2：栅格地图20×20，障碍物覆盖率p＝20%，宽阔地带出现障碍物。路径规划轨迹如图12所示。

图12 宽阔地带出现障碍物路径规划轨迹

3）实验3：栅格地图20×20，障碍物覆盖率p＝20%，狭窄地带出现障碍物。路径规划轨迹如图13所示。

图13 狭窄地带出现障碍物路径规划轨迹

4）实验4：栅格地图20×20，障碍物覆盖率p＝20%，模拟较为复杂环境。路径规划轨迹如图14所示。

图14 较为复杂环境路径规划轨迹

通过上述4组对比实验分析可知，当地图中无随机障碍物出现时，改进A*算法与融合算法均可以规划一条从起点到终点的路径规划方法。然而，在广阔的区域中添加随机障碍物后，单独改进A*算法无法有效避免突然出现的随机障碍物。相比之下，融合算法实现了随机避障，同时确保规划路径保持全局最优性，并且使路径平滑，以适应无人驾驶车辆的跟踪行驶要求。后续通过改变障碍物的数量和位置来验证融合算法的鲁棒性。结果表明，在较为复杂的情况下本文融合算法仍然能够合理避开随机障碍物并到达目标点。融合算法路径规划性能参数如表2所示。

表2 融合算法路径规划性能参数

5 结论

1）本文中通过改进传统A*算法，在搜索空间上实现了大幅的降低，相较于Dijkstra算法的降低幅度在较为复杂的栅格地图环境中更是最大达到了88.37%。同时通过增加子节点优化规则和路径平滑度优化，使得改进后的A*算法搜索路径与障碍物保持安全距离，规划出的路径有效避免沿着障碍物边缘走或穿过相邻障碍物顶点的情况，路径转折次数更少，转折角度更小。这表明本研究中改进的A*算法具有一定的优势和有效性。

2）改进后的A*算法与动态窗口法相结合，能够成功避开随机障碍物到达目的地点。它不仅能根据全局路径修正局部路径，还能满足无人驾驶汽车在寻路过程中避免碰撞的要求。由此可见，该算法的可行性和有效性都具有一定的优势。

本文中的研究成果可用于复杂环境中无人驾驶汽车的路径规划。未来的研究可以通过调整动态窗口法的权值组合，提升环境适应能力，根据障碍物的实际分布情况进行对应优化。