程 宇，付悅文，李 魯

（1.海軍裝備部駐武漢地區(qū)第五軍事代表室，武漢 430000；2.江蘇自動(dòng)化研究所，江蘇 連云港 222000；3.中國船舶工業(yè)貿(mào)易有限公司，北京 100000）

0 引言

無人水面艇具有成本低、體積小、航時(shí)長(zhǎng)、速度快、智能化等優(yōu)點(diǎn)，已經(jīng)成為當(dāng)今軍事和民用領(lǐng)域發(fā)展的重點(diǎn)，特別是軍事領(lǐng)域［1］。國內(nèi)外學(xué)者對(duì)無人艇技術(shù)的研究較為全面，尤其在無人艇相關(guān)算法研究成果較為豐富，但是對(duì)仿真系統(tǒng)環(huán)境研究較少，普遍為單一特定功能，結(jié)構(gòu)較為簡(jiǎn)單，未結(jié)合實(shí)際應(yīng)用場(chǎng)景［6］。本文所研究的自主航行仿真系統(tǒng)具有全局航路、無人艇操縱性運(yùn)動(dòng)模型、環(huán)境信息、避障和航行控制等功能模塊，提供了相對(duì)完備的自主航行仿真系統(tǒng)，系統(tǒng)可實(shí)現(xiàn)模擬無人艇水上航行運(yùn)動(dòng)和環(huán)境信息，支持無人艇探測(cè)巡邏、目標(biāo)跟蹤等任務(wù)推演，也可回放實(shí)海域歷史數(shù)據(jù)，為無人艇提供算法調(diào)試環(huán)境、數(shù)據(jù)分析處理功能等，為無人艇開發(fā)和試驗(yàn)提供一個(gè)多功能的仿真驗(yàn)證平臺(tái)，可以有效減少海上調(diào)試試驗(yàn)周期，為海上試驗(yàn)做好充分保障和準(zhǔn)備。

1 無人水面艇自主航行控制仿真系統(tǒng)方案

無人水面艇自主航行控制仿真系統(tǒng)數(shù)據(jù)通信方案的流程如圖1 所示，各模塊之間主要通過UDP協(xié)議進(jìn)行數(shù)據(jù)通信。

圖1 無人水面艇自主航行控制仿真系統(tǒng)通信方案Fig.1 Communication scheme of autonomous navigation control simulation system of USV

無人水面艇自主航行控制仿真系統(tǒng)流程方案如圖2所示，顯控模塊顯示規(guī)劃航路、無人艇導(dǎo)航信息、航行態(tài)勢(shì)信息等，環(huán)境信息模塊實(shí)時(shí)地輸出周圍航行目標(biāo)信息至避障和全局規(guī)劃，全局規(guī)劃航路輸出至避障和航行控制模塊，避障對(duì)航行態(tài)勢(shì)進(jìn)行安全評(píng)估，若發(fā)生航行安全，進(jìn)行避障規(guī)劃，根據(jù)當(dāng)前情況輸出避障解算的期望航速、航向以及規(guī)劃航路傳給航行控制模塊，航行控制模塊根據(jù)規(guī)劃航路、期望航速、航向輸出相應(yīng)的舵角、油門傳給無人水面艇模型，無人水面艇操縱性運(yùn)動(dòng)模型根據(jù)舵角、油門和環(huán)境干擾，實(shí)時(shí)輸出當(dāng)前的姿態(tài)和位置信息給到避障模塊，直到任務(wù)結(jié)束。

圖2 無人艇自主航行控制仿真系統(tǒng)流程方案Fig.2 The process scheme of autonomous navigation control simulation system of USV

2 無人水面艇自主航行控制仿真系統(tǒng)構(gòu)成

2.1 顯控模塊

2.1.1 地圖顯示

顯控模塊是在開源柵格地理數(shù)據(jù)庫GDAL和開源跨平臺(tái)開發(fā)框架Qt 的基礎(chǔ)上實(shí)現(xiàn)的。遙感圖像一般較大，無法全部讀入內(nèi)存，結(jié)合Qt 的圖形視圖框架，可以只讀取當(dāng)前視圖內(nèi)對(duì)應(yīng)的圖像數(shù)據(jù)，并完成顯示圖像功能，減少計(jì)算機(jī)的內(nèi)存消耗，提高計(jì)算資源的利用效率。另外，遙感圖像除了包含原始的光柵數(shù)據(jù)外，還包含一些與之相關(guān)的信息，如圖像尺寸、波段數(shù)、空間投影信息等，在使用GDAL函數(shù)讀取圖像數(shù)據(jù)之前，可以先將這些信息讀取出來。遙感衛(wèi)星地圖相比海圖具有更清晰直觀的顯示效果、更低的價(jià)格成本以及開發(fā)時(shí)間，同時(shí)顯示范圍更大，也可支持內(nèi)河的顯示。針對(duì)實(shí)際環(huán)境中存在的固定礙行區(qū)可導(dǎo)入實(shí)海域試驗(yàn)數(shù)據(jù)進(jìn)行標(biāo)注，或?qū)胂闰?yàn)高精度激光雷達(dá)探測(cè)生成的柵格地圖，基本滿足無人艇仿真環(huán)境的功能實(shí)現(xiàn)，因此，采用了遙感衛(wèi)星地圖，如下頁圖3所示，為日照帆船基地港口地圖。

圖3 顯控界面圖Fig.3 Display and control interface

遙感圖像數(shù)據(jù)集的地理參照坐標(biāo)系統(tǒng)采用WKT 描述，GDAL 使用仿射坐標(biāo)系來描述遙感圖像像素坐標(biāo)與地理坐標(biāo)之間的關(guān)系，如式（1）所示。

2.1.2 全局航路規(guī)劃

A*算法又稱為啟發(fā)式搜索算法［3］，適用于在空間內(nèi)對(duì)已知起點(diǎn)至終點(diǎn)最小代價(jià)路徑的求解，航路規(guī)劃選用A*規(guī)劃算法時(shí)，對(duì)其規(guī)劃空間進(jìn)行網(wǎng)格化，網(wǎng)格密度越高，規(guī)劃精度就越高，計(jì)算量和運(yùn)行時(shí)間也就越大，一般根據(jù)實(shí)際需求進(jìn)行網(wǎng)格密度設(shè)定，網(wǎng)格數(shù)據(jù)填入網(wǎng)格中心點(diǎn)的經(jīng)緯度以及網(wǎng)格坐標(biāo)，通過計(jì)算代價(jià)函數(shù)計(jì)算最優(yōu)代價(jià)航路。代價(jià)函數(shù)中加入啟發(fā)函數(shù)，使得規(guī)劃更有目的性，更快速地到達(dá)終點(diǎn)目標(biāo)，避免遍歷所有節(jié)點(diǎn)，從而提高搜索效率。A*算法的代價(jià)函數(shù)組成如下：

其中，f（n）為在節(jié)點(diǎn)n的總代價(jià)；g（n）是從初始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)；h（n）是從當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)，因?yàn)閔（n）為啟發(fā)函數(shù)，當(dāng)函數(shù)h（n）滿足條件時(shí)，如式（3）所示，A*算法存在最優(yōu)解且是唯一解。

其中，h*（n）是節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的真實(shí)最小代價(jià)。

在柵格化的搜索空間中，A*算法通過計(jì)算當(dāng)前節(jié)點(diǎn)代價(jià)與到周圍節(jié)點(diǎn)的代價(jià)之和，選取周圍最小代價(jià)的節(jié)點(diǎn)加入到集合中，作為下一節(jié)點(diǎn)進(jìn)行擴(kuò)展，同時(shí)更新代價(jià)函數(shù)，繼續(xù)下一輪的擴(kuò)展計(jì)算，直到找到滿足式（3）條件的節(jié)點(diǎn)為止。A*算法的基本流程：首先建立open 表和close 表，當(dāng)前節(jié)點(diǎn)周圍可通行點(diǎn)存放在open 表中［10］，已經(jīng)訪問過的節(jié)點(diǎn)存放在close表中，open表為優(yōu)先隊(duì)列，升序排列，隊(duì)列中第1 個(gè)節(jié)點(diǎn)為最優(yōu)節(jié)點(diǎn)，并將其放入close 表中存放，同時(shí)作為下一步的起始節(jié)點(diǎn)，當(dāng)搜索到目標(biāo)點(diǎn)或者open表為空時(shí)搜索結(jié)束。A*算法流程圖如圖4所示。

圖4 A*算法流程圖Fig.4 A*algorithm flow chart

A*規(guī)劃的路徑點(diǎn)相隔過密且折線較多，不適合直接作為無人艇的航路，可對(duì)其原始規(guī)劃航路進(jìn)行優(yōu)化，剔除多余航路點(diǎn)且不影響航行安全，將折線變?yōu)橹本€，保證不會(huì)出現(xiàn)鋸齒階梯狀規(guī)劃航路，路徑光順度提升，符合無人艇航行控制的要求，有利于無人艇任務(wù)執(zhí)行。本文采用的優(yōu)化方法為：規(guī)劃路徑點(diǎn)中選取連續(xù)點(diǎn)Pi、Pi+1、Pi+2，若Pi與Pi+2之間的連線不穿過任何障礙物，則刪去Pi+1；然后，連續(xù)判斷Pi與Pi+2后續(xù)節(jié)點(diǎn)連線，連線不碰撞任何障礙物，則刪去Pi+2；依次類推，直到Pi與后續(xù)節(jié)點(diǎn)直連穿過障礙物，并選取后續(xù)3 個(gè)節(jié)點(diǎn)賦值給Pi、Pi+1、Pi+2點(diǎn)，重復(fù)上述操作，直到遍歷完規(guī)劃路徑點(diǎn)。

2.1.3 任務(wù)推演

任務(wù)推演是任務(wù)規(guī)劃系統(tǒng)的主要功能之一，它是銜接任務(wù)規(guī)劃和任務(wù)執(zhí)行的重要環(huán)節(jié)之一。當(dāng)完成任務(wù)規(guī)劃后，通過任務(wù)推演系統(tǒng)對(duì)將要進(jìn)行的任務(wù)航路，通過本文的仿真系統(tǒng)進(jìn)行動(dòng)態(tài)反復(fù)的預(yù)先演示。通過演示結(jié)果，評(píng)判任務(wù)規(guī)劃的合理與否，并進(jìn)行任務(wù)的調(diào)整。在提前預(yù)設(shè)的偵察背景下，根據(jù)任務(wù)規(guī)劃中預(yù)先設(shè)定的任務(wù)以及航路，對(duì)有可能影響任務(wù)執(zhí)行結(jié)果的不同關(guān)鍵環(huán)節(jié)進(jìn)行連續(xù)重復(fù)的演練、調(diào)整，直到滿足任務(wù)執(zhí)行的條件。

本仿真系統(tǒng)基于無人艇的功能需求，對(duì)其任務(wù)進(jìn)行設(shè)計(jì)，以任務(wù)規(guī)劃為基礎(chǔ)，設(shè)計(jì)具備包括警戒巡邏、疑似目標(biāo)跟蹤或伴行、返航航行、定點(diǎn)搜索、目標(biāo)確認(rèn)等任務(wù)場(chǎng)景的推演能力，可輔助人員確認(rèn)任務(wù)執(zhí)行的合理性，圖5為某區(qū)域巡邏任務(wù)規(guī)劃。

圖5 巡邏任務(wù)規(guī)劃Fig.5 Patrol mission planning

2.1.4 數(shù)據(jù)回放

將從無人艇實(shí)海域測(cè)試網(wǎng)絡(luò)采集記錄得到的網(wǎng)絡(luò)包通過網(wǎng)絡(luò)仿真系統(tǒng)數(shù)據(jù)回放功能在地面實(shí)驗(yàn)室環(huán)境下進(jìn)行分析回放，可以真實(shí)再現(xiàn)艇上航行數(shù)據(jù)狀態(tài)及網(wǎng)絡(luò)狀態(tài)，從而不需要在艇載軟件開發(fā)調(diào)試過程在艇上與設(shè)備連接，同時(shí)也可以回放真實(shí)場(chǎng)景用來測(cè)試自主航行算法軟件，縮短了算法軟件調(diào)試開發(fā)時(shí)間，有效節(jié)約試驗(yàn)資源，提高軟件開發(fā)集成效率。主要利用WinPcap 提供的API 函數(shù)，對(duì)無人艇上交換機(jī)設(shè)備采集的pcap 格式IENA 網(wǎng)絡(luò)包進(jìn)行解析回放，數(shù)據(jù)回放功能主要包括：

1）數(shù)據(jù)回放功能：能夠打開保存到本地的歷史網(wǎng)絡(luò)報(bào)文數(shù)據(jù)包，將數(shù)據(jù)包存儲(chǔ)的網(wǎng)絡(luò)報(bào)文按照時(shí)間順序通過網(wǎng)絡(luò)回放發(fā)出。

2）報(bào)文方案設(shè)定功能：具體數(shù)據(jù)添加、修改、系數(shù)、刪除、導(dǎo)入等功能，根據(jù)外部提供正確數(shù)據(jù)包添加界面顯示等操作。

3）報(bào)文解析功能：根據(jù)添加或?qū)霐?shù)據(jù)包信息，解析正確數(shù)據(jù)包，并可查看解析后數(shù)據(jù)包格式等信息。

4）視頻解析功能：根據(jù)正確視頻數(shù)據(jù)包格式，選擇性轉(zhuǎn)換需要視頻格式播放。

2.2 無人艇操縱性運(yùn)動(dòng)模型模塊

在復(fù)雜環(huán)境外力作用下，無人艇為六自由度運(yùn)動(dòng)，為了降低計(jì)算量和系統(tǒng)復(fù)雜度，縱蕩、橫蕩、艏搖的影響可以忽略，采用MMG 分離建模［9］，建立以下三自由度無人艇操縱性運(yùn)動(dòng)方程：

式中，x為橫坐標(biāo)；y為縱坐標(biāo)；u為縱向速度；v為橫向速度；r為艏向角速度；ψ為艏向；m為質(zhì)量；Izz為轉(zhuǎn)動(dòng)慣量；xg為中心重心距；[XH YH NH]為水動(dòng)力；[XP YP NP]為推力；[XR YR NR]為舵力；[XE YE NE]為外力；外力主要由風(fēng)、浪、流3種環(huán)境力組成，其中

2.3 環(huán)境信息模塊

無人艇仿真系統(tǒng)中環(huán)境信息模塊主要分為內(nèi)部和外部信息，內(nèi)部信息主要包括人工設(shè)置或模擬傳感器信息以及環(huán)境干擾信息，外部信息包括無人艇實(shí)海域錄取的傳感器數(shù)據(jù)信息，通過數(shù)據(jù)回放實(shí)現(xiàn)數(shù)據(jù)場(chǎng)景重現(xiàn)。

2.3.1 內(nèi)部模擬信息

1）人工設(shè)置障礙物

通過仿真系統(tǒng)可設(shè)置靜態(tài)多邊形障礙物、動(dòng)態(tài)多邊形障礙物、禁航區(qū)域、隨機(jī)障礙物、目標(biāo)等對(duì)無人艇自主航行過程進(jìn)行人工約束，其中，障礙物可設(shè)置其位置、大小、屬性、運(yùn)動(dòng)速度、方向等，為無人艇自主航行仿真提供不同的測(cè)試場(chǎng)景和航行需求，同時(shí)也支持典型場(chǎng)景的任務(wù)設(shè)定。

2）感知模擬器

模擬無人艇航行過程中對(duì)目標(biāo)的探測(cè)能力，周圍存在的目標(biāo)通過模擬器探測(cè)感知獲得，獲得的感知數(shù)據(jù)均輸出到數(shù)據(jù)融合功能模塊中，不斷測(cè)試數(shù)據(jù)融合軟件的穩(wěn)定性和準(zhǔn)確性，更能較為準(zhǔn)確模擬實(shí)際環(huán)境中的系統(tǒng)狀態(tài)。

導(dǎo)航雷達(dá)模擬器2 s 扇掃一次設(shè)定范圍內(nèi)的目標(biāo)，雷達(dá)盲區(qū)為100 m，設(shè)置20%的隨機(jī)虛假目標(biāo)，同時(shí)對(duì)雷達(dá)數(shù)據(jù)加入隨機(jī)噪聲，更為真實(shí)地模擬實(shí)際雷達(dá)數(shù)據(jù)。雷達(dá)數(shù)據(jù)主要包括批號(hào)、位置、速度、航向、反射面積、DCPA、TCPA。

激光雷達(dá)模擬器一秒一次更新周圍200 m 以內(nèi)目標(biāo)信息，輸出目標(biāo)信息為點(diǎn)云聚類處理后的數(shù)據(jù)，主要包括目標(biāo)距離、方位、航速。

光學(xué)識(shí)別模擬器的視角可設(shè)置，一秒一次更新光學(xué)視角范圍內(nèi)的識(shí)別目標(biāo)，逐一地對(duì)雷達(dá)提供的目標(biāo)信息進(jìn)行識(shí)別確認(rèn)，同時(shí)給出目標(biāo)屬性，對(duì)識(shí)別后目標(biāo)不再進(jìn)行確認(rèn)。

3）環(huán)境干擾信息

無人艇所處的海洋環(huán)境復(fù)雜，易受到周圍環(huán)境影響，針對(duì)環(huán)境干擾力所帶來的問題，建立海洋環(huán)境干擾模型，主要考慮風(fēng)、浪、流3個(gè)方面的干擾。

a）風(fēng)力模型

風(fēng)對(duì)船舶的影響主要作用在船舶水線以上的船體，從而影響船舶的姿態(tài)。風(fēng)的作用大小由風(fēng)速、風(fēng)向決定，其公式如下：

式中，ρa(bǔ)為空氣密度；Ar、AL為水線以上船體的正投影面積和側(cè)投影面積；L為船長(zhǎng)；VW為相對(duì)風(fēng)速；CWX、CWY和CWN為風(fēng)力系數(shù)。

b）浪力模型

波浪對(duì)船體的干擾力一般分為兩種，一種是一階波浪力，也稱高頻干擾力，其主要由高頻小幅波浪產(chǎn)生，與波高成正比，影響船舶的艏向；另外一種是二階波浪力，也稱波浪漂移力，與波高的平方成正比，影響船舶的航跡。這里考慮二階波浪力，其公式如下：

式中，ρw為水密度；g為重力加速度；L為船長(zhǎng)；為波長(zhǎng)；hw為有義浪高；θ為波浪遭遇角；CXW（）、CYW（）和CNW（）為漂移力系數(shù)。

c）流力模型

對(duì)于流對(duì)船舶運(yùn)動(dòng)干擾的計(jì)算，一般考慮恒值海流下的運(yùn)動(dòng)模型。一般情況下，流對(duì)船舶作用力計(jì)算與風(fēng)的計(jì)算相似，其公式如下：

式中，ρC為流體密度Afw、Asw分別為水線以下船體的正投影面積和側(cè)投影面積；β為漂角；L為船長(zhǎng)；VC為流速；CX（β）、CY（β）和CN（β）為流的作用力矩系數(shù)。

2.3.2 外部信息

外部輸入信息主要包括實(shí)海域錄取的傳感器設(shè)備網(wǎng)絡(luò)報(bào)文數(shù)據(jù)，通過回放軟件，將網(wǎng)絡(luò)報(bào)文接入到仿真系統(tǒng)中，實(shí)時(shí)的態(tài)勢(shì)信息主要包括導(dǎo)航雷達(dá)信息、AIS 信息、光學(xué)識(shí)別信息、激光雷達(dá)信息，在仿真系統(tǒng)中，通過實(shí)海域態(tài)勢(shì)對(duì)無人艇自主航行控制算法進(jìn)行測(cè)試，更具可靠性和適用性，有利于實(shí)艇算法的移植和實(shí)用性測(cè)試。

2.4 航行控制模塊

航行控制模塊主要解決3 個(gè)方面的技術(shù)問題：航向、航速和航路跟蹤的控制問題，擬將技術(shù)分為3層功能實(shí)現(xiàn)。

1）導(dǎo)引層根據(jù)任務(wù)航路對(duì)無人艇位置進(jìn)行約束，驅(qū)動(dòng)無人艇收斂在期望航路上，解算生成導(dǎo)引控制指令，發(fā)送至控制層。

2）控制層根據(jù)導(dǎo)引層生成的航速航向控制指令，通過控制算法解算，生成執(zhí)行機(jī)構(gòu)控制指令，發(fā)送至執(zhí)行層。

3）執(zhí)行層根據(jù)執(zhí)行機(jī)構(gòu)控制指令，生成匹配的動(dòng)力指令，發(fā)送至無人艇操縱性模型模塊中，實(shí)現(xiàn)轉(zhuǎn)向和速度的改變，繼而更新無人艇的導(dǎo)航信息。

由此，航行控制模塊的導(dǎo)引、控制、執(zhí)行3 個(gè)功能流程中如圖6 所示，對(duì)每個(gè)功能層進(jìn)行算法針對(duì)性設(shè)計(jì)，在航速控制、航向控制、航路跟蹤中分別采用滑?！窠?jīng)網(wǎng)絡(luò)控制算法（SMCNN）、自抗擾控制算法（ADRC）、LOS 航路跟蹤算法，為無人艇自主航行提供基礎(chǔ)。

圖6 航行控制結(jié)構(gòu)圖Fig.6 Navigation control structure chart

視線（line of sight，LOS）算法是一種常用的無人艇航路跟蹤控制算法［4］，具有不依賴于模型、參數(shù)少、抗干擾強(qiáng)等特點(diǎn)，應(yīng)用范圍較為普遍。LOS通過計(jì)算當(dāng)前航線上的導(dǎo)引點(diǎn)［5］，引導(dǎo)無人艇向?qū)бc(diǎn)航行，從而驅(qū)動(dòng)無人艇收斂于規(guī)劃航路上。

圖7 為基于LOS 導(dǎo)引的無人艇航路跟蹤原理圖，坐標(biāo)系為北向坐標(biāo)系，α為當(dāng)前航路AB 的方向，θ為無人水面艇的航向，β為漂角，ψ為期望航向，u（t）為無人艇航速，h為水面無人艇當(dāng)前位置PUAV到在航路上投影點(diǎn)PT的距離［7］，Δ（t）為前視距離，與無人艇當(dāng)前航速和跟蹤誤差有關(guān)，是點(diǎn)PT到導(dǎo)航點(diǎn)PLOS之間的距離。本文在原有LOS 算法的基礎(chǔ)上進(jìn)行改進(jìn)［12］，加入對(duì)外部環(huán)境干擾觀測(cè)，對(duì)視線角計(jì)算進(jìn)行補(bǔ)償，It為自適應(yīng)補(bǔ)償項(xiàng)；同時(shí)考慮無人艇高速收斂航路過程中出現(xiàn)震蕩問題，加入微分環(huán)節(jié)，其中，dh為跟蹤誤差的微分，λ、λ1、kp、ρ、Δcst為可調(diào)參數(shù)，Δmin為常值，通常設(shè)定為1.5 倍艇長(zhǎng)，由公式求得：

圖7 基于LOS導(dǎo)引的無人艇航路跟蹤原理圖Fig.7 Schematic diagram of route tracking for USV base on LOS guidance algorithm

無人艇在高速航行時(shí)，水動(dòng)力所產(chǎn)生的動(dòng)升力能將艇體上抬，艇體抬起后，阻力降低，尤其是興波阻力大量降低。這樣的航行特性使其阻力非線性特性明顯，縱向運(yùn)動(dòng)比常規(guī)排水船要復(fù)雜，阻力參數(shù)隨運(yùn)動(dòng)狀態(tài)變化較大，模型參數(shù)隨時(shí)都在變化［8］，因此，很難對(duì)其進(jìn)行精確建模，而且外界環(huán)境干擾的不確定性，常規(guī)的PID 控制方法難以對(duì)這種不確定非線性系統(tǒng)得到較好的控制效果。

針對(duì)無人艇航行運(yùn)動(dòng)特點(diǎn)，仿真系統(tǒng)航行控制模塊中航向控制默認(rèn)采用經(jīng)典自抗擾控制算法（ADRC），該控制算法分為跟蹤微分器（TD）、擴(kuò)張狀態(tài)觀測(cè)器（ESO）、非線性狀態(tài)誤差反饋（NLSEF）3個(gè)模塊，進(jìn)行非線性轉(zhuǎn)換，將非線性系統(tǒng)轉(zhuǎn)化為線性系統(tǒng)，實(shí)現(xiàn)動(dòng)態(tài)系統(tǒng)的反饋線性化。跟蹤微分器將誤差進(jìn)行平緩過渡，同時(shí)計(jì)算微分量；擴(kuò)張狀態(tài)觀測(cè)器（ESO）的作用是估計(jì)系統(tǒng)狀態(tài)和不確定干擾的觀測(cè)估計(jì)值；利用安排非線性狀態(tài)誤差反饋（NLSEF）對(duì)系統(tǒng)狀態(tài)進(jìn)行控制。無人艇航向易受到外界環(huán)境的干擾，自抗擾控制算法能有效地估計(jì)擾動(dòng)和狀態(tài)值，補(bǔ)償干擾帶來的誤差。

航速控制采用滑模—神經(jīng)網(wǎng)絡(luò)相結(jié)合的控制算法（SMCNN），滑?？刂频膬?yōu)點(diǎn)是對(duì)干擾和模型動(dòng)態(tài)具有很強(qiáng)的魯棒性，控制算法實(shí)現(xiàn)簡(jiǎn)單，同時(shí)針對(duì)航速控制的特點(diǎn)，設(shè)計(jì)采用神經(jīng)網(wǎng)絡(luò)方法解決滑模抖振問題，將兩種算法結(jié)合設(shè)計(jì)了一種滑模—神經(jīng)網(wǎng)絡(luò)航速控制算法。

2.5 避障模塊

避障模塊主要分為反應(yīng)式避障和動(dòng)態(tài)規(guī)劃航路避障兩個(gè)模式，兩個(gè)模式可以隨時(shí)切換，算法模塊也可隨時(shí)替換，本文主要介紹反應(yīng)式避障算法。海上航行需要遵守海事規(guī)則，尤其是兩船或多船會(huì)遇時(shí)，海事規(guī)則根據(jù)相對(duì)方位角來區(qū)分和定義會(huì)遇局面，主要分為3種會(huì)遇情況，如圖8所示。

圖8 無人艇會(huì)遇場(chǎng)景定義Fig.8 Definition of USV encounter scene

速度障礙法（velocity obstacle，VO）作為反應(yīng)式避障算法中的應(yīng)用較多的方法［2］，具有響應(yīng)速度較快、計(jì)算量小的特點(diǎn)，因此，本文采用該算法設(shè)計(jì)無人艇避障模塊。當(dāng)無人艇前端傳感器發(fā)現(xiàn)礙航物時(shí)，無人艇根據(jù)礙航物信息采用速度避障法進(jìn)行規(guī)避，該算法的核心主要在無人艇的速度空間中，根據(jù)礙航物運(yùn)動(dòng)信息形成障礙物態(tài)勢(shì)區(qū)域，為無人艇和礙航物的預(yù)測(cè)碰撞區(qū)，對(duì)無人艇的速度進(jìn)行適當(dāng)改變，從而避免未來一段時(shí)間內(nèi)出現(xiàn)在預(yù)測(cè)避碰區(qū)，局面采取算法輸出一系列可航行無碰撞的速度向量，若發(fā)現(xiàn)障礙物為動(dòng)態(tài)船只，則基于國際海事規(guī)則的同時(shí)結(jié)合船只的運(yùn)動(dòng)狀態(tài)，對(duì)航行會(huì)遇態(tài)勢(shì)進(jìn)行決策判斷，分為對(duì)遇、左交叉、右交叉、追越4個(gè)會(huì)遇態(tài)勢(shì)，對(duì)遇局面采取右側(cè)避障的原則；交叉會(huì)遇采取從對(duì)方尾部避讓的原則，禁止從對(duì)方船艏通過；追越采取最優(yōu)原則，從靠近的一邊通過。根據(jù)上述海事規(guī)則、艇體操縱性、偏航距離等約束條件，結(jié)合最優(yōu)化算法計(jì)算，從可航行速度向量范圍中，得到一個(gè)最優(yōu)速度向量，作為無人艇下一時(shí)刻的期望速度向量。當(dāng)判斷無人艇無碰撞危險(xiǎn)之后，快速回歸到目標(biāo)航路上。

建立動(dòng)態(tài)障礙物的空間避碰模型［11］，如圖9 所示，其中，無人艇位于O點(diǎn)，當(dāng)探測(cè)到周圍存在動(dòng)態(tài)障礙物時(shí)，本艇采取轉(zhuǎn)向保速的方法對(duì)動(dòng)態(tài)障礙物進(jìn)行避障。首先對(duì)障礙物進(jìn)行膨脹化處理，根據(jù)探測(cè)到障礙物大小信息向外膨脹1～2 倍船長(zhǎng)做膨脹圓，從O點(diǎn)向膨脹圓做兩條切線［13］，切點(diǎn)為a、b點(diǎn)，VO為無人艇的速度，VT表示動(dòng)態(tài)障礙物的速度，VR表示無人艇相對(duì)于障礙物的相對(duì)速度。若VR的速度方向在碰撞區(qū)∠aOb的范圍內(nèi)，則存在碰撞危險(xiǎn)，需要調(diào)整無人艇航速航向從而改變相對(duì)速度方向遠(yuǎn)離碰撞區(qū)。

圖9 無人艇避障原理圖Fig.9 Schematic diagram of obstacle avoidance for USV

無人艇進(jìn)行動(dòng)態(tài)避障的過程中，實(shí)時(shí)判斷是否可以回歸航路。如圖10所示，無人艇位于點(diǎn)0位置，點(diǎn)2為目標(biāo)點(diǎn)，當(dāng)前航路L由點(diǎn)1、點(diǎn)2組成，點(diǎn)0到航路L的垂點(diǎn)和點(diǎn)0到點(diǎn)2組成的角度范圍ψ=［θ1，θ2］，當(dāng)范圍ψ在可航行安全航向范圍內(nèi)時(shí)，可以回歸航路。

圖10 航路回歸判斷示意圖Fig.10 Schematic diagram of judgment of return expectation of route

速度障礙法流程如圖11所示，具體步驟如下：

圖11 速度障礙法流程圖Fig.11 Flow chart of velocity obstacle avoidance method

1）艇載傳感器實(shí)時(shí)收取航行態(tài)勢(shì)信息，對(duì)周圍障礙物信息進(jìn)行預(yù)處理；

2）對(duì)障礙物進(jìn)行危險(xiǎn)判斷；

3）根據(jù)航行態(tài)勢(shì)解算當(dāng)前可航行安全航向和航速集合；

4）根據(jù)評(píng)價(jià)函數(shù)和最優(yōu)算法選取最優(yōu)航向和航速；

5）無人艇按照海事規(guī)則選取最優(yōu)的航向和航速航行；

6）實(shí)時(shí)判斷是否可以回歸航路，若是，進(jìn)入下一步，否則返回第1）步；

7）無人艇復(fù)航，返回至目標(biāo)航路。

3 自主航行仿真試驗(yàn)

在項(xiàng)目算法開發(fā)測(cè)試的背景下，為了減少實(shí)海域自主航行算法調(diào)試試驗(yàn)次數(shù)、提高試驗(yàn)效率，提前在本文設(shè)計(jì)的仿真系統(tǒng)環(huán)境中驗(yàn)證自主航行相關(guān)算法的可行性和可靠性，通過仿真試驗(yàn)不斷優(yōu)化和改進(jìn)算法以及模擬任務(wù)順利執(zhí)行，以下為無人艇全局航路規(guī)劃、自主避障、航行控制算法和任務(wù)推演的仿真試驗(yàn)。

仿真試驗(yàn)地圖為谷歌衛(wèi)星地圖，地圖位置為日照黃海近岸區(qū)域，仿真試驗(yàn)以某中型無人艇為對(duì)象進(jìn)行數(shù)學(xué)建模，具體參數(shù)如下頁表1所示。

表1 無人艇參數(shù)Table 1 Parameters of USV

環(huán)境干擾參數(shù)如表2所示。

表2 風(fēng)浪流參數(shù)Table 2 Wind-wave-current parameters

全局航路規(guī)劃設(shè)置柵格大小為10 m*10 m，柵格地圖范圍5 km*5 km；無人艇自主避障無人艇航速為18 kn，航路中同時(shí)規(guī)避一個(gè)靜態(tài)一個(gè)動(dòng)態(tài)障礙物，動(dòng)態(tài)障礙物航速為10 kn，與無人艇進(jìn)行左交叉航行，障礙物膨脹大小100 m，DCPA 閾值為300 m，TCPA 閾值為30 s；航行控制中跟蹤航路為直線和曲線的結(jié)合，航路跟蹤控制參數(shù)設(shè)置kp=0，ρ=2，λ=0.2，λ1=0.1，k=2，Δcst=20；航速航向控制算法對(duì)比仿真試驗(yàn)均在相同環(huán)境參數(shù)條件下進(jìn)行；目標(biāo)跟蹤伴隨仿真實(shí)驗(yàn)，人工設(shè)置目標(biāo)航速為10 kn，無人艇起始航速為18 kn，與目標(biāo)跟蹤距離為500 m，伴隨距離200 m，仿真試驗(yàn)結(jié)果如圖12～圖13所示。

圖12 全局航路規(guī)劃Fig.12 Global route planning

圖13 全局航路規(guī)劃平滑處理Fig.13 Smoothing process of global route planning

圖12 全局航路規(guī)劃結(jié)果，設(shè)置了3 個(gè)預(yù)置障礙物；圖13 為對(duì)全局規(guī)劃的路徑進(jìn)行優(yōu)化處理，可以看出經(jīng)處理后路徑點(diǎn)減少，更加平滑。

圖14為自主避障結(jié)果，同時(shí)規(guī)避了一個(gè)靜態(tài)和一個(gè)動(dòng)態(tài)障礙物，可以看出無人水面艇在遇到靜態(tài)障礙物后能夠按照正確的方向進(jìn)行避障，面對(duì)不遵守海事規(guī)則的船只也具備應(yīng)急避障能力，遵守但不受限于海事規(guī)則。

圖14 自主避障Fig.14 Autonomous obstacle avoidance

圖15、下頁圖16 為目標(biāo)跟蹤任務(wù)，圖17 為目標(biāo)伴隨任務(wù)，無人艇正常巡航階段，人工設(shè)置待跟蹤目標(biāo)，目標(biāo)航向、航速、大小可根據(jù)需求設(shè)置，無人艇感知系統(tǒng)探測(cè)到目標(biāo)，規(guī)劃跟蹤航路，快速抵進(jìn)目標(biāo)后方，若為目標(biāo)跟蹤則維持設(shè)定500 m 的距離保持跟蹤，若為目標(biāo)伴隨，則保持到目標(biāo)右側(cè)200 m 位置，持續(xù)獲取目標(biāo)的特征，供人員對(duì)其進(jìn)行危險(xiǎn)判斷。

圖15 抵近目標(biāo)階段Fig.15 Approaching the target stage

圖16 目標(biāo)跟蹤階段Fig.16 Target tracking stage

圖17 目標(biāo)伴隨Fig.17 An adjoint target

圖18為航路跟蹤結(jié)果，完成對(duì)任意曲線的航路跟蹤控制，跟蹤精度達(dá)到1 m以內(nèi)。

圖18 航路跟蹤Fig.18 Route tracking

圖19 為航向控制對(duì)比仿真試驗(yàn)，主要對(duì)比PID和ADRC 對(duì)于航向的控制效果，ADRC 存在5°的超調(diào)，收斂時(shí)間相比較短；圖20 為航速控制算法對(duì)比響應(yīng)圖，SMCNN算法具備更快的末端收斂性。

圖19 航向控制對(duì)比仿真試驗(yàn)Fig.19 Course control comparison simulation test

圖20 航速控制對(duì)比仿真試驗(yàn)Fig.20 Navigation speed control comparison simulation test

圖21為無人艇實(shí)海域某航次錄取的試驗(yàn)數(shù)據(jù)，仿真系統(tǒng)通過數(shù)據(jù)回放功能將當(dāng)時(shí)海上航行態(tài)勢(shì)場(chǎng)景重新輸出，1 377 批號(hào)目標(biāo)航速為6～7 kn，航向?yàn)?80°左右，仿真系統(tǒng)接收到的實(shí)海域航行態(tài)勢(shì)數(shù)據(jù)，結(jié)合無人艇操縱性數(shù)學(xué)模型、自主避障、航行控制等模塊進(jìn)行場(chǎng)景重現(xiàn)自主避障仿真試驗(yàn)，驗(yàn)證避障算法針對(duì)實(shí)海域復(fù)雜場(chǎng)景的可靠性和適應(yīng)性。

圖21 回放真實(shí)場(chǎng)景仿真試驗(yàn)Fig.21 Replay real scene simulation test

從上述仿真試驗(yàn)結(jié)果可以看出，本文設(shè)計(jì)的無人艇自主控制仿真系統(tǒng)，具備規(guī)劃、避障、控制等功能仿真驗(yàn)證，以及任務(wù)推演、數(shù)據(jù)回放存儲(chǔ)能力，且與實(shí)際航行數(shù)據(jù)交互，最大程度上驗(yàn)證相關(guān)軟件算法的可行性和可靠性。

4 結(jié)論

本文主要研究了無人水面艇自主航行控制仿真系統(tǒng)，該仿真系統(tǒng)包含了5個(gè)模塊，分別為顯控模塊、無人艇操縱性運(yùn)動(dòng)模型模塊、環(huán)境信息模塊、避障模塊和航行控制模塊，基于一致的數(shù)據(jù)接口，5 個(gè)模塊中+隨時(shí)進(jìn)行不同算法替換以及參數(shù)更改。仿真試驗(yàn)結(jié)果分析表明，該仿真系統(tǒng)具備對(duì)無人艇自主航行控制進(jìn)行功能測(cè)試和數(shù)據(jù)分析能力，可提高無人艇實(shí)艇工程開發(fā)效率和調(diào)試進(jìn)度，為無人艇自主航行任務(wù)演示提供數(shù)據(jù)分析和場(chǎng)景重現(xiàn)平臺(tái)。