基于改進RRT*FN算法的機械臂多場景運動規劃

房立金 吳政翰 王懷震

東北大學機器人科學與工程學院，沈陽,110000

0 引言

隨著機械臂的普及，機械臂在復雜環境中的運動規劃變得尤為重要。機械臂運動規劃的主要目的是在連續空間中尋找一條機械臂末端從起始位置移動到目標位置的無碰撞路徑。目前，機械臂通常采用基于采樣的規劃方法進行運動規劃。應用最廣泛的基于采樣的運動規劃算法是概率路線圖(probabilistic roadmap method, PRM)和快速擴展隨機樹(rapidly-exploring random trees, RRT)[1]。

與PRM算法相比，RRT算法更適用于解決高維空間中運動規劃問題，因此RRT算法在機械臂的運動規劃領域應用較為廣泛[2-3]。RRT算法是在起始位置構建一棵樹，通過向隨機樣本生長樹來探索狀態空間，當得到一條無碰撞路徑并到達目標位置時即停止。然而，由于RRT算法是隨機抽樣過程，在目標區域中發現一個樣本通常需要大量時間。針對這一問題，文獻[4]提出了RRT-Connect算法。與RRT相比，RRT-Connect算法用兩棵樹搜索狀態空間，從而提高了搜索效率，特別是當目標位置難以通過單向搜索進行采樣時效果更加明顯。雖然該算法可以有效解決運動規劃問題，但無法提供最優解。這是因為用隨機采樣探索狀態空間輸出的是一系列隨機樣本，沒有任何優化樹的過程，沒有考慮初始狀態和其他節點之間的運動最優性[5]。

為了解決上述問題，文獻[6]提出漸近最優RRT(RRT*)算法。通過執行局部優化來擴展RRT算法，從而改進全局運動規劃問題。在找到規劃路徑后，RRT*算法將繼續探索狀態空間，通過不斷采樣以優化當前規劃路徑。然而，為了實現漸進優化，RRT*算法需要進行大量的迭代過程，導致搜索效率下降。文獻[7]提出Informed RRT*算法，通過將搜索區域限制為狀態空間的子集來解決RRT*算法存在的缺陷，相較于傳統的RRT*算法，Informed RRT*算法能夠更快地返回接近最優解。然而，該方法無法限制樹的規模[8]。文獻[9]提出RRT*FN算法，該算法限制了樹中節點的最大值，并移除節點以在其增量采樣過程中添加新節點，雖然避免了樹的規模無限增長，但收斂精度較低[10]。機器人運動規劃過程中，通常將環境中所有障礙物視為靜態。然而，在現實場景中，環境中的障礙物并非總是靜止或工作的目標點并非固定不變，在動態環境下，上述運動規劃算法不能保證良好的適應性。文獻[11]提出RRT*FND算法，該算法改善了對動態環境的適應性，但依然存在搜索狹窄通道能力較差、收斂精度較低的問題。

針對上述問題，本文提出一種改進RRT*FN的機械臂運動規劃算法。在迭代過程中，采用帶有目標偏向性和橢球子集采樣的啟發式方法對采樣區域進行約束采樣，使采樣點能夠更快地收斂到最優值。在擴展節點時，配置樹中總節點數的預設值，并通過加權方法對樹中葉子節點進行刪減，有效避免樹的規模無限增長。采用對節點剪枝與連接的啟發式重規劃方法，使算法更適應動態環境。最后，通過仿真實驗進行了驗證。

1 相關工作

1.1 運動規劃問題定義

本文采用文獻[12]的方法對運動規劃問題進行定義。設X為狀態空間，與障礙物發生碰撞的空間定義為Xobs?X，與障礙物不發生任何碰撞的空間定義為Xfree即自由空間。設起始位置為xstart∈Xfree，目標位置為xgoal∈Xfree。因此，起始位置和目標區域之間的路徑將被定義為一個集合{σ(0)，σ(1)}?Xfree，其中σ(0)=xstart和σ(1)=xgoal。

定義路徑成本為c:Xfree→R，R表示非負實數集合。尋找路徑成本較低的路徑是最優路徑規劃的目標，定義最低路徑成本函數為σ*：

(1)

Xf是狀態空間的一個子集，Xf?X，它可以提供比現有解決方案更好的方案，設cbest為當前最短路徑的成本，則

Xf={x∈X∣f(x)

(2)

如果規劃時將搜索區域限制為Xf，則其狀態比整個狀態空間少，將可以更快地返回接近最優的解。

1.2 碰撞檢測

機械臂的碰撞檢測包括對機械臂末端進行碰撞檢測，以及對機械臂各個關節與連桿進行碰撞檢測。碰撞檢測通常通過空間幾何包絡法簡化障礙物和機械臂模型[13]。在現實環境中，障礙物的形狀一般都是不規則的，很難精確建模。本文使用圓柱體描述機械臂連桿，機械臂的碰撞檢測問題轉換為圓柱體、球體、長方體之間的碰撞檢測。

如圖1所示，設置球體中心的坐標為(x，y，z)。其中圓柱體是構型空間中機械臂的簡化形式，Ai、Bi是連桿i的兩端，半徑為ωi。針對球形包圍盒的碰撞檢測，球體是構型空間中障礙物的簡化形式，半徑為r；球體中心到圓柱體中心軸的距離為di。當di>r+ωi時，機械臂不與障礙物碰撞，反之則發生碰撞。針對軸向包圍盒的碰撞檢測，若線段AiBi在障礙物的軸向包圍盒外，則機械臂不與障礙物發生碰撞，反之則發生碰撞。

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

2 改進RRT*FN算法

2.1 基本RRT*FN算法

ADIYATOV等[9]基于RRT*算法[6]提出了一種RRT*FN算法，該算法中采樣、擴展節點、選擇父節點的方式均與RRT*算法相同。

首先，從無障礙區域隨機抽取xrand節點，定位到xrand的最近節點，并將其表示為xnear。然后，通過將xnear的距離擴展到xrand獲得新節點xnew，并且找到與xnew的距離小于采樣半徑r的節點形成集合Xnear。從Xnear中選擇節點并進行擴展，從而最小化xnew的累積成本，如圖2所示。最后，執行重剪枝程序。重剪枝程序偽代碼如下：

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

圖2 擴展節點示意圖Fig.2 Schematic diagram of expansion node

對于Xnear中的每個節點，確定將其父節點設置為xnew是否會降低其累積成本，如果是，則修改節點。RRT*FN算法的重剪枝不同于RRT*算法的重剪枝，因為當樹中的節點數大于閾值M時，需要檢查每個重新連接的節點的原始父節點是否成為葉節點，如果它成為葉節點，則將其從樹中移除。在重剪枝之后，如果樹的節點數量T仍然大于M，則隨機移除葉節點。

基本的RRT*FN算法是在傳統的RRT*算法基礎上對樹中的最大節點數進行預設，當節點數大于預設值時，樹中的葉子節點將被隨機刪除，使得算法只使用一定內存空間來完成運動規劃任務。然而，RRT*FN算法依然存在收斂精度和搜索效率較低的問題。

2.2 改進RRT*FN算法

針對上述RRT*FN算法存在的問題，本文提出了改進的RRT*FN算法。首先，本文對采樣方法進行改進，將啟發式采樣方法加入RRT*FN中，然后采用啟發式重規劃方法來提高算法對環境的適應性。

改進的RRT*FN算法偽代碼如下：

1:V←{xinit};E←?;T←T∪xinit

2:fori=1 toNdo

3: ifT.size+1>Mthen

4:T0←T

5: ifP<αthen

6:xrand←SAMPLEFREE()

7: else

8:xrand←InformedSample()

9:xnearest←NEAREST(T,xrand)

10:xnew←steer(xnearest,xrand,d)

11: if ObstacleFree(xnew) then

12:xnear←NEAR(xnew)

13: CHOOSEPARENT(xnear,xnew)

14: REWIRE(xnear,xnew)

15: FORCEREMOVAL(T,xnew)

16: if distance(xnew-xgoal)

17: ifM

18:T←T0

19: path=CONNECT(T,xgoal)

20: if environmental change then

21: Replanning()

22:return path

首先，對算法進行初始化，確定初始位置與目標位置，并建立障礙環境。其次，進行采樣過程，對于xrand采用帶有目標偏向的啟發式隨機采樣：

(3)

(4)

其中，P為一個隨機數，P∈[0,1];α為自定義常數;InformedSample()表示橢球子集采樣；d為擴展步長。

采樣時，設定一個基準值α，以概率α向目標點方向采樣，概率1-α在自由空間內隨機采樣，保證了搜索目標的隨機性和快速性。α的設置根據障礙物確定，當障礙物較少時，α設為較大值；障礙物較多時，α設為較小值。隨機樹每次在自由空間中采樣，按均勻概率分配隨機產生一個概率值P。如果P小于原先設定的基準值，則對采樣點xrand采用帶有目標偏向的隨機采樣。如果P大于原先設定的基準值，則采樣點進行橢球子集約束采樣。

完成搜索最近鄰近點、擴展新節點等操作后，在障礙環境中規劃出機械臂的一條可行路徑，機械臂開始按照規劃路徑運動，環境變化則進行重規劃操作。最后，機械臂末端運動到指定位置，運動規劃任務完成。改進RRT*FN算法流程如圖3所示。

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

2.3 橢球子集采樣

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

此采樣過程需要將樣本xellipse均勻地分布在子集Xellipse內。通過將樣本均勻地分布在一個單位半徑為n的球上，然后將xball轉移到橢球子集Xball來實現[14]：

xellipse=Lxball+xcenter

(5)

xcenter=(xf1+xf2)/2

(6)

Xball={x∈X∣‖x‖2≤1}

(7)

其中，xf1和xf2為橢球子集的焦點；‖x‖2為x的二范數，利用超橢球矩陣S∈Rn×n的Cholesky分解得到變換：

LLT≡S

(8)

(x-xcenter)TS(x-xcenter)=1

(9)

(10)

經過分解運算得到

(11)

將樣本從一個半徑為n的球轉換為一個n維橢球后，必須旋轉到世界坐標系。根據解決Wahba問題的思想[15]，旋轉矩陣為

C=Udiag(1,…,1,det(U)det(V))VT

(12)

其中det()為矩陣行列式，U∈Rn×n、V∈Rn×n通過奇異值分解UΣVT≡M的酉矩陣得出，Σ是主對角線上元素絕對值為1的對角陣，M通過恒等矩陣的第一列的外積和世界坐標系上的橫軸長度a1計算得到：

M=a1I

(13)

a1=(xgoal-xstart)/‖xgoal-xstart‖2

(14)

其中，I是單位矩陣。

通過下式得到子集：

(15)

橢球子集采樣偽代碼如下：

1:ifcbest<∞ then

2:cmin←‖xgoal-xstart‖2

3:ccenter←(xstart+xgoal)/2

4:c←RotationToWorldFrame(xstart,xgoal)

5:r1←cbest/2

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

8:xball← SampleUnitBall()

9:xrand←(CLxball+xcenter)∩x

10:else

11:xrand～U(x)

12:returnxrand

RotationToWorldFrame()函數生成一個從橢球坐標系到世界坐標系的旋轉矩陣，SampleUnitNBall()函數在以原點為中心的單位半徑的球內進行均勻采樣，最后將xrand映射到橢球內。

2.4 重規劃

在找到目標點后，由于障礙物和目標的位置可能會變化，導致規劃好的路徑不再適用，所以關鍵問題是既要使用之前規劃生成的樹，又要平衡使用上次規劃的樹和在此規劃中取樣的新點。針對這些問題，需要對路徑進行重新搜索和優化。

對于移動或未知障礙物的路徑運動問題，傳統重規劃的方法是更新環境模型，并在所有障礙物都是靜態的情況下重新運行一個新的運動規劃過程。該方法將丟棄舊樹，并丟失算法運行時生成的有價值信息。丟失的信息包括障礙物碰撞檢測例程的結果和通過樹搜索配置空間的結果。本文采用對節點剪枝與連接的啟發式重規劃方法，能保留舊樹中的重要信息并盡可能重復利用。

在得到初始解后，若環境變化則執行重規劃例程，其偽代碼如下：

1:τ←ImprovedRRT*FN(xinit)

2:xcur←xstart

3:σ← SolutionPath(,xcur)

4:InitMovement()

5:whilexcur!=xgoaldo

6:D← UpdateObtacles()

7: if DetectCollision(σ,xcur) then

8: StopMovement()

9:τ← SelectBranch(xcur,τ)

10:xsep← ValidPath(σ)

11: ReconnectFailed ← true

12:xnear← Near(,xsep)

13: forxnear∈xneardo

14: if ObstacleFree(xnear,xsep) then

15:τ←Reconnect(xnear,xsep,τ)

16: ReconnectFailed← false

17: break

18: if Reconnect Failed = true then

19:τ←Regrow(τ,xsep)

20: SetBias(σsep)

21:σ← SolutionPath(τ,xcur)

22: ResumeMovement()

23:path=CONNECT(τ,xgoal)

24:return path

規劃路徑σ(實現從xinit開始的路徑節點的鏈表)將得到進一步優化，并更好地探索配置空間。然后，機器人開始移動。一旦在σ上到達新的節點xcur，障礙物信息將會更新。如果在xcur和xgoal之間的任何路徑段中檢測到障礙物碰撞，則機器人停止移動。執行SelectBranch和ValidPath例程，SelectBranch通過移除舊節點從xinit到xcur創建新節點。ValidPath檢查先前規劃的路徑，移除與新障礙物及其碰撞的所有節點，保留連接到xgoal的規劃路徑的可用部分。這部分稱為σsep，從節點xsep開始，到目標節點xgoal結束。

第二階段包括兩個例程Reconnect和Regrow。首先，Reconnect查找σsep中每個節點的附近節點，并判斷舊樹中的任意節點是否可以連接到這些節點的其中一個。如果存在這樣的節點，則舊樹重新連接到此路徑節點，并刪除σsep中所有前面的路徑節點。如果Reconnect中找不到規劃方案，則執行Regrow。在Regrow中，主樹會一直生長，直到xsep中的任何節點都可以重新連接到主樹為止。找到規劃路徑后，將刪除未連接到主樹上的節點。最后，根據規劃方案生成一條新的無碰撞路徑，重規劃完成。

3 仿真與實驗分析

為了驗證本文算法的有效性和可靠性，進行了仿真和實驗分析，并與RRT*、RRT*FN、Informed RRT*算法在多場景下進行對比。

3.1 仿真分析

3.1.1二維仿真

設置650×650的場景1，將起點設為(50,50)，目標點設為(600，600)，設置啟發式概率α=0.15，步長為5，令固定的最大節點數為1000，最大迭代次數為5000。

如圖5所示，綠線部分為搜索形成的隨機樹，紅線部分為規劃的路徑。通過對比分析搜索時間與路徑長度的差異，即可驗證改進算法的性能。

(a)RRT* (b)RRT*FN

(c)Informed RRT* (d)本文算法圖5 場景1中算法測試結果Fig.5 Algorithm test results in scenario 1

經過100組重復實驗，實驗結果見表1。通過對比可以看出，四種算法在場景1中具有較高的成功率。RRT*FN與本文算法的規劃時間較短。為了驗證不同的迭代次數對算法路徑收斂結果的影響，在場景1中進行測試對比，結果如圖6所示。相較于其他三種算法,本文算法具備更短的路徑長度和更快的收斂速度。

表1 場景1中算法性能對比Tab.1 Performance comparison of the algorithms in scenario 1

圖6 場景1中算法路徑收斂結果Fig.6 Path convergence results of the algorithms in scenario 1

相較于場景1，場景2的起點與終點間存在一條狹窄通道，用來驗證算法對狹窄通道的搜索能力。將起點設為(50,50)，目標點設為(600，600)，設置啟發式概率α=0.15，步長為5，最大迭代次數為5000。四種算法的實驗結果如圖7和表2所示。通過對比可以看出，Informed RRT*和本文算法在狹窄通道場景下具有較高的成功率；相較于Informed RRT*算法，本文算法規劃路徑更短、規劃速度更快。

(a)RRT* (b)RRT*FN

(c)Informed RRT* (d)本文算法圖7 場景2中算法測試結果Fig.7 Algorithm test results in scenario 2

表2 場景2中算法性能對比Tab.2 Performance comparison of the algorithms in scenario 2

相較于場景1和場景2，場景3的環境更為復雜，用來驗證算法對動態環境的適應能力。最初將起點設為(50,50)，目標點設為(600，600)，設置啟發式概率α=0.15，步長為5。本文算法的實驗結果如圖8所示。通過對比圖8a、圖8b可以看出，當目標點發生改變時，本文算法可以通過重規劃進行搜索，最終規劃出一條新的無碰撞路徑。圖8c、圖8d中環境發生變化，阻擋了原有規劃路線。對比發現，當環境發生改變時算法可以重新規劃一條新的無碰撞路徑，成功避開障礙物。對比場景3的規劃結果得出，本文算法在重規劃時，舊樹并沒有被完全刪除，而是通過舊節點的重新采樣和擴展來進行搜索，這樣大大提高了搜索效率和收斂速度。

(a)初始位置 (b)目標點變化

(c)初始環境 (d)環境變化圖8 場景3中本文算法在動態環境的測試結果Fig.8 Test results of the algorithm in this paper in scenario 3 with dynamic environment

3.1.2三維仿真

為了驗證本文算法在三維場景中的有效性，搭建了三維空間避障環境，并對規劃算法和重規劃算法進行了仿真對比驗證，如圖9所示。

對比圖9a、圖9b可以看出，RRT*算法的搜索樹幾乎布滿了整個空間，而本文算法以概率α向目標點進行采樣，簡化了搜索過程，軌跡較為光滑，有效提高了軌跡規劃效率。如圖9c、圖9d所示，當障礙物大小發生變化后，原規劃路徑無法實現避障任務，本文設計的重規劃算法將刪減部分原有搜索樹并搜索新樹，重新規劃出一條無障礙路徑，證明本文算法能較好地適應動態環境并完成避障任務。

(a)RRT* (b)本文算法

(c)初始環境 (d)環境變化圖9 三維環境規劃結果Fig.9 3D environmental planning results

為了進一步驗證本文算法在虛擬環境中的有效性，建立了七自由度機械臂模型,并用本文算法對其進行了運動規劃仿真實驗。如圖10所示，實驗結果表明本文算法可以有效應用于虛擬環境。

(a)虛擬環境 (b)規劃結果圖10 虛擬環境規劃結果Fig.10 Virtual environment planning results

3.2 實驗分析

Sawyer機器人是Rethink Robotics公司推出的智能協作機器人，具備七自由度，能夠適應狹窄和復雜空間的作業環境。利用機器人操作系統(ROS)對Sawyer機械臂進行仿真環境配置，并將本文算法用于Sawyer機械臂的運動規則，結果如圖11所示。通過對比實驗結果可以看出，機械臂在狹窄通道、障礙物環境以及環境變化時，能夠快速規劃路線并成功地從起始位置到達目標位置。

(a)狹窄通道 (b)狹窄通道規劃結果

(c)復雜環境 (d)復雜環境規劃結果

(e)動態環境 (f)動態環境規劃結果圖11 ROS仿真規劃結果Fig.11 ROS simulation planning results

圖12所示為Sawyer機械臂在真實環境下的運動規劃驗證實驗。設定白色長方體盒為障礙物，左側位置為起始點，目標是避開障礙物將其移動到另一側的魔方位置。在路徑規劃完成后，將得到的路徑信息發送至機械臂控制器，從而實現機械臂從起始位置經過一條無碰撞的路徑到達目標位置的規劃任務。由圖12可以看出，本文算法在真實環境中能夠有效完成避障任務，證明了本文算法的實用性。

(a)初始環境

(b)環境變化圖12 真實環境下的避障過程Fig.12 Obstacle avoidance process in actual environment

4 結論

本文提出了一種改進RRT*FN機械臂多場景運動規劃算法。針對基本RRT*FN算法的缺陷進行了兩點改進：

(1)結合目標偏向隨機采樣和橢球子集采樣的優勢，提出新的啟發式采樣方法對采樣區域進行約束，從而保證路徑收斂速度更快，搜索路徑更優。

(2)在動態環境下，不需要完全重新規劃，而是采用對舊節點重新剪枝與連接的啟發式重規劃方法，提高了算法對環境的適應能力，規劃效率更高。

與傳統RRT*、RRT*FN、Informed RRT*算法相比，本文算法在規劃過程中收斂速度更快、規劃效率更高，在狹窄通道環境中成功率更高。在障礙物環境變化或目標點改變時，可以快速地適應環境，并能夠有效實現機械臂的快速運動規劃。