搜索算法在無人艇航跡規(guī)劃中的應(yīng)用

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

1 天津理工大學(xué) 環(huán)境科學(xué)與安全工程學(xué)院，天津 300380

2 國家塔桅產(chǎn)品質(zhì)量監(jiān)督檢驗中心，河北 衡水 053500

0 引 言

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

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

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

1 環(huán)境模型的建立

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

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

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

2 搜索掃描算法原理及優(yōu)化

2.1 搜索掃描路徑規(guī)劃原理

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

掃描射線長度計算公式為

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

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

代價函數(shù)為：

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

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

最終確定的路徑為

2.2 算法步驟

1） 建立環(huán)境模型，確定柵格粒度，設(shè)置障礙柵格、可行柵格、起始柵格和目標(biāo)柵格；

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

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

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

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

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

7） 判斷子節(jié)點到目標(biāo)點均有障礙物后，通過代價函數(shù)得出第1 代最優(yōu)子節(jié)點；

8） 在第1 代最優(yōu)子節(jié)點的基礎(chǔ)上，按以上步驟掃描出第2 代子節(jié)點；

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

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

2.3 輻射掃描算法優(yōu)化

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

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

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

改進算法后，開始分析規(guī)劃次數(shù)與所用時間的關(guān)系，以及路徑長度隨規(guī)劃次數(shù)的走勢?？梢钥闯觯捌谒脮r間隨規(guī)劃次數(shù)的增多升高較快。在規(guī)劃次數(shù)超過15 次之后，所用時間呈穩(wěn)定增長的趨勢（圖3）。路徑長度從第3 次開始收斂，之后趨于穩(wěn)定（圖4）。

3 仿真實驗與實地測試

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

圖3 規(guī)劃次數(shù)與所用時間關(guān)系圖Fig. 3 Relationship between planned times and used time

圖中紅點是路徑規(guī)劃的起點和目標(biāo)?？梢钥闯觯诶砉ず抡鏈y試中，規(guī)劃的路徑避開了障礙物，得到了平穩(wěn)和較短的路徑。

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

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

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

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

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

圖7 兩種算法在長距離下規(guī)劃路徑效果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 結(jié) 語

本文針對海上無人艇執(zhí)行任務(wù)的特點及存在的問題，通過仿真程序?qū)资N不同地形圖進行了仿真實驗，實驗結(jié)果表明，所提的搜索掃描算法能避開障礙物，在環(huán)境模擬場所規(guī)劃的路徑安全可行，并能提高生成路徑的質(zhì)量。

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

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