基于改進RRT-Connect算法的機械臂路徑規(guī)劃

韓 康 程衛(wèi)東

(北京交通大學(xué)機械與電子控制工程學(xué)院 北京 100044)

0 引 言

路徑規(guī)劃是指在狀態(tài)空間中規(guī)劃一條包含初始狀態(tài)和目標(biāo)狀態(tài)的無碰撞路徑[1]。近年來，由于實際生產(chǎn)活動對智能自動化的高度需求，路徑規(guī)劃被廣泛地應(yīng)用到自動駕駛汽車[2]、無人機航跡規(guī)劃[3]和機器人路徑規(guī)劃[4]等多個領(lǐng)域中。多數(shù)應(yīng)用場景下，對快速獲得低成本路徑都有需求。機械臂是一種常應(yīng)用于工業(yè)生產(chǎn)的機器人，由于其狀態(tài)空間維度高，一些傳統(tǒng)路徑規(guī)劃方法無法在短時間內(nèi)得到解決方案[5]，目前主要使用的方法是快速擴展隨機樹[6](Rapid-exploration Random Tree，RRT)系列算法。

RRT是由Lavalle等提出的基于采樣的路徑規(guī)劃算法。該算法理論上可以解決任意維度空間的路徑規(guī)劃問題，但在高維空間中速度比較慢。因此，其又提出改進算法RRT-Connect[1]，通過雙樹擴展提高算法速度。為解決RRT算法易產(chǎn)生高成本路徑的問題，Karaman等[7]提出最優(yōu)路徑規(guī)劃算法RRT*。該算法通過在每次迭代中尋找最優(yōu)父節(jié)點以及節(jié)點的重新布線來降低路徑成本，從而使路徑收斂至最優(yōu)，但收斂速度較慢。許多學(xué)者為加速收斂速度不斷提出新的改進算法。Jordan等[8]提出的B-RRT*算法是RRT*的雙樹版本，使用雙向搜索和局部偏置優(yōu)化的方法來加速收斂速度；Islam等[9]提出的RRT*-Smart算法使用RRT*生成初始路徑后，通過刪除冗余節(jié)點和偏置采樣，改善了RRT*收斂緩慢問題；Gammell等[10]提出Informed RRT*算法，采用直接限制采樣范圍來提高整體收斂速度，在此基礎(chǔ)上Burget等[11]結(jié)合B-RRT*提出雙樹版本BI-RRT*算法；Zhang等[12]提出的Self-learning RRT*采用基于自學(xué)習(xí)策略和混合偏差抽樣方案來提高規(guī)劃效率；陳晉音等設(shè)計了EB-RRT*[13]和貪心MB-RRT*算法[14]，分別利用地圖柵格分區(qū)和貪心策略來提高算法速度。這些RRT*的改進算法在實現(xiàn)路徑規(guī)劃時仍存在以下兩個問題：

(1) 在近鄰節(jié)點查詢過程中，大量的節(jié)點遍歷操作嚴(yán)重影響算法速度；

(2) 局部偏置采樣注重收斂速度而犧牲隨機探索特性，可能會丟失更優(yōu)解[15]，導(dǎo)致最終優(yōu)化路徑的質(zhì)量往往取決于初始路徑的質(zhì)量。

針對RRT系列算法運行時間與路徑成本之間的平衡問題，本文提出Obi-RRT進行解決。Obi-RRT通過添加目標(biāo)偏置并且拒絕可能產(chǎn)生高成本路徑的采樣點，快速得到較低成本的初始路徑，進而進行路徑修剪得到路徑關(guān)鍵點；在關(guān)鍵點周圍，定義三種采樣空間，不斷進行隨機采樣，當(dāng)使用新點得到的路徑成本低于原始成本并且路徑通過碰撞檢測時，替換舊點，從而使路徑不斷收斂至最優(yōu)。由于省去了RRT*系列算法近鄰節(jié)點查詢的過程，Obi-RRT能在更短的時間內(nèi)產(chǎn)生低成本路徑。此外，因為路徑中節(jié)點很少，更有利于機械臂控制。

1 Obi-RRT算法

1.1 問題描述與定義

文中所研究的狀態(tài)空間分別為二維平面和六維工業(yè)機械臂關(guān)節(jié)空間。平面空間的狀態(tài)節(jié)點定義為在直角坐標(biāo)系下點的坐標(biāo)所組成的向量，記為q2(x,y)。節(jié)點之間成本定義為坐標(biāo)系下兩點的直線距離。機械臂關(guān)節(jié)空間的狀態(tài)節(jié)點定義為機械臂某個姿態(tài)下關(guān)節(jié)角所組成的六維向量q6(θ1,θ2,θ3,θ4,θ5,θ6)，節(jié)點之間成本定義為它們關(guān)節(jié)角向量的歐拉距離。

給定狀態(tài)空間X，障礙物區(qū)域和無障礙區(qū)域分別定義為Xobs和Xfree。起始狀態(tài)qinit∈Xfree，目標(biāo)狀態(tài)qgoal∈Xfree。Obi-RRT算法的目的是找到一條路徑s，使qinit，qgoal∈s和s∈Xfree并且使s的成本達到最小。為此，算法構(gòu)建了兩棵隨機樹Ta=(Va，Ea)，Tb=(Vb，Eb)，其中Va，Vb∈Xfree是樹中狀態(tài)節(jié)點集合，Ea，Eb∈Xfree是連接兩個狀態(tài)節(jié)點邊的集合。

1.2 算法基本流程

Obi-RRT算法分為路徑搜索、路徑修剪和路徑優(yōu)化三個階段。路徑搜索階段使用改進的RRT-Connect算法快速得到一條較低成本的路徑。再經(jīng)過路徑修剪階段，連接了路徑中可直接相連的節(jié)點來初步降低路徑成本，并大幅度降低路徑中節(jié)點數(shù)量，得到路徑關(guān)鍵點集keypoints。在路徑優(yōu)化階段，通過在關(guān)鍵點周圍采樣不斷更新關(guān)鍵點，產(chǎn)生更低成本的路徑，達到快速收斂的目的。算法主體過程如算法1所示。

算法1Obi-RRT算法

Obi-RRT(qinit,qgoal)

1Va={qinit};Vb={qgoal};Ea=?;Eb=?;i=0;

2while(i

3qrand=SmartSample(Ta,Tb);i++;

4ifnot(Extend(Ta,qrand)=Trapped)then

5if(Connect(Tb,qnew)=Reached)then

6returnpath=Path(Ta,Tb);

7Swap(Ta,Tb);

8returnFailure;

9keypoints=GetKeyPointsPWTBX](path);

10lengthold=PathLength(keypoints);

11 (keypoints,length)=PathOptimization(keypoints);

12returnpath=keypoints;

1.3 路徑搜索階段

Obi-RRT算法同RRT-Connect算法一樣，使用兩棵樹Ta和Tb分別從起始狀態(tài)和目標(biāo)狀態(tài)開始，向?qū)Ψ缴L。不同的是在新算法中使用目標(biāo)偏置采樣和啟發(fā)式采樣來加速算法。

算法存在兩種目標(biāo)偏向，一種是以一定概率采用qinit或qgoal作為采樣點，一種是以一定概率采用對方最新擴展的狀態(tài)節(jié)點qpre_node作為采樣點。第一種目標(biāo)偏置引導(dǎo)兩棵樹偏向起點和終點生長，第二種目標(biāo)偏置也引導(dǎo)兩棵樹偏向?qū)Ψ阶钚聰U展節(jié)點生長，通過兩種偏置結(jié)合，加速了兩棵隨機樹的連接過程。

同時，算法還使用了啟發(fā)式采樣來保證獲取到較低成本的初始路徑。假設(shè)當(dāng)前為樹Ta生長，采樣點為qrand，其與Ta距離最近的節(jié)點為qnst_a，與Tb距離最近的節(jié)點為qnst_b，計算路徑預(yù)期成本Cex：

Cex=qnst_a.cost+qnst_b.cost+

Cost(qrand,qnst_a)+Cost(qrand,qnst_b)

(1)

當(dāng)Cex>Cmax時，qrand被拒絕，并重新進行采樣。拒絕低質(zhì)量采樣點可以減少不必要的擴展過程，提高了算法速度。算法采樣過程偽代碼如算法2所示。

算法2采樣過程

SmartSample(Ta,Tb)

1while(qrand=null)do

2p=Random(0, 1);

3if(p

4if(p>pgoal)returnqrand=qpre_node;break;

5qrand=Random();

6qnst_a=Nearest(Ta,qrand)；

7qnst_b=Nearest(Tb,qrand)；

8h=qnst_a.cost+qnst_b.cost+

9Cost(qnst_a,qrand)+Cost(qnst_b,qrand);

10if(h>hpre_set)qrand=null;

11elsereturnqrand

1.4 路徑修剪階段

當(dāng)算法成功得到一條無碰撞路徑時，便進入路徑修剪階段。該階段以三角不等式原理來優(yōu)化路徑。如圖1所示，c邊的長度總是小于a邊和b邊的總和。

圖1 基于三角不等式優(yōu)化路徑原理

路徑修剪將連接路徑節(jié)點中可以直接相連的節(jié)點，刪掉中間節(jié)點，得到路徑關(guān)鍵點集keypoints。該點集中任意兩個非相鄰節(jié)點不可以直接相連。由此點集構(gòu)成的路徑成本比原路徑更低。路徑修剪過程的偽代碼如算法3所示。

算法3路徑修剪過程

GetKeyPoints(path)

1q1=qgoal;q2=q1.parent.parent;keypoints=?;

2while(q2!=null)do

3ifObstacleFree(q1, q2) q2=q2.parent;

4elsekeypoints={q2.child,keypoints}

5q1=q2.child;q2=q1.parent.parent;

6keypoints={qinit, keypoints,qgoal}

7for(q1=qgoal;q1!=qinit.child.child;q1=q1.parent)

8for(q2=q1.parent.parent;q1!=qinit;q2=q2.parent)

9ifObstacleFree(q1,q2)

10keypoints=Remove(q2.child);

11returnkeypoints;

keypoints中節(jié)點數(shù)量代表著此條路徑中需要轉(zhuǎn)折的次數(shù)，也一定程度上反映了該狀態(tài)空間的復(fù)雜程度。路徑關(guān)鍵點越多，狀態(tài)空間中障礙物分布越復(fù)雜。

1.5 路徑優(yōu)化階段

路徑優(yōu)化階段中將在關(guān)鍵點周圍的特定范圍空間內(nèi)產(chǎn)生一定數(shù)量的樣本,并假設(shè)新點替代原關(guān)鍵點，進行路徑成本計算與碰撞檢測。如果使用新點可以產(chǎn)生更低成本的路徑并且通過了碰撞檢測則使用新點替換原始關(guān)鍵點。

本文提出三種采樣空間，圖2以二維平面空間進行說明。如圖2(a)所示，一個關(guān)鍵點集合中，隨機選擇一個關(guān)鍵點b作為采樣基準(zhǔn)，節(jié)點a為其父節(jié)點，節(jié)點c為其子節(jié)點。采樣將圍繞這三個連續(xù)的節(jié)點進行。第一種采樣范圍是由這三個連續(xù)節(jié)點組成的最小包圍盒，稱作A類空間，如圖2(b)所示。第二種采樣范圍是由以節(jié)點a和節(jié)點c的中心節(jié)點o為中心，ob為半徑的空間球體,稱作B類空間，如圖2(c)所示。第三種采樣范圍是由節(jié)點b為中心，半徑為r的空間球體中進行采樣，稱作C類空間，如圖2(d)所示。

圖2 三種采樣空間二維示意圖

路徑關(guān)鍵點一定意義上代表著路徑的轉(zhuǎn)折點，其給出了關(guān)于障礙物頂點或者邊緣的信息[9]。局部重采樣希望在已有路徑關(guān)鍵點周圍盡可能生成靠近障礙的節(jié)點，進行關(guān)鍵點更新，從而實現(xiàn)路徑快速收斂于最優(yōu)。路徑優(yōu)化過程偽代碼如算法4所示。

算法4路徑優(yōu)化過程

PathOptimization(keypoints)

1while(i

2qn=RandomSelectionNode(keypoints);

3qnp=qn.parent;qnc=qn.child;

4p=Random(0, 1);

5if(p

6if(p>pb)qrand=BSample(qn,qnp,qnc);break;

7qrand=CSample(qn,r);

8lengthold=Cost(qn,qnp)+Cost(qn,qnc)

9lengthnew=Cost(qrand,qnp)+Cost(qrand,qnc)

10if(lengthnew

11if(ObstacleFree(qnp,qrand,qnc))

12qn=qrand;

13length=length-lengthold+lengthnew

14if(TimeOut‖IterationOverflow)break;

15return(keypoints,length)

采樣范圍的大小決定了優(yōu)化的效率。對于上述所提出的三種采樣空間，采樣范圍大小一般是A大于C，B大于C。其中，A類和B類采樣空間的大小與連續(xù)三個節(jié)點的分布有關(guān)，其大小無法直接比較，不過隨著節(jié)點的不斷更新，A、B兩類采樣空間的大小在不斷減小。C類采樣空間需要人為指定半徑r。其設(shè)置得過大則失去了降低采樣范圍的意義，過小又會導(dǎo)致采樣次數(shù)的增加，兩者都不同程度上影響優(yōu)化速度，其取值與狀態(tài)空間障礙物的分布有關(guān)。這也是引入A類和B類采樣空間的原因，它們的大小完全取決于節(jié)點本身。

一般來說，只使用C類空間算法會更快，但會產(chǎn)生路徑只收斂至局部最優(yōu)的問題，丟失了全局更優(yōu)解。如圖3(a)的情況，初始路徑并不理想，如果此時單獨使用C類采樣空間，則最終只能得到圖3(a)中a-b’-c的結(jié)果。而最優(yōu)路徑是a-boptimal-c。引入A類和B類采樣空間可以改善這個問題。如圖3(b)和圖3(c)所示，在這兩種采樣空間中始終可以獲得靠近boptimal的點，從而使路徑收斂向最優(yōu)。在圖示情況下，一開始A類采樣空間效率要高于B類。但這并不是固定不變的，在路徑較為平緩的情況下，B類采樣空間效率要高于A類。A類和B類采樣均有一定幾率將質(zhì)量較差的路徑修正。

圖3 使用不同采樣方法得到的最優(yōu)結(jié)果

2 狀態(tài)空間模型

仿真實驗將在平面和機械臂關(guān)節(jié)兩種狀態(tài)空間中進行。隨著狀態(tài)空間維度增加，計算復(fù)雜度依次增加。測試的算法為RRT*和Obi-RRT。

平面空間的仿真采用MATLAB 2018仿真平臺，圖4是兩種不同的平面空間，大小均為500×500。其中，深色部分為障礙物，白色部分為自由空間。每個空間的起點和終點均固定。

圖4 平面仿真環(huán)境

六軸機械臂關(guān)節(jié)空間仿真使用OpenRAVE仿真平臺。圖5展示了機器人關(guān)節(jié)空間仿真環(huán)境的三維場景，其中：(a)為機器人起始位置，(b)為機器人目標(biāo)位置。這個仿真實驗?zāi)M機械臂抓取深箱物體的場景。

圖5 六軸機械臂關(guān)節(jié)空間仿真環(huán)境

3 仿真與實驗驗證

3.1 二維平面空間實驗結(jié)果

首先對RRT*與Obi-RRT算法進行了相同迭代次數(shù)的對比實驗。圖6(a)和圖6(b)分別是RRT*迭代2 000次和4 000次的結(jié)果，圖6(c)和圖6(d)分別是Obi-RRT迭代2 000次和4 000次的結(jié)果。

圖6 RRT*與Obi-RRT在相同迭代次數(shù)下仿真結(jié)果

在迭代2 000次時，RRT*得到的初始路徑成本更低。這是由于Obi-RRT得到了一條較差的初始路徑導(dǎo)致的。當(dāng)?shù)螖?shù)到達4 000時，Obi-RRT算法所得到的路徑成本低于RRT*。值得注意的是，Obi-RRT通過A類和B類采樣，將質(zhì)量較差的初始路徑成功修正。RRT*和Obi-RRT兩種算法執(zhí)行完4 000步迭代分別需要10.56 s和0.423 s，得到的路徑長度分別為969.77 m和951.35 m?？梢钥闯?，Obi-RRT可以在更短的時間內(nèi)獲得低成本路徑。

對圖4(b)所示的平面狀態(tài)空間分別使用兩種算法進行測試，實驗結(jié)果如圖7所示。

圖7 兩種算法在復(fù)雜平面空間中的實驗結(jié)果

為了證明算法的一般性，對每個空間使用兩種算法分別運行100次求算法運行耗時與路徑成本的平局值，實驗統(tǒng)計結(jié)果如圖8所示。在所有的實驗中，僅RRT*出現(xiàn)四次路徑搜索失敗的情況。圖8(a)顯示，Obi-RRT算法運行時長平均不超過0.5 s，僅為RRT*算法的十分之一。圖8(b)顯示，Obi-RRT算法規(guī)劃的路徑平均成本更低。

圖8 兩種算法在平面狀態(tài)空間的實驗結(jié)果

3.2 六維機械臂關(guān)節(jié)空間實驗結(jié)果

對圖5所示的機械臂關(guān)節(jié)空間分別使用兩種算法進行路徑規(guī)劃。兩算法搜索到的路徑結(jié)果如圖9所示。

圖9 兩種算法在六軸機械臂關(guān)節(jié)空間中搜索到的路徑結(jié)果

為了證明算法的一般性，在同一個狀態(tài)空間中，每個算法分別運行100次，對計算耗時與路徑成本進行統(tǒng)計，實驗統(tǒng)計結(jié)果如圖10所示。

圖10 兩種算法在六軸機械臂關(guān)節(jié)狀態(tài)空間的實驗結(jié)果

RRT*算法運行時間長，平均為26.67 s。Obi-RRT算法平均運行時長為0.815 s，較RRT*有很大改善。在路徑成本方面，從圖10(b)可以看出，RRT*算法所規(guī)劃出的路徑成本波動性仍比較大，其路徑平均成本為2.759 rad。Obi-RRT算法所規(guī)劃出的路徑成本波動性很小，其路徑的平均成本為1.684 rad，比RRT*所規(guī)劃的路徑平均成本低39%。此外，在100次迭代中，RRT*算法有31次沒能找到可行路徑。這主要是由于在高維環(huán)境下，采樣的隨機性太強，導(dǎo)致樹沒有及時探索到目標(biāo)狀態(tài)附近導(dǎo)致的。

4 結(jié) 語

針對RRT系列算法規(guī)劃的路徑成本較高或收斂速度慢的不足，本文提出了一種快速收斂至最優(yōu)路徑的Obi-RRT算法。算法做出了以下幾點改進:

(1) 路徑搜索階段，采用智能采樣，加速搜索過程并拒絕生長那些可能產(chǎn)生高成本路徑的采樣點以保證算法得到較低成本的初始路徑；

(2) 路徑修剪階段，對路徑節(jié)點進行修剪得到少量路徑關(guān)鍵點；

(3) 路徑優(yōu)化階段，在關(guān)鍵點周圍進行重新采樣，有效減小采樣范圍，提高算法收斂速度。

仿真實驗表明所提算法是有效的。Obi-RRT可以在短時間內(nèi)得到最優(yōu)或近似最優(yōu)的路徑，符合實際工程使用的需求。通過測試結(jié)果可以看到，在文中測試的所有維度的狀態(tài)空間中，Obi-RRT算法運行時間僅是RRT*的十分之一，并且所產(chǎn)生的路徑成本波動很小。隨著維度增加，Obi-RRT的優(yōu)勢越明顯。