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

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

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

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

0 引 言

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

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

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

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

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

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

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

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

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

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

掃描射線長(zhǎng)度計(jì)算公式為

掃描射線前一掃描點(diǎn)的長(zhǎng)度計(jì)算公式為

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

代價(jià)函數(shù)為：

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

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

最終確定的路徑為

2.2 算法步驟

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

2） 從起點(diǎn)向終點(diǎn)方向進(jìn)行搜索，判斷掃描路徑上是否有障礙；

3） 碰到障礙物后，以起點(diǎn)為中心進(jìn)行360 度掃描；

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

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

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

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

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

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

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

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

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

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

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

改進(jìn)算法后，開(kāi)始分析規(guī)劃次數(shù)與所用時(shí)間的關(guān)系，以及路徑長(zhǎng)度隨規(guī)劃次數(shù)的走勢(shì)。可以看出，前期所用時(shí)間隨規(guī)劃次數(shù)的增多升高較快。在規(guī)劃次數(shù)超過(guò)15 次之后，所用時(shí)間呈穩(wěn)定增長(zhǎng)的趨勢(shì)（圖3）。路徑長(zhǎng)度從第3 次開(kāi)始收斂，之后趨于穩(wěn)定（圖4）。

3 仿真實(shí)驗(yàn)與實(shí)地測(cè)試

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

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

圖中紅點(diǎn)是路徑規(guī)劃的起點(diǎn)和目標(biāo)。可以看出，在理工湖仿真測(cè)試中，規(guī)劃的路徑避開(kāi)了障礙物，得到了平穩(wěn)和較短的路徑。

圖4 路徑長(zhǎng)度收斂趨勢(shì)圖Fig. 4 Path length convergence trend

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

圖6 理工湖仿真實(shí)驗(yàn)Fig. 6 Science and technology lake simulation experiment

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

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

圖7 兩種算法在長(zhǎng)距離下規(guī)劃路徑效果Fig. 7 Two algorithms plan path effects over long distances

圖8 理工湖測(cè)試圖Fig. 8 Science and technology lake test

圖9 渤海灣測(cè)試圖Fig. 9 Bohai bay test

4 結(jié) 語(yǔ)

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

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

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