大范圍復雜戰場環境下自主式水下機器人多目標路徑規劃的研究

于暉+蘇延森

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

【關鍵詞】自主式水下機器人；路徑規劃；多目標；戰場；快速步進法。

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

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

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

1 三維空間Fast Marching方法

Fast Marching方法是一種基于離散網格的數值近似方法[8]。與傳統的基于離散網格的算法相同，Fast Marching算法的路徑規劃過程也分為兩步：探索過程，即建立整個地圖上每個網格頂點的距離函數值；開發過程，即通過所求解到的最優代價值，從目標點向起始點回溯形成最優路徑。

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

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

利用公式（2）計算每個網格單元距離函數值的一階近似估計之后，再通過從目標點的梯度下降方向進行回溯，形成一條從起始點到目標點的最優路徑。Fast Marching算法更新過程的細節請參考文獻[8]。

2 代價函數

本節討論AUV在三維大范圍復雜戰場水環境下執行軍事任務需要考慮的約束條件，以及通過這些約束條件構造Fast Marching算法的代價函數。

2.1 決策目標

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

（1）安全性目標：在 AUV 的運動規劃過程中，路徑的安全性是需要重點考慮的約束。在 AUV 執行軍事偵查任務時，在水雷區域航行和與靜態障礙物或動態航行器發生碰撞會影響AUV 的安全性。因此在仿真實驗中，安全目標由避碰規則和在水雷區域航行的風險性組成。

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

在 AUV 執行任務時要盡量避免在水雷密集的區域航行，以減小碰撞水雷的風險。雖然碰撞水雷不會直接造成人員的傷亡，但是這也意味著任務的失敗，并且有可能會暴露任務目標。在仿真實驗中，碰撞水雷風險性的費用函數用該區域內水雷的密度表示。

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

（3）任務效率目標：航路規劃也需要考慮任務本身效率目標的最優化。這些目標包括航行時間和燃料消耗。假設AUV單位時間的能耗是常數，也就是能量消耗是正比于航行時間，則能量消耗和航行時間兩個目標等價。因此在 AUV的任務效率費用矩陣中，障礙物所在區域為不可通過，其它區域的費用值反比于AUV穿越該區域的速度。

（4）可操控性約束：為了提高所規劃航路的可達性，需要考慮 AUV 的可操控性，包括最小轉彎半徑和安全深度。因此，所規劃路徑的曲率半徑的下確界必須大于 AUV 的最小轉彎半徑，并且所穿越區域的最大深度必須在安全深度之上。Fast Marching算法規劃的路徑已經非常平滑，一般可以滿足 AUV 對轉彎半徑的要求。如果所規劃的路徑曲率半徑的下確界沒有達到要求，可以使用均值濾波或增加補償的方式通過改變總費用函數來改善，詳細內容見文獻[10]。但是為了不改變總費用函數的值所代表的每個決策目標的意義，使用增加補償的方式更加合適。

2.2 代價函數矩陣

在AUV的航路規劃中，需要滿足并且最優化上述提到的所有決策目標，但是要使所有目標都達到最優是不可能的，因為某些目標就是矛盾的，比如安全性和到達時間。通常的做法是使用加權求和法或者模糊映射法。

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

最后，如果某些約束因素（例如地形、障礙物、禁航區域）是需要避免的，則對應的這些網格單元的總費用函數為∞。為了達到仿真的目的，假設所有的環境信息都是已知的。

3 仿真實驗

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

所有的仿真實驗都按照下面的步驟進行：

1）分別計算每個網格單元的碰撞風險費用Cr、水雷風險費用Mr、反偵察費用C和燃料消耗費用Fc。

2）根據任務要求通過經驗選擇每個決策目標的加權系數，并通過費用函數生成每個頂點的費用？棕。

3）選擇起始點和目標點。

4）通過Fast Marching算法計算每個網格單元的距離函數值，直到達到目標點停止。

5）通過從目標點距離函數梯度下降的方向進行回溯，直到到達起始點。最優路徑形成。

6）評價路徑質量。

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

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

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

當僅考慮 AUV 執行軍事任務的隱蔽性，也就是費用函數 W = C，生成了另一條最優路徑，如圖5和 圖3 所示。生成的路徑完全繞開了敵方聲納的探測區域。

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

需要注意，當任務需求發生改變，可通過修改費用的權重？棕i來生成一條滿足約束條件的最優路徑。

4 結論

本文將Fast Marching算法用于解決AUV在大范圍復雜戰場環境下的路徑規劃問題。在進行路徑規劃時考慮了安全性、隱蔽性、任務效率和可操控性四個決策目標。實驗結果表明，根據每個決策目標在任務中的優先級順序和它們對任務的影響程度建立合理的費用函數，可以規劃出滿足任務要求的最優路徑。

【參考文獻】

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

[責任編輯：田吉捷]