农业机器人路径规划算法研究

成家骏，谢 锐，荆瑞俊

（1.山西农业大学 软件学院，晋中030800；2.中北大学 电气与控制工程学院，太原030051）

规模化农业已成为现代农业的主流形式，在此背景下，作物检测对人力物力消耗极大，成为现代农业发展过程中，亟待解决的问题。现阶段有多种算法可用于农业机器人的路径规划，比如A* 算法[1-2]、RRT 算法[3-4]、Dijkstra 算法[5]等。这些算法能找到较为可靠路径，但存在各种缺点。Dijkstra 算法进行搜索时，搜索以辐射状扩散，虽能找到最优路径，但遍历点过多，严重增加了算法时间复杂度；A* 算法在Dijkstra 算法基础上加入启发式函数， 有较强的目的性，但也存在不能保证最优路径的问题；RRT算法进行随机点连线搜索，难以在狭小空间找到最优路径。为使算法能够更好的适应复杂环境，随之出现许多改进算法，如文献[6]提出一种改进多步长的蚁群算法，用来提高算法搜索效率；文献[7]运用人工势场算法调整信息素量的方法提高蚂蚁避障能力。蚁群算法ACA（ant colony algorithm）是一种模拟蚂蚁觅食的启发式随机搜索算法[8-11]，具有自组织性、正反馈等优点，并在此基础上有较好的鲁棒性，是很多文献解决最短路径问题研究的算法之一。

1 农业机器人模型

1.1 激光雷达传感器

激光探测及测距LiDAR（LightLaser Deteetion and Ranging）系统中，激光器是雷达的辐射源。其工作原理是向目标发射探测信号，将接收到的从目标反射回来的目标回波与发射信号进行比较，可以获取目标物体距离、方位、高度、速度等参数的电子设备，被广泛用于机器人二维感知的检测传感器。

根据设备特征，八叉树搜索策略如图1 所示，建立仿真模型。

图1 八叉树搜索策略Fig.1 Octree search strategy

图中，灰色圆形代表机器人，在实际仿真工作中每步长移动的起点和终点均为栅格地图中栅格的中心点，也为机器人实际体积的中心点；白色圆环代表激光雷达扫描范围；黑色方块为膨胀后在栅格地图中机器人感知范围。采用八叉树搜索策略，机器人可以根据算法进行八方移动，解决了路线易发生偏移、转弯较多、路线长度增加等不足，使得栅格之间移动更加自由，路径搜索效率更高。

1.2 坐标变换与位姿模型

在三维空间中，由于移动机器通常在平面中运动，可以简化模型，从而建立二维固定的世界坐标系xOy；把小车看成三维空间的刚体，建立左手正交坐标系x′O′y′。小车在空间运动，从而产生坐标系的空间变换，运动学模型如图2 所示。

当机器人从点O（x，y）运动到点O′（x′，y′）时，有

图2 运动学模型示意图Fig.2 Schematic diagram of kinematics model

由式（1）（2），可推出从以坐标原点O 为原点的xOy 到x′O′y′的坐标变换，其矩阵形式为

其中

为本次旋转的旋转矩阵，再加上平移量，得

由上述推导可以得出，在固定不动的世界坐标系中，可以通过三维坐标系中任意2 个坐标系的旋转，平移得出位置变换，从而获得机器人在运动中的运动方向位置信息。但由于移动机器人在运动的过程中， 激光雷达必然会出现不可避免的误差，通常由回环检测、 噪声滤波来矫正传输回来的数据，达到更加精准的目的。

由于坐标变换与位姿模型中计算多为矩阵与向量计算， 故将经典算法中坐标表示绘制路径用quiver 二维矢量函数进行处理绘制出每一步的方向，并采用1.2 步长突出每一步的方向变化。

1.3 农田鸟瞰图模型建立

在路径规划的环境模型建立中，大多应用栅格法建立环境建模，因此在MatLab 环境下建立30 m×30 m 栅格仿真地图（如图3 所示），作为农田鸟瞰图模型。

图中，3×3 的灰格为农田，白格为通路；起点为左上角（0.5，19.5），终点为右下角（19.5，0.5），从起点开始到终点结束。

图3 仿真环境栅格地图Fig.3 Raster map of simulated environment

2 退火蚁群算法

经典蚁群算法， 模拟蚂蚁觅食的过程模型，由于正反馈机制的存在，在算法运行到一定程度时容易进入局部最优解的现象。在整个蚁群算法迭代完成后进行模拟退火的运行优化可能路径，在完成一轮蚁群算法迭代后，取精英蚂蚁进行节点段的平移和连接操作，并进行组合以产生新解，按照退火规则接受新解，从而完成模拟退火与蚁群算法的结合，完成基于模拟退火算法改进的蚁群算法。具体步骤如图4 所示。

图4 退火蚁群算流程Fig.4 Calculation flow chart of anneal ant colony algorithm

在此以退火蚁群算法为基础，加入机器人每次移动改变的位姿及坐标变换的角度变化，以及坐标信息的输出规则，并在算法模拟中用箭头来表示。假设，蚂蚁数量为n，蚂蚁从固定节点出发，为了保证算法比较迭代次数相同，如果经典蚁群算法迭代次数为m，则退火蚁群算法中，蚁群算法迭代次数为m/2，预设温度为蚁群算法运行2 次（蚁群算法→退火算法→蚁群算法→结束）。

3 试验与结果分析

该测试所用的操作系统为Windows 10，内存容量为8 GB，电源功率为120 W。为检验模拟退火算法对蚁群算法的优化效果， 通过MatLab 仿真软件将其与经典蚁群算进行对比试验，对比路径结果如图5 所示。由图可见，机器人路径沿左上→右下对角线基本对称， 但在图5a 中纵坐标30—25 处经典算法产生略微偏差，陷入局部最优解的情况。

图5 不同蚁群算法的路径Fig.5 Paths of different ant colony algorithms

退火蚁群算法与经典算法变化折线图如图6所示。由图可见，在50 次迭代前，两算法最短路径变化抖动都较为明显；在50 次迭代后，退火蚁群算法在略微抖动后趋于收敛，与抖动较为明显的经典算法形成较强的对比。

图6 退火蚁群算法与经典算法的变化趋势Fig.6 Change trend of annealed ant colony algorithm and classical algorithm

4 结语

通过深入研究退火蚁群算法在农业机器人路径规划问题，分析了通过激光雷达在算法中的膨胀处理，并在算法中加入了坐标及位姿变换相关内容，可以输出每次坐标旋转角度以及位置姿势的变化情况， 最后在MatLab 仿真环境中进行了退火蚁群算法，验证了基于退火蚁群算法在农业机器人路径规划方面， 可以极大程度地改善算法的收敛情况，并有效缓解经典算法陷入局部最优解的问题。