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

于暉+王永驥

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

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

中圖分類號：TP242.6 文獻標(biāo)識碼：A

1引言

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

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

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

2Fast Marching方法

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

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

t=T（x，y）

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

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

將式（1）兩邊對t求導(dǎo)，則可以得到初至函數(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算法的計算復(fù)雜度是O（Nlog（N）），其中N是工作空間中網(wǎng)格的數(shù)量。在進行FM方法的整個過程中，速度函數(shù)F始終不會改變符號，即波前總是朝著一個方向在擴散，也就是說，當(dāng)F>0時，波前總是朝外在擴散；反之亦然。這說明波前只會經(jīng)過工作空間中的每個網(wǎng)格點一次。當(dāng)沒有環(huán)境力的作用下，移動機器人可以沿著波傳播的方向（即▽T的方向）運動，所以F（x，y，n）=f（x，y）。本文假設(shè)AURF在執(zhí)行任務(wù)的過程中速度大小不變，則F與位置和方向無關(guān)（即F（x，y，n）=F=f），并且所規(guī)劃路徑的長度最小問題可以等價于時間最小問題。波傳播問題即可轉(zhuǎn)化為求解程函方程：

2.1

迎風(fēng)策略

FM算法使用一階數(shù)值近似（前向差分算子和后向差分算子）來求解程函方程。假設(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使用迎風(fēng)策略來估計T（x，y）的梯度▽T

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

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

2.2Fast Marching算法的實現(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é)請參考文獻。

3MDFM方法

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

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

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

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

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

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

初始化：將每一個目標(biāo)點對應(yīng)的搜索樹初始化，而這些目標(biāo)點為對應(yīng)搜索樹所構(gòu)建的搜索樹的起始點。

開始循環(huán)：

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

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

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

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

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

3.2MDFM算法的應(yīng)用

MDFM算法分別被應(yīng)用在兩個目標(biāo)點和多個目標(biāo)點的實例中。

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

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

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

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

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

4.1實驗的建立

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

4.2實驗結(jié)果

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

5結(jié)論

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