基于構型空間先驗知識引導點的柑橘采摘機械臂運動規劃

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

?

基于構型空間先驗知識引導點的柑橘采摘機械臂運動規劃

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

（1. 重慶理工大學機械工程學院，重慶 400054；2. 重慶大學機械工程學院，重慶 400044）

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

機器人；運動規劃；避障；構型空間；引導點；RRT-connect；柑橘采摘

0 引 言

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

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

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

1 機械臂構型空間降維映射建模

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

1.1 采摘機器人原理

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

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

1.2 構型空間降維映射建模

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

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

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

注：為6個關節角度值。

底座構型空間的映射有助于分析底座外殼的合理性，是否對機械臂運動規劃造成了不良影響，對于改進底座的外形有指導意義。

構型空間中封閉多邊形的拓撲性質是同胚的[23-24]。對封閉多邊形枝干障礙物在構型空間的映射limb進行了建模分析。圖3b、圖3c分別為正方框形障礙物、圓環形障礙物在構型空間映射的不同角度視圖。

通過圖3b、圖3c發現正方框形與圓環形在構型空間內的投影拓撲的性質：封閉多邊形障礙物的構型空間投影可以簡化為上下2部分，上下2部分會有部分聯結，中間部分為空腔與外界free相通，且只有上下兩個開口，若機械臂終點位姿位于空腔內（機械臂伸入封閉多邊形）則只能通過上下開口（1為3.14和0時，實際為同一個構型，在圖中顯示為開口，實為封閉）。

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

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

對構型空間的降維映射建模為離線構建，由于封閉多邊形障礙物的拓撲結構相似性，對構型空間的建模目的在于探尋不同的封閉多邊形障礙物的共同特性，為規劃算法提供改進依據，無需每次規劃都對構型空間進行構建。

2 基于構型空間引導點的RRT-connect算法

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

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

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

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

注：qinit為起始構型，qgoal為目標構型,qmid為引導構型。

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

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

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

具體流程為：

1）獲取起點、終點關節角度值init、goal。

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

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

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

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

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

7）步驟6）中沒有找到窄通道，則認為在當前1下不存在通道，對1進行“震蕩”調整，返回步驟3。“震蕩”調整即以增量的方式左右搖擺：1、1+01、1?01、1+02、1?02、1+03、…，直到找到窄通道或1超出±3.14。

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

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

圖5 IGPRRT-connect流程圖

2.3 2種算法的并行編程

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

圖6 RRT-connect和IGPRRT-connect算法并行規劃框架

3 仿真及試驗驗證

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

3.1 仿真與控制平臺搭建

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

整個系統避障任務工作流程如圖8，逆運動學算法求解任務信息中目標位置對應的機械臂逆解，提供給RRT- connect算法作為構型空間規劃目標，RRT-connect在節點擴展過程中調取碰撞檢測插件進行路徑碰撞檢測。得到無碰撞路徑后，對路徑應用5次樣條曲線進行路徑平滑。然后平滑后的路徑數據通過CAN總線發送給驅動器，由驅動器驅動電機轉動完成機器人的采摘動作。機器人采集的位置、速度等信息實時反饋到規劃場景中，對規劃場景中的機械臂各關節位置進行更新。

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

表1 集成的插件及參數設置

圖8 柑橘采摘機器人避障運動規劃仿真及控制系統

3.2 仿真試驗

3.2.1 試驗設計

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

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

3.2.2 結果對比分析

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

a. 起始構型 a. Initial configurationb. 目標構型 b. Target configurationc. 運動軌跡 c. Trajectoryd. 引導構型 d. Guided configuration

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

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

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

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

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

3.3 室內試驗

3.3.1 試驗設計

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

3.3.2 結果及分析

圖13為采摘機械臂在仿真及實驗室環境下的避障試驗運動過程。試驗統計結果如表2。

圖12 室內試驗環境

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

表2 并行算法室內試驗結果

注：每組共進行50次試驗。

Note: Fifty experiments are conducted in each group.

可以看到并行算法在各種環境下表現穩定，失敗的原因主要為障礙物布置過程中產生的誤差及機械臂運動誤差所導致的碰撞。

室內試驗結果表明：使用本文提出的并行算法，對封閉障礙物和未封閉障礙物的均有較好的避障效果，平均規劃時間為1.263 s，避障成功率為91%。可為柑橘采摘機器人的采摘作業提供新的運動規劃算法選擇，為進一步的研究工作奠定了基礎。

4 結 論

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

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

[1] 王亞珍，汪安明，趙坤，等. 基于薄壁圓環理論的機器人用柔性軸承變形特征快速求解[J]. 農業工程學報，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]. 農業機械學報，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]. 農業機械學報，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] 陽涵疆，李立君，高自成. 基于關節構形空間的混聯采摘機械臂避障路徑規劃[J]. 農業工程學報，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]. 控制與決策，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]. 機器人，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]. 機械工程學報，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]. 農業機械學報，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]. 農業機械學報，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]. 農業機械學報，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]. 農業工程學報，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]. 控制理論與應用，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] 鄒宇星，李立君，高自成. 基于改進PRM的采摘機器人機械臂避障路徑規劃[J]. 傳感器與微系統，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] 李揚，楊長輝，胡友呈，等. 基于凸殼及距離變換的重疊柑橘目標識別與定位方法[J]. 現代制造工程，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] 王毅，許洪斌，張茂，等. 仿蛇嘴咬合式柑橘采摘末端執行器設計與實驗[J]. 農業機械學報，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]. 中國農業科技導報，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]. 機械傳動，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] 張鳳茹. 同胚映射在拓撲空間中的應用[J]. 哈爾濱師范大學自然科學學報，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

重慶市重點產業共性關鍵技術創新專項（cstc2015zdcyztzx70003）；重慶市基礎科學與前沿技術研究一般項目（cstc2016jcyjA0444）資助

馬冀桐，主要從事智能農業機械研究。Email：mjt0419@163.com

王 毅，博士生，副教授，主要從事智能農業機械研究。 Email：wangyi_cqut@163.com

10.11975/j.issn.1002-6819.2019.08.012

S24

A

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

馬冀桐，王 毅，何 宇，王 愷，張藝譚. 基于構型空間先驗知識引導點的柑橘采摘機械臂運動規劃[J]. 農業工程學報，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