基于擴(kuò)展角度控制的改進(jìn)RRT機(jī)械臂路徑規(guī)劃*

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

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

0 引言

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

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

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

1 RRT算法介紹

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

圖1 RRT算法擴(kuò)展原理

新節(jié)點生成公式為:

(1)

2 改進(jìn)RRT路徑規(guī)劃算法

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

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

2.1 概率目標(biāo)偏置函數(shù)

根據(jù)路徑規(guī)劃空間中障礙物的分布情況設(shè)置目標(biāo)概率a(a∈[0,1]),當(dāng)路徑規(guī)劃空間中障礙物較多時,a取較小值,反之a(chǎn)取較大值。在區(qū)間0～1內(nèi)隨機(jī)生成一個概率p,當(dāng)隨機(jī)概率pa時,采樣點仍為隨機(jī)點。隨機(jī)點公式為:

(2)

式中:Randomsample(x)為隨機(jī)采樣函數(shù)。

2.2 新節(jié)點擴(kuò)展角度約束

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

圖3 擴(kuò)展角示意圖

擴(kuò)展角θ的計算公式為:

(3)

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

(4)

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

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

(5)

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

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

(6)

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

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

(7)

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

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

新擴(kuò)展角生成后,新采樣點生成公式為:

(8)

圖4 擴(kuò)展角約束示意圖

以新擴(kuò)展角生成的新節(jié)點生成公式為:

(9)

故新節(jié)點生成公式為:

(10)

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

擴(kuò)展角閾值過大,會導(dǎo)致算法的規(guī)劃效率變低,導(dǎo)致優(yōu)化效果不明顯;擴(kuò)展角閾值過小,則會使路徑陷入局部最優(yōu),導(dǎo)致規(guī)劃失敗。所以要根據(jù)當(dāng)前環(huán)境合理選擇擴(kuò)展角閾值的大小,以提高算法的穩(wěn)定性。

2.3 路徑剪枝優(yōu)化

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

圖5 剪枝優(yōu)化流程圖

圖6中,原始路徑為起點—5—4—3—2—1—終點,剪枝優(yōu)化后的路線為起點—3—2—終點。通過對比發(fā)現(xiàn),剪枝優(yōu)化后的路徑節(jié)點更少,路徑長度也大大縮短。

2.4 路徑平滑優(yōu)化

通過剪枝優(yōu)化可以去除路徑中的冗余節(jié)點,但路徑中仍然存在一部分必要節(jié)點。由于這些節(jié)點的存在使路徑發(fā)生轉(zhuǎn)折,導(dǎo)致機(jī)械臂在實際運行中末端通過轉(zhuǎn)折點時,機(jī)械臂關(guān)節(jié)角會產(chǎn)生突變,不僅會降低機(jī)械臂運行的平穩(wěn)性,還會降低電機(jī)的壽命。因此需要對路徑進(jìn)行平滑處理,使其滿足路徑要求。

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

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

(11)

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

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

圖7 曲線擬合示意圖

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

3 仿真實驗及數(shù)據(jù)分析

3.1 二維空間仿真實驗

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

(a) 簡單環(huán)境 (b) 復(fù)雜環(huán)境

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

3.1.1 普通環(huán)境

由圖9和表1可以得出,在障礙物較少的環(huán)境下,兩種算法均有較高的成功率。相比于普通RRT算法,改進(jìn)RRT算法規(guī)劃出的路徑有更強(qiáng)的目標(biāo)導(dǎo)向性,減少了大量的無用路徑。由圖10和表2可以得出,在障礙物較多的環(huán)境下,普通RRT算法的成功率明顯下降。相比之下,改進(jìn)RRT算法具有更好的收斂性和穩(wěn)定性,規(guī)劃效率更高。

表1 簡單環(huán)境仿真數(shù)據(jù)統(tǒng)計

表2 復(fù)雜環(huán)境仿真數(shù)據(jù)統(tǒng)計

(a) 原始RRT (b) 改進(jìn)RRT

3.1.2 特殊環(huán)境

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

表3 狹窄通道環(huán)境仿真數(shù)據(jù)統(tǒng)計

(a) 原始RRT (b) 改進(jìn)RRT

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

表4 迷宮環(huán)境仿真數(shù)據(jù)展示

(a) 原始RRT (b) 改進(jìn)RRT

3.2 三維空間仿真實驗

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

(a) 原始RRT (b) 改進(jìn)RRT

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

3.3 機(jī)械臂避障實驗

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

采用標(biāo)準(zhǔn)DH法建立機(jī)械臂數(shù)學(xué)模型,DH參數(shù)如表5所示。

表5 機(jī)械臂DH參數(shù)表

其中,θ表示連桿角度,d表示連桿偏距,a表示連桿長度,α表示連桿轉(zhuǎn)角。

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

圖14 機(jī)械臂運行仿真結(jié)果

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

為了驗證路徑優(yōu)化策略的可行性,根據(jù)原始RRT算法規(guī)劃出的路徑節(jié)點和改進(jìn)RRT算法規(guī)劃出的路徑節(jié)點,分別求得機(jī)械臂關(guān)節(jié)角度變化圖如圖15、圖16所示。

圖15 原始RRT關(guān)節(jié)角度變化圖 圖16 改進(jìn)RRT關(guān)節(jié)角度變化圖

通過對比圖15、圖16可以看出,使用改進(jìn)RRT算法的機(jī)械臂在運動的過程中,關(guān)節(jié)角度變化更加平穩(wěn),沒有出現(xiàn)較大的波動,由此可以驗證算法路徑平滑優(yōu)化策略的可行性。

4 結(jié)束語

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

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

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

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

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