改进A*和DWA算法的果园喷雾机器人路径规划

陆向龙，吴春笃，杨官学，张 波，陈 振

江苏大学电气信息工程学院，江苏镇江212013

农业是国民经济建设和社会发展的根基。其中，水果产业由于经济效益高，在农业生产中占有相当大的地位[1]。果园规模一般较为庞大，在进行生产作业时将会耗费较多人工成本和体力。为减轻农业生产人员的劳动负担，降低人工成本，减少工作重复率，越来越多的无人操作智能装备已被广泛研发并应用于农业生产之中。因此果园喷雾机器人在果园植保中替代人工作业将会大大降低人力成本。应用在果园环境中的果园喷雾机器人因其具有负载大量药剂、实现持续作业的优点，得到了研究人员的密切关注。由于果园地形复杂，光照变化较为明显，这些因素会对传感器产生干扰。通过传感器感知所在环境果树和障碍物位置等信息时，为了将数据映射到地图中，需对传感器数据优化处理，处理后数据与实际环境会存在一定偏差，将会对机器人的路径规划产生影响。因此机器人作业时应与作物和障碍物保持安全距离，实时避开动态障碍物，在保证安全的前提下，规划出一条可行性最优的路径，完成喷雾工作[2-3]。

针对路径规划领域的研究，A*算法[4]、Dijkstra 算法[5]、遗传算法[6]、蚁群算法[7]、人工势场法[8]以及DWA算法[9]等已经被广泛应用于无人驾驶。其中，A*算法是静态网络中求解路径最优的直接搜索方法，同时具有根据搜索方式设计启发函数的优点。但是A*算法也存在着规划的路径拐点大、平滑性差、算法运行时间长、单向递进式搜索的缺点[10]。孔继利等[11]通过加入双向搜索机制并优化评价函数，提高了A*算法搜索效率，缩短了路径长度。焦铮等[12]通过改进A*算法函数并引入避障冗余距离，同时结合质量评价函数，实现全局—局部融合的砾石土均匀摊铺路径规划。

DWA 算法广泛用于移动机器人的局部路径规划，具有良好的动态避障能力，适用于动态环境中的移动机器人路径规划，但是在复杂的大规模环境中存在陷入局部最优的死循环，导致路径规划失败。刘建娟等[13]通过调节A*算法启发函数权重，设计了路径节点优化算法和DWA 算法评价函数，实现路径的全局最优和动态避障。庞永旭等[14]通过将障碍物占比引入传统A*算法，改进A*算法评价函数，提高其在不同环境下的搜索效率，然后改进子节点选择方式，融合动态窗口法，实现复杂环境下机器人的实时动态避障。王子静等[15]通过动态改变步长方法设计了一种改进的快速平滑A*算法，并在DWA 的评价函数中增加路径偏差项，将全局规划与局部规划相融合，实现了动态环境下无人艇的路径规划。劳彩莲等[16]通过对关键点选取策略进行改进，结合动态窗口法，构建路径评价函数，减少了路径多余转折点，提高了路径的平滑性。

本文对果园喷雾机器人作业需求进行研究。针对以上不同的算法改进，结合果园实际环境，本文提出了一种改进A*和DWA 算法的全局路径规划方法。首先对A*算法启发函数进行优化，确保机器人在躲避障碍物的前提下能沿着果树行间中心线行走，接着加入引导点实现果园的初步全局路径规划，然后通过DWA 算法精准的追踪初步全局作业路径，在规避作业过程中的动态障碍物前提下，实现减少路径冗余点和提高作业安全性的目的。

1 果园喷雾机器人环境模型建立

在果园进行路径规划时，首先需要建立简化后的果园环境地图[17]。为了便于实验研究，本文在进行地图边界、果树和障碍物构建时，考虑到机器人行驶的安全距离，将果树和障碍物点化后适当膨胀，以保障机器人作业时的安全。在果园环境模型建立时将果园喷雾机器人简化为一个质点，果树与障碍物也进行简化，近似认为单个果树和障碍物所占面积相等。模拟果园环境构建的30 m×30 m栅格地图如图1所示。图1果园地图中的果树位置和作业生产中临时的一些障碍物统一用黑色正方块表示。此外，把地图分为两部分，一部分为施药区域，另一部分称为地头调转区域。在图1建立的栅格地图中共有8 行果树，两果树行间组成一块施药区域，共计7块施药区域。施药区域已在图中用红色矩形框包围，施药区域统一用T1 表示。地头调转区域为地图中施药区域之外的区域，如图1中绿色矩形框包络的区域，地头调转区域统一用T2表示。

图1 模拟果园环境地图构建Fig.1 Construction of simulated orchard environment map

2 基于改进A*算法的路径规划

A*算法是在静态环境中求解全局最短路径的寻路算法，是一种快速高效的启发式算法，属于最佳优先算法。但是A*算法在复杂果园环境规划的路径不适合果园喷雾机器人作业。因此本文通过对传统A∗算法的评价函数进行改进，再通过加入引导点，引导A*算法迭代，实现果园喷雾机器人初步全局路径规划。

2.1 传统A＊算法

A*算法从起点开始搜索，通过代价函数对邻域周围8个方向的点代价进行计算[18]。代价函数的运动搜索方向如图2所示。红色的点为当前位置p，箭头所指方向为可移动方向，由图中可以看出移动过程中最小转角为π/4，单次移动最小距离为1或

图2 运动搜索方向Fig.2 Motion search direction

A*算法通过函数表达式计算运动搜索方向节点的代价值，然后进行选取代价最小的节点作为下一步移动的节点，重复此操作，直至找到目标点。A*启发函数如下：

其中，f(p)为从起点S到终点E的预估代价函数；g(p)为当前点p到起点S的实际代价函数；h(p)为当前点p到终点E的估计代价值。在搜索路径过程中，p点每移动一个单位，g(p)值将增加1 或者。由于果园环境较为复杂，果园喷雾机器人在移动过程中并不能完全沿着相对径直的线路行走，因而当h(p)采用曼哈顿距离估计从当前节点p到终点S距离时，h(p)值会比实际移动距离大，采用欧几里德距离得到的h(p)会比实际移动距离小。本文采用了一种介于两者之间，更能准确估算节点p到终点距离的相对复杂的对角线距离，其公式可表示为：

其中，Ex为终点E在栅格地图的横坐标，Ey为终点E在栅格地图的纵坐标，E为当前点p所在栅格地图的横坐标，py为当前点p在栅格地图的纵坐标，D为移动1个单位所消耗距离代价。A*算法路径规划过程中，每次搜索下一步路径均需依靠预估代价函数进行估值，从而规划出相对最优的下一步节点，直至抵达终点。通过对预估函数进行改进，可以得到一条与预期接近的路径，进而满足果园喷雾机器人作业需求。

2.2 A＊算法优化

针对果园环境的施药任务要求，需要对A*算法预估函数进行优化：确保在果树施药区域时，果园喷雾机器人能够沿着果树行间中心线施药；当在地头调转区域，能够规划出最短路径以进行施药工作，以此减少不必要的能量、时间浪费。实现方式是通过加入施药区域代价函数w(p)计算施药区域中每个节点距离最近的果树或者障碍物位置的比较复杂的对角线距离的倒数与施药区域代价增益的乘积。由此可知，当施药区域代价增益一定时，与果园喷雾机器人距离最近的果树相对位置越接近，施药区域代价就越大。因此，可以利用控制施药区域代价函数的增益可以实现控制果园喷雾机器人的大概走向。此外，在地头调转区域无需进行w(p)计算，即w(p)=0，只需果园喷雾机器人寻找到最短路径即可。优化后的启发函数为：

其中，ox为距离当前点p直线距离最近的果树或障碍物的横坐标，oy为距离当前点p直线距离最近的果树或障碍物的纵坐标，∂为施药区域代价增益，T1为施药区域，T2为地头调转区域。优化前后的A*算法在14×14模拟果园地图规划出的仿真路径如图3、图4所示。

图3 A*算法Fig.3 A* algorithm

图4 优化的A*算法Fig.4 Optimized A* algorithm

图3、图4 中设定S点为路径规划起点，E点为路径规划终点。传统A*算法路径规划耗时1.043 4 s，距离代价11.242 6 m，加入施药区域代价函数的A*算法路径规划耗时1.055 8 s，距离代价12.242 6 m。比较两种不同预估函数的路径规划，两种方法耗时几乎相等，优化的A*算法所规划出的路径在果园场景作业更为适合。故加入施药区域代价函数的A*算法路径规划优于传统A*算法路径规划。

2.3 改进A＊算法

预估函数优化后的A*算法在路径规划中，只能在果园中实现单行作业。一般果园种植规模较大，单行作业是远远不能满足任务要求的。本文在加入施药区域代价函数的基础上，在保证算法区域有效性的基础前提下，加入多个引导点，通过A*算法迭代实现全局路径规划。算法以第一个引导点S为起点开始规划路径，直至寻找到第二个引导点，再以第二个引导点为起点，寻找第三个引导点，依次迭代，直至寻找到终点E，实现果园全局作业。通过加入引导点迭代的改进A*算法流程如图5所示。

图5 改进A*算法流程Fig.5 Improved A* algorithm process

3 DWA算法

DWA算法是一种考虑机器人动力学约束的局部路径规划算法。它采样了机器人运动过程中的多组速度，模拟出机器人在一定时间内在该速度下的轨迹，对轨迹进行评价，选取评价最高的一组轨迹。DWA 算法根据机器人的性能在一个动态范围内限定速度采样空间。因此DWA算法可以用于在复杂的果园环境中实时动态避障，以保障机器人安全作业，但是该算法不能保证规划出的路径全局最优，导致路径不利于机器人作业。本文通过调整DWA 算法的评价函数系数，实现机器人在实时动态避障的前提下更精准的对改进A*算法的初步全局路径追踪。

3.1 机器人运动模型

在动态窗口中首先要建立机器人的运动学模型，机器人的运动状态可由机器人的线速度vt、角速度wt表示。(vt,wt)代表一组运动轨迹。在时间间隔Δt内，设机器人做匀速直线运动，机器人只能进行前进和旋转，不能全向运动。在很短的时间间隔内，可以将运动轨迹近似为一段直线，只需要将Δt的位移增量累计求和，运动学模型可表示为：

3.2 速度采样

在速度(vt,wt)空间中，存在多组速度，根据机器人的硬件性能和环境等多种限制可以得到速度约束。

机器人自身速度大小限制：

机器人受电机性能影响，加速度会受到限制，机器人速度约束：

式中，vc、wc表示当前速度，va、wa表示机器人最大加速度，vb、wb表示机器人最大减速度。

机器人安全距离约束：为了保证机器人运动的安全性，当机器人遇到障碍物前可以及时停止运动，需要对机器人速度进行约束，机器人运动学约束模型为：

式中，dist(v,w)是(v,w)对应轨迹距离障碍物的最近距离。

3.3 评价函数

在采样的速度中，机器人有多组采样轨迹，不同的采样轨迹可以规划出不同路径，需要用一组评价标准对多组轨迹进行评价。评价标准函数如下：

式中，heading(v,w)为方位角评价函数，表示当前速度下轨迹与目标点的方位角偏差，dist(v,w)为轨迹与静态障碍物的最近距离，velocity(v,w)为当前速度大小的评价函数。σ为平滑系数，α、β、γ为各项的加权系数。

4 算法融合

由于果园规模庞大，改进的A*算法只能规划出果树行间中心线路径，但机器人很难实现全局覆盖作业。通过对改进的A*算法加入引导点，引导改进A*算法规划出初步全局路径。为了更好地完成机器人喷药作业，提取初步全局路径关键节点，引导DWA 算法在含有动态障碍的果园实现全局作业。通过改进A*和DWA 算法结合，兼顾全局路径规划和局部路径规划的优点，实现全局果树行间中心线作业和实时动态避障。融合算法具体实现步骤如下：

（1）将机器人所获数据处理后建立果园环境地图，选定起点和终点。

（2）在地图中选取节点作为改进A*算法引导点。

（3）改进A*算法通过启发函数计算出从起点到第一个引导点路径，接着计算出第一个引导点到第二个引导点路径，依次迭代，直至终点，规划出初步全局路径并储存。

（4）在地图中加入动态障碍物，验证融合算法躲避动态障碍物能力。

（5）提取步骤（3）规划的初步全局路径关键点，作为DWA算法的子目标点。

（6）初始化DWA算法初始参数，根据评价函数选取最优轨迹，然后按照最优轨迹移动，追踪子目标点。

（7）判断是否到达终点，若未到达终点，继续执行步骤（5），直至到达终点。

5 仿真研究

为验证改进A*算法与DWA 算法路径规划的有效性，在环境为30 m×30 m的模拟果园的栅格地图上进行仿真实验，如图6，起点S（3.5，2.5），终点E（28.5，28.5），障碍物覆盖率约为25.8%。在模拟果园环境地图中，机器人在静态地图中首先通过A*算法进行全局路径规划，然后在模拟果园地图随机加入动态障碍物，如图6（c）的红色正方块，验证融合算法躲避动态障碍物能力。传统A*算法、改进A*算法与融合算法进行仿真验证，规划的路径如图6（a）~（c）所示。图中S点为机器人起点，E点机器人为终点，图6（b）中G点为加入的引导点。

图6 三种算法仿真结果Fig.6 Simulation results of three algorithms

对比传统A*算法，改进的A*算法在果树中心线没有障碍物时，在工作区域中心线行走路径100%，且能实现对果园环境进行全局路径规划，但A*算法存在着无法躲避地图中的动态障碍物的问题，不能保障机器人的安全作业。融合算法在全局路径的基础上进行路径规划，可以进行实时避障，高效地追踪全局路径。本文算法输出的机器人控制参数如图7（a）~（c）所示，输出结果为机器人姿态、线速度和角速度参数。

图7 机器人控制参数Fig.7 Robot control parameters

在30 m×30 m 果园环境地图中，对比改进A*算法和融合算法的路径及路径数据，转折点个数从20 个减小到14 个，转折点优化了30%，累积转弯角从2 250°减少到1 409.3°，累积转弯角度优化了36.17%。相对于传统A*算法，融合算法能够根据周围环境做出速度匹配，保证安全与高效，且规划出的路径平滑，路径曲率连续，更利于机器人的动力学控制。机器人在规划时实时进行姿态、线速度和角速度输出，方便对机器人状态进行监控与控制。通过以上数据和任务需求对比，融合算法在工作任务限制的前提下，减小了距离代价，提高了机器人工作的有效性和可靠性。整体来说，本文算法兼顾了A*算法与DWA 算法的优点，弥补了A*算法与DWA算法的缺陷，能够满足果园喷雾机器人的作业需求。

6 结论

本文针对果园喷雾机器人在果园环境作业这一情景，提出了一种基于改进A*与DWA算法的全局路径规划方法。在A*算法启发函数中加入工作区域代价函数，使果园喷雾机器人能够规划出沿着果树行间中心线路径，地头调转时，寻找最短距离进行调头；通过对栅格地图中加入多个引导点，引导A*算法进行迭代，实现初步全局规划；最后，DWA 算法对初步全局路径追踪，实现对果园全局的规划和动态障碍物的实时躲避，保障喷雾机器人作业的安全，提高了作业效率。将改进A*算法与DWA算法进行融合，通过仿真实验分析比较，改进的A*算法与DWA 算法能够规划出果园喷雾机器人在果园环境的工作路径，算法具有较好的实时性，更高的安全性，规划出的路径平滑度较高，更适合果园喷雾机器人作业。