基于Fast Marching方法的多目標點路徑規(guī)劃的研究

于暉+王永驥

摘要：目前，水下自主機器魚已經(jīng)被應用于對水域多個目標點依次進行水質(zhì)監(jiān)測，因此有必要研究多個目標點的路徑規(guī)劃。針對遍歷多個目標點的路徑規(guī)劃問題，提出一種Multi-DirectionFastMarching（MDFM）方法和遺傳算法相結合的路徑規(guī)劃方法。該方法首先使用MDFM方法對工作站和多個目標點兩兩之間進行路徑規(guī)劃，然后使用遺傳算法規(guī)劃出遍歷所有點的最短路徑，最后通過仿真實驗驗證算法的可行性。

關鍵詞：機器魚；路徑規(guī)劃；多目標點；Fast Marching

中圖分類號：TP242.6 文獻標識碼：A

1引言

隨著當今社會城鎮(zhèn)化步伐的加快和區(qū)域經(jīng)濟的發(fā)展，水質(zhì)污染狀況日趨嚴重，給自然環(huán)境和人類身體健康造成嚴重的危害。對企業(yè)廢水、城鄉(xiāng)生活污水以及一些河流的定期水質(zhì)監(jiān)測成為我們當今環(huán)境保護的重要工作。目前我國的水質(zhì)監(jiān)測多采用人工操作和遠程監(jiān)測數(shù)據(jù)采集系統(tǒng)，監(jiān)測范圍難以滿足水質(zhì)監(jiān)測的需要。隨著仿生機器魚技術的日趨成熟和性能的不斷提高，其機動靈活，監(jiān)測范圍大，成本低，對水環(huán)境的影響小等優(yōu)勢使得將它用于水質(zhì)污染監(jiān)測有著得天獨厚的條件。Roberr Bogue對用于環(huán)境監(jiān)測的機器人的發(fā)展現(xiàn)狀進行了綜述，文章的討論包括了水下機器人，陸地機器人和空中機器人。英國牛津大學研究了一種水下移動機器人網(wǎng)絡，用來監(jiān)測核廢料蓄水池是否有核泄漏。2009年歐洲的SHOAL項目計劃開發(fā)一種機器魚編隊用于在港口或其他水域進行水質(zhì)監(jiān)測和污染源搜索。目前SHOAL已經(jīng)開發(fā)成功并且投產(chǎn)。在國內(nèi)，華中科技大學研究了一種用于水質(zhì)監(jiān)測的自主式水下機器魚（Autono-mousUnderwaterRoboticFish，AURF），可以在復雜的水下環(huán)境進行巡游，檢測水質(zhì)并繪制3D的水質(zhì)分析圖，并且可以對水域中的多個排污口依次進行水質(zhì)監(jiān)測。

在移動機器人相關技術的研究中，導航技術是其核心，而路徑規(guī)劃是自主式移動機器人導航的基本環(huán)節(jié)之一。在移動機器人的導航中，大多數(shù)方法都是使用柵格法對機器人工作環(huán)境進行分解。每個網(wǎng)格單元可以用二值信息來描述環(huán)境（障礙物或自由空間），也可以用一個相關的權值來表示穿過這個區(qū)域的代價值。A*算法是非常普及的基于柵格的路徑規(guī)劃方法。它在求解從起始位置到目標位置最短路徑時進行啟發(fā)式的搜索，所以它對從單個點到目標點進行路徑規(guī)劃是非常有效的?；跂鸥穹ǖ乃阉魉惴ㄔ陔x散的網(wǎng)格空間中一般會使用4元或者8元的后繼節(jié)點，所以規(guī)劃后得到的路徑會出現(xiàn)急轉(zhuǎn)彎，并且會限制移動機器人在運動的過程中只能以π/2或π/4的整數(shù)倍進行轉(zhuǎn)向。當移動機器人沿著規(guī)劃好的路徑運動，它會停止，改變下一步的航向，然后再加速。這樣會浪費移動機器人的燃料和時問。所以通過這些方法得到的路徑是一條次優(yōu)路徑。Mihail提出一種向量A*算法，它是A*算法的一種擴展算法。向量A*算法的后繼節(jié)點有16個，其規(guī)劃后的路徑可以讓移動機器人的航向角分辨率達到22.5°，提高了算法的最優(yōu)性和完備性。Wu針對無人機的運動規(guī)劃提出了Multi-StepA*（MSA*）算法，不僅其規(guī)劃出的路徑提高了移動機器人航向角的分辨率，而且減小了算法的復雜度，提高了算法的運算效率。但是向量A*算法和MSA*算法得到的還是一組離散的解，其不能收斂到平滑連續(xù)解，并且規(guī)劃后的路徑仍然會產(chǎn)生急轉(zhuǎn)彎。

目前對于移動機器人路徑規(guī)劃的研究都集中于兩點之間最優(yōu)路徑搜索的情況，而用于多目標點路徑規(guī)劃的旅行商（TSP）問題是在已知兩兩點之問的最短路徑和路徑代價的基礎上，規(guī)劃出一條最短路徑，其能夠遍歷所有的目標點一次，然后回到出發(fā)點。根據(jù)AURF進行多目標點水質(zhì)監(jiān)測的任務，我們對路徑規(guī)劃提出了如下要求：1）AURF從操作站出發(fā)，遍歷所有的監(jiān)測點并獲取當?shù)氐乃|(zhì)數(shù)據(jù)后，返回操作站；2）工作環(huán)境已知，但操作站和所有監(jiān)測點兩兩之間的最優(yōu)路徑和路徑代價均未知，需要首先進行規(guī)劃；3）得到遍歷所有監(jiān)測點一次并回到操作站的最短路徑。與兩點間路徑規(guī)劃和TSP問題相比，多目標點的路徑規(guī)劃問題更為復雜。本文提出了一種Multi-DirectionFastMarching（MDFM）方法可以同時對多個目標點兩兩之間進行路徑規(guī)劃，并結合遺傳算法（GA）規(guī)劃出遍歷所有目標點的最短路徑。

2Fast Marching方法

所謂路徑規(guī)劃是指，在具有障礙物的環(huán)境中，移動機器人按照某一性能指標（如距離、時間、能量等）搜索一條從起始狀態(tài)到目標狀態(tài)的無碰、平滑的最優(yōu)或近似最優(yōu)路徑?；跂鸥穹ǖ穆窂揭?guī)劃方法通常分為兩步：探索過程，即建立整個地圖上每個網(wǎng)格的最優(yōu)代價值（距離函數(shù)）；開發(fā)過程，即通過所求解的最優(yōu)代價值，從目標點向起始點回溯形成最優(yōu)路徑。

探索過程與光波的傳播過程非常相似。假設在起始點有一個向四周發(fā)射光波的光源，光從光源到達目標點的路徑可以認為是路徑規(guī)劃的最優(yōu)路徑，所以我們可以根據(jù)光的傳播來建立距離函數(shù)。光在傳播的過程中，在某一個瞬時時刻所到達的所有點的軌跡稱為波前。計算初至時間T可以用來描述波傳播過程中的波前位置。考慮一個二維的光波傳播初至時間的問題：

t=T（x，y）

T（x₀，y₀）=0 （1）

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

將式（1）兩邊對t求導，則可以得到初至函數(shù)T（x，y）的梯度▽T和機器人速度f兩個向量的內(nèi)積：

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

Fast Marching（FM）方法是Sethian首先提出的用來進行圖像處理的一種解決波傳播問題的水平集方法。像大部分的柵格搜索算法一樣，F(xiàn)M算法的計算復雜度是O（Nlog（N）），其中N是工作空間中網(wǎng)格的數(shù)量。在進行FM方法的整個過程中，速度函數(shù)F始終不會改變符號，即波前總是朝著一個方向在擴散，也就是說，當F>0時，波前總是朝外在擴散；反之亦然。這說明波前只會經(jīng)過工作空間中的每個網(wǎng)格點一次。當沒有環(huán)境力的作用下，移動機器人可以沿著波傳播的方向（即▽T的方向）運動，所以F（x，y，n）=f（x，y）。本文假設AURF在執(zhí)行任務的過程中速度大小不變，則F與位置和方向無關（即F（x，y，n）=F=f），并且所規(guī)劃路徑的長度最小問題可以等價于時間最小問題。波傳播問題即可轉(zhuǎn)化為求解程函方程：

2.1

迎風策略

FM算法使用一階數(shù)值近似（前向差分算子和后向差分算子）來求解程函方程。假設一個方程T在網(wǎng)格點（i，j）的值為T_i，j=T（x_i，j），其中網(wǎng)格間距為h。在x方向上的前向差分算子為D_i，j^+x=（u_i+1-u_i，j）/h，后向差分算子為D_i，j^-x=（u_i，j-u_i-1，j）/h。在y方向上的前向和后向差分算子類似。

FM使用迎風策略來估計T（x，y）的梯度▽T

其中τ_i，j=τ（x_i，j）。

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

2.2Fast Marching算法的實現(xiàn)

FM算法的核心思想是使用迎風值來系統(tǒng)地構建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是按照升序排列。隊列頂部的元素代價值最小，其對應的網(wǎng)格點稱為trial。FM算法探索過程中的每一次迭代會將trial網(wǎng)格點由Open類型變成Dead類型，然后將它的鄰接點的代價值更新，并且將類型為Far的鄰接點變?yōu)镺pen。FM算法更新過程的細節(jié)請參考文獻。

3MDFM方法

目前，F(xiàn)M方法已經(jīng)直接用于解決移動機器人的路徑規(guī)劃問題，但是這些問題都是兩點之問的路徑規(guī)劃。而對于多點之問的路徑規(guī)劃問題，普通的Fast Marching算法需要逐一計算每對目標點之間的最優(yōu)路徑。本節(jié)提出一種新的算法，可以同時計算多目標點之間的最優(yōu)路徑，節(jié)約了計算時間。

考慮在區(qū)域Ω中，已知目標點集合O，解決多目標點路徑規(guī)劃問題的其中一個關鍵工作就是找到每對目標點之間的最優(yōu)路徑。在各向同性的工作環(huán)境中，任意兩個目標點之間的往返路徑（路徑的軌跡和代價）相同。本節(jié)提出的Multi-DirectionFastMarching（MDFM），可以在各項同性的環(huán)境下快速找到每對目標點之間的最優(yōu)路徑。

目前兩點之間最優(yōu)路徑搜索算法主要分為三類：前向路徑搜索算法，指從起始點x_s創(chuàng)建搜索樹，到達目標點結束搜索；后向路徑搜索算法，指從目標點x_g創(chuàng)建搜索樹，到達起始點結束搜索；雙向路徑搜索算法，指從起始點x_s和目標點x_g同時創(chuàng)建搜索樹，兩個搜索樹相遇時搜索結束。雙向Fast Marching（BDFM）方法是MDFM方法的一種特殊情況。MDFM算法的搜索方向從多個目標點開始，是一種同時在多個目標點對之間進行路徑規(guī)劃的算法。

3.1MDFM算法的執(zhí)行過程

假設在Ω中有M（M≥2）個目標點（O₁，O₂，…，O_M）構成的集合O，每個目標點x_i（1≤i≤M）對應Ω上的一個網(wǎng)格點。在計算所有目標點對之問的最優(yōu)路徑和路徑代價時，MDFM會在M個目標點上同時創(chuàng)建搜索樹，并使用Fast Marching方法進行搜索。當所有的搜索樹都相交時，搜索結束。

MDFM算法的執(zhí)行過程如下：

初始化：將每一個目標點對應的搜索樹初始化，而這些目標點為對應搜索樹所構建的搜索樹的起始點。

開始循環(huán)：

比較每個搜索樹上Fast Marching方法的tri-al節(jié)點的估計值，選擇估計值為最小的trial節(jié)點所對應的搜索樹繼續(xù)運行，而其它的搜索樹則掛起等待。

當目標點O_i對應的Fast Marching方法FMi構建的搜索樹tree_i與另一個目標點O_i對應的Fast Marching方法FM_j構建的搜索樹tree_i相交于節(jié)點x′，則O_i與O_j之間的路徑代價為x′在FM_i和FM_i距離函數(shù)值之和。

當FMi構建的搜索樹與所有其它的搜索樹都相交，則下一次循環(huán)時終止FM_i的所有操作。

當所有的搜索樹都相互相交時，終止循環(huán)。

當目標點Q_i與目標點Q_j的搜索樹相交于網(wǎng)格點x′，則Q_i和O_j之間的路徑代價值D=T_i（x′）+T_i（x′）（T_i（x′）表示x′在FM_i的距離函數(shù)值），最優(yōu)路徑可以通過x′分別向O_i和O_j回溯得到。

因為Fas tMarching方法的計算復雜度為O（Nlog（N）），所以MDFM算法的計算復雜度為O（M/2·N·logN），其中M是目標點總數(shù)，N是網(wǎng)格上的網(wǎng)格點總數(shù)。如果使用Fast Marching方法對單個目標點進行路徑規(guī)劃，那么需要使用M-1次才能夠得到每對目標點的最優(yōu)路徑，這種方法的總的計算復雜度為O（（M-1）·N·logN）。顯然，當M≥2時，MDFM算法比后一種方法的運行速度要快，并且隨著M的增大，MDFM算法的相對計算時間越小。

3.2MDFM算法的應用

MDFM算法分別被應用在兩個目標點和多個目標點的實例中。

1）兩個目標點：如圖1所示，從Start Pointl和Start Point2產(chǎn)生一個個前向波不斷向外擴散，在同一水平集上的點代價值相等。當擴散到x′處時，兩個起始點的前向波相交，算法結束。最后返回兩目標點之間路徑代價，并形成最優(yōu)路徑。

2）多個目標點：如圖2所示，從多個目標點同時產(chǎn)生前向波不斷向外擴散，當Start Pointi的前向波與其它幾個目標點的前向波都相交時，StartPointi停止算法迭代，不再向外擴散。當所有目標點的前向波都兩兩相交時，算法結束。返回兩兩目標點之間的路徑代價，并形成最優(yōu)路徑。

4多目標點的路徑規(guī)劃

通過上一節(jié)MDFM方法得到的結果，我們可以將多目標點的路徑規(guī)劃問題轉(zhuǎn)換為TSP問題。TSP問題的歷史很久，最早的描述是1759年歐拉研究的騎士周游問題。目前求解TSP問題的算法主要有經(jīng)典的確定性算法，包括動態(tài)規(guī)劃算法，分支界限法等，和現(xiàn)代流行的智能算法，包括遺傳算法，粒子群算法，蟻群算法和模擬退火算法等。由于這些算法各有優(yōu)缺點，許多學者通過將各種算法相結合進行混合優(yōu)化，取長補短。由于遺傳算法結構簡單，易于實現(xiàn)，能夠并行化，并且具有強魯棒性和全局搜索能力，本文選用遺傳算法來搜索能夠遍歷多目標點的最優(yōu)路徑。

遺傳算法是計算機科學人工智能領域中用于解決最優(yōu)化的一種搜索啟發(fā)式算法，是進化算法的一種。這種啟發(fā)式通常用來生成有用的解決方案來優(yōu)化和搜索問題。由于篇幅的限制，遺傳算法的詳細介紹請參考文獻。

4.1實驗的建立

在工作空間C（如圖7所示）中，AURF需要從工作站出發(fā)，遍歷所有10個目標點并進行水質(zhì)監(jiān)測，完成任務后返回工作站。多目標路徑規(guī)劃的任務就是找到一條能夠遍歷所有目標點并返回工作站的最短路徑。

4.2實驗結果

通過使用MDFM和GA結合的方法，我們得到了一條無碰平滑最優(yōu)的路徑，能夠遍歷所有10個目標點并返回工作站（如圖3）。在一個分辨率為400*400的工作空問中，整個算法所使用的時間為8.857s。

5結論

本文提出了一種MDFM方法和GA相結合的方法，用來解決多目標點路徑規(guī)劃的問題。其中MDFM方法是FM方法的一種擴展，它繼承了FM方法計算快速，所求的解可以收斂到連續(xù)解等優(yōu)點，并且在處理多目標點兩兩之間最優(yōu)路徑和路徑代價時計算時間復雜度小。因此在AURF執(zhí)行任務的過程中，使用MDFM方法也可以進行在線路徑的重規(guī)劃。