基于擴展角度控制的改進RRT機械臂路徑規劃*

朱 花,郝 赫,陽 明,劉正超

(江西理工大學機電工程學院,贛州 341000)

0 引言

隨著工業自動化水平的不斷提升,越來越多的機械臂被應用到工業生產領域,以代替人類完成一些高精度或特殊環境下的工作任務,有效提升了生產效率[1]。路徑規劃技術是機械臂的研究熱點和難點之一[2-6],其任務是使機械臂自主地規劃出一條從起始位置到目標位置,且能避開所有障礙物的路徑[7]。路徑規劃的成功率以及路徑質量的高低是影響機械臂能否完成任務的關鍵因素[8]。

機械臂的路徑規劃算法主要有Dijkstra算法[9]、A*算法[10]、PRM算法[11]和RRT算法等[12]?；诓蓸勇窂降腞RT算法具有結構簡單,搜索能力強,且能適應高維空間的優點,被廣泛應用到機械臂的路徑規劃中[13]。但是RRT算法也存在著缺乏導向性、節點數量多、擴展方向隨機性強且路徑不是最優等問題[14]。目前,國內外大量學者針對RRT算法的不足提出了改進方案。鐘華庚等[15]采用重選父節點和區域排斥策略剔除無效節點,縮短迭代時間;JEONG等[16]提出一種Quick-RRT*算法,將父節點點集擴大到預設的范圍內,減小路徑成本;LAVALLE等[17]提出一種Bi-RRT算法,分別從起點和終點生成兩棵隨機樹,加快收斂速度;張勤等[18]通過柯西分布的方法進行啟發式采樣,引入節點拒絕策略,提高局部搜索速度;韓康等[19]提出一種Obi-RRT算法,采用智能采樣,拒絕高成本路徑點,降低算法的路徑成本;崔永杰等[20]引入評價指數,實時引導隨機樹的擴展。

上述文獻提及算法主要對搜索速度、采樣方式和路徑質量進行了優化,但是沒有對新節點的擴展方式進行改進。本文主要針對RRT算法新節點擴展方向隨機性強的問題,提出一種改進RRT算法,首先引入概率目標偏置函數,使新節點以一定概率朝向目標點生長;然后提出擴展角的概念,并通過障礙占空比約束擴展角的大小,使算法增加導向性的同時保留一定的隨機性,提高算法的成功率;隨后對生成的路徑進行優化,增加機械臂運動時的平穩性;最后通過MATLAB平臺對本文算法進行仿真,驗證算法的有效性和可行性。

1 RRT算法介紹

RRT算法是一種基于隨機采樣的全局路徑規劃算法,其原理如下:①首先確定路徑的起始點xstart和目標點xgoal,以起始點xstart作為根節點開始構建樹;②在規劃空間內隨機采樣生成隨機點xrand;③遍歷樹,找到樹中距離隨機點最近的節點,記為最近點xnear,并以隨機點xrand方向擴展一個步長的距離,得到新節點xnew;④若新節點與最近點的連線存在障礙物,則舍棄新節點,重新開始采樣;若連線無障礙物,則將新節點添加到樹中;⑤重復上述步驟,直到新節點與目標點之間的距離小于給定的目標點閾值,則連接目標點與新節點,成功生成路徑,規劃結束。擴展原理如圖1所示。

圖1 RRT算法擴展原理

新節點生成公式為:

(1)

2 改進RRT路徑規劃算法

本文提出的改進RRT算法首先使用概率目標偏置函數,使采樣點以一定概率偏向目標點,隨后計算擴展角和障礙占空比的大小,通過障礙占空比約束擴展角的大小,生成新采樣點。隨后生成新節點,并判斷新節點與最近點連線上是否有障礙物。算法整個流程如圖2所示。

圖2 改進RRT算法流程圖

2.1 概率目標偏置函數

根據路徑規劃空間中障礙物的分布情況設置目標概率a(a∈[0,1]),當路徑規劃空間中障礙物較多時,a取較小值,反之a取較大值。在區間0～1內隨機生成一個概率p,當隨機概率pa時,采樣點仍為隨機點。隨機點公式為:

(2)

式中:Randomsample(x)為隨機采樣函數。

2.2 新節點擴展角度約束

首先引入擴展角θ的概念。當采樣點的采樣方式為隨機采樣時,分別求得最近點xnear與隨機點xrand,最近點xnear和目標點xgoal的方向向量,將兩向量的夾角定義為擴展角,如圖3所示。

圖3 擴展角示意圖

擴展角θ的計算公式為:

(3)

擴展角的大小反映了新節點的擴展方向。擴展角越大,說明新節點的擴展方向離目標點越遠,導致算法的規劃效率變低。所以需要控制擴展角的大小,具體的做法為:設置擴展角閾值為α,擴展角閾值的大小根據路徑規劃空間中障礙的分布情況決定,當障礙物較多時,α取較大值;當障礙物較少時,α取較小值。算法進行一次隨機采樣后,計算擴展角的大小θ,并判斷擴展角的大小是否在擴展角閾值內。當擴展角小于閾值α時,不對擴展角進行處理;當擴展角大于閾值α時,通過式(4)生成新的擴展角θ1:

(4)

式中:k為障礙占空比,其值為二維空間下以最近點為圓心,兩個步長為半徑畫圓,圓內障礙物與圓面積的比值;三維空間下以最近點為球心,兩個步長為半徑畫球體,球內障礙物與球體積的比值。

為求出障礙占空比k,需先求得二維空間下障礙物的面積,三維空間下障礙物的體積。在二維空間中,為便于計算,取障礙物邊界函數為障礙物與檢測圓兩交點的一次函數。障礙物的面積Sobstacle計算公式為:

(5)

式中:x1、x2為檢測圓與障礙物的交點,d為最近點與x1x2連線的距離,s為步長。

在三維空間中,為便于計算,將障礙物簡化為球形。則障礙物的體積Vobstacle計算公式為:

(6)

式中:h1、h2為球缺的高,r1為障礙物半徑,r2為檢測球半徑。

由此可求得障礙占空比k的表達式為:

(7)

式中:R為采樣空間維度。

k值的大小反映了最近點附近的障礙物分布情況。k值較大時,說明最近點附近的障礙較多,此時適當降低對擴展角的約束以獲得較大的擴展角,以此來增強算法的探索性,避免算法陷入局部最優。當k值較小時,說明最近點附近障礙物較少,故增大對擴展角的限制,使擴展角變小,增加算法的收斂性,使算法更快收斂到目標點。擴展角被約束后,隨機點位置也隨之變化,整個過程如圖4所示。

新擴展角生成后,新采樣點生成公式為:

(8)

圖4 擴展角約束示意圖

以新擴展角生成的新節點生成公式為:

(9)

故新節點生成公式為:

(10)

式中:s為步長,θ為擴展角,α為擴展角閾值。

擴展角閾值過大,會導致算法的規劃效率變低,導致優化效果不明顯;擴展角閾值過小,則會使路徑陷入局部最優,導致規劃失敗。所以要根據當前環境合理選擇擴展角閾值的大小,以提高算法的穩定性。

2.3 路徑剪枝優化

通過前文所述的改進方法可以大大縮短RRT算法的規劃時間,但是由于RRT算法的隨機性,其規劃出的路徑仍含有冗余節點,這不僅會增加路徑成本,還會導致機械臂在實際運行中的平穩性降低。本文使用剪枝優化刪去冗余節點,算法流程和優化過程如圖5和圖6所示。

圖5 剪枝優化流程圖

圖6中,原始路徑為起點—5—4—3—2—1—終點,剪枝優化后的路線為起點—3—2—終點。通過對比發現,剪枝優化后的路徑節點更少,路徑長度也大大縮短。

2.4 路徑平滑優化

通過剪枝優化可以去除路徑中的冗余節點,但路徑中仍然存在一部分必要節點。由于這些節點的存在使路徑發生轉折,導致機械臂在實際運行中末端通過轉折點時,機械臂關節角會產生突變,不僅會降低機械臂運行的平穩性,還會降低電機的壽命。因此需要對路徑進行平滑處理,使其滿足路徑要求。

本文采用二階貝塞爾曲線對路徑轉折點處進行曲線擬合,其公式為:

B(t)=(1-t)2P0+2t(1-t)P1+t2P2

(11)

式中:t為比例系數,P0、P2為數據點坐標,P1為控制點坐標,B(t)為擬合后的曲線坐標。

取要優化的節點為P1,為了防止擬合后的曲線碰到障礙物,將轉折點P1前后路徑方向距離1/2步長處分別設置為數據點P0、P2。擬合過程如圖7所示。

圖7 曲線擬合示意圖

圖7中,黑色線段為原路徑,紅色線段為擬合后的路徑。對比發現,擬合后的路徑有效祛除了轉折點,平滑度大大提升。

3 仿真實驗及數據分析

3.1 二維空間仿真實驗

為了驗證上述改進算法的可行性,將本文算法與普通RRT算法通過MATLAB仿真平臺在不同環境下進行對比仿真實驗,仿真環境分為普通環境和特殊環境兩種,其中普通環境分為簡單環境和復雜環境;特殊環境分為狹窄通道環境和迷宮環境,具體環境如圖8所示。

(a) 簡單環境 (b) 復雜環境

4種環境地圖大小均為[800,800];起始點坐標為(10,10);目標點坐標為(750,750);步長為10;最大迭代次數為5000;擴展角閾值設置為90°。將改進RRT算法與普通RRT算法分別進行50次實驗,并收集路徑長度、規劃時間和成功率,計算出平均路徑長度和平均規劃時間,通過分析數據以驗證改進RRT算法的可行性。

3.1.1 普通環境

由圖9和表1可以得出,在障礙物較少的環境下,兩種算法均有較高的成功率。相比于普通RRT算法,改進RRT算法規劃出的路徑有更強的目標導向性,減少了大量的無用路徑。由圖10和表2可以得出,在障礙物較多的環境下,普通RRT算法的成功率明顯下降。相比之下,改進RRT算法具有更好的收斂性和穩定性,規劃效率更高。

表1 簡單環境仿真數據統計

表2 復雜環境仿真數據統計

(a) 原始RRT (b) 改進RRT

3.1.2 特殊環境

圖11主要測試改進RRT算法在狹窄通道下的規劃能力。根據圖11和表3可以看出,在狹窄通道環境下普通RRT算法的規劃成功功率很低,改進RRT算法的成功率可以達到80%的成功率,平均路徑長度縮短19.6%,平均規劃時間減少52.7%。由此可見,改進RRT算法在狹窄通道環境下有更好的效率和性能。

表3 狹窄通道環境仿真數據統計

(a) 原始RRT (b) 改進RRT

由圖12和表4可知,在迷宮環境下改進RRT依然有更短的路徑、更少的規劃時間和更高的成功率。通過以上仿真實驗可以得出,本文提出的改進RRT算法在多種環境下均有良好的穩定性和收斂性,有效的驗證了本文算法的可行性。

表4 迷宮環境仿真數據展示

(a) 原始RRT (b) 改進RRT

3.2 三維空間仿真實驗

為了驗證本文算法在三維環境下的規劃能力,使用MATLAB仿真平臺對本文算法進行三維仿真實驗,設置地圖大小為[250,250,250],起始點坐標為(10,10,10),終點坐標為(150,150,150),步長為10,仿真結果如圖13所示。

(a) 原始RRT (b) 改進RRT

根據圖13可以得出,改進RRT算法在三維環境下可以有效減少無用節點和多余分叉樹,且生成的路徑更加平滑,驗證了本文算法在三維環境下的可行性。

3.3 機械臂避障實驗

為了驗證本文算法在機械臂實際運行中的可行性,設計一臺六自由度機械臂,并使用MATLAB RoboticsToolbox平臺進行路徑規劃仿真實驗。實驗任務為:使用改進RRT算法規劃出一條從起始點到目標點之間可以避開障礙物的路徑,并驅動機械臂模型進行實驗驗證,未碰撞到障礙物即為實驗成功。

采用標準DH法建立機械臂數學模型,DH參數如表5所示。

表5 機械臂DH參數表

其中,θ表示連桿角度,d表示連桿偏距,a表示連桿長度,α表示連桿轉角。

仿真具體參數設置為:設置起點坐標為(30,-50,0),終點坐標為(30,50,50),步長為5,最大迭代次數為10 000,障礙物簡化為實心球體,仿真結果如圖14所示。

圖14 機械臂運行仿真結果

由圖14可以看出,機械臂沿著規劃好的軌跡從起點運行到了目標點,且成功避開障礙物。驗證了算法在機械臂運動中的可行性。

為了驗證路徑優化策略的可行性,根據原始RRT算法規劃出的路徑節點和改進RRT算法規劃出的路徑節點,分別求得機械臂關節角度變化圖如圖15、圖16所示。

圖15 原始RRT關節角度變化圖 圖16 改進RRT關節角度變化圖

通過對比圖15、圖16可以看出,使用改進RRT算法的機械臂在運動的過程中,關節角度變化更加平穩,沒有出現較大的波動,由此可以驗證算法路徑平滑優化策略的可行性。

4 結束語

本文針對RRT算法存在收斂速度慢、路徑冗長且在復雜的環境下成功率低的問題,提出一種改進RRT算法,對原始RRT算法做出以下幾點改進:

(1)使用概率目標偏置策略,提升算法的收斂性和規劃速度;

(2)使用擴展角度控制策略,限制新節點的擴展方向,有效減少了無用節點的生成;

(3)使用剪枝優化和二階貝塞爾曲線對生成的路徑進行優化,使生成的路徑更符合機械臂的運動方式。

通過對比在不同環境下的仿真實驗可以得出,改進的RRT算法可以有效提升規劃的成功率,且降低了路徑長度和規劃時間,在不同環境下也有較高的穩定性。