改進RRT算法的采摘機械臂路徑規劃

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

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

0 引言

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

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

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

1 算法基礎

1.1 機械臂運動學建模

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

圖1 6自由度機械臂關節示意圖

圖2 關節坐標系示意圖

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

表1 D-H參數

圖3 D-H連桿空間狀態關系示意圖

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

(1)

(2)

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

圖4 Matlab機械臂建模示意圖

1.2 機械臂碰撞檢測

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

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

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

1.3 障礙物最小安全閾值

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

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

(3)

式中:B代表樹枝;i代表樹枝數目;j代表機械臂關節。

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

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

(4)

式中:F代表果實;i代表果實數目。

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

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

(5)

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

2 算法原理

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

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

圖5 RRT算法原理示意圖

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

3 改進的算法

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

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

(6)

式中σ指高斯函數的標準差,也代表著高斯函數的規模大小。

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

(7)

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

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

(8)

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

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

(9)

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

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

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

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

(10)

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

圖6 A*代價函數更新節點

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

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

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

圖7 貪婪算法優化示意圖

圖8 改進RRT算法流程框圖

4 仿真實驗

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

4.1 二維仿真實驗

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

圖9 簡單二維環境算法測試示意圖

圖10 復雜二維環境算法測試示意圖

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

表2 二維環境算法性能

4.2 三維仿真實驗

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

表3 三維環境算法性能

圖11 三維環境算法測試示意圖

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

5 結束語

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

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