融合改进A*算法和动态窗口法的AGV路径规划

房殿军，王少杰，蒋红琰，，陆谦谦，Rolf Schmidt

（1.同济大学 机械与能源工程学院，上海 200092；2.青岛中德智能技术研究院，山东 青岛 266000；3.苏州罗伯特木牛流马物流技术公司，江苏 苏州 215000）

0 引言

自动导引车（Automated Guiede Vehicle,AGV）作为物流自动化技术领域重要的代表之一，已经广泛地应用于仓储物流领域。在现代物流系统中，AGV已经成为自动化立体仓库和分布式物流系统中最重要的无人驾驶操作工具之一。为了使AGV 能够高效、准确、安全的运行，路径规划成为了关键的技术保障之一[1]。路径规划是指用算法为AGV在地图中搜索一条无障碍的最优或次优路径。根据周围环境是否已知，AGV的路径规划可分为全局路径规划和局部路径规划。全局路径规划用于解决环境中具有已知、静态障碍物的路径规划问题，常用的全局路径规划算法包括A*算法、Dijkstra 算法、快速扩展随机树算法等；局部路径规划用于解决环境中具有未知、动态障碍物的路径规划问题，常用的局部路径规划问题包括动态窗口法、人工势场法、粒子群算法等。

A*算法是目前最常见的全局路径规划算法，但是其需要搜索较多的节点、求解轨迹有较多的转弯。张辉，等[2]针对A*算法在路径规划时存在路径转折多、与障碍物过近、轨迹不平滑以及求解时间随着栅格规模指数增长等问题，通过碰撞场模型改进了A*算法；张建光，等[3]将估计代价影响系数引入到评价函数，并且优先扩展目标节点方向的相邻节点，有效提高A*算法的计算效率。

动态窗口法（Dynamic Window Approach，DWA）是一种较为常见的局部路径规划算法，它的避障能力很强，而且轨迹平滑，但是在复杂环境中易陷入局部最优，无法到达终点。Chang，等[4]通过添加新的评估函数来改进原始评估函数增强全局导航的能力，通过Q-learning 算法学习提出的DWA参数获得训练后的agent以适应未知环境，该算法具有较高的导航效率和成功率。

当路径规划的场景变得复杂时，需要将全局路径规划与局部路径规划相结合进行研究。郭园园，等[5]针对复杂场景中移动机器人路径规划耗时长的问题，将改进的A*算法与动态窗口法相结合，使移动机器人在复杂场景中的规划效率更高；劳彩莲，等[6]针对转折点多的问题，改进了选择关键点的策略，通过超声波传感器实现局部避障，实现实时最优的路径规划；槐创锋，等[7]将搜索领域扩展，去除多余子节点，最后融合了动态窗口法，实验结果表明，所提出的算法求解效率更高；Sun，等[8]通过优化A*算法的评估函数提高其搜索效率，使用策略去除冗余点，在相邻节点之间使用动态窗口法进行动态路径规划，实验证明该算法可以减少规划时间和缩短轨迹长度，并且可以可视化随机避障的分辨率；吴飞龙，等[9]将AGV的位置信息引入代价函数，提出了权重函数，通过去除多余的节点平滑轨迹，最后融合了A*算法与动态窗口法，从而提高了路径规划的实时性。

上述文献使用不同的方法改进了A*算法和动态窗口法，但是都没有完全解决搜索节点多、转折点多、路径不够平滑和易陷入局部最优的问题。本文融合了改进的A*算法和动态窗口法，实验表明本文的融合算法可以为AGV规划一条距离较短且平滑的路径。

1 改进A＊算法

1.1 环境模型描述

常用的环境建模方法有栅格法、拓扑法和几何法，其中栅格法描述环境较为准确，本文采用栅格法为AGV建立环境模型。如图1所示，地图被分为大小相等的栅格，其中用白色栅格表示可通行空间，用黑色栅格表示环境中的障碍物。根据直角坐标系，横轴用x坐标表示，纵轴用y坐标表示。

图1 栅格地图

1.2 传统A＊算法

A*算法是最有效的求解全局路径的启发式搜索方法，是贪心算法和Dijkstra 算法的结合[10]。传统A*算法的代价函数如下：

其中，f(n)为子节点n 的代价，即实际代价与估计代价之和；g(n)为子节点n 距离起点的实际代价；h(n)为子节点n距离终点的估计代价。

本文选择欧氏距离计算子节点的估计代价h(n)，欧氏距离的公式为：

其中，(xn,yn)表示子节点坐标；(xs,ys)表示起点坐标；(xg,yg)表示终点坐标。

1.3 引入对数衰减因子

A*算法使用传统的代价函数进行路径搜索时，容易出现往返搜索的情况，从而使算法效率降低。当节点n的估计代价h(n)的值小于实际路径距离时，会导致搜索节点数量过多，因此，本文采用动态权重的方法，在起点附近增大估计代价h(n)的权重来减少不必要的搜索节点数量，以便更好地指导搜索算法朝着正确的方向前进，提高搜索效率。

本文考虑到当前节点的位置对于估计代价占实际代价的比重具有影响，因此，当前节点到目标点的估计路径代价被视为最小路径代价，其值小于实际路径代价。本文为启发函数引入了对数衰减因子，可以动态地调整h(n)的权重。当h(n)较大时其权重较大，这样可以使节点快速朝着目标点移动；当h(n)较小时，权重变小，确保能够到达目标点。改进A*算法的代价函数如下：

图2-图5为采用传统A*算法和改进A*算法规划的路径和遍历的节点。由表1中的实验数据可见，虽然改进A*算法和传统A*算法的路径长度相同，但是相比传统A*算法的结果，改进A*算法的规划时间减少了21.5%，累计转弯角度减少了8.3%，遍历节点总数减少了43.8%，因此，本文的改进A*算法可以提高传统A*算法的效率。

表1 传统A*算法和改进A*算法的性能对比

图2 传统A＊算法规划的路径

图3 传统A＊算法遍历的节点

图5 改进A＊算法遍历的节点

1.4 关键节点提取

由图4可知，本文的改进A*算法仍然有较多的转折点，AGV转弯时的速度变慢，因此会降低AGV的工作效率。针对这种问题，本文提出一种关键节点提取策略，以减少路径中的转折点，提取关键节点策略的步骤如下：

（1）将改进A*算法规划得到的全部节点存入节点集U{Pi,1 ≤i ≤n}，P1表示路径的起点，Pn表示路径的终点。初始化关键节点集V，将起点和终点存入关键节点集。

（2）将P1依次与P3，P4，…，Pm连接，检查直线是否穿过障碍物，若P1Pm为第一条穿过障碍物的直线，则节点Pm-1为路径的关键节点，将节点Pm-1添加到关键节点集V 中，将P1与Pm-1之间的节点判定为冗余节点。然后以同样的方式从Pm开始依次连接后面的节点，直到连接到终点Pn。

（3）按顺序连接关键节点集V 中的所有节点，所得到的轨迹即为提取关键节点后的路径，本文改进A*算法提取关键节点后的路径如图6所示。

图6 关键节点路径

表2为提取关键节点前后的对比，由实验数据可知，提取关键节点后的路径长度从24.485 缩短到了22.882，优化了6.5%；转弯次数从7 次变为了2 次，优化了71.4%；路径累积转弯角度从494.771 降为158.606，优化了67.9%。

表2 提取关键节点前后对比

可见，利用关键节点提取策略对路径优化之后，路径长度缩短、转弯次数减少，但是路径仍然存在不够平滑、与障碍物过近的问题。

2 改进动态窗口算法

动态窗口法是路径规划领域常用的局部路径规划算法，此算法可以使AGV顺利避开场景中的未知障碍物，也可以增加路径的平滑性。该算法首先在速度空间中随机采样多组线速度和角速度，然后使用这些速度组预测AGV在下一段时间内的运动轨迹。接下来，对于每个预测的轨迹，判断是否与障碍物发生冲突，如果有，则将其从速度组中剔除。最后，通过评价函数选取最优的线速度和角速度，作为AGV的控制指令，使其能够安全地运行。

2.1 运动学模型

动态窗口法是通过对机器人在一个窗口区域内的速度空间（υt,ωt)进行采样来模拟机器人在时间t内的行驶轨迹。机器人的运动状态由线速度υt和角速度ωt表示。利用评价函数筛选出在所有可行轨迹中表现最优的轨迹。AGV在一个时间间隔Δt内的运动学模型如下：

2.2 速度采样

在AGV的速度空间中，有多个速度组(υ,ω)可供选择。然而，由于AGV受到自身硬件条件和外部环境的限制，其速度被限制在一个特定的范围内。这些限制条件如下：

（1）AGV的速度约束：

（2）在预测的时间间隔内电机的加速约束和减速约束：

（3）为了确保AGV能够安全地避开动态障碍物，需要在碰撞前以最大减速度将速度降为0。此时，AGV的制动速度受到一定的限制，约束条件如下：

其中，dist(υ,ω)为AGV 当速度为(υ,ω)时与障碍物的最短距离。

2.3 改进评价函数

为了选取一条最优轨迹作为AGV的实际轨迹，速度空间中有多组采样速度是可行的。为此，需要设计评价函数进行评估。本文评价函数的设计准则是，在与障碍物保持一定距离的前提下，尽可能快地到达终点。改进的评价函数为：

其中，σ为平滑系数；α、β、γ、λ为每一项的系数；head(υ,ω)是方位角的评价子函数，用于衡量轨迹终点方向与目标位置之间的方位角偏差；dist(υ,ω)是障碍物距离的评价子函数，用于计算模拟轨迹终点与任意障碍物之间的最短距离；vel(υ,ω)是评价当前速度大小的子函数；goal(υ,ω)是代价评价子函数，计算预测点到终点的代价值。

2.4 融合改进A＊算法和动态窗口法

改进的A*算法虽然可以为AGV迅速规划一条全局最优路径，但是它无法适应环境中动态障碍物的变化；改进的动态窗口法虽然在避开障碍物方面表现出色，但它也存在容易陷入局部最优解，而无法到达终点等问题。

为了解决A*算法和动态窗口法各自存在的问题，本文将两种算法进行了融合，融合算法首先从改进A*算法规划的路径中提取关键节点，然后使用改进的动态窗口法在相邻的节点间进行局部路径规划，直到终点。融合算法保证了路径的全局最优性，并且克服了动态障碍物对路径规划的影响。相比于单独使用两种算法，该融合算法能够更有效地为AGV进行路径规划。融合算法的流程图如图7所示。

图7 融合算法的流程图

本文对比了传统动态窗口法和本文融合算法的效果，图8和图9为两种算法规划的路径，传统动态窗口法和本文融合算法的对比数据见表3。

表3 动态窗口法与本文融合算法性能对比

图8 传统动态窗口法规划路径

图9 本文融合算法规划路径

通过对比可知，本文融合算法相比传统动态窗口法规划时间从31.502 减少到9.031，优化了71.3%；路径长度从23.156 缩短到22.417，优化了3.2%；累计转弯角度从2 908.410 减少到449.541，优化了84.5%。可见本文融合算法加快了搜索速度，缩短了路径长度，转弯也更加平滑，提高了算法的效率。

3 仿真实验与分析

为了验证本文融合算法的有效性，对传统A*算法、传统动态窗口法和本文融合算法进行了仿真对比实验，并对实验结果进行了分析。

在文献[11]中的大小为20×20的栅格地图上，分别采用三种算法对AGV的路径规划进行仿真，为了保证有较好的对比效果，AGV在仿真实验中的参数都和文献[11]中所设置的参数相同，最大线速度为1m/s，最大角速度为20rad/s，最大线加速度为0.2m/s2，最大角加速度为50rad/s2，线速度分辨率为0.01m/s，角速度分辨率为1rad/s，时间分辨率为0.1s，预测周期为2s。路径规划的起点为（1，1），终点为（11，20）。实验环境所用的操作系统为64 位的WIN10 操作系统，运行内存为16G，实验平台为MATLABR2021b。

采用传统A*算法、传统动态窗口法和本文融合算法仿真的结果如图10-图12所示，三种算法的对比数据见表4。

表4 三种算法性能对比

图10 传统A＊算法规划仿真实验结果

图11 传统动态窗口法规划仿真实验结果

图12 本文融合算法规划仿真实验结果

采用传统动态窗口法得到的路径如图11所示，此时由于环境复杂，算法陷入局部最优，导致寻路失败。

由表4可知，虽然传统A*算法规划时间更短，但是它不能考虑动态障碍物；本文融合算法路径长度比传统A*算法减少了3.942，优化了14.2%；累计转角比传统A*算法减少了170.229，优化了34.4%。

对比三种算法规划的路径可知，传统A*算法规划的全局路径中有较多的转弯和冗余路径，路径也不够平滑；传统动态窗口法规划的路径易陷入局部最优，传统动态窗口法更加适用于局部路径规划，而在全局路径规划中难以到达终点；本文融合算法规划得到的路径转弯明显减少，路径曲率变化比较连续，平滑度比较高。

4 结语

为了使AGV能够在变化的仓储物流环境中快速响应新的任务，针对传统路径规划算法规划的路径转折点多、冗余节点多、路径不够平滑和易陷入局部最优的问题，本文融合了改进的A*算法和动态窗口法。在全局路径规划方面，首先，通过将对数衰减因子作为权重改进A*算法，然后，使用关键点提取策略去除冗余节点；在局部路径规划方面，改进了动态窗口法，使AGV既能快速无碰撞到达终点，又能与障碍物保持一定的距离；最后，将改进A*算法与动态窗口法融合，弥补了A*算法和动态窗口法各自的不足。通过仿真实验与其它路径规划算法进行对比，验证了本文融合算法性能更好，效率更高。下一步我们计划将本文融合算法应用在实际的AGV上，以进一步验证该算法在实际场景中路径规划的有效性。