基于改進(jìn)LGM-BRRT＊的移動(dòng)機(jī)器人路徑規(guī)劃算法

謝志長(zhǎng)，嚴(yán)華

（四川大學(xué)電子信息學(xué)院，成都610065）

0 引言

隨著移動(dòng)機(jī)器人在核設(shè)施、宇宙探索、救援任務(wù)等領(lǐng)域應(yīng)用的快速增長(zhǎng)，移動(dòng)機(jī)器人在給定的工作環(huán)境中如何選擇行走路徑、避免碰撞障礙物、快速到達(dá)指定目標(biāo)的路徑規(guī)劃問(wèn)題成為移動(dòng)機(jī)器人研究熱點(diǎn)之一。傳統(tǒng)的路徑規(guī)劃算法有：人工勢(shì)場(chǎng)法、BUG 算法、A*算法、可視圖法等。人工勢(shì)場(chǎng)法是一種成熟的局部避障算法，但是容易陷入死鎖現(xiàn)象[1]，存在局部最小問(wèn)題。為了解決這一問(wèn)題，Kavraki 在文獻(xiàn)[2-3]中提出PRM算法，但是該算法不適合在未知環(huán)境下做路徑規(guī)劃[2]。BUG 算法運(yùn)行時(shí)間長(zhǎng)，容易陷入局部最優(yōu)等問(wèn)題[4]。A*算法在復(fù)雜情況下計(jì)算量急劇增長(zhǎng)，效率低下[5]。RRT（Rapidly-exploring Random Tree）算法是一種單查詢方法[6]，能夠解決大多數(shù)運(yùn)動(dòng)規(guī)劃問(wèn)題，并且它的改進(jìn)具有更好的性能。例如：B-RRT[7-8]、RRT-connect[9]、目標(biāo)偏向RRT[10]。然而，這些算法的局限性在于它們沒(méi)有考慮路徑成本，因此無(wú)法保證最佳解決方案。

為了解決這個(gè)問(wèn)題，Karaman 等人引入了RRT 的最佳變體RRT*[11]。RRT*首先找到一條初始路徑，然后通過(guò)重選父節(jié)點(diǎn)，重布線優(yōu)化路徑，這使得RRT*漸進(jìn)最優(yōu)，這意味著當(dāng)樣本數(shù)無(wú)限大時(shí)，可以保證收斂到最優(yōu)解。但是由于RRT*執(zhí)行的是純隨機(jī)探索，它的收斂速度非常地慢。

為了克服RRT*算法收斂速度慢的問(wèn)題，RRT*及其變體在最近幾年得到了廣泛的研究。文獻(xiàn)[12]提出了B-RRT*算法，通過(guò)兩棵樹(shù)交替擴(kuò)展，提高了算法的收斂速度；文獻(xiàn)[13]提出的RP-RRT*算法，通過(guò)融入人工勢(shì)場(chǎng)法優(yōu)化采樣過(guò)程，在勢(shì)力場(chǎng)的作用下執(zhí)行目標(biāo)偏差采樣，能夠在較短時(shí)間內(nèi)生成較優(yōu)路徑；文獻(xiàn)[14]提出包圍盒頂點(diǎn)引導(dǎo)的BNM 算法，通過(guò)障礙物包圍盒頂點(diǎn)引導(dǎo)路徑擴(kuò)展，該方法能夠產(chǎn)生接近最優(yōu)的無(wú)碰撞路徑；文獻(xiàn)[15]提出本地引導(dǎo)多個(gè)B-RRT*（LGMBRRT*）算法，該算法通過(guò)引入橋接測(cè)試和基于本地的新穎搜索策略解決了RRT*收斂速度低下的問(wèn)題。

LGM-BRRT*算法雖然解決了RRT*算法效率低下的問(wèn)題，但是在選擇本地橋接點(diǎn)時(shí)可能采用對(duì)目標(biāo)方向無(wú)用的橋接點(diǎn)，缺乏導(dǎo)向型，耗費(fèi)時(shí)間。因此，本文引入本地橋接點(diǎn)目標(biāo)導(dǎo)向策略避免獲取無(wú)用的橋接點(diǎn)，并在擴(kuò)展新節(jié)點(diǎn)時(shí)引入目標(biāo)偏向策略，進(jìn)一步提升收斂速度。在此基礎(chǔ)上，采用改進(jìn)的去冗余處理策略和三次B 樣條采樣擬合路徑，以獲取更優(yōu)的路徑。最后，在不同環(huán)境中與RRT*、B-RRT*和LGM-RRT*進(jìn)行了對(duì)比。仿真實(shí)驗(yàn)結(jié)果表明改進(jìn)算法具有更好的收斂性、穩(wěn)定性與有效性。

1 背景知識(shí)

定義1：根據(jù)文獻(xiàn)[15]給出的路徑規(guī)劃問(wèn)題定義：X∈Rd表 示 地 圖 范 圍，Xobs∈X代 表 障 礙 物 區(qū) 間，Xfree?XXobs表示無(wú)障礙物區(qū)間。Xinit表示起始點(diǎn)，Xgoal表示目標(biāo)點(diǎn)。路徑規(guī)劃問(wèn)題就是在Xfree區(qū)間找到一條從起始點(diǎn)Xinit到目標(biāo)點(diǎn)Xgoal的可通行路徑。

1.1 RRT＊算法介紹

RRT 算法首先初始化樹(shù)T，并將起始節(jié)點(diǎn)Xinit添加到該樹(shù)，在無(wú)障礙物空間中生成一個(gè)隨機(jī)節(jié)點(diǎn)Xrand，并在樹(shù)T 中找到最近的節(jié)點(diǎn)Xnearest。然后，沿Xnearest到Xrand的路線擴(kuò)展一步得到新節(jié)點(diǎn)Xnew，如果T 中最近的節(jié)點(diǎn)Xnearest與新節(jié)點(diǎn)Xnew之間的路徑無(wú)障礙物，則將新節(jié)點(diǎn)Xnew插入到樹(shù)T 中，重復(fù)此過(guò)程n 次，直到找到完整的可行路徑為止，如果在n 次迭代后仍未生成可行路徑，則意味著規(guī)劃失敗。算法原理如圖1 所示。由于RRT 算法以隨機(jī)方式快速生成一條可行的路徑，無(wú)法獲得漸進(jìn)最優(yōu)的路徑，RRT*通過(guò)在添加新節(jié)點(diǎn)時(shí)重選父節(jié)點(diǎn)和重布線減少路徑成本，獲得盡可能最優(yōu)的路徑。在添加新節(jié)點(diǎn)時(shí)，相較于RRT 選擇最近的鄰居作為父節(jié)點(diǎn)，RRT*選擇鄰域內(nèi)代價(jià)最小的節(jié)點(diǎn)作為父節(jié)點(diǎn)。在這基礎(chǔ)上，如果用新節(jié)點(diǎn)替換父節(jié)點(diǎn)能夠獲得更小代價(jià)，并且它們之間的路徑?jīng)]有沖突，則重新布線。這些改進(jìn)幫助RRT*找到接近最優(yōu)的路徑，但代價(jià)是執(zhí)行時(shí)間更長(zhǎng)。

圖1 RRT算法原理

1.2 LGM-BRRT＊算法介紹

通過(guò)前述分析，RRT*最大的問(wèn)題是時(shí)間消耗大，所以加快收斂速度是RRT*的主要改進(jìn)方向。Tahir 等人[12]提出B-RRT*算法提高RRT*的收斂速度。BRRT*使用兩棵樹(shù)Ta和Tb交替擴(kuò)展，所以在擴(kuò)展過(guò)程中，需要檢查兩棵樹(shù)是否已經(jīng)連接，若已連接，則記錄當(dāng)前路徑，若無(wú)連接，則進(jìn)行下一次迭代。如果迭代沒(méi)有結(jié)束，則將不斷優(yōu)化現(xiàn)有路徑。然而B(niǎo)-RRT*在面對(duì)狹窄通道等復(fù)雜地圖環(huán)境時(shí)仍然采用簡(jiǎn)單的隨機(jī)擴(kuò)展，需要更多的迭代次數(shù)才能從狹窄通道中搜索出可行路徑。

Shu 等人[14]提出的LGM-RRT*算法，引入本地橋接點(diǎn)引導(dǎo)樹(shù)策略找出地圖中的狹窄通道，快速搜索出可行路徑，很好地適應(yīng)了復(fù)雜地圖環(huán)境。LGM-RRT*需要提前對(duì)地圖進(jìn)行預(yù)處理，找出地圖中的狹窄通道點(diǎn)。橋接法[16]是識(shí)別狹窄通道的有效方法之一，它在障礙物空間Xobs中隨機(jī)生成兩個(gè)端點(diǎn)qf和qs組成的一個(gè)線段，若線段中點(diǎn)qm位于自由空間Xfree中，這樣的線段稱為一個(gè)橋，如圖2 所示。顯然，在狹窄區(qū)域建造一座“橋梁”要比在寬闊自由空間建造一座“橋梁”要容易得多[17]。最后，通過(guò)聚類分析方法獲得每個(gè)狹窄通道的唯一識(shí)別點(diǎn)[17-18]。Zhong[19]提出正交測(cè)試法以避免在拐角附近建造“橋梁”，如圖3 所示。

圖2 橋接法原理（實(shí)線表示成功，虛線表示失敗）

圖3 拐角附近的“橋梁”

B-RRT*和LGM-BRRT*對(duì)地圖一的規(guī)劃結(jié)果如圖4-5 所示。從圖中可以看出LGM-BRRT*算法的路徑更短，但存在無(wú)效的本地點(diǎn)引導(dǎo)。原因在于LGMBRRT*只采用了一個(gè)歐氏距離范圍來(lái)限制選擇本地點(diǎn)引導(dǎo)，距離小于設(shè)定閾值時(shí)就觸發(fā)本地點(diǎn)引導(dǎo)，但是此時(shí)的引導(dǎo)可能已經(jīng)完全偏離目標(biāo)方向。

圖4 BRRT*地圖一規(guī)劃結(jié)果

圖5 LGM-BRRT*地圖一規(guī)劃結(jié)果

2 改進(jìn)的LGM-BRRT＊算法

本節(jié)將介紹一種加入目標(biāo)方向約束的本地點(diǎn)引導(dǎo)策略，消除LGM-BRRT*的冗余引導(dǎo)，以幫助LGMBRRT*快速規(guī)劃初始路徑。同時(shí)，采樣點(diǎn)加入目標(biāo)偏向，以削弱采樣的隨機(jī)性，加速找到最佳路徑。最后，再做相應(yīng)的路徑優(yōu)化策略。

2.1 本地點(diǎn)目標(biāo)約束

LGM-BRRT*本地橋接點(diǎn)引導(dǎo)觸發(fā)的條件是距離小于設(shè)定閾值Ltri。在此基礎(chǔ)上，改進(jìn)算法加入角度閾值θtri約束方向，當(dāng)新增節(jié)點(diǎn)和本地橋接點(diǎn)的距離小于Ltri并且和目標(biāo)點(diǎn)構(gòu)成的角度小于θtri時(shí)，觸發(fā)本地引導(dǎo)。加入角度閾值θtri約束方向后，可以避免冗余的本地點(diǎn)引導(dǎo)，能夠快速到達(dá)目標(biāo)點(diǎn)，提高收斂速度。

如圖6 所示，在二維地圖環(huán)境中，假設(shè)目標(biāo)為Pgoal(xg,yg) ，本地橋接點(diǎn)為Plocal(xl,yl) ，新擴(kuò)展節(jié)點(diǎn)為Ptemp(xt,yt)，這三點(diǎn)構(gòu)成的角度為θ。

θ的角度公式為：

圖6 約束角度示意圖

2.2 目標(biāo)偏向采樣

LGM-RRT*算法擴(kuò)展新節(jié)點(diǎn)時(shí)，是在自由空間中生成一個(gè)隨機(jī)點(diǎn)Prand(xr,yr)，然后在擴(kuò)展樹(shù)中找到離隨機(jī)點(diǎn)最近的節(jié)點(diǎn)擴(kuò)展一個(gè)步長(zhǎng)。這種擴(kuò)展方式隨機(jī)性太強(qiáng)，當(dāng)?shù)貓D存在較小出口時(shí)，從出口向外擴(kuò)展成功的概率相應(yīng)減小，使得從出口向外擴(kuò)展的時(shí)間更長(zhǎng)。因此，改進(jìn)算法加入目標(biāo)方向上的引導(dǎo)能夠提升擴(kuò)展效率。如圖7 所示，往隨機(jī)方向與目標(biāo)方向的合成向量方向擴(kuò)展一個(gè)步長(zhǎng)。其中Pgoal(xg,yg) 為目標(biāo)點(diǎn)，Pnearest(xn,yn)為擴(kuò)展樹(shù)上最近的點(diǎn)，λ1為合成系數(shù)，新擴(kuò)展點(diǎn)的坐標(biāo)為Ptemp(xt,yt)。

圖7 目標(biāo)偏向采樣示意圖

擴(kuò)展新節(jié)點(diǎn)函數(shù)算法1 所示，generateNewNode 根據(jù)公式（2）、公式（3）、公式（4）生成Ptemp點(diǎn)，如果Pnearest到Ptemp點(diǎn)的路徑之間無(wú)障礙物，將Ptemp點(diǎn)加入樹(shù)T 中。

算法1：Expand（xnearest，xrand）

2.3 使用本地點(diǎn)約束和目標(biāo)偏向采樣策略

設(shè)置本地點(diǎn)目標(biāo)約束和目標(biāo)偏向采樣策略后，改進(jìn)算法的擴(kuò)展策略如算法2 所示，稱起始點(diǎn)向目標(biāo)點(diǎn)擴(kuò)展的樹(shù)為Ta，稱目標(biāo)點(diǎn)向Ta擴(kuò)展的樹(shù)為Tb。

算法2：改進(jìn)LGM-BRRT*

2.4 路徑優(yōu)化處理

使用以下三種策略進(jìn)行路徑優(yōu)化處理。

（1）考慮路徑機(jī)器人寬度的膨脹檢測(cè)

碰撞檢測(cè)時(shí)，對(duì)機(jī)器人路徑進(jìn)行膨脹處理。膨脹后的機(jī)器人路徑將向外擴(kuò)張長(zhǎng)度為λ2Rrt（Rrt為機(jī)器人半徑）。此時(shí)兩個(gè)路徑節(jié)點(diǎn)之間機(jī)器人路徑可看做是長(zhǎng)條矩形，如圖8 所示。

圖8 碰撞檢測(cè)示意圖

（2）改進(jìn)去冗余處理策略

從起始點(diǎn)后的第二個(gè)節(jié)點(diǎn)開(kāi)始，依次向前遍歷，若當(dāng)前節(jié)點(diǎn)與起始點(diǎn)不發(fā)生碰撞且該兩點(diǎn)與其中間點(diǎn)不在同一直線上，則將中間點(diǎn)刪除；否則以中間點(diǎn)為起始點(diǎn)重新向下遍歷。如圖9 所示，start 到step4 之間有三個(gè)路徑節(jié)點(diǎn)，去除step2 可以減小路徑長(zhǎng)度。而使用傳統(tǒng)策略，去除的是step1 和step3，這樣只減少了路徑節(jié)點(diǎn)，而并未縮短路徑。

圖9 路徑平滑操作

（3）使用路徑控制點(diǎn)的曲線擬合處理

在曲線擬合之前對(duì)原路經(jīng)進(jìn)行插值處理，當(dāng)路徑節(jié)點(diǎn)與左右兩點(diǎn)的路徑長(zhǎng)度大于最小障礙物包圍盒寬度時(shí)，則在該路徑節(jié)點(diǎn)中間靠近路徑節(jié)點(diǎn)位置插入控制節(jié)點(diǎn)，插值之后在使用三次B 樣條曲線擬合路徑，以避免直接采用曲線擬合會(huì)導(dǎo)致擬合后路徑在轉(zhuǎn)彎處偏離原路經(jīng)而產(chǎn)生碰撞。

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

為了驗(yàn)證改進(jìn)算法的有效性，在虛擬機(jī)Ubuntu 14.04（內(nèi)存2GB，硬盤25GB，處理器2 核）搭建機(jī)器人操作系統(tǒng)（Robots On System，ROS），在該環(huán)境下實(shí)現(xiàn)改進(jìn)算法，并在ROS 可視化工具（3D visualization tool for ROS，RVIZ）中顯示完整的路徑規(guī)劃結(jié)果。用于安裝虛擬機(jī)的計(jì)算機(jī)配置為：（處理器：Intel Core i7-4710HQ 2.50 GHz CPU，內(nèi)存：8GB，系統(tǒng)類型64 位操作系統(tǒng)）。

環(huán)境地圖設(shè)置成200×200，起點(diǎn)坐標(biāo)（3，3），目標(biāo)位置（197，197），擴(kuò)展步長(zhǎng)設(shè)置為3，當(dāng)樹(shù)和目標(biāo)點(diǎn)之間的距離小于步長(zhǎng)時(shí)，則認(rèn)為樹(shù)已擴(kuò)展到目標(biāo)。當(dāng)兩棵樹(shù)之間的距離小于3 時(shí)，則認(rèn)為兩棵樹(shù)已連接。設(shè)定距離閾值Ltri=25，角度閾值θtri=45，設(shè)置路徑寬度為機(jī)器人半徑的1.5 倍，即λ2=1.5。不同參數(shù)設(shè)置對(duì)RRT 算法的效率有不同的影響，但是為了便于比較，在模擬測(cè)試中設(shè)置了相同的值。

在地圖一中，存在很多障礙物，包括9 個(gè)狹窄出口，這使得擴(kuò)展難度增加。圖10、圖11、圖12 和圖13分別顯示了在不同迭代次數(shù)下RRT*、B-RRT*、LGMBRRT*與改進(jìn)LGM-BRRT*的在地圖一環(huán)境下的仿真實(shí)驗(yàn)結(jié)果。在圖10 中，當(dāng)?shù)螖?shù)增加到1000 次時(shí)，RRT*才能規(guī)劃出一條路徑，這是四種算法所需的最大迭代次數(shù)。而改進(jìn)LGM-B-RRT*算法僅迭代了150次就規(guī)劃出了路徑，此時(shí)RRT*還未找到第二個(gè)狹窄出口，B-RRT*兩棵樹(shù)尚未相交。對(duì)比LGM-BRRT*，改進(jìn)的LGM-BRRT*擴(kuò)展樹(shù)枝更加收攏，具有目標(biāo)性，無(wú)效擴(kuò)展少。從圖10、圖11、圖12 和圖13 的處理過(guò)程可以看出，改進(jìn)的LGM-BRRT*比RRT*、B-RRT*和LGM-BRRT*更早進(jìn)入收斂狀態(tài)。為進(jìn)一步觀察改進(jìn)LGM-BRRT*算法的穩(wěn)定性，在地圖二中進(jìn)行了實(shí)驗(yàn)，圖14 展示了在地圖二下部分迭代次數(shù)的仿真結(jié)果?？梢钥闯?，在不同的地圖環(huán)境下，該算法依然能在較少迭代次數(shù)下獲得很好的仿真結(jié)果，進(jìn)一步證明了本文所提算法的有效性。

圖10 RRT*的地圖一仿真結(jié)果（從左到右依次是迭代次數(shù)150、300、500、1000、1500）

圖11 B-RRT*的地圖一仿真結(jié)果（從左到右依次是迭代次數(shù)150、300、500、1000、1500）

圖12 LGM-BRRT*的地圖一仿真結(jié)果（從左到右依次是迭代次數(shù)150、300、500、1000、1500）

圖13 改進(jìn)LGM-BRRT*的地圖一仿真結(jié)果（從左到右依次是迭代次數(shù)150、300、500、1000、1500）

圖14 改進(jìn)LGM-BRRT*在地圖二下的仿真結(jié)果（從左到右依次是迭代次數(shù)150、300、500、1500、2500）

為了觀察每種算法在不同迭代次數(shù)中的規(guī)劃能力，對(duì)每種算法進(jìn)行迭代次數(shù)為150、300、500、1000、1500、2000、2500、3000 的實(shí)驗(yàn)，統(tǒng)計(jì)十次規(guī)劃成功的次數(shù)。圖15 顯示了隨著迭代次數(shù)的增加，四種算法在不同環(huán)境中的成功次數(shù)的統(tǒng)計(jì)。從圖15 可以看出，改進(jìn)LGM-BRRT*最先進(jìn)入收斂狀態(tài)。除此之外，路徑成本也是度量規(guī)劃算法的重要依據(jù)，為了避免單項(xiàng)測(cè)試的偶然性，在每個(gè)迭代次數(shù)下進(jìn)行十次實(shí)驗(yàn)，并將十次實(shí)驗(yàn)的平均路徑成本作為最終路徑成本值。圖16 繪制了在不同迭代次數(shù)下的路徑成本統(tǒng)計(jì)圖。從圖16 可以看出，RRT*、Bi-RRT*、LGM-BRRT*和改進(jìn)LGMBRRT*算法的路徑成本隨著迭代次數(shù)的增加而降低。當(dāng)?shù)螖?shù)相同時(shí)，改進(jìn)LGM-BRRT*路徑成本比其他三種算法要小。

綜合以上實(shí)驗(yàn)與分析可以證明，改進(jìn)LGM-BRRT*算法運(yùn)行結(jié)果收斂更快，路徑成本更低，適合狹窄通道地圖路徑規(guī)劃。

圖15 四種算法在不同迭代下成功次數(shù)統(tǒng)計(jì)

圖16 四種算法在不同迭代下路徑成本統(tǒng)計(jì)

4 結(jié)語(yǔ)

為了解決LGM-BRRT*算法存在選擇本地橋接點(diǎn)時(shí)缺乏導(dǎo)向性導(dǎo)致算法效率低的問(wèn)題，提出了改進(jìn)LGM-BRRT*算法，通過(guò)引入本地橋接點(diǎn)目標(biāo)導(dǎo)向策略，并在擴(kuò)展新節(jié)點(diǎn)時(shí)引入目標(biāo)偏向提高收斂速度。在ROS 仿真驗(yàn)證了改進(jìn)策略的有效性。但是，算法需要對(duì)地圖做一定的預(yù)處理找出本地引導(dǎo)點(diǎn)，而在實(shí)際環(huán)境中地圖是未知的，需要融合實(shí)時(shí)避障策略進(jìn)行處理。