于暉+蘇延森

【摘 要】高質(zhì)量的規(guī)劃路徑對自主式水下機(jī)器人（AUV）安全航行和高效成功完成任務(wù)起到了至關(guān)重要的作用。本文使用Fast Marching算法來解決AUV在三維大范圍戰(zhàn)場環(huán)境下的路徑規(guī)劃問題，并且在規(guī)劃路徑時考慮了碰撞障礙物和水雷的風(fēng)險、被聲納發(fā)現(xiàn)的概率、路徑長度、AUV下潛深度和轉(zhuǎn)彎半徑等多個因素的影響。最后，在一組水下地形高程圖和聲納與水雷密度圖上通過仿真實(shí)驗(yàn)驗(yàn)證所提出的算法在大范圍戰(zhàn)場環(huán)境下進(jìn)行路徑規(guī)劃的可行性、安全性和高效性。

【關(guān)鍵詞】自主式水下機(jī)器人；路徑規(guī)劃；多目標(biāo)；戰(zhàn)場；快速步進(jìn)法。

【Abstract】Path planning plays an extremely important role in the operation of Autonomous Underwater Vehicles（AUVs） to accomplish the underwater task safely，successfully and efficiently.This paper introduces the Fast Marching method for the path planning of AUVs in 3D large-scale battlefield probability of being found， navigation security and low fuel consumption.The algorithm is able to find the optimal path between two waypoints in the work space and comprehensively takes factors such as the risk of collision with obstacles and mine，detection probability，sailing depth and path length into account.It also considers the maneuverability constraints of the AUV，including the safety depth and turning radius，to obtain the final flyable path.Finally，the proposed algorithm is tested in a large-scale area containing underwater terrain elevation map，sonar and mine density map to evaluate its feasibility， stability and efficiency.

【Key words】AUV；Path planning；Multi-objective；Battlefield；Fast Marching method

0 引言

AUV在軍事領(lǐng)域的應(yīng)用非常廣泛[1]，主要用于執(zhí)行水下偵查[2]、海底搜索和勘探[3-4]、導(dǎo)航援助[5]和潛艇的追蹤和跟蹤[6-7]等任務(wù)。對所有任務(wù)來說，路徑規(guī)劃是保證AUV能夠安全航行和成功完成任務(wù)的一個重要環(huán)節(jié)。AUV的路徑規(guī)劃是指，在特定的約束條件下（水下地形、動態(tài)障礙物、路徑長度、能源消耗等），找出一條從起始狀態(tài)到目標(biāo)狀態(tài)的無碰平滑最優(yōu)路徑。本文以AUV在大范圍復(fù)雜戰(zhàn)場水下環(huán)境下執(zhí)行軍事任務(wù)作為研究背景，所執(zhí)行的任務(wù)是在一些特定約束條件下，使用AUV航行去一個偏遠(yuǎn)位置執(zhí)行偵查任務(wù)。在制定執(zhí)行軍事偵查任務(wù)的運(yùn)動規(guī)劃中，需要考慮一組內(nèi)部約束條件（例如到達(dá)時間、燃料消耗、轉(zhuǎn)彎半徑、航行深度）和外部約束條件（地形、被敵方聲納探測到的概率、避開水雷）的限制。

Fast Marching方法是由Sethian首先提出的用來進(jìn)行圖像處理的一種解決波傳播問題的水平集方法[8]，已經(jīng)直接用于解決移動機(jī)器人的路徑規(guī)劃問題[9-11]，但是對于三維動態(tài)復(fù)雜水環(huán)境下AUV的路徑規(guī)劃，F(xiàn)ast Marching方法已有的研究還存在如下的問題：1）大多是在二維或者2.5維環(huán)境下進(jìn)行路徑規(guī)劃。2）環(huán)境建模簡單，障礙物很少并且為簡單、規(guī)則的形狀，而像水雷、敵方聲納等給 AUV執(zhí)行任務(wù)帶來威脅的因素都沒有考慮。3）沒有考慮 AUV下潛的安全深度和燃料消耗等約束，因此規(guī)劃出的路徑可能不可達(dá)。

本文將使用Fast Marching算法在三維大范圍復(fù)雜戰(zhàn)場水環(huán)境下對AUV進(jìn)行路徑規(guī)劃，并且考慮了安全性、隱蔽性、任務(wù)效率、可操控性等約束條件。主要結(jié)構(gòu)如下：首先介紹FM方法的基本原理，以及在三維環(huán)境下的應(yīng)用。然后介紹復(fù)雜戰(zhàn)場環(huán)境下各約束因素對AUV所執(zhí)行任務(wù)的影響，以及代價函數(shù)的建立。接下來根據(jù)不同的任務(wù)要求使用Fast Marching算法對AUV執(zhí)行任務(wù)的航路進(jìn)行規(guī)劃，并評價規(guī)劃結(jié)果。最后給出本文小結(jié)。

1 三維空間Fast Marching方法

Fast Marching方法是一種基于離散網(wǎng)格的數(shù)值近似方法[8]。與傳統(tǒng)的基于離散網(wǎng)格的算法相同，F(xiàn)ast Marching算法的路徑規(guī)劃過程也分為兩步：探索過程，即建立整個地圖上每個網(wǎng)格頂點(diǎn)的距離函數(shù)值；開發(fā)過程，即通過所求解到的最優(yōu)代價值，從目標(biāo)點(diǎn)向起始點(diǎn)回溯形成最優(yōu)路徑。

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

其中D-和D+分別表示后向和前向差分算子。

利用公式（2）計算每個網(wǎng)格單元距離函數(shù)值的一階近似估計之后，再通過從目標(biāo)點(diǎn)的梯度下降方向進(jìn)行回溯，形成一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑。Fast Marching算法更新過程的細(xì)節(jié)請參考文獻(xiàn)[8]。

2 代價函數(shù)

本節(jié)討論AUV在三維大范圍復(fù)雜戰(zhàn)場水環(huán)境下執(zhí)行軍事任務(wù)需要考慮的約束條件，以及通過這些約束條件構(gòu)造Fast Marching算法的代價函數(shù)。

2.1 決策目標(biāo)

在AUV執(zhí)行軍事偵察任務(wù)的路徑規(guī)劃中，需要考慮一組內(nèi)部約束條件（例如到達(dá)時間、燃料消耗、轉(zhuǎn)彎半徑、航行深度）和外部約束條件（地形、被敵方聲納探測到的概率、避開水雷）的限制。我們通過這些約束條件生成每個網(wǎng)格單元的費(fèi)用函數(shù)來表示 AUV 穿越該單元的難度。每一種約束都給出了潛在的數(shù)據(jù)來源、數(shù)據(jù)存儲結(jié)構(gòu)和對 AUV 運(yùn)動規(guī)劃的影響。針對所提出的任務(wù)背景，在對 AUV 進(jìn)行運(yùn)動規(guī)劃時主要考慮如下四個決策目標(biāo)：1）安全性；2）隱蔽性；3）任務(wù)效率；4）AUV的可操控性。

（1）安全性目標(biāo)：在 AUV 的運(yùn)動規(guī)劃過程中，路徑的安全性是需要重點(diǎn)考慮的約束。在 AUV 執(zhí)行軍事偵查任務(wù)時，在水雷區(qū)域航行和與靜態(tài)障礙物或動態(tài)航行器發(fā)生碰撞會影響AUV 的安全性。因此在仿真實(shí)驗(yàn)中，安全目標(biāo)由避碰規(guī)則和在水雷區(qū)域航行的風(fēng)險性組成。

避碰規(guī)則要求 AUV 在航行過程中與靜態(tài)障礙物和動態(tài)航行器之間保持安全距離。由于從導(dǎo)航傳感器、環(huán)境感知傳感器和環(huán)境電子圖所獲得的環(huán)境信息存在著不確定性和不精確性，這樣會造成 AUV 的定位和環(huán)境的信息存在誤差。這種不確定性會增加與障礙物碰撞的風(fēng)險，并且離障礙物越近的單元風(fēng)險越大。如果環(huán)境模型的單元尺寸很大，則這種不確定性可以被忽略。在仿真實(shí)驗(yàn)中，為了使所規(guī)劃的路徑與靜態(tài)障礙物保持一定的安全距離，可以通過生成費(fèi)用矩陣來表示與靜態(tài)障礙物的碰撞風(fēng)險。在距離靜態(tài)障礙物一定范圍內(nèi)的區(qū)域，其對應(yīng)的費(fèi)用矩陣的值與到靜態(tài)障礙物最短距離有關(guān)。離靜態(tài)障礙物越近，穿越的費(fèi)用越大；離靜態(tài)障礙物越遠(yuǎn)，穿越的費(fèi)用越小。而對于動態(tài)障礙物（比如其它執(zhí)行任務(wù)的AUV和船只），可以將其它 AUV 建模為一個動態(tài)的球形障礙物，將船只建模為一個動態(tài)圓柱形障礙物。

在 AUV 執(zhí)行任務(wù)時要盡量避免在水雷密集的區(qū)域航行，以減小碰撞水雷的風(fēng)險。雖然碰撞水雷不會直接造成人員的傷亡，但是這也意味著任務(wù)的失敗，并且有可能會暴露任務(wù)目標(biāo)。在仿真實(shí)驗(yàn)中，碰撞水雷風(fēng)險性的費(fèi)用函數(shù)用該區(qū)域內(nèi)水雷的密度表示。

（2）隱蔽性目標(biāo)：AUV 的隱蔽性是其在戰(zhàn)場環(huán)境中執(zhí)行任務(wù)的一個最重要的戰(zhàn)術(shù)指標(biāo)之一，而聲隱蔽是最重要的一個影響因素。如果不進(jìn)行聲隱蔽，AUV 有可能會被敵方聲納發(fā)現(xiàn)，從而會導(dǎo)致任務(wù)失敗，甚至不可估算的損失。因此，在 AUV 的航路規(guī)劃過程中需要考慮在執(zhí)行任務(wù)時被敵方聲納偵查到的概率。不同的聲納有不同的探測范圍。在仿真實(shí)驗(yàn)中，聲納的探測范圍用一個圓柱形表示，圓柱形內(nèi)部區(qū)域的費(fèi)用為其外部區(qū)域費(fèi)用的兩倍，而AUV被該聲納發(fā)現(xiàn)的概率可以用通過該威脅區(qū)域的路徑段長度表示[12]。

（3）任務(wù)效率目標(biāo)：航路規(guī)劃也需要考慮任務(wù)本身效率目標(biāo)的最優(yōu)化。這些目標(biāo)包括航行時間和燃料消耗。假設(shè)AUV單位時間的能耗是常數(shù)，也就是能量消耗是正比于航行時間，則能量消耗和航行時間兩個目標(biāo)等價。因此在 AUV的任務(wù)效率費(fèi)用矩陣中，障礙物所在區(qū)域?yàn)椴豢赏ㄟ^，其它區(qū)域的費(fèi)用值反比于AUV穿越該區(qū)域的速度。

（4）可操控性約束：為了提高所規(guī)劃航路的可達(dá)性，需要考慮 AUV 的可操控性，包括最小轉(zhuǎn)彎半徑和安全深度。因此，所規(guī)劃路徑的曲率半徑的下確界必須大于 AUV 的最小轉(zhuǎn)彎半徑，并且所穿越區(qū)域的最大深度必須在安全深度之上。Fast Marching算法規(guī)劃的路徑已經(jīng)非常平滑，一般可以滿足 AUV 對轉(zhuǎn)彎半徑的要求。如果所規(guī)劃的路徑曲率半徑的下確界沒有達(dá)到要求，可以使用均值濾波或增加補(bǔ)償?shù)姆绞酵ㄟ^改變總費(fèi)用函數(shù)來改善，詳細(xì)內(nèi)容見文獻(xiàn)[10]。但是為了不改變總費(fèi)用函數(shù)的值所代表的每個決策目標(biāo)的意義，使用增加補(bǔ)償?shù)姆绞礁雍线m。

2.2 代價函數(shù)矩陣

在AUV的航路規(guī)劃中，需要滿足并且最優(yōu)化上述提到的所有決策目標(biāo)，但是要使所有目標(biāo)都達(dá)到最優(yōu)是不可能的，因?yàn)槟承┠繕?biāo)就是矛盾的，比如安全性和到達(dá)時間。通常的做法是使用加權(quán)求和法或者模糊映射法。

碰撞風(fēng)險、水雷風(fēng)險、隱蔽性和燃料消耗等目標(biāo)分別用費(fèi)用矩陣Cr、Mr、C和Fc表示，并且全部進(jìn)行歸一化處理。這四個矩陣的階數(shù)與網(wǎng)格單元矩陣相同，每個元素代表環(huán)境模型上對應(yīng)網(wǎng)格單元的費(fèi)用函數(shù)。三維網(wǎng)格模型每個單元的總費(fèi)用函數(shù)W是通過加權(quán)求和法將這四個目標(biāo)融合在一起得到，則所生成的路徑代價為：

最后，如果某些約束因素（例如地形、障礙物、禁航區(qū)域）是需要避免的，則對應(yīng)的這些網(wǎng)格單元的總費(fèi)用函數(shù)為∞。為了達(dá)到仿真的目的，假設(shè)所有的環(huán)境信息都是已知的。

3 仿真實(shí)驗(yàn)

仿真模型包含了水底地形圖、水雷密度圖和敵方聲納探測范圍等信息。首先需要對水底地形環(huán)境進(jìn)行建模。本算法通過高度圖對水底地形環(huán)境進(jìn)行描述，如圖1所示。然后使用規(guī)則網(wǎng)格對整個搜索空間進(jìn)行建模，則其它環(huán)境信息（比如水雷密度圖和聲納探測區(qū)域）也可以通過網(wǎng)格單元表示。

所有的仿真實(shí)驗(yàn)都按照下面的步驟進(jìn)行：

1）分別計算每個網(wǎng)格單元的碰撞風(fēng)險費(fèi)用Cr、水雷風(fēng)險費(fèi)用Mr、反偵察費(fèi)用C和燃料消耗費(fèi)用Fc。

2）根據(jù)任務(wù)要求通過經(jīng)驗(yàn)選擇每個決策目標(biāo)的加權(quán)系數(shù)，并通過費(fèi)用函數(shù)生成每個頂點(diǎn)的費(fèi)用？棕。

3）選擇起始點(diǎn)和目標(biāo)點(diǎn)。

4）通過Fast Marching算法計算每個網(wǎng)格單元的距離函數(shù)值，直到達(dá)到目標(biāo)點(diǎn)停止。

5）通過從目標(biāo)點(diǎn)距離函數(shù)梯度下降的方向進(jìn)行回溯，直到到達(dá)起始點(diǎn)。最優(yōu)路徑形成。

6）評價路徑質(zhì)量。

下面根據(jù)不同任務(wù)要求改變總費(fèi)用函數(shù)W中加權(quán)系數(shù)？棕i的值，進(jìn)行了幾組路徑規(guī)劃的仿真實(shí)驗(yàn)。所有仿真實(shí)驗(yàn)使用的環(huán)境模型、水雷密度圖、聲納探測范圍、其它動態(tài)航行器的運(yùn)動狀態(tài)以及起始點(diǎn)、目標(biāo)點(diǎn)都是相同的。起始點(diǎn)和目標(biāo)點(diǎn)分別位于一個山脈的兩邊。圓柱形表示聲納的探測區(qū)域，軸心在水平坐標(biāo)（110NM，170NM）處，探測半徑為5NM ，探測高度為 200M。水雷密度圖使用顏色圖表示。

首先給出航行時間為最優(yōu)的路徑規(guī)劃仿真實(shí)驗(yàn)的結(jié)果。圖2和圖3給出了從起始點(diǎn)到目標(biāo)點(diǎn)航行時間最短的路徑。由圖可見，所生成的路徑越過山脈，穿越高密度水雷區(qū)域和敵方聲納探測區(qū)域，直接到達(dá)目標(biāo)點(diǎn)。該路徑的評價結(jié)果如表 1所示，其中Ttotal為路徑總航行時間，Lsonar表示在敵方聲納探測區(qū)域內(nèi)路徑段的長度，Lobs為路徑靠近障礙物的最短距離，Rmin表示路徑曲率半徑的下確界，Dmax表示整條路徑的最大深度。由表可見，該路徑離障礙物非常近，有很大的碰撞風(fēng)險，并且穿越聲納區(qū)域的路徑段很長，被敵方發(fā)現(xiàn)的概率很大。

如果任務(wù)的要求僅需要保障 AUV 的安全性，要求所生成的路徑盡可能的遠(yuǎn)離障礙物，并且要繞開高密度的水雷區(qū)域，則我們構(gòu)建總費(fèi)用函數(shù)為W=0.2*Cr+0.8*Mr。生成的安全性最優(yōu)的路徑如圖4和圖3所示。表1給出了該路徑的評價結(jié)果。與航行時間最優(yōu)的路徑相比較，所生成的路徑遠(yuǎn)離了障礙物，并且繞開了高密度水雷區(qū)域。

當(dāng)僅考慮 AUV 執(zhí)行軍事任務(wù)的隱蔽性，也就是費(fèi)用函數(shù) W = C，生成了另一條最優(yōu)路徑，如圖5和 圖3 所示。生成的路徑完全繞開了敵方聲納的探測區(qū)域。

最后，制定路徑規(guī)劃的目標(biāo)如下：Ttotal？燮13h，Lsonar？燮1NM，最大下潛深度Dmax<150M，曲率半徑的下確界Rmin？叟10M，在滿足上述要求前提下，盡量選擇與障礙物距離遠(yuǎn)和水雷密度較小的區(qū)域穿越。通過經(jīng)驗(yàn)設(shè)定費(fèi)用函數(shù)為W=0.1*Cr+0.5*Mr+0.1*C+0.4*Fc時，生成的最優(yōu)路徑如圖6和圖3所示。表1給出了該路徑的屬性參數(shù)。

需要注意，當(dāng)任務(wù)需求發(fā)生改變，可通過修改費(fèi)用的權(quán)重？棕i來生成一條滿足約束條件的最優(yōu)路徑。

4 結(jié)論

本文將Fast Marching算法用于解決AUV在大范圍復(fù)雜戰(zhàn)場環(huán)境下的路徑規(guī)劃問題。在進(jìn)行路徑規(guī)劃時考慮了安全性、隱蔽性、任務(wù)效率和可操控性四個決策目標(biāo)。實(shí)驗(yàn)結(jié)果表明，根據(jù)每個決策目標(biāo)在任務(wù)中的優(yōu)先級順序和它們對任務(wù)的影響程度建立合理的費(fèi)用函數(shù)，可以規(guī)劃出滿足任務(wù)要求的最優(yōu)路徑。

【參考文獻(xiàn)】

[1]Fletcher B.UUV master plan：a vision for navy UUV development.in： Proceedings of OCEANS 2000 MTS/IEEE Conference and Exhibition.IEEE，2000，65-71.

[2]Le Bouffant N，Pidsley P，Malkasse J P，et al.Automatic MCM mission control for AUV systems.in：Proceedings of Oceans 2005-Europe.IEEE，2005，930-936.

[3]Smith WH，Marks K M.Sea floor in the Malaysia Airlines Flight MH370 Search Area.Eos，Transactions American Geophysical Union，2014，95（21）：173-174.

[4]Kohut J，Roarty H，Licthenwalner S，et al.The Mid-Atlantic regional coastal ocean observing system：Serving coast guard needs in the Mid-Atlantic Bight.in： Proceedings of US/EU-Baltic International Symposium，2008 IEEE/OES.IEEE，2008，1-5.

[5]Freitag L E，Grund M，Partan J，et al.Multi-band acoustic modem for the communications and navigation aid AUV.in：Proceedings of OCEANS，2005. Proceedings of MTS/IEEE.IEEE，2005，1080-1085.

[6]Wernli R L.Low Cost UUVs for Military Applications：Is the Technology Ready？ Technical report，DTIC Document，2000.

[7]Nguyen C，Samuel R，Nguyen H，et al.Unmanned Systems Network-Centric Operations.Technical report，DTIC Document，2005.

[8]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.

[9]Petres，Clement and Pailhas， Yan and Patron，Pedro and Petillot，Yvan and Evans， Jonathan and Lane，David，“Path planning for autonomous underwater vehicles”， IEEE Transactions on Robotics，23（2）：331-341，2007.

[10]G′omez，Javier V and Lumbier，Alejandro and Garrido，Santiago and Moreno， Luis，“Planning robot formations with fast marching square including uncertainty conditions”，Robotics and Autonomous Systems，61（2）：137-152， 2013.

[11]Garrido，Santiago and Malfaz，Mar′ ？覦a and Blanco，Dolores，“Application of the fast marching method for outdoor motion planning in robotics”，Robotics and Autonomous Systems，61（2）：106-114，2013.

[12]Lin T ZH.Research on submarine counter-detection probability by threat surface ship sonar.Computer Science，2009.

[責(zé)任編輯：田吉捷]