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

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

0 引言

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

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

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

1 機械臂運動規(guī)劃基本原理

1.1 運動規(guī)劃定義

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

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

(1)

1.2 碰撞檢測技術

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

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

2 改進RRT*-Connect算法

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

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

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

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

2.1.1目標偏向策略

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

(2)

(3)

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

InformedSample——橢球子集采樣

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

2.1.2橢球子集采樣

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

為了實現(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)隨機采樣點

L——變換矩陣

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

xf1、xf2——焦點

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

S≡LLT

(7)

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

(8)

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

(9)

通過分解運算得

(10)

為了實現(xiàn)超橢球體旋轉到世界坐標系，選取旋轉矩陣的計算方法為

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

(11)

x=FLxball+xcenter

(12)

最后，返回橢球內(nèi)的采樣點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é)點時，存在節(jié)點數(shù)無限增長的缺點，這在高維空間中將對內(nèi)存造成計算負擔。將采用搜索樹優(yōu)化剪枝策略限制搜索樹中的節(jié)點數(shù)，以降低運行時間。

本文所設計的REWIRE例程與原始的REWIRE不同，當樹中的節(jié)點數(shù)量大于閾值N時，需要檢驗每個重新布線的節(jié)點的原始父節(jié)點是否成為葉節(jié)點，如果成為葉節(jié)點，則從樹中移除。在重新布線過程之后，如果樹的大小仍然大于N，則執(zhí)行強制移除，即隨機移除一個葉節(jié)點。最后，如果重新布線和強制移除都不能使節(jié)點數(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為隨機樹中與xrand鄰節(jié)的點，xnew為重新布線生成新節(jié)點，xparent為父節(jié)點，cnew為新節(jié)點與父節(jié)點之間的距離，cnear為鄰節(jié)點與父節(jié)點之間的距離，E為路徑點的集合，T為隨機搜索樹。

2.3 自適應步長策略

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

(13)

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

(14)

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

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

(15)

(16)

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

(x2,y2)——Tree2節(jié)點

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

(17)

(18)

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

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

r——半徑

最終改進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分別為從起始點和目標點生成的隨機樹，xnearest1和xnearest2為隨機樹中與xrand的最近鄰節(jié)點，xnew1和xnew2為重新布線生成新節(jié)點，b為設定閾值。

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

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

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

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

(3)確定離Tree1/Tree2最近節(jié)點，通過自適應步長策略確定動態(tài)步長。

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

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

(6)當搜索樹中總節(jié)點數(shù)大于預設閾值時，通過搜索樹優(yōu)化剪枝策略對搜索樹進行優(yōu)化剪枝。

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

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

3 仿真與實驗

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

3.1 Matlab仿真對比

3.1.1場景1

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

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

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

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

3.1.2場景2

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

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

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

3.1.3場景3

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

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

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

3.1.4場景4

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

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

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

3.2 機械臂實驗驗證

3.2.1Matlab仿真

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

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

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

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

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

3.2.2ROS中機械臂規(guī)劃實驗

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

圖11 Sawyer機械臂實驗平臺Fig.11 Sawyer robot experiment platform

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

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

表4 實驗場景1中4種算法測試結果對比Tab.4 Test data comparison results of four algorithms in experiment scenario 1

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

表5 實驗場景2中4種算法測試結果對比Tab.5 Test data comparison results of four algorithms in experiment scenario 2

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

4 結束語

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