改进A算法与动态窗口法的机器人动态路径规划

在移动机器人诸多技术的研究中，路径规划是技术研究中的重要部分之一，是机器人研究领域的热点，目的是在有障碍物的环境中按照某一种评价指标寻找一条从起始点位置到目标点位置的最优无碰撞路径[1-2]。

栅格法环境建模的经典方法，通过利用栅格对环境信息进行表示，栅格的大小决定了环境信息储存量的大小以机器人进行路径规划的时间长短[3-4]。

A*算法[5]是路径规划算法中经典的启发式搜索算法。

A*算法由Hart等[6]提出，结合Dijkstra算法和最佳优先搜索算法的优点。

文献[7]改进A*算法，在启发函数中加入余弦相似性和方向信息，做归一化处理。

文献[8]提出改进A*算法的扩展搜索邻域的思路，利用最小二叉堆优化A*算法OPEN列表存储结构提高搜索效率，但无法完成动态避障。

文献[9]成功地扩大搜索邻域，通过重新定义中心节点的位置，在每个节点的周围扩大无限可搜索邻域，较为快速实现无碰撞路径规划。

文献[10]基于A*算法，设计了优化的启发搜索函数，选取策略是使用关键点，剔除多余路径点以及非必要的转折点。

文献[11]通过引入二次A*算法，减少路径轨迹冗余点，缩短路径长度并且采用自适应圆弧优化算法与加权障碍物步长调
改进A*算法与动态窗口法的机器人动态路径规划
槐创锋，郭龙，贾雪艳，张子昊
华东交通大学机电与车辆工程学院，南昌330013
摘要：针对传统A*算法自身节点搜索策略存在路径转折点多、转折角度大、可行路径不是理论上的最优路径等缺点，将传统A*算法3×3的搜索邻域扩展为7×7，同时去除扩展邻域同方向的多余子节点，改进为7×7的A*算法，消除了传统A*算法的3×3邻域搜索和节点移动方向仅为0.25π的整数倍的限制，优化了搜索角度。

其次，针对移动机器人在复杂环境下动态路径规划问题，将改进7×7的A*算法与动态窗口算法进行融合，设计了一种全局最优路径的动态窗口评价函数，综合考虑移动速度、转角平滑度、安全性等因素，将改进7×7的A*算法与动态窗口法的融合算法与多种算法仿真比较，结果表明：改进7×7的A*算法与动态窗口法的融合算法更具有高效性和可行性。

关键词：改进的A*算法；动态窗口法；动态路径规划；融合算法
文献标志码：A中图分类号：TP242.6doi：10.3778/j.issn.1002-8331.2008-0063
Improved A*Algorithm and Dynamic Window Method for Robot Dynamic Path Planning HUAI Chuangfeng,GUO Long,JIA Xueyan,ZHANG Zihao
School of Mechanical and Electrical and Vehicle Engineering,East China Jiaotong University,Nanchang330013,China
Abstract：In view of the disadvantages of traditional A*algorithm’s own node search strategy,such as many path turn-ing points,large turning angles,and feasible paths that are not theoretically optimal paths,the traditional A*algorithm3×3
search neighborhood is expanded to7×7,at the same time the redundant sub-nodes in the same direction in the extended neighborhood are removed and it is improved to the7×7A*algorithm,eliminating the traditional A*algorithm’s3×3 neighborhood search and the restriction that the node moving direction is only an integer multiple of0.25π,and the search angle is optimized.Secondly,for the problem of dynamic path planning of mobile robots in complex environments, the improved7×7A*algorithm and dynamic window algorithm are combined,and a dynamic window evaluation func-tion of the global optimal path is designed,taking into account the moving speed and turning angle.For factors such as smoothness and security,the fusion algorithm of the improved7×7A*algorithm and the dynamic window method is com-pared with a variety of algorithm simulations.The results show that the improved7×7A*algorithm and the fusion algo-rithm of the dynamic window method are better.It is highly efficient and feasible.
Key words：improved A*algorithm;dynamic window algorithm;dynamic path planning;fusion algorithm
节了算法，采用预瞄偏差角追踪法捕捉移动目标点，有效提升路径规划效率。

文献[12]以时间耗费作为评价指标，假设AGV通过每个弧或节点的时间成本是固定的，改进A*算法通过搜寻最小时间耗费来选择最优路径。

文献[13]A*算法的改进，创新了高层建筑逃生路径规划算法，对节点扩展优化、权值优化方面进行改进。

文献[14]修改A*算法的地图信息，扩展了一层障碍物，改进了代价函数F作二次扩展，并且考虑机器人半径。

针对传统路径规划算法无法高效地、安全地完成动态环境下的路径规划，将传统A*算法扩展搜索邻域，去除了扩展邻域同方向的子节点，改进成了7×7的A*算法。

然后基于改进7×7的A*算法和动态窗口法的移动机器人在栅格法环境模型中实时仿真动态路径规划。

1改进的A*算法
1.1传统A*算法
A*算法是启发式的搜索算法，可以实现全局路径规划。

根据定义的估价函数在静态环境模型中搜索理论上的最优路径，则代价函数表示为：
f(n)=g(n)+h(n)（1）式中，n代表当前节点；f(n)是当前节点n的代价函数；g(n)为移动机器人起始节点到达当前节点n的实际代价值；h(n)是从当前节点n到达目标点的代价估计值，是代价函数中最重要的部分，正确地选择h(n)能够提升A*算法的成功率和准确性。

选取欧氏距离作为h(n)的代价函数，函数表示为：
h(n)=(x n-x g)2+(y n-y g)2（2）式中，(x n,y n)代表当前路径节点所在栅格中心的坐标，(x g,y g)代表目标点节点所在栅格中心坐标。

传统的A*算法搜索从起始点到达目标点的最优路径过程中，规划的全局路径存在多余节点，运动轨迹折线较多，在路径节点处存在转折角度过大以致于不符合移动机器人运动学原理等缺陷，为此提出一种改进的A*算法。

1.2改进7×7的A*算法
传统A*算法搜索扩展节点采用3×3邻域算法，如图1所示。

任意搜索方向之间夹角均为0.25π，节点转折角均为0.25π的倍数，因此传统A*算法的规划路径不是理论最短，而且规划路径上因为转折点较多、转折角过大不够平滑。

针对传统A*算法3×3的邻域搜索缺陷，提出扩展A*算法搜索邻域。

以图1中7×7正方栅格图为例，当前节点n位于中心节点，其周围第一层（3×3的邻域）中8个节点是传统A*算法待扩展的节点。

改进算法除了采用3×3邻域搜索算法外，还采用当前节点n周围第二层即5×5的搜索邻域，共计16个节点纳入算法待扩展节点，算法待搜索邻域节点数即为24个，节点移动方向是16个方向，改进之处在于去除同方向的多余子节点，则改进5×5的A*算法待搜索节点个数减少为16个。

这样既扩大了搜索邻域，优化了搜索角度，又提高了算法效率，如图2所示。

当A*算法的扩展搜索邻域到达第三层，如图3所示为7×7的搜索邻域，去除同方向的多余子节点后，待搜索邻域个数为32，可移动方向为32，则将传统A*算法成功改进为7×7的A*算法。

2动态窗口算法
传统DWA算法进行规划时，缺少全局规划路径的指引，只有目的地位置方向的指引，而中间具体路径是对环境中的障碍物及时避障实时规划的路径[15]。

然而，在障碍物较多情况下，移动过程容易陷入局部最优，导致全局路径距离变大。

将改进的7×7的A*算法与动态窗口算法进行融合，在局部路径规划时，机器人对临时
图2改进5×5的A*算法邻域搜索示意图图3改进7×7的A*算法邻域搜索示意图
空间上的约束优化问题。

动态窗口法的目的是：在速度二维空间中采样多组速度（线速度、角速度），在一定时间间隔内，同时模拟移动机器人在这些速度下的轨迹。

获取多组可行轨迹后，根据设计的评价函数，选取最优轨迹所对应的速度（线速度、角速度），驱动机器人完成局部路径规划。

2.1机器人运动学模型
动态窗口算法针对窗口区域内移动机器人的速度
进行空间采样，并且模拟出移动机器人的运动轨迹。

机器人的运动状态由机器人的线速度和角速度来反馈，(v t ,w t )表示轨迹，通过评价函数在所有可行轨迹里选取
最佳轨迹，在时间间隔Δt 内，假设机器人作匀速直线运动，运动学模型为：
ìíîï
ï
x =x +v x Δt cos θt -v y Δt sin θt y =y +v x Δt sin θt +v y Δt cos θt θt =θt +ωt Δt
（3）
2.2机器人速度采样
在速度空间中存在无穷多组(v ,ω)，要依据机器人
的实际情况对采样速度的范围进行约束。

（1）机器人的速度约束为：
V m ={}
()v,ω|v ∈[]v min ,v max ,ω∈[]ωmin ,ωmax （4）
（2）机器人电机加减速约束。

动态窗口移动时间间隔内，机器人加速度所带来的最大、最小速度为：
V d ={()v,ω|v ∈[]v c -v b Δt,v c +v a Δt ,
}ω∈[]ωc -ωb Δt,ωc +ωa Δt （5）
式中，v c 、w c 代表当前速度；v a 、w a 代表机器人最大加
速度；
v b 、w b 代表机器人最大减速度。

机器人制动距离约束。

在局部路径规划时为了确保机器人安全，在最大减速度条件要求下，机器人在撞击障碍物之前速度减为0，则：
V a ={
()v,ω|v ≤()2dist ()v,ωv b 1/2
,
}
ω≤()
2dist ()v,ωωb 1/2
（6）
式中，
dist (v ,w )是(v ,w )对应轨迹距离障碍物的最近距离。

2.3机器人评价函数
速度空间存在若干组的采样速度是可行的，于是设
计评价函数以便选取机器人最优轨迹。

评价函数设计准则为局部路径规划，机器人应尽量避开障碍物，用最短时间到达目标点位置。

设计的评价函数为：
G ()v,ω=σ()αhead ()v,ω+βdist ()v,ω+γvel ()v,ω（7）
式中，方位角评价函数head (v ,w )，代表了当前速度下
3融合算法
传统DWA 算法进行规划时，缺少全局规划路径的
指引，只有目的地位置方向的指引，故采用改进7×7A*算法进行全局路径规划，融合动态窗口法进行动态避障，提升动态规划路径的全局最优性。

为了提升实时避障在路径转弯处的能力，提高路径平滑性，在避障过程中，需要实时预估路径的弯曲程度，遇到急弯时提前减速。

为每个子目标点G i 添加一个参
数Δθ表示全局路径在该子目标点的弯曲程度，
Δθ是G i 分别与前后两个相邻子目标点的夹角。

当Δθ接近π时，相邻两个子目标点连线的方向近乎平行，机器人以较高的速度运行，当Δθ接近直角时，路径的弯曲程度较大，机器人应降低速度规划平滑路径准备转弯。

当运动轨迹近乎直线时，机器人线速度较大，当路径需要避障转弯时，线速度降低，增加路径平滑度。

为避免动态窗口算法陷入局部最优，设计了一种全局最优路径的动态窗口评价函数：
G ()v,ω=σ(αZhead ()v,ω,G i +βdist ()v,ω-)
γcos Δθi vel ()v,ω（8）
式中，
Zhead (v ,w ,G i )为模拟轨迹终点方向与子目标点之间的方位角偏差。

Δθi 是G i 分别与前后两个相邻子
目标点的夹角，子目标点G i 是机器人静态规划下的前进方向上距离当前点最近的静态环境下的全局最优路径点。

此评价函数使得局部路径规划遵循全局路径规划路径序列点，从而保证全局路径最佳。

3.1完成全局规划和临时动态避障
为验证所提融合算法在复杂环境中进行动态路径规划的有效性，在所建的栅格图环境模型中，首先标定目标点的位置和机器人起始点的位置，应用算法进行仿真验证，图4栅格地图环境中可以看出机器人避开所有静态障碍物到达目标点位置，完成了静态环境下路径规划。

为了验证动态环境下的路径规划，在机器人规划好1
4
7
1013161922252831
3128
252219161310741图4融合算法静态环境路径规划
（起始点位置标记为△，目标点位置标记为○）
程图，显示了移动机器人成功地避开动态障碍物到达目标点位置。

从图5、6可以看出，改进的融合算法，可以实现局部路径规划，躲避动态障碍物，还能得到平滑而且安全的规划路径。

3.2输出运动参数
所改进的融合算法可实时输出机器人的控制参数，
有利于机器人的自动反馈控制，机器人线速度的变化、机器人姿态（弧度）的变化、机器人角速度的变化结果如图7所示。

图7反映了机器人动态路径规划时，控制参数的变化，当机器人局部路径躲避障碍物时，线速度减小，弧度也减小。

3.3验证算法高效性和适用性，对比其他算法
为了验证基于改进7×7的A*算法与动态窗口法的
融合算法的高效性，与标准A*算法、改进7×7的A*算法
在相同的仿真环境中作出对比，仿真实验结果如图8所示。

每组重复仿真10次，取平均值记录路径规划的时间和路径长度，各种算法的性能指标如表1所示。

1
4
7
1013161922252831
3128252219161310741
目标点
动态障碍物动态障碍物
起始位置
图5临时设置动态障碍物
（请使用鼠标左键选择动态障碍物位置
（可以点在全局规划的路径上））
1
4
7
1013161922252831
3128252219161310741红色为临时动态障碍物
初始动态规划
（a ）动态1开始避障
1
4
7
1013161922252831
3128252219161310741动态避障
红色为临时动态障碍物
（b ）动态2准备避障
31282522191613107红色为临时动态障碍物
动态避障完成
31282522191613107红色为临时动态障碍物
动态规划路径
静态规划路径
100200300400500600700800
控制节点个数
0.70.60.50.40.30.20.1线速度/(m ⋅s -1)
100200300400500600700800
控制节点个数
3.23.02.82.62.42.22.01.81.6
机器人姿态/r a d
0（a ）机器人线速度的变化
（b ）机器人姿态（弧度）的变化图7
机器人的控制参数变化图
由图8、表1可知，三种算法都能够在静态环境下规划出从起始点到目标点的路径。

传统A*算法路径转折角多，转折角度大，改进的7×7的A*算法平滑性最好，路径最短，用时最少。

将3×3的搜索邻域扩展为7×7后，同时去除扩展邻域同方向的多余子节点，实质上搜索方向由8个增加到32个，优化了搜索角度；从表中可以看出改进7×7的A*算法距离为40.5634mm ，时间为0.038546s 。

传统A*算法路径距离为40.8701mm ，时间为0.060966s 。

改进7×7的A*算法增加了搜索方向，并没有因为增加算法计算量而延长路径规划时间，相反因为优化了搜索角度，取消了节点移动方向仅为0.25π的整数倍的限制，提高了搜索效率，因此减少了路径规划时间；并且改进7×7的A*算法优化了全局路径，减少了路径长度，增加了路径平滑性，可以表明改进7×7的A*算法比传统A*算法更具有适用性。

相比传统A*算法和改进7×7的A*算法，基于改进7×7的A*算法与动态窗口算法的融合算法，规划路径更短为38.6122mm ，平滑性更好，路径规划更好。

4结论
将传统A*算法，扩展其搜索邻域同时去除了同方
向的多余子节点，改进为一种7×7的A*算法，有效地消除了传统路径转折角多、转折角度大的缺陷。

为提升在动态复杂环境下移动机器人路径规划效率，提出一种基于改进7×7的A*算法与动态窗口法的融合算法，设计了一种全局最优路径的动态窗口评价函数，采用动态窗口
参考文献：
[1]STENTZ A.Optimal and efficient path planning for par-tially known environments[C]//IEEE International Confer-ence on Robotics and Automation.Piscataway ，NJ ，USA ：IEEE ，1994：3310-3317.
[2]EELE A J ，RICHARDS A.Path-planning with avoidance
using nonlinear branch-and-bound optimization[J].Journal of Guidance Control &Dynamics ，2015，32（2）：384-394.[3]张彪，曹其新，王雯珊.使用三维栅格地图的移动机器人路
径规划[J].西安交通大学学报，2013，47（10）：57-61.[4]田欣.基于改进遗传算法的移动机器人路径规划研究[D].
郑州：郑州大学，2016.
[5]王殿君.基于改进A~*算法的室内移动机器人路径规划[J].
清华大学学报（自然科学版），2012，52（8）：1085-1089.[6]HART P E ，NILSSON N J ，RAPHAEL B.A formal basis
for the heuristic determination of minimum cost paths in graphs[J].IEEE Transactions on Systems Science and Cybernetics ，1968，4（2）：100-107.
[7]石征锦，宿一凡，卜春光，等.基于改进A*的移动机器人路径
规划算法[J].单片机与嵌入式系统应用，2020，20（6）：13-15.[8]张敬寒，陶兆胜，彭澎，等.基于扩大搜索邻域A~*算法的
平滑路径规划[J].长春理工大学学报（自然科学版），2018，41（6）：124-127.
[9]辛煜，梁华为，杜明博，等.一种可搜索无限个邻域的改进
A*算法[J].机器人，2014，36（5）：627-633.
[10]程传奇，郝向阳，李建胜，等.融合改进A~*算法和动态窗
口法的全局动态路径规划[J].西安交通大学学报，2017，51（11）：137-143.
[11]王洪斌，尹鹏衡，郑维，等.基于改进的A~*算法与动态窗口
法的移动机器人路径规划[J].机器人，2020，42（3）：346-353.[12]廉胤东，谢巍.基于视觉引导多AGV 系统的改进A*路径
规划算法研究[J/OL].控制与决策：1-10[2020-08-20].https ：///10.13195/j.kzyjc.2019.1670.
[13]靳海亮，王赢乐，袁鸣，等.改进A~*的高层建筑逃生路径
规划算法研究[J].测绘通报，2019（11）：17-21.
[14]徐思远.基于ROS 平台的轮式机器人路径规划算法研究[D].
51015202530
0510152025300510152025300510152025
30
1471013161922252831
31
28252219161310741
目标点位置
黑色方格为静态障碍物
融合算法规划路径
起始点位置
（a ）传统A*算法路径规划
（b ）改进7×7的A*算法（c ）改进7×7的A*算法与动态窗口算法融合
图8
三种算法全局路径规划仿真图
算法
传统A*算法改进7×7的A*算法
改进7×7的A*算法与DWA 算法融合
距离/mm 40.870140.563438.6122
用时/s 0.0609660.03854610.180544
表1
三种算法性能指标的对比。