基于方向選擇的移動機器人路徑規(guī)劃方法

吳 錚，陳彥杰，3+，何炳蔚，林立雄，王耀南

(1.福州大學(xué)機械工程與自動化學(xué)院，福建 福州 350108;2.湖南大學(xué)電氣與信息工程學(xué)院，湖南 長沙 410082;3.機器人視覺感知與控制技術(shù)國家工程實驗室，湖南 長沙 410082)

0 引言

移動機器人已廣泛應(yīng)用于生產(chǎn)生活的各個方面，如智能物料運輸[1]，無人駕駛汽車[2]、服務(wù)機器人等。在移動機器人運動過程中，路徑規(guī)劃是實現(xiàn)機器人自主導(dǎo)航的重要組成部分之一。路徑規(guī)劃的主要作用是為機器人提供一條從起始狀態(tài)到目標狀態(tài)的連續(xù)路徑[3]，并滿足一些任務(wù)需要所設(shè)定的性能指標，如時間、距離和能量等。移動機器人通過路徑規(guī)劃能夠?qū)崿F(xiàn)自主探索或擴展于多機協(xié)同應(yīng)用任務(wù)[4]。

基于采樣的運動規(guī)劃方法因能為機器人快速地提供路徑方案而廣受歡迎。其中，概率路線圖(Probabilistic Roadmaps，PRM)[5]和快速探索隨機樹(Rapidly exploring Random Trees，RRT)[6]是具有代表性的兩個方法。它們雖然計算效率較高，但是被證明無法提供最優(yōu)的路徑[7]。因此，研究人員在其基礎(chǔ)上提出了漸進最優(yōu)的規(guī)劃方法，如PRM*和RRT*及變種等[8-10]。其中，快速行進樹方法(Fast Marching Tree，F(xiàn)MT*)兼具了PRM*和RRT*的優(yōu)點，展現(xiàn)出良好的應(yīng)用前景[11]。但在環(huán)境復(fù)雜以及需要大量樣本來獲取最優(yōu)路徑的情況下，F(xiàn)MT*存在冗余探索的現(xiàn)象，導(dǎo)致計算效率低下。為了提高FMT*的探索效率，Starek等[12]提出了雙向擴展的FMT*方法(Bidirectional Fast Marching Tree，BFMT*)，它通過交替從起始狀態(tài)和目標狀態(tài)進行樹擴展來提升效率。但是，BFMT*仍然沒有改善樹擴展時的低效探索問題，導(dǎo)致冗余探索現(xiàn)象仍存在于擴展過程中。Salzman等[13]通過運動規(guī)劃下界(Motion Planning using Lower Bounds，MPLB)進行區(qū)域分析來排除多余點的探索;相似地，Xu等[14]在FMT*上應(yīng)用了Informed-RRT*[15]中的收縮橢圓區(qū)域思想(Informed Anytime Fast Marching Tree，IAFMT*)，通過空間約束避免了對整個空間的冗余探索。但是，MPLB和IAFMT*的空間約束劃分高度依賴于預(yù)處理和初始路徑的質(zhì)量，失當(dāng)?shù)念A(yù)處理和曲折的初始路徑均會導(dǎo)致區(qū)域劃分不合理和探索效率低下，極端情況下甚至無法找到質(zhì)量較高的路徑。Ichter等[16]使用圖形處理器(Graphics Processing Unit，GPU)來處理FMT*的擴展過程(Group Marching Tree，GMT*)，利用GPU 的并行處理能力對擴展樣本批量處理提高效率，該方法具有一定的使用門檻，并且與硬件性能高度相關(guān)。上述文獻大多從預(yù)處理或空間局部采樣的角度出發(fā)進行改進，但在擴展過程中仍對周圍所有樣本進行廣度搜索，計算效率具有一定的提升空間。啟發(fā)式函數(shù)可以對樣本的擴展情況進行評估，引導(dǎo)方法進行有目標性的深度探索。Gao等[17]將BFMT*與傳統(tǒng)A*方法結(jié)合形成雙向啟發(fā)式方法(Heuristic Bidirectional Fast Marching Tree，HBFMT*)，利用啟發(fā)式函數(shù)的代價計算方式，引導(dǎo)樣本擴展方向提高方法探索效率。然而，傳統(tǒng)啟發(fā)函數(shù)使用的樣本信息不夠充分，有時會影響計算效率。因此，通過對啟發(fā)函數(shù)進一步改進來改善規(guī)劃方法的探索性能也成為了研究熱點之一[18-19]。

為了解決FMT*在探索過程中由于廣度搜索導(dǎo)致的冗余探索問題，本文提出了基于方向選擇的快速行進樹方法(DS-FMT*)。該方法設(shè)計了一種方向選擇的新型啟發(fā)式函數(shù)，可根據(jù)周圍環(huán)境情況來構(gòu)造方向信息，以此來改變樣本的代價梯度變化。隨后，通過評估樣本代價向有利于探索的方向引導(dǎo)，減少了對冗余空間的探索。同時，借鑒了LBTRRT(Lower Bound Tree-RRT)[20]的近似漸進最優(yōu)性，將規(guī)劃終止條件設(shè)置為最優(yōu)解的倍以內(nèi)，實現(xiàn)保證良好規(guī)劃質(zhì)量的同時提升計算效率。對于移動機器人的應(yīng)用，本文選擇路徑長度作為規(guī)劃效果的度量指標。最后，通過仿真對比和移動機器人實際實驗，驗證了所提出的DS-FMT*方法的有效性。

1 問題描述

FMT*的遞歸擴展過程是對整個空間的所有樣本進行廣度搜索，過多不必要的樣本計算會導(dǎo)致規(guī)劃效率的降低。但是，F(xiàn)MT*的漸進最優(yōu)性則要求充足的樣本數(shù)量以確保良好的規(guī)劃質(zhì)量。從圖1的FMT*仿真結(jié)果可以證明，在不同樣本數(shù)量下，F(xiàn)MT*都對整個空間進行了廣度搜索。在圖1b中，為了獲得良好的規(guī)劃效果，F(xiàn)MT*需要至少5 000個樣本才能獲得圖示的結(jié)果。但是，圖中左邊空間通道的探索不僅不會提升最終規(guī)劃效果的質(zhì)量，還會降低規(guī)劃方法的計算效率。因此，若能引導(dǎo)樣本探索向有利于提升規(guī)劃效果的方向，將極大地提升規(guī)劃方法計算效率。

FMT*廣度搜索導(dǎo)致的冗余探索問題是因為樣本的代價梯度變化平緩，使得方法對代價相近的樣本都要進行計算。為了進一步分析FMT*產(chǎn)生冗余探索的原因，將FMT*結(jié)合傳統(tǒng)啟發(fā)式函數(shù)A*(FMT*-A)進行仿真，獲得擴展過程中樣本代價變化及其熱力圖表示。圖2 展示2000 樣本下的FMT*-A的規(guī)劃效果，圖中左上角為起始位置，右下角為目標位置，各虛線圈表示當(dāng)前探索過程和對應(yīng)樣本代價變化。在熱力圖中，不同顏色反映了樣本代價的大小，亮黃色表示代價最高，標記為未探索樣本;其余根據(jù)右側(cè)顏色條對應(yīng)，深藍色色表示代價最低，標記為起點。

在圖2a中，圈2通道分布的樣本不會提升方法的規(guī)劃質(zhì)量且消耗大量的計算量，但FMT*-A探索了該通道樣本。原因如圖2b所示，由圈3和圈4中的顏色分布可以看出，兩邊通道的代價變化相近，造成FMT*-A 對兩邊通道都進行了探索。在圖2c中，F(xiàn)MT*-A通過圈5中的樣本最終獲得了路徑結(jié)果，此時FMT*-A探索了整個地圖?？梢钥闯龅貓D左側(cè)通道并不影響規(guī)劃結(jié)果卻花費了計算時間，表明了FMT*-A規(guī)劃的樣本代價梯度變化平緩導(dǎo)致冗余探索。

綜上所述，通過修改樣本的代價梯度分布來引導(dǎo)樣本的探索方向，可以減少FMT*冗余探索的影響，能夠進一步提高FMT*的計算效率。

2 算法構(gòu)建和分析

為解決上述FMT*存在的問題，本文設(shè)計了DS-FMT*算法。本章首先解釋了DS-FMT*的整體框架，并改進了FMT*算法的擴展方式。隨后對所提方向選擇的新啟發(fā)函數(shù)進行闡述，最后對算法進行了理論分析。

2.1 DS-FMT*算法

DS-FMT*算法的整體結(jié)構(gòu)如算法1所示，主要使用以下函數(shù):①SampleFree(n)函數(shù)返回一組均勻分布的n個樣本;②Near(V，v，r)返回以v為中心，半徑為r的范圍內(nèi)的鄰居集合。③Expected-Direction(z)如算法2所述，根據(jù)周圍環(huán)境情況返回當(dāng)前的期望方向向量Ovector。④Find-Orientation(Ovector，xcurrent，xparent)函數(shù)將期望方向和當(dāng)前運動方向綜合考慮，最終得到樣本的期望方向成本Or(x);⑤F(x)，G(x)，H(x)函數(shù)和A*啟發(fā)函數(shù)定義一樣:G(x)表示由初始狀態(tài)到當(dāng)前點的實際成本，H(x)表示當(dāng)前點到目標位置的估算成本，通常采用歐幾里得直線距離計算，F(xiàn)(x)將兩者以及Or(x)結(jié)合，綜合評價當(dāng)前樣本的代價值;⑥Path函數(shù)在算法找到目標點時，根據(jù)擴展樹的連接情況返回路徑解。

算法1DS-FMT*。

受RRT*方法中樣本擴展方式的啟發(fā)，結(jié)合樣本代價分布，從計算復(fù)雜度的角度來設(shè)計DSFMT*擴展結(jié)構(gòu)?？紤]到FMT*的遞歸擴展方式，遍歷Xnear中的每個樣本都要經(jīng)過一次Near函數(shù)計算和碰撞檢測過程，花費的計算數(shù)量是O(log(n))，導(dǎo)致遍歷完Xnear的操作時間總計為O(nlog(n))。為了降低遞歸擴展的計算復(fù)雜度，舍去Xnear中的Near函數(shù)計算，使用樣本代價對比進行篩選，

若式(1)成立，從算法1的7～12行和式(1)可得，若Xnear中的樣本x代價小于經(jīng)過當(dāng)前點z到達該樣本的代價，則表明不需要修改樣本x的原來代價，可以跳過對x的碰撞檢測過程。通過樣本代價篩選使得遍歷Xnear的計算量降為O(n)，DS-FMT*的計算效率得到改善。

2.2 基于方向選擇的啟發(fā)式函數(shù)

為了改變探索中樣本的代價梯度來引導(dǎo)探索方向，DS-FMT*設(shè)計了一種基于方向選擇的啟發(fā)式函數(shù)，可以根據(jù)當(dāng)前樣本的環(huán)境信息估算樣本相應(yīng)的代價。主要流程如算法2的偽代碼所示。

算法2Expected-Direction(z)。

算法2主要返回一個期望的方向向量Ovector，如圖3所示。首先給定一個方向集合Pick-Direction(z，L)，以當(dāng)前點z為中心，向周圍發(fā)出均勻分布的方向線，并為每條線賦予固定值L。隨后，通過以下幾種信息的分析綜合考慮L的值:

(1)給定布爾函數(shù)Is-Unexplored-Area(xvector)，判斷該方向xvector是否屬于未探索區(qū)域方向。若是指向當(dāng)前點的父節(jié)點附近，則將不考慮該方向的后續(xù)處理，從而排除一些不必要的方向。

(2)Uncolliding-Length(xvector)函數(shù)是對當(dāng)前點的周圍障礙物進行碰撞檢測，并返回該方向能夠無碰撞的長度M1，作為方向選擇的參考信息，該值越大則代表該方向?qū)⒃狡x障礙物。

(3)布爾函數(shù)Is-Approach-Goal(xvector)判斷方向是否指向目標位置，若指向目標則為該方向加權(quán)值M2，反之則減去權(quán)值。最后綜合選取L最大的方向為期望方向向量Ovector。該期望方向?qū)⒂兄谝龑?dǎo)DS-FMT*向有利的方向探索。

隨后引入向量叉積修正法，標記為Find-Orientation(Ovector，xcurrent，xparent)函數(shù)，以獲得樣本在當(dāng)前探索下與期望方向偏離的計算成本Or(x)。首先計算當(dāng)前的探索方向和期望方向的偏差，構(gòu)造兩個方向向量，如圖4所示。DVector1定義為當(dāng)前點xcurrent的父節(jié)點xparent指向該點的向量，即接下來的實際探索方向，DVector2是由算法2得到的期望方向向量Ovector。將兩個方向向量求夾角OAngle，如式(2)所示:

夾角OAngle度量了路徑偏離期望方向的程度。通過判斷兩個方向是否一致，取正負固定值M3，與期望方向偏離小于45°則取正值，然后將M3賦予Or(x)。將Or(x)與當(dāng)前點到目標的預(yù)估成本H(x)結(jié)合，二者和的最大值小于單個預(yù)估成本H(x)的倍。Or(x)將改變樣本整體的代價梯度分布，將有利的樣本靠前排序，引導(dǎo)算法更傾向于擴展與期望方向一致的樣本，從而減少探索冗余。

2.3 DS-FMT*算法分析

通過DS-FMT*算法的結(jié)構(gòu)和原理，本節(jié)為算法提供了相應(yīng)的理論依據(jù)。

定理1概率完備性。給定一個路徑規(guī)劃問題，若問題存在解，則當(dāng)算法的迭代次數(shù)無窮大時，找到可行解的概率為1，即

證明 基于以下幾個依據(jù):DS-FMT*最終生成一棵連續(xù)的樹，當(dāng)一個未知樣本被探測時，它將與樹中的樣本相連。樹的根部是從初始狀態(tài)開始生長的，而目標位置存在于采樣樣本集合中。當(dāng)規(guī)劃問題存在解時，則目標位置將被算法探測到，意味著在樹中存在一條可行路徑，它連接著初始狀態(tài)和目標位置。因此，當(dāng)樣本數(shù)均勻覆蓋所有空間時，樹找到一個可行解的概率接近1?；谏鲜稣擖c，表明DSFMT*算法具備概率完備性。

定理2近似漸進最優(yōu)性。令(χfree，xinit，χgoal)是d維空間中的路徑規(guī)劃問題。令σ*表示最優(yōu)路徑，c*表示其長度。cn表示DS-FMT*在n個樣本下返回的路徑長度，且算法需要滿足式(3)半徑:

式中:η為正常數(shù)，d表示空間維數(shù)，μ(χfree)表示無障礙空間的勒貝格測度(即體積或面積)，ζd表示在d維歐幾里得空間中單位球的體積。則當(dāng)樣本趨于無窮時，DS-FMT*找到倍最優(yōu)解的概率為1，即

證明DS-FMT*算法基于FMT*結(jié)構(gòu)建立，當(dāng)無方向信息時啟發(fā)式函數(shù)等于標準的A*算法。通過Likhachev 等[18]進行的論證，當(dāng)H(x)≤ωH*(x)時，可以使得算法找到最優(yōu)解的ω倍，其中H*(x)表示問題的實際代價。因此，使用可允許的啟發(fā)式方法得到的解決方案保證是有界的。DSFMT*算法采用的Or(x)是小于預(yù)估函數(shù)H(x)至少倍的值，算法找到的解的長度不大于最優(yōu)解長度的倍。因此，DS-FMT*算法可以得到近似漸進最優(yōu)的性能。

定理3計算復(fù)雜度?？紤]路徑規(guī)劃問題(χfree，xinit，χgoal)，樣本數(shù)為n，則計算一個解會占用O(nlog(n))的時間。同時，DS-FMT*算法會占用O(nlog(n))個空間。

證明由于DS-FMT*是基于FMT*算法的結(jié)構(gòu)建立，調(diào)用代價函數(shù)的次數(shù)是O(nlog(n))，碰撞檢測次數(shù)為O(n)。對求取鄰居點集也是考慮O(nlog(n))時間復(fù)雜度。DS-FMT*算法將FMT*算法中對Ynear的計算用重布線方式替代，其復(fù)雜度從O(nlog(n))降為O(n)。對啟發(fā)式函數(shù)的構(gòu)建過程復(fù)雜度為O(n)，算法中其他部分復(fù)雜度均小于等于O(nlog(n))。根據(jù)大O法則，DS-FMT*的計算復(fù)雜度為O(nlog(n))。由于算法是邊構(gòu)建圖邊查詢的，沒有獨立的構(gòu)圖和查詢階段，DS-FMT*具有O(nlog(n))空間復(fù)雜性。

3 仿真及結(jié)果分析

3.1 DS-FMT*原理對比

為了表示DS-FMT*算法通過方向選擇函數(shù)達到修改樣本代價梯度分布的目的，從而引導(dǎo)樣本的探索方向和減少冗余探索的效果。將DS-FMT*與圖2的FMT*-A在相同環(huán)境下使用不同樣本數(shù)進行仿真對比，每個樣本數(shù)進行5次仿真并將數(shù)據(jù)取平均值匯總在表1中。DS-FMT*的樣本代價變化同樣使用熱力圖表示。圖5展示2000樣本下DSFMT*方法的擴展過程圖和相對應(yīng)的樣本代價變化圖，圖中左上角為起始位置，右下角為目標位置，各虛線圈表示當(dāng)前探索過程和對應(yīng)樣本代價變化。

從圖5可以得到，由于圖5b的圈4樣本代價高于圈3樣本代價，導(dǎo)致DS-FMT*不再擴展圖5a的圈2部分，只擴展圈1中方向。因此，圖5c中圈5將直接擴展至目標，對應(yīng)圖5d圈6中的代價值逐漸遞減，表明該區(qū)域一直優(yōu)先擴展。從圖5c可以表明，DS-FMT*有效減少了探索冗余現(xiàn)象。

將圖5中的數(shù)據(jù)用表1表示，結(jié)合上述對算法的樣本變化分析可得，DS-FMT*通過改變原來的代價梯度分布，在同等樣本數(shù)下，使得探索的樣本數(shù)比FMT*算法少了至少一倍。從表1的時間對比可得，DS-FMT*比FMT*算法快了3，4倍左右，意味著探索樣本數(shù)的減少可以提升計算效率。另外，DS-FMT*的路徑質(zhì)量與FMT*所得解的比值大約在1.01左右，保持了良好的路徑質(zhì)量。綜上分析可得DS-FMT*通過有效地改變樣本代價梯度，減少了冗余探索次數(shù)。

表1 仿真結(jié)果

3.2 仿真對比實驗

為進一步驗證所提算法在效率上體現(xiàn)的快速性，將DS-FMT*算法分別與先進的算法(即FMT*、PRM*、RRT*算法)在各種場景中進行模擬仿真對比。為了確保仿真的有效性，每個環(huán)境均采用3種樣本數(shù)進行仿真。算法基于MATLAB R2016b編寫和仿真，采用的計算機配置為:Windows 10操作系統(tǒng)，處理器為i7-6700HQ，運行內(nèi)存為8 G。

對于所比較的算法而言，DS-FMT*與FMT*、RRT*和PRM*都使用了完全相同的子函數(shù)(例如，最近鄰搜索、碰撞檢查、數(shù)據(jù)處理等)來確保對比過程的公平。對于每個問題設(shè)置，這些仿真以10組為單位，在每組中運行相同數(shù)量的樣本，最終結(jié)果取平均值。樣本大小表示搜索樹RRT*的迭代數(shù)，以及路線圖DS-FMT*和FMT*、PRM*的自由空間樣本數(shù)。對于給定的算法，只有在至少50%成功地找到可行的解決方案時，才記錄為給定樣本數(shù)的一組模擬數(shù)據(jù)。

地圖環(huán)境如圖6所示，MAP 1～MAP 4均為靜態(tài)地圖。起始狀態(tài)和終止位置如圖中所示，圖中展示了最大樣本數(shù)下各算法的路徑效果。MAP 1地圖展示了兩種長度不同的路徑通道，需要算法辨別最優(yōu)路徑所在區(qū)域;MAP 2需要算法在多個障礙物中找到路徑;MAP 3屬于采樣算法的經(jīng)典陷阱問題，需要算法快速找到路徑逃離陷阱。MAP 4是迷宮環(huán)境，是路徑規(guī)劃的一個典型參照。

從圖6的MAP1可以看出，各方法都能夠找到最優(yōu)路徑所在區(qū)域。從表2數(shù)據(jù)可以看出，在不同樣本數(shù)下，DS-FMT*所消耗的搜索時間均小于其他規(guī)劃方法，是FMT*和PRM*計算速度的4～5倍，是RRT*速度的2倍左右。而在路徑長度方面，F(xiàn)MT*和PRM*獲得的路徑長度基本一致，并且規(guī)劃質(zhì)量最優(yōu);DS-FMT*獲得的路徑長度略差于PRM*，表明DS-FMT*所獲規(guī)劃質(zhì)量良好。地圖MAP2的仿真結(jié)果與MAP1類似，各方法均能夠在多障礙環(huán)境中找到路徑，DS-FMT*仍以超過其他方法幾倍的速度獲得同樣質(zhì)量級別的規(guī)劃路徑，并且，路徑質(zhì)量會隨樣本數(shù)增加而得到改善。

表2 各地圖仿真結(jié)果

續(xù)表2

觀察MAP 3可得，DS-FMT*比其他算法效率提高了10倍。雖然在樣本數(shù)較低時DS-FMT*得到的路徑質(zhì)量較差，但隨著樣本數(shù)的增加，它與PRM*的路徑之比接近1.01，說明DS-FMT*的路徑質(zhì)量得到改善，體現(xiàn)了其近似漸進最優(yōu)性的性能。對于陷阱問題，RRT*需要隨機樣本恰好位于通道口，才能向外探索，否則會被困于陷阱。因此，RRT*計算效率雖然良好，但改善路徑的迭代次數(shù)下降，路徑長度的平均值高于其他算法。而DSFMT*和FMT*、PRM*等路線圖算法基本不受該問題的影響，只要分布足夠的樣本時，他們都能夠成功地擴展，因此數(shù)據(jù)穩(wěn)定。MAP 4的迷宮環(huán)境情況與MAP 3類似，DS-FMT*和FMT*、PRM*的樣本只需均勻覆蓋整個地圖，就能得到路徑解。但RRT*需要超過比其他算法更多的樣本數(shù)來保證穿過迷宮，成功率不到50%，因此MAP 4仿真不考慮RRT*。通過數(shù)據(jù)比較，DS-FMT*仍以高效率優(yōu)于其他兩類，路徑質(zhì)量也得到了良好的保證。

為了進一步分析數(shù)據(jù)，用柱狀圖和折線圖直觀地表示算法的規(guī)劃趨勢。柱狀圖表示樣本數(shù)和時間的關(guān)系，折線圖表示樣本數(shù)和路徑長度的關(guān)系。圖7a～圖7d分別對應(yīng)4個地圖的算法結(jié)果。從4個圖的柱狀圖可以看出，F(xiàn)MT*和PRM*的探索時間的增長幅度大于其他算法，而DS-FMT*的增長幅度相對平穩(wěn)，表明DS-FMT*的運算速度優(yōu)于其他算法，因為其主要計算速度來自于執(zhí)行較少的碰撞檢查和采樣點的探索。從折線圖可以綜合得出，F(xiàn)MT*和PRM*的路徑質(zhì)量相接近，質(zhì)量隨樣本數(shù)增加而逐漸改善。DS-FMT*算法的收斂趨勢和二者類似，但在路徑質(zhì)量上會存在一定的偏差，意味著DS-FMT*能夠找到近似漸進最優(yōu)解，其解存在一定的上界。對于RRT*，需要足夠的迭代次數(shù)來改善路徑質(zhì)量，但由于仿真迭代數(shù)的限制，RRT*的路徑質(zhì)量低于其他算法。

綜上所述，DS-FMT*通過改變擴展方式和采用方向選擇的啟發(fā)式函數(shù)，為規(guī)劃問題提供了良好的路徑質(zhì)量的同時，有效地解決了FMT*的探索冗余問題，提升了FMT*的計算效率。

4 移動機器人實驗

4.1 仿真實驗

為檢驗DS-FMT*算法在移動機器人上的應(yīng)用效果，在ROS操作系統(tǒng)上對移動機器人Turtlebot2進行仿真實驗。采用DS-FMT*、FMT*和PRM*算法作為其規(guī)劃算法，算法基于C++編寫，并在rviz可視化界面上顯示DS-FMT*的路徑規(guī)劃結(jié)果，如圖8所示。

仿真結(jié)果如表3所示，使用規(guī)劃時間、路徑長度、探索點數(shù)作為衡量算法性能的指標。規(guī)劃算法均在10 000個采樣數(shù)下進行規(guī)劃，共進行10次實驗，取平均值進行數(shù)據(jù)分析。

從仿真結(jié)果可以看出，DS-FMT*算法比FMT*和PRM*運行速度快了至少2倍和9倍，探索點數(shù)也少了近2倍，表明本文算法有效地通過減少探索的數(shù)量來引導(dǎo)算法快速的指向路徑;而DSFMT*的路徑質(zhì)量與另外兩者質(zhì)量的比值為1.11和1.09，在當(dāng)前最優(yōu)情況下的倍以內(nèi)，表明所得路徑質(zhì)量良好。這些結(jié)果證明了DS-FMT*算法的快速性和近似漸進最優(yōu)性。

表3 Turtlebot2仿真結(jié)果

4.2 實際實驗

為進一步證明算法在實際環(huán)境下的有效性，在現(xiàn)實場景中進行移動機器人實驗。仍使用DSFMT*、FMT*和PRM*算法進行對比。實驗場景如圖9所示，地圖大小為10 m×10 m，機器人Turtlebot2需要從起點移動到目標位置，同時避開障礙物。

各個算法均在10 000個樣本分布下進行規(guī)劃，并進行5次實驗，取平均值對數(shù)據(jù)進行分析，實驗結(jié)果如表4所示。以運行時間和路程長度作為評價算法性能的標準。由表4各算法的運行時間對比可以看出，DS-FMT*比FMT*和PRM*的運行速度快了至少2倍和6倍，表明了DS-FMT*有效地通過降低計算復(fù)雜度來減少運行內(nèi)存開銷。對應(yīng)DSFMT*的探索點數(shù)比其他算法少，說明探索冗余的減少有利于提高算法的計算效率。表4的路程長度表明，DS-FMT*的路徑質(zhì)量在可接受的允許范圍內(nèi)[20]，符合算法的近似漸進最優(yōu)性。實驗結(jié)果表明，DS-FMT*算法有效提高了計算效率并能夠提供合適的路徑方案。

通過仿真和實際實驗，證明了本文設(shè)計的基于方向選擇的啟發(fā)式函數(shù)充分利用了周圍環(huán)境的信息來合理分布樣本的代價梯度，為算法提供了良好的擴展方向，引導(dǎo)算法快速探索到目標位置。所提算法DS-FMT*在性能上具有著良好的表現(xiàn)。

表4 Turtlebot2實驗結(jié)果

5 結(jié)束語

為解決FMT*規(guī)劃算法存在的探索冗余的缺陷，本文提出一種基于方向選擇的啟發(fā)式策略算法DS-FMT*。該算法改進的擴展方式可以降低計算復(fù)雜度，并減少程序運行的內(nèi)存開銷。設(shè)計的方向選擇啟發(fā)函數(shù)在搜索過程中，有效利用障礙物信息來選擇行進方向。通過改變樣本代價梯度的分布，引導(dǎo)算法探索并加快了算法的運行速度。算法仿真結(jié)果和移動機器人實驗表明，DS-FMT*算法具有高效的計算速度和良好的路徑質(zhì)量保證。

未來計劃將DS-FMT*擴展到動態(tài)環(huán)境，同時研究該算法的實時性，包括采樣策略、碰撞檢測等。另外，考慮到現(xiàn)實機器人的應(yīng)用范圍，將DS-FMT*延伸到高維空間也是一個挑戰(zhàn)。