基于混合搜索快速步進(jìn)算法的移動(dòng)機(jī)器人路徑規(guī)劃研究

于暉

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

【關(guān)鍵詞】移動(dòng)機(jī)器人；路徑規(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)境中，移動(dòng)機(jī)器人按照某一性能指標(biāo)（如距離、時(shí)間、能量等）搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的無碰、平滑的最優(yōu)或近似最優(yōu)路徑[1]。路徑規(guī)劃是移動(dòng)機(jī)器人導(dǎo)航中必不可少的一個(gè)重要環(huán)節(jié)。所規(guī)劃的路徑質(zhì)量對(duì)移動(dòng)機(jī)器人能夠安全航行和成功完成任務(wù)起到了至關(guān)重要的作用。

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

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

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

1 Fast Marching方法

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

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

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

t=T（x，y）

T（x，y）=0（1）

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

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

1=·+·（2）

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

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

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

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

T==（4）

T（x，y）=0

1.1 迎風(fēng)策略

FM方法使用迎風(fēng)策略來估計(jì)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算法的實(shí)現(xiàn)

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

2 HSFM算法

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

2.1 啟發(fā)式方法

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

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

2.2 HSFM算法

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

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

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

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

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

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

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

【參考文獻(xiàn)】

[1]戴博，肖曉明，蔡自興.移動(dòng)機(jī)器人路徑規(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算法的無人機(jī)航跡規(guī)劃算法研究[J].兵工學(xué)報(bào)，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é)任編輯：田吉捷]