無人艇智能航路規(guī)劃系統(tǒng)研究

賈 宇，艾 維

(華中光電技術(shù)研究所武漢光電國家研究中心，湖北武漢430223)

0 引 言

當(dāng)前，無人平臺(tái)的發(fā)展速度越來越快，有關(guān)研究也越來越廣泛。無人平臺(tái)是指無人操作的智能性任務(wù)執(zhí)行平臺(tái)，其包括無人機(jī)、無人船、無人車以及機(jī)器人等，是相關(guān)無人設(shè)備的總稱，如圖1所示。其技術(shù)涉及到機(jī)械設(shè)計(jì)、運(yùn)動(dòng)控制、數(shù)據(jù)融合、人工智能等多個(gè)專業(yè)領(lǐng)域，研究內(nèi)容包括任務(wù)規(guī)劃與智能決策、路徑規(guī)劃與智能避障、自主航行控制、綜合導(dǎo)航與高精度定位、智能感知、遠(yuǎn)程無線高速通信、武器發(fā)射控制等眾多方面。進(jìn)入21世紀(jì)以來，隨著計(jì)算機(jī)、網(wǎng)絡(luò)、傳感器、人工智能等技術(shù)的發(fā)展，這些平臺(tái)的“ 無人化”水平越來越高，多種學(xué)科交叉融合的“無人技術(shù)”在其中扮演越來越重要的角色，同時(shí)促進(jìn)了多種無人平臺(tái)的發(fā)展，無人平臺(tái)的發(fā)展趨勢也由遙控型向半自主、全自主發(fā)展[1]。根據(jù)無人平臺(tái)智能程度的不同可將其分為3類：1）遙控型無人平臺(tái)，如遙控?zé)o人機(jī)等；2）半自主型的無人平臺(tái)，指能夠按照預(yù)設(shè)程序航行并完成相應(yīng)任務(wù)的無人平臺(tái)，如有人/無人半自主的無人船等；3）具有自主任務(wù)規(guī)劃、自主航行功能，并可通過搭載的不同傳感器及設(shè)備自主完成環(huán)境感知目標(biāo)探測等任務(wù)的全自主型無人平臺(tái)。

圖1 無人平臺(tái)分類Fig.1 Classification of unmanned platforms

本文以無人水面艇（Unmanned Surface Vehicle，USV）為代表的無人平臺(tái)來分析，其中航路規(guī)劃系統(tǒng)在無人平臺(tái)中起著至關(guān)重要的作用，也是體現(xiàn)無人平臺(tái)自主性與智能性關(guān)鍵技術(shù)。無人艇的航路規(guī)劃是指在具有障礙物的環(huán)境中，能夠規(guī)劃出一條從起始位置狀態(tài)到目標(biāo)位置狀態(tài)無碰撞的最優(yōu)路徑或次優(yōu)路徑，并滿足所有約束條件，最優(yōu)狀態(tài)下完成設(shè)定任務(wù)以及處理突發(fā)情況。對于無人艇的航路規(guī)劃系統(tǒng)而言，根據(jù)航路規(guī)劃的作用范圍，可以將其分為路徑規(guī)劃、任務(wù)規(guī)劃兩大規(guī)劃單元。路徑規(guī)劃單元根據(jù)作用域的大小可以進(jìn)一步細(xì)分為全局路徑規(guī)劃和局部路徑規(guī)劃[2]。任務(wù)規(guī)劃單元更加側(cè)重于執(zhí)行對應(yīng)的任務(wù)時(shí)路徑規(guī)劃的策略研究，也可稱之為任務(wù)策略。目前全局路徑規(guī)劃方法主要有可視圖法[3]、自由空間法、Voronoi圖法、柵格法[4]、A*搜索算法[5]，RRT算法[6]，粒子群算法[7]、蟻群算法、遺傳算法等。局部路徑規(guī)劃方法主要有人工勢場法[8]、向量場直方圖法[9]、動(dòng)態(tài)窗口法、速度障礙法[10]等優(yōu)化避障方法。本文提出基于無人艇自身信息與外界環(huán)境信息的智能航行系統(tǒng)，針對無人艇的任務(wù)執(zhí)行與軌跡生成，基于無人艇自身特性設(shè)計(jì)了相關(guān)規(guī)劃單元，更好地符合無人艇的特性，提高無人艇的自主航行與智能規(guī)劃能力。

1 智能航路規(guī)劃系統(tǒng)設(shè)計(jì)

1.1 總體框架設(shè)計(jì)

智能航行規(guī)劃系統(tǒng)整體模塊框架如圖2所示，其設(shè)計(jì)是基于分層策略展開的。

整個(gè)智能航行系統(tǒng)各單元功能如下：

1）人機(jī)交互單元。主要實(shí)現(xiàn)任務(wù)下發(fā)、確定策略，數(shù)據(jù)加載及實(shí)時(shí)顯示功能。完成相關(guān)初始化的任務(wù)，包括本地?cái)?shù)據(jù)庫的下載，相關(guān)地圖的加載，設(shè)備初始化驗(yàn)證工作等。

圖2 智能航行系統(tǒng)單元示意圖Fig.2 Schematic diagram of intelligent navigation system unit

2）全局規(guī)劃單元。主要實(shí)現(xiàn)大范圍區(qū)域的全局路徑規(guī)劃設(shè)計(jì)。根據(jù)給點(diǎn)的起點(diǎn)、終點(diǎn)信息，按照目標(biāo)約束策略，進(jìn)行大范圍粗粒度的路徑規(guī)劃。

3）局部規(guī)劃單元。主要實(shí)現(xiàn)小范圍區(qū)域的航路重規(guī)劃設(shè)計(jì)。通過感知傳感器更新數(shù)據(jù)，探測未知的靜態(tài)障礙物，并調(diào)整航路，避開危險(xiǎn)障礙物。

4）任務(wù)規(guī)劃單元。主要實(shí)現(xiàn)在任務(wù)執(zhí)行的局部區(qū)域內(nèi)，完成區(qū)域內(nèi)的路徑規(guī)劃設(shè)計(jì)。需針對幾種特定的任務(wù)，制定相關(guān)的任務(wù)策略，快速、及時(shí)完成相應(yīng)任務(wù)。

5）底層避障單元。主要實(shí)現(xiàn)近距離的動(dòng)態(tài)障礙物的避碰調(diào)整，保證無人艇在動(dòng)態(tài)環(huán)境下的安全性。利用傳感器信息探測動(dòng)態(tài)船舶等信息，實(shí)時(shí)調(diào)整本艇的航向航速，達(dá)到安全航行的目的。

6）軌跡規(guī)劃單元。主要實(shí)現(xiàn)無人艇在動(dòng)力學(xué)約束下的軌跡優(yōu)化問題，保證無人艇的軌跡可行，實(shí)時(shí)可控。

7）異常處理單元。主要實(shí)現(xiàn)無人艇發(fā)生異常情況下，規(guī)劃策略調(diào)整或緊急制動(dòng)。

1.2 航路規(guī)劃算法設(shè)計(jì)

智能航行系統(tǒng)的分層策略包括兩部分：慎思式AI模型，也稱為基于模型的規(guī)劃法。是以環(huán)境模型為基礎(chǔ)進(jìn)行全局無碰路徑規(guī)劃，可方便找到全局目標(biāo)，難以適應(yīng)動(dòng)態(tài)變化的環(huán)境，一般用于全局與局部。反應(yīng)式AI模型，不需要事先進(jìn)行規(guī)劃，而是讓平臺(tái)直接去響應(yīng)環(huán)境變化，根據(jù)傳感器探測的環(huán)境數(shù)據(jù)實(shí)時(shí)的回避障礙，具有快速、實(shí)時(shí)、高效的特點(diǎn)，一般用于局部與底層避障。其中規(guī)劃的分層策略設(shè)計(jì)為：任務(wù)規(guī)劃、路徑規(guī)劃、智能避障3個(gè)部分，分別對應(yīng)不同階段的規(guī)劃方案。其數(shù)據(jù)流圖如圖3所示。

其中，全局路徑規(guī)劃是在已知海洋信息（電子海圖）前提下，建立數(shù)字海圖環(huán)境模型，并通過一定的搜索算法搜索路徑，進(jìn)行全局范圍路徑規(guī)劃；而局部路徑規(guī)劃是根據(jù)無人艇上的雷達(dá)以及視覺傳感器對四周未知環(huán)境信息進(jìn)行探測，進(jìn)而得到無人艇的周圍未知障礙物分布信息，最后通過安全有效的算法對無人艇進(jìn)行方向和速度控制，達(dá)到避開障礙物的目標(biāo)，實(shí)

現(xiàn)智能避碰，屬于小范圍規(guī)劃。其中全局規(guī)劃采用的A*算法，局部規(guī)劃采用的基于碰撞錐的局部點(diǎn)插入算法，在底層避障上采用的是速度避障法，都有成熟算法介紹，本文重點(diǎn)關(guān)注無人艇智能航行的任務(wù)規(guī)劃與軌跡規(guī)劃算法。算法流程如圖4所示。

圖4 無人艇智能航行系統(tǒng)算法流程圖Fig.4 Flow chart of intelligent navigation system of USV

1.2.1 任務(wù)規(guī)劃算法

任務(wù)規(guī)劃與避障的更新策略不同，避障需要不斷的實(shí)時(shí)刷新，但任務(wù)規(guī)劃可以采取一定的時(shí)間間隔進(jìn)行更新，只需確保實(shí)際過程中任務(wù)的偏差量符合任務(wù)要求。為區(qū)別于普通航路點(diǎn)信息，任務(wù)規(guī)劃生成的或者任務(wù)要求的點(diǎn)統(tǒng)一定義為任務(wù)點(diǎn)，屬性要多于普通航路點(diǎn)，優(yōu)先級更高，而由避障產(chǎn)生的點(diǎn)定義為避障點(diǎn)，其優(yōu)先級最高。當(dāng)三者同時(shí)產(chǎn)生的時(shí)候，優(yōu)先避障，其次執(zhí)行任務(wù)點(diǎn)，最后是航行點(diǎn)。因此可以將航路點(diǎn)屬性分為3類：航行點(diǎn)、任務(wù)點(diǎn)、避障點(diǎn)（見表1）。航行點(diǎn)為離線算法生成得到，任務(wù)點(diǎn)可以離線設(shè)定或者實(shí)時(shí)生成，避障點(diǎn)則是由實(shí)時(shí)生成得到的。

表1 航路點(diǎn)的屬性表Tab.1 The property table of navigation points

USV任務(wù)規(guī)劃根據(jù)已知作業(yè)環(huán)境信息，與外界感知得到的數(shù)據(jù)相融合，在任務(wù)區(qū)域內(nèi)，執(zhí)行相應(yīng)的任務(wù)。其中感知數(shù)據(jù)包括光電信息，雷達(dá)信息，導(dǎo)航信息等，光電信息，雷達(dá)信息可以感知目標(biāo)的方位與速度等，導(dǎo)航信息可以感知當(dāng)前無人艇所處的位置信息。一旦進(jìn)入相應(yīng)的任務(wù)區(qū)域，則執(zhí)行任務(wù)規(guī)劃策略。

以定點(diǎn)巡邏任務(wù)為例，定點(diǎn)巡邏是指無人艇在一定區(qū)域內(nèi)，對區(qū)域內(nèi)幾個(gè)定點(diǎn)位置進(jìn)行探測巡邏（順序可以為有序也可以為無序）。算法流程如下：

1）先將起始位置點(diǎn)與所需巡邏點(diǎn)構(gòu)建一個(gè)拓?fù)鋱D，其中拓?fù)鋱D的構(gòu)建可采用A星算法，對兩兩最短路徑進(jìn)行探索，得出兩兩之間的距離拓?fù)鋱D，并記錄兩兩之間序列點(diǎn)。

2）根據(jù)巡邏順序進(jìn)行判斷，若有序，則按照規(guī)定順序規(guī)劃一條合理路徑巡邏，若無序，則利用動(dòng)態(tài)規(guī)劃算法[11]，設(shè)定相應(yīng)的代價(jià)，主要考慮對無人艇航行有較大影響的因素距離代價(jià)、轉(zhuǎn)彎代價(jià)。

3）合理規(guī)劃出一條最小代價(jià)的航路，實(shí)現(xiàn)對各個(gè)巡邏點(diǎn)的監(jiān)測，獲取路徑序列點(diǎn)。

4）利用軌跡規(guī)劃的算法在巡邏點(diǎn)中插值上述序列點(diǎn)，規(guī)劃出一條離線無碰最短路徑。

以實(shí)際的例子來說明，在柵格化之后的地圖上，隨機(jī)生成幾個(gè)巡邏點(diǎn)，通過A星算法得出兩兩點(diǎn)之間的距離，然后利用動(dòng)態(tài)規(guī)劃算法規(guī)劃好相關(guān)路徑，如圖5所示，路徑2為得到的路徑，并根據(jù)運(yùn)動(dòng)學(xué)約束模擬無人艇的行進(jìn)路徑，進(jìn)行碰撞檢測，最終生成無人艇可航行路徑，如路徑1所示。

1.2.2 軌跡規(guī)劃算法

對于原始航跡中存在直線航跡段，無人艇按此直線航行，而無需考慮其他機(jī)動(dòng)性能約束。但在相鄰航跡段之間的銜接存在一定的夾角，此時(shí)必須進(jìn)行轉(zhuǎn)彎，所以保持直航顯然不可能，此時(shí)需要滿足船速、船首向、角速度、最小轉(zhuǎn)彎半徑等約束。

圖5 無人艇定點(diǎn)巡邏任務(wù)示意圖Fig.5 Sketch map of fixed-point patrol m ission of USV

首先需要結(jié)合無人艇的船體模型來分析無人艇的動(dòng)力學(xué)特性，在動(dòng)力學(xué)分析上，無人艇一般來說是具有非完整約束?？梢远x無人艇的動(dòng)力學(xué)模型為(x,y,φ,v,ω)， 其中 (x,y) 描述的無人艇的當(dāng)前位置，φ描述無人艇當(dāng)前航向，(v,ω)描述無人艇當(dāng)前速度和角速度。可以推導(dǎo)得出在?t間隔時(shí)間內(nèi)無人艇的狀態(tài)更新方程：

其 中 (v˙,w˙)分 別 為加速度 和角加速度。在?t時(shí) 間內(nèi)，無人艇的轉(zhuǎn)彎參數(shù)也可以計(jì)算出來：

由無人艇的動(dòng)力學(xué)特性可以確定無人艇的動(dòng)力學(xué)參數(shù)。在軌跡規(guī)劃單元中，通過動(dòng)力學(xué)參數(shù)，可以得到無人艇的速度，角速度的采樣區(qū)間：

其中 (v˙max,w˙max)分別為最大的加速度和角加速度，而最大加速度的則是由船體本身的動(dòng)力學(xué)特性分析，由此可以得到速度約束下的無人艇的最小轉(zhuǎn)彎半徑rmin。

B樣條曲線法[12]是目前比較實(shí)用的航跡平滑算法，該方法具有整體全局優(yōu)化的優(yōu)勢，因此所獲得的航跡整體過渡自然，B樣條函數(shù)具有曲率變化比較均勻及一、二階導(dǎo)數(shù)連續(xù)等特點(diǎn)，因此可滿足無人艇實(shí)際航行中速度及加速度連續(xù)變化的要求。航跡的B樣條曲線的曲率表達(dá)式推導(dǎo)如下。每一段二次均勻B樣條曲線參數(shù)方程記作：

根據(jù)曲線曲率計(jì)算公式：

考慮到在實(shí)際運(yùn)行中，生成的軌跡可能會(huì)與障礙物發(fā)生碰撞，如圖6所示，線條1代表原規(guī)劃路徑，路徑2為采樣路徑，無人艇由ab方向轉(zhuǎn)到bc方向，通過圓形擬合選取一個(gè)較大的半徑R時(shí)候，此時(shí)發(fā)現(xiàn)此路徑經(jīng)過了障礙物點(diǎn)，因此此條路徑不可取?；诖?，需要對生成的曲線進(jìn)行碰撞檢測，當(dāng)檢測到路線經(jīng)過障礙物時(shí)，需要對路線進(jìn)行一個(gè)重新采樣，通過在當(dāng)前半徑上加減，設(shè)定不同的采樣半徑，形成一組不同半徑的采用空間，直至選取了一條無障礙軌跡，確定最終的轉(zhuǎn)彎半徑，完成最終的軌跡規(guī)劃方案。以5個(gè)航路點(diǎn)做測試序列，實(shí)際規(guī)劃的路徑如圖中路徑2，由于大轉(zhuǎn)彎與多轉(zhuǎn)彎的存在，無人艇無法準(zhǔn)確按照此路徑行駛，此時(shí)采用B樣條曲線法擬合以及設(shè)定無人艇的轉(zhuǎn)彎半徑，最終得到一條合理的路徑如圖中路徑1所示，既能保持起點(diǎn)與終點(diǎn)的斜率一致，又能滿足無人艇的運(yùn)動(dòng)約束，實(shí)現(xiàn)對無人艇的最優(yōu)規(guī)劃，便于無人艇后期的控制。

圖6 軌跡規(guī)劃采樣圖Fig.6 Sampling diagram of trajectory planning

圖7 軌跡規(guī)劃仿真Fig.7 The simulation of trajectory planning

2 實(shí)驗(yàn)結(jié)果

圖8 GIS圖與柵格化結(jié)果Fig.8 GISmap and rasterized results

利用GIS信息，并用柵格化的算法對渤海灣區(qū)域構(gòu)建出二值化編碼的地圖[13]。圖8為GIS圖與柵格化結(jié)果。針對智能航行系統(tǒng)開展仿真測試，在地圖上設(shè)定起點(diǎn)在（120°E，38°N），終點(diǎn)在（132°E，39°N），以柵格化的電子海圖為參考，其中線1為全局規(guī)劃的結(jié)果，線2為對全局規(guī)劃進(jìn)行局部任務(wù)規(guī)劃，確定關(guān)鍵任務(wù)點(diǎn)，最后利用這些關(guān)鍵點(diǎn)信息，進(jìn)行軌跡規(guī)劃，形成最終的曲線3，其結(jié)果驗(yàn)證了本論文提出的智能航行系統(tǒng)的有效性。圖9為形成的航路規(guī)劃系統(tǒng)界面圖與規(guī)劃結(jié)果。

圖 9航路規(guī)劃結(jié)果與界面Fig. 9 Resultsand interface of route planning

3 結(jié) 語

無人艇航路規(guī)劃系統(tǒng)實(shí)現(xiàn)合理規(guī)劃航路、緊急避碰、完成指定任務(wù)等，極大地降低無人艇對遠(yuǎn)程操控人員的依賴，同時(shí)擴(kuò)展無人艇的多任務(wù)作戰(zhàn)能力。該系統(tǒng)能夠讓無人艇具有較高的自主路徑規(guī)劃和智能避障能力，同時(shí)無人艇可以獨(dú)立地執(zhí)行遠(yuǎn)程探測、信息搜集、巡邏跟蹤等任務(wù)，基本上實(shí)現(xiàn)了無人艇的全自主性與智能性。