USV 地形測量路徑自主規(guī)劃研究與應用

宋吉廣，張京晶，馮 亮，林 揚，李德隆，谷海濤

(1. 中國科學院沈陽自動化研究所 機器人學國家重點實驗室，遼寧 沈陽 110016；2. 中國科學院 機器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110169；3. 中國人民解放軍91053 部隊，北京 100070)

0 引 言

一直以來，近岸海域水上水下地形測量是海洋工程領域的一項重要內(nèi)容，對于海運安全、港口領航、維護國家主權、沿海城市及島礁的海洋開發(fā)、規(guī)劃決策等都具有重要意義[1]。近岸海域周圍具有環(huán)境復雜、缺乏先驗信息和海況惡劣等特點，而無人水面艇作為一種小型智能無人平臺，具備自主感知環(huán)境信息、路徑規(guī)劃、自主航行的能力，可通過人工預置或自主信息采集處理的方式完成環(huán)境感知、目標識別與探測、自主規(guī)劃等多種任務。近年來，相關研究人員針對不同的任務需求進行水運航道、水庫、島嶼等水域的水上水下地形測量。但在測量過程中，多以人工設定測量路徑的方式為主[2-5]，在測量區(qū)域較大或較為復雜的情況下，需耗費巨大人力、物力和財力，且布設測量路徑帶有人為主觀性，影響最終測量效果。

操文芷[6]使用電子海圖與雷達圖像疊加匹配方式，在此基礎上進行無人水面艇路徑規(guī)劃，可有效進行障礙物規(guī)避以及局部路徑規(guī)劃動任務。劉強等[7]采用船載多傳感器一體化測量技術對海島礁、港口地區(qū)與水中構筑物、湖泊、庫區(qū)進行測量，完成了對測量區(qū)域的水上水下一體化點云無縫拼接。苗潤龍等[8]使用基于重復海灣和遺漏海灣的搜索行為設計方法完成了海洋自主航行器多海灣區(qū)域完全遍歷路徑規(guī)劃試驗，規(guī)劃結果在遍歷性、重復率方面優(yōu)于其他遍歷方法。上海大學 “精海3 號”無人艇水面船在金塘大橋附近按設定的規(guī)劃航線進行測繪工作，因人為規(guī)劃航線只能掃測航線所覆蓋部分的區(qū)域，鐘雨軒等[9]提出一種考慮主動方向的動態(tài)柵格法與啟發(fā)式搜索算法的無人水面艇島礁海域完全遍歷路徑規(guī)劃方法，并通過了仿真驗證。

由現(xiàn)研究現(xiàn)狀分析知，在進行水上水下一體化測量時均將海圖作為已知輸入，若在未知地形復雜的近岸海域進行測量工作時，無法進行測量相關的路徑規(guī)劃工作，同時易出現(xiàn)擱淺、刮底事故，不能保障研究人員及設備安全。因此，設計一種能夠適應近岸復雜的水上水下環(huán)境，準確感知周圍態(tài)勢信息并合理規(guī)劃測量路徑的方法是解決以上問題的關鍵?，F(xiàn)階段，采用具有自主定位、導航、感知及控制功能的無人水面艇，攜帶相關測量設備，無疑是解決上述問題、并可以更快、更經(jīng)濟和更安全地完成近岸水上水下地形測量任務的一種理想手段。本文以USV 為研究平臺，搭載導航雷達、深度計、POS MV、多波束測深儀及激光掃描儀和其他必要設備，面向近岸地形測量任務需求，基于離線電子海圖，實現(xiàn)測量路徑在線規(guī)劃，以完全覆蓋預設待測量海域，最終獲取未知環(huán)境的水上水下地形地貌。首先，基于感知、測量設備信息，實現(xiàn)在線地圖構建工作[10]，本文的在線地圖信息輸入來自于雷達感知及深度計信息，其中，雷達感知數(shù)據(jù)提供無人水面艇所在環(huán)境周圍態(tài)勢信息，由深度計提供無人水面艇實時深度信息，通過POS MV 獲取USV 實時位姿，融合信息輸入，實時構建測量地圖。在此基礎上，采用基于神經(jīng)元激勵方法下的完全遍歷路徑規(guī)劃設計，結合A 星算法路徑搜尋優(yōu)勢防止路徑規(guī)劃鎖死，為USV 提供安全有效的規(guī)劃路徑，提高USV 測量工作的安全性、合理性、有效性。同時，考慮實時水深對測量設備掃測的影響，自主調節(jié)測量路徑的梳型間距，以達到要求的掃描重疊率，實現(xiàn)完全覆蓋預定測量區(qū)域[11]。

1 地圖構建方法設計

首先，存在一個基本問題，即如何使用地圖描述測量區(qū)域環(huán)境，目前已存在多種地圖表示方法，主要分為柵格地圖、特征地圖和拓撲地圖3 類[12]。本文采用柵格地圖方式，使用占據(jù)概率的柵格地圖將測量區(qū)域分成若干相同邊長的柵格，將每個柵格分配概率值以表示被障礙物占據(jù)的可能性大小。無人艇搭載導航雷達和深度計設備，實時更新柵格地圖，根據(jù)路徑規(guī)劃方法生成符合海底地形測量要求的實時梳型路徑[13-14]，能應對復雜環(huán)境下各類問題，在實時性和安全性方面具有較強優(yōu)勢[15]。

在線地圖構建過程如圖1 所示。其中，基準地圖是指由初始離線地圖和實時的雷達感知信息以及水深等信息所構建的實時地圖，地圖北向為等深線[16]由深至淺方向；測量地圖是由基準地圖作為輸入，根據(jù)水深情況以及海底地形測量標準需求更新柵格大小，該地圖直接用于測量路徑規(guī)劃算法輸入。

圖1 在線地圖構建流程圖Fig. 1 Flow chart of online map construction

地圖構建采用一種基于貝葉斯估計方式的占據(jù)柵格建圖方法。占據(jù)柵格地圖把測量范圍分解成若干二元隨機變量填充的柵格，分為占據(jù)或空閑2 種狀態(tài)。每一個柵格具有獨立性，估計環(huán)境地圖需要對每個獨立的柵格進行估計，更新地圖時，通過傳感器觀測結果計算獲得[17]。

初始離線地圖/基準地圖采用一系列二值隨機變量表示，該二值數(shù)據(jù)表示該位置是否被占據(jù)，即是否存在障礙物[18]，而待更新地圖處理方法是對二值隨機變量進行后驗估計?，F(xiàn)已知USV 位姿信息x和測量值z，為構建規(guī)范化地圖m，設其根據(jù)給定的位姿和測量信息計算整個地圖的后驗概率為p(m|z1:k,x1:k)。其中z1:k表示1 到k時刻的測量值，二值化后取0 或1（有障礙物1，無障礙物0），x1:k表示1 到k時刻的位姿組成的路徑（梳型完全遍歷），地圖將二維空間分割為有限多個柵格單元（即網(wǎng)格），地圖表示如下：

其中，mij為地圖中第i行j列個柵格單元，mij的值代表柵格是否被占據(jù)（是否存在障礙物），M為地圖的柵格最大行數(shù)，N為地圖的柵格最大列數(shù)，如果被占據(jù)則值為“1”，未被占據(jù)則為“0”。p(mij)為該柵格單元被占據(jù)的可能性。對于后驗概率p(m|z1:k,x1:k)，由于m的聯(lián)合概率計算的維數(shù)為指數(shù)級2|MN|，因此假設地圖中的每個柵格單元mij相互獨立，通過條件獨立性假設，可用邊緣概率乘積近似表示整個地圖m的后驗概率。

地圖中每個柵格單元的占據(jù)概率由靜態(tài)二值貝葉斯濾波方法獲得，該方法依靠傳感器的測量信息來估計環(huán)境地圖中一個固定的二值數(shù)，該二值數(shù)分別表示此次更新為占據(jù)或空閑，且假設地圖在此期間的狀態(tài)不改變。該方法中，占據(jù)概率用后驗概率的對數(shù)幾率表示，以保證0 和1 附近數(shù)值的穩(wěn)定性。

則在時刻k第i行j列柵格單元的占據(jù)概率pk,ij為：

因此，可得后驗概率為：

證明式（3）。

對于后驗概率，由貝葉斯準則有：

對于zk，由馬爾可夫過程屬性可知，當xk已知時其與x1:k-1和z1:k-1無關，對式(5)有：

再對p(zk|mij,xk)應用貝葉斯準則有：

在zk未知時，有， 且。再將式（8）代入式(5)中有：

對于mij的對立事件，同理有：

則式(9)除以式(10)，并取對數(shù)，可得：

其中，p(mij)為先驗概率，定義測量前初值為：

則有：

將式(13)更新方式為迭代更新，則有：

對于環(huán)境中未被探索區(qū)域中柵格，設初始化值p0。pk為時刻k對地圖的更新。

由于無人艇在不同位姿x時，雷達感知環(huán)境不同，經(jīng)過研究雷達感知數(shù)據(jù)表明，無人艇姿態(tài)穩(wěn)定狀態(tài)下，雷達將會獲得更優(yōu)的感知數(shù)據(jù)，且對不同角度、不同距離的障礙物感知效果不一。故在獲得感知數(shù)據(jù)后，取無人艇前向 2φ角度、距離dR∈[Dmin,Dmax]內(nèi)數(shù)據(jù)。其中，Dmin和Dmax為設定的最近和最遠可用感知數(shù)據(jù)距離。為排除雷達雜波的干擾，求取每個柵格倍距離內(nèi)測量值z二值化豎直為“1”柵格的個數(shù) κ，通過以下形式獲得存在障礙物的概率值：

其中，pSij為構成當前柵格的輸入小柵格面積相關概率值。

初始離線地圖柵格化邊長或輸入雷達感知信息和深度計信息所構成的柵格邊長設為l，均小于基準地圖柵格大小L，已知2 個地圖間的旋轉角度 θ，首先依次判斷小柵格的4 個頂點是否在大柵格內(nèi)部，經(jīng)對各種情況分析有：當有2 個點在基準地圖柵格外時，查看基準地圖端點是否有在小柵格內(nèi)部，若不在且距離小柵格中心最近的端點和基準地圖內(nèi)部端點異側，與基準地圖柵格有3 個交點否則有2 個交點。在依次獲得各個交點后，進行逆時針排序，得到多邊形的邊數(shù)n，通過如下公式進行面積占據(jù)求?。?/p>

通過以上方法得到基準地圖每個柵格的面積大小。由此可得面積覆蓋比率，等效為面積覆蓋概率，有

式中：pSij為由雷達和深度計獲得信息在基準地圖中，第i行j列為占用狀態(tài)的面積概率；K為更新信息落在該基準地圖中的小柵格個數(shù)。

將式(15)結果代入式(3)獲得在時刻k第i行j列柵格單元的占據(jù)概率pk,ij。通過設定閾值 α來確定柵格的二值化值zk,ij。

測量地圖的設定需按照海底地形測量及多波束掃描重復率相關要求進行設計。首先，確定測量地圖柵格邊長Lsd，如下式：

式中： η為多波束掃描重復率設置值；dwat為當前的深度計測量最淺水深； Ω為測量狀態(tài)下的多波束開角；Γ為當前基準地圖的橫向或縱向邊長，其所在邊與主體遍歷方向垂直，與等深線方向約平行。L為基準地圖柵格邊長；Lsdmax為完全遍歷梳型路徑間的最大間距，即柵格的最大邊長；dl為多波束射線可掃描的最遠距離； Ω為多波束掃描開角；ceil()為任意數(shù)的向上取整函數(shù)；floor()為任意數(shù)的向下取整函數(shù)。

然后，將基準地圖信息按照上述使用面積法求取概率，并依據(jù)式(17)求出在時刻k第i行j列柵格單元的占據(jù)概率p'k,ij。同時設定閾值 β來確定柵格的二值化值z'k,ij，所獲得的z'k作為完全遍歷路徑規(guī)劃方法輸入。

2 完全遍歷路徑規(guī)劃方法

在k時刻測量地圖為Mc×Nc二值化矩陣，令每個柵格等效為神經(jīng)元，并被賦予活性值，鄰近活性值最大的神經(jīng)元具有被選擇優(yōu)先性。神經(jīng)元活性值由固定活性值和外部激勵確定[19]，當為障礙物時，設其活性值為-1，若不為障礙物則其活性值計算公式為：

式中： λ為該柵格的活性值，由其所在位置及遍歷方向決定，遍歷方向與等深線方向垂直；x為在測量地圖坐標系橫軸坐標值；c為常數(shù)。 τ為神經(jīng)元活性值外部激勵，由于在海底地形測量過程中，無人艇頻繁的轉彎會使多波束測深儀掃描點云數(shù)據(jù)拼接后誤差增大，為保證無人艇的良好運動姿態(tài)，設τ為關于轉彎角度和移動距離的函數(shù)，則有：

式中： δ為當前規(guī)劃路徑方向轉向預規(guī)劃柵格所需要的角度；dm為當前所在柵格位置移動到預規(guī)劃柵格的距離。

將式(21)代入式(20)，分別選擇當前柵格周圍的7 個柵格(不計上一周期所在柵格，盡量避免USV 原地掉頭)作為下一移動目標，在其中選擇活性值最大的作為移動目標。

在實際海底地形測量過程中，無人艇所在區(qū)域環(huán)境復雜，由于海底地形測量要求以及無人艇操縱性限制，在路徑規(guī)劃過程中可能出現(xiàn)鎖死現(xiàn)象，因此加入A*算法[20]作為疏導策略，尋找距離當前位置最近的非障礙物柵格位置。同時，對A*所規(guī)劃路徑進行平滑處理，使得無人艇最快速度到達指定柵格位置。

此外，在出現(xiàn)危險狀況時，如突然進入測量區(qū)域的非合作船舶、水深變淺等，伴隨測量地圖的變化，進行路徑重規(guī)劃工作，為防止再次將已完成測量的區(qū)域作為規(guī)劃區(qū)域，在進行測量路徑重規(guī)劃時，將歷史形式區(qū)域神經(jīng)元活性值置-1，以保證測量工作的高效性。

在Matlab 仿真平臺下，設定測量地圖z柵格化后，行數(shù)Mc=30，列數(shù)Nc=29。隨機設置障礙物的位置及形狀，其仿真結果如圖2 所示。

可知，該路徑規(guī)劃方法可在復雜環(huán)境中較好地規(guī)劃出符合海底地形測量標準的梳型路徑，對所測區(qū)域進行全覆蓋遍歷，且有效避免了規(guī)劃過程中出現(xiàn)規(guī)劃鎖死現(xiàn)象，以最快方式到達符合要求的下一規(guī)劃位置，繼續(xù)完成路徑規(guī)劃任務。這種方法在實際的復雜環(huán)境中具有較好的環(huán)境適應性和安全性，尤其可較好地應對未知海域島岸環(huán)境。

3 實航驗證情況

基于水上水下一體化無人測量任務需求，以中科院沈陽自動化研究所研制的“問?！碧枱o人水面測量艇作為試驗平臺，對本文提出的在線地圖構建方法和完全遍歷路徑規(guī)劃方法在松花湖某區(qū)域進行實航驗證。

設定測量區(qū)域為700×800 m 近岸水域，多波束掃描重復率設置值η=21%，測量狀態(tài)下的多波束開角Ω=120°，基準地圖柵格邊長L=15，完全遍歷梳型路徑間距Lsd=50，即柵格邊長，多波束射線可掃描的最遠距離dl=200，則有行數(shù)Mc=15，列數(shù)Nc=13。輸入空白初始離線地圖，設定測量艇航行速度為4 kn，“問?！碧朥SV 通過搭載的導航雷達、深度計、多波束、激光掃描儀等設備，自主感知環(huán)境信息按照文中所設計方法進行地圖構建，實時更新基準地圖，當歷史規(guī)劃路徑與現(xiàn)地圖環(huán)境沖突時，進行路徑重規(guī)劃，無人艇對規(guī)劃路徑進行實時的軌跡跟蹤，按此步驟直至完成設定區(qū)域的水上水下一體化測量。考慮到水深變化導致的USV 安全問題，在實際應用中加入等深線方向思路，同時自主更改測量區(qū)域的邊界情況，以此方式提高測量工作的效率。其實航試驗結果如圖3所示。

圖3 初始全局完全遍歷路徑規(guī)劃結果Fig. 3 Initial global complete traversal path planning result

圖4 初次基準地圖構建結果Fig. 4 Initial baseline map construction results

由試驗結果分析可知，在設定測量區(qū)域后，問海號USV 可實時感知周圍環(huán)境。經(jīng)過實時的雷達感知信息柵格化處理以及占據(jù)概率計算形成數(shù)據(jù)更新源，實時改變基準地圖，最終構建測量地圖，經(jīng)過與實際的測量區(qū)域衛(wèi)星圖比對，地圖構建精度滿足測量路徑規(guī)劃要求。基準地圖的實時更新驅動路徑規(guī)劃方法的重規(guī)劃功能，如圖5 和圖7 所示。此時USV 在圖中所示的起點位置附近，按照路徑規(guī)劃方法設計，其能夠在此種環(huán)境中進行任意的梳型路徑規(guī)劃，且可保證避免規(guī)劃過程中的鎖死情況。通過圖8 和圖10 可知，基于完全遍歷路徑規(guī)劃方法，可實現(xiàn)待測區(qū)域的測量路徑全覆蓋。圖11 為一體化成圖效果，完成了水上水下的無縫拼接。然后對測量質量進行評估，多波束測深儀測量內(nèi)均方差不大于0.03 m，可形成1∶500 數(shù)字高程地圖。該結果滿足測量精度和測量效率的要求，即本文提出的在線地圖構建方法以及完全遍歷路徑規(guī)劃方法設計合理可行，測量結果滿足海洋工程地形測量規(guī)范要求。

圖5 初次路徑重規(guī)劃結果Fig. 5 Result of the initial path replanning

圖6 二次雷達感知信息處理結果Fig. 6 Results of secondary radar sensing information processing

圖7 二次路徑重規(guī)劃結果Fig. 7 Result of the secondary path replanning

圖8 測量地圖構建結果Fig. 8 Mapping and updating map construction results

圖9 測量區(qū)域衛(wèi)星圖截圖Fig. 9 Screenshot of satellite image of measurement area

圖10 USV 航行路徑結果Fig. 10 USV navigation path result

圖11 水上水下一體化無人測量結果效果圖Fig. 11 Above and underwater integrated unmanned measurement result rendering

4 結 語

針對近岸水域周邊的地形測量需求，本文提出基于貝葉斯估計的在線地圖構建方法和完全遍歷路徑規(guī)劃方法，給出了一種解決方案，并利用無人水面測量艇進行了實航驗證，測量結果滿足海洋工程地形測量規(guī)范要求。通過試驗結果，分析可得出結論如下：

1）USV 搭載導航雷達、深度計、多波束測深儀及激光測距儀等設備，使用基于貝葉斯估計的在線地圖構建方法，該基準地圖與實際環(huán)境相比平均誤差不大于15 m，可保證無人艇航行安全；

（2）基于在線地圖構建結果，通過完全遍歷路徑規(guī)劃方法得到符合近岸水域地形測量要求的規(guī)劃路徑。USV 依據(jù)規(guī)劃路徑內(nèi)容執(zhí)行，實現(xiàn)了對預定水域水上水下地形測量的全覆蓋，可使多波束點云數(shù)據(jù)進行無縫拼接，測量結果滿足海洋工程地形測量規(guī)范標準，即表明了設計方案的可行性。

按本文方法設計，USV 在搭載指定設備后，可自主完成近岸水域地形的測量工作。