韓 康 程衛(wèi)東

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

0 引 言

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

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

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

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

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

1 Obi-RRT算法

1.1 問題描述與定義

文中所研究的狀態(tài)空間分別為二維平面和六維工業(yè)機(jī)械臂關(guān)節(jié)空間。平面空間的狀態(tài)節(jié)點(diǎn)定義為在直角坐標(biāo)系下點(diǎn)的坐標(biāo)所組成的向量，記為q2(x,y)。節(jié)點(diǎn)之間成本定義為坐標(biāo)系下兩點(diǎn)的直線距離。機(jī)械臂關(guān)節(jié)空間的狀態(tài)節(jié)點(diǎn)定義為機(jī)械臂某個(gè)姿態(tài)下關(guān)節(jié)角所組成的六維向量q6(θ1,θ2,θ3,θ4,θ5,θ6)，節(jié)點(diǎn)之間成本定義為它們關(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的成本達(dá)到最小。為此，算法構(gòu)建了兩棵隨機(jī)樹Ta=(Va，Ea)，Tb=(Vb，Eb)，其中Va，Vb∈Xfree是樹中狀態(tài)節(jié)點(diǎn)集合，Ea，Eb∈Xfree是連接兩個(gè)狀態(tài)節(jié)點(diǎn)邊的集合。

1.2 算法基本流程

Obi-RRT算法分為路徑搜索、路徑修剪和路徑優(yōu)化三個(gè)階段。路徑搜索階段使用改進(jìn)的RRT-Connect算法快速得到一條較低成本的路徑。再經(jīng)過路徑修剪階段，連接了路徑中可直接相連的節(jié)點(diǎn)來初步降低路徑成本，并大幅度降低路徑中節(jié)點(diǎn)數(shù)量，得到路徑關(guān)鍵點(diǎn)集keypoints。在路徑優(yōu)化階段，通過在關(guān)鍵點(diǎn)周圍采樣不斷更新關(guān)鍵點(diǎn)，產(chǎn)生更低成本的路徑，達(dá)到快速收斂的目的。算法主體過程如算法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(zhǎng)。不同的是在新算法中使用目標(biāo)偏置采樣和啟發(fā)式采樣來加速算法。

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

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

Cex=qnst_a.cost+qnst_b.cost+

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

(1)

當(dāng)Cex>Cmax時(shí)，qrand被拒絕，并重新進(jìn)行采樣。拒絕低質(zhì)量采樣點(diǎn)可以減少不必要的擴(kuò)展過程，提高了算法速度。算法采樣過程偽代碼如算法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)算法成功得到一條無碰撞路徑時(shí)，便進(jìn)入路徑修剪階段。該階段以三角不等式原理來優(yōu)化路徑。如圖1所示，c邊的長(zhǎng)度總是小于a邊和b邊的總和。

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

路徑修剪將連接路徑節(jié)點(diǎn)中可以直接相連的節(jié)點(diǎn)，刪掉中間節(jié)點(diǎn)，得到路徑關(guān)鍵點(diǎn)集keypoints。該點(diǎn)集中任意兩個(gè)非相鄰節(jié)點(diǎn)不可以直接相連。由此點(diǎn)集構(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é)點(diǎn)數(shù)量代表著此條路徑中需要轉(zhuǎn)折的次數(shù)，也一定程度上反映了該狀態(tài)空間的復(fù)雜程度。路徑關(guān)鍵點(diǎn)越多，狀態(tài)空間中障礙物分布越復(fù)雜。

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

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

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

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

路徑關(guān)鍵點(diǎn)一定意義上代表著路徑的轉(zhuǎn)折點(diǎn)，其給出了關(guān)于障礙物頂點(diǎn)或者邊緣的信息[9]。局部重采樣希望在已有路徑關(guān)鍵點(diǎn)周圍盡可能生成靠近障礙的節(jié)點(diǎn)，進(jìn)行關(guān)鍵點(diǎn)更新，從而實(shí)現(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)化的效率。對(duì)于上述所提出的三種采樣空間，采樣范圍大小一般是A大于C，B大于C。其中，A類和B類采樣空間的大小與連續(xù)三個(gè)節(jié)點(diǎn)的分布有關(guān)，其大小無法直接比較，不過隨著節(jié)點(diǎn)的不斷更新，A、B兩類采樣空間的大小在不斷減小。C類采樣空間需要人為指定半徑r。其設(shè)置得過大則失去了降低采樣范圍的意義，過小又會(huì)導(dǎo)致采樣次數(shù)的增加，兩者都不同程度上影響優(yōu)化速度，其取值與狀態(tài)空間障礙物的分布有關(guān)。這也是引入A類和B類采樣空間的原因，它們的大小完全取決于節(jié)點(diǎn)本身。

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

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

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

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

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

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

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

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

3 仿真與實(shí)驗(yàn)驗(yàn)證

3.1 二維平面空間實(shí)驗(yàn)結(jié)果

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

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

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

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

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

3.2 六維機(jī)械臂關(guān)節(jié)空間實(shí)驗(yàn)結(jié)果

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

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

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

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

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

4 結(jié) 語

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

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

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

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

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