馬冀桐，王 毅,2※，何 宇，王 愷，張藝譚

?

基于構(gòu)型空間先驗知識引導(dǎo)點的柑橘采摘機(jī)械臂運動規(guī)劃

馬冀桐1，王 毅1,2※，何 宇1，王 愷1，張藝譚1

（1. 重慶理工大學(xué)機(jī)械工程學(xué)院，重慶 400054；2. 重慶大學(xué)機(jī)械工程學(xué)院，重慶 400044）

柑橘采摘過程中機(jī)械臂有時需要深入樹冠內(nèi)部進(jìn)行采摘，而在樹冠內(nèi)眾多枝干往往構(gòu)成一個個封閉的多邊形通道，比起單個枝條的障礙物，封閉多邊形障礙物更加難以避開，需要更長的時間進(jìn)行規(guī)劃。針對此問題，該文通過對構(gòu)型空間的離線構(gòu)建，分析了封閉多邊形障礙物在構(gòu)型空間的拓?fù)浣Y(jié)構(gòu)性質(zhì)，根據(jù)這一性質(zhì)對雙向快速擴(kuò)展隨機(jī)樹算法（RRT-connect）進(jìn)行改進(jìn)，提出了一種基于構(gòu)型空間先驗知識引導(dǎo)點的RRT-connect算法（informed guidance point RRT-connect，IGPRRT-connect），并將RRT-connect與IGPRRT- connect進(jìn)行了并行規(guī)劃編程，提高在不同環(huán)境下的適應(yīng)性。通過仿真：所提出的并行算法在各種環(huán)境下規(guī)劃時間均處于較低水平，以邊長為30 cm與25 cm的正方形封閉通道為例，與RRT-connect相比規(guī)劃時間分別縮短了51%、86%。同時進(jìn)行室內(nèi)避障試驗，試驗結(jié)果表明，使用提出的并行算法，對封閉障礙物和未封閉障礙物均有較好的避障效果，平均規(guī)劃時間為1.263 s左右，成功率為91%，可為柑橘采摘機(jī)器人在不同環(huán)境下的運動規(guī)劃問題提供參考。

機(jī)器人；運動規(guī)劃；避障；構(gòu)型空間；引導(dǎo)點；RRT-connect；柑橘采摘

0 引 言

農(nóng)業(yè)是中國經(jīng)濟(jì)的重要支柱之一[1]，果蔬產(chǎn)品作為農(nóng)業(yè)生產(chǎn)的重要一部分，其收獲采摘約占整個作業(yè)量的40%[2]，而柑橘等季節(jié)性水果的收獲作業(yè)更是需要在短時間內(nèi)密集的體力勞動，需要在短時間內(nèi)找到大量勞動力，人力成本巨大[3]。為了能夠使柑橘收獲過程自動化，已經(jīng)開發(fā)了一部分柑橘收獲機(jī)械，如旋轉(zhuǎn)拍打桿或枝干搖動設(shè)備。但是因為果實受到震動沖擊的影響，這些方法只適合收獲用于二次加工的柑橘(如制作果汁)，并不適合無損柑橘的收獲[3]，隨著計算機(jī)技術(shù)、人工智能等領(lǐng)域的快速發(fā)展，采摘機(jī)器人技術(shù)已從前瞻性研究開始成為現(xiàn)實需求[4]。采摘機(jī)器人的市場應(yīng)用可以大幅減少人力成本。但是在非結(jié)構(gòu)性的自然環(huán)境下，被采摘柑橘常常會被枝干遮擋，給機(jī)器人的自動采摘作業(yè)造成很大困難。這就需要機(jī)械臂具有自主運動規(guī)劃的能力，即避開障礙，到達(dá)柑橘目標(biāo)。因此機(jī)械臂的運動規(guī)劃問題成為柑橘采摘機(jī)器人的關(guān)鍵問題之一[5]。

機(jī)械臂的運動規(guī)劃與智能車等的路徑規(guī)劃不同，機(jī)械臂運動規(guī)劃不是一個點的避障軌跡規(guī)劃，而是在給定障礙物以及機(jī)械臂起始、目標(biāo)位姿的條件下，搜尋一組連續(xù)的關(guān)節(jié)角度值序列，該角度序列能夠驅(qū)動機(jī)械臂安全、無碰撞地從起始位姿運動到目標(biāo)位姿[6]。機(jī)械臂的自由度往往更多，這就導(dǎo)致了在智能車上應(yīng)用較多的A*、Dijkstra等算法應(yīng)用在機(jī)械臂上時會出現(xiàn)“指數(shù)爆炸”難題[7]，為了應(yīng)對這一難題國內(nèi)外學(xué)者對機(jī)械臂避障路徑規(guī)劃問題開展了大量研究。祁若龍等[8]將空間機(jī)械臂運動規(guī)劃問題轉(zhuǎn)化為一個多目標(biāo)優(yōu)化求解的問題，然后利用遺傳算法在構(gòu)型空間下規(guī)劃出一條無碰撞的動作序列。但是隨著自由度增加遺傳算法的待求解參數(shù)增加、碰撞檢測的計算成本增加，計算時間將會隨之成指數(shù)形式增長；賈慶軒等[9]先依據(jù)障礙物位姿坐標(biāo)求解機(jī)械臂的無碰撞自由工作空間，然后利用A*算法在空間機(jī)械臂的自由工作空間進(jìn)行無碰撞路徑搜索；Gómez-Bravo等[10]首先利用快速擴(kuò)展隨機(jī)樹法（rapidly exploring random tree,RRT）提供一組可行的路徑，然后使用遺傳算法根據(jù)適應(yīng)度函數(shù)對原始解進(jìn)行優(yōu)化；蔡健榮等[11]利用概率地圖法（probabilistic road map method，PRM）對多自由度機(jī)械臂進(jìn)行避障路徑規(guī)劃，該方法不需要求障礙物在機(jī)械臂構(gòu)形空間中的精確映射模型，只要通過采樣獲得部分機(jī)械臂構(gòu)形空間內(nèi)的無碰撞構(gòu)型，通過連接無碰撞構(gòu)型獲得機(jī)械臂避障路徑，但受限于迭代次數(shù)、搜索時間等因素，無法求解出所有可能的路徑，也無法求解到最優(yōu)解；姚立健等[12]對SCORBOT-ER4u型茄子采摘機(jī)械臂進(jìn)行了避障運動規(guī)劃，算法將障礙物映射到收獲機(jī)械臂構(gòu)形空間中，在構(gòu)形空間中利用A*算法搜索避障路徑，但該方法為對路徑規(guī)劃模型進(jìn)行簡化將空間障礙物投影到二維平面中，無法充分發(fā)揮高自由度采摘機(jī)械臂在避障方面的優(yōu)勢；尹建軍等[13]將高自由度機(jī)械臂避障路徑規(guī)劃問題轉(zhuǎn)化為多個平面2轉(zhuǎn)動關(guān)節(jié)機(jī)械臂避障問題，但算法需要對高自由度機(jī)械臂整體進(jìn)行逆運動學(xué)分析，并在每個無障礙的平面內(nèi)搜尋關(guān)節(jié)角度，算法前期理論分析較復(fù)雜。羅陸鋒等[14]則采用人工勢場法對采摘機(jī)械臂進(jìn)行無碰運動規(guī)劃，在仿真試驗中，34次規(guī)劃成功了28次。謝龍等[15]對人工勢場法進(jìn)行改進(jìn)，目標(biāo)點對機(jī)械臂的末端施加引力，障礙物則對最近機(jī)械臂的連桿施加斥力，在兩個力的合力下引導(dǎo)機(jī)械臂到達(dá)目標(biāo)點。鄒宇星等[16]通過對PRM算法進(jìn)行改進(jìn)，對采摘機(jī)器人機(jī)械臂進(jìn)行運動規(guī)劃，首先對碰撞檢測數(shù)據(jù)庫進(jìn)行構(gòu)建以減少每次規(guī)劃的碰撞檢測時間，再利用PRM進(jìn)行運動規(guī)劃。Nguyen等[3]對蘋果采摘環(huán)境下多種算法進(jìn)行了試驗驗證，發(fā)現(xiàn)雙向連接快速擴(kuò)展隨機(jī)樹算法（RRT-connect）效果最好?？梢钥吹侥壳皩τ跈C(jī)械臂運動規(guī)劃的研究主要集中在A*、人工勢場法、PRM、RRT、RRT-connect等算法上，但A*、人工勢場法在高維機(jī)械臂上存在計算量大等問題，而基于隨機(jī)采樣的PRM、RRT、RRT-connect等算法，尤其是RRT-connect在高自由度機(jī)械臂表現(xiàn)較好，但是針對柑橘采摘環(huán)境下的運動規(guī)劃問題研究還較為缺乏。

本文基于團(tuán)隊所研發(fā)的柑橘采摘機(jī)器人，針對機(jī)械臂有時需要深入樹冠內(nèi)部進(jìn)行采摘，樹冠內(nèi)眾多枝干往往構(gòu)成一個個封閉的多邊形通道的情況，對采摘環(huán)境進(jìn)行合理歸納，機(jī)械臂需要深入到封閉多邊形內(nèi)部，對果實進(jìn)行采摘。本文提出了一種構(gòu)型空間投影降維方法將六維構(gòu)型空間投影為三維進(jìn)行可視化，方便對構(gòu)型空間進(jìn)行分析，為運動規(guī)劃算法的開發(fā)提供指導(dǎo)。針對封閉多邊形投影后的構(gòu)型空間進(jìn)行分析，提出了一種針對該采摘環(huán)境的基于構(gòu)型空間先驗知識引導(dǎo)點的雙向連接快速擴(kuò)展隨機(jī)樹運動規(guī)劃算法，并進(jìn)行了改進(jìn)前后算法的并行編程，以提高不同環(huán)境適應(yīng)性，最后通過仿真試驗驗證所提出的方法與現(xiàn)有RRT-connect算法相比具有顯著優(yōu)越性。

1 機(jī)械臂構(gòu)型空間降維映射建模

針對研發(fā)的采摘機(jī)器人，使用本文提出的基于降維投影的構(gòu)型空間映射方法對底盤（本文底盤是對底盤和外殼的統(tǒng)稱）在構(gòu)型空間的映射進(jìn)行了建模，然后對封閉多邊形障礙物在構(gòu)型空間的映射進(jìn)行了離線構(gòu)建，為規(guī)劃算法的改進(jìn)提供依據(jù)。

1.1 采摘機(jī)器人原理

本文所采用的采摘機(jī)器人為團(tuán)隊研發(fā)，結(jié)構(gòu)如圖1所示，該機(jī)器人主要由機(jī)械臂、移動底盤及視覺系統(tǒng)等組成，機(jī)械臂采用遨博公司生產(chǎn)的aubo_i5六自由度機(jī)械臂，臂展1 m，安裝位置距離地面1 m，采摘范圍為0～2 m，可以對矮化的柑橘樹進(jìn)行采摘，移動底盤采用山東極創(chuàng)機(jī)器人公司Komodo-2型履帶底盤，上方安裝有自主設(shè)計的外殼，視覺系統(tǒng)由BB2雙目相機(jī)獲取柑橘及障礙物信息[17]，實驗室前期研究中雙目相機(jī)精度達(dá)到1.58%[18]，能夠滿足采摘作業(yè)中的定位要求。機(jī)械臂經(jīng)過運動規(guī)劃到達(dá)采摘位置，末端執(zhí)行器進(jìn)行果柄剪切及果實夾持[19-20]，然后通過外殼上方的開孔將柑橘放入果籃。由于本文在于研究機(jī)械臂避障方法的可行性，因此對末端執(zhí)行器的影響暫未考慮。

1. 履帶底盤 2. 雙目相機(jī) 3. 機(jī)械臂 4. 末端執(zhí)行器 5. 外殼 6. 果籃 7. 工控機(jī)

1.2 構(gòu)型空間降維映射建模

對于機(jī)械臂，點的坐標(biāo)則表示為每個關(guān)節(jié)的角度值(1,2,…,θ)，六自由度的機(jī)械臂對應(yīng)六維構(gòu)型空間：6。過高的維度導(dǎo)致了難以準(zhǔn)確表達(dá)構(gòu)型空間[21]，且無法對高維空間進(jìn)行可視化觀察，增大了分析難度。因此?的降維處理對于算法的前期理論分析尤為重要，本文提出一種降維投影的方法：對避障影響最大的前3個大關(guān)節(jié)進(jìn)行構(gòu)型空間構(gòu)建并依據(jù)后3個關(guān)節(jié)進(jìn)行修正，將構(gòu)型空間由六維投影為三維：≈6×-3。

具體過程為：使用蒙特卡洛方法對前3個關(guān)節(jié)（肩關(guān)節(jié)、大臂關(guān)節(jié)、小臂關(guān)節(jié)）角度值進(jìn)行隨機(jī)采樣，后3個關(guān)節(jié)（腕關(guān)節(jié)1、腕關(guān)節(jié)2、腕關(guān)節(jié)3）角度值為0，每次采樣對機(jī)械臂進(jìn)行碰撞檢測，判斷該點在free或obs，若無碰撞則該構(gòu)型在free，為了減少投影過程中后3個關(guān)節(jié)的信息丟失量，再以隨機(jī)采樣的方式更改碰撞構(gòu)型的后3個關(guān)節(jié)角度值，循環(huán)500次進(jìn)行碰撞檢測，若有無碰撞構(gòu)型，說明前3個關(guān)節(jié)角度值為當(dāng)前值時，存在與障礙物的無碰撞構(gòu)型，則將該點列入free，反之認(rèn)為前3個關(guān)節(jié)角度值為當(dāng)前值時所有構(gòu)型均為碰撞構(gòu)型，列入obs得到點云數(shù)據(jù)，將obs的點進(jìn)行繪制，應(yīng)用alphashape算法提取點云輪廓[22]，使構(gòu)型空間可視化，便于分析，流程如圖2所示。

首先對底座在構(gòu)型空間內(nèi)的映射base進(jìn)行建模。為方便觀察，base為半透明，顏色較深處為free與base交界面，如圖3a，可以看到構(gòu)型空間邊界由障礙物包圍，內(nèi)部為free空腔，機(jī)械臂在內(nèi)部運行。

注：為6個關(guān)節(jié)角度值。

底座構(gòu)型空間的映射有助于分析底座外殼的合理性，是否對機(jī)械臂運動規(guī)劃造成了不良影響，對于改進(jìn)底座的外形有指導(dǎo)意義。

構(gòu)型空間中封閉多邊形的拓?fù)湫再|(zhì)是同胚的[23-24]。對封閉多邊形枝干障礙物在構(gòu)型空間的映射limb進(jìn)行了建模分析。圖3b、圖3c分別為正方框形障礙物、圓環(huán)形障礙物在構(gòu)型空間映射的不同角度視圖。

通過圖3b、圖3c發(fā)現(xiàn)正方框形與圓環(huán)形在構(gòu)型空間內(nèi)的投影拓?fù)涞男再|(zhì)：封閉多邊形障礙物的構(gòu)型空間投影可以簡化為上下2部分，上下2部分會有部分聯(lián)結(jié)，中間部分為空腔與外界free相通，且只有上下兩個開口，若機(jī)械臂終點位姿位于空腔內(nèi)（機(jī)械臂伸入封閉多邊形）則只能通過上下開口（1為3.14和0時，實際為同一個構(gòu)型，在圖中顯示為開口，實為封閉）。

注：從左至右代表不同視角下構(gòu)型空間的視圖，視角方向依次為(1,1,1)、(0,1,1.9)、(1,0,0)。

為了對構(gòu)型空間有整體觀察，將底盤和障礙物（40 cm框型為例）的映射進(jìn)行并集操作如圖3d所示，空白部分為機(jī)械臂可以無碰撞運動的自由空間。

對構(gòu)型空間的降維映射建模為離線構(gòu)建，由于封閉多邊形障礙物的拓?fù)浣Y(jié)構(gòu)相似性，對構(gòu)型空間的建模目的在于探尋不同的封閉多邊形障礙物的共同特性，為規(guī)劃算法提供改進(jìn)依據(jù)，無需每次規(guī)劃都對構(gòu)型空間進(jìn)行構(gòu)建。

2 基于構(gòu)型空間引導(dǎo)點的RRT-connect算法

首先對RRT-connect算法及橋測試原理進(jìn)行闡述，然后對構(gòu)型空間的映射進(jìn)行簡化分析，以此作為先驗知識，提出了針對深入樹冠內(nèi)部采摘任務(wù)的基于先驗知識的橋測試策略，以此策略對RRT-connect算法進(jìn)行改進(jìn)，提出基于構(gòu)型空間引導(dǎo)點的RRT-connect算法，并對RRT及改進(jìn)后的算法進(jìn)行并行編程，解決2種算法在不同環(huán)境下適應(yīng)性較差的問題。

2.1 RRT-connect算法及橋測試原理

RRT-connect算法[25]的主要思想是：從初始點和目標(biāo)點出發(fā)，并行構(gòu)建2棵搜索樹；在每次迭代過程中，2棵搜索樹總是彼此朝著對方擴(kuò)展，直至2棵搜索樹相遇為止。RRT-connect構(gòu)建過程如下：在每次迭代中，先擴(kuò)展其中一棵搜索樹，然后嘗試將另一棵搜索樹擴(kuò)展到當(dāng)前搜索樹擴(kuò)展的新節(jié)點；2棵搜索樹init和goal交替擴(kuò)展，直至2棵搜索樹相遇為止[26]。

對柑橘采摘作業(yè)中常見的封閉多邊形障礙物降維投影到了構(gòu)型空間后，對空間進(jìn)行進(jìn)一步抽象簡化，如圖4，起始構(gòu)型init點，到達(dá)終點goal，只能通過上下開口，這種環(huán)境為窄通道環(huán)境。由于RRT-connect對于窄通道的通過性較差，針對這一問題有研究人員提出使用橋測試算法尋找中間點引導(dǎo)通過窄通道[27]，橋測試算法是在構(gòu)型空間隨機(jī)采樣2個端點，若2個端點位于obs且它們的中點mid位于free則認(rèn)為通過了橋測試，mid被認(rèn)為位于窄通道處，但往往該算法會在free進(jìn)行大量無用的搜索，且實際的構(gòu)型空間內(nèi)往往有很多凹區(qū)域，會導(dǎo)致橋測試算法找到不合適的中間構(gòu)型。

注：qinit為起始構(gòu)型，qgoal為目標(biāo)構(gòu)型,qmid為引導(dǎo)構(gòu)型。

2.2 基于先驗知識的橋測試策略

由于傳統(tǒng)的橋測試進(jìn)行全局搜索，會浪費大量時間在無用區(qū)域上，且由于凹區(qū)域的存在，使得使用橋測試往往無法找到正確的窄通道位置。若機(jī)械臂伸入封閉多邊形框內(nèi)，在構(gòu)型空間內(nèi)則體現(xiàn)為由障礙物外部空間進(jìn)入到空腔中，需要通過上下開口處的窄通道位置，這可以視為先驗知識。

有了這個先驗知識，就可以制定策略，縮小橋測試的搜索范圍，已經(jīng)知道窄通道的粗略位置，則只需在這2個開口處進(jìn)行檢測，如圖4所示。為了減少碰撞檢測量，只在、兩條線段上進(jìn)行橋測試：由于底盤的構(gòu)型空間映射總是固定的，所以將線段端點設(shè)在處將不必對底盤進(jìn)行碰撞檢測，只需對障礙物進(jìn)行碰撞檢測，在線段上隨機(jī)取點mid，若mid位于free，則mid把分為2段mid,mid。若這2段進(jìn)行碰撞檢測都無法通過，則認(rèn)為mid位于窄通道位置。

具體流程為：

1）獲取起點、終點關(guān)節(jié)角度值init、goal。

2）確定mid的1起始值，為盡可能縮短總的路徑，mid的1初始值取init及goal的中值。

3）由于1值已經(jīng)被賦予初始值，此時線段位于mid?1的橫切面上，隨機(jī)取(01)區(qū)間內(nèi)的數(shù)值，作為對插值比例。

4）對mid進(jìn)行碰撞檢測，判斷mid位于free還是obs。

5）若mid位于free則分別對mid、mid進(jìn)行碰撞檢測，反之進(jìn)入步驟7。

6）若mid,mid均有碰撞則mid位于窄通道位置，結(jié)束；反之，返回步驟3）。若重復(fù)30次無法找到窄通道處mid則進(jìn)入步驟7）。

7）步驟6）中沒有找到窄通道，則認(rèn)為在當(dāng)前1下不存在通道，對1進(jìn)行“震蕩”調(diào)整，返回步驟3?！罢鹗帯闭{(diào)整即以增量的方式左右搖擺：1、1+01、1?01、1+02、1?02、1+03、…，直到找到窄通道或1超出±3.14。

8）若步驟7）無法找到mid，則將線段換為線段，重復(fù)步驟3）。

通過上述步驟找到窄通道處的引導(dǎo)構(gòu)型后分別對initmid、midgoal運行RRT-connect算法進(jìn)行運動規(guī)劃：由引導(dǎo)構(gòu)型mid向init規(guī)劃使得RRT-connect不用花費大量的時間去探索尋找窄通道的出入口，mid位于出入口處，再結(jié)合RRT-connect的雙向生長特性使得mid與init、goal相連接都比較容易。由此得到基于構(gòu)型空間先驗知識引導(dǎo)點的RRT-connect算法（RRT-connect with informed guidance point, IGPRRT-connect），算法整體流程如圖5所示。

圖5 IGPRRT-connect流程圖

2.3 2種算法的并行編程

IGPRRT-connect只針對封閉多邊形這一特定環(huán)境，在單根枝干的情況下會消耗大量時間去尋找并不存在的中間構(gòu)型，這就需要人為去判斷在當(dāng)前環(huán)境下需要RRT-connect算法還是IGPRRT-connect算法。為了解決這一問題，采用并行編程的方法，創(chuàng)建2個線程：一個線程運行RRT-connect，另一個線程運行IGPRRT-connect算法，當(dāng)其中一個線程完成規(guī)劃，2個線程全部停止，輸出規(guī)劃路徑，這樣就避免了人為選擇算法，提高了采摘機(jī)器人的智能程度，并行編程有利于簡化程序解決方案：無需再編寫算法判斷當(dāng)前環(huán)境下該采用RRT-connect還是IGPRRT-connect。并行計算框架如圖6所示。

圖6 RRT-connect和IGPRRT-connect算法并行規(guī)劃框架

3 仿真及試驗驗證

本文搭建了柑橘采摘機(jī)器人仿真與控制平臺，然后對IGPRRT-connect與并行算法進(jìn)行仿真與室內(nèi)試驗 驗證。

3.1 仿真與控制平臺搭建

基于機(jī)器人操作系統(tǒng)（robot operating system，ROS）[28]開發(fā)了柑橘采摘機(jī)器人仿真與控制平臺，其界面如圖7所示，進(jìn)行柑橘采摘機(jī)器人運動規(guī)劃仿真與控制方面的研究。該平臺集成了碰撞檢測、正逆運動學(xué)求解、軌跡平滑、軌跡可視化等功能，部分功能的插件選擇及參數(shù)設(shè)置如表1所示。

整個系統(tǒng)避障任務(wù)工作流程如圖8，逆運動學(xué)算法求解任務(wù)信息中目標(biāo)位置對應(yīng)的機(jī)械臂逆解，提供給RRT- connect算法作為構(gòu)型空間規(guī)劃目標(biāo)，RRT-connect在節(jié)點擴(kuò)展過程中調(diào)取碰撞檢測插件進(jìn)行路徑碰撞檢測。得到無碰撞路徑后，對路徑應(yīng)用5次樣條曲線進(jìn)行路徑平滑。然后平滑后的路徑數(shù)據(jù)通過CAN總線發(fā)送給驅(qū)動器，由驅(qū)動器驅(qū)動電機(jī)轉(zhuǎn)動完成機(jī)器人的采摘動作。機(jī)器人采集的位置、速度等信息實時反饋到規(guī)劃場景中，對規(guī)劃場景中的機(jī)械臂各關(guān)節(jié)位置進(jìn)行更新。

圖7 柑橘采摘機(jī)器人仿真與控制平臺界面（移動底盤以包圍盒方法進(jìn)行了簡化）

表1 集成的插件及參數(shù)設(shè)置

圖8 柑橘采摘機(jī)器人避障運動規(guī)劃仿真及控制系統(tǒng)

3.2 仿真試驗

3.2.1 試驗設(shè)計

為驗證所提出IGPRRT-connect算法以及RRT- connect與IGPRRT-connect并行計算算法的性能（下文簡稱并行算法），在搭建的仿真平臺下進(jìn)行仿真試驗，試驗環(huán)境為在windows下安裝的Ubuntu虛擬機(jī)，分配的內(nèi)存大小為4 GB、4核，GPU為NVIDIA GeForce GTX 1060 6 GB。

為驗證本文提出的IGPRRT-connect算法的適應(yīng)范圍和使用并行算法的效果，進(jìn)行了2個仿真試驗：1）設(shè)定不同正方形邊長障礙物試驗，來驗證不同大小封閉障礙物下各算法的表現(xiàn)，將作為障礙物的正方形框的邊長逐次減小，以5 cm為間隔，從50到25 cm，共進(jìn)行6組試驗，每組試驗分別應(yīng)用IGPRRT-connect、RRT-connect及并行算法進(jìn)行20次規(guī)劃，并計算平均規(guī)劃時間；2）設(shè)置不同形狀障礙物試驗，來進(jìn)一步驗證驗證所提出算法的優(yōu)越性，障礙物共分為6組：4組不同形狀位置的封閉多邊形框、1組兩根平行障礙物，1組V型障礙物，每組試驗同樣應(yīng)用3個算法進(jìn)行20次規(guī)劃，統(tǒng)計規(guī)劃時間。

3.2.2 結(jié)果對比分析

不同正方形框邊長障礙物試驗過程如圖9所示，圖10為使用RRT-connect、IGPRRT-connect和并行算法3種算法進(jìn)行運動規(guī)劃出的平均時間。

a. 起始構(gòu)型 a. Initial configurationb. 目標(biāo)構(gòu)型 b. Target configurationc. 運動軌跡 c. Trajectoryd. 引導(dǎo)構(gòu)型 d. Guided configuration

注：并行算法為RRT-connect與IGPRRT-connect雙線程并行規(guī)劃算法。

由圖10可看出，當(dāng)障礙物邊長≥40 cm時，3種算法規(guī)劃時間相差不大，均為1.5 s左右，當(dāng)障礙物邊長≤35 cm時，RRT-connect算法規(guī)劃時間開始大幅增加，IGPRRT-connect與并行算法規(guī)劃時間增加的幅度較?。哼呴L為35、30、25 cm時，RRT-connect規(guī)劃時間分別為1.7、3.1、18.1 s，IGPRRT-connect規(guī)劃時間分別為1.2、1.6、2.6 s，相比RRT-connect減少了29%、48%、85%，并行算法規(guī)劃時間分別為1.3、1.6、2.3 s，相比RRT-connect減少了24%、51%、86%。在不同障礙物大小下，IGPRRT-connect平均規(guī)劃時間1.56 s相比RRT-connect的4.33 s減少了64%，說明IGPRRT-connect算法具有較大的優(yōu)越性。

不同形狀的障礙物仿真規(guī)劃如圖11a所示。其中不透明機(jī)械臂為起始構(gòu)型，半透明為終點構(gòu)型，曲線為某次試驗的運動軌跡。數(shù)據(jù)如圖11b所示，可以看到并行算法在各種環(huán)境下相比RRT-connect和IGPRRT- connect更加穩(wěn)定，在第1、3組時所消耗的平均時間最少，雖然第2、4、5、6組試驗并行算法平均時間并不是最少，但是與消耗時間最少的算法相差不大。在第1、2、3、4組試驗中RRT-connect算法規(guī)劃時間大幅大于IGPRRT- connect與并行算法，進(jìn)一步驗證了本文所提出的IGPRRT-connect算法優(yōu)越性。而第6組試驗說明在障礙物不封閉的情況下，IGPRRT-connect失去優(yōu)勢，時間大幅增加，時間主要消耗在尋找并不存在的引導(dǎo)構(gòu)型， 6組試驗中RRT-connect平均耗時為2.94 s，IGPRRT-connect為1.37 s，相比RRT-connect降低了53%，并行算法為1.26 s，相比RRT-connect降低了57%。

注：6組障礙物形狀分別為長方框形障礙物、不規(guī)則多邊框形障礙物、平行障礙物和V形障礙物。

仿真結(jié)果驗證了IGPRRT-connect算法在障礙物為封閉多邊形時的優(yōu)勢，以及將RRT-connect、IGPRRT- connect并行編程的并行算法的適應(yīng)性。

3.3 室內(nèi)試驗

3.3.1 試驗設(shè)計

為了驗證采摘機(jī)器人平臺的可行性，本節(jié)應(yīng)用并行算法進(jìn)行室內(nèi)避障試驗，控制主機(jī)內(nèi)存大小為4GB、4核。由鋁質(zhì)型材搭建坐標(biāo)系框架，原點為機(jī)械臂安裝位置垂直于地面的點，障礙物位置由導(dǎo)軌調(diào)整，如圖12所示。共進(jìn)行2組試驗，一組為封閉多邊形障礙物，封閉多邊形障礙物采用30、40 cm邊長的正方形框及30、40、50 cm邊長的菱形框模擬樹冠內(nèi)部的封閉多邊形，另一組為邊長為30、40、50 cm的未封閉V形障礙物，模擬分叉枝干，障礙物由木條組成，布置范圍距離機(jī)器人底座0.5～0.8 m，每組由隨機(jī)起始構(gòu)型試驗規(guī)劃50次，統(tǒng)計平均規(guī)劃時間及成功率。

3.3.2 結(jié)果及分析

圖13為采摘機(jī)械臂在仿真及實驗室環(huán)境下的避障試驗運動過程。試驗統(tǒng)計結(jié)果如表2。

圖12 室內(nèi)試驗環(huán)境

a. 仿真運動過程a. Simulation for motion process b. 實際運動過程b. Actual motion process

表2 并行算法室內(nèi)試驗結(jié)果

注：每組共進(jìn)行50次試驗。

Note: Fifty experiments are conducted in each group.

可以看到并行算法在各種環(huán)境下表現(xiàn)穩(wěn)定，失敗的原因主要為障礙物布置過程中產(chǎn)生的誤差及機(jī)械臂運動誤差所導(dǎo)致的碰撞。

室內(nèi)試驗結(jié)果表明：使用本文提出的并行算法，對封閉障礙物和未封閉障礙物的均有較好的避障效果，平均規(guī)劃時間為1.263 s，避障成功率為91%?？蔀楦涕俨烧獧C(jī)器人的采摘作業(yè)提供新的運動規(guī)劃算法選擇，為進(jìn)一步的研究工作奠定了基礎(chǔ)。

4 結(jié) 論

本文提出一種構(gòu)型空間映射方法將高維構(gòu)型空間進(jìn)行降維投影并可視化，針對柑橘采摘環(huán)境，將封閉多邊形障礙物映射到構(gòu)型空間進(jìn)行分析，并提出了基于構(gòu)型空間先驗知識的引導(dǎo)點RRT-connect算法（IGPRRT-connect），并針對IGPRRT-connect在非封閉環(huán)境下適應(yīng)性差的問題，將RRT-connect與IGPRRT-connect算法進(jìn)行并行規(guī)劃編程。

所提出的構(gòu)型空間映射方法主要考慮了對機(jī)械臂影響較大的前3個關(guān)節(jié)，同時也將后3個關(guān)節(jié)考慮對避障的影響考慮在內(nèi)，減小了降維過程中的信息丟失量，構(gòu)型空間的可視化對于后期的規(guī)劃算法分析具有一定指導(dǎo)意義。對RRT-connect算法改進(jìn)的仿真結(jié)果表明，所提出的IGPRRT-connect算法在封閉多邊形環(huán)境下規(guī)劃時間大幅減少，在不同邊長正方形框障礙物仿真下，平均規(guī)劃時間為1.56 s，降低了64%，而RRT-connect與IGPRRT- connect的并行算法則在不同的形狀障礙物環(huán)境下都具有很好的表現(xiàn)，平均規(guī)劃時間為1.26 s，降低了57%。在室內(nèi)試驗中，并行算法平均規(guī)劃時間為1.263 s，避障成功率為91%。所提出的IGPRRT-connect算法解決了在RRT-connect封閉通道下規(guī)劃時間長的問題，進(jìn)一步的并行規(guī)劃算法則提高了對不同環(huán)境的適應(yīng)性，對柑橘采摘機(jī)器人的研究具有重要意義。

[1] 王亞珍，汪安明，趙坤，等. 基于薄壁圓環(huán)理論的機(jī)器人用柔性軸承變形特征快速求解[J]. 農(nóng)業(yè)工程學(xué)報，2019，35(3)：60－66. Wang Yazhen, Wang Anming, Zhao Kun, et al. Fast solution for deformation characteristics of flexible bearing of robot based on thin-walled ring theory[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(3): 60－66. (in Chinese with English abstract)

[2] 宋健，張鐵中，徐麗明，等. 果蔬采摘機(jī)器人研究進(jìn)展與展望[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2006，37(5): 158－162. Song Jian, Zhang Tiezhong, Xu Liming, et al. Research actuality and prospect of picking robot for fruits and vegetables[J]. Transactions of the Chinese Society for Agricultural Machinery, 2006, 37(5): 158－162. (in Chinese with English abstract)

[3] Nguyen T T, Kayacan E, Baedemaeker J D, et al. Task and motion planning for apple harvesting robot[J]. Ifac Proceedings Volumes, 2013, 46(18): 247－252.

[4] 劉繼展. 溫室采摘機(jī)器人技術(shù)研究進(jìn)展分析[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2017，48(12): 1－18. Liu Jizhan. Research progress analysis of robotic harvesting technologies in greenhouse[J]. Transactions of The Chinese Society for Agricultural Machinery, 2017, 48(12): 1－18. (in Chinese with English abstract)

[5] 陽涵疆，李立君，高自成. 基于關(guān)節(jié)構(gòu)形空間的混聯(lián)采摘機(jī)械臂避障路徑規(guī)劃[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(4): 55－62. Yang Hanjiang, Li Lijun, Gao Zicheng. Obstacle avoidance path planning of hybrid harvesting manipulator based on jointconfiguration space[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 55－62. (in Chinese with English abstract)

[6] Lozano-P, Rez T. Spatial Planning: A Configuration Space Approach[M]. New York: Springer, 1990.

[7] 唐華斌，王磊，孫增圻. 基于隨機(jī)采樣的運動規(guī)劃綜述[J]. 控制與決策，2005，20(7): 4－9. Tang Huabin, Wang Lei, Sun Zengqi. A survey on randomized sampling-based motion planning[J]. Control and Decision, 2005, 20(7): 4－9. (in Chinese with English abstract)

[8] 祁若龍，周維佳，王鐵軍. 一種基于遺傳算法的空間機(jī)械臂避障軌跡規(guī)劃方法[J]. 機(jī)器人，2014，36(3)：263－270. Qi Ruolong, Zhou Weijia, Wang Tiejun. An obstacle avoidance trajectory planning scheme for space manipulators based on genetic algorithm[J]. Robot, 2014, 36(3): 263－270. (in Chinese with English abstract)

[9] 賈慶軒，陳鋼，孫漢旭，等. 基于A*算法的空間機(jī)械臂避障路徑規(guī)劃[J]. 機(jī)械工程學(xué)報，2010，46(13)：109－115. Jia Qingxuan, Chen Gang, Sun Hanxu, et al. Path planning for space manipulator to avoid obstacle based on A* algorithm[J]. Journal of Mechanical Engineering, 2010, 46(13): 109－115. (in Chinese with English abstract)

[10] Gómez-Bravo F, Carbone G, Fortes J C. Collision free trajectory planning for hybrid manipulators[J]. Mechatronics, 2012, 22(6): 836－851.

[11] 蔡健榮，趙杰文，Thomas Rath，等. 水果收獲機(jī)器人避障路徑規(guī)劃[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2007，38(3)：102－105. Cai Jianrong, Zhao Jiewen, Thomas Rath, et al. Path planning of fruits harvesting robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2007, 38(3): 102－105. (in Chinese with English abstract)

[12] 姚立健，丁為民，陳玉侖，等. 茄子收獲機(jī)器人機(jī)械臂避障路徑規(guī)劃[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2008，39(11)：94－98. Yao Lijian, Ding Weimin, Chen Yulun, et al. Obstacle avoidance path planning of eggplant harvesting robot manipulator[J]. Transactions of the Chinese Society for Agricultural Machinery, 2008, 39(11): 94－98. (in Chinese with English abstract)

[13] 尹建軍，武傳宇，Yang Simon X，等. 番茄采摘機(jī)器人機(jī)械臂避障路徑規(guī)劃[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2012，43(12): 171－175. Yin Jianjun, Wu Chuanyu, Yang Simon X, et al. Obstacle- avoidance path planning of robot arm for tomato-picking robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2012, 43(12): 171－175. (in Chinese with English abstract)

[14] 羅陸鋒，鄒湘軍，程堂燦，等. 采摘機(jī)器人視覺定位及行為控制的硬件在環(huán)虛擬試驗系統(tǒng)設(shè)計[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(4)：39－46. Luo Lufeng, Zou Xiangjun, Cheng Tangcan, et al. Design of virtual test system based on hardware-in-loop for picking robot vision localization and behavior control[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 39－46. (in Chinese with English abstract)

[15] 謝龍，劉山. 基于改進(jìn)勢場法的機(jī)械臂動態(tài)避障規(guī)劃[J]. 控制理論與應(yīng)用，2018，35(9): 1239－1249. Xie Long, Liu Shan. Dynamic obstacle-avoiding motion planning for manipulator based on improved artificial potential filed[J]. Control Theory & Applications, 2018, 35(9): 1239－1249. (in Chinese with English abstract)

[16] 鄒宇星，李立君，高自成. 基于改進(jìn)PRM的采摘機(jī)器人機(jī)械臂避障路徑規(guī)劃[J]. 傳感器與微系統(tǒng)，2019，38(1): 52－56. Zou Yuxing, Li Lijun, Gao Zicheng. Obstacle avoidance path planning for harvesting robotarm based on improved PRM[J]. Transducer and Microsystem Technologies, 2019, 38(1): 52－56. (in Chinese with English abstract)

[17] Liu Y P, Yang C H A, Huang L, et al. Visual system of citrus picking robot using convolutional neural networks[C]// International Conference on Systems and Informatics, [S.l.]: IEEE, 2018: 344－349. DOI: 10.1109/ICSAI.2018.8599325

[18] 李揚(yáng)，楊長輝，胡友呈，等. 基于凸殼及距離變換的重疊柑橘目標(biāo)識別與定位方法[J]. 現(xiàn)代制造工程，2018(9): 82－87. Li Yang, Yang Changhui, Hu Youcheng, et al. Overlapping citrus target recognition and localization method based on convex shell and distance transformation[J]. Modern Manufacturing Engineering, 2018(9): 82－87. (in Chinese with English abstract)

[19] 王毅，許洪斌，張茂，等. 仿蛇嘴咬合式柑橘采摘末端執(zhí)行器設(shè)計與實驗[J]. 農(nóng)業(yè)機(jī)械學(xué)報，2018，49(10)：61－71. Wang Yi, Xu Hongbin, Zhang Mao, et al. Design and experiment of bite-model end-effector for citrus harvesting by simulating with mouth of snake[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(10): 61－71. (in Chinese with English abstract)

[20] 王毅，付舜，張哲，等. 柑橘采摘機(jī)器人末端執(zhí)行器設(shè)計與試驗[J]. 中國農(nóng)業(yè)科技導(dǎo)報，2018，20(1)：69－77. Wang Yi, Fu Shun, Zhang Zhe, et al. Design and experimentation on end-effector of citrus picking robot[J]. Journal of Agricultural Science and Technology, 2018, 20(1): 69－77. (in Chinese with English abstract)

[21] Pan Jia. Efficient configuration space construction and optimization for motion planning[J]. Engineering, 2015, 1(1): 46－57.

[22] 范啟忠，陳是扦，刁燕，等. 基于Alpha Shapes三維重建原理的機(jī)器人工作空間求解方法[J]. 機(jī)械傳動，2014，38(9): 27－29，59. Fan Qizhong, Chen Shiqian, Diao Yan, et al. Solution method of robot workspace based on Alpha Shapes 3D reconstruction principle[J]. Journal of Mechanical Transmission, 2014,38(9): 27－29,59. (in Chinese with English abstract)

[23] 張鳳茹. 同胚映射在拓?fù)淇臻g中的應(yīng)用[J]. 哈爾濱師范大學(xué)自然科學(xué)學(xué)報，2010，26(4): 19－21. Zhang Fengru. The application of homeomorphic mapping in topological space[J]. Natural Science Journal of Harbin Normal University, 2010, 26(4): 19－21. (in Chinese with English abstract)

[24] Lavalle S M. Planning Algorithms[M]. Cambridge: Cambridge University Press, 2006: 127－133.

[25] Lozano-Perez T. Automatic Planning of Manipulator Transfer Movements[J]. IEEE Transactions on Systems Man & Cybernetics, 1980, 11(10): 681－698.

[26] Kuffner J J, Lavalle S M. RRT-connect: An efficient approach to single-query path planning[C]//IEEE International Conference on Robotics and Automation, 2000. Proceedings. ICRA. IEEE, 2002: 995－1001.

[27] Sun Z, Hsu D, Jiang T, et al. Narrow passage sampling for probabilistic roadmap planning[J]. IEEE Transactions on Robotics, 2005, 21(6): 1105－1115.

[28] ROS|Powering the world's robots[EB/OL]. http://www.ros. org/

[29] Pan J, Chitta S, Manocha D. FCL: A general purpose library for collision and proximity queries[C]//International Conference on Robotics and Automation. Saint Paul: IEEE, 2012: 3859－3866.

[30] Beeson P, Ames B. TRAC-IK: An open-source library for improved solving of generic inverse kinematics[C]// International Conference on Humanoid Robots. [S.l.]: IEEE, 2015: 928－935.

Motion planning of citrus harvesting manipulator based on informed guidance point of configuration space

Ma Jitong1, Wang Yi1,2※, He Yu1, Wang Kai1, Zhang Yitan1

(1.400054,; 2.400044,)

Harvesting robot is the representative of agricultural intellectualization. In the process of citrus harvesting, the manipulator sometimes needs to go deep into the canopy to harvest citrus. Many branches inside the canopy often form a closed polygon channel. Compared with single branch obstacle, the closed polygon obstacle is more difficult to avoid and it takes longer time to plan trajectory for obstacle avoidance. In order to solve this problem, an off-line configuration space mapping method is proposed in this paper, which can reduce the dimension and visualize the high-dimensional configuration space. The first three joints which have great influence on the manipulator are mainly considered, and the impact of the latter three joints on obstacle avoidance also taken into account, thus reducing the information lost in the process of dimension reduction. The visualization of configuration space for crawler chassis and obstacles has certain guiding significance for the later planning algorithm analysis. The topological properties of closed polygonal obstacles in configuration space are analyzed. The projection of closed polygonal obstacles in configuration space can be simplified into upper and lower parts. The upper and lower parts will be partially connected. The middle part is what the cavity is connected with the outside non-collision configuration space, with only two openings. If the end position of the manipulator is located in the cavity (the manipulator extends into the closed polygon), it can only pass through the upper and lower openings. According to this property, the bidirectional fast extended random tree algorithm (RRT-connect) is improved, and an RRT-connect algorithm with informed guidance point (IGPRRT-connect) based on prior knowledge guidance points in configuration space is proposed. The algorithm searches for narrow-channel guidance points in configuration space according to the topological properties of closed polygons and applies the bridge test algorithm to find the correct narrow-channel. Planning from the guiding point to the starting point and the end point respectively greatly speeds up the planning speed of the RRT-connect algorithm in a closed polygon environment. Taking the square box obstacle as an example, the simulation results show that RRT is faster than IGPRRT-connect when the side length is larger than 40 cm, while less than 40 cm, the IGPRRT-connect has advantages as follows: it takes 1.7 s and 1.2 s for RRT-connect and IGPRRT-connect algorithm respectively to motion planning for the obstacle with side length of 35 cm; that of 3.1 s and 1.6 s respectively for side length of 30 cm; when the edge length is reduced to 25 cm, the planning time for RRT-connect algorithm is as high as 18.1 s, while that of IGPRRT-connect is only 2.6 s, which decreases by 86%. At the same time, simulation experiments are carried out under different shapes of obstacles. The results show that IGPRRT-connect algorithm often takes less time than RRT-connect in closed polygon environment. Because IGPRRT-connect algorithm spends a lot of time in searching for non-existent boot configurations in unclosed polygon environment, RRT-connect algorithm performs better than IGPRRT-connect algorithm in unclosed polygon environment. In order to solve the problem of IGPRRT-connect algorithm in unclosed polygon environment, parallel programming for RRT-connect and IGPRRT-connect is carries out in this paper, and two threads are created: one thread runs RRT-connect algorithm and the other thread runs IGPRRT-connect algorithm. When one thread completes the planning, both threads stop completely and output the planned path, thus avoiding the artificial choice of which algorithm to use, which improves the intelligence of the harvesting robot. Parallel programming is beneficial to simplify the program solution: it is not needed to write the algorithm to judge whether RRT-connect or IGPRRT-connect should be used in the current environment. Simulation results show that the parallel algorithm performs well in various environments. Finally, an indoor obstacle avoidance experiment is carried out using parallel algorithm on the prototype of Citrus harvesting robot. In the experiment, the average planning time in the closed polygon obstacle environment is 1.431 s, the successful rate of obstacle avoidance is 88%, while that in the unclosed polygon obstacle environment are 1.064 s and 94%. The experimental results show that the IGPRRT-connect algorithm proposed in this paper has a good obstacle avoidance effect on both closed and unclosed obstacles, which is of great significance to the research of Citrus harvesting robot.

robots; motion planning; obstacle avoidance; configuration space; guidance point; RRT-connect; citrus harvesting

2018-11-14

2019-04-11

重慶市重點產(chǎn)業(yè)共性關(guān)鍵技術(shù)創(chuàng)新專項（cstc2015zdcyztzx70003）；重慶市基礎(chǔ)科學(xué)與前沿技術(shù)研究一般項目（cstc2016jcyjA0444）資助

馬冀桐，主要從事智能農(nóng)業(yè)機(jī)械研究。Email：mjt0419@163.com

王 毅，博士生，副教授，主要從事智能農(nóng)業(yè)機(jī)械研究。 Email：wangyi_cqut@163.com

10.11975/j.issn.1002-6819.2019.08.012

S24

A

1002-6819(2019)-08-0100-09

馬冀桐，王 毅，何 宇，王 愷，張藝譚. 基于構(gòu)型空間先驗知識引導(dǎo)點的柑橘采摘機(jī)械臂運動規(guī)劃[J]. 農(nóng)業(yè)工程學(xué)報，2019，35(8)：100－108. doi：10.11975/j.issn.1002-6819.2019.08.012 http://www.tcsae.org

Ma Jitong, Wang Yi, He Yu, Wang Kai, Zhang Yitan.Motion planning of citrus harvesting manipulator based on informed guidance point of configuration space[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(8): 100－108. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.08.012 http://www.tcsae.org