呂霞付, 程啟忠, 李森浩, 林 政

基于改進(jìn)A*算法的無人船完全遍歷路徑規(guī)劃

呂霞付, 程啟忠, 李森浩, 林 政

(重慶郵電大學(xué) 工業(yè)互聯(lián)網(wǎng)與網(wǎng)絡(luò)化控制教育部重點(diǎn)實(shí)驗(yàn)室, 重慶, 400065)

針對無人船在復(fù)雜環(huán)境下完全遍歷路徑規(guī)劃算法效率差、普適性低的問題, 文中提出了一種基于改進(jìn)A*算法的無人船完全遍歷路徑規(guī)劃方法。首先通過地面站上位機(jī)電子地圖界面發(fā)布任務(wù)區(qū)域, 將該任務(wù)區(qū)域轉(zhuǎn)換為柵格地圖; 然后通過內(nèi)螺旋算法開始對柵格地圖進(jìn)行遍歷; 最后當(dāng)無人船陷入死角時(shí), 通過改進(jìn)A*算法搜索最優(yōu)路徑, 逃逸死角繼續(xù)遍歷, 直到完成所有可達(dá)區(qū)域的遍歷。仿真結(jié)果表明, 相比現(xiàn)有完全遍歷的優(yōu)化方法, 該方法規(guī)劃的路徑步數(shù)從814步減少到784步, 重復(fù)率從優(yōu)化前的7.8%改善至3.98%, 改善了性能指標(biāo), 具有較好的應(yīng)用前景。

無人船; 路徑規(guī)劃; 完全遍歷; A*算法

0 引言

隨著人類對水資源的保護(hù)、對深海區(qū)域的探索和海上軍事的開展, 無人船(unmanned surface vehicle, USV)的研究和開發(fā)越來越受到關(guān)注與重視[1]。

USV作為水域交通系統(tǒng)的重要個(gè)體, 其路徑規(guī)劃一直是研究熱點(diǎn)[2]。根據(jù)全局和局部的不同, 將USV路徑規(guī)劃分為2種類型: 完全遍歷路徑規(guī)劃和點(diǎn)對點(diǎn)路徑規(guī)劃[3]。其中, USV完全遍歷路徑規(guī)劃主要應(yīng)用于水面漂浮物清理, 水底地形探測, 海灣水域掃雷等方面。不同的完全遍歷算法效果也不一樣。

Balch[4]采用隨機(jī)式遍歷算法讓機(jī)器人直線行走, 當(dāng)遇到障礙物隨機(jī)轉(zhuǎn)動(dòng)一個(gè)角度后繼續(xù)行駛, 該算法比較簡單, 但局限性很大, 會出現(xiàn)重復(fù)清掃相同區(qū)域并且有的區(qū)域沒有清掃的情況; 王琦斐等[5]采用內(nèi)螺旋覆蓋算法讓機(jī)器人按約定的方向順時(shí)針或逆時(shí)針進(jìn)行遍歷, 當(dāng)前方區(qū)域?yàn)檎系K物或已經(jīng)被遍歷過, 則向右(或向左)旋轉(zhuǎn)90°繼續(xù)行駛, 該算法過于簡單, 容易陷入局部死角或有些區(qū)域未被遍歷; 許興軍[6]提出了一種改進(jìn)反復(fù)式全區(qū)域覆蓋算法, 并引入了區(qū)域分割的方式, 根據(jù)障礙物分布將任務(wù)區(qū)域分割成多個(gè)子區(qū)域, 再通過往復(fù)式算法遍歷子區(qū)域, 該方法完成了在實(shí)際應(yīng)用中完全遍歷的路徑規(guī)劃, 比較簡單易用, 但該方法不適用其他環(huán)境, 重復(fù)率高, 區(qū)域劃分不明顯, 算法精度低。

近些年來, 隨著搜索算法的發(fā)展, 出現(xiàn)了模糊控制、人工勢場法、神經(jīng)網(wǎng)絡(luò)、蟻群算法、遺傳算法和A*算法等, 能夠應(yīng)付復(fù)雜和多變的工作環(huán)境, 解決了死角和漏掃的問題, 實(shí)現(xiàn)了在未知環(huán)境中的運(yùn)動(dòng)和規(guī)劃[7]。張赤斌等[8]將蟻群算法應(yīng)用于完全遍歷中, 但該算法運(yùn)算量大、實(shí)時(shí)性差; Mohamed等[9]提出了一種改進(jìn)的遺傳算法, 通過改變種群的編碼方式, 在傳統(tǒng)遺傳算法的步驟上增加3個(gè)新的遺傳操作: 復(fù)原、重構(gòu)和錄優(yōu), 使得尋優(yōu)的路徑更加平滑, 但該算法需要大量的遺傳迭代, 增加的3個(gè)遺傳操作使得迭代次數(shù)更多, 遺傳過程被重復(fù), 運(yùn)算量變大; 陳超等[10]提出了一種基于可視圖的A*算法, 將起點(diǎn)與障礙物的特征點(diǎn)相連, 去除中間因其他障礙物遮擋的線段, 將A*算法與可視圖法相結(jié)合, 提高了對環(huán)境的適應(yīng)性, 但該可視圖法需要用線段連接所有起點(diǎn)到障礙物特征點(diǎn), 當(dāng)環(huán)境中出現(xiàn)動(dòng)態(tài)變化的障礙物, 需要通過可視圖法重新建立環(huán)境模型, 普適性差。USV路徑規(guī)劃技術(shù)取得了豐碩的成果, 但每一種方法都由于其自身的優(yōu)點(diǎn)和不足不能應(yīng)用于所有場合。目前USV完全遍歷路徑規(guī)劃算法創(chuàng)新之處在于多傳感器、多種路徑規(guī)劃技術(shù)的融合。

針對上述問題, 文中提出了一種改進(jìn)A*算法的USV內(nèi)螺旋完全遍歷路徑規(guī)劃算法。首先采用內(nèi)螺旋遍歷算法進(jìn)行遍歷, 當(dāng)陷入死角時(shí), 通過改進(jìn)A*算法逃逸死角, 行進(jìn)至最近的未被遍歷的柵格并繼續(xù)完成遍歷。仿真結(jié)果表明, 該方法行駛步數(shù)少、覆蓋率高、重復(fù)率小, 在復(fù)雜環(huán)境中性能更強(qiáng)。

1 基于柵格法的USV環(huán)境建模

1.1 柵格法環(huán)境建模概述

環(huán)境建模就是將能夠表示環(huán)境的數(shù)據(jù)信息進(jìn)行抽象化, 以數(shù)學(xué)方法描述物體和它們之間的空間關(guān)系。柵格法環(huán)境建模容易實(shí)現(xiàn), 是當(dāng)前研究和應(yīng)用最為廣泛的環(huán)境建模方法[11]。柵格法USV環(huán)境建模就是將USV工作的二維水面環(huán)境地圖劃分成若干個(gè)大小相同的柵格, 并根據(jù)可達(dá)區(qū)域和不可達(dá)區(qū)域的情況, 確定每個(gè)柵格的占有值。柵格法建模過程簡單, 運(yùn)算量小, 更能直觀表示環(huán)境地圖遍歷的情況, 并且柵格坐標(biāo)系的坐標(biāo)點(diǎn)與經(jīng)緯度坐標(biāo)系一一對應(yīng), 能夠準(zhǔn)確定位, 方便于環(huán)境建模和算法規(guī)劃。文中將采用柵格法完成USV環(huán)境建模, 為客觀地表示柵格法環(huán)境建模, 作如下定義。

定義1:

1.2 柵格大小選取

在柵格環(huán)境建模過程中, 柵格大小的選取[12]是一個(gè)很重要的問題, 其直接影響環(huán)境地圖信息的精度, 也同樣影響著完全遍歷路徑規(guī)劃算法的性能和效率。柵格大小的選取需要考慮以下4個(gè)因素: 環(huán)境地圖的大小與障礙物的復(fù)雜程度; USV的船身尺寸; 實(shí)際應(yīng)用情況以及不同的應(yīng)用場景和用途; 計(jì)算機(jī)的運(yùn)算和存儲能力。

1.3 環(huán)境建模過程

11) 重復(fù)步驟1)、步驟3)和步驟6), 柵格化障礙物;

13) 基于柵格法的環(huán)境建模完成。

1.4 環(huán)境建模試驗(yàn)

為驗(yàn)證上述環(huán)境建模方法的可行性, 在地面站上位機(jī)電子地圖軟件選出4個(gè)坐標(biāo)點(diǎn), 經(jīng)緯度如下:

用線段依次把這些坐標(biāo)點(diǎn)連接形成的四邊形就是邊界。圖1中P1~P4圍成的不規(guī)則四邊形即地面站上位機(jī)軟件發(fā)布的任務(wù)區(qū)域。

2 基于柵格地圖的完全遍歷算法

在建立的柵格地圖中, 在滿足一項(xiàng)或多項(xiàng)評價(jià)指標(biāo)最優(yōu)的情況下, 通過完全遍歷算法可以遍歷所有可達(dá)區(qū)域, 并規(guī)劃出一條最優(yōu)路徑。

2.1 算法性能評價(jià)指標(biāo)

USV完全遍歷算法的性能主要采用以下5個(gè)性能指標(biāo)來衡量。

1) 遍歷覆蓋率

遍歷覆蓋率是指路徑規(guī)劃完成后, USV所行駛過的面積與可達(dá)區(qū)域的比值, 即

完全遍歷時(shí), 盡可能保證所有可達(dá)安全區(qū)域都要遍歷, 接近100%。遍歷覆蓋率百分比越高, 說明該算法的性能越好。

2) 遍歷重疊率

遍歷重疊率是指路徑規(guī)劃完成后, USV行駛中出現(xiàn)2次或多次重復(fù)遍歷的面積總和與可達(dá)安全區(qū)域面積的比值, 即

完全遍歷時(shí), 在保證覆蓋率達(dá)到100%時(shí), 出現(xiàn)重復(fù)遍歷的情況, 該數(shù)值越小, 完全遍歷算法性能越好。

3) 轉(zhuǎn)彎次數(shù)

4) 步數(shù)或路徑總長度

相同的遍歷覆蓋率下, 路徑總長度越小, 控制USV的算法性能越好。

5) 總用時(shí)間

2.2 完全遍歷算法比較

圖3 3種完全遍歷算法示意圖

表1為3種遍歷算法評價(jià)指標(biāo)結(jié)果。由表中可知, 3種遍歷算法覆蓋率都為100%。傳統(tǒng)的往復(fù)前進(jìn)算法, 雖然重復(fù)率為0, 但其以犧牲路徑總長度為代價(jià), 遍歷時(shí)間和遍歷路徑遠(yuǎn)遠(yuǎn)大于其他2種遍歷算法。相比于改進(jìn)往復(fù)前進(jìn)算法, 螺旋式算法步數(shù)為350步, 減少了116步, 重復(fù)率只有2.94%, 遠(yuǎn)低于改進(jìn)往復(fù)前進(jìn)算法的24.12%, 并且總用時(shí)減少了114 s。螺旋式遍歷算法不僅達(dá)到了完全覆蓋的效果, 而且極大地降低了重復(fù)率, 減少了USV的航行步數(shù)。

表1 3種完全遍歷算法評價(jià)指標(biāo)

2.3 死角情況

在遍歷規(guī)劃時(shí), 死角是USV特別需要注意的局部環(huán)境, 也是體現(xiàn)算法有效性和優(yōu)越性的環(huán)境模型[16]。死角是指USV執(zhí)行遍歷任務(wù)到達(dá)柵格地圖某處時(shí), 相鄰的8個(gè)柵格是障礙物或者已經(jīng)遍歷。陷入死角有2種情境, 情境1: USV行駛到某位置時(shí), 所在柵格周圍相鄰的8個(gè)柵格除了上一步經(jīng)過的柵格外其余都是障礙柵格, 如圖4(a)所示; 情境2: USV所在的柵格相鄰8個(gè)柵格都已經(jīng)被遍歷過, 如圖4(b)所示。當(dāng)USV陷入上述死角時(shí), 需要搜索算法搜索最近的可達(dá)柵格, 并規(guī)劃一條最短路徑, 使得USV可以逃逸死角。

圖4 USV陷入死角示意圖

2.4 改進(jìn)A*算法的完全遍歷路徑規(guī)劃算法

A*算法是一種啟發(fā)式搜索算法[13], 它結(jié)合了Dijkstra算法的迭代式檢查和最佳優(yōu)先算法(best-first search, BFS)的方向性, 提高了效率, 減少了路程。由于其具有較好的性能和準(zhǔn)確性, 可適用于各種環(huán)境, 經(jīng)常被用于最優(yōu)路徑的求解。

2.4.1 A*算法成本估計(jì)函數(shù)

A*算法成本估計(jì)函數(shù)為

利用A*算法在柵格地圖上進(jìn)行規(guī)劃時(shí), 選取2個(gè)節(jié)點(diǎn)中心點(diǎn)間的歐幾里得距離（直線距離）作為估計(jì)代價(jià), 即

2.4.2 改進(jìn)A*算法

1) 模型再分析

圖5 A*算法流程圖

圖6 柵格模型分析

2) 改進(jìn)A*算法過程

改進(jìn)A*算法的過程如下。

⑥算法結(jié)束, BestList鏈表中的節(jié)點(diǎn)為最優(yōu)路徑。

3) 改進(jìn)A*算法實(shí)例分析

BestList鏈表存放的節(jié)點(diǎn)即為改進(jìn)A*算法的最優(yōu)路徑。結(jié)合圖7說明改進(jìn)A*算法的過程。

圖7 利用改進(jìn)A*算法尋求最優(yōu)路徑過程示意圖

根據(jù)點(diǎn)到線段的距離公式

由表2可知, 同傳統(tǒng)的A*算法相比, 改進(jìn)A*算法可使USV朝任意角度轉(zhuǎn)向, 減少了不必要的節(jié)點(diǎn), 路徑總長度變短。

表2 傳統(tǒng)A*算法與改進(jìn)A*算法數(shù)據(jù)對比

2.4.3 改進(jìn)A*算法的USV完全遍歷

為了完成柵格地圖的完全遍歷, 現(xiàn)將內(nèi)螺旋遍歷算法與改進(jìn)A*算法融合, USV執(zhí)行完全遍歷任務(wù)時(shí), 首先執(zhí)行內(nèi)螺旋遍歷算法, 當(dāng)陷入死角時(shí), 通過改進(jìn)A*算法跳出死角, 繼續(xù)遍歷。改進(jìn)A*算法的USV完全遍歷框架如圖8所示。

圖8 改進(jìn)A*算法的USV完全遍歷框架

3 仿真試驗(yàn)與分析

為了驗(yàn)證文中完全遍歷算法的有效性, 建立與文獻(xiàn)[16]相似的環(huán)境柵格地圖。地圖由32×32個(gè)柵格組成, 其中黑色區(qū)域?yàn)檫吔缁蛘系K物。當(dāng)USV進(jìn)行完全遍歷任務(wù)時(shí), 內(nèi)螺旋遍歷時(shí)陷入死角, 坐標(biāo)點(diǎn)為(17, 19), 如圖9所示。

圖10為遍歷陷入死角時(shí)傳統(tǒng)A*算法與改進(jìn)A*算法搜索路徑示意圖。圖10(a)中的黃色路徑為傳統(tǒng)A*算法的路徑節(jié)點(diǎn)示意圖, 將柵格(12, 16)作為終點(diǎn), 路徑包括19個(gè)節(jié)點(diǎn)。將這19個(gè)黃色節(jié)點(diǎn)存放在BestList鏈表中, 去掉中間不必要的節(jié)點(diǎn)并搜索最近未遍歷的柵格, 圖10(b)中綠色柵格路徑為改進(jìn)A*算法的路徑節(jié)點(diǎn)示意圖, 終點(diǎn)從(12, 16)變成(12, 12), 路徑減少到3個(gè)節(jié)點(diǎn), 大大減少了路程總長度。

圖9 USV內(nèi)螺旋遍歷陷入死角

圖10 2種算法路徑示意圖

圖11為USV完全遍歷路徑規(guī)劃完成示意圖。由圖可以看出, 在復(fù)雜環(huán)境模型中, USV的完全遍歷路徑規(guī)劃的覆蓋率為100%, 在遍歷時(shí)USV遇到2次死角, 坐標(biāo)點(diǎn)為(12, 21)、(17, 19), 通過改進(jìn)A*算法逃逸死角, 繼續(xù)完成遍歷。

圖11 USV完全遍歷路徑規(guī)劃示意圖

表3為在環(huán)境柵格地圖下, 2種完全遍歷算法評價(jià)指標(biāo)結(jié)果。由表可知, 基于改進(jìn)A*算法的完全遍歷規(guī)劃路徑步數(shù)為784步, 重復(fù)率為3.98%。相比文獻(xiàn)[16]遍歷算法步數(shù)814步, 重復(fù)率為7.8%的結(jié)果, 文中方法不僅達(dá)到了完全覆蓋的效果, 而且降低了重復(fù)率、減少了步數(shù)和路徑長度, 因此更具優(yōu)越性。

表3 2種完全遍歷算法評價(jià)指標(biāo)

4 結(jié)束語

針對USV在復(fù)雜環(huán)境下完全遍歷路徑規(guī)劃算法效率差、普適性低的問題, 提出了一種改進(jìn)A*算法的內(nèi)螺旋完全遍歷路徑規(guī)劃算法。將電子地圖邊界轉(zhuǎn)化為柵格環(huán)境模型, 并在建立的柵格地圖中比較3種遍歷算法的優(yōu)缺點(diǎn), 得出內(nèi)螺旋遍歷算法性能更優(yōu)。USV執(zhí)行遍歷任務(wù)時(shí), 首先通過內(nèi)螺旋遍歷算法開始遍歷, 若在行進(jìn)過程中遇到死角, 采用改進(jìn)A*算法實(shí)現(xiàn)USV以最短路徑逃逸死角并繼續(xù)完成遍歷。仿真結(jié)果表明, 文中方法不僅能夠有效地實(shí)現(xiàn)USV100%的完全遍歷, 而且降低了重復(fù)率, 減少了步數(shù)和路徑長度, 在復(fù)雜環(huán)境下具有較高的規(guī)劃效率。下一步將通過實(shí)物測試, 驗(yàn)證該算法。

[1] 曹娟, 王雪松. 國內(nèi)外無人船發(fā)展現(xiàn)狀及未來前景[J]. 中國船檢, 2018, 1(5): 94-97.

[2] 楊俊成, 李淑霞, 蔡增玉. 路徑規(guī)劃算法的研究與發(fā)展[J]. 控制工程, 2017, 24(7): 1473-1480.Yang Jun-cheng, Li Shu-xia, Cai Zeng-yu. Research and Development of Path Planning Algorithm[J]. Control Engineering of China, 2017, 24(7): 1473-1480.

[3] 張?jiān)? 清潔機(jī)器人全覆蓋路徑規(guī)劃研究[D]. 重慶: 重慶大學(xué), 2015.

[4] Balch T. The Case for Randomized Search[C]//Workshop on Sensors and Motion, IEEE International Conference on Robotics and Automation, San Francisco, CA: IEEE, 2000: 213-215.

[5] 王琦斐, 楊軍. 基于內(nèi)螺旋覆蓋算法的多AUV協(xié)作反水雷路徑規(guī)劃研究[J]. 計(jì)算機(jī)測量與控制, 2012, 20(1): 144-146, 160.

Wang Qi-fei, Yang Jun. Path Planning of Multiple AUVs for Cooperative Mine Countermeasure Using Internal Spiral Coverage Algorithm[J]. Computer Measurement & Control, 2012, 20(1): 144-146, 160.

[6] 許興軍. 智能割草機(jī)的區(qū)域全覆蓋算法設(shè)計(jì)與仿真[J]. 機(jī)電工程, 2012, 29(3): 302-306.

Xu Xing-jun. Design and Simulation on Regional All-covered Algorithm of Intelligent Mower[J]. Mechanical & Electrical Engineering Magazine, 2012, 29(3): 302-306.

[7] Yan R, Pang S, Sun H, et al. Development and Missions of Unmanned Surface Vehicle[J]. Journal of Marine Science and Application, 2010, 9(4): 451-457.

[8] 張赤斌, 王興松. 基于蟻群算法的完全遍歷路徑規(guī)劃研究[J]. 中國機(jī)械工程, 2008, 19(16): 1945-1949.Zhang Chi-bin, Wang Xing-song. Complete Coverage Path Planning Based on Ant Colony Algorithm[J]. China Mechanical Engineering, 2008, 19(16): 1945-1949.

[9] Mohamed A Y, Mohamed T L. The Path Planning of Cleaner Robot for Coverage Region Using Genetic Algorithms[J]. Journal of Innovation in Digital Ecosystems, 2016, 3(1): 37-43.

[10] 陳超, 唐堅(jiān). 基于可視圖法的水面無人艇路徑規(guī)劃設(shè)計(jì)[J]. 中國造船, 2013, 54(1): 129-135.Chen Chao, Tang Jian. A V-Graph Based Path Planning for Unmanned Surface Vehicle[J]. Shipbuilding of China, 2013, 54(1): 129-135.

[11] Arleo A, Millan J R, Floreano D. Efficient Learning of Variable Resolution Cognitive Maps for Autonomous Indoor Navigation[J]. IEEE Transactions on Robotics and Automation, 1999, 15(6): 990-1000.

[12] 彭剛, 沈宇. 一種變柵格地圖的巡檢機(jī)器人路徑規(guī)劃方法研究[J]. 智能機(jī)器人, 2017, 1(4): 41-43, 68.

[13] Hart P E, Nilsson N J, Raphael B. A Formal Basis for the Heuristic Determination of Minimum Cost Paths[J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.

[14] 姚雨, 李慶, 陳曦. 優(yōu)化的A*算法在航跡規(guī)劃上的應(yīng)用[J]. 微電子學(xué)與計(jì)算機(jī), 2017, 34(7): 51-55.Yao Yu, Li Qing, Chen Xi. Optimization of the Application of A* Algorithm in Path Planning[J]. Microelectronics & Computer, 2017, 34(7): 51-55.

[15] 趙曉, 王錚, 黃程侃, 等. 基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃[J]. 機(jī)器人, 2018, 40(6): 903-910.Zhao Xiao, Wang Zheng, Huang Cheng-kan, et al. Mobile Robot Path Planning Based on an Improved A* Algorithm[J]. Robot, 2018, 40(6): 903-910.

[16] 溫志文, 楊春武, 蔡衛(wèi)軍, 等. 復(fù)雜環(huán)境下UUV完全遍歷路徑規(guī)劃方法[J]. 魚雷技術(shù), 2017, 25(1): 22-26, 31.Wen Zhi-wen, Yang Chun-wu, Cai Wei-jun, et al. A Complete Coverage Path Planning Method of UUV under Complex Environment[J]. Torpedo Technology, 2017, 25(1): 22-26, 31.

Unmanned Surface Vehicle Full Traversal Path Planning Based on Improved A* Algorithm

Lü Xia-fu, CHENG Qi-zhong, LI Sen-hao, LIN Zheng

(Chongqing University of Posts and Telecommunications, Key Laboratory of Industrial Internet of Things & Network Control, Ministry of Education, Chongqing 400065, China)

To improve the efficiency and universality of full traversal path planning algorithm for unmanned surface vehicle(USV) in complex environment, a new full traversal path planning algorithm based on the improved A* algorithm is proposed. Firstly, user publishes the task area through the electronic map interface of the host computer in ground station, and converts the task area into a grid map. Then, the grid map is traversed through the inner spiral algorithm. Finally, when the USV is going into the dead angle, optimal path is searched through the improved A* algorithm, and the escape dead angle is traversed until all the reachable areas are traversed. Simulation results show that compared with the existing full traversal optimization method, the proposed algorithm reduces the planned path steps from 814 to 784, and improves the repetition rate from 7.8% to 3.98%. The algorithm improves the performance and has a good application prospect.

unmanned surface vehicle(USV); path planning; full traversal; A* algorithm

U664.82; TP301.6

A

2096-3920(2019)06-0695-09

10.11993/j.issn.2096-3920.2019.06.014

2019-03-09;

2019-04-04.

國家自然科學(xué)基金(61071117); 重慶市研究生科研創(chuàng)新項(xiàng)目(CYS14151).

呂霞付(1966-), 男, 博士, 教授, 主要研究方向?yàn)橹悄軆x器儀表.

呂霞付, 程啟忠, 李森浩, 等. 基于改進(jìn)A*算法的無人船完全遍歷路徑規(guī)劃[J]. 水下無人系統(tǒng)學(xué)報(bào), 2019, 27(6): 695-703.

(責(zé)任編輯: 許 妍)