路春暉, 尹美琳, 付振楷, 李興盛, 張嘉琪

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

0 引 言

目前，人們對海上無人船的研究越來越感興趣。通過派遣無人船執(zhí)行危險(xiǎn)任務(wù)會(huì)大大降低人員受傷的風(fēng)險(xiǎn)[1]。在無人船應(yīng)用中，一個(gè)部署是環(huán)境監(jiān)控，在高度污染的湖泊中，可以通過無人船來有效地收集水樣采集數(shù)據(jù)，而不會(huì)將人類暴露于有害元素之中;另一個(gè)潛在的應(yīng)用是災(zāi)后場景中的搜救任務(wù)。在多次實(shí)驗(yàn)中已經(jīng)證明，無人船可以有效縮短響應(yīng)時(shí)間，增加救援任務(wù)的成功率[2]。而簡單、有效、快速的航跡規(guī)劃算法成為提高任務(wù)成功率的重要保障。目前，常用的航跡規(guī)劃算法主要有A*算法[3-5]、人工勢場算法[6-7]、粒子群算法[8-10]和蟻群算法[11-12]等。

本文針對傳統(tǒng)路徑算法的思路和現(xiàn)有缺陷進(jìn)行研究，尋求一種新型路徑規(guī)劃方法。在起點(diǎn)和終點(diǎn)之間存在障礙物的情況下，向兩側(cè)掃描獲取周圍障礙物信息，用以獲取最短路徑。通過使用LabVIEW2017平臺(tái)編寫了算法仿真軟件，進(jìn)行仿真實(shí)驗(yàn)，最終確定路徑規(guī)劃算法的可靠性。

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

在規(guī)劃無人船全局路徑時(shí)，首先針對已知信息進(jìn)行環(huán)境空間建模[13]，根據(jù)建模結(jié)果獲得包含環(huán)境的搜索空間。本文采用柵格法作為建模方法表示障礙物，將規(guī)劃路徑的區(qū)域劃分成大小相同的柵格，原本有障礙物的柵格標(biāo)記為不可行區(qū)域；剩下的柵格就是正常情況下的可行區(qū)域。

1.1 柵格粒度的確定

在利用柵格法進(jìn)行空間環(huán)境建模過程中，柵格粒度的確定影響著路徑規(guī)劃的準(zhǔn)確性[14]。柵格粒度的取值如果過小，將會(huì)造成環(huán)境空間的信息量過大，使系統(tǒng)負(fù)擔(dān)過大；柵格粒度的取值如果過大，當(dāng)障礙物較多時(shí)，將導(dǎo)致無法找到有效路徑。

將柵格粒度的確定與環(huán)境中障礙物的疏密程度相結(jié)合，粒度的大小可以通過計(jì)算各障礙物面積的總和與計(jì)算整體環(huán)境的面積來獲得。計(jì)算障礙物的面積時(shí)，對于不是矩形的障礙區(qū)域，對障礙物邊緣進(jìn)行擴(kuò)充，以構(gòu)成矩形區(qū)域，并將擴(kuò)充部分一并算為障礙物進(jìn)行考慮。

確定柵格粒度的具體方法：通過將各個(gè)障礙物矩形化，確定各個(gè)障礙物的面積，利用Sz=∑Sn計(jì)算障礙物總面積,其中：Sn為第n個(gè)障礙物的面積；Sz為障礙物總面積。確定柵格粒度：

式中：S總為需規(guī)劃二維空間的面積；lmax為柵格坐標(biāo)系的長度；lmin為人為限定的柵格最小邊長；l為經(jīng)計(jì)算所得的柵格邊長(柵格粒度)。

1.2 柵格空間布局

在二維路徑規(guī)劃空間中，將水平右方向設(shè)置為正x軸方向，水平向上方向設(shè)置為正y軸方向。以這種方式構(gòu)造了二維空間直角坐標(biāo)系。柵格分為兩種類型，第1種類型在其覆蓋范圍內(nèi)不含任何障礙物；第2種類型在其覆蓋范圍內(nèi)含有障礙物。將第1種柵格稱之為自由柵格，用白色表示；第2種柵格稱為障礙柵格，用黑色表示(見圖1、2)。

圖1 未處理前柵格障礙物形狀

圖2 處理后柵格障礙物形狀

2 輻射掃描算法原理、模型及優(yōu)化

2.1 輻射掃描路徑規(guī)劃原理

在起點(diǎn)與終點(diǎn)之間存在障礙物的情況下，起點(diǎn)沿障礙物向兩側(cè)進(jìn)行掃描，得出子節(jié)點(diǎn)。再由子節(jié)點(diǎn)向終點(diǎn)方向進(jìn)行掃描，若存在障礙物，則繼續(xù)沿起點(diǎn)之前的掃描方向掃描，得出下一代的子節(jié)點(diǎn)。子節(jié)點(diǎn)再掃描下一代子節(jié)點(diǎn)時(shí)，已經(jīng)被掃描過的子節(jié)點(diǎn)將不會(huì)再被定義為子節(jié)點(diǎn)。掃描到終點(diǎn)為最后一代節(jié)點(diǎn)，掃描過程終止，每代子節(jié)點(diǎn)依次反選，最后反選回起點(diǎn)，得出最短路徑。

2.2 算法實(shí)現(xiàn)

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

Step2從起點(diǎn)向終點(diǎn)方向發(fā)射射線，掃描路上有沒有障礙(注意：射線真實(shí)寬度不是1，而是單個(gè)柵格的邊長)。

Step3碰到障礙物后，生成兩條射線向兩側(cè)掃點(diǎn)，以原向量角度沿障礙物邊界前進(jìn)，為起點(diǎn)到該點(diǎn)的新向量。

Step5比較該方向掃描新向量的長度與舊向量長度，當(dāng)差值超過單位柵格粒度時(shí)，則判斷當(dāng)前位置存在可走空間。

Step6此時(shí)，在判斷有可走空間的位置，被阻擋的可行柵格向射線方向下一個(gè)柵格建立子節(jié)點(diǎn)。

Step7子節(jié)點(diǎn)只需向外發(fā)射一條射線，新掃描的柵格需要不斷判斷是否超過了上級(jí)起點(diǎn)的掃描范圍。如果超過，則上級(jí)起點(diǎn)也需要繼續(xù)掃描(防止出現(xiàn)迷宮里的死胡同問題，同時(shí)防止所走路線并非最短路徑)。

Step8如果沿障礙物前進(jìn)所更新向量位置坐標(biāo)的柵格不是障礙柵格的話，則需要該點(diǎn)到起點(diǎn)方向的向量的下一個(gè)柵格建立子節(jié)點(diǎn)。

Step9當(dāng)沿障礙物掃描方向又出現(xiàn)不可行柵格時(shí)，則判斷遇到內(nèi)拐點(diǎn)，此時(shí)由原向量向邏輯掃描方向(一條射線為順時(shí)針；另一條為逆時(shí)針)轉(zhuǎn)動(dòng)90°，繼續(xù)掃描。

Step10重復(fù)當(dāng)前所有步驟進(jìn)行掃描。

Step11到達(dá)終點(diǎn)后，從終點(diǎn)開始反向連接父節(jié)點(diǎn)，直至回到起點(diǎn)，尋路完成。

2.3 輻射掃描算法模型

建立環(huán)境柵格，在環(huán)境模型中，設(shè)點(diǎn)(13,17)為要規(guī)劃路徑的起點(diǎn)，目標(biāo)為(13,4)點(diǎn)。從起點(diǎn)(13,17)出發(fā)，向終點(diǎn)(13,4)發(fā)射射線，發(fā)現(xiàn)中間有障礙物(見圖3)。由于起點(diǎn)(13,17)到終點(diǎn)(13,4)中發(fā)現(xiàn)障礙物，那么程序?qū)⑸蓛蓷l射線向兩側(cè)掃描(見圖4)。

掃描過程中發(fā)現(xiàn)在點(diǎn)(15,11)處，掃描射線的新向量與舊向量長度差超過1柵格寬度，判斷此處有缺口。點(diǎn)(15,11)為射線中心遇到障礙物前的最后一個(gè)柵格。這時(shí)在點(diǎn)(15,11)向射線方向下一個(gè)柵格點(diǎn)(15,10)建立子起點(diǎn)(見圖5)。

圖5 建立一級(jí)子節(jié)點(diǎn)圖

點(diǎn)(15,10)只向外發(fā)射1條射線，首先判斷此點(diǎn)與終點(diǎn)之間是否存在障礙物，若存在則按上級(jí)點(diǎn)(這里的上級(jí)點(diǎn)是起點(diǎn))掃描方向繼續(xù)掃描，并且不斷判斷是否超過了上級(jí)起點(diǎn)范圍，如果超過則上級(jí)起點(diǎn)也需要掃描。點(diǎn)(15,10)掃描到障礙物點(diǎn)(17,9)時(shí)，開始超過起點(diǎn)(13,17)的掃描范圍，那么起點(diǎn)也繼續(xù)掃描(見圖6)。

起點(diǎn)(13,17)掃描過程中至點(diǎn)(21,12)處由不可行柵格變?yōu)榭尚袞鸥瘢袛嘣擖c(diǎn)到起點(diǎn)方向的向量的下一個(gè)柵格(21,13)建立子節(jié)點(diǎn)(見圖7)。而點(diǎn)(15,10)由于掃描角度變化一周后無子節(jié)點(diǎn)而被舍棄。重復(fù)上述步驟，依次在點(diǎn)(21,13)、(23,11)、(23,8)、(21,6)、(18,4)、(16,2)、(13,2)建立子節(jié)點(diǎn)，點(diǎn)(13,2)向終點(diǎn)(13,4)發(fā)射射線，發(fā)現(xiàn)無障礙物?？芍苯拥竭_(dá)終點(diǎn)，則掃描過程結(jié)束(見圖8)。

圖6 起點(diǎn)與一級(jí)子節(jié)點(diǎn)同時(shí)掃描圖

圖7 建立二級(jí)子節(jié)點(diǎn)圖

圖8 掃描終止后路線圖

最終程序?qū)ふ页龅穆窂綖椋浩瘘c(diǎn)(13,17)至(21,13)至(23,11)至(23,8)至(21,6)至(18,4)至(16,2)至(13,2)至 終點(diǎn)(13,4)(見圖9)。

圖9 掃描算法所尋路線圖

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

通過實(shí)例分析不難看出，障礙物分布與子節(jié)點(diǎn)的位置關(guān)系為左上、左下、右上、右下4種位置關(guān)系。當(dāng)尋找的路徑通過了子節(jié)點(diǎn)和其周邊障礙物方格所夾方格時(shí)，則可忽略子節(jié)點(diǎn)，直接將兩個(gè)所夾方格相連。如圖10所示，點(diǎn)(21,13)為黃色子節(jié)點(diǎn)柵格，點(diǎn)(20,12)為其周邊障礙物柵格，并在其右上位置。點(diǎn)(21,13)與點(diǎn)(20,12)相夾柵格為(21,12)和(20,13)，通過圖10不難看出所規(guī)劃路線需經(jīng)過(21,12)和(20,13)兩點(diǎn)。那么當(dāng)尋路完畢后，從起點(diǎn)沿著所尋路徑發(fā)射信號(hào)，當(dāng)?shù)竭_(dá)點(diǎn)(20,13)時(shí)，跳過子節(jié)點(diǎn)(21,13)直接走到點(diǎn)(21,12)。最終規(guī)劃路徑(見圖10)。

圖10 射線掃描算法最終規(guī)劃路線圖

3 仿真實(shí)驗(yàn)

為了驗(yàn)證輻射掃描算法的正確性，用C++實(shí)現(xiàn)了算法，并將其封裝到動(dòng)態(tài)鏈接庫中，在LabVIEW 2017開發(fā)環(huán)境下做程序主界面并調(diào)用C++庫。仿真軟件主界面如圖11所示，左側(cè)用來顯示無人船工作環(huán)境，圖中顯示的是天津于橋水庫。路徑規(guī)劃的結(jié)果會(huì)直接繪制在地圖上，并且在右側(cè)顯示規(guī)劃的路徑中包含的GPS坐標(biāo)和算法耗時(shí)。為了方便使用，軟件在內(nèi)部對GPS坐標(biāo)和柵格模型的序號(hào)之間做轉(zhuǎn)換，所以軟件的輸入和輸出均是GPS坐標(biāo)，不需要用戶手動(dòng)輸入起點(diǎn)和目標(biāo)所在柵格的序號(hào)。

圖11 仿真軟件界面

使用明理湖和于橋水庫的GPS坐標(biāo)生成的柵格模型做了大量仿真實(shí)驗(yàn)。明理湖的每個(gè)柵格的邊長是10 m，柵格模型共75行70列；于橋水庫的每個(gè)柵格邊長40 m，柵格模型共198行428列。仿真結(jié)果如圖12、13所示。

圖12、13中的紅色圓點(diǎn)是路徑規(guī)劃的起點(diǎn)和目標(biāo)?？梢钥闯觯涸诿骼砗袦y試，規(guī)劃的路徑繞開了湖中的湖心島，是一條近似最優(yōu)的路徑。在于橋水庫中的測試規(guī)劃的路徑也避開了水庫的邊緣，也是一條近似最優(yōu)的路徑?？梢钥闯?，在不同的環(huán)境、起點(diǎn)和目標(biāo)的情況下，算法均能正確地規(guī)劃出一條避開障礙物的路徑。

圖12 明理湖仿真實(shí)驗(yàn)

圖13 于橋水庫仿真實(shí)驗(yàn)

實(shí)際情況下，無人船的航程常常會(huì)達(dá)到數(shù)km至幾十km。為了比較在長距離情況下輻射掃描算法的性能和魯棒性，將柵格蟻群算法和離散蟻群算法進(jìn)行對比，并在于橋水庫做了長距離路徑規(guī)劃的仿真。仿真實(shí)驗(yàn)中算法的起點(diǎn)和目標(biāo)相同，仿真結(jié)果如圖14所示。

(a)柵格-蟻群算法

(b)離散-蟻群算法

(c)輻射掃描算法

從圖14可以看出：柵格-蟻群算法在長距離下雖然可以工作，但輸出的路徑曲折迂回，沒有實(shí)際使用價(jià)值。離散-蟻群算法由于步長可以自適應(yīng)調(diào)節(jié)，所以在長距離下仍然可以正常工作。輻射掃描路徑規(guī)劃算法規(guī)劃的路徑最短，表現(xiàn)最優(yōu)。

4 結(jié) 論

針對海上無人船執(zhí)行任務(wù)的特點(diǎn)及存在的問題，通過仿真程序?qū)资N不同地形圖進(jìn)行仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明。

(1)在環(huán)境模擬場所利用改進(jìn)后的掃描算法規(guī)劃路徑是安全可行的，該算法既能保證避開障礙物，也可以實(shí)時(shí)動(dòng)態(tài)避開危險(xiǎn)區(qū)域。

(2)相較蟻群算法能夠規(guī)劃出更短的路線。

(3)在較為復(fù)雜的近海領(lǐng)域，有待進(jìn)行深一步研究。

航跡規(guī)劃為無人船提供航線，具體如何航行需要導(dǎo)航避障系統(tǒng)和控制器協(xié)作配合完成。不僅如此，無人船在水面航行還會(huì)受到自身狀態(tài)、傳感器噪聲、風(fēng)浪和天氣等因素的影響[15]。對無人船航跡規(guī)劃還有很大的研究空間。