基于改进蚁群算法的多农业机器人路径规划研究

罗智杰，黄子涛，许嘉志，潘仲宇，曹 亮，刘双印

（1.仲恺农业工程学院信息科学与技术学院，广东 广州 510225；2.仲恺农业工程学院智慧农业工程技术研究中心，广东 广州 510225；3.广州市农产品质量安全溯源信息技术重点实验室，广东 广州 510225）

0 引言

进入电子信息化时代，农业领域已向机械化、智能化、信息化方向靠拢。农业机器人已成为一种具备路径规划,环境障碍感知和能够完成一系列动作行为等功能为一体的智能化农业设备。现阶段，国内外现有的农业机器人的主要类型有播种机器人、采摘机器人、果蔬分选机器人、除草机器人等[1,2]。为保障机器人能够适应复杂多变的农业环境，其主要技术主要包括定位导航技术、运动控制技术、传感器技术、图像识别技术[3]，要求农业机器人具备较高的环境感知、定位和避障的能力[4]。对于企业用户来说，农业机器人的效率和路径合理度是一个重要的指标。因此如何合理规划路径是农业机器人研究的热点之一。近年来，有关机器人路径规划算法的研究不断被提出和研究，其中几种典型的算法有：人工势场法[5]、蚁群算法[6]、模拟退火法[7]、粒子群算法[8]、A*算法[9]等。

在实际农业场景中，尤其是对大型农场而言，单靠一个农业机器人运作处理是难以实现的，而多个机器人作业又涉及到协作问题，因此有必要考虑多个机器人的路径规划。为了解决蚁群算法在多移动机器人路径规划应用中的不足，本文在基本蚁群算法的基础上，对启发信息函数做出了改进，同时提出死锁问题的解决方案，既保证了算法的速度，又避免了算法的提前收敛。另外，针对未知运动状态和已知运动状态的障碍物，本文分别讨论了动态窗口搭配区域膨胀以及正碰、侧碰两种避碰策略，进一步加强了机器人应对多变的室内农业环境的能力。本文所研究改进的算法对于用在障碍物较多，路径规划较复杂的大型农场中有较好的效果。本文最后也讨论了多个机器人在同一环境下的路径规划问题。

1 空间模型建立

本研究首先采用栅格法[10-12]进行环境地图的创建，以此来模拟应用场景，规格为30×30。对着环境地图从上到下，从左到右分别进行1～900 的编号，如图1（a）所示。在这个环境中，黑色栅格表示静态障碍物，白色栅格表示机器人的自由可行区域，分别如图1（b）所示。建立工作空间环境时，把实际的障碍物经过膨胀处理后映射到栅格地图中，因此可认为栅格地图中的边界均为安全边界，并将机器人视为质点。

图1 环境的栅格地图

环境地图中各栅格的中心点坐标由其对应的栅格序号（xi，yi）决定，其算法为

式中 i为栅格序号；Nx为每行栅格的个数；Ny为每列栅格的个数。

根据实际的农业场景情况，规定机器人的行走方向为西北、北、东北、东、东南、南、西南、西8个方向，且每次前进一个步长的距离。如图2 所示。

图2 机器人行走方向

2 改进的蚁群算法研究

2.1 基本的蚁群算法

蚁群算法[13]是一种应用于组合优化问题的启发式搜索算法，在解决离散组合优化方面具有良好的性能，最早应用在旅行商（TSP）问题上。通常，我们用(t)表示t 时刻蚂蚁k 选择从栅格i 转移到栅格j 的概率，也称随机比例规则。信息素浓度τxy(t)和局部启发信息ηxy(t)共同决定(t)，其算法为

蚂蚁k 下一步允许选择的栅格的集合为

式中 m为蚁群中蚂蚁的数量，只；禁忌表tabuk(i)记录了蚂蚁k当前所走过的栅格；α为信息素启发因子；β为期望值启发因子；τij(t)为t时刻在栅格i和j中间残留的信息素，初始时刻，各条路径上的信息素相等，即τij(0)=C(const)。

启发信息函数ηij(t)计算公式为

蚂蚁完成一次遍历后，需要对各条路径上的残留信息素作消散处理，避免信息素过多影响遍历结果。更新机制为

其中，式(6)表示第k 只蚂蚁留在路径（i,j）上的信息素增量，Q 为常数，Lk为优化问题的目标函数值，表示第k 只蚂蚁在本次循环中所走路径的长度；式(7)表示每只蚂蚁在栅格i 和j 路径上增加的信息素之和；ρ为信息素挥发系数。

值得注意的是，现阶段，在不同的数学模型（如蚂蚁数量系统、蚂蚁密度系统）中，对于Δτkij(t)给出的定义略有区别。

2.2 改进蚁群算法

2.2.1 死锁回退策略

基本蚁群算法存在的一个问题是容易过早收敛，导致规划的路径得不到较优解。另外，在复杂的空间模型中（如大面积农场），蚂蚁容易陷入死锁问题（如图3），造成算法停滞。一种解决的办法是采用早期死亡蚂蚁策略[14]，即当蚂蚁陷入死锁时，为防止程序停滞，结束本只蚂蚁的搜索，并且不对当前这只蚂蚁做任何处理，即不将该蚂蚁的搜索路径、留下的信息素等信息进行保留。这种方案会导致有效蚂蚁的数量在减少，此外还可能会导致蚁群算法得不到较优解。

图3 蚂蚁陷入死锁现象

考虑到室内农业应用的实际情况，本文利用死锁回退策略[15,16]，该策略通过牺牲一些算法时间来得到路径的较优解。蚂蚁死锁回退策略具体为：若蚂蚁陷入死锁，则回退至上一次所在的栅格，并将蚂蚁陷入死锁的所在栅格临时标记为静态障碍物，防止蚂蚁再次陷入同样的死锁问题。在回退处理之后，再次判断该只蚂蚁在当前栅格是否还是陷入死锁。若是，则再次进入死锁回退的策略当中；否则，进入下一个栅格的选择步骤。在本文的研究试验中，我们将各只蚂蚁视为相互独立，即它们在陷入死锁问题后所产生的静态障碍物只对自己有效，一只蚂蚁完成一轮搜索后，其产生的临时静态障碍物也随之消失。这种方法能够有效避免早期蚂蚁陷入死锁，产生过多的静态障碍物导致算法提前收敛，规划出不理想的搜索路径。在空间越大的模型（如大面积的农场）中，这种效果表现的就越显著。

2.2.2 启发信息函数改进

此外，本文对启发信息函数做了一些针对性的优化，即将每一栅格到目标栅格的直线距离的倒数作为启发信息函数，减少了一定的计算量，提高算法速度。修改后的启发信息函数表示为

式中 i为当前所在的栅格；j为可前往的栅格的编号；e为目标栅格的编号。

2.2.3 路径规划步骤

本算法的程序流程如图4 所示，具体算法步骤如下。

图4 本研究的算法程序流程图

Step1：根据实际农业应用场景，利用栅格法对整体空间进行二维平面建模。

Step2：对算法的相关参数进行初始化操作。

Step3：根据轮盘赌法选择下一个相邻的自由栅格。

Step4：更新禁忌表，将当前所在栅格标记为已去过栅格，避免该栅格被本只蚂蚁再次选择。

Step5：当本次迭代的所有蚂蚁完成路径搜索后，对所有路径上的信息素进行更新操作。

Step6：如果迭代完毕，则输出算法结果；否则，重置禁忌表，并跳转到Step3。

2.3 算法的试验数据对比

为了对两种算法进行比较，在Matlab R2018b 平台上进行仿真试验。试验中算法的各项参数分别为α=1.5，β=7，ρ=0.3，Q=5，蚂蚁数量为50 只，最大迭代次数为100。图5—图8 分别为采用基本蚁群算法和改进蚁群算法对单个机器人进行路径规划的收敛曲线趋势图及运动轨迹图。

图5 基本蚁群算法的运动路径

图6 改进蚁群算法的运动路径

图7 基本蚁群算法的收敛曲线

图8 改进蚁群算法的收敛曲线

值得注意的是，从本次试验比较结果来看，虽然两种算法都能得到给定空间模型的最优解，但从多次试验数据观察发现，改进蚁群算法得到优解的概率要高于基本蚁群算法，显得更加稳定。表1 给出了在参数相同的情况下两种算法的各10 次试验数据统计。从表1 中的数据可以看到，改进蚁群算法10 次试验得到的平均路径长度要优于基本蚁群算法得到的平均路径长度，反映了改进蚁群算法的稳定性要高于基本蚁群算法的稳定性，且改进蚁群算法出现的最差路径长度接近于平均值，没有出现像基本蚁群算法那样有较大的波动。

表1 10次试验数据统计

3 讨论

3.1 动态环境下单个机器人的路径规划

在实际的室内农业环境中，为保证蔬菜等农产品的精准培育，智能监测设备的数目会多于室外农业环境。因此其不可预测事件发生的概率也较高。例如，为了改善室内农业的监测系统，会将技术更加先进的传感器设备添加到室内农业场景中，而这个设备相对于系统之前建模的空间来说，是未知的，对于室内农业机器人而言，新添加的传感器设备则视为一个未知的动态障碍物。因此，为了提高农业机器人的有效运作，未知物体的存在因素是必须考虑的。假设未知物体的运动状态是不可预测的，此时，需要借助机器人的传感器设备完成对动态障碍物的检测，必要时采取预先设定的避障策略进行避障处理。本文采用的是基于滚动窗口策略对动态障碍物进行检测[17,18]。机器人每到达一个栅格位置，进行一次窗口检测。如果检测到动态障碍物在滚动窗口范围内，则以动态障碍物当前位置为中心做膨胀处理，即将其附近的栅格设为灰色障碍物（不包括机器人所在栅格，如图9）。如果膨胀区域占据了机器人的原路径，则在原路径的基础上查找机器人当前所在栅格之后的第一个未被占据的栅格，并以该节点为目标节点，当前节点为源节点，做局部路径规划，将该局部路径添加到原路径中，形成机器人的实际行走路径。

图9 动态未知障碍物膨胀处理

本试验在环境地图及各项参数依旧不变的情况下，在栅格地图空间中设置了两个动态障碍物，这两个动态障碍物相对机器人而言其运动状态是未知的。如图10（a）所示，两个星号分别代表两个动态障碍物，红色质点代表机器人，红色虚线表示机器人初始规划的路径。如图10（b）所示，蓝色实线代表机器人实际的行走路线。图10（c）表示当机器人遇到第一个动态障碍物时，且该动态障碍物在滚动窗口范围内，对动态障碍物进行膨胀处理。图10（d）表示机器人触发了避障策略，进行局部路径规划动作。图10（e）表示机器人遇到第二个动态障碍物，且该动态障碍物与机器人相邻，在对其作膨胀处理时，机器人所在的栅格不作处理，并进行局部路径规划。图10（f）、图10（g）表示机器人成功避开第二个动态障碍物后，继续按原路径前进。图10（h）表示机器人到达目的地，并给出了实际路线（蓝色实线）和初始路线（红色虚线）的对照。

图10 改进算法的动态避障仿真试验图

从试验结果可以看出，依赖本研究改进的蚁群算法，农业机器人可以顺利避开静态和动态障碍物，顺利的到达目标位置。并且对比默认路径与动态修改后的路径，其运动轨迹较为接近。因此表示局部规划的路径较为有效和稳定。

3.2 动态环境下多个机器人的路径规划

一般来说，单个移动机器人的工作能力是有限的，而多个移动机器人的协调工作能使得工作效率得到大幅度的提升。在室内农业的应用场景下，多个机器人各自规划的路线难免重合产生碰撞的风险，从而影响有序的正常工作状态。假设在各个机器人之间具备一定的通信能力的情况下，机器人彼此之间能够知道对方的运动状态（位置）。如果能够将动态窗口扫描到的机器人障碍物确定为已知运动状态的机器人，即可以采用更合理的避碰策略。因此在有限的空间范围内，如何让机器人相互之间避免碰撞是提高工作效率的关键所在。在本研究中，把机器人之间可以看作是已知运动状态的动态障碍物。但是与未知运动状态的动态障碍物的避碰策略不同，将不再采用膨胀的方法进行处理，而是根据运动方向判定两个机器人是否存在碰撞的可能性。根据实际应用调研发现，碰撞大致可以分为两大类：正碰和侧碰。

本文采用的一种判定是否碰撞的思想是：将机器人的当前节点和下一节点两点之间看作是一条线段，两个机器人则有两条线段，通过判定这两条线段是否相交，则可判定两个机器人下一步是否产生碰撞。实现这种思想的一种常用的方法是通过向量叉积来判断，这种方法判断线段是否相交需要完成两个关键步骤：快速排斥试验和跨立试验。

1）快速排斥试验。判断两条线段在坐标轴x 以及在坐标轴y 上的投影是否有重合。即判断一条线段中x 轴坐标较大的端点是否小于另一条线段中x 轴坐标较小的端点，若是，则说明两条线段必然没有交点。否则，同理判断y 轴坐标。

2）跨立试验。如果两线段相交那么就意味着它们互相跨立。如图11 所示，点A 和点B 分别在线段CD 两侧，点C 和点D 分别在线段AB 两侧。判断A点与B 点是否在线段CD 的两侧，即向量AD 与向量BD 分别在向量CD 的两端，也就是其叉积是异号的。同时证明C 点与D 点在线段AB 的两端，两个条件同时满足，则表示线段相交。

图11 两线段相交

当经过快速排斥试验和跨立试验判定两条线段是否相交后，如果不相交，则两个机器人按原路径继续前进。如果相交，则将机器人的当前节点和下一节点当作向量计算，通过比较两个向量是否方向相反，如果是则为正碰，如果不是则为侧碰。若为正碰，根据机器人的优先级决定优先级低的机器人重新规划路径，优先级高的机器人按原路径前进。若为侧碰，根据机器人的优先级决定优先级低的机器人等待一个步长，让优先级高的机器人优先通过。正碰时的局部路径规划：将下一个节点设为临时的灰色障碍物，以下个节点为目标节点，当前节点为源节点，进行局部路径规划。图12 给出了基于本研究算法的一次多个农业机器正碰试验演示。在本试验中，算法的各项参数分别是：α=1.5，β=7，ρ=0.3，Q=5，蚂蚁数量为50 只，最大迭代次数为100，红色机器人的优先级低于蓝色机器人。

如图12（a）所示，两个机器人各自从起点出发，且其初始路径正好反向重叠。图12（b）、图12（c）、图12（d）表示低优先级的机器人做出了临时的局部路径规划以避免碰撞。图12（e）表示两个机器人避开碰撞后顺利到达终点。

4 结语

本文在基础蚁群算法的基础上，提出了死锁问题的解决方案，并对启发信息函数进行了改进，在Matlab-R2018b 平台上进行仿真试验。针对室内农业复杂多变的场景环境，本文考虑了动态障碍物的存在和多农业机器人作业两个重要方面，提出相应的避碰策略，分别为机器人与障碍物之间的动态窗口扫描搭配区域膨胀和机器人与机器人之间的正碰、侧碰做出了针对性的处理。仿真试验结果证明，研究的改进蚁群算法在多农业机器人运作场景中能取得较好的运行效果，在有静态和动态障碍物的环境下，具备较好的稳定性与实时性。因此认为该研究可以推广应用在室内农业多机器人作业的复杂场景中。