于暉+王永驥

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

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

中圖分類號(hào)：TP242.6 文獻(xiàn)標(biāo)識(shí)碼：A

1引言

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

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

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

2Fast Marching方法

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

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

t=T（x，y）

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

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

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

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

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

2.1

迎風(fēng)策略

FM算法使用一階數(shù)值近似（前向差分算子和后向差分算子）來(lái)求解程函方程。假設(shè)一個(gè)方程T在網(wǎng)格點(diǎn)（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)策略來(lái)估計(jì)T（x，y）的梯度▽T

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

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

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

FM算法的核心思想是使用迎風(fēng)值來(lái)系統(tǒng)地構(gòu)建T的解，即前端的波只會(huì)由T值小的位置向T值更大的方向傳播。FM算法將網(wǎng)格點(diǎn)分成三種類型：Dead類型表示此網(wǎng)格點(diǎn)的T值已經(jīng)計(jì)算過(guò)并且確定；Open表示此網(wǎng)格點(diǎn)的T值是估計(jì)值，沒(méi)有確定；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算法探索過(guò)程中的每一次迭代會(huì)將trial網(wǎng)格點(diǎn)由Open類型變成Dead類型，然后將它的鄰接點(diǎn)的代價(jià)值更新，并且將類型為Far的鄰接點(diǎn)變?yōu)镺pen。FM算法更新過(guò)程的細(xì)節(jié)請(qǐng)參考文獻(xiàn)。

3MDFM方法

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

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

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

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

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

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

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

開(kāi)始循環(huán)：

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

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

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

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

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

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

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

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

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

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

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

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

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

4.1實(shí)驗(yàn)的建立

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

4.2實(shí)驗(yàn)結(jié)果

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

5結(jié)論

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