李 偉，金世俊

（東南大學(xué)儀器科學(xué)與工程學(xué)院，南京 210096）

0 引言

路徑規(guī)劃是移動機器人導(dǎo)航的關(guān)鍵問題之一，目前移動機器人的路徑規(guī)劃算法主要分為兩類：基于地圖的全局路徑規(guī)劃和基于傳感器未知環(huán)境的局部路徑規(guī)劃。

全局路徑規(guī)劃有A*算法、D*算法、快速搜索隨機樹（Rapidly exploring Random Tree，RRT）算法等。A*算法、D*算法基于柵格地圖環(huán)境，可以獲取最優(yōu)路徑，但是算法的表現(xiàn)受地圖分辨率影響：地圖分辨率較小時，算法速度較快，但是對于環(huán)境信息無法完全表現(xiàn)；地圖分辨率較大時，對環(huán)境的模擬效果提高，但是算法運行時間極大增加。

局部路徑規(guī)劃有傳統(tǒng)的人工勢場（Artificial Potential Field，APF）法、粒子群算法、蟻群算法和遺傳算法等。人工勢場法容易實現(xiàn)，計算量小，路徑規(guī)劃實時性高，但存在局部最小值；粒子群算法智能的精度高、收斂快但易于陷入局部最優(yōu)；蟻群算法和遺傳算法易于獲取最優(yōu)路徑，不易陷入局部最優(yōu)但受初始條件影響較大。

RRT 算法［1］是眾多路徑規(guī)劃算法中一種基于采樣的算法，相較于A*算法、D*算法基于柵格地圖且地圖分辨率對于算法表現(xiàn)有較大的影響，它無需對環(huán)境進行結(jié)構(gòu)化建模，同時RRT 算法在高維空間路徑規(guī)劃也有出色的表現(xiàn)［2-3］。RRT 算法具有概率完備性的特征，無論在多么復(fù)雜的環(huán)境下，只要有路徑存在，在不限制采樣次數(shù)的條件下，RRT算法最終都會找到一條可行路徑。

RRT 將起點作為擴展樹的根節(jié)點，利用隨機采樣的方式擴展樹，一旦隨機樹觸及目標點或者隨機樹的某一葉子節(jié)點進入目標點一定范圍，則隨機樹立刻停止擴展，返回由起點到目標點的一條路徑。RRT-Connect［4］算法用于提高RRT 算法的搜索速度，采用由起點和目標點各建立一棵搜索樹，一旦兩棵樹相遇，則停止擴展，返回路徑。該類算法能夠在較短的時間內(nèi)找到一條無碰撞的可行路徑，但是由于采樣次數(shù)往往較少，所獲得的路徑具有較大的代價。

為了解決RRT類算法最終獲得路徑的非最優(yōu)性，Esposito等［5］和Karaman 等［6］基于快速搜索隨機樹的概率完備特征，提出了具有漸進最優(yōu)性的快速搜索隨機樹（Rapidly exploring Random Tree star，RRT*）算法，在RRT算法的基礎(chǔ)上加入了重新選擇父節(jié)點以及調(diào)整現(xiàn)有節(jié)點父節(jié)點的策略，使其具有漸近最優(yōu)的特點。RRT*-Connect 算法［7-9］同樣用于提高RRT*算法的搜索速度，采用起點和終點兩棵樹同時搜索的策略，提高算法的收斂速度。此類算法雖然可以獲得漸近最優(yōu)的路徑，但是其建立在對整個環(huán)境空間的隨機均勻采樣上，往往需要足夠多的采樣次數(shù)才能獲取較優(yōu)的路徑，實時性較差。

為了優(yōu)化算法的實時性，一些基于優(yōu)化采樣策略的算法被提出。P-RRT*（Potential RRT*）算法［10］在獲得隨機點時融入了人工勢場法，使得隨機點趨向目標點直到與障礙物碰撞或者到達目標點，以獲取新的采樣點，提升每次隨機采樣點對算法整體的效果，降低隨機點為冗余點的幾率，提高算法效率；但是最終生成的路徑往往沿著障礙物邊緣，無法獲取全局最優(yōu)路徑。國內(nèi)一些學(xué)者同樣也在RRT*算法的采樣過程中加入人工勢場的思想：文獻［11］提出了一種改進的RRT 路徑規(guī)劃算法，引入人工勢場法，改進隨機樹的生長角度，縮短路徑規(guī)劃時間；文獻［12］提出了PRRT-Connect（Potential RRTConnect）算法，該算法結(jié)合了人工勢場法和RRT-Connect 算法，優(yōu)化采樣和擴展策略，提高算法效率，很好地解決了復(fù)雜環(huán)境下路徑規(guī)劃問題；文獻［13］提出了一種改進RRT*路徑規(guī)劃算法，快速瞄準包含起點和終點的連通區(qū)域，采樣過程中加入人工勢場法的策略，實現(xiàn)偏向目標采樣，加速RRT*收斂。但是這些算法都只將人工勢場法與采樣過程結(jié)合，并沒有避免對整個環(huán)境空間均勻采樣。

啟發(fā)式RRT*（Informed RRT*，Informed-RRT*）算法［14-15］利用RRT 算法獲取一條非最優(yōu)的路徑，然后在這條路徑的基礎(chǔ)上采用啟發(fā)集合（Informed Set）采樣的方式縮小采樣范圍，提高采樣效率，但是由于RRT 算法的隨機性，獲取初始路徑的時間代價和路徑代價穩(wěn)定性都不高，基于其構(gòu)成的啟發(fā)集合較冗余，效率不高。

本文提出了一種結(jié)合人工勢場法和啟發(fā)采樣策略的快速獲取最優(yōu)路徑的RRT*（Potential Informed-RRT*，PI-RRT*）算法，該算法基于人工勢場法快速獲取初始路徑，基于啟發(fā)集合采樣縮小隨機均勻采樣范圍，該算法通過穩(wěn)定性較好的人工勢場法獲取原始的可行路徑，再以原始路徑構(gòu)建初始啟發(fā)集合，通過限定在啟發(fā)集合內(nèi)進行均勻采樣，并在算法進行過程中動態(tài)調(diào)整啟發(fā)集合范圍，提高采樣效率，降低冗余采樣次數(shù)?；贛atlab對PI-RRT*算法和已有算法進行仿真對比，實驗結(jié)果表明本文算法在采樣點數(shù)以及算法運行時間方面，較已有算法都有較明顯的改進。

1 背景知識

1.1 問題背景

將整個狀態(tài)空間定義為X=(0，1)d，Xobs表示障礙物空間，Xfree表示為整個狀態(tài)空間除去障礙物空間后剩下的無障礙空間，記為Xfree=XXobs。(Xfree，xinit，Xgoal)定義了一個路徑規(guī)劃問題，其中xinit∈Xfree表示路徑規(guī)劃問題中的初始狀態(tài)，Xgoal?Xfree表示目標集合。令連續(xù)函數(shù)f：[0，1]?X表示從起點到目標點的一條路徑，則對于?τ∈[0，1]有f(τ) ∈Xfree。

針對以上路徑規(guī)劃問題，有以下兩個規(guī)定：

1）如果某路徑規(guī)劃算法，可以生成一條連續(xù)的路徑f，f與障礙物無碰撞，且滿足：

則稱此路徑為可行路徑，產(chǎn)生此路徑的算法為可行的路徑規(guī)劃算法。

2）令c(f)表示經(jīng)路徑f由起點xinit到目標點xgoal的代價函數(shù)（本文將路徑的歐幾里得距離作為路徑的代價），記f={x1，x2，…，xi，…，xn}，其中xi表示路徑上的第i個節(jié)點，所以有，使得代價函數(shù)c(f)取得最小值時的路徑f*為路徑規(guī)劃問題的最優(yōu)路徑。

1.2 基本的RRT*算法

RRT*算法的隨機樹擴展過程如圖1所示。

圖1 RRT*路徑規(guī)劃算法的擴展過程Fig.1 Expansion process of RRT*path planning algorithm

RRT*算法詳細步驟如下：

步驟1 初始化隨機樹G，包括隨機樹節(jié)點集合V，隨機樹邊集合E，并將路徑規(guī)劃起點xinit加入隨機樹節(jié)點集合V。

步驟2 判斷采樣次數(shù)是否達到上限n：如果達到，輸出最優(yōu)路徑，結(jié)束循環(huán)；如果沒有達到，進行步驟3。

步驟3 由隨機采樣函數(shù)Sample（）在狀態(tài)空間X中進行均勻隨機采樣獲得隨機點xrand。

步驟4 由函數(shù)Nearest（）獲取當(dāng)前隨機樹G中離xrand距離最小的點xnearest。

步驟5 函數(shù)Steer（）由xnearest開始向隨機點xrand擴展步長ε，生成新的節(jié)點xnew。

步驟6 利用Collision（）函數(shù)判斷xnearest到xnew的路徑是否與障礙物或者環(huán)境邊界發(fā)生碰撞：如果碰撞發(fā)生，則返回步驟2進行循環(huán)迭代；如果新的路徑?jīng)]有發(fā)生碰撞，執(zhí)行下一步。

步驟7 用函數(shù)Neighbors（）生成隨機樹G的節(jié)點集合V中位于新節(jié)點xnew一定范圍鄰域內(nèi)的節(jié)點集合Xnear。

步驟8 在Xnear內(nèi)利用chooseParent（）函數(shù)獲取離xnew路徑代價最小的節(jié)點作為xnew的新的父節(jié)點xparent。

步驟9 更新隨機樹G，將xnew加入隨機樹節(jié)點集合V，將xparent到xnew的路徑加入隨機樹邊集合E。

步驟10 在xnew鄰域Xnear內(nèi)重新遍歷所有節(jié)點，調(diào)整加入xnew后Xnear內(nèi)節(jié)點的最優(yōu)路徑，使得其中的節(jié)點路徑代價最小。

步驟11 返回步驟2 進行循環(huán)迭代，直到達到最大采樣次數(shù)，輸出全局最優(yōu)路徑。

RRT*路徑規(guī)劃算法的偽代碼如下所示：

2 PI-RRT*算法

RRT*路徑規(guī)劃算法在RRT 算法的基礎(chǔ)上降低了最終路徑的代價，但是往往實時性較差，原因在于其采樣是基于狀態(tài)空間X完全意義上的均勻采樣，所以狀態(tài)空間X中任意一個點被選中的概率都是相等的；然而移動機器人使其可行路徑往往只占空間中的很小一部分，這就導(dǎo)致大量冗余采樣點的存在，降低算法的效率。本文通過人工勢場法獲取環(huán)境中一條初始路徑，并利用這條路徑建立啟發(fā)集合，通過啟發(fā)集合內(nèi)的均勻采樣減少冗余采樣點，提高了RRT*路徑規(guī)劃算法的效率。

2.1 基于人工勢場法建立初始擴展路徑

人工勢場法的基本原理是將環(huán)境地圖建立為一個二維的虛擬力場，目標點xgoal產(chǎn)生引力場Uat，吸引機器人向目標點運動；同時障礙物Xobs在其一定范圍內(nèi)產(chǎn)生斥力場Ure，阻礙機器人向障礙物移動。引力場Uat和斥力場Ure共同形成整個環(huán)境中的虛擬力場Ures，虛擬力場作用于機器人上表現(xiàn)為機器人受到一個虛擬力Fres的驅(qū)動作用，F(xiàn)res的方向沿著機器人所在位置力場梯度下降方向，驅(qū)動機器人朝目標點運動［16-17］，大致過程如圖2所示。

圖2 人工勢場法下的路徑規(guī)劃Fig.2 Path planning under artificial potential field

綜上所述，人工勢場法具有目的性強的特點，且無需對環(huán)境進行建模，同時人工勢場法具有和RRT 算法相似的路徑擴展特征，因此本文選用人工勢場法生成初始路徑。在生成初始路徑的過程中，將人工勢場法的節(jié)點和路徑邊作為RRT 搜索樹的節(jié)點和路徑邊，由此構(gòu)成初始的RRT 搜索樹，有效地融合了人工勢場法和RRT 算法。另一方面，Informed-RRT*算法采用RRT 算法構(gòu)建初始路徑，RRT 算法存在隨機性強、獲取初始路徑所需采樣點數(shù)較多、所需時間及路徑代價不穩(wěn)定、魯棒性較差的缺點。因此，利用人工勢場法代替RRT 算法構(gòu)建初始路徑，穩(wěn)定性強，可以明顯地縮短Informed-RRT*算法生成初始路徑的時間以及減小初始路徑代價，縮小初始啟發(fā)采樣集合。

相較于傳統(tǒng)的分段人工勢場引力方程，本文采用單一距離平方正比關(guān)系的引力方程，主要是由于利用人工勢場算法直接進行機器人路徑規(guī)劃時，為了防止出現(xiàn)目標不可達的情況，人為地將目標點附近的引力減小；而本文僅僅利用人工勢場算法生成一條初始路徑，一旦路徑點進入目標點范圍，則直接結(jié)束算法。傳統(tǒng)的人工勢場法在路徑生成的過程中會出現(xiàn)路徑震蕩的問題，本文為了避免此問題的產(chǎn)生，利用RRT*中路徑重寫的思想，在人工勢場法生成路徑的過程中優(yōu)化路徑，解決初始路徑震蕩的問題。

目標點xgoal產(chǎn)生引力場Uat公式如下：

其中：Ka表示引力系數(shù)；d(x-xgoal)表示當(dāng)前位置x到目標點xgoal的距離。

障礙物Xobs產(chǎn)生的斥力場Ure公式如下：

其中：Kr表示斥力系數(shù)；dmin表示機器人距離障礙物最短的距離；x′表示Xobs中距離當(dāng)前位置最近的點；表示障礙物的斥力作用范圍。如果機器人和障礙物之間的距離超過，障礙物對機器人沒有斥力作用。

由F=-?U可以得到：

其中：

綜上，通過Fres=Fat+Fre計算出環(huán)境中某一位置所受到的虛擬勢場力大小。

基于人工勢場生成初始擴展路徑的算法Get-Potential-Path（）偽代碼如下所示。

由RRT和人工勢場法構(gòu)建的初始路徑如圖3所示。

圖3 兩種算法的初始路徑對比Fig.3 Comparison of initial paths of two algorithms

顯然，人工勢場法建立初始路徑所需的采樣點數(shù)以及最終的路徑長度明顯優(yōu)于RRT建立的初始路徑。

當(dāng)算法結(jié)束時，該算法返回初始路徑總代價ccurr，ccurr將作為初始參數(shù)用于建立啟發(fā)采樣集合。

2.2 建立動態(tài)啟發(fā)采樣集合

顯然，當(dāng)環(huán)境中存在一條路徑fcurr，則?τ∈[0，1]，有fcurr(τ) ∈Xfree，且滿足：

則稱fcurr為一條可行路徑。當(dāng)fcurr非最優(yōu)路徑f*時，一定?x∈Xfree，使得：

Xfree狀態(tài)空間中滿足式（7）的所有x的集合記作Ximp，Ximp∈Xfree，若限制RRT*算法在Ximp內(nèi)采樣，并通過父節(jié)點重選和路徑重寫，則能夠保證路徑趨向于最優(yōu)路徑。由于避免了對無效范圍的采樣，可以提高RRT*算法收斂到最優(yōu)路徑的速度，提高規(guī)劃效率。

在規(guī)劃過程中，由于環(huán)境中未探索點路徑代價無法提前獲取，所以Ximp僅僅是理想的采樣集合，在算法實現(xiàn)過程中做不到嚴格的Ximp內(nèi)采樣。與此同時，在二維平面中一定有式（8）成立，所以環(huán)境中某點x如果可以使得路徑代價減小，則一定滿足式（9）：

為Ximp的估計采樣集合，一定有，即滿足式（7）的所有采樣點一定屬于為啟發(fā)采樣集合。

顯然，滿足式（9）的所有狀態(tài)點構(gòu)成的集合是平面中的橢圓區(qū)域，分別以xinit和xgoal作為橢圓區(qū)域的左右焦點，長軸長度為當(dāng)前路徑代價c(fcurr)，短軸長度為：

其中cmin是xinit和xgoal之間的直線距離。

基于前文人工勢場法的初始路徑，建立初始啟發(fā)采樣集合，如圖4 所示，隨著路徑規(guī)劃算法的進行，當(dāng)前路徑代價c(fcurr)逐漸減小，橢圓區(qū)域在保持焦點不變的情況下，長短軸長度都會減小，最終導(dǎo)致橢圓區(qū)域收斂于一個極限狀態(tài)，即基于最優(yōu)路徑f*所構(gòu)建的啟發(fā)采樣集合。

圖4 啟發(fā)式采樣集合Fig.4 Informed sampling set

2.3 啟發(fā)采樣集合內(nèi)的均勻采樣

RRT*算法的概率完備性是建立在對采樣區(qū)域均勻采樣的基礎(chǔ)上，環(huán)境中各點作為采樣點的機會都是相同的。為了保持算法的理論有效性，需要對啟發(fā)采樣集合進行均勻采樣。

基于啟發(fā)采樣集合的均勻采樣方式主要有兩種：一是拒絕式采樣（Rejection Sampling），通過全環(huán)境的均勻采樣獲取隨機點，然后判斷隨機點是否在橢圓形啟發(fā)采樣集合內(nèi)，再做是否保留當(dāng)前隨機點的決定；二是通過在單位圓內(nèi)的均勻隨機采樣［18］，再利用坐標變換轉(zhuǎn)化為橢圓區(qū)域內(nèi)隨機點的方式，獲取隨機點。

拒絕式采樣的采樣方式仍然為全環(huán)境內(nèi)均勻采樣，在采樣后需要對獲取的隨機點加以判斷，判斷其是否在啟發(fā)采樣集合的范圍內(nèi)，然后取舍。由于啟發(fā)采樣集合只占整個環(huán)境的一小部分且隨著算法進行動態(tài)縮小，所以拒絕采樣方式下隨機點落于啟發(fā)集合內(nèi)的概率取決于啟發(fā)采樣集合占據(jù)整個環(huán)境的比例。顯然隨著算法的進行，想要獲取落于啟發(fā)采樣集合內(nèi)的隨機點則需要重復(fù)多次采樣。通過單位圓內(nèi)均勻采樣后轉(zhuǎn)化為啟發(fā)采樣集合內(nèi)的隨機點的方式則沒有多次重復(fù)采樣后取舍的問題，可以提高算法效率。所以本文采用第二種采樣策略。

設(shè)xcircle為以原點為圓心的單位圓內(nèi)的一個均勻采樣隨機點，即xcircle～U(Xcircle)，其中Xcircle={x∈X|‖x‖2≤1}，則有：

其中：xellipse～xcenter=(xinit+xgoal)/2 表示分別以xinit和xgoal作為左右焦點的橢圓形啟發(fā)采樣區(qū)域的中心；L表示由單位圓到橢圓采樣區(qū)域的線性變換矩陣；C表示由以啟發(fā)采樣區(qū)域兩焦點連線為橫軸的相對坐標系到世界坐標系的坐標變換矩陣。

取xcircle為單位圓Xcircle內(nèi)某點，令為單位圓到橢圓的線性變換，則有：

由式（11）得到：

再有xellipse=為橢圓啟發(fā)采樣區(qū)域的相對坐標系到絕對坐標系的坐標變換，其中式（14）為坐標變換矩陣：

其中：θ∈為xinit和xgoal所在直線和絕對坐標系橫軸的夾角。

綜合式（10）、（13）和（14），單位圓內(nèi)均勻采樣可以由式（15）轉(zhuǎn)化為橢圓形啟發(fā)采樣區(qū)域內(nèi)的均勻采樣。

啟發(fā)集合內(nèi)均勻采樣算法Informed-Set-Sampling（）的偽代碼如下所示：

3 仿真驗證與結(jié)果分析

根據(jù)上述算法設(shè)計，本文在Inter Core i5-9300H 2.40 GHz主頻PC 上采用Matlab 2020a 進行算法編程仿真測試。仿真基于40×40的地圖環(huán)境，模擬的是非結(jié)構(gòu)化環(huán)境，采用不規(guī)則多邊形模擬障礙物。在兩種不同的環(huán)境下，分別采用RRT*、Informed-RRT*以及本文提出的PI-RRT*算法獲取路徑。環(huán)境地圖以及獲取相同路徑代價時的規(guī)劃路徑如圖5～6所示。

圖5 仿真環(huán)境一中的算法表現(xiàn)Fig.5 Algorithm performance in the first simulation environment

兩種環(huán)境下起點坐標都為(-15，-15)，終點坐標都為（15，15），RRT*、Informed-RRT*以及本文提出的PI-RRT*算法在保證路徑代價相近的情況時，實驗結(jié)果如圖5～6 所示。兩種環(huán)境中獲取的路徑代價以及獲取對應(yīng)路徑所消耗的時間和所需采樣點數(shù)分別如表1～2所示。

圖6 仿真環(huán)境二中的算法表現(xiàn)Fig.6 Algorithm performance in the second simulation environment

表1 仿真環(huán)境一中的實驗結(jié)果Tab.1 Experimental results in the first simulation environment

表2 仿真環(huán)境二中的實驗結(jié)果Tab.2 Experimental results in the second simulation environment

由各算法的實驗結(jié)果可知，在不同環(huán)境中，分別獲取相同路徑代價所消耗的采樣次數(shù)以及算法執(zhí)行時間，PI-RRT*相較于RRT*，采樣點數(shù)減少約67%，算法運行時間平均縮短約74.5%；相較于Informed-RRT*，采樣點數(shù)減少約40%～50%，算法運行時間平均縮短約62.5%。PI-RRT*算法表現(xiàn)明顯優(yōu)于RRT*以及Informed-RRT*算法。PI-RRT*算法由于采用了目標導(dǎo)向型的人工勢場算法生成初始路徑的策略，使得相對于Informed-RRT*算法由RRT 生成的初始路徑，具有較小的路徑代價，提高了收斂到最優(yōu)路徑的速度。

由圖7 分析可知，在相同的采樣次數(shù)下，本文的PI-RRT*路徑規(guī)劃算法得到的路徑相較于RRT*和Informed-RRT*算法獲取的路徑具有明顯小的路徑代價。

圖7 不同仿真環(huán)境各算法路徑代價隨采樣次數(shù)的變化曲線Fig.7 Curves of path costs of algorithms in different simulation environments varying with sampling number

圖8表明，當(dāng)算法運行時間相同時，RRT*算法和Informed-RRT*在算法開始的前一段時間表現(xiàn)較為接近，之后由于Informed-RRT*算法的局部采樣策略，使得路徑收斂到最優(yōu)路徑的速度較RRT*算法加快；而PI-RRT*算法又優(yōu)于Informed-RRT*算法的初始路徑，使得路徑可以更加快速地收斂到最優(yōu)路徑。

圖8 不同仿真環(huán)境各算法路徑代價隨時間變化的曲線Fig.8 Curves of path costs of algorithms in different simulation environments varying with time

4 結(jié)語

本文提出了基于人工勢場法和啟發(fā)采樣的最優(yōu)路徑收斂方法。該方法利用人工勢場法構(gòu)建初始路徑，繼而生成初始啟發(fā)采樣集合，限制RRT*算法于啟發(fā)式采樣集合內(nèi)進行均勻采樣，同時在算法運行的過程中計算現(xiàn)有最優(yōu)路徑的代價，調(diào)整啟發(fā)采樣集合的范圍，極大減少冗余采樣次數(shù)，加速路徑收斂。通過仿真實驗驗證，本文提出的PI-RRT*算法較RRT*算法以及Informed-RRT*算法性能上有很大的提升，可以提高最優(yōu)路徑的生成速度。

PI-RRT*路徑規(guī)劃算法也有不足的地方，雖然對于非結(jié)構(gòu)化道路具有良好的路徑收斂速度，但是在迷宮類的環(huán)境下，由于路徑拐點較多，最優(yōu)路徑代價本身較大，效果并不明顯，未來仍有改進的空間。