改進RRT算法的采摘機械臂路徑規(guī)劃

趙 輝,鄭縉奕,岳有軍,王紅君

(1.天津理工大學 電氣工程與自動化學院, 天津 300384;2.天津市復雜系統(tǒng)控制理論與應用重點實驗室, 天津 300384)

0 引言

機械臂路徑規(guī)劃是果園實現(xiàn)自動化采摘的關鍵技術,也為采摘的效率、品質(zhì)提供了重要保證。由于水果果實具有密集程度不一,生長位置各異,樹枝遮擋等特點,造成機械臂自動化采摘路徑規(guī)劃的難度較大。目前投入使用的農(nóng)業(yè)采摘機械臂大多是由工業(yè)機械臂改制而來,基于果園采摘的實際應用環(huán)境需求,6自由度的機械臂得到了廣泛應用,因為6自由度在保障空間采摘可行性的同時兼顧了靈活性,為了采摘機械臂能夠更好地完成復雜的任務,對于其路徑規(guī)劃的研究有著重要意義[1]。

針對機械臂的運動路徑規(guī)劃問題,國內(nèi)外學者都做出了巨大的貢獻[2-10]。在解決一些二維空間的運動規(guī)劃問題中,常使用A*[2]、D*[3]、遺傳算法[4]等處理,取得了較好的效果,但隨著機械臂自由度的增加和環(huán)境趨于復雜,這些算法變得效率低下甚至無法解決實際問題。為了解決空間路徑規(guī)劃問題,有學者提出了一種基于采樣的快速搜索隨機樹(RRT)算法,但RRT基礎算法有著搜索速度慢,生成的路徑拐點多,占用計算內(nèi)存大等缺點。針對這些問題,張建冬等[5]將障礙物因子、變步長等策略引入了RRT算法,有效提高了機械臂的避障效率;賈浩鐸等[6]把人工勢場法與Informed-RRT*算法相結合,改進后的算法在路徑規(guī)劃速度上有顯著提升,且對于動態(tài)環(huán)境的適應性也增強;張勤等[7]基于柯西采樣函數(shù)提出了一種改進的目標引力雙向RRT*算法,使得機械臂運動路徑更加平滑可靠;張婷婷等[8]把目標導向以及冗余檢測策略與RRT算法相結合,解決了原算法存在的路徑搜索時間長,路徑質(zhì)量低等問題,最終通過仿真實驗證明了算法的有效性;Mohammed等[9]將RRT*隨機樹節(jié)點的生成概率呈正態(tài)分布,避免了過度搜索和陷入局部極值;潘振釗等[10]提出了一種改進的P-RRT*算法,將目標概率導向以及剪枝策略與原算法結合,改進的算法對于路徑的長度和規(guī)劃時間均有明顯的縮短。

本文中針對傳統(tǒng)的RRT算法路徑規(guī)劃速度慢、路徑長度過長、采樣點隨機等問題,融入高斯采樣增加算法搜索的目標導向性;加入A*代價函數(shù),提高搜索速度,減少路徑冗余;最后再采用貪婪算法優(yōu)化路徑,結合Matlab驗證了改進算法的有效性。

1 算法基礎

1.1 機械臂運動學建模

選用如圖1所示的6自由度機械臂進行研究。首先對6自由度機械臂采用D-H參數(shù)法進行運動學建模,該方法首先要在機械臂各連桿的關節(jié)軸處建立一個平面直角坐標系,6自由度機械臂的關節(jié)坐標系如圖2所示。在完成運動學模型的構建并且求解出路徑后,就可以經(jīng)由運動學逆向求解的方法,使機械臂遵循求解路徑進行動作。

圖1 6自由度機械臂關節(jié)示意圖

圖2 關節(jié)坐標系示意圖

建立D-H連桿坐標系使用的4個參變量及各自的含義為:關節(jié)角用θi表示,連桿偏移量用di表示,連桿長度用ai表示,連桿偏轉角用αi表示。由這4個參變量描述的鄰接的兩連桿間的空間狀態(tài)關系如圖3所示,同時機械臂的D-H參數(shù)如表1所示。

表1 D-H參數(shù)

圖3 D-H連桿空間狀態(tài)關系示意圖

在D-H參數(shù)法的基礎上,6自由度機械臂相鄰連桿坐標系之間的齊次變換矩陣為:

(1)

(2)

再按照機械臂的逆運動學求解途徑,可以將機械臂在路徑規(guī)劃中求解出的各點,在平面直角坐標系中的描述方式,轉化為在關節(jié)構型空間中各連桿要求的變換[11]。基于以上理論,在Matlab中使用Robotics toolbox工具箱仿真出文中所選用的6自由度機械臂,如圖4所示。

圖4 Matlab機械臂建模示意圖

1.2 機械臂碰撞檢測

機械臂路徑規(guī)劃過程中最重要的一個步驟是對障礙物進行碰撞檢測,基于文章采用的6自由度機械臂自身的屬性,在搭建的仿真環(huán)境中,果樹的樹葉因為自身的柔軟性,對于采摘工作的擾動可以忽略不記,因此,不考慮機械臂與樹葉的碰撞[12],僅需要對機械臂的連桿與樹枝、果實之間進行碰撞檢測處理。由于實際環(huán)境中的障礙物往往大小不一,為了減少多余的時間損耗和內(nèi)存空間占用,本文中將障礙物抽象簡化為長方體和球體,即采用長方體包絡檢測法[13]以及球體包絡檢測法[14]進行機械臂的碰撞檢測。

首先,將機械臂的各個連桿簡化成為圓柱體[15],針對球體障礙物,只需要判斷機械臂連桿中心點和球心的間距,即可確定碰撞是否產(chǎn)生;針對長方體包絡法處理的障礙物,則需要分為3類:機械臂連桿圓柱體與長方體障礙物各面異面、相交、平行。由于使用長方體包絡法處理的障礙物的各個面都平行于坐標平面,因此,僅需要分別判斷3個軸方向的交疊部分是否大于設定的閾值,若大于,則產(chǎn)生碰撞。

其次,對于機械臂連桿之間的碰撞,可以通過檢測2個圓柱體中軸線線段之間的距離來進行判斷,假設線段AB是2條線段之間的最短距離,記作d,通過計算可求得機械臂之間的最小距離為dmin=d-(r1+r2),r1和r2分別是2個圓柱體的底面半徑,若最小距離dmin大于0,則連桿之間發(fā)生碰撞,若小于0,則無碰撞發(fā)生。

1.3 障礙物最小安全閾值

在機械臂工作時,為了避免受自然風影響,關節(jié)與樹枝果實產(chǎn)生不必要的碰撞,在分別計算得到機械臂的關節(jié)與樹枝,以及果實之間的距離后,將二者進行比較,取較小者作為機械臂與障礙物的距離。假設q為機械臂位姿,D1(q)則表示樹枝和各個關節(jié)的最小間距,如式(3)所示。

D1(q)=min{d(Bi,qj)|j=1,2,3,4,5,6}

(3)

式中:B代表樹枝;i代表樹枝數(shù)目;j代表機械臂關節(jié)。

機械臂的關節(jié)和果實的最小間距D2(q)如式(4)所示。

D2(q)=min{d(Fi,qj)|j=1,2,3,4,5,6}

(4)

式中:F代表果實;i代表果實數(shù)目。

由上文可得,機械臂與障礙物間的最小間距可由式(5)表示。

D(q)=min{D1(q),D2(q)}

(5)

同時再設定一個安全閾值Dsafe,只要機械臂與障礙物的距離處于閾值范圍內(nèi),即可確保機械臂和障礙物無碰撞,如果D(q)>Dsafe,那么選取Dsafe作為間距;如果D(q)

2 算法原理

RRT算法[17]是一種概率完備且基于采樣的算法,算法通過對狀態(tài)空間中的隨機采樣點進行碰撞檢測,避免了空間建模,能夠快速有效地搜索高維空間,從而尋找到一條從起始點到目標點的規(guī)劃路徑。

不同于和RRT算法同類型的算法PRM,PRM算法在開始時會先進行隨機采樣,構建完整的路徑拓撲圖,之后再進行搜索;RRT算法將二者同時進行,即邊采樣邊搜索。RRT算法比較依賴參數(shù)設置的合理程度,若參數(shù)設置不恰當,則很難得到合適的解。

圖5 RRT算法原理示意圖

然而,由于RRT算法的隨機采樣特性,致使其缺乏明確的目標導向性,最終路徑求解的時間也會相應變長[18],同時隨著迭代次數(shù)逐步增多,傳統(tǒng)算法極易陷入局部最優(yōu);隨機樹的擴展存在較多無用節(jié)點,造成路徑代價的增加;路徑生成不平穩(wěn),對于需要在復雜環(huán)境中完成采摘工作的機械臂而言,則會形成不必要的機械和能量損耗,因此,需要對傳統(tǒng)的RRT算法進行局部改良。

3 改進的算法

3.1 高斯分布采樣策略(S-RRT)

高斯分布采樣是一種在數(shù)據(jù)聚類中大量使用的采樣方法,該方法通過限制空間采樣區(qū)域,由數(shù)學方法對隨機搜索樹進行區(qū)間約束,隨著迭代次數(shù)的增加,在無障礙區(qū)域會有少量的采樣點生成,相反,在包含障礙物的復雜區(qū)域范圍內(nèi)或者障礙物之間的狹窄過道生成大量的采樣點,從而取代RRT算法本身的隨機采樣策略,進而加速路徑規(guī)劃的進程。高斯密度函數(shù)的n維函數(shù)表達式如式(6)所示。

(6)

式中σ指高斯函數(shù)的標準差,也代表著高斯函數(shù)的規(guī)模大小。

定義障礙函數(shù)obs(c),與障礙物發(fā)生碰撞時,值為1,否則為0,這樣就可以針對性地進行障礙物區(qū)域的采樣,構型空間中任意點被采樣的概率如式7所示。

(7)

從式(7)中可以看出,采樣點周圍的障礙物越多,式(7)的值越大,同時為了防止在障礙物內(nèi)部進行采樣,再定義一個函數(shù),如式(8)所示,顯然采樣點在障礙物中時,式(8)為0,否則g(c;σ)=f(c;σ)。

g(c;σ)=max(0,f(c;σ)-obs(c))

(8)

下面是高斯采樣生成采樣點的算法流程:

1) 計算出路徑規(guī)劃中起始點與目標點相隔的歐氏距離cmin;

(9)

式中:cbest指搜索得到的可行解中的最小成本解;μ指采樣中心點的位置。

3) 最后再從xball中隨機選擇出一個采樣點。

3.2 路徑冗余處理(A-RRT)

在進行高斯分布采樣策略處理后的RRT算法有效縮短了搜索時間,但仍存在許多冗余隨機點。因此,再將改進后的RRT算法與A*搜索算法相結合。利用A*代價函數(shù)搜索最佳采樣點,減少冗余計算,優(yōu)化路徑。在RRT算法中,從初始點xstart到所選節(jié)點的距離稱為正向代價,也可以稱為啟發(fā)式函數(shù)H(i);xrand和xgoal之間的距離稱為后向代價B(i)。則當前節(jié)點的代價函數(shù)可用F(i)表示,如式(10)所示。

(10)

如圖6所示,在得到一系列隨機節(jié)點6、7、8之后,分別計算與父節(jié)點xnearest之間的前向代價,以及與終點的后向代價,二者疊加,選擇代價函數(shù)最小的節(jié)點作為新節(jié)點xnew,并將其加入隨機樹列表。重復計算以上過程,直至擴展到目標節(jié)點。

圖6 A*代價函數(shù)更新節(jié)點

3.3 路徑簡化處理(G-RRT)

加入高斯采樣策略以及A*代價函數(shù)的RRT算法,能夠快速地向目標逼近,然而采樣的隨機點分布并不規(guī)律,導致規(guī)劃出的路徑存在許多轉折點,尤其是在果實和樹枝交錯生長的果園環(huán)境中,這樣會大幅度降低機械臂的工作效率,并且可能會導致機械臂發(fā)生不可逆的機械損耗和不必要的能量消耗,因此,繼續(xù)采用貪婪算法[19]簡化路徑。

算法的具體步驟為:首先從路徑終點xgoal開始,將xgoal與之前的路徑點按順序進行連接,如果2個節(jié)點的連線不會與障礙物相碰撞,那么就把當前路徑點剔除,再將xgoal和剔除節(jié)點的上一個節(jié)點相連,重復上述步驟。若與障礙物發(fā)生碰撞就留存碰撞節(jié)點的前一個沒有發(fā)生碰撞的節(jié)點,讓該節(jié)點成為父節(jié)點重復進行上述的操作,一直到與起始點xstart相連接算法結束,圖7為貪婪算法優(yōu)化示意圖。 改進的算法流程如圖8所示。

圖7 貪婪算法優(yōu)化示意圖

圖8 改進RRT算法流程框圖

4 仿真實驗

為了驗證改進RRT算法的有效性和優(yōu)越性,在Matlab中針對靜態(tài)障礙物進行了二維及三維實驗仿真,并且對改進的SAG-RRT算法和RRT算法進行了比較。二維環(huán)境中,將障礙物設置為方形及一些不規(guī)則的形狀,三維環(huán)境中設置為球體,此外,考慮到RRT算法本身的隨機性和對算法性能的客觀評價,因此,基于不同的實驗場景,針對RRT算法和改進的SAG-RRT算法分別進行100次模擬仿真實驗,將RRT算法的步長在二維、三維仿真中均設置為20,以便于記錄比較數(shù)據(jù),最后計算實驗評價指標的均值并做統(tǒng)計。

4.1 二維仿真實驗

簡單場景的算法測試如圖9所示,復雜場景如圖10所示,分別對比2組算法仿真圖可得:基本RRT算法運行時穩(wěn)定性較差,特別是在復雜二維環(huán)境下,算法會生成很多不必要的節(jié)點。然而改進的RRT算法在運行時表現(xiàn)穩(wěn)定,且在復雜環(huán)境下,改進后的算法相比原RRT算法擴展節(jié)點數(shù)減少,導向性顯著增強,節(jié)點樹簡單,路徑更加平滑。

圖9 簡單二維環(huán)境算法測試示意圖

圖10 復雜二維環(huán)境算法測試示意圖

由表2可知,在二維環(huán)境下,RRT算法的平均采樣點數(shù)為965個,而改進RRT算法的平均采樣點數(shù)為338個,減少了65%;此外,RRT算法的平均運行時間為7.82 s,而改進后的RRT為4.44 s,縮短了43%,這是因為后者在碰撞檢測次數(shù)和失敗的節(jié)點增長次數(shù)上比前者有明顯縮減。所以改進的SAG-RRT算法性能良好。

表2 二維環(huán)境算法性能

4.2 三維仿真實驗

設置一個三維環(huán)境,起點設置為(0,0,0),終點設置為(700,900,900),實驗結果如圖11和表3所示,在三維仿真環(huán)境中,基本的RRT算法規(guī)劃的路徑由于算法自身局限產(chǎn)生了較多的分支,冗余點多,規(guī)劃時間長。然而改進的RRT算法則能夠在短時間內(nèi)找到代價相對小的路徑,并且對比表2和表3可以看出:算法在三維環(huán)境中表現(xiàn)較二維環(huán)境更加優(yōu)越,這也保證了機械臂在實際工作場景中的應用。

表3 三維環(huán)境算法性能

圖11 三維環(huán)境算法測試示意圖

在第1節(jié)建立的6自由度機械臂運動學模型基礎上,經(jīng)過一系列的正運動學和逆運動學計算,再經(jīng)由改進的RRT算法可以得到各個關節(jié)以及末端的關節(jié)序列點,最后結合多項式插值法,可以在Matlab平臺上得到機械臂的避障運動軌跡,如圖12所示。

5 結束語

利用高斯采樣函數(shù)提升了RRT算法的導向性,同時加入了A*代價函數(shù)去除路徑的冗余點,最后融入貪婪算法簡化路徑。

在Matlab平臺通過不同的實驗環(huán)境證實了改進的SAG-RRT算法避障效果良好,路徑較平滑,效率更高,符合采摘機械臂實際工作需求。