基于MOBDB-RRT*算法的移動(dòng)機(jī)器人路徑規(guī)劃

張 瑞， 周 麗,2， 劉震鍇

(1.南京信息工程大學(xué)，南京 210000； 2.江蘇省大氣環(huán)境與裝備技術(shù)協(xié)同創(chuàng)新中心，南京 210000)

0 引言

21世紀(jì)是信息化與智能化的時(shí)代，而機(jī)器人作為現(xiàn)代高自動(dòng)化產(chǎn)物，被廣泛應(yīng)用于人類的日常生活。機(jī)器人技術(shù)的快速發(fā)展，提高了生產(chǎn)效率，促進(jìn)了經(jīng)濟(jì)發(fā)展，給人們的生活帶來(lái)了便利。因而，近年來(lái)國(guó)內(nèi)外對(duì)于機(jī)器人的研究也更加深入，其中，路徑規(guī)劃問(wèn)題是機(jī)器人研究領(lǐng)域的一個(gè)重要分支[1]?？紤]到機(jī)器人現(xiàn)實(shí)工作環(huán)境的復(fù)雜情況，需要事先為機(jī)器人規(guī)劃一條從起點(diǎn)到終點(diǎn)的安全無(wú)碰撞最優(yōu)路徑。傳統(tǒng)的路徑規(guī)劃算法有人工勢(shì)場(chǎng)法[2]、A*算法[3]、可視圖法[4]等?；谥悄芩惴ǖ挠羞z傳算法[5]、粒子群算法[6]、蟻群算法[7]等。其中，傳統(tǒng)的路徑規(guī)劃算法需要對(duì)復(fù)雜的規(guī)劃空間和障礙物進(jìn)行精確建模，而環(huán)境的復(fù)雜程度越高，算法的規(guī)劃效率也就越低。智能算法雖然學(xué)習(xí)能力非常強(qiáng)，但實(shí)時(shí)性差、計(jì)算量較大，需要占用大量的存儲(chǔ)空間[8]。

為了加強(qiáng)在復(fù)雜障礙物環(huán)境中的規(guī)劃能力，基于采樣的單一查詢快速擴(kuò)展隨機(jī)樹(shù)(Rapidly-exploring Random Tree，RRT)算法被提出[9]。RRT由于結(jié)構(gòu)簡(jiǎn)單、概率完備性、強(qiáng)大的搜索擴(kuò)展能力以及避免對(duì)復(fù)雜空間精確建模等優(yōu)點(diǎn)，被廣泛應(yīng)用于機(jī)器人的路徑規(guī)劃。然而，現(xiàn)有的RRT算法有著規(guī)劃效率低、易陷入局部極小值、路徑長(zhǎng)度不最優(yōu)、路徑不光滑等缺點(diǎn)。為了解決RRT算法存在的問(wèn)題，諸多學(xué)者提出了改進(jìn)方法。KUFFNER等[10]提出了RRT-connect算法，在起點(diǎn)和終點(diǎn)同時(shí)生成兩棵隨機(jī)樹(shù)，通過(guò)雙向擴(kuò)展縮短了規(guī)劃時(shí)間，但算法缺乏對(duì)路徑成本的考慮；KARAMAN等[11]提出了RRT*算法，可在迭代次數(shù)充足的條件下找到最優(yōu)路徑，但算法搜索時(shí)所花費(fèi)的時(shí)間較長(zhǎng)；JORDAN等[12]提出了B-RRT*算法，在RRT*算法的基礎(chǔ)上引入雙向搜索思想，加快了RRT*算法的收斂速度，但采樣較為隨機(jī)，節(jié)點(diǎn)的利用率不高；NASIR等[13]提出了RRT*-smart算法，通過(guò)智能采樣的方式提高了規(guī)劃效率，但自適應(yīng)能力較差；王兆光[14]提出了改進(jìn)的概率偏向目標(biāo)RRT算法，提高規(guī)劃效率的同時(shí)有效解決了局部極小值的問(wèn)題，但不能保證路徑長(zhǎng)度達(dá)到最優(yōu)；張偉民等[15]提出了一種基于目標(biāo)約束采樣和目標(biāo)偏置擴(kuò)展的改進(jìn)RRT*算法，加快算法搜索速度的同時(shí)路徑也變得更加光滑，但沒(méi)有考慮到機(jī)器人的安全運(yùn)行距離。上述文獻(xiàn)對(duì)RRT算法進(jìn)行了有效的改進(jìn)，但在復(fù)雜的環(huán)境中，障礙物的不規(guī)則性和狹窄的間距可能會(huì)降低算法的規(guī)劃效率，而且對(duì)路徑的安全性也提出了較高的要求。

本文結(jié)合已有的研究及存在的問(wèn)題，提出了一種基于膨脹化障礙物環(huán)境的雙向動(dòng)態(tài)目標(biāo)偏置RRT*(Magnified Obstacles-based Bidirectional Dynamic Goal Bias RRT*，MOBDB-RRT*)算法。針對(duì)RRT*算法規(guī)劃的路徑距離障礙物近的問(wèn)題，采用對(duì)障礙物進(jìn)行膨脹化處理方法，給機(jī)器人預(yù)留出一定的安全距離；為了提高算法的規(guī)劃效率并解決易陷入局部極小值的問(wèn)題，基于RRT*算法引入雙向動(dòng)態(tài)目標(biāo)偏置策略，使生成的節(jié)點(diǎn)具有明確的方向性，減少了對(duì)不必要區(qū)域的搜索；為了得到更短、更光滑的路徑，本文還對(duì)已規(guī)劃好的路徑采用修剪算法和三次貝塞爾曲線進(jìn)行二次優(yōu)化，使最終路徑更有利于移動(dòng)機(jī)器人安全運(yùn)行。

1 RRT*算法

RRT算法在路徑規(guī)劃過(guò)程中只連接到最近的點(diǎn)，并不能保證路徑的成本達(dá)到最低，因此KARAMAN等在2011年提出了RRT*算法。RRT*算法是一種漸近最優(yōu)算法，在RRT算法的基礎(chǔ)上加入了重新選擇父節(jié)點(diǎn)以及重新布線的過(guò)程，這樣既保留了RRT算法的概率完備性，又能使路徑達(dá)到相對(duì)最優(yōu)。

RRT*算法的第一層優(yōu)化：重新選擇父節(jié)點(diǎn)。具體過(guò)程如圖1所示，首先以qnew為圓心和事先定義好的半徑畫圓，找到這個(gè)圓內(nèi)qnew的所有近鄰點(diǎn)作為其備選父節(jié)點(diǎn)，分別為點(diǎn)E，F(xiàn)，H，J。從圖中可以看出，A-B-D-K這條路徑為RRT算法規(guī)劃的初始未優(yōu)化路徑，其路徑成本為9。當(dāng)備選父節(jié)點(diǎn)連接到qnew時(shí)，路徑分別為A-E-K，A-B-D-F-K，A-E-H-K，A-B-D-J-K，路徑成本分別為8，13，12，13。由于路徑A-E-K的成本最低，所以將路徑A-E-K替換掉原來(lái)路徑A-B-D-K。

圖1 重新選擇父節(jié)點(diǎn)Fig.1 Reselect parent nodes

RRT*算法的第二層優(yōu)化：隨機(jī)樹(shù)重新布線。為了進(jìn)一步減小路徑成本，試圖為qnew尋找下一個(gè)可連接的近鄰點(diǎn)，若此路徑成本小于原先路徑到達(dá)該點(diǎn)的成本，則將其替換。具體過(guò)程如圖2所示，D，F(xiàn)，H，J為qnew的近鄰點(diǎn)，其中路徑A-E-K-D的成本為10，而原先路徑A-B-D成本為7，不滿足要求。同理，連接到其他近鄰點(diǎn)的成本分別為10，11，11，與之對(duì)應(yīng)的原先路徑成本為11，9，10。因此，將路徑A-E-K-F替換掉原先路徑A-B-D-F，成為新的隨機(jī)樹(shù)。

圖2 隨機(jī)樹(shù)的重新布線Fig.2 Rerouting of random trees

2 MOBDB-RRT*算法

RRT*是一種漸近優(yōu)化算法，當(dāng)路徑中節(jié)點(diǎn)的數(shù)量不斷增加時(shí)，算法的計(jì)算量也隨之增加，因此需要較長(zhǎng)的運(yùn)算時(shí)間才能逼近最優(yōu)解。雖然RRT*算法能使路徑達(dá)到相對(duì)最優(yōu)，但沒(méi)有解決RRT算法盲目搜索帶來(lái)規(guī)劃效率低的問(wèn)題，反而進(jìn)一步增加了規(guī)劃時(shí)間，且路徑中還存在著冗余節(jié)點(diǎn)，無(wú)法使路徑成本達(dá)到最低?？紤]到安全性，線性連接路徑的不光滑以及靠近障礙物也不利于機(jī)器人移動(dòng)。針對(duì)以上RRT*算法存在的缺點(diǎn)，本文提出了一種MOBDB-RRT*算法，該算法將障礙物進(jìn)行膨脹化處理，避免了路徑距離障礙物太近，保證機(jī)器人安全運(yùn)行；以雙向動(dòng)態(tài)偏向目標(biāo)代替單向隨機(jī)擴(kuò)展，提高了RRT*算法的搜索效率；利用修剪算法和三次貝塞爾曲線對(duì)路徑進(jìn)行優(yōu)化處理，最終生成一條有利于機(jī)器人移動(dòng)的光滑路徑。

2.1 設(shè)置膨脹化障礙物

為了獲得成本最小的路徑，RRT*算法可能會(huì)連接靠近障礙物的節(jié)點(diǎn)，雖然沒(méi)有發(fā)生碰撞，但這樣的路徑顯然不適合實(shí)體移動(dòng)機(jī)器人運(yùn)行。因此，需要為機(jī)器人設(shè)置安全運(yùn)行距離(一般為機(jī)器人寬度的一半)。本文選擇將障礙物進(jìn)行膨脹化處理，在放大了的障礙物環(huán)境中進(jìn)行路徑規(guī)劃，這樣不僅可以避免路徑緊貼著障礙物，而且為后續(xù)光滑處理后路徑的避障帶來(lái)了便利。

本文以多邊形障礙物為例，為移動(dòng)機(jī)器人設(shè)置復(fù)雜的運(yùn)行環(huán)境。圖3為膨脹化處理后的障礙物環(huán)境，其中,膨脹系數(shù)k即原障礙物邊和膨脹后障礙物邊之間的距離,可根據(jù)機(jī)器人的大小進(jìn)行調(diào)整。

圖3 膨脹化障礙物Fig.3 Magnified obstacles

2.2 雙向動(dòng)態(tài)目標(biāo)偏置策略

由于RRT*算法的路徑節(jié)點(diǎn)是朝隨機(jī)點(diǎn)的方向生成，擴(kuò)展方式為單向，在復(fù)雜的障礙物環(huán)境中，無(wú)疑增加了規(guī)劃的難度。所以希望通過(guò)起點(diǎn)和終點(diǎn)同時(shí)擴(kuò)展兩棵隨機(jī)樹(shù)，并改變路徑規(guī)劃過(guò)程中節(jié)點(diǎn)的生成方向，讓其盡可能朝著目標(biāo)點(diǎn)方向擴(kuò)展，從而達(dá)到縮短規(guī)劃時(shí)間的目的。

首先定義兩棵隨機(jī)擴(kuò)展樹(shù)V1和V2，一棵從起點(diǎn)開(kāi)始擴(kuò)展，另一棵從終點(diǎn)開(kāi)始擴(kuò)展，每次迭代生成兩個(gè)點(diǎn)。其中,新節(jié)點(diǎn)qnew以動(dòng)態(tài)偏向目標(biāo)的方式生成，在隨機(jī)樹(shù)的擴(kuò)展過(guò)程中，首先對(duì)樹(shù)節(jié)點(diǎn)中的最近點(diǎn)qnearest和隨機(jī)點(diǎn)qrand的連線進(jìn)行障礙物判斷，有障礙物時(shí)按隨機(jī)點(diǎn)方向生成新節(jié)點(diǎn)qnew，其算式為

(1)

若沒(méi)有障礙物，取隨機(jī)點(diǎn)qrand方向的步長(zhǎng)為λ1，終點(diǎn)qgoal方向的步長(zhǎng)為λ2，其中λ2=2λ1，根據(jù)矢量合成原理，可得到新節(jié)點(diǎn)qnew，其算式為

(2)

當(dāng)碰撞檢測(cè)失敗即最近點(diǎn)qnearest和新節(jié)點(diǎn)qnew之間有障礙物，新節(jié)點(diǎn)qnew生成無(wú)效，此時(shí)將步長(zhǎng)進(jìn)行反轉(zhuǎn)變換，讓隨機(jī)點(diǎn)qrand方向的步長(zhǎng)為λ2，終點(diǎn)qgoal方向步長(zhǎng)為λ1，原理同上可得到新節(jié)點(diǎn)qnew，其算式為

(3)

如果碰撞檢測(cè)還是失敗，無(wú)法得到有效的新節(jié)點(diǎn)qnew時(shí)，這說(shuō)明偏向出現(xiàn)了困難，此時(shí)，只需讓新節(jié)點(diǎn)qnew按隨機(jī)點(diǎn)方向生成即可。圖4為雙向動(dòng)態(tài)目標(biāo)偏置的具體擴(kuò)展示意圖。

圖4 雙向動(dòng)態(tài)目標(biāo)偏置的具體擴(kuò)展示意圖Fig.4 Specific expansion diagram of bidirectional dynamic goal bias

上文描述的雙向動(dòng)態(tài)目標(biāo)偏置策略，加快了RRT*算法的收斂速度，提高了路徑的規(guī)劃效率，其中，動(dòng)態(tài)變步長(zhǎng)的搜索方式有效避免了在復(fù)雜障礙物環(huán)境中陷入局部極小值的問(wèn)題。

2.3 路徑優(yōu)化

2.3.1 路徑縮短

RRT*算法的隨機(jī)擴(kuò)展會(huì)生成一些冗余節(jié)點(diǎn)，從而導(dǎo)致路徑較為曲折，在滿足避障要求的前提下，去除這些多余的節(jié)點(diǎn)，有利于進(jìn)一步減小路徑成本。路徑縮短處理采用修剪算法，基本原理如下：首先讓終點(diǎn)qgoal和起點(diǎn)qstart直接相連，如果無(wú)碰撞，最終路徑就為起點(diǎn)連接終點(diǎn)的直線，如果有碰撞，選擇終點(diǎn)qgoal的前一個(gè)點(diǎn)和起點(diǎn)qstart相連，按照這樣的方式，依次對(duì)各個(gè)路徑點(diǎn)和qstart進(jìn)行障礙物碰撞判斷，直至找到無(wú)碰撞的那個(gè)路徑點(diǎn)，記為新起點(diǎn)q′start，重復(fù)上述過(guò)程，一旦找到能與終點(diǎn)qgoal直接相連且無(wú)碰撞的新起點(diǎn)就結(jié)束。最終的優(yōu)化路徑就由起點(diǎn)、中間這些新的起點(diǎn)和終點(diǎn)連接而成。

2.3.2 路徑光滑

RRT*算法規(guī)劃的路徑最終是點(diǎn)與點(diǎn)的線性連接，而這種鋸齒狀不光滑路徑不利于機(jī)器人移動(dòng)，因此需要對(duì)上述規(guī)劃好的路徑進(jìn)行光滑處理。為了平滑地?cái)M合路徑節(jié)點(diǎn)，本文采用貝塞爾曲線。設(shè)n次貝塞爾曲線有n+1個(gè)控制點(diǎn)(P0,P1,…,Pn)，其定義為

(4)

式中：s∈[0,1]；Bn,i(s)為Berstein多項(xiàng)式。

根據(jù)式(4)可知，當(dāng)n越大時(shí)，算法的計(jì)算成本會(huì)隨之增加，因此較為低階的貝塞爾曲線是首選，而三次貝塞爾曲線是可以生成連續(xù)曲率軌跡的最小次數(shù)曲線。本文基于三次貝塞爾曲線對(duì)折線路徑進(jìn)行光滑處理。

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

利用Matlab R2016a 對(duì)本文提出的MOBDB-RRT*算法進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證。筆記本電腦的系統(tǒng)為Windows10，處理器為Intel?CoreTMi5-10210U CPU @1.6 GHz/2.11 GHz，運(yùn)行內(nèi)存為16 GiB。

為了驗(yàn)證所提出MOBDB-RRT*算法的優(yōu)越性，本文將RRT*，B-RRT*和MOBDB-RRT*這3種算法在3種不同的二維仿真環(huán)境下做對(duì)比實(shí)驗(yàn)，地圖大小為400 dm×400 dm，障礙物均設(shè)置為多邊形，將3種算法在每個(gè)地圖中各運(yùn)行50次，表1記錄了規(guī)劃路徑時(shí)所花費(fèi)的平均擴(kuò)展節(jié)點(diǎn)、平均規(guī)劃時(shí)長(zhǎng)和平均路徑長(zhǎng)度。

表1 實(shí)驗(yàn)數(shù)據(jù)Table 1 Experimental data

圖5為狹窄不規(guī)則障礙物環(huán)境(地圖1)規(guī)劃對(duì)比圖。起點(diǎn)為(10 dm，10 dm)，終點(diǎn)為(400 dm，400 dm)，地圖特點(diǎn)是障礙物較大且間距狹窄。圖5(a)為RRT*算法規(guī)劃的一條可行路徑，由于RRT*算法的盲目搜索特性，生長(zhǎng)樹(shù)會(huì)隨機(jī)地分布在無(wú)障礙區(qū)域，雖然路徑相對(duì)較優(yōu)，但規(guī)劃所需時(shí)間較長(zhǎng)。圖5(b)為B-RRT*算法規(guī)劃的一條可行路徑，雖然引入了雙向擴(kuò)展思想，改善了RRT*算法規(guī)劃效率低的缺點(diǎn)，但生長(zhǎng)樹(shù)的擴(kuò)展依然充滿了隨機(jī)性，無(wú)效節(jié)點(diǎn)較多。圖5(c)為MOBDB-RRT*算法規(guī)劃的一條可行路徑，安全距離和雙向動(dòng)態(tài)偏向目標(biāo)的引入使算法在較為狹窄的環(huán)境中也能快速找到一條較優(yōu)的路徑，擴(kuò)展節(jié)點(diǎn)利用率高，減少了對(duì)不必要空間的探索。

圖5 不規(guī)則障礙物環(huán)境規(guī)劃對(duì)比圖Fig.5 Comparison of path planning in an environment with irregular obstacles

從表1的地圖1實(shí)驗(yàn)數(shù)據(jù)中可以看出:MOBDB-RRT*算法的平均擴(kuò)展節(jié)點(diǎn)數(shù)為60，比RRT*算法減少97.06%，比B-RRT*算法減少70.44%；平均規(guī)劃時(shí)長(zhǎng)為0.580 2s，比RRT*算法縮短95.57%，比B-RRT*算法縮短53.62%；平均路徑長(zhǎng)度為637.790 1 dm，比B-RRT*算法減少2.35%。

圖6為大量規(guī)則障礙物環(huán)境(地圖2)規(guī)劃對(duì)比圖，起點(diǎn)為(400 dm,0 dm)，終點(diǎn)為(0 dm,400 dm)，地圖特點(diǎn)是障礙物較多，起點(diǎn)到終點(diǎn)有多條可行路徑。

圖6 大量規(guī)則障礙物環(huán)境規(guī)劃對(duì)比圖Fig.6 Comparison of path planning in an environment with a large number of regular obstacles

圖6(a)中，RRT*算法通過(guò)全圖擴(kuò)展隨機(jī)樹(shù)，利用自身的重布線特性，規(guī)劃出一條相對(duì)較優(yōu)路徑，但擴(kuò)展隨機(jī)樹(shù)所需節(jié)點(diǎn)較多，規(guī)劃效率不高。圖6(b)中，B-RRT*算法由于隨機(jī)采樣和雙向擴(kuò)展可能導(dǎo)致路徑偏差較大，無(wú)法找到較優(yōu)路徑。圖6(c)中，MOBDB-RRT*算法由于加入雙向目標(biāo)偏置策略，兩棵生長(zhǎng)樹(shù)都朝著各自的目標(biāo)擴(kuò)展，在保證路徑成本達(dá)到相對(duì)最小的同時(shí)提高了算法的規(guī)劃效率，擴(kuò)展節(jié)點(diǎn)明顯減少，路徑也變得更加平滑。

從表1的地圖2實(shí)驗(yàn)數(shù)據(jù)中可以看出:MOBDB-RRT*算法的平均擴(kuò)展節(jié)點(diǎn)數(shù)為63，比RRT*算法減少96.64%，比B-RRT*算法減少57.72%；平均規(guī)劃時(shí)長(zhǎng)為0.587 8 s，比RRT*算法縮短96.27%，比B-RRT*算法縮短64.51%；平均路徑長(zhǎng)度659.347 6 dm，比B-RRT*算法減少2.29%。

圖7為單通道障礙物環(huán)境(地圖3)規(guī)劃對(duì)比圖，起點(diǎn)為(30 dm,50 dm)，終點(diǎn)為(370 dm,350 dm)，地圖特點(diǎn)是從起點(diǎn)到終點(diǎn)需要多次轉(zhuǎn)折，偏向目標(biāo)較為困難。

圖7 單通道障礙物環(huán)境規(guī)劃對(duì)比圖Fig.7 Comparison of path planning in an environment with single channel obstacles

RRT*算法除了上述提到的規(guī)劃速度慢之外，在障礙物較為稀疏的環(huán)境中易出現(xiàn)冗余節(jié)點(diǎn)，路徑折線較多;B-RRT*算法規(guī)劃的兩端路徑較優(yōu)，但隨機(jī)擴(kuò)展特性使節(jié)點(diǎn)的利用率不高;而MOBDB-RRT*算法在偏向困難的障礙物環(huán)境中，依然能夠以較快的速度找到一條光滑的安全無(wú)碰撞路徑。

從表1的地圖3實(shí)驗(yàn)數(shù)據(jù)中可以看出:MOBDB-RRT*算法的平均擴(kuò)展節(jié)點(diǎn)數(shù)為272，比RRT*算法減少81.03%，比B-RRT*算法減少47.18%；平均規(guī)劃時(shí)長(zhǎng)為3.547 2 s，比RRT*算法縮短70.56%，比B-RRT*算法縮短48.22%；由于3種算法在圖7這種單通道障礙物環(huán)境中探索方式相似，因此平均路徑長(zhǎng)度相差不大，其次MOBDB-RRT*算法設(shè)置了安全運(yùn)行距離，所以會(huì)比B-RRT*算法規(guī)劃的路徑要長(zhǎng)一點(diǎn)。

根據(jù)上述的仿真圖和實(shí)驗(yàn)數(shù)據(jù)可以明顯看出，RRT*算法雖然在路徑長(zhǎng)度上占有優(yōu)勢(shì)，但是時(shí)間耗費(fèi)非常大，而B(niǎo)-RRT*算法雖然能在較短的時(shí)間內(nèi)找到路徑，但隨機(jī)擴(kuò)展導(dǎo)致無(wú)效節(jié)點(diǎn)較多，而且在有多條可行路徑的障礙物環(huán)境中路徑成本相對(duì)較大。除了規(guī)劃時(shí)間和路徑成本外，這兩種算法規(guī)劃的路徑不光滑且靠近障礙物，機(jī)器人運(yùn)行的安全性無(wú)法得到保障。本文所提出的MOBDB-RRT*算法在3種復(fù)雜的障礙物環(huán)境中，規(guī)劃效率更高、路徑更光滑且更安全。

4 結(jié)束語(yǔ)

本文在現(xiàn)有的RRT*算法的基礎(chǔ)上提出了MOBDB-RRT*算法，該算法所設(shè)置的膨脹化障礙物，為機(jī)器人留出安全的運(yùn)行距離，保證了路徑的可行性。通過(guò)引入雙向動(dòng)態(tài)目標(biāo)偏置的思想，加快了隨機(jī)樹(shù)的生長(zhǎng)速度，提高了算法的規(guī)劃效率，大大縮短了搜索路徑的時(shí)間。結(jié)合修剪算法和三次貝塞爾曲線對(duì)規(guī)劃好的路徑進(jìn)行優(yōu)化處理，去除冗余節(jié)點(diǎn)的同時(shí)使路徑更加光滑。通過(guò)與RRT*算法和B-RRT*算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比，證明了本文提出的MOBDB-RRT*算法在路徑規(guī)劃效率和路徑質(zhì)量上更具優(yōu)越性，該算法對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃具有一定的參考意義。