搜索算法在無人艇航跡規劃中的應用

路春暉，張博，付振楷，尹美琳，張嘉琪*

1 天津理工大學 環境科學與安全工程學院，天津 300380

2 國家塔桅產品質量監督檢驗中心，河北 衡水 053500

0 引 言

當派遣無人艇（USV）執行危險任務時，由于操作人員的參與程度低，人員受傷的風險可大幅降低[1-2]。進行環境監控時，在高度污染的湖泊中，可以通過無人艇來收集水樣和采集數據，避免將人暴露于有害元素之中。執行災后場所的搜救任務時，采用無人艇可以有效縮短響應時間，增加救援任務的成功率[3]。簡單有效的航跡規劃算法是提高任務成功率的重要保證，目前，常用的航跡規劃算法主要有A*算法[4-7]、粒子群算法[8-11]、人工勢場算法[12]和蟻群算法[13-14]等。

A*算法作為啟發式搜索算法之一，能夠快速搜索出路徑，但當存在多個最小值時，不能保證搜索的路徑最優。粒子群優化算法具有參數較少、容易實現等優點，但存在著收斂精度低、搜索停滯等缺點。傳統人工勢場法存在局部最優和目標不可達的問題。蟻群算法受起止點位置和障礙分布的影響，環境復雜時容易陷入不可行點，甚至出現路徑迂回和死鎖的情況。

本文將在研究一般路徑算法的過程中，針對傳統路徑算法的思路和現有缺陷，在保證最短路徑和快速搜索的前提下，結合2D 掃描思想，尋求一種新型路徑規劃方法。在起點和終點之間存在障礙物的情況下，通過掃描獲取周圍障礙物信息，以獲取最短路徑。利用LabView2017 平臺編寫仿真程序，以驗證規劃算法的可靠性。

1 環境模型的建立

在規劃無人艇的全局路徑時，首先針對已知信息進行環境空間建模[15]，然后，根據建模結果獲得包含障礙物信息的搜索空間，利用具體的算法對搜索空間進行搜索。本文采用柵格法模擬障礙物。將計劃搜索區域劃分為相同大小的網格，原始障礙物標記為不可行區域，剩余的網格則為正常條件下的可行區域。

在利用柵格法進行空間環境建模的過程中，柵格粒度的確定影響著路徑規劃的準確性[16]。柵格粒度的取值如果過小，會造成環境空間的信息量過大，使系統負擔過重；而柵格粒度的取值如果過大，當障礙物較多時，會導致無法找到有效路徑。因此，需通過環境中障礙物的疏密度來確定柵格粒度。在計算障礙物面積時，對于不是矩形的障礙區域，對障礙物邊緣進行擴充，以構成矩形區域，并將擴充部分一并算為障礙物進行考慮。

通過將障礙物矩形化來確定和計算障礙物的面積。然后利用障礙物總面積所占柵格空間總面積的比例，來確定柵格粒度l。

2 搜索掃描算法原理及優化

2.1 搜索掃描路徑規劃原理

首先，在系統內設定起點S(xs,ys)和目標點T(xt,yt)。由起點向目標點發射射線，射線若被障礙物阻擋，則判斷起點與目標點之間存在障礙物。然后，以起點為中心向Y軸正方向發射射線，被障礙物或地圖邊緣阻擋后，開始順時針進行360°掃描。此時，通過式（3）計算起點S(xs,ys)至當前掃描點F(xf,yf)的距離（也即掃描射線的長度）Df(gf,gs)。Df(gf,gs)隨掃描方向的變化而不斷變化。Dfp(gfp,gs)為 掃描射線長度值Df(gf,gs)前一時刻掃描點p(xfp,yfp)至 起點S(xs,ys)的掃描射線長度。

掃描射線長度計算公式為

掃描射線前一掃描點的長度計算公式為

掃描射線長度變化值計算公式為當M≥l時，判斷射線掃描超出障礙物阻擋范圍，在最后阻擋的柵格向掃描變化方向的下一個柵格建 立 子 節 點；當 ?l＜M＜l時，繼 續 掃 描；當M≤?l時，判斷射線掃描到新的障礙物阻擋區域，在被阻擋柵格向掃描變化反方向的下一個柵格建立子節點。

代價函數為：

若通過比較 minP發現存在2 個或多個最小代價值點，則比較這幾個點的 minH。若比較minH之后仍存在2 個或多個點，則暫時隨機選取其中一個。

當某代子節點向目標點發射射線，射線未被障礙物阻擋，則判斷掃描到終點，掃描結束。若最后一代子節點中，存在2 個或多個點可直接掃描到目標點，則比較 minP選取最小代價值點。若還存在多個點，則比較 minH，確定最終路徑。

最終確定的路徑為

2.2 算法步驟

1） 建立環境模型，確定柵格粒度，設置障礙柵格、可行柵格、起始柵格和目標柵格；

2） 從起點向終點方向進行搜索，判斷掃描路徑上是否有障礙；

3） 碰到障礙物后，以起點為中心進行360 度掃描；

4） 掃描過程中，當產生的掃描新向量比舊向量長度差超過1 個格子長度時，立即判斷此處有缺口；

5） 在判斷為缺口的位置，將被阻擋的格子，在搜索射線方向或反方向的下一個格子上建立子起點；

6） 以起點為中心掃描出第1 代子節點，并判斷第1 代各子節點到目標點是否存在障礙物阻擋；

7） 判斷子節點到目標點均有障礙物后，通過代價函數得出第1 代最優子節點；

8） 在第1 代最優子節點的基礎上，按以上步驟掃描出第2 代子節點；

9） 按上述方法，直至掃描到終點，掃描過程結束，尋路完成（圖1）。

圖1 掃描算法最終路徑圖Fig. 1 Final path diagram of the scanning algorithm

2.3 輻射掃描算法優化

通過上述分析可以看出，搜索掃描算法可以順利規劃出可行路徑。但在某些情況下，存在可優化空間。在多次實驗中發現，算法所規劃的路徑中某些柵格可以略過，以達到減少路徑長度的目的。具體優化方法：從路徑第1 個柵格開始，向每個中間柵格掃描，判斷是否穿過障礙物。在未穿過障礙物的前提下，計算直線路徑與原始路徑的長度。當直線路徑長度小于原始路徑長度時，將直線路徑代替原始路徑。以此類推，直至目標點柵格。

另外，當比較 minP后，若發現存在2 個或多個最小代價值點，則比較這幾個點的 minH。若比較 minH之后仍存在2 個或多個點，暫時隨機選取其中一個，則在某些情況下將會得出不同的路徑。通過算法隨路徑進行多次規劃，并通過路徑長度比較出最短路徑。在此基礎上，進行下一輪規劃，至相鄰兩輪規劃的路徑完全一樣，則判斷結果為最佳路徑（圖2）。

圖2 算法優化前后路徑對比圖Fig. 2 Path comparison before and after algorithm optimization

改進算法后，開始分析規劃次數與所用時間的關系，以及路徑長度隨規劃次數的走勢?？梢钥闯?，前期所用時間隨規劃次數的增多升高較快。在規劃次數超過15 次之后，所用時間呈穩定增長的趨勢（圖3）。路徑長度從第3 次開始收斂，之后趨于穩定（圖4）。

3 仿真實驗與實地測試

為了驗證搜索掃描算法的正確性，利用Lab-View 2017 開發環境編寫仿真程序。將開發的航跡規劃程序與無人艇岸基站控制軟件相連接，通過軟件環境模擬和無人艇實地檢驗驗證程序的可行性。將理工湖地形圖導入仿真程序進行測試，仿真軟件主界面如圖5 所示，理工湖仿真試驗如圖6 所示。

圖3 規劃次數與所用時間關系圖Fig. 3 Relationship between planned times and used time

圖中紅點是路徑規劃的起點和目標?？梢钥闯?，在理工湖仿真測試中，規劃的路徑避開了障礙物，得到了平穩和較短的路徑。

圖4 路徑長度收斂趨勢圖Fig. 4 Path length convergence trend

圖5 仿真軟件界面Fig. 5 Simulation software interface

圖6 理工湖仿真實驗Fig. 6 Science and technology lake simulation experiment

為方便使用，在軟件內部轉換GPS 坐標和柵格模型的序號，因此軟件的輸入和輸出是GPS 坐標。無需用戶手動輸入目標所在網格的起點和坐標。在實際情況下，無人艇的航程往往達數公里至數十公里。為了比較搜索掃描算法在遠距離條件下的性能，相較蟻群算法在湖中進行了遠程路徑規劃仿真。仿真實驗中，算法的起點和目標是相同的（圖7）。從圖中可以看出，搜索掃描路徑規劃算法的表現較優。

為了驗證搜索掃描算法的實地應用能力，分別在理工湖和渤海灣進行了實地測試。在理工湖測試（圖8）中，無人艇能夠根據軟件規劃的路徑順利到達終點。在渤海灣測試（圖9）中，無人艇在PID 控制系統的輔助下，能夠在風浪中按規劃路徑穩定前進。

圖7 兩種算法在長距離下規劃路徑效果Fig. 7 Two algorithms plan path effects over long distances

圖8 理工湖測試圖Fig. 8 Science and technology lake test

圖9 渤海灣測試圖Fig. 9 Bohai bay test

4 結 語

本文針對海上無人艇執行任務的特點及存在的問題，通過仿真程序對幾十種不同地形圖進行了仿真實驗，實驗結果表明，所提的搜索掃描算法能避開障礙物，在環境模擬場所規劃的路徑安全可行，并能提高生成路徑的質量。

該改進算法主要有以下特點：1）建立了一種新的程序思路用于生成路徑；2）對算法中未能實現最短路徑的情況提出了新的自適應更新策略；3）通過仿真實驗，驗證了本文所提射線搜索算法在進一步提高二維空間中生成路徑解質量的有效性。

對于無人艇的導航問題，路徑規劃雖不可或缺，但也不是問題的全部。路徑規劃為無人艇提供航線，具體如何航行還需要導航避障系統和控制器協作配合完成。本文所研究的路徑規劃算法僅在該領域進行了初步探討。