一种AGV路径规划的融合改进A＊算法

赵云龙，王新杰，成 奎，唐 林

（1.四川轻化工大学自动化与信息工程学院，四川 宜宾 644000；2.宜宾学院三江人工智能与机器人研究院，四川 宜宾 644000）

引言

制造业是中国发展的经济支柱，也是我国转型发展为制造强国的主战场，为了实现制造业的转型升级，中国制造2025计划提出将信息技术与制造业融合发展，推动传统制造业转型发展为智能化、数字化和自动化[1]。工业机器人作为制造业转型发展的关键，发展工业机器人技术成为制造业智能数字化的重要一环将会迎来新的发展机遇[2]。自动导引车作为智能工业机器人中重要的分支，在智能制造中发挥着重要的作用。在智能工厂中自动导引车(Automated Guided Vehicle，AGV)承担流水作业中物料的搬运、回收、配送的角色，不仅极大地提高了智能工厂的生产效率，还能提高企业管理智能化生产的水平[3]。通过AGV 作为生产的物流运输下，企业建造智能工厂自动化配送系统对AGV 的路径规划能力有着更高的要求[4-7]。

针对传统A*算法搜索效率低、冗余节点多等问题，国内学者提出了诸多改进方法。陈若男等[8]利用角度与距离信息改进传统A*算法的代价函数，提升了算法的搜索效率和安全性。沈可宇等[9]通过加入父节点对当前节点的影响力、转弯惩罚函数和冗余节点平滑等政策，提升了算法的效率和路径的平滑度。何光层等[10]利用三次样条插值法减少路径冗余点，提高了路径的平滑度。曹祥红等[11]通过优化储存结构和扩大G值来解决火灾情况下的疏散路径规划的问题，以适应火场。辛煜等[12]解决了A*算法搜索领域个数的限制，扩展为无数个任意搜索方向，这样搜索出来的路径降低了转折点的个数，路径更加平滑。张辉等[13]利用提取预览地图中的关键节点判断是否增量搜索，并加入安全距离改进代价函数，改善了路径的安全性。陈靖辉等[14]利用算法定向搜索消除路径的对称性，极大地提高了搜索效率。以上文献对改进A*算法提供了理论依据和思路。

综上所述，针对传统A*算法在路径规划中出现的搜索效率低下、节点冗余和路径不平滑等问题都提出了解决办法，但都是在已知全局环境下进行路径规划，无法对环境中的移动或未知障碍物做出避障，且是在牺牲其他指标性能的前提下对特定指标进行优化。为了获得更好的路径规划效果，本文提出了一种改进A*算法和动态窗口法结合的融合算法，通过对启发式函数、优化冗余节点、扩展安全搜索领域和改进动态窗口法评价函数等策略，AGV 在利用融合算法路径规划时，以达到实时避障且安全到达目标点完成作业的目的。

1 传统A*算法

A*算法是一种常见的经典启发式搜索算法，其融合了Dijkstra 算法的和最佳优先算法搜索（Greedy Best First Search）的优势[15]。算法核心是在已知地图上规划出一条从起点到目标点的具有最小移动代价的路径[16]，起始点到目标点的移动代价估值函数可以表示为：

其中，n表示在搜索过程中的当前节点位置，g(n)是距离函数，表示当前节点与起点的实际距离，h(n)表示当前节点到目标节点的距离估计即代价函数。

在规划过程中，AGV 小车通过不断的迭代循环找到终点，图1所示为传统A*算法的流程图。

图1 传统A＊算法流程图

2 改进A*算法

传统A*算法在任意已知地图中自适应性较差，在路径规划过程中容易存在重复搜索节点从而使搜索效率较低、启发式函数h(n) 代价值与实际代价差距较大从而导致搜索空间较大和路径冗余、将机器人看作了一个质点和路径拐点较多，容易导致机器人斜穿障碍物使机器人出现故障[17]。本文针对性改进以上不足问题，对传统A*算法进行以下改进：

1）扩展A*算法的搜索邻域，使之不再局限与搜索方向夹角为0.25π，提高搜索效率。

2）改进启发式函数，缩小在路径规划过程中估计的代价值与实际代价值差距，使扩展的节点更有贪心性和启发性。

3）使用垂距限制法剔除多余冗余节点，最后用3 次均匀B 曲线优化路径，使路径更加符合机器人运动学要求。

2.1 安全扩展搜索领域策略

传统A*算法一般为3×3 搜索领域（图2(a)），搜索领域过小会导致AGV 在作业时规划路径不是最优，搜索领域过大会增加规划过程中计算量从而降低效率[18]。为此，在改进过程中适当扩大搜索领域不仅可以提高搜索效率，还能去除多余的子节点，因此本文采用5×5 搜索领域（图2(b)）。安全扩展领域如图2(c)所示，其中A表示子节点，B表示父节点，黑色方块表示障碍物，在节点扩展的同时，需要根据父节点周围的环境信息调整搜索方向，若在搜索过程中，存在障碍物，则将该节点的评价函数设置为无穷大，调整扩展其他节点，避免了AGV 碰到障碍物，如果周围没有障碍物，则对周围子节点进行正常扩展。

图2 搜索领域对比图

2.2 改进代价函数

考虑AGV 在实际作业过程中，作业环境中的障碍物较多，且AGV 在大部分情况下不能直线到达终点，为了增加路径选择的多样性，采用曼哈顿距离表示，如式(2)所示：

其中，(Sx,Sy)为当前节点坐标，(Gx,Gy)为目标节点坐标。

针对传统A*算法路径规划时间较长、路径拐点较多，对h(n)进行优化，首先适量增大h(n)值，提高效率，加入拐弯惩罚函数，避免算法在出现局部最优解时，出现不必要的拐弯。由式(2)，设初始节点为(Nx,Ny)，则x和y方向的曼哈顿距离表示为：

由向量平行原理得到一个拐弯惩罚函数：

其中k为常数，取值区间为（0，1），用于提高f(n)的精度。其改进的代价函数如式(5)所示：

其中R为初始节点与目标节点的曼哈顿距离。

2.3 冗余节点平滑策略

对于AGV 在路径规划过程中存在大量冗余路段，采用垂距限值法优化冗余节点。传统A*算法的规划后的路径如图3(a)所示，路径不平滑且拐点较多；优化后路径如图3(b)所示，路径平滑度大幅提高，拐点数量也大幅减少。

图3 传统A＊算法优化前后对比

垂距限制法示意图如图4 所示，图中Last-node表示先前节点，pos表示当前节点，Next-node表示下一节点，其核心思想是计算pos 到last-node 与nextnode连线的距离d，d超过一定的阈值就从总集合中删除当前节点pos。但垂距限值法并非从整体角度去优化一条完整的路径，而是从第一个节点即起点开始依次筛选，去除冗余点。其具体实现步骤如下：

图4 垂距限制法

1)首先将路径规划中的所有节点加入总集合{x1,x2,…,xn}中，设置w为关键节点集合。

2)从x1出发连接所有节点，在连接节点过程中如果出现障碍物，则将该节点xi的商议节点加入w中，则x1～xi-1加入优化节点集合Z。

3)利用垂距限值法优化冗余节点集合Z。

4)采取均匀B曲线优化路径。

3 动态窗口算法

在AGV 执行作业时，在已有地图环境的情况下，在实时作业的时候需要观察周围环境，因此本文在使用改进的A*算法作为全局路径规划的基础上，局部路径规划上使用动态窗口法（Dynamic Window Approach，DWA）。AGV 通过自身所带传感器利用DWA 对作业环境进行检测，在进行实时路径规划时，可以使AGV 发现周围障碍物，增加了AGV 的实时避障能力[19]。在AGV 获取作业环境的栅格地图的基础上，通过优化后的A*算法进行全局路径规划，然后AGV 收到电脑下达的行动命令开始行动，由于AGV 受现场环境的影响和运动学约束，AGV 在作业过程中会产生误差，因此需要针对DWA 算法的不足进行改进。DWA 算法的原理是基于在速度空间中采样多组线速度和角速度的基础上，预测下一时间的运动轨迹，通过评价函数对所有获取的运动轨迹得到一条最优的实时轨迹。

3.1 机器人运动模型

本文AGV 采用四轮全向机器人并建立运动学模型，如图5所示。

图5 AGV运动学模型

设作业的AGV 在△t时间内是直线运动且全向行驶，坐标系为世界坐标系，图5 中AGV 的自身实时水平速度和垂直速度分别为vx、vy，角速度为ω，机器人的中心为Or，机器人的航向角为θt，那么可以得到机器人的航迹推算公式[20]：

3.2 速度采样

在速度空间中存在无穷多个数据，由于AGV 自身实际的硬件条件以及工作环境影响，需要对采样速度进行约束。

1）机器人实际最大、最小速度约束为：

2）机器人由于自身电机功率有限，因此机器人运动过程中电机加速度约束为：

其中，αx,min、αx,max为AGV 在X轴上的最大减速度和最大加速度；αy,min、αy,max为AGV 在Y轴上最大减速度和最大加速度；αω,min、αω,max为AGV 在角速度上的最大减速度和最大加速度。

3）为了保障机器人的安全，在机器人与障碍物碰撞之前应立即停止，保持安全距离，则安全距离约束为：

其中dist(vx,vy,ω)为机器人当前速度对应的轨迹距离障碍物的最小值。

3.3 改进评价函数

在速度空间中有无穷多组个速度组，而在采样的速度中有多组轨迹，在机器人选出最优轨迹时需要一个评价函数，传统DWA评价函数如下：

其中，α、β、γ分别为距离、夹角角度以及子函数的加权系数，为3 项子函数的归一化参数，head(vx,vy,ω)为方位角偏差评价函数，评价目标点与机器人末端朝向的方位角偏差，vel(vx,vy,ω)为机器人当前速度评价函数。

由式(10)可知，α、β、γ作为加权系数是固定值，为了提高机器人快速向目标前行，并安全绕开障碍物，对加权系数做出调整，使加权系数具有动态性。当机器人离障碍物较远时，提高机器人速度占比，使机器人快速到达目标点，当前进过程中，障碍物距离机器人较近时，提高机器人方向性占比，让机器人安全通过障碍物到达目标节点[21]。改进的评价函数如式(11)所示：

其中，k表示障碍物直径，在实际运动中，AGV 运行环境可能没有障碍物，这将导致AGV 运行时速度占比较大导致失控，因此在计算过程中，设定dist(v,ω)最大值为1.5k，因此dist(v,ω) ∈(0,1.5k)。

4 融合算法

在路径规划过程中，全局和局部路径规划都有各自的缺点，A*算法拥有在已知地图的基础下寻找最优路径的优点，却无法有效地避开环境中的动态障碍物；DWA动态窗口法拥有避开未知环境下的障碍物的能力但容易陷入局部最优情况从而无法到达目标点[22]。本文在分别改进两种算法的前提下，将A*算法使用垂距限制法优化后的节点作为DWA引导节点融合两种算法，在两种改进算法融合的基础上，AGV不仅能在全局规划出最佳路径还能实时避开环境中的动态障碍物，融合算法流程图如图6所示。

图6 融合算法流程图

5 仿真分析

通过Matlab2021 平台对算法进行仿真实验，构造栅格地图表示全局环境信息，黑色栅格表示环境中的障碍物，空白栅格表示空阔部分。分两组实验对改进算法进行验证。

5.1 路径优化程度和效率

对A*算法改进前后的路径优化程度和效率进行验证，如图7所示。3种算法实验结果见表1。

表1 不同路径规划算法实验对比

图7 不同算法路径规划对比图

从表1 可知，相较于传统A*算法，优化改进的A*算法路径长度缩短了12.49%，路径规划时间缩短了36.23%，拐点减少了62.5%。文献[10]算法仅对A*算法进行3次曲线优化，可以看出相比传统A*算法其路径规划只有路径长度提升相对较大，本文改进算法相比文献[10]算法路径规划长度缩短了3.39%，时间缩短了32.87%，拐点数量减少了40%，路径比文献[10]更加平滑，更加符合AGV 安全运行和工作。综上所述，本文优化后的A*算法符合AGV工作全局路径规划性能。

5.2 动态避障能力

在路径规划过程中加入未知障碍物和移动障碍物如图8所示，其中实验1(1)为加入一个障碍物和一个动态障碍物对比实验，实验0(1)为加入一个动态障碍物对比实验，图8(e)为本文算法加入两个障碍物的规划图。图9所示为加入两个障碍物后AGV的姿势变化，图中可见AGV 在遇到障碍物变化较大，避开障碍物后趋于平稳。图10所示为加入两个障碍物后AGV 线速度和角速度变化情况，可以发现速度总体平稳，没有出现较大的波动。

图8 融合算法对比图

图9 AGV姿态角度图

图10 AGV速度对比图

用改进DWA 算法优化路径中加入动态障碍物和未知障碍物时，AGV 依旧能安全地从初始点到达目标点并且避开了优化路径中的障碍物，完成动态避障，不同场景下融合算法实验对比见表2。从表2可知，在加入动态障碍物后，本文融合算法规划的路径相比于文献[10]算法减少了8.93%，时间减少了5.96%；在加入一个未知障碍物后，本文融合算法规划的路径相比于文献[10]算法减少8.94%，时间缩短了5.83%。可以看出改进后的算法路径更短，用时更少，进一步验证了算法在AGV 作业过程中的可行性和效率。

表2 融合算法实验对比

6 结束语

为了让AGV 在路径规划时实现动态避障，本文针对传统A*算法做了3 个方面的优化，分别是改进代价函数、领域扩展和平滑处理路径。结果表明本文改进A*算法相较于文献[10]算法分别从拐点、路径规划时间和路径长度减少了62.5%、36.23%和12.49%，本文改进A*算法路径搜索更高效和更加平滑。为了实现了动态避障，融合改进后的评价函数的动态窗口法，通过多次仿真测试结果显示，融合算法相较于文献[10]分别从路径规划时间和路径长度方面减少了5.96%和8.93%。本文改进的融合算法可以让AGV 在实现动态避障的前提下规划出一条全局最优平滑的路径，能满足AGV 的实际需求，具有一定的应用价值。