基于改進(jìn)RRT的采摘機(jī)械臂避障運動規(guī)劃研究

陶 亮，陳成鵬，潘偉豪，湯先美，辜麗川，焦 俊

（1.安徽農(nóng)業(yè)大學(xué)信息與計算機(jī)學(xué)院，合肥 230036；2.安徽中煙工業(yè)有限公司蚌埠卷煙廠，安徽蚌埠 233010）

機(jī)械臂路徑規(guī)劃是機(jī)器人技術(shù)的一個重要研究領(lǐng)域，主要為機(jī)械臂提供一條安全、有效、精確的路徑，使機(jī)械臂能夠完成預(yù)定的任務(wù)。在科學(xué)技術(shù)發(fā)展和國家政策的支持下，中國對果實采摘機(jī)器人研究也取得重大進(jìn)展，其研發(fā)部門主要是一些科研院所和高校。[1-4]為了使機(jī)械臂能夠完成這些任務(wù)，需要對機(jī)械臂的路徑進(jìn)行規(guī)劃。由于機(jī)械臂具有多自由度和高度的路徑靈活性，所以它的路徑規(guī)劃問題具有很高的復(fù)雜度，需要一些高效算法來解決。適合機(jī)械臂路徑規(guī)劃的算法有很多，如遺傳算法[5]、粒子群算法[6]、A*算法[7]、人工勢場法[8]、蟻群算法[9]、RRT 算法等。由于RRT 算法有很好的魯棒性和適應(yīng)性，故搜索效率高，不需要考慮算法運行的場景問題，即便有錯綜復(fù)雜的狀態(tài)下，它都可以處理路徑規(guī)劃方面的問題，這使得該算法近年來在路徑規(guī)劃領(lǐng)域有很好的應(yīng)用前途和價值。因此，選擇RRT算法進(jìn)行采摘機(jī)械臂路徑規(guī)劃實驗研究具有一定的可行性。

路徑規(guī)劃時，機(jī)械臂必須避開障礙物，同時保證路徑的精度和平穩(wěn)性。在機(jī)械臂路徑規(guī)劃方面，相關(guān)論文在國內(nèi)外研究中有很多：文獻(xiàn)[10-13]對六自由度機(jī)械臂的構(gòu)建，進(jìn)行了路徑學(xué)和工作空間研究分析，不足之處在于沒有對機(jī)械臂路徑規(guī)劃仿真和理論分析；文獻(xiàn)[14-16]提出了一種改進(jìn)RRT*算法，RRT*算法，比傳統(tǒng)RRT 算法在節(jié)點上增加了優(yōu)化剪枝方法，而RRT*算法在隨機(jī)樹搜索過程中具有盲目性，無法找到最短搜索路徑；文獻(xiàn)[17-19]提出了一種RRT-Connect 算法，該算法從起點和終點位置進(jìn)行檢索并擴(kuò)展搜索樹，隨著一次次的檢索過程中，這2 棵搜索樹可能會發(fā)生交匯，這樣的話會結(jié)束檢索并產(chǎn)生無碰撞軌跡。該算法缺點是隨機(jī)樹在搜索過程中產(chǎn)生擴(kuò)展節(jié)點太多，影響算法的效率。

本文研究基于改進(jìn)RRT 的采摘機(jī)械臂避障運動規(guī)劃。首先，對其進(jìn)行機(jī)械臂運動學(xué)分析與碰撞檢測；然后，引入APF 算法和搜索樹優(yōu)化剪枝策略改進(jìn)RRT算法模型，并進(jìn)行實驗和比較分析；最后，采用改進(jìn)的RRT進(jìn)行機(jī)械臂避障仿真實驗。實驗證明，改進(jìn)RRT算法在農(nóng)業(yè)采摘機(jī)械臂避障路徑規(guī)劃中尋優(yōu)路線是有效的。

1 機(jī)械臂運動學(xué)分析與碰撞檢測

1.1 機(jī)械臂運動學(xué)分析

本文以Aubo-i5 機(jī)械臂(見圖1)為例，進(jìn)行機(jī)械臂運動學(xué)分析，其連桿坐標(biāo)系及相對應(yīng)的D-H 參數(shù)表，如圖2、表1所示。求出坐標(biāo)系{i}相對于坐標(biāo)系{i-1}的變換，相鄰連桿之間變換的的位姿，進(jìn)而求出相鄰連桿坐標(biāo)系的變換矩陣。

圖1 Aubo-i5機(jī)械臂

圖2 機(jī)械臂連桿坐標(biāo)系

表1 D-H參數(shù)表

在表1 的連桿參數(shù)中，an表示連桿長度，dn表示關(guān)節(jié)偏置距離，αn表示連桿扭腳，關(guān)節(jié)轉(zhuǎn)角θn為未知參數(shù)。

根據(jù)式（1）和表1，得到連桿坐標(biāo)變換矩陣，通過計算機(jī)械臂連桿坐標(biāo)變換矩陣的乘積，進(jìn)而知道機(jī)械臂末端坐標(biāo)系相對于基座坐標(biāo)系的齊次變換矩陣，即為機(jī)械臂的總變換矩陣：

在式（2）中，機(jī)械臂的位資由m、n、k三個列向量表示，它的三維坐標(biāo)由p所在的列向量表示。

1.2 碰撞檢測

碰撞檢測是路徑規(guī)劃中的一個不可缺少的步驟，通過檢測機(jī)械臂和工作空間中障礙物的位置，計算出它們之間的距離大小?？紤]到機(jī)械臂在路徑規(guī)劃中多發(fā)生在剛體間，進(jìn)行簡化處理，對于機(jī)械臂關(guān)節(jié)、連桿和障礙物間的采用包絡(luò)法處理，因此能夠?qū)⑴鲎矙z測問題變換為球體和球體、柱體和球體的關(guān)系，由此計算量大幅減少，算法效率得到提升。

1.2.1 球體之間的包絡(luò)法

簡化的機(jī)械臂關(guān)節(jié)與球狀障礙物之間的位置關(guān)系如圖3所示。

圖3 簡化的關(guān)節(jié)與球狀障礙物

在三維空間中O1=(x1,y1,z1)，O2=(x2,y2,z2)。O1與O2的為

在圖3、（3）中，O1與O2分別表示機(jī)械臂關(guān)節(jié)與球狀障礙物的球心，r1與r2分別表示機(jī)械臂關(guān)節(jié)與球狀障礙物的半徑，|O1O2|表示機(jī)械臂關(guān)節(jié)與球狀障礙物的球心間的距離。

1）若|O1O2|≥r1+r2，則機(jī)械臂關(guān)節(jié)與球狀障礙物沒有發(fā)生碰撞；

2）若|O1O2|

1.2.2 柱體與球體之間的包絡(luò)法

簡化的機(jī)械臂連桿與球狀障礙物之間的位置關(guān)系如圖4 所示，這里為了方便計算，將機(jī)械臂連桿與球狀障礙物的碰撞檢測轉(zhuǎn)換成線段與球狀障礙物的位置關(guān)系。

圖4 簡化的連桿與球狀障礙物

設(shè)點O2到直線PQ存在最短距離dmin，連桿與球狀障礙物的位置關(guān)系有以下幾種情況：

1）若dmin≥r1+r2，則機(jī)械臂連桿與球狀障礙物沒有發(fā)生碰撞；

2）若dmin

①若|O2P|

圖5 線段與球狀障礙物的位置關(guān)系

②若|O2P|>r1+r2∧|O2Q|>r1+r2，則有以下2種情況存在：

(i)當(dāng)點P、Q在點O2的兩側(cè)，機(jī)械臂連桿與球狀障礙物發(fā)生碰撞，如圖5（b）;

(ii)當(dāng)點P、Q在點O2的同一側(cè)，機(jī)械臂連桿與球狀障礙物沒有發(fā)生碰撞，如圖5（c）。

2 改進(jìn)RRT算法

由于傳統(tǒng)RRT 算法采用了隨機(jī)擴(kuò)展方式，搜索空間比較大，隨機(jī)擴(kuò)展的節(jié)點數(shù)量較多，導(dǎo)致算法的時間和空間復(fù)雜度較高等問題。針對上述存在的不足，本文對傳統(tǒng)RRT算法進(jìn)行改進(jìn)。將改進(jìn)RRT算法的整體思路是在原有RRT 算法的基礎(chǔ)上，通過引入新的算法策略、優(yōu)化其節(jié)點擴(kuò)展等方式，進(jìn)一步提高其路徑規(guī)劃效率和質(zhì)量。具體思路如下：

（1）引入新的機(jī)制：引入人工勢場法，以減少整個搜索空間的大小，降低算法的時間復(fù)雜度和空間復(fù)雜度。

（2）優(yōu)化節(jié)點擴(kuò)展策略：對節(jié)點的擴(kuò)展策略進(jìn)行優(yōu)化，引入廣度優(yōu)先搜索算法進(jìn)行搜索樹優(yōu)化剪枝路徑，再進(jìn)行路徑規(guī)劃等，從而避免節(jié)點擴(kuò)展過程中的冗余計算，提高算法的運行速度。

（3）改進(jìn)算法流程：在原有RRT 算法的基礎(chǔ)上，可以對其算法流程進(jìn)行改進(jìn)，將RRT 算法中的部分操作替換成其改進(jìn)版，從而提高路徑規(guī)劃效率和質(zhì)量。

2.1 引入APF算法

人工勢場法（又稱APF算法）在RRT算法中添加APF算法機(jī)制，APF算法的工作原理是在機(jī)械臂路徑規(guī)劃避障過程中會設(shè)置倆個勢場，分別為引力場和斥力場。如果將APF算法添加到RRT算法中，引力場會將搜索樹的節(jié)點逐步向目標(biāo)節(jié)點擴(kuò)展，減少算法迭代的時間；斥力場會將搜索樹的節(jié)點在擴(kuò)展搜索過程中盡早避開障礙物。這樣的話，在引力場和斥力場存在的復(fù)合作用下，將使搜索樹節(jié)點在沒有發(fā)生碰撞的情況下更好的向目標(biāo)節(jié)點擴(kuò)展。搜索樹在RRT算法中擴(kuò)展過程的節(jié)點在人工勢場下的受力情況，如圖6所示。

圖6 搜索樹節(jié)點的受力情況

搜索樹節(jié)點在引力場下所受到的引力為Fg，此時引力勢函數(shù)可表示為：

在公式4中，kg表示引力常系數(shù)，dg表示目標(biāo)點Xgoal距離搜索樹節(jié)點Xnew的大小，dg′表示搜索樹節(jié)點Xnew到目標(biāo)點Xgoal方向上的方向?qū)?shù)。

搜索樹節(jié)點在斥力場下所受到的斥力為Fr，此時斥力勢函數(shù)可表示為：

在公式5中，kr表示斥力常系數(shù)，do表示障礙物距離搜索樹節(jié)點Xnew的大小，p0表示斥力場的作用范圍半徑大小，p′0表示搜索樹節(jié)點Xnew到障礙物方向上的方向?qū)?shù)。

故搜索樹節(jié)點在引力場和斥力場下所受到的合力為Fs

引入APF 算法機(jī)制后，搜索樹節(jié)點擴(kuò)展過程中明顯趨向于目標(biāo)節(jié)點速度加快，但在路徑規(guī)劃過程中仍然存在較多搜索節(jié)點，這時候需要進(jìn)行剪枝處理，接下來繼續(xù)進(jìn)行優(yōu)化。

2.2 搜索樹優(yōu)化剪枝策略

傳統(tǒng)的RRT 算法在搜索樹節(jié)點擴(kuò)展過程中，存在搜索節(jié)點無限擴(kuò)展的劣勢，這樣會大量占用計算機(jī)內(nèi)存，影響算法效率。下面將采用搜索樹優(yōu)化剪枝策略減少節(jié)點數(shù)量，減少RRT 算法迭代次數(shù)，進(jìn)而提高算法性能。這里引入廣度優(yōu)先搜索算法進(jìn)行搜索樹優(yōu)化剪枝路徑，如圖7 所示。其中紅色節(jié)點Xstart表示起點，黃色節(jié)點Xgoal表示目標(biāo)點，藍(lán)色節(jié)點X2、X6、X8、表示冗余的點，黑色矩形、圓表示障礙物。

圖7 搜索樹節(jié)點優(yōu)化剪枝方法

具體搜索樹優(yōu)化剪枝策略步驟如下：（1）將搜索樹節(jié)點在擴(kuò)展過程中形成的所有節(jié)點進(jìn)行相互連接，若存在與障礙物沖突的搜索路徑，進(jìn)行刪除操作，并且進(jìn)行計算，將搜索樹節(jié)點之間的搜索路徑設(shè)置為帶權(quán)路徑長度，并統(tǒng)計記錄最小的權(quán)重搜索節(jié)點；（2）及時迭代計算搜索樹起始節(jié)點Xstart的最小的權(quán)重和最小權(quán)重的搜索樹節(jié)點Xi，并繼續(xù)進(jìn)行搜索樹循環(huán)擴(kuò)展，直到擴(kuò)展到目標(biāo)點Xgoal變成最小的權(quán)重點；（3）根據(jù)圖7 的優(yōu)化剪枝策略方法，可以得到含有更少節(jié)點的搜索樹路徑，進(jìn)而提高機(jī)械臂路徑規(guī)劃的效率。

2.3 改進(jìn)RRT算法流程

與傳統(tǒng)RRT算法相比，改進(jìn)后的RRT算法的流程圖，如圖8所示。

圖8 改進(jìn)RRT算法的流程圖

3 仿真實驗

3.1 二維環(huán)境下仿真實驗

為了驗證本文所提出的改進(jìn)算法在二維環(huán)境下的可行性，本次實驗設(shè)計了三種不同復(fù)雜度的地圖環(huán)境，黑色部分表示靜態(tài)障礙物，而空白區(qū)域表示可進(jìn)入?yún)^(qū)域。設(shè)置地圖大小為500x500，起始點位置坐標(biāo)為[10，10]，目標(biāo)點位置坐標(biāo)為[490，490]，固定步長為10，閥值為10，分別進(jìn)行300 次實驗，并使用MATLAB仿真實驗對不同算法進(jìn)行了測試。

3.1.1 場景1簡單環(huán)境

二維環(huán)境簡單地圖下，進(jìn)行四種算法路徑規(guī)劃對比實驗。其中，簡單環(huán)境下4 種算法路徑規(guī)劃軌跡、實驗數(shù)據(jù)結(jié)果分別如圖9、表2所示。

圖9 簡單環(huán)境下四種算法的路徑規(guī)劃

表2 簡單環(huán)境下四種算法的路徑規(guī)劃實驗數(shù)據(jù)對比

由圖9 可以看出，RRT 算法的的搜索樹節(jié)點較多且路徑較長，RRT*算法雖然減少了搜索書的節(jié)點，但是搜索路徑明顯更長，RRT-Connect算法使用2棵樹同時進(jìn)行搜索，雖然搜索時間有所提升，但是搜索樹的節(jié)點數(shù)太多，搜索過程更為復(fù)雜，程序運行是需要占用更多的內(nèi)存。結(jié)合圖9和表2中的實驗數(shù)據(jù)結(jié)果對比，可知本文算法在相同迭代次數(shù)環(huán)境下，其搜索路經(jīng)長度更短，搜索樹節(jié)點更少，收斂速度更快且搜索成功率達(dá)到了98%。

3.1.2 場景2復(fù)雜環(huán)境

跟場景1 相比，場景2的地圖環(huán)境更為復(fù)雜，設(shè)置更多的障礙物是為了檢驗不同算法的對復(fù)雜環(huán)境的自適應(yīng)性，更能突出改進(jìn)算法的優(yōu)勢。復(fù)雜環(huán)境下4種算法路徑規(guī)劃軌跡、實驗數(shù)據(jù)結(jié)果分別如圖10、表3所示。

圖10 復(fù)雜環(huán)境下四種算法的路徑規(guī)劃

表3 復(fù)雜環(huán)境下四種算法的路徑規(guī)劃實驗數(shù)據(jù)對比

由圖10 可以看出，相比于其他3 種基本算法，在復(fù)雜環(huán)境中本文算法能夠進(jìn)行更好的避障和適應(yīng)性。從表3 中可以得出，本文算法在平均路經(jīng)長度和平均運行時間方面，能夠在更短的時間內(nèi)獲得低成本的路徑長度。

3.1.3 場景3狹窄通道環(huán)境

跟場景1、場景2相比，在場景3的環(huán)境中有相同的起點和終點中間設(shè)置了三條狹窄通道，該設(shè)置的初衷是為了檢測不同RRT算法在狹窄通道中的路勁搜索規(guī)劃準(zhǔn)確性。圖11 和表4 展示了在狹窄通道環(huán)境下四種算法的路徑規(guī)劃和實驗分析。

圖11 狹窄通道環(huán)境下四種算法的路徑規(guī)劃

表4 狹窄通道環(huán)境下四種算法的路徑規(guī)劃實驗數(shù)據(jù)對比

由圖11 和表4 中可以看出，在狹窄通道環(huán)境中，RRT 算法和RRT*算法的檢索路徑較長，但RRT*算法由于優(yōu)化剪枝的過程，故搜索樹節(jié)點相對較少；RRT-Connect 算法與本文算法的檢索路徑短，檢索時間少，但本文算法在搜索過程中的搜索樹節(jié)點更少，可以得到較短的搜索路徑，很好的體現(xiàn)出窄通道環(huán)境下剪枝策略的搜索優(yōu)勢。總的來看，在狹窄通道中，本文算法搜索路徑、運行時間更短，成功率更高。

為了驗證本研究所提出的改進(jìn)算法的性能，在二維環(huán)境下構(gòu)建了三種復(fù)雜度不同的地圖，并進(jìn)行了路徑規(guī)劃仿真實驗。算法的收斂速度是衡量其效率的重要指標(biāo)，通過對300 次實驗數(shù)據(jù)的記錄和統(tǒng)計分析，得到了四種算法的平均運行時間隨著實驗次數(shù)的變化曲線，該曲線的結(jié)果圖如圖12所示。實驗證明本文算法的的收斂速度較快，效率高。

圖12 四種算法平均運行時間對比

3.2 三維環(huán)境下機(jī)械臂避障抓取仿真實驗

為了進(jìn)一步驗證本文提出的改進(jìn)RRT 算法在復(fù)雜障礙環(huán)境下的適用性，設(shè)計了障礙物環(huán)境，在MATLAB 2021a中建立Aubo-i5機(jī)械臂模型，并將本文改進(jìn)的RRT算法應(yīng)用到采摘機(jī)械臂的避障路徑規(guī)劃中進(jìn)行了驗證。

如圖13所示，其中綠色長方體表示障礙物，空白區(qū)域則代表著采摘機(jī)械臂可進(jìn)入的區(qū)域。在障礙物環(huán)境下，進(jìn)行了采摘機(jī)械臂的路徑規(guī)劃實驗，機(jī)械臂從起始狀態(tài)到達(dá)終點狀態(tài)時并未發(fā)生碰撞，且路徑是平滑的，說明本文算法在機(jī)械臂避障路徑規(guī)劃是可行的。

圖13 障礙物環(huán)境下采摘機(jī)械臂避障路徑規(guī)劃

4 結(jié)論

本文針對傳統(tǒng)RRT 算法在農(nóng)業(yè)采摘機(jī)械臂避障路徑規(guī)劃過程中，存在的的搜索方向具有盲目性、搜索節(jié)點較多等問題，采用一種改進(jìn)的RRT 算法設(shè)計進(jìn)行解決。先對機(jī)械臂進(jìn)行運動學(xué)分析和建模，然后引入APF 算法機(jī)制和廣度優(yōu)先搜索算法優(yōu)化策略改進(jìn)RRT算法，進(jìn)行二維、三維環(huán)境下仿真避障實驗，最后實驗結(jié)果表明改進(jìn)后的算法相對于傳統(tǒng)算法收斂速度加快，盲目性、搜索節(jié)點較多問題也得到解決。對RRT算法的研究具有一定的參考價值，為進(jìn)一步研究果實采摘機(jī)器人提供了實驗依據(jù)。