王懷震 高 明 王建華 房立金 李洪生

(1.浪潮集團(tuán)山東新一代信息產(chǎn)業(yè)技術(shù)研究院有限公司，濟(jì)南 250101;2.東北大學(xué)機(jī)器人科學(xué)與工程學(xué)院，沈陽 110819)

0 引言

隨著機(jī)器人技術(shù)的不斷發(fā)展，機(jī)械臂在工業(yè)生產(chǎn)和農(nóng)業(yè)采摘領(lǐng)域已經(jīng)得到廣泛應(yīng)用。機(jī)械臂的避障運(yùn)動(dòng)規(guī)劃已經(jīng)成為機(jī)械臂應(yīng)用的前提。其主要目的是為了實(shí)現(xiàn)機(jī)械臂在復(fù)雜環(huán)境中，可以自主地進(jìn)行搜索并規(guī)劃出一條從起始位置到目標(biāo)位置的無碰撞路徑[1-3]。目前，機(jī)器人規(guī)劃方法主要有：①基于圖搜索的規(guī)劃方法，如Dijkstra算法和A*算法[4]。由于基于圖搜索的方法難以實(shí)現(xiàn)高維度環(huán)境的機(jī)械臂運(yùn)動(dòng)規(guī)劃，因此該類方法多數(shù)應(yīng)用于移動(dòng)機(jī)器人的路徑規(guī)劃。②基于采樣的規(guī)劃方法，如隨機(jī)路圖法(Probabilistic roadmap method,PRM)[5]和快速搜索隨機(jī)樹(Rapidly-exploring random trees，RRT)[6-7]。在運(yùn)用采樣方法進(jìn)行搜索路徑時(shí)，其不會(huì)存在隨環(huán)境維數(shù)和大小變化而導(dǎo)致的規(guī)劃效率低和成功率低等問題。RRT算法與PRM算法相比，其更適合解決高維空間中路徑搜索問題。因此，RRT算法普遍應(yīng)用于在機(jī)械臂的路徑規(guī)劃領(lǐng)域。

近年來，基于RRT算法的機(jī)械臂路徑規(guī)劃得到國內(nèi)外學(xué)者的廣泛關(guān)注和研究[8-12]。RRT算法是一種隨機(jī)采樣的搜索算法,通過擴(kuò)展固定搜索步長的隨機(jī)樹進(jìn)行路徑搜索。然而，傳統(tǒng)的RRT方法在生成新節(jié)點(diǎn)時(shí)，隨機(jī)擴(kuò)展策略往往會(huì)出現(xiàn)路徑規(guī)劃速度較慢和節(jié)點(diǎn)數(shù)量多的問題。因此，在實(shí)際應(yīng)用時(shí)，該算法存在諸多局限性。文獻(xiàn)[13]在傳統(tǒng)RRT算法的基礎(chǔ)上提出了一種雙向版本的RRT(RRT-Connect)方法。該方法可以在初始位置和目標(biāo)位置同時(shí)采樣并生成擴(kuò)展隨機(jī)樹，當(dāng)兩棵隨機(jī)樹之間建立連接時(shí)，算法停止搜索并輸出一條無碰撞的路徑。雖然RRT-Connect算法可以更高效地解決機(jī)械臂路徑規(guī)劃問題，但由于缺乏優(yōu)化過程，RRT和RRT-Connect皆無法提供路徑的最優(yōu)解。為了解決上述問題，文獻(xiàn)[14]提出了一種漸進(jìn)最優(yōu)RRT*算法。RRT*算法相比于RRT方法，改變了父節(jié)點(diǎn)的選取方式并增加了剪枝優(yōu)化過程。該算法在返回第一條路徑后，會(huì)繼續(xù)在環(huán)境中尋找最優(yōu)路徑，以生成一條漸進(jìn)最優(yōu)路徑。但是，RRT*算法在全局環(huán)境中仍然采用均勻采樣的方式，導(dǎo)致路徑規(guī)劃優(yōu)化效率較低。文獻(xiàn)[15]結(jié)合RRT-Connect和RRT*算法的研究思想，提出了一種RRT*-Connect算法。該算法能夠有效地降低單向RRT*方法在規(guī)劃過程的時(shí)間成本，但并沒有解決RRT*算法優(yōu)化效率較低的問題。為此，JONATHAN等[16]研究了一種更為先進(jìn)的Informed RRT*(IRRT*)算法。該方法通過設(shè)計(jì)橢球子集約束對采樣區(qū)域進(jìn)行空間配置，縮小了自由空間中節(jié)點(diǎn)的采樣范圍，從而保證路徑可以有效地收斂到全局最優(yōu)解[17]。然而，當(dāng)目標(biāo)位置位于狹窄通道或障礙物較為復(fù)雜的環(huán)境時(shí)，IRRT*算法存在適應(yīng)性較差和收斂精度較低的問題。值得注意的是，以上基于RRT的算法皆采用了固定步長的擴(kuò)展方式進(jìn)行路徑規(guī)劃，采樣搜索樹需要較長時(shí)間才能擴(kuò)展到目標(biāo)位置，這在一定程度上限制了算法的收斂速度[18]。

本文研究一種自適應(yīng)步長的啟發(fā)式RRT*-Connect機(jī)械臂運(yùn)動(dòng)規(guī)劃算法。引入目標(biāo)偏向策略進(jìn)行橢球子集約束采樣，使采樣點(diǎn)能夠更快地收斂到最優(yōu)值。在擴(kuò)展節(jié)點(diǎn)時(shí)，采用一種自適應(yīng)步長策略以加快本文算法的收斂速度，并有效縮短規(guī)劃路徑的長度。當(dāng)搜索樹中總節(jié)點(diǎn)達(dá)到預(yù)設(shè)值時(shí)，通過搜索樹優(yōu)化剪枝策略對搜索樹進(jìn)行剪枝，刪除無效的采樣點(diǎn)，進(jìn)一步降低運(yùn)行時(shí)間。并進(jìn)行仿真與實(shí)驗(yàn)驗(yàn)證該算法的優(yōu)勢和適應(yīng)能力。

1 機(jī)械臂運(yùn)動(dòng)規(guī)劃基本原理

1.1 運(yùn)動(dòng)規(guī)劃定義

設(shè)集合X?R，稱為運(yùn)動(dòng)規(guī)劃狀態(tài)空間。將Xobs?X的集合定義為障礙空間，Xfree=XXobs的集合稱為無障礙空間，即自由空間。在自由空間中定義xstart∈Xfree為起始位置，定義xgoal∈Xfree為目標(biāo)位置。運(yùn)動(dòng)規(guī)劃問題是在空間X中尋找一條從初始位置σ(0)=xstart開始，到達(dá)目標(biāo)位置σ(1)=xgoal的無碰撞路徑，將其定義為集合σ：[0,1]→Xfree[19]。

定義c(σ)為每條無碰撞路徑映射到一個(gè)非負(fù)實(shí)數(shù)的成本函數(shù)。最優(yōu)運(yùn)動(dòng)規(guī)劃問題就是尋找一條無碰撞最短路徑以解決運(yùn)動(dòng)規(guī)劃問題，設(shè)最低路徑成本函數(shù)為σ*，即

(1)

1.2 碰撞檢測技術(shù)

串聯(lián)機(jī)械臂本體是由多個(gè)關(guān)節(jié)構(gòu)成的鏈?zhǔn)浇Y(jié)構(gòu)。機(jī)械臂各關(guān)節(jié)的角度可以通過末端位置和方向進(jìn)行逆運(yùn)動(dòng)學(xué)求解。根據(jù)正運(yùn)動(dòng)學(xué)可求出各連桿坐標(biāo)系的齊次變換矩陣，進(jìn)而得到各環(huán)節(jié)的等效空間線方程。由于機(jī)械臂本體固定于底座，因此只需要對各個(gè)關(guān)節(jié)進(jìn)行檢測就可以判斷機(jī)械臂是否與障礙物發(fā)生碰撞。本文采用幾何包絡(luò)法[20]實(shí)現(xiàn)機(jī)械臂的碰撞檢測。為了簡化障礙物和機(jī)械臂模型，使用圓柱體來描述機(jī)械臂連桿，將不規(guī)則的障礙物采用球體、長方體進(jìn)行描述。這樣，機(jī)械臂的碰撞檢測問題就可以轉(zhuǎn)換為圓柱體、球體、長方體之間的碰撞檢測。碰撞檢測簡化模型如圖1所示。

圖1 碰撞檢測模型Fig.1 Collision detection model

2 改進(jìn)RRT*-Connect算法

KLEMM等[14]針對RRT的缺陷提出了更為高效的RRT*方法，并在RRT-Connect算法原理的基礎(chǔ)上，采用RRT*取代RRT，研究了RRT*-Connect算法。該算法雖然提高了RRT*的搜索效率，但在搜索過程中，由于算法在采樣時(shí)采樣點(diǎn)過于隨機(jī)，采樣點(diǎn)的分布過于分散，導(dǎo)致在復(fù)雜障礙物環(huán)境中仍然存在搜索效率和收斂精度較低的問題。

針對上述問題，提出了一種漸近最優(yōu)的RRT*-Connect規(guī)劃算法。采用一種新的啟發(fā)式采樣方法，對RRT*-Connect采樣方法進(jìn)行優(yōu)化。在擴(kuò)展節(jié)點(diǎn)時(shí)運(yùn)用搜索樹優(yōu)化剪枝策略和自適應(yīng)步長策略，進(jìn)一步提高了搜索效率。與常規(guī)的RRT*-Connect相比，可以用更少的迭代次數(shù)返回比當(dāng)前更優(yōu)的路徑。

2.1 啟發(fā)式采樣策略

常規(guī)的RRT*-Connect采樣方法存在采樣點(diǎn)過于隨機(jī)分散的問題，為了提高采樣效率，提出一種啟發(fā)式采樣方法，引入目標(biāo)偏向策略進(jìn)行橢球子集約束采樣，使采樣點(diǎn)能夠更快地收斂到最優(yōu)值。

2.1.1目標(biāo)偏向策略

設(shè)定一個(gè)閾值α，根據(jù)障礙物環(huán)境決定α。當(dāng)環(huán)境復(fù)雜且障礙物較多時(shí)，α設(shè)為較小值，反之α設(shè)為較大值。在采樣時(shí)以概率α向目標(biāo)方向采樣，以概率1-α在自由空間內(nèi)隨機(jī)采樣，這樣在保證了搜索目標(biāo)隨機(jī)性的同時(shí)提高了搜索效率。在搜索過程中，同時(shí)從起始點(diǎn)和目標(biāo)點(diǎn)開始采樣生成Tree1和Tree2。隨機(jī)樹的具體采樣方式為

(2)

(3)

式中P——隨機(jī)數(shù)，取0～1

InformedSample——橢球子集采樣

如式(2)、(3)所示，隨機(jī)樹每次在自由空間采樣時(shí)，按均勻概率會(huì)隨機(jī)產(chǎn)生一個(gè)概率P。如果P小于α，則采樣點(diǎn)xrand采用帶有目標(biāo)偏向采樣。這時(shí)，Tree1將目標(biāo)點(diǎn)作為目標(biāo)方向進(jìn)行采樣，而Tree2將起始點(diǎn)作為目標(biāo)方向進(jìn)行采樣。如果P大于α，則采樣點(diǎn)進(jìn)行橢球子集約束采樣。當(dāng)搜索到一條路徑方案之后，橢球子集半徑隨之縮小，使采樣點(diǎn)在不斷壓縮的橢球范圍里進(jìn)行采樣。因此，搜索路徑將不斷優(yōu)化，有效提高了規(guī)劃效率。

2.1.2橢球子集采樣

圖2 橢球子集采樣Fig.2 Ellipsoid subset sampling

為了實(shí)現(xiàn)橢球子集內(nèi)的均勻分布采樣，將均勻分布的樣本從半徑為n的球變換到橢球子集，xellipse～μ(Xellipse)，即

xellipse=Lxball+xcenter

(4)

其中

xcenter=(xf1+xf2)/2

(5)

xball={x∈X|；‖x‖2≤1}

(6)

式中xcenter——超橢球體的中心

xball——橢球內(nèi)隨機(jī)采樣點(diǎn)

L——變換矩陣

‖x‖2——x的二范數(shù)

xf1、xf2——焦點(diǎn)

該變換將通過超橢球矩陣S∈Rn×n的Cholesky分解來計(jì)算[21]，即

S≡LLT

(7)

(x-xcenter)TS(x-xcenter)=I

(8)

變換矩陣L使xellipse保持均勻分布。采樣點(diǎn)子集x的估計(jì)可以通過橫截半徑和橫軸來實(shí)現(xiàn)。橫軸對角矩陣為

(9)

通過分解運(yùn)算得

(10)

為了實(shí)現(xiàn)超橢球體旋轉(zhuǎn)到世界坐標(biāo)系，選取旋轉(zhuǎn)矩陣的計(jì)算方法為

F=Udiag{1,…,1,det(U)det(V)}VT

(11)

x=FLxball+xcenter

(12)

最后，返回橢球內(nèi)的采樣點(diǎn)xrand。橢球子集采樣算法偽代碼為

1:ifcbest<∞ then

2:cmin←‖xgoal-xstart‖2

3:xcenter←(xstart+xgoal)/2

4:c←RotationToWorldFrame(xstart,xgoal)

5:r1←cbest/2;

7:L←diag{r1,r2,…,rn}

8:xball←SampleUnitBall

9:xrand←(FLxball+xcenter)∩X

10:else

11:xrand～μ(X)

12:returnxrand

2.2 搜索樹優(yōu)化剪枝策略

常規(guī)的RRT*-Connect方法在搜索樹中的節(jié)點(diǎn)時(shí)，存在節(jié)點(diǎn)數(shù)無限增長的缺點(diǎn)，這在高維空間中將對內(nèi)存造成計(jì)算負(fù)擔(dān)。將采用搜索樹優(yōu)化剪枝策略限制搜索樹中的節(jié)點(diǎn)數(shù)，以降低運(yùn)行時(shí)間。

本文所設(shè)計(jì)的REWIRE例程與原始的REWIRE不同，當(dāng)樹中的節(jié)點(diǎn)數(shù)量大于閾值N時(shí)，需要檢驗(yàn)每個(gè)重新布線的節(jié)點(diǎn)的原始父節(jié)點(diǎn)是否成為葉節(jié)點(diǎn)，如果成為葉節(jié)點(diǎn)，則從樹中移除。在重新布線過程之后，如果樹的大小仍然大于N，則執(zhí)行強(qiáng)制移除，即隨機(jī)移除一個(gè)葉節(jié)點(diǎn)。最后，如果重新布線和強(qiáng)制移除都不能使節(jié)點(diǎn)數(shù)量低于N，則返回到采樣前的狀態(tài)。優(yōu)化剪枝算法偽代碼為

1:procedure REWIRE(T,Xnear,xnew)

2:for ?xnear∈Xneardo

3:cnear←COST(xnear)

4:cnew←COST(xnew)+MOTIONCOST(xnew,xnear)

5: ifcnew

6: if COLLISIONFREE(xnew,xnear)then

7:xparent← PARENT(xnear)

8: ifxnearhas no brothers∧M

9: REMOVENODE(xparent)

10:E←E{(xparent,xnear)}

11:E←E∪{(T,xnew,xnear)}

12:returnT

其中，xnear為隨機(jī)樹中與xrand鄰節(jié)的點(diǎn)，xnew為重新布線生成新節(jié)點(diǎn)，xparent為父節(jié)點(diǎn)，cnew為新節(jié)點(diǎn)與父節(jié)點(diǎn)之間的距離，cnear為鄰節(jié)點(diǎn)與父節(jié)點(diǎn)之間的距離，E為路徑點(diǎn)的集合，T為隨機(jī)搜索樹。

2.3 自適應(yīng)步長策略

在常規(guī)RRT*-Connect算法中搜索樹一般采用固定步長進(jìn)行擴(kuò)展，生成路徑轉(zhuǎn)折較多，路徑長度較長。本文提出一種自適應(yīng)步長RRT*-Connect方法。當(dāng)Tree1通過隨機(jī)采樣生成隨機(jī)節(jié)點(diǎn)xrand1后，判斷得到最近的鄰節(jié)點(diǎn)xnearest1，以步長d向xrand1方向擴(kuò)展，即

(13)

生成新節(jié)點(diǎn)xnew1點(diǎn)后，判斷得到xnew1距離Tree2最近的鄰節(jié)點(diǎn)xnearest2，然后以步長d向xnew1方向擴(kuò)展，即

(14)

同理，Tree2采用同樣的方式進(jìn)行擴(kuò)展。這里，步長d并非固定值，擴(kuò)展的步長由機(jī)械臂和障礙物環(huán)境決定。其設(shè)計(jì)方法如下：

設(shè)定參數(shù)：小步長d1、大步長d2，設(shè)定閾值b1和b2。當(dāng)距離Dtree小于閾值b1時(shí)，采用小步長進(jìn)行擴(kuò)展，這樣可以增加搜索樹之間的連接成功率。當(dāng)兩樹之間的距離Dtree大于等于閾值b1時(shí)，則采用大步長d2進(jìn)行擴(kuò)展。自適應(yīng)步長優(yōu)化方法為

(15)

(16)

式中 (x1,y1)——Tree1節(jié)點(diǎn)

(x2,y2)——Tree2節(jié)點(diǎn)

如果機(jī)械臂與障礙物距離Dobs大于等于閾值b2，判定機(jī)械臂附近是自由空間沒有障礙物，則采用大步長d2進(jìn)行擴(kuò)展。反之如果機(jī)械臂與障礙物距離小于設(shè)定閾值b2時(shí)，認(rèn)為機(jī)械臂附近存在障礙物環(huán)境，采用小步長d1進(jìn)行擴(kuò)展。這樣既能保證機(jī)械臂的避障效果，又能提高規(guī)劃路徑的效率。

(17)

(18)

式中 (x,y)——Tree節(jié)點(diǎn)

(x0,y0)——障礙物圓心

r——半徑

最終改進(jìn)RRT*-Connect算法偽代碼為

1:T1←{xstart};T2←{xgoal}

2:fori=1 toNdo

3: ifP<αthen

4:xrand1←xgoal

5:xrand2←xstart

6: else

7:xrand←InformedSample

8:xnearest←NEAREST(T,xrand)

9: ifDobs

10:d=d1

11: else

12:d=d2

13:xnew1←steer(xnearest,xrand,d)

14:xnew2←steer(xnearest,xnew1,d)

15: if ObstacleFree(xnew1)then

16:Xnear←NEAR(xnew1)

17: CHOOSEPARENT(Xnear,xnew1)

18: REWIRE(Xnear,xnew1)

19: FORCEREMOVAL(T1,xnew1)

20: ifDtree

21: if ObstacleFree(xnew2)then

22:Xnear←NEAR(xnew2)

23: CHOOSEPARENT(xnear,xnew2)

24: REWIRE(xnear,xnew2)

25: FORCEREMOVAL(T2,xnew2)

26: ifDtree

27:path=CONNECT(T1,T2)

28:return path

其中，T1、T2分別為從起始點(diǎn)和目標(biāo)點(diǎn)生成的隨機(jī)樹，xnearest1和xnearest2為隨機(jī)樹中與xrand的最近鄰節(jié)點(diǎn)，xnew1和xnew2為重新布線生成新節(jié)點(diǎn)，b為設(shè)定閾值。

為了更好地呈現(xiàn)本文方法的運(yùn)行過程，文中給出改進(jìn)RRT*-Connect算法的流程圖，如圖3所示。本文算法的步驟為：

圖3 算法流程圖Fig.3 Algorithm flow chart

(1)初始化地圖，設(shè)置起始位置和終止位置、啟發(fā)率、采樣節(jié)點(diǎn)總數(shù)、動(dòng)態(tài)步長等參數(shù)。

(2)引入目標(biāo)偏向策略，Tree1以起始位置為起點(diǎn),以終止位置為目標(biāo)(Tree2以終止位置為起點(diǎn),以起始位置為目標(biāo)),進(jìn)行橢球子集約束生成隨機(jī)采樣點(diǎn)進(jìn)行啟發(fā)式采樣。

(3)確定離Tree1/Tree2最近節(jié)點(diǎn)，通過自適應(yīng)步長策略確定動(dòng)態(tài)步長。

(4)生成Tree1/Tree2新節(jié)點(diǎn)，并通過碰撞檢測判斷兩樹是否滿足連接，能連接則跳至步驟(7)，不能連接則進(jìn)行步驟(5)；未通過碰撞檢測則返回步驟(2)重新進(jìn)行采樣。

(5)對新的節(jié)點(diǎn)選擇父節(jié)點(diǎn)并重新布線操作。

(6)當(dāng)搜索樹中總節(jié)點(diǎn)數(shù)大于預(yù)設(shè)閾值時(shí)，通過搜索樹優(yōu)化剪枝策略對搜索樹進(jìn)行優(yōu)化剪枝。

(7)判斷兩樹距離是否小于一個(gè)步長。如果小于一個(gè)步長則通過連接程序?qū)蓸溥B接起來，輸出路徑信息path。反之則不能連接返回步驟(2)繼續(xù)進(jìn)行搜索。

(8)算法迭代完成生成最終的規(guī)劃路徑，規(guī)劃任務(wù)結(jié)束。

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

為了驗(yàn)證本文改進(jìn)算法的優(yōu)勢和有效性，通過Matlab仿真平臺對算法進(jìn)行驗(yàn)證并分別與RRT*[13]、RRT*-Connect[14]、IRRT*[15]算法在多場景下的規(guī)劃結(jié)果進(jìn)行數(shù)據(jù)對比。為了更好地驗(yàn)證本文規(guī)劃算法的實(shí)用性，在Matlab和ROS平臺搭建機(jī)械臂實(shí)驗(yàn)環(huán)境，實(shí)現(xiàn)本文算法在不同應(yīng)用場景的機(jī)械臂運(yùn)動(dòng)規(guī)劃。

3.1 Matlab仿真對比

3.1.1場景1

設(shè)置650 mm×650 mm環(huán)境場景1，設(shè)(20 mm,20 mm)為起始位置，(630 mm，630 mm)為目標(biāo)位置，設(shè)置啟發(fā)式概率α=0.15，最小步長為5 mm，最大步長為15 mm，設(shè)置固定的最大節(jié)點(diǎn)數(shù)為1 000，最大的迭代次數(shù)為5 000。4種算法的仿真結(jié)果如圖4所示。進(jìn)行100組重復(fù)實(shí)驗(yàn)，4種算法測試實(shí)驗(yàn)結(jié)果見表1。

圖4 場景1中不同算法的規(guī)劃結(jié)果Fig.4 Planning results of different algorithms in scenario 1

由圖4可以看出，RRT*-Connect比本文算法的搜索過程更為復(fù)雜且路徑明顯更長。IRRT*同樣采用橢圓子集進(jìn)行采樣，但搜索樹包含節(jié)點(diǎn)更多。對比表1中的測試結(jié)果,可得本文算法在相同迭代次數(shù)下，具有更短的路徑長度、更快的收斂速度且成功率達(dá)到98%。

表1 場景1中4種算法測試結(jié)果對比Tab.1 Test comparison results of four algorithms in scenario 1

3.1.2場景2

相較于場景1，場景2在同樣的起始位置與目標(biāo)位置間存在一條狹窄通道，其設(shè)計(jì)目的是為了驗(yàn)證各個(gè)算法在狹窄通道內(nèi)的路徑規(guī)劃能力。4種算法的路徑規(guī)劃結(jié)果如圖5所示。進(jìn)行100組重復(fù)實(shí)驗(yàn)，4種算法測試實(shí)驗(yàn)結(jié)果見表2。

由圖5和表2可以看出，在狹窄通道場景下，RRT*-Connect比RRT*和IRRT*的搜索路徑成本較低，搜索時(shí)間更短，而RRT*的搜索成功率最低，充分表明了雙向搜索RRT*的優(yōu)勢。與RRT*-Connect相比較，本文算法的路徑長度更短，體現(xiàn)出自適應(yīng)步長在狹窄通道內(nèi)搜索的優(yōu)勢。從全局?jǐn)?shù)據(jù)來看，本文算法在狹窄通道內(nèi)規(guī)劃路徑更短、效率和成功率更高。

表2 場景2中4種算法測試結(jié)果對比Tab.2 Test comparison results of four algorithms in scenario 2

3.1.3場景3

相較于場景1和場景2，場景3的環(huán)境設(shè)置相對復(fù)雜，目的是為了驗(yàn)證算法對復(fù)雜環(huán)境的適應(yīng)能力。本文算法的實(shí)驗(yàn)結(jié)果如圖6所示。進(jìn)行100組重復(fù)實(shí)驗(yàn)，4種算法測試實(shí)驗(yàn)結(jié)果見表3。

表3 場景3中4種算法測試結(jié)果對比Tab.3 Test comparison results of four algorithms in scenario 3

由圖6可得，本文算法在復(fù)雜環(huán)境內(nèi)同樣具備較強(qiáng)的適應(yīng)能力。通過對比測試數(shù)據(jù)可以看出，本文算法在相同仿真條件下，實(shí)現(xiàn)路徑長度更短，平均運(yùn)行時(shí)間更優(yōu)，提高了路徑搜索的成功率。

3.1.4場景4

為了驗(yàn)證本文算法在三維場景中的有效性，利用Matlab在三維空間內(nèi)進(jìn)行了規(guī)劃算法對比實(shí)驗(yàn)，結(jié)果如圖7所示。

圖7 三維環(huán)境規(guī)劃結(jié)果Fig.7 3D environmental planning results

由圖7可以看出，本文算法在三維環(huán)境下的搜索樹包含節(jié)點(diǎn)更少，規(guī)劃路徑更短且相對平滑，有效地證明了本文算法的優(yōu)越性。

3.2 機(jī)械臂實(shí)驗(yàn)驗(yàn)證

3.2.1Matlab仿真

為了驗(yàn)證本文算法在機(jī)械臂中的有效性，在Matlab中建立了7自由度平面機(jī)械臂模型，并將本文算法應(yīng)用于機(jī)械臂運(yùn)動(dòng)規(guī)劃仿真實(shí)驗(yàn)。圖8～10為不同場景下，機(jī)械臂的初始狀態(tài)、結(jié)束狀態(tài)和規(guī)劃結(jié)果。

圖8 不同場景機(jī)械臂初始狀態(tài)Fig.8 Initial state of robot in different scenarios

圖9 不同場景機(jī)械臂結(jié)束狀態(tài)Fig.9 End state of robot in different scenarios

由圖10可以看出，機(jī)械臂在不同場景中，由起始點(diǎn)向目標(biāo)點(diǎn)不斷擴(kuò)展節(jié)點(diǎn)進(jìn)行搜索，運(yùn)行軌跡較為平滑，能夠有效地避開障礙物，完成指定任務(wù)。

圖10 不同場景機(jī)械臂規(guī)劃結(jié)果Fig.10 Robot planning results in different scenarios

3.2.2ROS中機(jī)械臂規(guī)劃實(shí)驗(yàn)

Sawyer機(jī)械臂是Rethink Robotics公司推出的7自由度智能協(xié)作機(jī)器人，能夠較好地適應(yīng)狹窄和稠密環(huán)境的作業(yè)任務(wù)?；贚inux系統(tǒng)對Sawyer機(jī)械臂進(jìn)行仿真環(huán)境配置，利用ROS(Robot operating system)中的MoveIt!控制Sawyer機(jī)械臂進(jìn)行避障運(yùn)動(dòng)規(guī)劃，將本文算法在Sawyer機(jī)械臂上進(jìn)行驗(yàn)證。Sawyer機(jī)械臂是7自由度協(xié)作機(jī)器人，每個(gè)關(guān)節(jié)可以在位置模式、速度模式和力矩模式下進(jìn)行控制，其質(zhì)量為19 kg，臂展為1 260 mm，有效載荷為4 kg，重復(fù)定位精度為±0.1 mm，Sawyer機(jī)械臂實(shí)驗(yàn)平臺如圖11所示。

圖11 Sawyer機(jī)械臂實(shí)驗(yàn)平臺Fig.11 Sawyer robot experiment platform

在Sawyer機(jī)械臂實(shí)驗(yàn)平臺設(shè)置障礙物的實(shí)驗(yàn)場景，其中擺放長方體盒子為障礙物，左側(cè)平板位置為起始點(diǎn)，將其移動(dòng)到障礙物另一側(cè)的目標(biāo)位置方塊處。實(shí)驗(yàn)分別與前文中RRT*、RRT*-Connect、IRRT*算法在多場景下的規(guī)劃結(jié)果進(jìn)行數(shù)據(jù)對比與分析，每種算法進(jìn)行100 次重復(fù)實(shí)驗(yàn)。

由表4、5可以得出，RRT*-Connect算法比另外2種已有算法搜索軌跡成本低，能夠有效縮短軌跡長度，但成功率低于IRRT*算法。通過對比全局實(shí)驗(yàn)數(shù)據(jù)可以看出，本文算法在不同的實(shí)驗(yàn)場景中可以實(shí)現(xiàn)軌跡長度更短，平均運(yùn)行時(shí)間更優(yōu)，提高了軌跡搜索的成功率。實(shí)驗(yàn)結(jié)果表明，本文算法在不同障礙環(huán)境下具有較強(qiáng)的適應(yīng)能力。

表4 實(shí)驗(yàn)場景1中4種算法測試結(jié)果對比Tab.4 Test data comparison results of four algorithms in experiment scenario 1

由圖12可以看出，機(jī)械臂運(yùn)行過程平穩(wěn)且流暢，說明了本文算法能夠在真實(shí)機(jī)械臂上較好地實(shí)現(xiàn)避障運(yùn)動(dòng)規(guī)劃，完成稠密環(huán)境的作業(yè)任務(wù)。實(shí)驗(yàn)結(jié)果證明了本文算法的有效性。

表5 實(shí)驗(yàn)場景2中4種算法測試結(jié)果對比Tab.5 Test data comparison results of four algorithms in experiment scenario 2

圖12 本文算法ROS中機(jī)械臂的規(guī)劃結(jié)果Fig.12 Planning results of proposed method for robot in ROS

4 結(jié)束語

提出了一種采用自適應(yīng)步長的啟發(fā)式RRT*-Connect機(jī)械臂運(yùn)動(dòng)規(guī)劃算法。在迭代過程中，引入目標(biāo)偏向策略進(jìn)行橢球子集約束采樣，使采樣點(diǎn)能夠更快地收斂到最優(yōu)值。在擴(kuò)展節(jié)點(diǎn)時(shí)，設(shè)計(jì)了一種自適應(yīng)步長策略以加快本文算法的收斂速度，有效地縮短規(guī)劃路徑的長度。同時(shí)，在擴(kuò)展節(jié)點(diǎn)過程，配置樹中總節(jié)點(diǎn)數(shù)的預(yù)設(shè)值，采用搜索樹優(yōu)化剪枝策略對搜索樹進(jìn)行剪枝，從而降低規(guī)劃的運(yùn)行時(shí)間。在多場景下進(jìn)行仿真與實(shí)驗(yàn)，并與RRT*、RRT*-Connect和IRRT*算法進(jìn)行了對比，實(shí)驗(yàn)結(jié)果充分驗(yàn)證了本文算法的有效性和實(shí)用性。