基于改进的烟花-蚁群算法和B样条曲线的农业机器人路径规划

王红君, 叶 荣*, 赵 辉,2, 岳有军

(1.天津理工大学天津市复杂系统控制理论与应用重点实验室， 天津 300384; 2.天津农学院工程技术学院， 天津 300384)

近年来，随着人工智能的积极推行和迅猛发展，对包括农业在内的许多行业造成了深远的影响和巨大的挑战，作为精准农业的支柱力量，人工智能技术已经深深地渗透入农业生产的每个环节，将智能算法和机器人路径规划技术结合运用具有较大的应用价值和广阔的研究前景，也是实现机器人智能作业的重要基础[1]。

目前应用较多的智能算法包括粒子群算法、遗传算法、人工蜂群算法等[2-3]。为了弥补单一算法的缺点，文献[4]提出了ACO-PSO(ant colony-particle swarm optimization)混合算法，提高了计算效率和搜索精度。文献[5]将GA算法和PSO算法相结合，提高粒子多样性，增强了全局搜索能力，为机器人路径规划问题提供了一种有效的方法；王学武等[6]、Douan[7]采用Levy飞行与PSO算法相结合对机器人进行全局路径规划时，Levy飞行策略能够扩大搜索空间，增加粒子种群多样性，提高寻优能力，但因其具有随机性，耗时就较长；刘俊等[8]提出的PSO-ACO(swarm-ant colony algorithm)融合算法改善了粒子群算法中粒子群种类少、易早熟的现象，同时解决了蚁群算法中初始化信息素匮乏的问题；孙纯哲等[9]在传统蚁群算法融合人工势场思想，引入引力概率函数作为启发因子，加快收敛速度；张琦等[10]在蚁群算法引入交叉操作后调整α、β和ρ的值，可以提高算法搜索能力上述融合算法，虽然在一定程度上解决了机器人路径规划中遇到的基本问题，但大部分都是基于耗时长短或者搜索路径长度而建立的评价指标，忽略了如平滑度、能耗等路径规划过程中的实际问题。鉴于此，现对烟花算法(fireworks algorithm，FWA)进行了改进，利用烟花之间多点同时爆炸和子代烟花之间充分的交互机制，建立密度峰值火花和探测火花后结合蚁群算法对路径进行搜索，使用改进后的IFWA-ACO(improved fireworks-ant colony algorithm)算法和B-spline函数来探索适合机器人智能作业的平滑路径。

1 农业机器人路径规划问题描述

1.1 二维平面下的农业机器人模型

在真实的复杂环境中，不是所有的客体都能用简单的数学语言来描述。为了达到探索性和普适性，对于大规则几何形状的农业机器人，由于机械臂较大，特征点较多，不易确定，难以满足实时性要求。忽略农业机器人的大小，将其抽象为质点，则满足在二维平面的动力学方程，即

(1)

式(1)中：qi代表机器人的位置；pi代表机器人的速度；ui代表机器人的输入机制。

1.2 农业机器人作业流程

农业机器人在农田、果园、农场进行作业的流程如图1所示。作业前首先要完成三项任务:

(1)通过无人机设备得到位置信息和环境信息；

(2)将拍摄得到的环境信息传输到计算机分析平台，进行信息预处理，完成资源存储、分配和调度工作；

(3)算法分析，在计算机终端调用相关算法完成搜索。

1.3 基于栅格地图的环境地图建模

常用的表示方法一般可以分为三种：①几何特征地图；②拓扑地图；③栅格地图。以上建模方法相比，栅格法具有建模复杂性低、环境建模适应能力强及易于实现和存储等优点。利用无人机采集信息，计算机收集信息后做出分析处理，就可以准确地判断出哪些区域存在障碍物，通常情况下采集到的障碍物属于稳定的，因此采用栅格法建立农业机器人的二维环境模型。

1.4 启发函数的设计

机器人从起点和终点，在路径优劣的评价指标中引入以下几个机制[11]。

(1)鉴于机器人智能作业时安全性要求比较高，引入路径平滑度评价函数，采用余弦函数[式(2)]，余弦值越小，路径越平滑。

(2)

路径段qi-1qi与路径段qiqi+1之间的计算公式为

(3)

(2)在保证平滑度小、安全性高的同时也需要保证距离最短，即

i=1,2，…,n

(4)

(3)完成作业时间最少。

(5)

(6)

式中：ts,mn表示农业机器人完成任务(搜索)的时间；li表示机器人搜索得到的第i条路径的长度；vs表示机器人执行任务(搜索)速度，因此整个机器人完成执行任务的时间跨度就是ts,mn中的最大值，表示为

f3=max(ts,mnij)

(7)

因此，建立机器人路径规划函数求解最优路径，即

F=a1f1+a2f2+a3f3

(8)

式(8)中：a1、a2、a3为权值常数。

2 改进烟花算法

2.1 采用爆炸与迁移相结合的交互机制

基本烟花算法是由谭营等在2010年首次提出的一种群体算法，基本组成部分可以由文献[12-14]可知，现主要对火花交互机制和选择策略上进行改进。

在多个烟花同时爆炸过程中高适应度的烟花会在小范围产生较多的爆炸火花(局部搜索)和低适应度的烟花会在大范围内产生较少的火花(全局搜索)，在爆炸完成后都要都对每个火花适应度进行评价，大大地增加计算时间，而且烟花和火花的相互作用，也会降低种群的多样性，为此，建立烟花爆炸与生物多样性迁移相结合的交互机制。BBO 算法来源于生物地理学理论，在函数优化、图像处理、机器人路径规划等方面都取得了不错的成果。

设定X(t)={x1,x2,…,xN}为第t次迭代的初始烟花集合。其中,N为烟花个数，xi为第i个烟花在解空间中的位置信息。

(1)爆炸火花个数。

(9)

(2)爆炸幅度。

(10)

(3)交互机制。

(11)

(12)

式中：M为常数，可以调整火花的初始个数；ε为趋于0的常数，防止分母为0；f(xi)为第i个火花的适应度；ymax、ymin分别为当前火花适应度的最大值和最小值；μi为迁出率；λi为迁入率；I为迁入率最大值；E为迁出率最大值，取I=E=1。

BBO算法中的迁移模型如图2所示[15]。

图2 迁移模型图Fig.2 Migration model diagram

在这种交互机制下，每一次迭代中烟花只爆炸一部分，而剩余烟花则被迁移到新的地点，当烟花xi迁移时，λi的值就会被改变，那么新的值需要和另一个烟花xj来相互作用确定，并且xj被选择的概率与μj成正比。

Xi(d)=Xj(d)

(13)

Xi(d)=Xi(d)+ω[Xj(d)-Xi(d)]

(14)

式(13)为克隆迁移公式；式(14)为混合迁移公式；d为某一纬度。每一个烟花都有ρ的概率被迁移，相反有1-ρ的概率爆炸，一般取概率ρ=0.5，当烟花Xi迁移一个维度时，克隆迁移和混合迁移有相同的机会被使用。这样的组合策略每次只需要采用一种迁移，不需要对每个火花适应度进行评价，降低计算成本；也使高适应度的火花可以迁移到低适应度，增加了火花之间的信息共享和多样性。

2.2 改进选择策略

基本烟花算法中的选择策略公式为

(15)

(16)

式中：R(xi)为同一维度上两个火花之间的欧氏距离；p(xi)为在同一维度上被选择的概率。

火花粒子子代烟花选择的方式从式(15)中看出是依靠欧氏距离大小从而单一做出的选择，造成同一维度上的距离相同或者相似的火花粒子具备相似的搜索空间和范围，这样既没有减少空间复杂度，也大大增加了时间复杂度。针对这一缺点，采用密度峰值聚类算法中的密度概念和聚类中心来改进，将密度看作为适应度，密度峰值聚类中心看作是烟花爆炸邻域内适应度值最优的火花，在保证火花适应度优良的同时，也能保证火花粒子在全局范围内的探测性质，故提出基于适应度值和距离的密度峰值火花和探测火花。

(1)建立火花适应度值，构造函数fi(i=1,2,…,n)，表示第i朵火花xi的适应度值。

(2)建立转义适应度值并且作归一化处理，公式为

(17)

(3)对转义适应度中的任意两个火花间的最下距离δi进行定义并且作归一化处理。λmin、λmax分别为同一维度火花粒子之间距离最小和最大的值。

δi=mind(xi,xj)

(18)

(19)

通过式(18)、式(19)可知当δ′i较大时，δi也相对较大，同一维度上的f′i就会较小，由此可知此时火花适应度值fi较好。

(4)密度峰值火花γi为转义适应度值与其归一化距离的乘积，公式为

γi=f′iδ′i

(20)

式(20)表明密度峰值火花γi不仅仅是从欧氏距离大小来选择最优火花，也充分考虑了适应度值大小对时间复杂度和全局范围内探测的影响，通常情况下成为密度峰值火花都具备较大的转义适应度值以及相对较远的距离，其邻域内的其他火花粒子将无法成为密度峰值火花，有效地避免了探测能力相似的火花，进一步提高了火花粒子搜索效率。在剩下的N-1朵密度峰值火花充当下一代烟花中，由于当前火花群体最优火花的转义适应度值且归一化距离都较大，因此能够保证当前火花群体适应度值最低的火花粒子能够连续成为密度峰值火花并充当下一代爆炸的烟花。

为维持烟花在全局范围内的探测性，将所有子代火花中距离最远的火花，定义Xl为探测火花，保证每一代烟花的搜索能力，公式为

(21)

可以看出探测火花继承了烟花算法中搜索范围大，探测性能强的优点。

通过仿真试验结果发现在烟花算法中加入密度峰值火花和探测火花后(图3)，能够以很快的速度搜索到目标，以简单直接的方式寻得最终路径，如图4所示。

图3 IFWA算法流程图Fig.3 Flow chart of IFWA algorithm

图4 IFWA算法路径图Fig.4 IFWA algorithm path diagram

IFWA算法拥有较快的收敛速度和能够快速跳出局部的能力，但不具备避障能力，考虑到蚁群算法具备较强的鲁棒性和避障能力，将IFWA算法得到的参考路径换算成加强信息素，由于起点和终点连线附近往往存在最优路径，当加大了起始点和终点连线附近的信息素浓度后，会更快地找到最优路径，提高搜索效率。

2.3 IFWA-ACO算法

基本蚁群算法是自然界蚂蚁群体的觅食行为的一种描述，公式为

(22)

首先通过无人机采集到的环境信息得到信息素分布初始值τ′ij，在结合IFWA算法得到信息素加强值Δτij，得到新的初始信息素分布为

τij=τ′ij+Δτij

(23)

蚂蚁分泌信息素的同时，各个节点之间的信息素逐渐消失，当所有蚂蚁完成一次循环后，各个节点之间也进行了信息素浓度的实时更新，公式为

τij(t+n)=ρτij(t)+Δτij

(24)

(25)

μ+ν=K

(26)

IFWA-ACO算法的主要步骤如下：

Step1通过无人机探测得到环境信息和路径状况，建立农业机器人栅格地图模型。

Step2初始化参数，设置蚁群个数，最大迭代次数、信息素权重因子α、期望程度权重因子β等，利用改进后的烟花算法得到全局范围的最优路径并换算为信息素加强值Δτij，计算式(23)，然后根据作业要求，建立路径起点和目标点。

Step3将蚁群置于路径起点后开始迭代搜索，搜索过程中蚂蚁由当前栅格(xi,yi)向下一栅格(xj,yj)转移的概率可由式(22)计算得到，待栅格(xj,yj)确定后，采用式(24)～式(26)对蚂蚁刚经过的栅格路段的信息素进行实时更新。

Step4判断蚁群是否完成本次循环搜索，若蚁群全部收敛到一条路径或达到迭代阈值，则循环结束，否则转至Step2。

Step5输出当前搜索线路中的最优路径。

Step6利用B样条插值函数对路径做平滑处理，最后输出路径轨迹。

IFWA-ACO算法流程如图5所示。

图5 IFWA-ACO算法流程图Fig.5 IFWA-ACO algorithm flow chart

3 采用B样条函数进行折线插值拟合

B-spline插值是一种将散乱稀疏的，分布不均匀的点，通过在各个点之间插值的方法，构造平滑曲线的技术。使用B-spline插值有利于将由折线构成的最短路径优化为平滑曲线最优路径，通过B样条函数对路径进行平滑处理，避免机器人与障碍物发证碰撞，减小能耗。

B样条函数曲线具有以下优点：①计算成本较低；②能够保证路径的连续性；③可以通过节点改变控制点来影响局部曲线。因此在路径平滑性上具有明显优势。

B样条函数具有贝塞尔函数所没有的特性，更加复杂，一般定义为

(27)

式(27)中：m为节点xi的个数，xi的取值范围为⎣x0,xm-1」，x0

(28)

(29)

4 仿真与数据分析

4.1 实验参数设定

在MATLAB上进行仿真实验。建立30×30栅格地图。起点为(0.5,0.5)，目标点为(29.5,29.5)。机器人按照改进后的IFWA-ACO算法进行路径规划。因为烟花算法得到的初始参考路径可以很大程度上避免了蚂蚁的迷失，所以设置初始蚁群数量值应较小，避免增加收敛时间[16]。

4.2 仿真实验

为了模拟改进算法的可行性，在图6～图8中，与ACO算法、Levy-PSO算法做了对比，通过相同的参数设置后，在图8(a)中，可以清晰地看到红色细条行走的路径长度优势，同时，也可以对平滑度进行分析。由图6～图8、表1可知，改进后的IFWA-ACO算法比Levy-PSO算法和基本蚁群算法拥有更快的收敛时间，路径长度也优于其他算法。

图6 ACO算法路径与收敛曲线Fig.6 ACO algorithm path and convergence curve

图7 Levy-PSO算法路径与收敛曲线Fig.7 Levy-PSO algorithm path and convergence curve

图8 结合B-spline函数后路径图与收敛曲线Fig.8 Combined with B-spline path graph and convergence curve

表1 各算法结果比较Table 1 Comparison of each algorithm results

5 结论

(1)针对农业机器人在路径规划时的寻优路线、时间成本等问题，对基本烟花中交互机制做出改进，提出爆炸与迁移相结合的策略以及密度峰值火花、探测火花概念，在提升寻找最优解能力的同时，结合蚁群算法，从而避免蚁群盲目搜索。融合后的IFWA-ACO算法在时间和路径长度上均优于传统ACO算法、IFWA算法和Levy-PSO算法，并且随着环境复杂度的增加，IFWA-ACO算法路径长度上会有更加明显的优势。

(2)IFWA-ACO算法能够更加快速地跳出局部最优，迭代次数很少，收敛速度很快，完成作业时间缩短，大大提高机器人的工作效率，结合B-spline函数后，可以更好地使农业机器人完成避障而且路径更加平滑，降低能耗，具有较高的实际工程意义。