黃軼文，張 梅

( 1.廣東工程職業(yè)技術(shù)學(xué)院 信息工程學(xué)院，廣州 510520；2.華南理工大學(xué) 自動化科學(xué)與工程學(xué)院，廣州 510640)

?

基于蟻群算法的六自由度采摘機(jī)器人軌跡規(guī)劃研究

黃軼文1，張 梅2

( 1.廣東工程職業(yè)技術(shù)學(xué)院 信息工程學(xué)院，廣州 510520；2.華南理工大學(xué) 自動化科學(xué)與工程學(xué)院，廣州 510640)

在現(xiàn)代農(nóng)業(yè)生產(chǎn)中，果蔬采摘作業(yè)復(fù)雜而繁重，采摘機(jī)器人在作業(yè)過程中常常需要經(jīng)歷成千上萬個果蔬采摘點(diǎn)，面對這樣巨大的工作量，采摘機(jī)器人移動路徑規(guī)劃顯得非常重要。為此，以采摘機(jī)器人運(yùn)動軌跡為研究對象，以其運(yùn)動軌跡總長最短為研究目標(biāo)，針對機(jī)器人各關(guān)節(jié)機(jī)構(gòu)運(yùn)動速度變化情況及機(jī)器人運(yùn)動特性，利用基本蟻群原理對六自由度采摘機(jī)器人的路徑進(jìn)行規(guī)劃。實(shí)驗(yàn)結(jié)果表明：所設(shè)計(jì)的采摘機(jī)器人軌跡優(yōu)化技術(shù)不但路徑優(yōu)化能力強(qiáng)、運(yùn)動軌跡平滑，還具有可靠性強(qiáng)及穩(wěn)定性好的優(yōu)點(diǎn)。

蟻群算法；六自由度；采摘機(jī)器人；路徑優(yōu)化

0 引言

近年來，隨著新農(nóng)業(yè)生產(chǎn)模式和計(jì)算機(jī)技術(shù)的快速發(fā)展，智能機(jī)器人的研究有了很大的突破，且在農(nóng)業(yè)生產(chǎn)中的應(yīng)用越來越普及，已成為農(nóng)業(yè)生產(chǎn)不可缺少的部分。在機(jī)器人的研究中，設(shè)計(jì)規(guī)劃一條運(yùn)動軌跡最優(yōu)、躲避障礙次數(shù)最少、運(yùn)行代價最低及軌跡最平滑的路線是重中之重。目前，機(jī)器人路徑優(yōu)化的研究方法主要有蟻群、Hopfield、遺傳、網(wǎng)絡(luò)神經(jīng)及人工勢場等算法。本文將根據(jù)果蔬采摘作業(yè)環(huán)境的特點(diǎn)，結(jié)合機(jī)器人整體結(jié)構(gòu)運(yùn)動特性，采用改進(jìn)蟻群算法，對果蔬采摘機(jī)器人運(yùn)動軌跡計(jì)算優(yōu)化，實(shí)現(xiàn)果蔬采摘機(jī)器人運(yùn)動路徑最優(yōu)化。

1 軌跡規(guī)劃的蟻群算法

1.1 蟻群算法概述

20世紀(jì)90年代，意大利著名研究學(xué)家Dorigo提出了一種仿生的螞蟻算法。螞蟻群覓食過程如圖1所示。其中，圖1(a)表示剛開始時螞蟻群尋找食物的運(yùn)動路線；圖1(b)表示在兩條路線長短不一樣的情況下，螞蟻基本上會選擇路徑短的線路。因?yàn)槁窂蕉?，螞蟻?zhàn)咄耆绦枰臅r間相對較少，因此在相同時間的情況下，走過的螞蟻會多一些，路上的信息素也會越多，后面螞蟻選擇短路程的機(jī)率就大一些。

(a) (b)圖1 螞蟻群覓食示意圖Fig.1 Schematic diagram of ant colony foraging

開始時刻，將螞蟻置于相同的地點(diǎn)，每條路徑都有一定初始的信息素，假設(shè)距離近的路徑為S，距離遠(yuǎn)的路徑為L，M1和M2為經(jīng)過S和L的螞蟻數(shù)量之和，則

M=M1+M2

(1)

當(dāng)M只螞蟻從S和L經(jīng)過以后，第M+1只螞蟻選擇S路線的機(jī)率為

(2)

其中，a和h為參數(shù)值。第M+1只螞蟻選擇時，計(jì)算PS(M)，得到[0,1]之間的概率φ，若φ≤PS(K)，則選擇S，不然選擇L。

1.2 路徑規(guī)劃建模

機(jī)器人在采摘作業(yè)中，工作環(huán)境比較復(fù)雜，首先需要確定自己的任務(wù)，感知所處環(huán)境，然后對所走路線進(jìn)行規(guī)劃。假設(shè)采摘機(jī)器人要成功從果樹A移動到果樹B，在避開其他障礙時，整個運(yùn)動過程應(yīng)該滿足：

1)用時最少，則

f1=min(maxt)， 1≤t≤6

(3)

其中，f1為從A到B所需最短時間；t為某一段的運(yùn)動時間。

2)移動距離最短，則

(4)

其中，f2為從A到B所走最短路程；t為某一段的運(yùn)動路程；(xi，yi)為路徑點(diǎn)坐標(biāo)。

3)路徑最平滑，則

(5)

其中，f2為最小沖力；t為走完該路徑時間。

路徑優(yōu)化是在同一時刻處理多個目標(biāo)，對于優(yōu)化總目標(biāo)則需要綜合多個目標(biāo)來規(guī)劃。采摘機(jī)器人的路徑優(yōu)化問題需要同時考慮用時最少、移動距離最短和路徑最平滑3個因素，因此建立機(jī)器人軌跡規(guī)劃函數(shù)為

F=a1f1+a2f2+a3f3

(6)

其中，a1、a2、a3為函數(shù)的權(quán)。

1.3 實(shí)時更新信息素

螞蟻行走以后，若路徑上的信息素太多，會導(dǎo)致啟發(fā)信息的消失。為了解決這個問題，一般需要在螞蟻完成一個節(jié)點(diǎn)路徑時，實(shí)時更新信息素。更新信息素包括全局更新和局部更新兩部分。由于采摘機(jī)器人在作業(yè)中可能會因地理環(huán)境進(jìn)入陷阱而導(dǎo)致算法終止。因此，本文采用全局更新的方法，則在t+n時刻某一節(jié)點(diǎn)路徑信息素更新準(zhǔn)則為

(7)

(8)

其中，ρ?[0,1)為路徑上信息素消失系數(shù)。

在更新信息素時，對于螞蟻到達(dá)沒有后繼路徑且沒有到達(dá)終點(diǎn)時，自動判斷該螞蟻死亡，信息素更新退回至前一路徑節(jié)點(diǎn)。

2 路徑點(diǎn)坐標(biāo)識別

機(jī)器人在采摘作業(yè)中，最重要的是對目標(biāo)的識別和定位。實(shí)現(xiàn)機(jī)器人準(zhǔn)確采摘首先需要感知目標(biāo)果實(shí)的具體三維信息，現(xiàn)階段獲取目標(biāo)空間信息一般采用雙目視覺技術(shù)。本文采用雙目視覺模型，坐標(biāo)轉(zhuǎn)化關(guān)系如圖2所示

圖2 三維空間坐標(biāo)轉(zhuǎn)化關(guān)系Fig.2 three dimensional space coordinate transformation relation

由三維空間坐標(biāo)轉(zhuǎn)化關(guān)系可得

(9)

(10)

(11)

(12)

T=R(α,β,γ)

(13)

其中，R為標(biāo)準(zhǔn)正交矩陣；T為平移值；f為雙目攝像機(jī)焦距。

圖像定義的直角坐標(biāo)系“u-O-v”中，像素坐標(biāo)(u,v)為該像素在數(shù)組中的行數(shù)和列數(shù)，即(u,v)是以像素為單元的圖像坐標(biāo)。假設(shè)包括N個坐標(biāo)點(diǎn)，采用奇異值分析算法可以得到圖像中的內(nèi)部、外部參數(shù)，由此可進(jìn)行運(yùn)動路線的優(yōu)化。雙目視覺模型示意圖如圖3所示。

3 最優(yōu)路徑識別規(guī)劃

3.1 路徑編碼

首先給所有的路徑點(diǎn)編上序號，即在坐標(biāo)系XOY中，將直角坐標(biāo)系等分為N個柵格，并依次編號，則每個序號表示一個唯一對應(yīng)的柵格。令C={1,2,3,…,N}，gi∈(xi,yi),序號與柵格坐標(biāo)對應(yīng)關(guān)系如圖4所示。

圖3 雙目視覺模型示意圖Fig.3 Schematic diagram of binocular vision model

圖4 序號與柵格對應(yīng)關(guān)系圖Fig.4 The relationship between serial number and grid

將兩者轉(zhuǎn)化為公式，有

(14)

其中，mod是求余運(yùn)算；int為求整運(yùn)算。

3.2 求出映射關(guān)系

為了使計(jì)算機(jī)在處理圖像的更加方便快捷，采用序號與柵格坐標(biāo)相對應(yīng)標(biāo)識，即可以根據(jù)序號求目標(biāo)的直角坐標(biāo)。首先設(shè)定像素(u,v)與序號的映射關(guān)系為

(u,v)=m×(j-1)+i

(15)

求出像素(u,v)與目標(biāo)坐標(biāo)的映射關(guān)系為

(16)

最后，得到目標(biāo)坐標(biāo)值與序號的關(guān)系為

(u,v)=m×(y+0.5δ-1)+x+0.5δ

(17)

3.3 求解路徑節(jié)點(diǎn)距離與合適度函數(shù)

在軟件仿真之前先確定路徑節(jié)點(diǎn)的具體位置，本文采用計(jì)算機(jī)編碼生成直角坐標(biāo)參數(shù)。路徑節(jié)點(diǎn)距離采取N×N矩陣D進(jìn)行保存，D(i-1,i)表示第(i-1)個路徑節(jié)點(diǎn)和第i個路徑節(jié)點(diǎn)間的實(shí)際距離。兩點(diǎn)之間的距離公式為

(18)

在求解最優(yōu)函數(shù)時，所有節(jié)點(diǎn)總距離與合適度函數(shù)成反比，即總距離越大，該路徑合適度越低。根據(jù)所有節(jié)點(diǎn)總距離，合適度函數(shù)為

(19)

在進(jìn)行最優(yōu)路徑選擇時，一般去除節(jié)點(diǎn)總距離大的，保留節(jié)點(diǎn)總距離小的。

3.4 最有路徑求解的算法流程

蟻群算法解決路徑優(yōu)化問題流程如圖5所示。

圖5 蟻群算法解決路徑優(yōu)化問題流程圖

Fig.5 Flow chart of ant colony algorithm to solve path optimization problem

具體路徑求解的算法流程：

1)建立環(huán)境模型，初始化螞蟻算法，格式化信息素，清空迭代次數(shù)。

2)設(shè)定起點(diǎn)和終點(diǎn)，將螞蟻群置于起始點(diǎn)S。

3)螞蟻按概率函數(shù)在各路徑間運(yùn)動，并對移動次數(shù)自動計(jì)數(shù)。

4)當(dāng)移動總次數(shù)小于軟件設(shè)置值，且螞蟻沒有到達(dá)終點(diǎn)時，回到步驟3)重新開始；反之，大于或者等于軟件設(shè)置值，且沒有到達(dá)終點(diǎn)，將螞蟻重新初始化， 清空該螞蟻所有數(shù)據(jù)，并將螞蟻重置數(shù)加1；如果重新初始化小于設(shè)定值則返回至步驟3),否則系統(tǒng)自動判斷該螞蟻死亡。

5)統(tǒng)計(jì)成功到達(dá)終點(diǎn)螞蟻需要的路徑代價。

6)所有螞蟻重復(fù)執(zhí)行執(zhí)行步驟3)到5)。

7)計(jì)算路徑代價最小值。

8)判斷螞蟻到達(dá)各路徑節(jié)點(diǎn)時運(yùn)動方向，實(shí)時更新信息素值，并將迭代次數(shù)自動加1后保存。

9)輸出最優(yōu)路徑。

4 實(shí)驗(yàn)仿真與結(jié)果分析

為驗(yàn)證該蟻群算法的六自由度采摘機(jī)器人軌跡規(guī)劃可靠性，利用MatLab仿真軟件對其進(jìn)行有效性仿真實(shí)驗(yàn)，并在該實(shí)驗(yàn)中特別加入各種障礙物和難度系數(shù)大的工作環(huán)境。為了更好地驗(yàn)證螞蟻算法，首先按需要給路徑節(jié)點(diǎn)編號，根據(jù)所走路徑占據(jù)的柵格數(shù)量來判斷路徑的長度，在求解最優(yōu)路徑時，引入平滑度參數(shù)，以路徑中拐點(diǎn)數(shù)判斷該路徑的平滑度。在實(shí)試驗(yàn)中，設(shè)定障礙系數(shù)為16%，螞蟻總數(shù)為1 000，移動次數(shù)NC=200，起始點(diǎn)為A，終點(diǎn)為B，仿真結(jié)果如圖6所示。

(a) (b)圖6 螞蟻算法仿真結(jié)果圖Fig.6 Simulation results of ant algorithm

圖6(a)中是沒有考慮平滑度因素的軌跡圖；圖6(b)中是加入平滑度因素的軌跡圖。軌跡優(yōu)化性能主要拐點(diǎn)數(shù)、路徑長度和移動次數(shù)。兩種不同算法反復(fù)試驗(yàn)50次后，性能狀況如表1所示。

表1 軌跡優(yōu)化性能狀況表Table 1 Performance status of trajectory optimization

結(jié)合圖6和表1可以看出：在路徑長度相同的情況下，軌跡平滑度對軌跡優(yōu)化影響很大；在拐點(diǎn)數(shù)減少的同時，可以減少機(jī)器人移動次數(shù)，降低機(jī)器人采摘作業(yè)中的機(jī)械損傷。因此，對于精確度高的軌跡優(yōu)化需要加入軌跡平滑度參數(shù)。采用螞蟻算法的軌跡優(yōu)化如圖7所示。

5 結(jié)論

針對六自由度采摘機(jī)器人在采摘作業(yè)過程中行走路徑過長及效率低的問題，以采摘機(jī)器人運(yùn)動軌跡為研究對象，以其運(yùn)動軌跡總長最短為研究目標(biāo)，加入軌跡平滑度約束條件，采用基本蟻群算法原理對六自由度采摘機(jī)器人的路徑進(jìn)行規(guī)劃，并運(yùn)用MatLab對螞蟻算法進(jìn)行仿真實(shí)驗(yàn)，對機(jī)器人運(yùn)動軌跡優(yōu)化結(jié)果進(jìn)行驗(yàn)證。結(jié)果表明：所設(shè)計(jì)研究的采摘機(jī)器人軌跡優(yōu)化算法是可行的，其路徑優(yōu)化能力強(qiáng)，運(yùn)動軌跡平滑，具有可靠性強(qiáng)及穩(wěn)定性好的特點(diǎn)。

圖7 軌跡規(guī)劃圖Fig.7 Trajectory planning

[1] 蘇玉.蟻群算法的研究及其在路由選擇方面的應(yīng)用[D].無錫：江南大學(xué),2009.

[2] 陳天宏.雙目采摘機(jī)器人路徑優(yōu)化設(shè)計(jì)的研究[D].哈爾濱：東北農(nóng)業(yè)大學(xué),2010.

[3] 王鑫.基于蟻群算法的6自由度工業(yè)點(diǎn)焊機(jī)器人的軌跡優(yōu)化及仿真[D].上海：華東理工大學(xué),2011.

[4] 宋世杰,劉高峰,周忠友,等.基于改進(jìn)蟻群算法求解最短路徑和TSP問題[J].計(jì)算機(jī)技術(shù)與發(fā)展,2010(4):144-147.

[5] 陳天宏,崔天時,李廣軍.基于遺傳算法的采摘機(jī)器人軌跡規(guī)劃[J].農(nóng)機(jī)化研究,2010,32(8):31-34.

[6] 蔡文彬,朱慶保.具有感覺適應(yīng)功能蟻群算法的機(jī)器人路徑規(guī)劃[J].計(jì)算機(jī)工程與應(yīng)用,2010(31): 215-218.

[7] 杜占瑋,楊永健,孫永雄,等.基于互信息的混合蟻群算法及其在旅行商問題上的應(yīng)用[J].東南大學(xué)學(xué)報：自然科學(xué)版,2011(3):478-481.

[8] 張凌.蟻群算法的研究及其在網(wǎng)絡(luò)路由優(yōu)化上的應(yīng)用[D].無錫：江南大學(xué),2008.

[9] 劉國才.無人機(jī)單目機(jī)器視覺著降定位的算法研究及DSP實(shí)現(xiàn)[D].成都：電子科技大學(xué),2008.

[10] 李慶瀛.基于視覺注意的移動機(jī)器人目標(biāo)跟蹤技術(shù)研究[D].大連：大連理工大學(xué),2008.

[11] 王倩.基于視覺導(dǎo)航的模型車自主行駛研究[D].南京：南京航空航天大學(xué),2007.

[12] 雷春英.基于改進(jìn)蟻群算法的火災(zāi)疏散路徑優(yōu)化研究[D].武漢：武漢理工大學(xué),2014.

[13] 賈磊,程乃偉.基于蟻群算法的動態(tài)疏散路徑改進(jìn)[J]. 科技傳播,2013(20):85-86.

[14] 王海員.無線傳感器網(wǎng)絡(luò)能量均衡數(shù)據(jù)匯集算法研究[D].重慶：重慶大學(xué),2011.

[15] 李志鵬.藍(lán)莓采摘機(jī)采摘策略及軌跡規(guī)劃研究[D].哈爾濱：東北林業(yè)大學(xué),2011.

[16] 呂繼東.蘋果采摘機(jī)器人視覺測量與避障控制研究[D].鎮(zhèn)江：江蘇大學(xué),2012.

[17] 黃鋁文.蘋果采摘機(jī)器人視覺識別與路徑規(guī)劃方法研究[D].楊凌：西北農(nóng)林科技大學(xué),2013.

[18] 馬強(qiáng).蘋果采摘機(jī)器人關(guān)鍵技術(shù)研究[D].北京：中國農(nóng)業(yè)機(jī)械化科學(xué)研究院,2012.

[19] 胡薈.基于改進(jìn)蟻群算法的機(jī)器人三維路徑規(guī)劃技術(shù)的研究[D].杭州：浙江師范大學(xué),2012.

[20] 王輝.機(jī)器視覺技術(shù)在果園自動化中的應(yīng)用研究[D].北京：中國農(nóng)業(yè)機(jī)械化科學(xué)研究院,2011.

[21] 嚴(yán)卉.基于視覺的輪式移動機(jī)器人目標(biāo)跟蹤技術(shù)研究[D].南京：南京理工大學(xué),2005.

[22] 胡龍.基于雙目立體視覺的旋轉(zhuǎn)體三維重建技術(shù)研究[D].大連：大連理工大學(xué),2007.

[23] 張昌遠(yuǎn).基于蟻群的P2P網(wǎng)絡(luò)副本一致性維護(hù)策略[D].大連：大連理工大學(xué),2013.

[24] 劉瑛.基于蟻群優(yōu)化算法的車輛出行問題研究與應(yīng)用[D].重慶：重慶理工大學(xué),2013.

[25] 李鶴喜.基于視覺反饋的焊接機(jī)器人自主示教關(guān)鍵技術(shù)研究[D].廣州：華南理工大學(xué),2010.

[26] 王燕,張果,葛運(yùn)旺.四自由度關(guān)節(jié)型采摘機(jī)械手軌跡規(guī)劃與實(shí)驗(yàn)研究[J].現(xiàn)代制造工程,2013(7): 27-31.

[27] 吳靚,何清華,黃志雄,等.基于蟻群算法的多機(jī)器人集中協(xié)調(diào)式路徑規(guī)劃[J].機(jī)器人技術(shù)與應(yīng)用,2006(3):32-37.

[28] 尹向東. 基于螞蟻優(yōu)化的QoS約束分布式多播路由算法研究與實(shí)現(xiàn)[D].長沙：中南大學(xué),2008.

[29] 崔玉潔,張祖立,白曉虎.采摘機(jī)器人的研究進(jìn)展與現(xiàn)狀分析[J].農(nóng)機(jī)化研究,2007(2):4-7.

Research on Trajectory Planning of Six-freedom-degree Picking Robot Based on Ant Colony Algorithm

Huang Yiwen1, Zhang Mei2

(1.Department of Information Engineering, Guangdong Engineering Polytechnic, Guangzhou 510520, China; 2.School of Automation Science and Control Engineering, South China University of Technology, Guangzhou 510640, China)

In modern agriculture planting picking, picking fruits and vegetables operating in a complex and arduous, picking robot in the process often need to experience thousands of fruit and vegetable picking point, the face of such a huge workload, picking mobile robot path planning is very important. The to picking robot trajectory as the research object, takes the shortest length of the motion trajectory as the research target, for each robot joint mechanism motion speed changes, combined with robot motion characteristics, the basic ant colony algorithm of six degree of freedom picking robot path planning. The experimental results show that: the design of the picking robot trajectory optimization technology not only has strong ability of path optimization, smooth motion trajectory, but also has the advantages of strong reliability and good stability.

ant colony algorithm; six degree of freedom; picking robot; path optimization

2015-01-25

廣東省工業(yè)攻關(guān)科技計(jì)劃項(xiàng)目(2013B010401002)

黃軼文(1974-)，女，廣州人，高級工程師，碩士，(E-mail)yw_huang1974@sina.com。

S225.92;TP242.6

A

1003-188X(2017)03-0242-05