基于USVSim的無人艇綜合仿真平臺設計與驗證

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

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

1 引言

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

為了解決上述問題，在實艇試驗前使用仿真平臺對算法進行驗證可以測試算法缺陷并提供豐富的驗證數據集。傳統純數字仿真雖然能夠快速驗證數學公式形式的算法，但是存在建模難以十分準確、模擬跟環境交互傳感器過于復雜、難以測試極端工況等問題。基于虛擬引擎的仿真器更加逼近真實環境，而且相比于二維數據圖表，三維視景仿真效果更加直觀，同時個性化、多樣化的需求對仿真器可擴展性要求更高。

昆士蘭理工大學在2016年開發了高保真自主水面車輛和水上環境模擬器AMSVS，該模擬器通過真實操作環境中生成高保真傳感器數據，并模擬ASV運動動力學，作為硬件在環仿真的接口，基于OpenGL技術極大改善視覺仿真的質量。Ricardo等研發了一種根據物理定律設計風浪流的多機器人仿真器Kelpic，該仿真器支持無人機、無人車、無人艇及無人水下航行器等無人設備多機協同仿真。USVSim仿真器通過改進Freefloating插件優化環境干擾力和浮力，同時提供制導、導航和控制的模塊化組件，實現了高保真場景下的水面無人艇仿真測試。

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

2 實驗平臺架構

本實驗平臺面向快速開發且自主性高的任務場景，設計流程分為3部分，如圖1所示。首先，Matlab作為純數字仿真器驗證控制算法的快速收斂性、穩定性以及準確性，在Matlab端通過數學模型建立被控對象與環境擾動。其次，通過Solidworks完成裝配體模型，確定無人艇各部分零件的構型和幾何關系。最后，將無人艇的艇體模型、運動模型、擾動模型導入到ROS中。

圖1 平臺總體設計流程框圖Fig.1 Block diagram of overall platform design flow

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

圖2 仿真平臺結構框圖Fig.2 Simulation platform structure block diagram

平臺各部分通過網絡交互信息，Matlab作為人機顯示界面通過TCP/IP協議下發任務指令，Gazebo作為顯示與仿真端，在導入被控對象運動學、動力學、坐標系以及三維模型后，在ROS框架下通過話題的形式實時交互，同時使用ROS強大的生態系統中已開源的各類機器人的感知、規劃、控制等功能包，如相機、激光雷達等傳感器，也可以接入真實的設備或者控制器聯合仿真。在系統初始化時重力、浮力、環境干擾力通過插件的形式作用到模型上，使得仿真更加接近實際湖上試驗。

3 基于PID和視線角制導的路徑跟蹤設計

Matlab作為常用的仿真軟件，其強大的運算能力、豐富的函數以及簡單易學習的特點在仿真上有很強的優勢。本文設計的控制器充分利用了Matlab上述優點，并實現了如下功能：

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

2) 作為人機交互界面，負責下發指令以及分析ROS返回的數據。

3.1 被控對象運動學模型

在Matlab中驗證控制算法時，需要先建立被控對象的模型。針對無人艇的運動學模型問題，Fossen提出了完整的六自由度水面無人艇模型，并依據操縱性試驗結果得出水動力參數。定義以地球表面某一點為坐標原點的慣性坐標系和以船體重心為坐標原點的附體坐標系，模型坐標系如圖3所示。

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

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

(1)

式中:、為無人艇在大地坐標系-下的坐標；、分別為無人艇的前向速度和側向速度；為航向角；為角速度；、、分別為無人艇的慣性質量及其附加質量矩陣的對角項；為前向推力；為扭矩；、、、||、||、||均為水動力導數，根據無人艇自身動力學參數進行計算。建立被控對象模型后，驗證控制算法在無擾動下運行效果。

3.2 控制與制導算法設計

為了驗證平臺仿真實際效果，本文使用基于LOS制導律和PID控制的無人艇路徑跟蹤算法，將路徑跟蹤任務分為4個層級-任務層、制導層、控制層和執行層。任務層負責規劃并下達航點任務，制導層用于產生期望航向，控制層負責根據期望和實際的航向偏差產生控制信號，使無人艇以較好的性能跟蹤給定的期望航向，執行層則根據控制信號驅動無人艇執行器，使無人艇產生相應的狀態變化，流程如圖4所示。

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

慣性坐標系下，航線由航路點(=1,2,3,…)構成，當無人艇與航線終點的距離、航向符合到點判斷邏輯時，路徑跟蹤器切換到下一航線。航線+1在慣性坐標系下的方向為，無人艇位置為()，以無人艇為圓心，設定半徑為的視線圓，以視線圓與+1的交點()為期望位置。根據航線與無人艇的位置關系，計算出期望航向，無人艇在直線+1上的垂足為()，無人艇到+1的距離為()，根據反正弦函數可以求出()()和()()之間的夾角

(2)

期望航向為

(3)

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

(4)

其中，的數值影響控制器的響應速度，無人艇在相同位置下，越小則期望航向越趨于垂直航線方向，系統對誤差的響應越快，無人艇位置會快速收斂，同時也會帶來振蕩現象，制導原理如圖5所示。

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

產生期望航向后，由控制器根據期望航向產生對無人艇的控制信號。以視線角制導算法產生的期望航向與無人艇當前航向之間的偏差為PID控制器的輸入，根據航向偏差進行比例、積分、微分運算，為防止長時間積分使積分項過大導致控制器失效，對于積分運算進行了飽和限制，控制流程如圖6所示。

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

離散系統的PID算式，該PID算式(5)為位置式PID算法：

(5)

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

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

4 基于ROS開發的仿真平臺

基于ROS架構的無人艇仿真平臺主要包括：被控對象的運動學模型、動力學模型、幾何模型、環境擾動模型和通信接口等。ROS既可以接收坐標和經緯度，并自動規劃軌跡跟蹤航點，也可以接收航向和力矩，測試控制效果。仿真軟件Gazebo啟動時自動加載環境、模型、作用力，在接收位置與速度信息后運動。

4.1 無人艇三維模型

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

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

4.2 無人艇動力學模型

根據剛體動力學和無人艇受力，可以將無人艇附體坐標系下的運動描述為式(6)：

(6)

式中:、和是作用于艇體的粘性水動力和力矩；帶有下標的變量是無人艇推進裝置產生的作用力和力矩；帶有下標Wind、Wave、Current的變量分別對應風、浪、流3種環境干擾力和力矩；下標和Buoyancy分別代表重力和浮力在該方向產生的力和力矩；、是、軸運動的附加質量；為無人艇繞軸旋轉的轉動慣量；是繞軸的附加慣性矩。

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

(7)

帶有下標Model的項為無人艇模型換算得到的等效力和力矩，包括慣性水動力、粘性水動力和推進器推力；下標Wind、Current、Wave、G、Buoyancy分別代表：風、水流、波浪、重力、浮力在各個方向所產生的力和力矩。

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

(8)

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

4.3 環境擾動模型

在風、浪、流等環境干擾的作用下，無人艇在航行中的航向、位置等狀態會發生偏離，對運動控制產生較大影響，為了使仿真平臺可以更加貼合真實環境，需要為仿真平臺添加風、浪、流環境干擾模型，同時由于環境擾動力之間的耦合關系比較復雜，所以本文將風浪流擾動解耦，分別建立數學模型，再由Gazebo計算出干擾力力矩作用在無人艇上。

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

(9)

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

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

(10)

在仿真平臺啟動時直接運行插件，加載作用力到模型上，可以通過修改功能包中的模型文件改變環境擾動力。

5 仿真平臺效果與實船驗證

5.1 平臺仿真結果

為了驗證本文提出的基于USVSim的仿真試驗平臺與Matlab交互的效果，本節采用視線角制導算法航點跟蹤進行測試。本次測試實驗使用兩臺計算機，一臺運行Matlab程序，另一臺運行USVSim仿真器，兩者通過以太網連接使計算機在同一網段下，通過ping命令確認通信正常。仿真運行時，Matlab端發送期望航點坐標，通過Publish模塊發送給communication_upway_points話題,Gazebo中的三維模型接收話題中的航點信息規劃航跡，同時仿真器將位姿信息存儲到話題communication_to_ui里面，Matlab端接收話題信息并顯示，通信模塊如圖8所示。

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

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

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

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

5.2 湖上驗證結果

為了驗證仿真試驗平臺結果的準確性，采用自行研發的1.75 m雙槳雙舵無人艇進行湖上測試，無人艇如圖11所示。

圖11 無人艇實物圖Fig.11 Photoview of unmanned surface vehicle

某一航次以(-40，-25)為起點、(90，-5)為終點，無人船艏向309.4°、初始航向303°、速度3 kn的制導任務實驗結果如圖12、圖13所示，規劃行線與航跡基本一致，穩態橫向偏距約0.5 m。

圖12 湖上實驗跟蹤曲線Fig.12 Experimental tracking curve on the lake

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

由圖9、圖10、圖12、圖13可知，仿真曲線相對平穩，參數合適時橫向偏距沒有超調，為一條平滑曲線，而湖上試驗航路跟蹤任務有小幅波動，30 s后橫向偏距收斂到0 m。仿真與物理實驗規律基本相符，說明設計的基于USVSim的仿真平臺能夠模擬無人船的運動特性及驗證控制算法功能。綜上，基于USVSim的無人艇綜合仿真系統可作為一種快速開發平臺，適用于無人艇運動控制系統的仿真與測試驗證。

6 結論

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