于暉

【摘 要】本文提出一種混合搜索快速步進(jìn)算法用于解決移動機器人的路徑規(guī)劃問題。該算法在不損失計算精度的前提下，提高了快速步進(jìn)算法的實時性。通過實驗表明了該算法可以生成一條連續(xù)、平滑、最優(yōu)的路徑，并且運行速度非?？?，可以進(jìn)行在線規(guī)劃。

【關(guān)鍵詞】移動機器人；路徑規(guī)劃；快速步進(jìn)法；混合搜索

【Abstract】In this paper，the Hybrid Search Fast Marching algorithm，which improves the real-time performance of Fast Marching algorithm without losing accuracy，was proposed to resolve mobile robot path planning problem.The simulation showed that the algorithm has the ability to find continuous smooth optimal paths，it rans fast， and it could work for online planning.

【Key words】Mobile robot；Path planning；Fast Marching method；Hybrid search

0 引言

所謂路徑規(guī)劃是指，在具有障礙物的環(huán)境中，移動機器人按照某一性能指標(biāo)（如距離、時間、能量等）搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的無碰、平滑的最優(yōu)或近似最優(yōu)路徑[1]。路徑規(guī)劃是移動機器人導(dǎo)航中必不可少的一個重要環(huán)節(jié)。所規(guī)劃的路徑質(zhì)量對移動機器人能夠安全航行和成功完成任務(wù)起到了至關(guān)重要的作用。

在移動機器人的導(dǎo)航中，大多數(shù)方法都是使用柵格法[2]對機器人工作環(huán)境進(jìn)行分解。每個網(wǎng)格單元可以用二值信息來描述環(huán)境（障礙物或自由空間），也可以用一個相關(guān)的權(quán)值來表示穿過這個區(qū)域的代價值。A*算法[3]及其擴展算法D*算法[4]，改進(jìn)A*算法[5]和D*Lite算法[6]在移動機器人的路徑規(guī)劃中應(yīng)用廣泛，但是這些搜索算法在離散的網(wǎng)格空間中一般會使用4元或者8元的后繼節(jié)點，這樣會限制移動機器人在運動的過程中只能以？仔/2或？仔/4的整數(shù)倍進(jìn)行轉(zhuǎn)向。向量A*算法[7]和Multi-Step A*（MSA*）算法[8]提高了移動機器人航向角的分辨率，但是它們得到的還是一組離散的解，其不能收斂到平滑連續(xù)解，并且規(guī)劃后的路徑仍然會產(chǎn)生急轉(zhuǎn)彎。所以通過這些方法得到的是一條次優(yōu)路徑。

與上述基于柵格的路徑規(guī)劃算法相比，雖然本文使用的快速步進(jìn)（FM）方法也是在離散的網(wǎng)格單元上計算最優(yōu)路徑，但是它使用一階數(shù)值近似來求解程函方程，所以能夠得到一條平滑、連續(xù)、最優(yōu)的路徑解。

本文提出了一種混合搜索快速步進(jìn)（HSFM）算法，其在不損失計算精度的前提下提高了FM算法的實時性。本文結(jié)構(gòu)如下：首先介紹FM方法的原理。然后提出一種新的算法——基于混合搜索的快速步進(jìn)算法。最后給出新算法在路徑規(guī)劃中的應(yīng)用實例。

1 Fast Marching方法

FM方法是由Sethian首先提出的用來進(jìn)行圖像處理的一種解決波傳播問題的水平集方法[9]。像大部分基于網(wǎng)格的搜索算法一樣，F(xiàn)M算法的計算復(fù)雜度是O（N log（N）），其中N是工作空間中網(wǎng)格的數(shù)量。

FM算法是一種連續(xù)的廣度優(yōu)先算法。與傳統(tǒng)的廣度優(yōu)先算法相同，F(xiàn)M算法的路徑規(guī)劃過程也分為兩步：探索過程，即建立整個地圖上每個網(wǎng)格頂點的距離函數(shù)值；開發(fā)過程，即通過所求解到的最優(yōu)代價值，通過梯度下降法從目標(biāo)點向起始點回溯形成最優(yōu)路徑。

在自然界中，有一種物理現(xiàn)象與FM算法的探索過程非常相似：光的傳播。假設(shè)在起始點有一個向四周發(fā)射光波的光源，那么光從起始點向目標(biāo)點傳播的路徑（即光線軌跡）可以認(rèn)為是機器人路徑規(guī)劃生成的最優(yōu)路徑。因此我們可以根據(jù)光的傳播現(xiàn)象來建立距離函數(shù)。光在傳播的過程中，在某一個瞬時時刻所到達(dá)的所有點的軌跡稱為波前（形成光波的等相面）計算初至?xí)r間T可以用來描述波傳播過程中的波前位置?？紤]一個二維的光波傳播初至?xí)r間的問題：

t=T（x，y）

T（x，y）=0（1）

其中，T（x，y）表示在位置（x，y）的初至?xí)r間，（x0，y0）是初始位置。

將式（1）兩邊對t求導(dǎo)，可以得到：

1=·+·（2）

則我們可以將式（2）轉(zhuǎn)換成初至函數(shù)T（x，y）的梯度T和機器人速度f兩個向量的內(nèi)積：

=1？圯T·F（x，y，n）=1（3）

其中，n=T/T表示T的等值面在所計算點的向外法向量，F(xiàn)稱為速度函數(shù)，表示T（x，y）沿著梯度T方向擴散的速度。

假設(shè)移動機器人沿著波傳播的方向（即T的方向）運動，并且速度大小不變，則F與位置和方向無關(guān)（即F（x，y，n）=F=f），并且所規(guī)劃路徑的長度最小問題可以等價于時間最小問題。波傳播問題即可轉(zhuǎn)化為求解程函方程：

T==（4）

T（x，y）=0

1.1 迎風(fēng)策略

FM方法使用迎風(fēng)策略來估計T（x，y）的梯度T的模：

Ti，j2≈maxDT，-DT，0+maxDT，-DT，0

DT和DT分別為在x方向上的前向和后向差分算子。在y方向上的前向和后向差分算子類似。則程函方程（式4）可以近似的表達(dá)為：

maxDT，-DT，0+maxDT，-DT，0=1/F

Sethian[9]已經(jīng)證明了這種數(shù)值方法收斂于正確的連續(xù)解。

1.2 FM算法的實現(xiàn)

FM算法的核心思想是使用迎風(fēng)值來系統(tǒng)地構(gòu)建T的解，即前端的波只會由T值小的位置向T值更大的方向傳播。FM算法將網(wǎng)格點分成三種類型：Dead類型表示此網(wǎng)格點的T值已經(jīng)計算過并且確定；Open表示此網(wǎng)格點的T值是估計值，沒有確定；Far表示此網(wǎng)格點的T值是未知的。Open類型的所有點被存儲在一個稱為窄帶的優(yōu)先級隊列Q中，Q根據(jù)網(wǎng)格點的代價值T是按照升序排列。隊列頂部的元素代價值最小，其對應(yīng)的網(wǎng)格點稱為trial。FM算法探索過程中的每一次迭代會將trial網(wǎng)格點由Open類型變成Dead類型，然后將它的鄰接點的代價值更新，并且將類型為Far的鄰接點變?yōu)镺pen。FM算法更新過程的細(xì)節(jié)請參考文獻(xiàn)[9]。

2 HSFM算法

對于移動機器人路徑規(guī)劃來說，計算時間和路徑的最優(yōu)性都是我們需要考慮的評價標(biāo)準(zhǔn)。本小節(jié)提出了HSFM算法，能夠從一個離散的環(huán)境下得到一條連續(xù)平滑的最優(yōu)路徑，并且在不損失計算精度的前提下提高了FM算法的實時性。

2.1 啟發(fā)式方法

啟發(fā)式方法可以為網(wǎng)格搜索算法的每個節(jié)點提供較低的分叉率，因此使用啟發(fā)式方法的網(wǎng)格搜索算法擁有較佳的計算能力。按照窄帶優(yōu)先隊列Q優(yōu)先級的分配方法，我們將網(wǎng)格搜索算法分為三類：廣度優(yōu)先搜索算法、啟發(fā)式搜索算法和混合搜索算法[10]。

FM算法與廣度優(yōu)先搜索算法的優(yōu)先級分配原則相同，算法不借助任何啟發(fā)信息。FM*[11]算法與啟發(fā)式搜索算法的優(yōu)先級分配原則相同，F(xiàn)M*算法將歐幾里德啟發(fā)信息加入評價函數(shù)，從而可以縮短搜索過程的時間。

2.2 HSFM算法

HSFM算法與混合搜索算法的優(yōu)先級分配原則相同，即將Q中e（x）最小的元素設(shè)為優(yōu)先級最高，其中e（x）=？琢v（x）+（1-？琢）h（x，x），v（x）為距離函數(shù)在位置x的估計值，h（x，xgoal）為歐幾里德啟發(fā)函數(shù)，？琢是一個常數(shù)（0.5？燮？琢？燮1）。FM算法和FM*算法都是HSFM算法的一種特殊情況。當(dāng)？琢=0.5時，HSFM算法就是FM*算法。當(dāng)？琢=1時，HSFM算法就是FM算法。

為了得到不同？琢值對HSFM算法的計算時間和計算精度的影響，我們適用蒙特卡羅模擬法，對不同？琢值的HSFM算法在1000組隨機產(chǎn)生的三維環(huán)境下進(jìn)行仿真實驗。每組隨機產(chǎn)生的仿真模型包含了地形圖、運動物體、障礙物等環(huán)境信息。起始點和目標(biāo)點也是隨機的。三維環(huán)境的分辨率為200×200×40。

仿真結(jié)果如表1所示，其中FMn代表？琢=0.01×n時的HSFM算法?？梢奆M55的平均路徑代價近似與FM算法（即FM100）相等，而平均計算時間卻減少38%。本論文后面部分，HSFM算法指？琢=0.55的HSFM算法。所以我們得到以下結(jié)論：HSFM算法在不損失FM算法計算精度的前提下，大大提高了實時性，可以在環(huán)境發(fā)生改變或面對移動障礙物時進(jìn)行在線規(guī)劃。

3 HSFM算法應(yīng)用實例

我們首先給出了HSFM算法和FM算法在二維環(huán)境下的比較，如圖所示。網(wǎng)格的分辨率為400×400。FM算法的計算時間為382ms，HSFM算法計算的路徑代價為FM算法的1.0086倍，而計算時間為207ms，幾乎為FM算法的一半。

最后給出了HSFM算法在路徑重規(guī)劃方面的應(yīng)用實例。在仿真實驗中，假設(shè)自主水下機器人（AUV）的活動區(qū)域為5km×5km×40m，每個網(wǎng)格單元的大小為25m×25m×2m。AUV沿初始航路航行40min17s時，發(fā)現(xiàn)右側(cè)新出現(xiàn)一個AUV，其運動軌跡與初始路徑在110min左右有重疊區(qū)域，也就是說我們的AUV與新出現(xiàn)的AUV有發(fā)生碰撞的風(fēng)險。需要使用HSFM算法對AUV的路徑進(jìn)行重新規(guī)劃。規(guī)劃出的新路徑如圖2所示，計算時間為0.7628s?？梢?HSFM算法是理想的在線路徑規(guī)劃算法。

圖2 HSFM算法在路徑重規(guī)劃中的應(yīng)用

【參考文獻(xiàn)】

[1]戴博，肖曉明，蔡自興.移動機器人路徑規(guī)劃技術(shù)的研究現(xiàn)狀與展望[J].控制工程，2005（3）：198-202.

[2]Metea M，Tsai J.Route planning for intelligent autonomous land vehicles using hierarchical terrain representation[C]// Robotics and Automation Proceedings 1987 IEEE International Conference on：IEEE；1987：1947-52.

[3]LaValle SM.Planning algorithms[M].Cambridge university press，2006.

[4]Stentz A.The focussed D* algorithm for real-time replanning[C]//IJCAI；1995， 1652-9.

[5]李季，孫秀霞.基于改進(jìn)A-Star算法的無人機航跡規(guī)劃算法研究[J].兵工學(xué)報，2008（7）：788-92.

[6]Koenig S，Likhachev M.Fast replanning for navigation in unknown terrain[J]. Robotics，IEEE Transactions on，2005，21（3）：354-63.

[7]Pivtoraiko M，Kelly A.Generating near minimal spanning control sets for constrained motion planning in discrete state spaces[C]// Intelligent Robots and Systems，2005（IROS 2005） 2005 IEEE/RSJ International Conference on：IEEE； 2005，3231-7.

[8]Wu P-Y，Campbell D，Merz T.Multi-objective four-dimensional vehicle motion planning in large dynamic environments[J].Systems， Man， and Cybernetics，Part B： Cybernetics，IEEE Transactions on，2011，41（3）：621-34.

[9]Sethian J A.Level set methods and fast marching methods：evolving interfaces in computational geometry，uid mechanics，computer vision，and materials science[M] （volume 3）.U.K.：Cambridge university press，1999.

[10]LaValle S M.Planning algorithms，1st ed.U.K.：Cambridge university press， 2006.

[11]Petres C，Pailhas Y，Patron P，et al.Path planning for autonomous underwater vehicles[J].IEEE Transactions on Robotics，2007，23（2）：331-341.

[責(zé)任編輯：田吉捷]