基于三維重建的柑橘采摘機(jī)械臂避障研究與實(shí)驗(yàn)

馬冀桐, 許洪斌,2, 王毅,2*, 劉艷平, 熊龍燁, 王卓, 何宇

(1.重慶理工大學(xué)機(jī)械工程學(xué)院, 重慶 400054; 2.重慶大學(xué)機(jī)械工程學(xué)院, 重慶 400044)

在果蔬生產(chǎn)作業(yè)中,收獲采摘約占整個(gè)作業(yè)量的40%[1]，柑橘等季節(jié)性水果的收獲作業(yè)更是需要在短時(shí)間內(nèi)找到大量勞動(dòng)力，人力成本巨大[2]。為了能夠使柑橘收獲過程自動(dòng)化，已經(jīng)開發(fā)了一些柑橘收獲機(jī)械，如旋轉(zhuǎn)拍打桿或枝干搖動(dòng)設(shè)備。但由于果實(shí)受到震動(dòng)沖擊，這些方法只適合收獲用于二次加工的柑橘(如制作果汁)，并不適合無損柑橘的收獲[2]。隨著計(jì)算機(jī)、機(jī)器人和人工智能等技術(shù)的快速發(fā)展，采摘機(jī)器人技術(shù)已從前瞻性研究轉(zhuǎn)為應(yīng)用需求，通過采摘機(jī)器人收獲柑橘可大幅減少人力成本。但當(dāng)前針對采摘機(jī)器人的研究還主要集中在溫室條件下[3]，針對室外非結(jié)構(gòu)環(huán)境下柑橘采摘的研究和應(yīng)用較少。在室外，采摘機(jī)器人要想順利采下柑橘果實(shí)，在機(jī)械臂的運(yùn)動(dòng)過程中要涉及避開樹枝等障礙問題，涉及機(jī)器人感知障礙和避開障礙物。

在機(jī)器人感知障礙方面，目前主要采用機(jī)器視覺及深度學(xué)習(xí)的方法來識別障礙。如蔡健榮等[4]通過圖像分割、形態(tài)學(xué)處理、區(qū)域細(xì)線化等步驟提取和處理枝干的骨架信息。Amatya等[5]通過貝葉斯分類器對枝干、櫻桃果實(shí)、葉子和背景進(jìn)行識別、分類。Majeed等[6]使用Kinect V2傳感器和基于深度學(xué)習(xí)卷積神經(jīng)網(wǎng)絡(luò)(SegNet)的語義分割方法對枝干進(jìn)行提取。Jing等[7]利用深度特征和區(qū)域卷積神經(jīng)網(wǎng)絡(luò)(R-CNN)對蘋果樹進(jìn)行枝干的識別。

在機(jī)器人的避障算法方面，姚立健等[8]對SCORBOT-ER4u型茄子采摘機(jī)械臂進(jìn)行了避障運(yùn)動(dòng)規(guī)劃，算法將障礙物映射到收獲機(jī)械臂構(gòu)形空間中，在構(gòu)形空間中利用A*算法搜索避障路徑，但該方法通過路徑規(guī)劃模型進(jìn)行簡化將空間障礙物投影到二維平面中，無法充分發(fā)揮高自由度采摘機(jī)械臂在避障方面的優(yōu)勢。Nguyen等[2]對蘋果采摘環(huán)境下多種算法進(jìn)行驗(yàn)證，發(fā)現(xiàn)雙向連接快速擴(kuò)展隨機(jī)樹算法(rapidlly exploring random tree-connect, RRT-connect)效果較好。陽涵疆等[9]提出了一種基于關(guān)節(jié)構(gòu)形空間的混聯(lián)采摘機(jī)械臂避障路徑規(guī)劃方法。

從以上分析可以看出，雖然障礙物的識別和避障的算法都取得了較為豐富的研究成果，但兩者的結(jié)合，即識別之后機(jī)器人感知、感知效果和實(shí)際的障礙有無差距、避障算法驅(qū)動(dòng)機(jī)械臂避開現(xiàn)實(shí)障礙的效果等研究尚存在不足。因此，本研究搭建了基于機(jī)器人操作系統(tǒng)(robot operating system，ROS)的柑橘采摘機(jī)器人實(shí)驗(yàn)平臺，使用深度學(xué)習(xí)Mask R-CNN152模型對枝干進(jìn)行識別與定位，基于視覺識別結(jié)果在ROS中對障礙物進(jìn)行重建，從而使機(jī)器人能夠正確感知障礙物的存在，應(yīng)用RRT-connect算法操縱機(jī)械臂進(jìn)行了避障仿真，并使用課題組自行研制的柑橘采摘機(jī)器人在實(shí)驗(yàn)室環(huán)境下對本文提出的避障方法與流程進(jìn)行了驗(yàn)證。

1 原理與方法

1.1 枝干識別

本文采用卷積神經(jīng)網(wǎng)絡(luò)Mask R-CNN[10]進(jìn)行枝干識別，該網(wǎng)絡(luò)采用ResNet和特征金字塔網(wǎng)絡(luò)(FPN)來融合提取樹干的多層特征，該特征經(jīng)過全卷積層輸出到分類器，實(shí)現(xiàn)樹干分類和位置邊框粗提取，對位置邊框使用Mask分支進(jìn)行像素分割，實(shí)現(xiàn)樹干區(qū)域的精細(xì)化提取。Mask R-CNN是在Faster RCNN基礎(chǔ)上添加一個(gè)掩碼mask分支實(shí)現(xiàn)分割任務(wù)，定義mask分支輸出感興趣區(qū)域(region of interest,ROI)的維度為Km2( K為對應(yīng)類別個(gè)數(shù)，m為分辨尺度)。定位和分類任務(wù)仍采用Faster RCNN原本的網(wǎng)絡(luò)結(jié)構(gòu)，采用ResNet和特征金字塔網(wǎng)絡(luò)(FPN)來融合提取多層特征。針對精確定位任務(wù)，采用ROIAlign替代Faster RCNN中的RoiPooling，即引入雙線性差值替代原來的最大值池化，優(yōu)化ROI輸入和FCN輸出特征之間的像素對應(yīng)關(guān)系，極大提高像素級的分割精度，其具有分割準(zhǔn)確率高的優(yōu)點(diǎn)。

首先制作訓(xùn)練集：對枝干使用四邊形框型進(jìn)行標(biāo)記，一段枝干劃分為若干小段。然后以ResNet152作為Mask R-CNN的主干網(wǎng)絡(luò)在Ubuntu16.04系統(tǒng)中配置的TensorFlow平臺下使用NVIDIA公司生產(chǎn)的GTX1080Ti GPU進(jìn)行加速訓(xùn)練和測試。應(yīng)用訓(xùn)練得到的模型對枝干進(jìn)行識別。每段識別的結(jié)果以最小外接矩形框出,作為定位框。

1.2 基于視覺識別結(jié)果的障礙物重建

物體的三維空間坐標(biāo)與圖像坐標(biāo)存在線性關(guān)系，世界坐標(biāo)系、相機(jī)坐標(biāo)系以及像素坐標(biāo)系之間的關(guān)系如圖1所示，圖中P點(diǎn)表示三維空間中某點(diǎn)，p點(diǎn)表示P在像素坐標(biāo)系中的投影點(diǎn)。

圖1 相機(jī)坐標(biāo)系Fig.1 Camera coordinate system

像素坐標(biāo)下p(u,v)對應(yīng)相機(jī)坐標(biāo)系下的空間三維坐標(biāo)P(Xc,Yc,Zc),其中Zc由深度鏡頭獲取。Xc、Yc的表達(dá)式[11]如下。

(1)

式中，fx、fy為攝像機(jī)在x、y方向的焦距，分別為1 176.220 5和1 175.132 2。

利用訓(xùn)練得到的Mask R-CNN152識別模型在對彩色圖中的枝干進(jìn)行識別時(shí)會得到一系列分割掩碼(mask)以及對應(yīng)掩碼的最小外接矩形定位框(box)，本文通過尋找彩色圖中矩形框兩個(gè)短邊中點(diǎn)所對應(yīng)的三維空間坐標(biāo)來對其進(jìn)行定位，圖2所示為最小外接矩定位框。

圖2 最小外接矩Fig.2 Minimum external moment

該定位框有四個(gè)角點(diǎn)：c1、c2、c3、c4，這些點(diǎn)的二維像素坐標(biāo)(x,y)已知，假設(shè)c1c2、c3c4為該矩形的兩個(gè)短邊，則兩個(gè)中點(diǎn)的像素坐標(biāo)(xm1,ym1)、(xm2,ym2)分別如式(2)和(3)所示。

(2)

(3)

然后通過式(1)得到對應(yīng)的三維空間坐標(biāo)(Xm1,Ym1,Zm1)、(Xm2,Ym2,Zm2)，該函數(shù)可以將彩色圖與深度圖對應(yīng)，由像素坐標(biāo)得到像素點(diǎn)的三維坐標(biāo)。

將c1、c2的深度值近似的等于m1的深度值Zm1，c3、c4的深度值近似等于Zm2，通過轉(zhuǎn)換關(guān)系式(1)得到四個(gè)角點(diǎn)的三維空間坐標(biāo)。該段枝干的直徑和半徑分別為式(4)和(5)。

d=||c1c2||=||c3c4||

(4)

(5)

由于m1、m2的深度值為枝干表明的深度，并非枝干圓柱體中心的深度，需要對深度值進(jìn)行一個(gè)值為r的偏移修正，使得m1、m2兩點(diǎn)能夠正確反映枝干的位置。

(6)

得到關(guān)鍵點(diǎn)坐標(biāo)m1(Xm1、Ym1,Zm1)、m2(Xm2,Ym2,Zm2)以及半徑r后，再將m1、m2點(diǎn)坐標(biāo)轉(zhuǎn)換到世界坐標(biāo)系，對障礙物進(jìn)行重建。

1.3 PPT-connect算法

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

V1←{qinit};E1←φ;T1←(V1,E1);V2←{qgoal};E2←φ;T2←(V2,E2);

Whilei

qrand←RandomSampling()；qnearest←Nearestnode(T1,qrand)

qnew←AddNewNode(qrand,qnearest,δ)

if NodeCollsionCheck(qnew) then

if LineCollsionCheck(qnew,qnearest) then

V1←V1∪{qnew}；E1←E1∪{(qnew,qnearest)}；

else break

return (V1,E1)

Swap(T1,T2)

(1)RandomSampling():該函數(shù)每次在C-space隨機(jī)產(chǎn)生一個(gè)構(gòu)型采樣點(diǎn)qrand，這些構(gòu)型為均勻分布。

(2)Distance(q1,q2)：該函數(shù)返回在兩個(gè)構(gòu)型之間路徑的代價(jià)，代價(jià)一般以歐幾里得距離度量。

(3)Nearestnode(T,qrand):該函數(shù)返回T的節(jié)點(diǎn)集V中與隨機(jī)采樣點(diǎn)qrand最近的頂點(diǎn)qnearest。采用Distance(q1,q2)度量頂點(diǎn)之間的遠(yuǎn)近。

(4)AddNewNode(qrand,qnearest,δ)：該函數(shù)以qnearest為起點(diǎn)朝著qrand方向前進(jìn)一個(gè)步長δ得到qnew節(jié)點(diǎn)。

(5)NodeCollsionCheck(q1):該函數(shù)檢查某個(gè)構(gòu)型q1是否在Cfree中，如果位于Cfree中，則說明該構(gòu)型沒有發(fā)生碰撞，函數(shù)返回TRUE，否則返回FALSE。

(6)LineCollsionCheck(q1,q2):該函數(shù)檢查以q1、q2為端點(diǎn)的路徑是否位于Cfree中，如果位于Cfree中，表明該段路徑?jīng)]有發(fā)生碰撞，函數(shù)返回TRUE，否則返回FALSE。

(7)Swap(T1,T2)：將T1、T2兩個(gè)擴(kuò)展樹進(jìn)行交換。

圖3為二維環(huán)境下RRT-connect算法規(guī)劃出來的避障路徑。

圖3 RRT-connect二維環(huán)境下的規(guī)劃Fig.3 RRT-connect planning in two-dimensional environment

1.4 實(shí)驗(yàn)平臺搭建

1.4.1柑橘采摘機(jī)器人 本文所采用的柑橘采摘機(jī)器人為團(tuán)隊(duì)研發(fā)，主要由機(jī)械臂、移動(dòng)底盤及視覺系統(tǒng)等組成[13]，視覺系統(tǒng)獲取柑橘及障礙物信息，機(jī)械臂經(jīng)過運(yùn)動(dòng)規(guī)劃到達(dá)采摘位置，末端執(zhí)行器進(jìn)行果柄剪切及果實(shí)夾持，然后通過外殼上方的開孔將柑橘放入果籃。移動(dòng)底盤模塊采用山東極創(chuàng)機(jī)器人公司Komodo-2形履帶底盤。機(jī)械臂采用遨博公司生產(chǎn)的aubo_i5六自由度機(jī)械臂。攝像機(jī)采用微軟公司生產(chǎn)的Kinect V2相機(jī)，該相機(jī)具有彩色攝像頭及深度攝像頭，可以對三維坐標(biāo)進(jìn)行測量。由于本文在于研究機(jī)械臂避障方法的可行性，因此對末端執(zhí)行器的影響暫未考慮。

1.4.2仿真與控制平臺搭建 基于ROS開發(fā)了柑橘采摘機(jī)器人仿真與控制平臺，其界面如圖4所示，進(jìn)行柑橘采摘機(jī)器人運(yùn)動(dòng)規(guī)劃仿真與控制方面的研究。

注：移動(dòng)底盤以包圍盒方法進(jìn)行了簡化。Note: Chassis is simplified by the method of enclosing the box.圖4 柑橘采摘機(jī)器人仿真與控制平臺界面Fig.4 Interface of simulation and control platform for citrus picking robot

該平臺集成了碰撞檢測、正逆運(yùn)動(dòng)學(xué)求解、軌跡平滑、軌跡可視化等功能，部分功能的插件選擇及參數(shù)設(shè)置如表1所示。

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

表1 集成的插件及參數(shù)設(shè)置Table 1 Integrated plug-ins and parameter settings

圖5 柑橘采摘機(jī)器人避障運(yùn)動(dòng)規(guī)劃仿真及控制系統(tǒng)Fig.5 Citrus harvesting robot obstacle avoidance motion planning simulation and control system

1.5 柑橘采摘機(jī)器人實(shí)驗(yàn)設(shè)計(jì)

本文在實(shí)驗(yàn)室環(huán)境下使用從果園采集的真實(shí)樹干作為障礙物，實(shí)驗(yàn)設(shè)備如圖6所示?？刂浦鳈C(jī)配置為CPU:intel i7 7800X；GPU:NVIDA1080Ti；內(nèi)存：32G。Kinect V2相機(jī)主要用于枝干障礙物的識別及定位。支架主要用于枝干的固定。

在重慶理工大學(xué)柑橘果園采集10組枝干進(jìn)行實(shí)驗(yàn)，將枝干布置在距離機(jī)器人底座0.5～0.8 m范圍內(nèi)，由支架進(jìn)行固定。

圖6 實(shí)驗(yàn)設(shè)備Fig.6 Experimental equipment

首先將障礙物進(jìn)行識別及場景重建，然后應(yīng)用RRT-connect算法進(jìn)行避障運(yùn)動(dòng)規(guī)劃，因?yàn)樵谧匀粻顟B(tài)下柑橘都是生長在枝干上，通過果柄與枝干連接，對每個(gè)枝干段中點(diǎn)附近的范圍取隨機(jī)點(diǎn)模擬長在枝干上的柑橘位置，如圖7所示，如果柑橘位置與枝干碰撞則重新選取。

圖7 隨機(jī)取點(diǎn)模擬柑橘位置Fig.7 Randomly take a simulated citrus location

分別對障礙物的識別與重建實(shí)驗(yàn)及避障運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)進(jìn)行統(tǒng)計(jì)。

在障礙物識別與重建實(shí)驗(yàn)中將枝干的粗細(xì)按直徑分為五組：0.5～1.5、1.5～2.5、2.5～3.5、3.5～4.5、>4.5 cm。統(tǒng)計(jì)每組的枝干段數(shù)以及識別出的枝干段數(shù)，其中,未識別出的部分按長度10 cm為一段計(jì)算，長度小于3 cm的忽略不計(jì)，統(tǒng)計(jì)識別成功率。

在避障運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)中，因?yàn)榛赗RT-connect的運(yùn)動(dòng)規(guī)劃算法無法判斷某個(gè)運(yùn)動(dòng)規(guī)劃任務(wù)是否有解，若不設(shè)置停止條件，在無解的情況下不停的計(jì)算，一般的做法是設(shè)置時(shí)間上限[16]，若超過時(shí)間上限則認(rèn)為該運(yùn)動(dòng)規(guī)劃任務(wù)無解，停止規(guī)劃，記為失敗，本次實(shí)驗(yàn)設(shè)置時(shí)間上限為5 s。

將規(guī)劃失敗及由于枝干與機(jī)械臂碰撞導(dǎo)致枝干的固定支架傾倒的情況視為失敗，輕微剮蹭在本文中不視為失敗。并將枝干類型劃分為三類進(jìn)行統(tǒng)計(jì)：枝干位于機(jī)械臂與柑橘中央;柑橘位于枝干后方;柑橘位于兩根枝干中間。

2 結(jié)果與分析

2.1 障礙物識別與重建實(shí)驗(yàn)

在室內(nèi)所搭建的環(huán)境下對采集的枝干進(jìn)行識別與重建，障礙物的識別效果和重建效果如圖8所示。

圖8 障礙物識別與重建效果Fig.8 Obstacle identification and reconstruction effect

可以看出，基于Mask R-CNN和Kinect v2的枝干障礙物識別及重建方法能夠有效的將障礙物進(jìn)行重建到規(guī)劃環(huán)境中。但有些較細(xì)的枝干沒有被識別出來，如圖8中黃色圈中的部分所示。通過表2可知，使用本文提出的枝干障礙物識別及重建方法，對于直徑1.5 cm以上的枝干都有較好的識別成功率，同時(shí)能夠在ROS中進(jìn)行三維信息重建，枝干越粗識別成功率越高。

表2 不同直徑枝干的識別成功率Table 2 Recognition success rate of branches of different diameters

2.2 避障運(yùn)動(dòng)規(guī)劃分析

本節(jié)進(jìn)行了375次運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)，圖9所示為部分實(shí)驗(yàn)的仿真。

由表3可知，綜合避障成功率為90.70%。平均規(guī)劃時(shí)間為1.514 s，柑橘采摘機(jī)器人已經(jīng)具有避障運(yùn)動(dòng)到目標(biāo)位置進(jìn)行采摘的能力。

圖9 避障實(shí)驗(yàn)Fig.9 Obstacle avoidance experiment

2.3 避障失敗原因分析

為了對接下來的研究工作提供進(jìn)一步改進(jìn)方向，對失敗原因進(jìn)行了分析、統(tǒng)計(jì)，發(fā)現(xiàn)避障失敗原因主要由以下四個(gè)原因造成：①逆解求解失敗，當(dāng)前柑橘坐標(biāo)及障礙物情況下無法求解出逆解；②規(guī)劃超時(shí)；③由于增量式路徑段碰撞檢測導(dǎo)致的規(guī)劃失?。虎芤?yàn)橐曈X定位誤差及機(jī)械臂運(yùn)動(dòng)誤差，機(jī)械臂實(shí)際運(yùn)動(dòng)過程中對枝干造成碰撞，導(dǎo)致枝干的固定支架傾倒。

對上述造成失敗原因的次數(shù)進(jìn)行統(tǒng)計(jì)(表4)，可以看出，無法求解出逆解為主要失敗原因，占總失敗次數(shù)的57%，其次是因?yàn)槁窂蕉闻鲎矙z測錯(cuò)誤導(dǎo)致的失敗，占31%左右，視覺定位誤差與機(jī)械臂運(yùn)動(dòng)誤差導(dǎo)致的失敗占少部分，為8.57%，說明對于枝干的定位比較準(zhǔn)確，而超出規(guī)劃時(shí)間的情況只出現(xiàn)了一次，說明所提出的規(guī)劃算法在規(guī)劃速度方面可以滿足采摘要求。

表3 柑橘與枝干不同相對位置下的規(guī)劃數(shù)據(jù)Table 3 Planning data for different relative positions of citrus and branches

表4 失敗原因統(tǒng)計(jì)Table 4 Failure reason statistics

對最主要的失敗原因逆解求解份分析，其主要是因?yàn)樵诋?dāng)前障礙物位置下，機(jī)械臂本身不存在到達(dá)該點(diǎn)的構(gòu)型，解決方法可以考慮將移動(dòng)底盤加入到避障運(yùn)動(dòng)規(guī)劃中來，例如在逆解求解失敗的情況下，移動(dòng)底盤進(jìn)行移動(dòng)，無逆解位置可能會變?yōu)橛心娼馕恢茫龠M(jìn)行一次規(guī)劃。

而碰撞檢測失敗原因在于碰撞檢測分辨率Δq的設(shè)置略大，導(dǎo)致部分情況下碰撞檢測跳過了障礙物，如圖10所示?？梢钥紤]設(shè)置更小的碰撞檢測分辨率來解決這個(gè)問題，但同時(shí)會導(dǎo)致規(guī)劃時(shí)間增長。

圖10 Δq過大導(dǎo)致碰撞檢測跳過障礙物Fig.10 Over-large Δq causes collision detection to skip obstacles

3 討論

國內(nèi)外研究多是單獨(dú)對障礙物識別和運(yùn)動(dòng)規(guī)劃進(jìn)行探討，目前很少對障礙物識別后如何對障礙物進(jìn)行重建用于避障運(yùn)動(dòng)規(guī)劃進(jìn)行討論，本文對障礙物的識別重建、避障運(yùn)動(dòng)規(guī)劃進(jìn)行了綜合研究，提出了從障礙物識別到運(yùn)動(dòng)規(guī)劃的整體避障方法。運(yùn)動(dòng)規(guī)劃成為機(jī)器人研究領(lǐng)域的一個(gè)標(biāo)準(zhǔn)問題,始于Lozano-Perez等[17]在1979 年引入構(gòu)型空間這一概念，不同形式的運(yùn)動(dòng)規(guī)劃問題由此轉(zhuǎn)化為在位形空間中尋找點(diǎn)的無碰路徑問題,并可能使用統(tǒng)一的數(shù)學(xué)方法求解。

由于傳統(tǒng)的完備性規(guī)劃方法，例如A*、單元分解法、勢場法等，在高維構(gòu)型空間情況下將面臨維數(shù)災(zāi)難的難題，為解決多自由度機(jī)器人的運(yùn)動(dòng)規(guī)劃問題，Lindstrom等[18]提出概率路標(biāo)法(probabilistic roadmap method, PRM)，LaValle[19]提出快速擴(kuò)展隨機(jī)樹方 法(rapidly-exploring random tree, RRT)。這兩種方法理論上都是概率完備的路徑規(guī)劃方法，即如果存在一條連接初始和目標(biāo)構(gòu)型的連通路徑，只要具備足夠多的采樣構(gòu)型，該算法找到解路徑的概率收斂到1。PRM方法求解運(yùn)動(dòng)規(guī)劃問題分為構(gòu)造和詢問兩個(gè)階段，在構(gòu)造階段通過對構(gòu)型空間的采樣創(chuàng)建路標(biāo)圖，在詢問階段通過圖搜索算法返回初始構(gòu)型到目標(biāo)構(gòu)型的一條無碰撞路徑。RRT 方法不需要任何前處理步驟，而且不用執(zhí)行圖搜索算法，其效率往往比 PRM 更高。 Kuffner 等[12]于2002年提出RRT改進(jìn)版本RRT-connect算法，比RRT效率更高。

本研究基于ROS搭建了柑橘采摘機(jī)器人仿真及控制平臺，使用Mask R-CNN對枝干障礙物進(jìn)行識別，并對識別出來枝干與Kinect深度圖進(jìn)行配準(zhǔn)得到枝干三維信息，在ROS中對障礙物進(jìn)行了重建，應(yīng)用RRT-connect算法進(jìn)行避障運(yùn)動(dòng)規(guī)劃，室內(nèi)實(shí)驗(yàn)的避障成功率達(dá)90%，在實(shí)驗(yàn)室環(huán)境下取得了較好效果。但真實(shí)采摘環(huán)境與實(shí)驗(yàn)室環(huán)境有一定的差距，對柑橘采摘機(jī)器人的研究還需進(jìn)一步深入。