基于改進的煙花-蟻群算法和B樣條曲線的農(nóng)業(yè)機器人路徑規(guī)劃

王紅君, 葉 榮*, 趙 輝,2, 岳有軍

(1.天津理工大學天津市復雜系統(tǒng)控制理論與應用重點實驗室， 天津 300384; 2.天津農(nóng)學院工程技術學院， 天津 300384)

近年來，隨著人工智能的積極推行和迅猛發(fā)展，對包括農(nóng)業(yè)在內(nèi)的許多行業(yè)造成了深遠的影響和巨大的挑戰(zhàn)，作為精準農(nóng)業(yè)的支柱力量，人工智能技術已經(jīng)深深地滲透入農(nóng)業(yè)生產(chǎn)的每個環(huán)節(jié)，將智能算法和機器人路徑規(guī)劃技術結合運用具有較大的應用價值和廣闊的研究前景，也是實現(xiàn)機器人智能作業(yè)的重要基礎[1]。

目前應用較多的智能算法包括粒子群算法、遺傳算法、人工蜂群算法等[2-3]。為了彌補單一算法的缺點，文獻[4]提出了ACO-PSO(ant colony-particle swarm optimization)混合算法，提高了計算效率和搜索精度。文獻[5]將GA算法和PSO算法相結合，提高粒子多樣性，增強了全局搜索能力，為機器人路徑規(guī)劃問題提供了一種有效的方法；王學武等[6]、Douan[7]采用Levy飛行與PSO算法相結合對機器人進行全局路徑規(guī)劃時，Levy飛行策略能夠擴大搜索空間，增加粒子種群多樣性，提高尋優(yōu)能力，但因其具有隨機性，耗時就較長；劉俊等[8]提出的PSO-ACO(swarm-ant colony algorithm)融合算法改善了粒子群算法中粒子群種類少、易早熟的現(xiàn)象，同時解決了蟻群算法中初始化信息素匱乏的問題；孫純哲等[9]在傳統(tǒng)蟻群算法融合人工勢場思想，引入引力概率函數(shù)作為啟發(fā)因子，加快收斂速度；張琦等[10]在蟻群算法引入交叉操作后調(diào)整α、β和ρ的值，可以提高算法搜索能力上述融合算法，雖然在一定程度上解決了機器人路徑規(guī)劃中遇到的基本問題，但大部分都是基于耗時長短或者搜索路徑長度而建立的評價指標，忽略了如平滑度、能耗等路徑規(guī)劃過程中的實際問題。鑒于此，現(xiàn)對煙花算法(fireworks algorithm，F(xiàn)WA)進行了改進，利用煙花之間多點同時爆炸和子代煙花之間充分的交互機制，建立密度峰值火花和探測火花后結合蟻群算法對路徑進行搜索，使用改進后的IFWA-ACO(improved fireworks-ant colony algorithm)算法和B-spline函數(shù)來探索適合機器人智能作業(yè)的平滑路徑。

1 農(nóng)業(yè)機器人路徑規(guī)劃問題描述

1.1 二維平面下的農(nóng)業(yè)機器人模型

在真實的復雜環(huán)境中，不是所有的客體都能用簡單的數(shù)學語言來描述。為了達到探索性和普適性，對于大規(guī)則幾何形狀的農(nóng)業(yè)機器人，由于機械臂較大，特征點較多，不易確定，難以滿足實時性要求。忽略農(nóng)業(yè)機器人的大小，將其抽象為質(zhì)點，則滿足在二維平面的動力學方程，即

(1)

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

1.2 農(nóng)業(yè)機器人作業(yè)流程

農(nóng)業(yè)機器人在農(nóng)田、果園、農(nóng)場進行作業(yè)的流程如圖1所示。作業(yè)前首先要完成三項任務:

(1)通過無人機設備得到位置信息和環(huán)境信息；

(2)將拍攝得到的環(huán)境信息傳輸?shù)接嬎銠C分析平臺，進行信息預處理，完成資源存儲、分配和調(diào)度工作；

(3)算法分析，在計算機終端調(diào)用相關算法完成搜索。

1.3 基于柵格地圖的環(huán)境地圖建模

常用的表示方法一般可以分為三種：①幾何特征地圖；②拓撲地圖；③柵格地圖。以上建模方法相比，柵格法具有建模復雜性低、環(huán)境建模適應能力強及易于實現(xiàn)和存儲等優(yōu)點。利用無人機采集信息，計算機收集信息后做出分析處理，就可以準確地判斷出哪些區(qū)域存在障礙物，通常情況下采集到的障礙物屬于穩(wěn)定的，因此采用柵格法建立農(nóng)業(yè)機器人的二維環(huán)境模型。

1.4 啟發(fā)函數(shù)的設計

機器人從起點和終點，在路徑優(yōu)劣的評價指標中引入以下幾個機制[11]。

(1)鑒于機器人智能作業(yè)時安全性要求比較高，引入路徑平滑度評價函數(shù)，采用余弦函數(shù)[式(2)]，余弦值越小，路徑越平滑。

(2)

路徑段qi-1qi與路徑段qiqi+1之間的計算公式為

(3)

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

i=1,2，…,n

(4)

(3)完成作業(yè)時間最少。

(5)

(6)

式中：ts,mn表示農(nóng)業(yè)機器人完成任務(搜索)的時間；li表示機器人搜索得到的第i條路徑的長度；vs表示機器人執(zhí)行任務(搜索)速度，因此整個機器人完成執(zhí)行任務的時間跨度就是ts,mn中的最大值，表示為

f3=max(ts,mnij)

(7)

因此，建立機器人路徑規(guī)劃函數(shù)求解最優(yōu)路徑，即

F=a1f1+a2f2+a3f3

(8)

式(8)中：a1、a2、a3為權值常數(shù)。

2 改進煙花算法

2.1 采用爆炸與遷移相結合的交互機制

基本煙花算法是由譚營等在2010年首次提出的一種群體算法，基本組成部分可以由文獻[12-14]可知，現(xiàn)主要對火花交互機制和選擇策略上進行改進。

在多個煙花同時爆炸過程中高適應度的煙花會在小范圍產(chǎn)生較多的爆炸火花(局部搜索)和低適應度的煙花會在大范圍內(nèi)產(chǎn)生較少的火花(全局搜索)，在爆炸完成后都要都對每個火花適應度進行評價，大大地增加計算時間，而且煙花和火花的相互作用，也會降低種群的多樣性，為此，建立煙花爆炸與生物多樣性遷移相結合的交互機制。BBO 算法來源于生物地理學理論，在函數(shù)優(yōu)化、圖像處理、機器人路徑規(guī)劃等方面都取得了不錯的成果。

設定X(t)={x1,x2,…,xN}為第t次迭代的初始煙花集合。其中,N為煙花個數(shù)，xi為第i個煙花在解空間中的位置信息。

(1)爆炸火花個數(shù)。

(9)

(2)爆炸幅度。

(10)

(3)交互機制。

(11)

(12)

式中：M為常數(shù)，可以調(diào)整火花的初始個數(shù)；ε為趨于0的常數(shù)，防止分母為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)中看出是依靠歐氏距離大小從而單一做出的選擇，造成同一維度上的距離相同或者相似的火花粒子具備相似的搜索空間和范圍，這樣既沒有減少空間復雜度，也大大增加了時間復雜度。針對這一缺點，采用密度峰值聚類算法中的密度概念和聚類中心來改進，將密度看作為適應度，密度峰值聚類中心看作是煙花爆炸鄰域內(nèi)適應度值最優(yōu)的火花，在保證火花適應度優(yōu)良的同時，也能保證火花粒子在全局范圍內(nèi)的探測性質(zhì)，故提出基于適應度值和距離的密度峰值火花和探測火花。

(1)建立火花適應度值，構造函數(shù)fi(i=1,2,…,n)，表示第i朵火花xi的適應度值。

(2)建立轉(zhuǎn)義適應度值并且作歸一化處理，公式為

(17)

(3)對轉(zhuǎn)義適應度中的任意兩個火花間的最下距離δi進行定義并且作歸一化處理。λmin、λmax分別為同一維度火花粒子之間距離最小和最大的值。

δi=mind(xi,xj)

(18)

(19)

通過式(18)、式(19)可知當δ′i較大時，δi也相對較大，同一維度上的f′i就會較小，由此可知此時火花適應度值fi較好。

(4)密度峰值火花γi為轉(zhuǎn)義適應度值與其歸一化距離的乘積，公式為

γi=f′iδ′i

(20)

式(20)表明密度峰值火花γi不僅僅是從歐氏距離大小來選擇最優(yōu)火花，也充分考慮了適應度值大小對時間復雜度和全局范圍內(nèi)探測的影響，通常情況下成為密度峰值火花都具備較大的轉(zhuǎn)義適應度值以及相對較遠的距離，其鄰域內(nèi)的其他火花粒子將無法成為密度峰值火花，有效地避免了探測能力相似的火花，進一步提高了火花粒子搜索效率。在剩下的N-1朵密度峰值火花充當下一代煙花中，由于當前火花群體最優(yōu)火花的轉(zhuǎn)義適應度值且歸一化距離都較大，因此能夠保證當前火花群體適應度值最低的火花粒子能夠連續(xù)成為密度峰值火花并充當下一代爆炸的煙花。

為維持煙花在全局范圍內(nèi)的探測性，將所有子代火花中距離最遠的火花，定義Xl為探測火花，保證每一代煙花的搜索能力，公式為

(21)

可以看出探測火花繼承了煙花算法中搜索范圍大，探測性能強的優(yōu)點。

通過仿真試驗結果發(fā)現(xiàn)在煙花算法中加入密度峰值火花和探測火花后(圖3)，能夠以很快的速度搜索到目標，以簡單直接的方式尋得最終路徑，如圖4所示。

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

圖4 IFWA算法路徑圖Fig.4 IFWA algorithm path diagram

IFWA算法擁有較快的收斂速度和能夠快速跳出局部的能力，但不具備避障能力，考慮到蟻群算法具備較強的魯棒性和避障能力，將IFWA算法得到的參考路徑換算成加強信息素，由于起點和終點連線附近往往存在最優(yōu)路徑，當加大了起始點和終點連線附近的信息素濃度后，會更快地找到最優(yōu)路徑，提高搜索效率。

2.3 IFWA-ACO算法

基本蟻群算法是自然界螞蟻群體的覓食行為的一種描述，公式為

(22)

首先通過無人機采集到的環(huán)境信息得到信息素分布初始值τ′ij，在結合IFWA算法得到信息素加強值Δτij，得到新的初始信息素分布為

τij=τ′ij+Δτij

(23)

螞蟻分泌信息素的同時，各個節(jié)點之間的信息素逐漸消失，當所有螞蟻完成一次循環(huán)后，各個節(jié)點之間也進行了信息素濃度的實時更新，公式為

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

(24)

(25)

μ+ν=K

(26)

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

Step1通過無人機探測得到環(huán)境信息和路徑狀況，建立農(nóng)業(yè)機器人柵格地圖模型。

Step2初始化參數(shù)，設置蟻群個數(shù)，最大迭代次數(shù)、信息素權重因子α、期望程度權重因子β等，利用改進后的煙花算法得到全局范圍的最優(yōu)路徑并換算為信息素加強值Δτij，計算式(23)，然后根據(jù)作業(yè)要求，建立路徑起點和目標點。

Step3將蟻群置于路徑起點后開始迭代搜索，搜索過程中螞蟻由當前柵格(xi,yi)向下一柵格(xj,yj)轉(zhuǎn)移的概率可由式(22)計算得到，待柵格(xj,yj)確定后，采用式(24)～式(26)對螞蟻剛經(jīng)過的柵格路段的信息素進行實時更新。

Step4判斷蟻群是否完成本次循環(huán)搜索，若蟻群全部收斂到一條路徑或達到迭代閾值，則循環(huán)結束，否則轉(zhuǎn)至Step2。

Step5輸出當前搜索線路中的最優(yōu)路徑。

Step6利用B樣條插值函數(shù)對路徑做平滑處理，最后輸出路徑軌跡。

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

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

3 采用B樣條函數(shù)進行折線插值擬合

B-spline插值是一種將散亂稀疏的，分布不均勻的點，通過在各個點之間插值的方法，構造平滑曲線的技術。使用B-spline插值有利于將由折線構成的最短路徑優(yōu)化為平滑曲線最優(yōu)路徑，通過B樣條函數(shù)對路徑進行平滑處理，避免機器人與障礙物發(fā)證碰撞，減小能耗。

B樣條函數(shù)曲線具有以下優(yōu)點：①計算成本較低；②能夠保證路徑的連續(xù)性；③可以通過節(jié)點改變控制點來影響局部曲線。因此在路徑平滑性上具有明顯優(yōu)勢。

B樣條函數(shù)具有貝塞爾函數(shù)所沒有的特性，更加復雜，一般定義為

(27)

式(27)中：m為節(jié)點xi的個數(shù)，xi的取值范圍為?x0,xm-1」，x0

(28)

(29)

4 仿真與數(shù)據(jù)分析

4.1 實驗參數(shù)設定

在MATLAB上進行仿真實驗。建立30×30柵格地圖。起點為(0.5,0.5)，目標點為(29.5,29.5)。機器人按照改進后的IFWA-ACO算法進行路徑規(guī)劃。因為煙花算法得到的初始參考路徑可以很大程度上避免了螞蟻的迷失，所以設置初始蟻群數(shù)量值應較小，避免增加收斂時間[16]。

4.2 仿真實驗

為了模擬改進算法的可行性，在圖6～圖8中，與ACO算法、Levy-PSO算法做了對比，通過相同的參數(shù)設置后，在圖8(a)中，可以清晰地看到紅色細條行走的路徑長度優(yōu)勢，同時，也可以對平滑度進行分析。由圖6～圖8、表1可知，改進后的IFWA-ACO算法比Levy-PSO算法和基本蟻群算法擁有更快的收斂時間，路徑長度也優(yōu)于其他算法。

圖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函數(shù)后路徑圖與收斂曲線Fig.8 Combined with B-spline path graph and convergence curve

表1 各算法結果比較Table 1 Comparison of each algorithm results

5 結論

(1)針對農(nóng)業(yè)機器人在路徑規(guī)劃時的尋優(yōu)路線、時間成本等問題，對基本煙花中交互機制做出改進，提出爆炸與遷移相結合的策略以及密度峰值火花、探測火花概念，在提升尋找最優(yōu)解能力的同時，結合蟻群算法，從而避免蟻群盲目搜索。融合后的IFWA-ACO算法在時間和路徑長度上均優(yōu)于傳統(tǒng)ACO算法、IFWA算法和Levy-PSO算法，并且隨著環(huán)境復雜度的增加，IFWA-ACO算法路徑長度上會有更加明顯的優(yōu)勢。

(2)IFWA-ACO算法能夠更加快速地跳出局部最優(yōu)，迭代次數(shù)很少，收斂速度很快，完成作業(yè)時間縮短，大大提高機器人的工作效率，結合B-spline函數(shù)后，可以更好地使農(nóng)業(yè)機器人完成避障而且路徑更加平滑，降低能耗，具有較高的實際工程意義。