一種水面無人艇多任務(wù)點(diǎn)路徑規(guī)劃方法

劉漸道 張英俊 朱飛祥

摘要：為解決多障礙物環(huán)境下水面無人艇（unmanned surface vehicle， USV）多任務(wù)點(diǎn)路徑規(guī)劃問題，提出一種基于改進(jìn)的快速探索隨機(jī)樹（rapidly-exploring random tree， RRT）的路徑規(guī)劃算法。在分析USV運(yùn)動數(shù)學(xué)模型和經(jīng)典RRT算法的基礎(chǔ)上，將USV的運(yùn)動數(shù)學(xué)模型融合到RRT算法中，預(yù)報(bào)兩個任務(wù)點(diǎn)之間的路徑曲線和距離;針對RRT算法隨機(jī)性的特點(diǎn)，設(shè)計(jì)RRT路徑優(yōu)化算法，刪除冗余路徑點(diǎn)，得到優(yōu)化路徑;最后利用改進(jìn)遺傳算法，確定多任務(wù)點(diǎn)的訪問順序，生成多任務(wù)點(diǎn)路徑，節(jié)省USV巡航路徑距離。仿真結(jié)果證明，在多任務(wù)點(diǎn)及多障礙物存在的條件下，該方法能夠確定一條合理的路徑，具有一定的實(shí)際意義。

關(guān)鍵詞： 水面無人艇（USV）; 多任務(wù)點(diǎn)路徑規(guī)劃; 快速探索隨機(jī)樹（RRT）; 運(yùn)動學(xué)約束

中圖分類號： U675.79 ? ?文獻(xiàn)標(biāo)志碼： A

Abstract： In order to solve the problem of multi-task point path planning for unmanned surface vehicles （USVs） in the multi-obstacle environment， a path planning algorithm based on the improved rapidly-exploring random tree （RRT） is proposed. Based on the analysis of USV’s motion mathematical model and the classical RRT algorithm， the USV’s motion mathematical model is integrated into the RRT algorithm， and the path curve and distance between two task points are forecasted. According to the randomness of RRT algorithm， an RRT path optimization algorithm is designed to delete the redundant path points so as to get an optimized path. Finally， the improved genetic algorithm is used to determine the access sequence of multi-task points and generate the multi-task point path， which can save the USV cruise path distance. The simulation result shows that the proposed method can determine a reasonable path under the condition of multi-task points and multiple obstacles， and has a certain practical significance.

Key words： unmanned surface vehicle （USV）; multi-task point path planning; rapidly-exploring random tree （RRT）; kinematical constraint

0 引 言

水面無人艇（unmanned surface vehicle， USV）是一種具有自主感知環(huán)境、規(guī)劃路徑、智能航行及自主避障能力的小型船艇，在水文探測、海域巡邏、軍事作戰(zhàn)等方面有廣泛的應(yīng)用。USV路徑規(guī)劃就是根據(jù)任務(wù)要求在一定的條件下按照安全性、經(jīng)濟(jì)性原則自動規(guī)劃從起點(diǎn)到終點(diǎn)的無障礙路徑[1]。路徑規(guī)劃是實(shí)現(xiàn)自主航行必須解決的問題，是USV領(lǐng)域的研究熱點(diǎn)。

現(xiàn)有的路徑規(guī)劃算法主要包括Dijkstra算法、A *算法等啟發(fā)式搜索算法和遺傳算法、粒子群算法、螢火蟲算法、神經(jīng)網(wǎng)絡(luò)算法等智能仿生優(yōu)化算法[2]。隨博文等[3]提出一種改進(jìn)的平滑A *算法，改進(jìn)折線轉(zhuǎn)彎為圓弧，使路徑更加平滑安全;范云生等[4]基于電子海圖采用柵格法建立環(huán)境模型，利用改進(jìn)的遺傳算法進(jìn)行全局路徑快速搜索，規(guī)劃出的路徑具有一定的合理性和有效性?？焖偬剿麟S機(jī)樹（rapidly-exploring random tree， RRT）算法在路徑規(guī)劃領(lǐng)域得到了廣泛的研究，針對RRT算法產(chǎn)生了很多改進(jìn)。最早采用的是單棵樹的搜索方式，研究人員提出偏向RRT、雙向RRT、RRT*等搜索算法以進(jìn)一步提高搜索效率。歐陽子路等[5]將雙向RRT算法與速度障礙法相結(jié)合，得到了基于改進(jìn)雙向-RRT的USV自動避碰算法，使算法搜索樹延伸失敗次數(shù)減少，規(guī)劃的路徑更短、更平滑;楊左華等[6]提出一種基于新的線段定理和RRT*的算法，并通過與現(xiàn)有算法避障規(guī)劃的比較說明該算法的有效性;莊佳園等[7]設(shè)計(jì)了一種基于改進(jìn)RRT算法的局部路徑規(guī)劃方法，引入限定轉(zhuǎn)角和距離啟發(fā)信息，改進(jìn)生長點(diǎn)和探索點(diǎn)的選擇，提高了算法的速度;曹凱等[8]提出基于概率P雙向RRT算法的移動機(jī)器人路徑規(guī)劃，所提算法在復(fù)雜環(huán)境中有著良好的收斂效果;徐娜等[9]將移動機(jī)器人的非完整約束條件與RRT算法相結(jié)合，實(shí)驗(yàn)表明該算法能有效解決存在非完整約束的機(jī)器人運(yùn)動規(guī)劃問題;宋金澤等[10]將非完整約束條件與雙向多步擴(kuò)展RRT算法相結(jié)合，用B樣條曲線擬合控制點(diǎn)生成平滑路徑，驗(yàn)證了算法的有效性。文獻(xiàn)[11]的RRT算法考慮了無人艇的操縱特性，與本文的區(qū)別在于采用的RRT算法不同，而且沒有對規(guī)劃后的路徑進(jìn)行優(yōu)化。在多任務(wù)點(diǎn)路徑規(guī)劃方面：文獻(xiàn)[12]針對有障礙區(qū)域的多無人機(jī)多目標(biāo)點(diǎn)路徑規(guī)劃問題，建立多約束優(yōu)化模型，應(yīng)用遺傳算法進(jìn)行求解;文獻(xiàn)[13]針對機(jī)器人遍歷多個目標(biāo)點(diǎn)的路徑規(guī)劃問題，提出基于改進(jìn)的粒子群算法與蟻群算法結(jié)合的路徑規(guī)劃方法，并在真實(shí)環(huán)境中驗(yàn)證其有效性;文獻(xiàn)[14]基于電子海圖數(shù)據(jù)建立環(huán)境模型，通過A *算法進(jìn)行無人艇多任務(wù)點(diǎn)路徑規(guī)劃（未考慮無人艇的運(yùn)動學(xué)約束），采用蟻群算法確定路徑點(diǎn)的訪問順序。

隨著研究的逐步深入，路徑規(guī)劃問題取得了豐碩的研究成果，但是也凸顯出一些亟待解決的問題。例如，USV路徑規(guī)劃不僅要在空間內(nèi)找到一條無碰撞路徑，還應(yīng)考慮USV的運(yùn)動學(xué)約束，否則規(guī)劃出的路徑有可能在實(shí)際應(yīng)用中不可行。RRT算法是基于采樣的規(guī)劃算法，適合處理含有運(yùn)動學(xué)約束的路徑規(guī)劃問題。對于多任務(wù)點(diǎn)的路徑規(guī)劃，還應(yīng)考慮任務(wù)點(diǎn)的訪問順序，減少航行距離。因此，本研究提出改進(jìn)經(jīng)典RRT算法，將其與USV運(yùn)動學(xué)約束相結(jié)合，生成含有運(yùn)動學(xué)約束的可執(zhí)行路徑曲線并確定任務(wù)點(diǎn)間距離，采用改進(jìn)遺傳算法優(yōu)化多任務(wù)點(diǎn)的訪問順序，減少巡航距離。

1 USV運(yùn)動模型及環(huán)境模型

1.1 USV運(yùn)動數(shù)學(xué)模型

2.5 多任務(wù)點(diǎn)訪問順序求解

為解決多任務(wù)點(diǎn)的路徑規(guī)劃問題，將目標(biāo)點(diǎn)的選擇規(guī)劃作為旅行商問題（traveling salesman problem， TSP）的一個分支。蟻群算法、模擬退火算法、遺傳算法等都是目前求解TSP的有效方法。有些學(xué)者嘗試結(jié)合兩種或多種算法以克服各種算法的缺點(diǎn)。遺傳算法的基本流程：首先對個體進(jìn)行編碼，生成初始種群;接著開始進(jìn)化過程，用適應(yīng)度函數(shù)判斷個體優(yōu)劣，優(yōu)秀的個體將獲得更多的選擇、交叉和變異的機(jī)會;不斷進(jìn)行上述操作，直到滿足迭代終止條件，得到最優(yōu)解。本文采用改進(jìn)的遺傳算法求解多任務(wù)點(diǎn)的訪問順序，使USV遍歷所有任務(wù)點(diǎn)后航程最短。方法如下：

（1）個體編碼。應(yīng)用路徑表示法進(jìn)行個體編碼，個體即為任務(wù)點(diǎn)。如（T1，T2，…，Tn）表示從T1出發(fā)，依次經(jīng)過任務(wù)點(diǎn)T2，T3，…，Tn-1，最后到達(dá)Tn。

（2）種群初始化。假定種群規(guī)模為N，其中0.8N個初始個體隨機(jī)產(chǎn)生，0.2N個初始個體由貪心算法生成。首先隨機(jī)選取一個任務(wù)點(diǎn)作為第一個基因，接著從余下的任務(wù)點(diǎn)中確定與第一訪問點(diǎn)最近的訪問點(diǎn)作為第二個基因，再從余下的任務(wù)點(diǎn)中找到距離第二訪問點(diǎn)最近的點(diǎn)，以此類推，直到完成n個任務(wù)點(diǎn)的遍歷。采用貪心算法[16]生成的個體在種群中的占比不大，不會影響種群多樣性，但能提高種群的質(zhì)量，有助于加快尋優(yōu)速度。

（3）適應(yīng)度函數(shù)選取。目標(biāo)函數(shù)為個體路徑總長度，取目標(biāo)函數(shù)的倒數(shù)作為適應(yīng)度函數(shù)。

（4）算子選擇。采用輪盤賭和最佳保留策略選擇算子。在保留最佳個體的同時引入新個體，提高算法收斂全局最優(yōu)的可能性。

（5）算子交叉。采用兩點(diǎn)交叉方法，在兩個個體編碼中設(shè)置兩個交叉點(diǎn)，對兩點(diǎn)間的基因進(jìn)行互換。之后進(jìn)行基因沖突檢測，根據(jù)兩組交換的基因建立映射關(guān)系，沖突基因經(jīng)過映射，形成子代無沖突基因編碼個體。

（6）算子變異。采用倒位變異算子[17]，在父個體中隨機(jī)選取兩個任務(wù)點(diǎn)并標(biāo)記，使這兩個任務(wù)點(diǎn)之間的基因倒序排列，并將兩個標(biāo)記的任務(wù)點(diǎn)相鄰排列，產(chǎn)生一個新個體。

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

為測試USV多路徑點(diǎn)路徑規(guī)劃算法的有效性，構(gòu)建環(huán)境模型仿真驗(yàn)證。USV的任務(wù)點(diǎn)共11個，起始點(diǎn)坐標(biāo)為（20 m，20 m），各任務(wù)點(diǎn)的坐標(biāo)見圖10。

利用第2.4節(jié)的改進(jìn)遺傳算法，確定多任務(wù)點(diǎn)訪問順序。每個個體采用11個路徑點(diǎn)編碼，初始種群大小設(shè)置為N=100，其中80個個體隨機(jī)產(chǎn)生，20個個體采用貪心算法生成，迭代次數(shù)n=1 000。算法經(jīng)過選擇、交叉和變異操作，逐漸收斂，最終確定最優(yōu)個體值為672，該個體路徑點(diǎn)順序?yàn)?→11→10→3→2→4→5→6→7→8→9。因此USV的最佳巡航距離為672 m。

在確定好各路徑點(diǎn)間距離和訪問順序的基礎(chǔ)上，規(guī)劃出USV最佳巡航路徑，見圖11。該路徑滿足USV的運(yùn)動學(xué)約束且巡航距離最短。

4 結(jié) 論

本文提出一種水面無人艇（USV）多任務(wù)點(diǎn)路徑規(guī)劃方法。首先建立了一種融合USV運(yùn)動學(xué)約束的快速探索隨機(jī)樹（RRT）算法，將USV的運(yùn)動狀態(tài)信息融入算法，規(guī)劃兩個任務(wù)點(diǎn)之間滿足USV運(yùn)動學(xué)約束的路徑，并得到兩個任務(wù)點(diǎn)之間的距離。其次提出RRT優(yōu)化路徑算法，針對RRT算法的隨機(jī)性特點(diǎn)，刪減一些不必要的路徑點(diǎn)，在一定程度上縮短路徑長度，且避免過多的轉(zhuǎn)向次數(shù)。最后通過改進(jìn)遺傳算法確定多個任務(wù)點(diǎn)的訪問順序，獲得最短巡航距離，節(jié)約巡航成本。

參考文獻(xiàn)：

[1] 張樹凱， 劉正江， 蔡垚， 等. 無人船艇航線自動生成研究現(xiàn)狀及展望[J]. 中國航海， 2019， 42（3）： 6-11.

[2] 于振中， 李強(qiáng)， 樊啟高. 智能仿生算法在移動機(jī)器人路徑規(guī)劃優(yōu)化中的應(yīng)用綜述[J]. 計(jì)算機(jī)應(yīng)用研究， 2019， 36（11）： 3210-3219. DOI： 10.19734/j.issn.1001-3695.2018.07.0483.

[3] 隨博文， 黃志堅(jiān). 基于改進(jìn)A *算法的水面無人艇路徑規(guī)劃[J]. 艦船科學(xué)技術(shù)， 2019， 41（12）： 162-166. DOI： 10.3404/j.issn.1672-7649.2019.12.031.

[4] 范云生， 趙永生， 石林龍， 等. 基于電子海圖柵格化的無人水面艇全局路徑規(guī)劃[J]. 中國航海， 2017， 40（1）： 47-52， 113.

[5] 歐陽子路， 王鴻東， 王檢耀， 等. 基于改進(jìn)Bi-RRT的無人水面艇自動避碰算法[J]. 中國艦船研究， 2019， 14（6）： 8-14. DOI： 10.19693/j.issn.1673-3185.01450.

[6] 楊左華， 王玉龍， 戚愛春. 基于改進(jìn)RRT*算法的無人艇全局避障規(guī)劃[J]. 艦船科學(xué)技術(shù)， 2019， 41（12）： 167-172. DOI： 10.3404/j.issn.1672-7649.2019.12.032.

[7] 莊佳園， 張磊， 孫寒冰， 等. 應(yīng)用改進(jìn)隨機(jī)樹算法的無人艇局部路徑規(guī)劃[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)， 2015， 47（1）： 112-117. DOI： 10.11918/j.issn.0367-6234.2015.01.017.

[8] 曹凱， 高佳佳， 李昂. 基于RRT優(yōu)化算法的移動機(jī)器人路徑規(guī)劃[J]. 兵工自動化， 2018， 37（9）： 74-79. DOI： 10.7690/bgzdh.2018.09.019.

[9] 徐娜， 陳雄， 孔慶生， 等. 非完整約束下的機(jī)器人運(yùn)動規(guī)劃算法[J]. 機(jī)器人， 2011， 33（6）： 666-672. DOI： 10.3724/SP.J.1218.2011.00666.

[10] 宋金澤， 戴斌， 單恩忠， 等. 一種改進(jìn)的RRT路徑規(guī)劃算法[J]. 電子學(xué)報(bào)， 2010， 38（2A）： 225-228.

[11] LOE A G. Collision avoidance for unmanned surface vehicles[D]. Trondheim： Norwegian University of Science and Technology， 2008.

[12] 肖春暉， 鄒媛媛， 李少遠(yuǎn). 有障礙區(qū)域的多無人機(jī)多目標(biāo)點(diǎn)路徑規(guī)劃[J]. 空間控制技術(shù)與應(yīng)用， 2019， 45（4）： 46-52. DOI： 10.3969/j.issn.1674-1579.2019.04.006.

[13] 蒲興成， 李俊杰， 吳慧超， 等. 基于改進(jìn)粒子群算法的移動機(jī)器人多目標(biāo)點(diǎn)路徑規(guī)劃[J]. 智能系統(tǒng)學(xué)報(bào)， 2017， 12（3）： 301-309. DOI： 10.11992/tis.201606046.

[14] WANG Yanlong， YU Xuemin， LIANG Xu. Design and implementation of global path planning system for unmanned surface vehicle among multiple task points[J]. International Journal of Vehicle Autonomous Systems， 2018， 14（1）： 82-105.

[15] 蒲紅紅， 黃海濱. 無人水面航行器全局路徑規(guī)劃方法研究[J]. 海洋科學(xué)， 2018， 42（1）： 93-105. DOI： 10.11759/hykx20171011008.

[16] 蔣然. 改進(jìn)遺傳算法在TSP問題中的應(yīng)用[J]. 軟件導(dǎo)刊， 2016， 15（12）： 127-129. DOI： 10.11907/rjdk.162168.

[17] 謝勝利， 唐敏， 董金祥. 求解TSP問題的一種改進(jìn)的遺傳算法[J]. 計(jì)算機(jī)工程與應(yīng)用， 2002（8）： 58-60， 245.

（編輯 賈裙平）