基于改進(jìn)RRT*FN算法的機(jī)械臂多場景運(yùn)動規(guī)劃

房立金 吳政翰 王懷震

東北大學(xué)機(jī)器人科學(xué)與工程學(xué)院，沈陽,110000

0 引言

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

與PRM算法相比，RRT算法更適用于解決高維空間中運(yùn)動規(guī)劃問題，因此RRT算法在機(jī)械臂的運(yùn)動規(guī)劃領(lǐng)域應(yīng)用較為廣泛[2-3]。RRT算法是在起始位置構(gòu)建一棵樹，通過向隨機(jī)樣本生長樹來探索狀態(tài)空間，當(dāng)?shù)玫揭粭l無碰撞路徑并到達(dá)目標(biāo)位置時即停止。然而，由于RRT算法是隨機(jī)抽樣過程，在目標(biāo)區(qū)域中發(fā)現(xiàn)一個樣本通常需要大量時間。針對這一問題，文獻(xiàn)[4]提出了RRT-Connect算法。與RRT相比，RRT-Connect算法用兩棵樹搜索狀態(tài)空間，從而提高了搜索效率，特別是當(dāng)目標(biāo)位置難以通過單向搜索進(jìn)行采樣時效果更加明顯。雖然該算法可以有效解決運(yùn)動規(guī)劃問題，但無法提供最優(yōu)解。這是因為用隨機(jī)采樣探索狀態(tài)空間輸出的是一系列隨機(jī)樣本，沒有任何優(yōu)化樹的過程，沒有考慮初始狀態(tài)和其他節(jié)點(diǎn)之間的運(yùn)動最優(yōu)性[5]。

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

針對上述問題，本文提出一種改進(jìn)RRT*FN的機(jī)械臂運(yùn)動規(guī)劃算法。在迭代過程中，采用帶有目標(biāo)偏向性和橢球子集采樣的啟發(fā)式方法對采樣區(qū)域進(jìn)行約束采樣，使采樣點(diǎn)能夠更快地收斂到最優(yōu)值。在擴(kuò)展節(jié)點(diǎn)時，配置樹中總節(jié)點(diǎn)數(shù)的預(yù)設(shè)值，并通過加權(quán)方法對樹中葉子節(jié)點(diǎn)進(jìn)行刪減，有效避免樹的規(guī)模無限增長。采用對節(jié)點(diǎn)剪枝與連接的啟發(fā)式重規(guī)劃方法，使算法更適應(yīng)動態(tài)環(huán)境。最后，通過仿真實驗進(jìn)行了驗證。

1 相關(guān)工作

1.1 運(yùn)動規(guī)劃問題定義

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

定義路徑成本為c:Xfree→R，R表示非負(fù)實數(shù)集合。尋找路徑成本較低的路徑是最優(yōu)路徑規(guī)劃的目標(biāo)，定義最低路徑成本函數(shù)為σ*：

(1)

Xf是狀態(tài)空間的一個子集，Xf?X，它可以提供比現(xiàn)有解決方案更好的方案，設(shè)cbest為當(dāng)前最短路徑的成本，則

Xf={x∈X∣f(x)

(2)

如果規(guī)劃時將搜索區(qū)域限制為Xf，則其狀態(tài)比整個狀態(tài)空間少，將可以更快地返回接近最優(yōu)的解。

1.2 碰撞檢測

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

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

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

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

2.1 基本RRT*FN算法

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

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

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 擴(kuò)展節(jié)點(diǎn)示意圖Fig.2 Schematic diagram of expansion node

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

基本的RRT*FN算法是在傳統(tǒng)的RRT*算法基礎(chǔ)上對樹中的最大節(jié)點(diǎn)數(shù)進(jìn)行預(yù)設(shè)，當(dāng)節(jié)點(diǎn)數(shù)大于預(yù)設(shè)值時，樹中的葉子節(jié)點(diǎn)將被隨機(jī)刪除，使得算法只使用一定內(nèi)存空間來完成運(yùn)動規(guī)劃任務(wù)。然而，RRT*FN算法依然存在收斂精度和搜索效率較低的問題。

2.2 改進(jìn)RRT*FN算法

針對上述RRT*FN算法存在的問題，本文提出了改進(jìn)的RRT*FN算法。首先，本文對采樣方法進(jìn)行改進(jìn)，將啟發(fā)式采樣方法加入RRT*FN中，然后采用啟發(fā)式重規(guī)劃方法來提高算法對環(huán)境的適應(yīng)性。

改進(jìn)的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

首先，對算法進(jìn)行初始化，確定初始位置與目標(biāo)位置，并建立障礙環(huán)境。其次，進(jìn)行采樣過程，對于xrand采用帶有目標(biāo)偏向的啟發(fā)式隨機(jī)采樣：

(3)

(4)

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

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

完成搜索最近鄰近點(diǎn)、擴(kuò)展新節(jié)點(diǎn)等操作后，在障礙環(huán)境中規(guī)劃出機(jī)械臂的一條可行路徑，機(jī)械臂開始按照規(guī)劃路徑運(yùn)動，環(huán)境變化則進(jìn)行重規(guī)劃操作。最后，機(jī)械臂末端運(yùn)動到指定位置，運(yùn)動規(guī)劃任務(wù)完成。改進(jìn)RRT*FN算法流程如圖3所示。

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

2.3 橢球子集采樣

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

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

xellipse=Lxball+xcenter

(5)

xcenter=(xf1+xf2)/2

(6)

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

(7)

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

LLT≡S

(8)

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

(9)

(10)

經(jīng)過分解運(yùn)算得到

(11)

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

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

(12)

其中det()為矩陣行列式，U∈Rn×n、V∈Rn×n通過奇異值分解UΣVT≡M的酉矩陣得出，Σ是主對角線上元素絕對值為1的對角陣，M通過恒等矩陣的第一列的外積和世界坐標(biāo)系上的橫軸長度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()函數(shù)生成一個從橢球坐標(biāo)系到世界坐標(biāo)系的旋轉(zhuǎn)矩陣，SampleUnitNBall()函數(shù)在以原點(diǎn)為中心的單位半徑的球內(nèi)進(jìn)行均勻采樣，最后將xrand映射到橢球內(nèi)。

2.4 重規(guī)劃

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

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

在得到初始解后，若環(huán)境變化則執(zhí)行重規(guī)劃例程，其偽代碼如下：

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

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

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

3 仿真與實驗分析

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

3.1 仿真分析

3.1.1二維仿真

設(shè)置650×650的場景1，將起點(diǎn)設(shè)為(50,50)，目標(biāo)點(diǎn)設(shè)為(600，600)，設(shè)置啟發(fā)式概率α=0.15，步長為5，令固定的最大節(jié)點(diǎn)數(shù)為1000，最大迭代次數(shù)為5000。

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

(a)RRT* (b)RRT*FN

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

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

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

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

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

(a)RRT* (b)RRT*FN

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

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

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

(a)初始位置 (b)目標(biāo)點(diǎn)變化

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

3.1.2三維仿真

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

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

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

(c)初始環(huán)境 (d)環(huán)境變化圖9 三維環(huán)境規(guī)劃結(jié)果Fig.9 3D environmental planning results

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

(a)虛擬環(huán)境 (b)規(guī)劃結(jié)果圖10 虛擬環(huán)境規(guī)劃結(jié)果Fig.10 Virtual environment planning results

3.2 實驗分析

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

(a)狹窄通道 (b)狹窄通道規(guī)劃結(jié)果

(c)復(fù)雜環(huán)境 (d)復(fù)雜環(huán)境規(guī)劃結(jié)果

(e)動態(tài)環(huán)境 (f)動態(tài)環(huán)境規(guī)劃結(jié)果圖11 ROS仿真規(guī)劃結(jié)果Fig.11 ROS simulation planning results

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

(a)初始環(huán)境

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

4 結(jié)論

本文提出了一種改進(jìn)RRT*FN機(jī)械臂多場景運(yùn)動規(guī)劃算法。針對基本RRT*FN算法的缺陷進(jìn)行了兩點(diǎn)改進(jìn)：

(1)結(jié)合目標(biāo)偏向隨機(jī)采樣和橢球子集采樣的優(yōu)勢，提出新的啟發(fā)式采樣方法對采樣區(qū)域進(jìn)行約束，從而保證路徑收斂速度更快，搜索路徑更優(yōu)。

(2)在動態(tài)環(huán)境下，不需要完全重新規(guī)劃，而是采用對舊節(jié)點(diǎn)重新剪枝與連接的啟發(fā)式重規(guī)劃方法，提高了算法對環(huán)境的適應(yīng)能力，規(guī)劃效率更高。

與傳統(tǒng)RRT*、RRT*FN、Informed RRT*算法相比，本文算法在規(guī)劃過程中收斂速度更快、規(guī)劃效率更高，在狹窄通道環(huán)境中成功率更高。在障礙物環(huán)境變化或目標(biāo)點(diǎn)改變時，可以快速地適應(yīng)環(huán)境，并能夠有效實現(xiàn)機(jī)械臂的快速運(yùn)動規(guī)劃。