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

房立金 吳政翰 王懷震

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

0 引言

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

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

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

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

1 相關(guān)工作

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

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

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

(1)

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

Xf={x∈X∣f(x)

(2)

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

1.2 碰撞檢測(cè)

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

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

圖1 碰撞檢測(cè)模型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*算法相同。

首先，從無(wú)障礙區(qū)域隨機(jī)抽取xrand節(jié)點(diǎn)，定位到xrand的最近節(jié)點(diǎn)，并將其表示為xnear。然后，通過(guò)將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

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

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

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

針對(duì)上述RRT*FN算法存在的問(wèn)題，本文提出了改進(jìn)的RRT*FN算法。首先，本文對(duì)采樣方法進(jìn)行改進(jìn)，將啟發(fā)式采樣方法加入RRT*FN中，然后采用啟發(fā)式重規(guī)劃方法來(lái)提高算法對(duì)環(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

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

(3)

(4)

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

采樣時(shí)，設(shè)定一個(gè)基準(zhǔn)值α，以概率α向目標(biāo)點(diǎn)方向采樣，概率1-α在自由空間內(nèi)隨機(jī)采樣，保證了搜索目標(biāo)的隨機(jī)性和快速性。α的設(shè)置根據(jù)障礙物確定，當(dāng)障礙物較少時(shí)，α設(shè)為較大值；障礙物較多時(shí)，α設(shè)為較小值。隨機(jī)樹(shù)每次在自由空間中采樣，按均勻概率分配隨機(jī)產(chǎn)生一個(gè)概率值P。如果P小于原先設(shè)定的基準(zhǔn)值，則對(duì)采樣點(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ī)械臂開(kāi)始按照規(guī)劃路徑運(yùn)動(dòng)，環(huán)境變化則進(jìn)行重規(guī)劃操作。最后，機(jī)械臂末端運(yùn)動(dòng)到指定位置，運(yùn)動(dòng)規(guī)劃任務(wù)完成。改進(jìn)RRT*FN算法流程如圖3所示。

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

2.3 橢球子集采樣

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

此采樣過(guò)程需要將樣本xellipse均勻地分布在子集Xellipse內(nèi)。通過(guò)將樣本均勻地分布在一個(gè)單位半徑為n的球上，然后將xball轉(zhuǎn)移到橢球子集Xball來(lái)實(shí)現(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)過(guò)分解運(yùn)算得到

(11)

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

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

(12)

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

M=a1I

(13)

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

(14)

其中，I是單位矩陣。

通過(guò)下式得到子集：

(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ù)生成一個(gè)從橢球坐標(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)的位置可能會(huì)變化，導(dǎo)致規(guī)劃好的路徑不再適用，所以關(guān)鍵問(wèn)題是既要使用之前規(guī)劃生成的樹(shù)，又要平衡使用上次規(guī)劃的樹(shù)和在此規(guī)劃中取樣的新點(diǎn)。針對(duì)這些問(wèn)題，需要對(duì)路徑進(jìn)行重新搜索和優(yōu)化。

對(duì)于移動(dòng)或未知障礙物的路徑運(yùn)動(dòng)問(wèn)題，傳統(tǒng)重規(guī)劃的方法是更新環(huán)境模型，并在所有障礙物都是靜態(tài)的情況下重新運(yùn)行一個(gè)新的運(yùn)動(dòng)規(guī)劃過(guò)程。該方法將丟棄舊樹(shù)，并丟失算法運(yùn)行時(shí)生成的有價(jià)值信息。丟失的信息包括障礙物碰撞檢測(cè)例程的結(jié)果和通過(guò)樹(shù)搜索配置空間的結(jié)果。本文采用對(duì)節(jié)點(diǎn)剪枝與連接的啟發(fā)式重規(guī)劃方法，能保留舊樹(shù)中的重要信息并盡可能重復(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ī)劃路徑σ(實(shí)現(xiàn)從xinit開(kāi)始的路徑節(jié)點(diǎn)的鏈表)將得到進(jìn)一步優(yōu)化，并更好地探索配置空間。然后，機(jī)器人開(kāi)始移動(dòng)。一旦在σ上到達(dá)新的節(jié)點(diǎn)xcur，障礙物信息將會(huì)更新。如果在xcur和xgoal之間的任何路徑段中檢測(cè)到障礙物碰撞，則機(jī)器人停止移動(dòng)。執(zhí)行SelectBranch和ValidPath例程，SelectBranch通過(guò)移除舊節(jié)點(diǎn)從xinit到xcur創(chuàng)建新節(jié)點(diǎn)。ValidPath檢查先前規(guī)劃的路徑，移除與新障礙物及其碰撞的所有節(jié)點(diǎn)，保留連接到xgoal的規(guī)劃路徑的可用部分。這部分稱(chēng)為σsep，從節(jié)點(diǎn)xsep開(kāi)始，到目標(biāo)節(jié)點(diǎn)xgoal結(jié)束。

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

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

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

3.1 仿真分析

3.1.1二維仿真

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

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

(a)RRT* (b)RRT*FN

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

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

表1 場(chǎng)景1中算法性能對(duì)比Tab.1 Performance comparison of the algorithms in scenario 1

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

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

(a)RRT* (b)RRT*FN

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

表2 場(chǎng)景2中算法性能對(duì)比Tab.2 Performance comparison of the algorithms in scenario 2

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

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

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

3.1.2三維仿真

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

對(duì)比圖9a、圖9b可以看出，RRT*算法的搜索樹(shù)幾乎布滿(mǎn)了整個(gè)空間，而本文算法以概率α向目標(biāo)點(diǎn)進(jìn)行采樣，簡(jiǎn)化了搜索過(guò)程，軌跡較為光滑，有效提高了軌跡規(guī)劃效率。如圖9c、圖9d所示，當(dāng)障礙物大小發(fā)生變化后，原規(guī)劃路徑無(wú)法實(shí)現(xiàn)避障任務(wù)，本文設(shè)計(jì)的重規(guī)劃算法將刪減部分原有搜索樹(shù)并搜索新樹(shù)，重新規(guī)劃出一條無(wú)障礙路徑，證明本文算法能較好地適應(yīng)動(dò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)一步驗(yàn)證本文算法在虛擬環(huán)境中的有效性，建立了七自由度機(jī)械臂模型,并用本文算法對(duì)其進(jìn)行了運(yùn)動(dòng)規(guī)劃仿真實(shí)驗(yàn)。如圖10所示，實(shí)驗(yàn)結(jié)果表明本文算法可以有效應(yīng)用于虛擬環(huán)境。

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

3.2 實(shí)驗(yàn)分析

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

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

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

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

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

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

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

4 結(jié)論

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

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

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

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