范易杰，楊少龍，黃 金，向先波

(華中科技大學(xué) 船舶與海洋工程學(xué)院, 武漢 430074)

1 引言

近年來，隨著無人裝備向低成本、高效率和智能化方向發(fā)展，無人艇廣泛應(yīng)用于海上偵察、海上搜救、排險(xiǎn)救災(zāi)、水文監(jiān)測(cè)等領(lǐng)域，代替人類完成高風(fēng)險(xiǎn)、高強(qiáng)度的作業(yè)任務(wù)。據(jù)統(tǒng)計(jì)，2022年中國(guó)無人船市場(chǎng)可達(dá)數(shù)十億美元。然而，在無人艇研發(fā)過程中，為了降低無人艇研制風(fēng)險(xiǎn)，不僅需要在相對(duì)安全的場(chǎng)景中測(cè)試，也需要在惡劣的海洋環(huán)境中測(cè)試。此外，無人艇執(zhí)行多樣化任務(wù)需要搭載不同載荷，新的傳感器與執(zhí)行器都需要校準(zhǔn)與性能測(cè)試，這些使得無人艇研發(fā)周期變長(zhǎng)、風(fēng)險(xiǎn)變高，而且受物理海洋環(huán)境約束，實(shí)艇自主航行控制器難以開展極端工況測(cè)試。

為了解決上述問題，在實(shí)艇試驗(yàn)前使用仿真平臺(tái)對(duì)算法進(jìn)行驗(yàn)證可以測(cè)試算法缺陷并提供豐富的驗(yàn)證數(shù)據(jù)集。傳統(tǒng)純數(shù)字仿真雖然能夠快速驗(yàn)證數(shù)學(xué)公式形式的算法，但是存在建模難以十分準(zhǔn)確、模擬跟環(huán)境交互傳感器過于復(fù)雜、難以測(cè)試極端工況等問題?；谔摂M引擎的仿真器更加逼近真實(shí)環(huán)境，而且相比于二維數(shù)據(jù)圖表，三維視景仿真效果更加直觀，同時(shí)個(gè)性化、多樣化的需求對(duì)仿真器可擴(kuò)展性要求更高。

昆士蘭理工大學(xué)在2016年開發(fā)了高保真自主水面車輛和水上環(huán)境模擬器AMSVS，該模擬器通過真實(shí)操作環(huán)境中生成高保真?zhèn)鞲衅鲾?shù)據(jù)，并模擬ASV運(yùn)動(dòng)動(dòng)力學(xué)，作為硬件在環(huán)仿真的接口，基于OpenGL技術(shù)極大改善視覺仿真的質(zhì)量。Ricardo等研發(fā)了一種根據(jù)物理定律設(shè)計(jì)風(fēng)浪流的多機(jī)器人仿真器Kelpic，該仿真器支持無人機(jī)、無人車、無人艇及無人水下航行器等無人設(shè)備多機(jī)協(xié)同仿真。USVSim仿真器通過改進(jìn)Freefloating插件優(yōu)化環(huán)境干擾力和浮力，同時(shí)提供制導(dǎo)、導(dǎo)航和控制的模塊化組件，實(shí)現(xiàn)了高保真場(chǎng)景下的水面無人艇仿真測(cè)試。

綜上，針對(duì)無人艇開發(fā)周期長(zhǎng)、控制及感知模塊難以綜合測(cè)試等問題，本文提出設(shè)計(jì)一款針對(duì)無人艇快速開發(fā)與驗(yàn)證且面向教學(xué)與科研的高保真仿真實(shí)驗(yàn)平臺(tái)設(shè)計(jì)，本平臺(tái)對(duì)于無人艇教學(xué)與科研領(lǐng)域具有重要意義和應(yīng)用價(jià)值。同時(shí)，充分發(fā)揮現(xiàn)有仿真工具進(jìn)行升級(jí)改造將更有助于模擬器加速應(yīng)用落地和快速上手使用。本文將Matlab與ROS結(jié)合，應(yīng)用于水面無人艇仿真，使實(shí)驗(yàn)平臺(tái)既有Matlab操作簡(jiǎn)單、運(yùn)算快速的特點(diǎn)，也具有ROS插件豐富，自主可控的優(yōu)點(diǎn)。使用本文開發(fā)的實(shí)驗(yàn)平臺(tái)不僅可以實(shí)現(xiàn)純數(shù)字仿真，也可以跨系統(tǒng)聯(lián)合仿真，引入U(xiǎn)SVSim擁有的環(huán)境干擾插件和豐富的環(huán)境交互傳感器接口，為半實(shí)物仿真設(shè)計(jì)打下基礎(chǔ)。同時(shí)，本平臺(tái)應(yīng)用于教學(xué)場(chǎng)景時(shí)，學(xué)生可只聚焦于Matlab平臺(tái)開發(fā)控制算法，不用開發(fā)ROS端，或者深入學(xué)習(xí)ROS中內(nèi)部話題通信機(jī)制以及物理建模與運(yùn)動(dòng)控制。應(yīng)用于無人艇復(fù)雜環(huán)境交互研究時(shí)，由于ROS有豐富的插件與對(duì)外接口，同時(shí)Gazebo平臺(tái)擁有ODE、Bullet、Dart、Simbody等多種物理引擎，使模型可以具有質(zhì)量、阻力、浮力等屬性，進(jìn)而組合形成多樣的場(chǎng)景及行為，模擬各種工況下無人艇的受力情況，更加貼近實(shí)際，數(shù)據(jù)與結(jié)論也更具有說服力。

2 實(shí)驗(yàn)平臺(tái)架構(gòu)

本實(shí)驗(yàn)平臺(tái)面向快速開發(fā)且自主性高的任務(wù)場(chǎng)景，設(shè)計(jì)流程分為3部分，如圖1所示。首先，Matlab作為純數(shù)字仿真器驗(yàn)證控制算法的快速收斂性、穩(wěn)定性以及準(zhǔn)確性，在Matlab端通過數(shù)學(xué)模型建立被控對(duì)象與環(huán)境擾動(dòng)。其次，通過Solidworks完成裝配體模型，確定無人艇各部分零件的構(gòu)型和幾何關(guān)系。最后，將無人艇的艇體模型、運(yùn)動(dòng)模型、擾動(dòng)模型導(dǎo)入到ROS中。

圖1 平臺(tái)總體設(shè)計(jì)流程框圖Fig.1 Block diagram of overall platform design flow

本平臺(tái)由2部分組成，分別是基于Matlab開發(fā)的控制器和基于ROS開發(fā)的仿真平臺(tái)，如圖2所示。

圖2 仿真平臺(tái)結(jié)構(gòu)框圖Fig.2 Simulation platform structure block diagram

平臺(tái)各部分通過網(wǎng)絡(luò)交互信息，Matlab作為人機(jī)顯示界面通過TCP/IP協(xié)議下發(fā)任務(wù)指令，Gazebo作為顯示與仿真端，在導(dǎo)入被控對(duì)象運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)、坐標(biāo)系以及三維模型后，在ROS框架下通過話題的形式實(shí)時(shí)交互，同時(shí)使用ROS強(qiáng)大的生態(tài)系統(tǒng)中已開源的各類機(jī)器人的感知、規(guī)劃、控制等功能包，如相機(jī)、激光雷達(dá)等傳感器，也可以接入真實(shí)的設(shè)備或者控制器聯(lián)合仿真。在系統(tǒng)初始化時(shí)重力、浮力、環(huán)境干擾力通過插件的形式作用到模型上，使得仿真更加接近實(shí)際湖上試驗(yàn)。

3 基于PID和視線角制導(dǎo)的路徑跟蹤設(shè)計(jì)

Matlab作為常用的仿真軟件，其強(qiáng)大的運(yùn)算能力、豐富的函數(shù)以及簡(jiǎn)單易學(xué)習(xí)的特點(diǎn)在仿真上有很強(qiáng)的優(yōu)勢(shì)。本文設(shè)計(jì)的控制器充分利用了Matlab上述優(yōu)點(diǎn)，并實(shí)現(xiàn)了如下功能：

1) 僅使用Matlab便可快速驗(yàn)證算法可行性

2) 作為人機(jī)交互界面，負(fù)責(zé)下發(fā)指令以及分析ROS返回的數(shù)據(jù)。

3.1 被控對(duì)象運(yùn)動(dòng)學(xué)模型

在Matlab中驗(yàn)證控制算法時(shí)，需要先建立被控對(duì)象的模型。針對(duì)無人艇的運(yùn)動(dòng)學(xué)模型問題，F(xiàn)ossen提出了完整的六自由度水面無人艇模型，并依據(jù)操縱性試驗(yàn)結(jié)果得出水動(dòng)力參數(shù)。定義以地球表面某一點(diǎn)為坐標(biāo)原點(diǎn)的慣性坐標(biāo)系和以船體重心為坐標(biāo)原點(diǎn)的附體坐標(biāo)系，模型坐標(biāo)系如圖3所示。

圖3 無人艇模型坐標(biāo)系示意圖Fig.3 Diagram of coordinate system of Unmanned surface vehicle model

對(duì)水平面運(yùn)動(dòng)產(chǎn)生影響的主要因素有縱蕩、橫蕩、艏搖3個(gè)方向作用力。受力包括粘性水動(dòng)力、慣性水動(dòng)力、環(huán)境干擾力和動(dòng)力裝置產(chǎn)生的推力力矩，所以將六自由度模型簡(jiǎn)化為三自由度運(yùn)動(dòng)模型(沿軸的前進(jìn)速度，沿軸的橫移速度，繞軸的艏搖角速度)，得到無環(huán)境擾動(dòng)下的模型表達(dá)式，如式(1)所示。

(1)

式中:、為無人艇在大地坐標(biāo)系-下的坐標(biāo)；、分別為無人艇的前向速度和側(cè)向速度；為航向角；為角速度；、、分別為無人艇的慣性質(zhì)量及其附加質(zhì)量矩陣的對(duì)角項(xiàng)；為前向推力；為扭矩；、、、||、||、||均為水動(dòng)力導(dǎo)數(shù)，根據(jù)無人艇自身動(dòng)力學(xué)參數(shù)進(jìn)行計(jì)算。建立被控對(duì)象模型后，驗(yàn)證控制算法在無擾動(dòng)下運(yùn)行效果。

3.2 控制與制導(dǎo)算法設(shè)計(jì)

為了驗(yàn)證平臺(tái)仿真實(shí)際效果，本文使用基于LOS制導(dǎo)律和PID控制的無人艇路徑跟蹤算法，將路徑跟蹤任務(wù)分為4個(gè)層級(jí)-任務(wù)層、制導(dǎo)層、控制層和執(zhí)行層。任務(wù)層負(fù)責(zé)規(guī)劃并下達(dá)航點(diǎn)任務(wù)，制導(dǎo)層用于產(chǎn)生期望航向，控制層負(fù)責(zé)根據(jù)期望和實(shí)際的航向偏差產(chǎn)生控制信號(hào)，使無人艇以較好的性能跟蹤給定的期望航向，執(zhí)行層則根據(jù)控制信號(hào)驅(qū)動(dòng)無人艇執(zhí)行器，使無人艇產(chǎn)生相應(yīng)的狀態(tài)變化，流程如圖4所示。

圖4 無人艇路徑跟蹤原理流程框圖Fig.4 Flowchart of unmanned surface vehicle path tracking principle

慣性坐標(biāo)系下，航線由航路點(diǎn)(=1,2,3,…)構(gòu)成，當(dāng)無人艇與航線終點(diǎn)的距離、航向符合到點(diǎn)判斷邏輯時(shí)，路徑跟蹤器切換到下一航線。航線+1在慣性坐標(biāo)系下的方向?yàn)?span id="syggg00" class="subscript">，無人艇位置為()，以無人艇為圓心，設(shè)定半徑為的視線圓，以視線圓與+1的交點(diǎn)()為期望位置。根據(jù)航線與無人艇的位置關(guān)系，計(jì)算出期望航向，無人艇在直線+1上的垂足為()，無人艇到+1的距離為()，根據(jù)反正弦函數(shù)可以求出()()和()()之間的夾角

(2)

期望航向?yàn)?/p>

(3)

當(dāng)無人艇與航線間的距離大于時(shí)，直接取無人艇到航線的垂線方向作為期望航向，完整的期望航向表示為：

(4)

其中，的數(shù)值影響控制器的響應(yīng)速度，無人艇在相同位置下，越小則期望航向越趨于垂直航線方向，系統(tǒng)對(duì)誤差的響應(yīng)越快，無人艇位置會(huì)快速收斂，同時(shí)也會(huì)帶來振蕩現(xiàn)象，制導(dǎo)原理如圖5所示。

圖5 視線角制導(dǎo)原理示意圖Fig.5 Schematic diagram of line-of-sight guidance

產(chǎn)生期望航向后，由控制器根據(jù)期望航向產(chǎn)生對(duì)無人艇的控制信號(hào)。以視線角制導(dǎo)算法產(chǎn)生的期望航向與無人艇當(dāng)前航向之間的偏差為PID控制器的輸入，根據(jù)航向偏差進(jìn)行比例、積分、微分運(yùn)算，為防止長(zhǎng)時(shí)間積分使積分項(xiàng)過大導(dǎo)致控制器失效，對(duì)于積分運(yùn)算進(jìn)行了飽和限制，控制流程如圖6所示。

圖6 無人艇航向控制器控制流程框圖Fig.6 Block diagram of control flow of unmanned surface vehicle heading controller

離散系統(tǒng)的PID算式，該P(yáng)ID算式(5)為位置式PID算法：

(5)

式中；()為舵角控制指令；()為航向偏差；為比例系數(shù)；=為積分系數(shù)，為積分時(shí)間常數(shù)；=為微分系數(shù)，為微分時(shí)間常數(shù)。

首先在Matlab中測(cè)試上述算法的快速性、準(zhǔn)確性和穩(wěn)定性，搭建Simulink仿真模塊，在收斂性和跟蹤誤差滿足預(yù)期后，使用代碼自動(dòng)生成技術(shù)將模型編譯成為C++文件放入ROS功能包，在ROS中驗(yàn)證存在擾動(dòng)情況下的跟蹤誤差是否滿足要求。

4 基于ROS開發(fā)的仿真平臺(tái)

基于ROS架構(gòu)的無人艇仿真平臺(tái)主要包括：被控對(duì)象的運(yùn)動(dòng)學(xué)模型、動(dòng)力學(xué)模型、幾何模型、環(huán)境擾動(dòng)模型和通信接口等。ROS既可以接收坐標(biāo)和經(jīng)緯度，并自動(dòng)規(guī)劃軌跡跟蹤航點(diǎn)，也可以接收航向和力矩，測(cè)試控制效果。仿真軟件Gazebo啟動(dòng)時(shí)自動(dòng)加載環(huán)境、模型、作用力，在接收位置與速度信息后運(yùn)動(dòng)。

4.1 無人艇三維模型

在Gazebo中加載無人艇物理模型有兩種方式，一種是直接編寫URDF模型文件，另一種是先通過SolidWorks建立三維模型，再通過SolidWorks to URDF Exporter插件將模型文件導(dǎo)出為URDF文件。本文使用SolidWorks定義各零件的質(zhì)量、位置關(guān)系等屬性，通過插件輸出URDF文件和STL網(wǎng)格文件后，在Gazebo中加載并顯示的效果如圖7所示。

圖7 無人艇三維模型效果圖Fig.7 3D model of unmanned surface vehicle

4.2 無人艇動(dòng)力學(xué)模型

根據(jù)剛體動(dòng)力學(xué)和無人艇受力，可以將無人艇附體坐標(biāo)系下的運(yùn)動(dòng)描述為式(6)：

(6)

式中:、和是作用于艇體的粘性水動(dòng)力和力矩；帶有下標(biāo)的變量是無人艇推進(jìn)裝置產(chǎn)生的作用力和力矩；帶有下標(biāo)Wind、Wave、Current的變量分別對(duì)應(yīng)風(fēng)、浪、流3種環(huán)境干擾力和力矩；下標(biāo)和Buoyancy分別代表重力和浮力在該方向產(chǎn)生的力和力矩；、是、軸運(yùn)動(dòng)的附加質(zhì)量；為無人艇繞軸旋轉(zhuǎn)的轉(zhuǎn)動(dòng)慣量；是繞軸的附加慣性矩。

無人艇在各自由度上的受力可以表示為：

(7)

帶有下標(biāo)Model的項(xiàng)為無人艇模型換算得到的等效力和力矩，包括慣性水動(dòng)力、粘性水動(dòng)力和推進(jìn)器推力；下標(biāo)Wind、Current、Wave、G、Buoyancy分別代表：風(fēng)、水流、波浪、重力、浮力在各個(gè)方向所產(chǎn)生的力和力矩。

結(jié)合式(6)和式(7)可以得到無人艇各方向上的作用力表達(dá)為式(8)。

(8)

力的加載由Gazebo插件實(shí)現(xiàn)，當(dāng)物體的link文件中掛載了某一插件時(shí)，在仿真時(shí)該插件就會(huì)自動(dòng)運(yùn)行，向link施加力或者其他操作。式(8)中的質(zhì)量、附加質(zhì)量、水動(dòng)力導(dǎo)數(shù)等系數(shù)為常數(shù)，需要通過回歸式計(jì)算，再通過實(shí)際試驗(yàn)修正。各方向的速度和角速度可以在插件程序中使用物理引擎提供的指針來獲取。

4.3 環(huán)境擾動(dòng)模型

在風(fēng)、浪、流等環(huán)境干擾的作用下，無人艇在航行中的航向、位置等狀態(tài)會(huì)發(fā)生偏離，對(duì)運(yùn)動(dòng)控制產(chǎn)生較大影響，為了使仿真平臺(tái)可以更加貼合真實(shí)環(huán)境，需要為仿真平臺(tái)添加風(fēng)、浪、流環(huán)境干擾模型，同時(shí)由于環(huán)境擾動(dòng)力之間的耦合關(guān)系比較復(fù)雜，所以本文將風(fēng)浪流擾動(dòng)解耦，分別建立數(shù)學(xué)模型，再由Gazebo計(jì)算出干擾力力矩作用在無人艇上。

本文參考Blendermann等提出的風(fēng)力計(jì)算式(9)，計(jì)算風(fēng)對(duì)無人艇的作用力

(9)

式中：為無人艇的總長(zhǎng)度；為空氣密度；為無人艇水線以上的側(cè)投影面積；為無人艇水線以上的正投影面積；()、()和()分別為、方向的風(fēng)壓力系數(shù)以及繞軸的風(fēng)壓力矩系數(shù)。

Daidola等提出的二階波浪干擾力式(10)得出波浪對(duì)無人艇縱蕩、橫蕩、艏搖3個(gè)自由度運(yùn)動(dòng)的影響采用插件的形式寫成功能包文件，由仿真軟件在初始化階段調(diào)用。

(10)

在仿真平臺(tái)啟動(dòng)時(shí)直接運(yùn)行插件，加載作用力到模型上，可以通過修改功能包中的模型文件改變環(huán)境擾動(dòng)力。

5 仿真平臺(tái)效果與實(shí)船驗(yàn)證

5.1 平臺(tái)仿真結(jié)果

為了驗(yàn)證本文提出的基于USVSim的仿真試驗(yàn)平臺(tái)與Matlab交互的效果，本節(jié)采用視線角制導(dǎo)算法航點(diǎn)跟蹤進(jìn)行測(cè)試。本次測(cè)試實(shí)驗(yàn)使用兩臺(tái)計(jì)算機(jī)，一臺(tái)運(yùn)行Matlab程序，另一臺(tái)運(yùn)行USVSim仿真器，兩者通過以太網(wǎng)連接使計(jì)算機(jī)在同一網(wǎng)段下，通過ping命令確認(rèn)通信正常。仿真運(yùn)行時(shí)，Matlab端發(fā)送期望航點(diǎn)坐標(biāo)，通過Publish模塊發(fā)送給communication_upway_points話題,Gazebo中的三維模型接收話題中的航點(diǎn)信息規(guī)劃航跡，同時(shí)仿真器將位姿信息存儲(chǔ)到話題communication_to_ui里面，Matlab端接收話題信息并顯示，通信模塊如圖8所示。

圖8 Simulink通信模塊框圖Fig.8 Simulink communication module

本節(jié)仿真起點(diǎn)默認(rèn)(8，-4)，目標(biāo)點(diǎn)(50，0)，ROS接收目標(biāo)點(diǎn)信息后跟蹤曲線如圖9，跟蹤誤差曲線如圖10，無人艇在接收到航點(diǎn)任務(wù)后從起點(diǎn)開始向目標(biāo)點(diǎn)航行，橫向偏距不斷減小，33 s到達(dá)目標(biāo)航點(diǎn)。

圖9 仿真跟蹤曲線Fig.9 Simulation tracking curve

圖10 跟蹤誤差曲線Fig.10 Tracking error curve

5.2 湖上驗(yàn)證結(jié)果

為了驗(yàn)證仿真試驗(yàn)平臺(tái)結(jié)果的準(zhǔn)確性，采用自行研發(fā)的1.75 m雙槳雙舵無人艇進(jìn)行湖上測(cè)試，無人艇如圖11所示。

圖11 無人艇實(shí)物圖Fig.11 Photoview of unmanned surface vehicle

某一航次以(-40，-25)為起點(diǎn)、(90，-5)為終點(diǎn)，無人船艏向309.4°、初始航向303°、速度3 kn的制導(dǎo)任務(wù)實(shí)驗(yàn)結(jié)果如圖12、圖13所示，規(guī)劃行線與航跡基本一致，穩(wěn)態(tài)橫向偏距約0.5 m。

圖12 湖上實(shí)驗(yàn)跟蹤曲線Fig.12 Experimental tracking curve on the lake

圖13 跟蹤誤差曲線Fig.13 Tracking error curve

由圖9、圖10、圖12、圖13可知，仿真曲線相對(duì)平穩(wěn)，參數(shù)合適時(shí)橫向偏距沒有超調(diào)，為一條平滑曲線，而湖上試驗(yàn)航路跟蹤任務(wù)有小幅波動(dòng)，30 s后橫向偏距收斂到0 m。仿真與物理實(shí)驗(yàn)規(guī)律基本相符，說明設(shè)計(jì)的基于USVSim的仿真平臺(tái)能夠模擬無人船的運(yùn)動(dòng)特性及驗(yàn)證控制算法功能。綜上，基于USVSim的無人艇綜合仿真系統(tǒng)可作為一種快速開發(fā)平臺(tái)，適用于無人艇運(yùn)動(dòng)控制系統(tǒng)的仿真與測(cè)試驗(yàn)證。

6 結(jié)論

提出了基于USVSim的仿真平臺(tái)與Matlab的無人艇綜合仿真系統(tǒng)，能夠在Matlab初步驗(yàn)證后快速修改移植到本平臺(tái)，在原有仿真架構(gòu)上沒有增加復(fù)雜程序，但在擴(kuò)展性和顯示效果上更加優(yōu)異。相比于數(shù)值二維曲線，三維交互顯示結(jié)果擁有直觀感受，利于科研與教學(xué)。平臺(tái)通過Simulink將控制器與ROS解耦，將仿真后控制算法直接植入物理控制器(如樹莓派)，可與ROS中模擬的逼真環(huán)境與傳感器(如激光雷達(dá)、攝像頭)交互，實(shí)現(xiàn)更加真實(shí)的仿真測(cè)試，有效釋放物理試驗(yàn)風(fēng)險(xiǎn)，降低測(cè)試成本，縮短開發(fā)周期。