基于混合搜索快速步進算法的移動機器人路徑規劃研究

于暉

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

【關鍵詞】移動機器人；路徑規劃；快速步進法；混合搜索

【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 引言

所謂路徑規劃是指，在具有障礙物的環境中，移動機器人按照某一性能指標（如距離、時間、能量等）搜索一條從起始狀態到目標狀態的無碰、平滑的最優或近似最優路徑[1]。路徑規劃是移動機器人導航中必不可少的一個重要環節。所規劃的路徑質量對移動機器人能夠安全航行和成功完成任務起到了至關重要的作用。

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

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

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

1 Fast Marching方法

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

FM算法是一種連續的廣度優先算法。與傳統的廣度優先算法相同，FM算法的路徑規劃過程也分為兩步：探索過程，即建立整個地圖上每個網格頂點的距離函數值；開發過程，即通過所求解到的最優代價值，通過梯度下降法從目標點向起始點回溯形成最優路徑。

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

t=T（x，y）

T（x，y）=0（1）

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

將式（1）兩邊對t求導，可以得到：

1=·+·（2）

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

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

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

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

T==（4）

T（x，y）=0

1.1 迎風策略

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

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

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

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

Sethian[9]已經證明了這種數值方法收斂于正確的連續解。

1.2 FM算法的實現

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

2 HSFM算法

對于移動機器人路徑規劃來說，計算時間和路徑的最優性都是我們需要考慮的評價標準。本小節提出了HSFM算法，能夠從一個離散的環境下得到一條連續平滑的最優路徑，并且在不損失計算精度的前提下提高了FM算法的實時性。

2.1 啟發式方法

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

FM算法與廣度優先搜索算法的優先級分配原則相同，算法不借助任何啟發信息。FM*[11]算法與啟發式搜索算法的優先級分配原則相同，FM*算法將歐幾里德啟發信息加入評價函數，從而可以縮短搜索過程的時間。

2.2 HSFM算法

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

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

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

3 HSFM算法應用實例

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

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

圖2 HSFM算法在路徑重規劃中的應用

【參考文獻】

[1]戴博，肖曉明，蔡自興.移動機器人路徑規劃技術的研究現狀與展望[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]李季，孫秀霞.基于改進A-Star算法的無人機航跡規劃算法研究[J].兵工學報，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.

[責任編輯：田吉捷]