基于A-star算法与DWA算法的小型农用运输车智能避障系统研究

安燕霞，黄 凯

（晋中信息学院，山西 太谷 030800）

矮化密植果园劳动环境差、人工劳动强度大，化肥、工具和果实等运输全靠人工作业，现有的大型农业设备无法进入田间作业，且在运输过程中，矮化密植果树对人的视线遮挡较多，因此对小型农业运输车需求不断增加。但由于果园环境复杂，为避免小型农用运输车在正常工作时出现碰撞现象，要求小型农业运输车对道路上存在的各种障碍物进行感知与识别，实现路径规划与其主避障。本文设计侧重于小型农用运输车的路径规划与避障系统的研究，在小型农用运输车上加上路径规划与自动避障模块，使得小型农用运输车在工作过程中时可以根据指令完成路径规划，得到到达目的点的最优策略，并在移动过程中检测环境的障碍物，做出相应的避障动作。

1 小型农用运输车智能避障总体设计思路

本文研究的小型农用运输车主要满足在矮化密植果园环境中，自动避障、保护果树，特别是在拐弯处、果树密集的地方。结合系统需求分析，小型农用运输车智能避障系统总体方案设计见图1。由图1 可知，小型农用运输车智能避障系统由环境感知模块、路径规划与避障模块、运动感知模块、运动控制模块、人机交互模块组成。

图1 小型农用运输车智能避障系统设计思路Fig.1 Design of intelligent obstacle avoidance system for electric luggage compartment

环境感知模块通过IMU、里程计、单线激光雷达等传感器进行信息读取，获取周围环境信息，判断小型农用运输车前进方向是否存在障碍物。IMU、里程计、单线激光雷达的作用在6 m 范围内，进行360°二维平面扫描，生成平面点云图，推算小型农用运输车行驶的轨迹以及其在环境的位置。

路径规划与避障模块读取预设的全局地图以及用户输入的目标点信息，完成从起始点到目标点的无碰撞路径规划。当行驶路径上存在障碍物时，进行局部路径规划，生成绕开障碍物的控制指令。

运动感知模块读取加速度计、里程计、罗盘等传感器的数据，计算得到小型农用运输车的位姿信息，为路径规划与避障提供输入信息。

运动控制模块是控制小型农用运输车运行的模块。本文选用2iMXRT1021 微控制器来根据接收到的控制指令，控制电机的运行。

人机交互模块提供用户选择环境模式。在用户设定目标点后，小型农用运输车通过智能避障系统内部运算推演出一条最适路线并自动行驶到目标位置；在自主模式下，小型农用运输车将跟随用户移动。本文主要研究在模拟环境中对障碍物的线路规划避障分析。

2 避障算法研究

A-star算法是基于Dijkstra改进的算法，在起点与终点之间距离较远时，全局搜索的距离和方向较Dijkstra 算法更有效率，减少了路径搜索的时间。A-star 算法主要是计算评估网格中每个节点的最佳值，即代价值，这里设定一个等式为F=G+H，其中G代表从起点到指定栅格点的代价值，一般是所走的实际值；H代表从终点到指定栅格点的估算代价值。估算H的公式有多种，比如曼哈顿距离、欧几里得距离等。为了简化计算，在该仿真实验中，选取的是曼哈顿距离（d=|x1-x2|+|y1+y2）|。A-star算法的流程见图2。

避障系统最重要的是路径规划与自主避障，路径规划软件是在给定全局地图，在给定起始点与目的点的情况下，采用栅格法构建运动模型路径规划的环境，对小型农用运输车进行路径规划，避开环境中的障碍物，得到最优路径。

A-star 算法是人工智能方面采用最广泛的路径规划算法，尽管它在正常情况下只有较优解，而不是唯一的解，但因为其算法效率高，应用较为广泛，因此在对于小型农用运输车路径规划采用A-star算法。

路径规划的实现主要基于A-star 算法实现，本文通过激光雷达等单片机获取相关信息编入ROS 平台的Gazebo 软件包，写入代价函数并进行运算。

2.1 A-star算法

2.2 路径规划

避障问题属于局部的路径规划，在无法预测的障碍物的动态的环境下，给定起止点和目标点，让其识别障碍物并完成自主导航。动态窗口算法（dynamic window approach，DWA）由于计算复杂度低、采样时间短、轨迹空间小、局部路径可以完成离线计算，因而在局部路径规划中被采用的较为频繁，因此本文自主避障也选择DWA算法。

本文采用DWA 算法进行动态轨迹分析，通过在速度空间内采用多组不同的线速度和角速度来模拟运动模型所产生的运动轨迹建立评价函数，从不同运动轨迹中选取评分最高的轨迹，MATLAB 平台下编写DWA 算法程序，通过改变障碍物的位置、数量、大小对避障过程进行仿真。

DWA 算法是当前在局部路径规划中经常使用的一种算法。它的主要思想是从运动模型当前所处的状态出发，在速度空间(v,w)内通过采用多组不同的线速度和角速度来模拟运动模型所产生的运动轨迹，再建立起评价函数对不同的运动轨迹进行评分，从中选取处评分最高的轨迹，该轨迹所对应的线速度和角速度在作为下一时刻的速度状态来继续引导运动模型运动。

在模拟小型农用运输车的运动轨迹时，需要知道小型农用运输车的运动模型，常用的运动模型主要是2种，分别是在相邻时间内沿直线行走的运动模型以及在相邻时间内沿圆弧行走的运动模型。考虑到小型农用运输车的使用环境，本文采用沿圆弧行走的运动模型见图3。

图3 小型农用运输车的运动模型Fig.3 Motion model of small farm transporter

运动模型的线速度与角速度之间关系见式（1）：

式中：v为运动模型的线速度；w为运动模型的角速度；r为圆弧半径。

根据运动模型的示意图可以得到相邻时间间隔的位移增量和下一时刻的位置，位移增量见式（2）：

式中：vi为运动模型当前运动线速度；wi为运动模型当前运动角速度；(xi,yi)为运动模型当前时刻的位置；θi为运动模型当前的速度与水平线所呈的夹角。

下一时刻运动模型的位置见式（3）：

式中：vi为运动模型当前运动线速度；wi为运动模型当前运动角速度；(xi,yi)为运动模型当前时刻的位置；(xi+1,yi+1)是运动模型下一时刻所处的位置；θi为运动模型当前的速度与水平线所呈的夹角。

相邻时间内沿圆弧行走的运动模型将角速度加入考虑，使得运动模型的运动方向不断发生改变并进行调整，与相邻时间内沿直线行走的运动模型相比较而言，与实际运动轨迹之间的误差较小。

DWA算法的流程见图4。

图4 DWA算法流程图Fig.4 DWA algorithm flow chart

3 算法验证

3.1 A-star算法验证

环境感知模块根据接收的数据给出栅格地图，即画出障碍物的位置、起始点、目的地，有网格的为障碍物，左下为起始点，右上为目的地点，并赋予运动模型前后左右4个移动方向，按照A-star 算法，设计路径规划程序。运行程序，在栅格地图上得到一条从起始点到达目的地的最佳路径，见图5。

图5 栅格地图及由A-star算法得到最优路径Fig.5 Grid map and optimal path obtained by A-star algorithm

3.2 DWA算法验证

在MATLAB 平台下编写DWA 算法程序，对避障过程进行仿真。通过改变障碍物的位置、数量、大小，总能得到最优的避障路径。DWA算法仿真见图6。

图6 DWA算法仿真结果Fig.6 Result of DWA algorithm simulation

由图6的DWA算法的仿真结果能够看出，随机改变障碍物的位置、数量、大小等，DWA 算法都可以帮助运动模型规划出最优路径，并使其达到目的地。

4 虚拟环境下避障系统验证

ROS 支持多种通讯类型和编程语言，易于扩展，已逐渐成为智能运动模型开发的标准平台。本文采用ROS 平台对整个系统设计进行虚拟现实模拟验证。

环境感知软件采用ROS 发布的SLAM 下的gmapping 功能包设计，gmapping 功能包提供了使用激光雷达数据构建二维栅格地图的功能。环境感知软件订阅tf 与scan 话题，并发布tf、map_metadata 以及map 话题。其中，tf 话题包含地图与里程计之间的坐标变换信息，map_metadata 描述了发布的地图属性，map则是周期性发布的栅格地图。

在Gazebo软件中搭建运动模型以及虚拟环境模型，并载入避障算法，对避障系统进行功能验证。最后在虚拟环境中，运动在安装各个传感器与模块的情况下，也能够独立完成路径规划与避障功能，见图7。

图7 ROS建图效果Fig.7 ROS drawing effect

5 结语

针对矮化密植果园运输问题，本文研究了小型农用运输车的路径规划与避障问题。研究采用A-star 算法进行路径规划，DWA 算法实现局部避障，为了验证2 种核心模块的功能，本文使用MATLAB 软件，对A-star 算法与DWA 算法进行算法仿真，使用仿真结果验证了该2种软件模块的可行性。