船舶智能航行仿真平臺設計*

秦有成 陳紅衛 袁 偉

（江蘇科技大學電子信息學院 鎮江 212100）

1 引言

近年來船舶制造業和船舶航運業為代表的傳統行業處于一種困境，船舶需求低迷，船舶運營成本增長、船舶操作復雜化以及環保法規日趨嚴格，因而如何走出困境成為了船舶行業共同思考的問題。在這種背景下，航運界不斷增加對船舶的技術投入，船舶的研發建造變得逐漸火熱，世界各主要造船國家紛紛開始研發智能應用平臺、自主航行系統、遠程控制系統以及具備相關功能的船舶。船舶有很多突出價值與意義，其可以執行特殊危險任務達到避免人員傷亡，且能長時間不間斷地航行，避免了由于駕駛人員疲勞而造成的風險，更可以降低人力的成本［1］。為了在設計船舶與智能航行算法時，保證開發的安全性，有效性和可持續性，必須進行廣泛的開發和測試［2］。然而傳統的測試昂貴耗時，具有風險性且只能夠在有限的場景下進行試驗。而智能航行仿真系平臺為這個難題提供了一種安全有效的解決途徑，借助仿真環境中對船舶動力學控制，多傳感器仿真，多場景模擬，可以極大地提高開發效率，驗證船舶的環境感知，定位，路徑規劃以及決策控制等多模塊算法的有效性［3］。

目前在關于船舶仿真的研究已經有了一些進展。侯玉乙［4］等針對船舶全局路徑規劃航線上可能存在未知靜態障礙物的問題，采用模糊控制算法實現自主避障功能，并應用Matlab軟件實現了相關仿真。吳青［5］等建立了風浪干擾下的船舶運動數學模型，運用模型預測控制算法對船舶運動軌跡進行控制，使用Matlab 與VC 混合編程搭建了船舶運動可視化仿真平臺。然而大多數的船舶的仿真是運用Matlab 的，不能夠立體化具象化的觀測。目前在仿真系統里能夠立體化具象化的觀測工具是基于ROS 的Gazebo 仿真器，相關的仿真研究也有了一些進展。崔靚［6］等針對服務機器人的智能導航技術問題，以機器人操作系統為開發平臺，應用三維物理仿真軟件Gazebo，結合Turtlebot3 機器人，對智能導航算法進行仿真驗證。結果表明機器人能準確構建地圖，實時避障，呈現良好的仿真效果。胡思旺［7］等針對移動機器人對自主導航的需求，設計、實現了基于ROS 的機器人自主導航解決方案。仿真試驗表明，該系統可以滿足移動機器人在陌生室內環境中的自主導航需求。本課題針對船舶仿真系統缺乏3D仿真而無法立體化觀測的現象，選用ROS 作為平臺，利用Gazebo 的三維可視化仿真工具進行船舶的仿真，使采用的算法效果更加直觀與真實，實現了三維立體化的船舶仿真平臺設計。

2 系統總體結構

本仿真平臺的系統架構流程圖如圖1 所示。主體兩大模塊，第一部分Gazebo 的船體建模模塊，包括船舶動力學模型設計與傳感器配置，并據此編寫Gazebo plugin 來使船舶能夠在Gazebo 的仿真環境下航行并獲得環境信息；第二部分ROS的算法導航模塊，包括感知系統設計，路徑規劃設計和航跡控制設計，這些設計可以使船舶更加精準地感知障礙物信息，規劃出全局路徑和局部路徑，并根據路徑和障礙物信息實現航跡控制，使船舶能夠避開障礙物的同時到達目標點。

圖1 系統總體結構

3 Gazebo的船體建模模塊

3.1 船舶動力學模型設計

船舶在航行時受到很多因素的影響，在仿真時，通過一組Gazebo 插件來模擬這些因素，包括船舶所受的水動力和力矩，以及船舶推力和力矩矢量。

船舶六自由度運動學動力學模型公式為

3.2 傳感器配置

船舶的感知系統主要利用船載傳感器設備以及信息處理技術設備，獲取船舶自身狀態信息和船體周圍各種信息，為船舶的避障、路徑規劃提供數據基礎，使船舶更加可靠、安全航行。單一的傳感器具有不同的成像機理，工作于不同的波長范圍，有不同的工作環境與要求，完成不同的功能［8］。但是單一傳感器不能獲取全面的數據信息，因此難以滿足實際的需求，同時利用多傳感器數據可以提供更多的數據以及更全面的觀測識別。

本課題設計的感知系統采用的傳感器有三目攝像頭、GPS定位系統、3D激光雷達、IMU慣性測量單元。通過在船體URDF 模型上加載上述傳感器模型，再利用Gazebo 的3D 動態模擬器提供的仿真環境，就可以實時地得到傳感器數據，最后通過ROS 的topic 機制傳遞給ROS，ROS 里加載的導航避障算法就能利用相應的傳感器數據進行路徑規劃和航跡控制。

4 智能航行控制系統設計

4.1 感知系統設計

本次設計采用激光雷達與視覺傳感器進行數據信息融合，利用各個傳感器的優點，取長補短，提高對障礙物信息獲取的可信度。具體設計如下：首先由于激光雷達與視覺傳感器在船舶不同位置上安裝，所以要把激光雷達與視覺傳感器進行空間融合，方法是了解相機和激光雷達的測距原理，提取出坐標轉換的模型，從而找到兩個傳感器坐標系與世界坐標系的轉換方程，最終完成激光雷達與視覺傳感器的空間融合。其次由于不同傳感器工作頻率不同，其中，激光雷達采集數據的頻率是10Hz，攝像機采集數據的頻率是30Hz，所以激光雷達和攝像機采集的數據都不是同一時間的信息，從而引起傳感器數據的時間上的不對應。所以激光雷達和攝像機數據信息還需要在時間上同步采集數據，實現數據時間上的融合。本次設計的方法是以采樣頻率低的激光雷達數據為基準，采用多線程同步方式從而實現傳感器在時間上的同步［9］。具體而言是創建攝像機和激光雷達線程以及數據融合線程，其中視覺線程是用來接收攝像機圖像數據并處理，激光雷達線程是用來接收激光雷達數據并處理，并將處理完的數據存放于獨立的鏈隊列中，然后作為融合線程的輸入，這樣可以保證激光雷達和視覺數據在融合時處于同一時刻點下，從而完成時間的融合。

4.2 路徑規劃設計

路徑規劃設計包括全局路徑規劃和局部路徑規劃。全局路徑規劃是在已知的環境中，給船舶規劃一條路徑，而局部路徑規劃是在環境信息完全未知或有部分可知情況下使用的。

本課題采用的全局規劃算法是Dijkstra 算法，Dijkstra 算法是單源最短路徑算法，以起始點為中心向外擴展到終點，來計算一個節點到其他所有節點的最短路徑［10］。算法過程如圖2。

圖2 Dijkstra算法流程圖

在船舶建立全局規劃時，使用的是改進的Dijkstra算法，代碼的具體方法與步驟如圖3。

圖3 全局規劃算法實際程序流程圖

局部規劃采用的是DWA 動態窗口法，主要原理是在速度空間中采樣多組速度，并模擬這些速度在一定時間內的運動軌跡，再通過一個評價函數對這些軌跡打分，最優的速度被選擇出來發送給下位機［11］。動態窗口法運行過程如圖4。

圖4 動態窗口法算法流程

在代碼加入動態窗口法時，整個的程序流程如圖5。

圖5 局部規劃算法實際程序流程圖

4.3 航跡控制設計

航跡控制采用的基于LOS 的顯式模型預測控制。主要原理船舶根據預設路徑及自身位置和艏向角信息，通過LOS 算法計算期望的艏向角ψlos作為跟蹤目標。艏向控制器采用顯式模型預測控制算法得到期望的艏搖方向力矩N，并作用于船舶，整個的設計圖如圖6所示。具體設計方法如下：

圖6 航跡控制流程圖

采用式（1）所示的船舶六自由度運動學動力學模型作為預測模型，并表達成離散化的狀態空間的形式：

其中：Ad，Bd，Cd為離散化后的狀態空間矩陣。

當受到環境干擾時，狀態量會存在靜差，因此采用增量形式的狀態空間模型，以實現無靜差控制［12］。另外，對于跟蹤問題，需要在狀態空間中加入參考狀態［13］，改寫的狀態空間模型如下所示：

定義基于Δu的性能指標函數為

其中：Ny和Nc分別是預測時域和控制時域，且Nc≤Ny；yk+i|k，rk+i|k和Δuk+i|k分別表示在k時刻系統實際輸出量，參考輸出量和控制增量在k+i時刻的預測值；是由最優控制增量構成的控制增量序列；Q，R 和P 分別是狀態，輸入和終端權重矩陣，且滿足Q=QT≥0 ，R=RT≥0，P≥0。

根據多參數二次規劃算法，可以求解該性能指標函數，以此來求取航跡控制的最優控制律。

5 仿真平臺實現與運行

在Gazebo 的船體建模和ROS 的算法導航的基礎上，添加模塊與模塊之間的數據處理和slam 建圖，再進行整體的數據流結構分析與設計如圖7，進而實現仿真平臺的設計。

圖7 仿真平臺設計圖

為了驗證所設計的船舶當真平臺的有效性，在Gazebo 仿真平臺中搭建真實海洋環境，并加載vrx船舶以及4個柱狀障礙物，如圖8所示。

圖8 Gazebo 仿真圖

在此基礎上，通過運用Google 的cartographer進行slam建圖，把海洋信息和障礙物通過柵格地圖的形式記錄下來。

把建好的地圖通過rviz 顯示出來，運行movebase 節點，選定船舶的初始位置和終點位置，即可進行全局規劃和局部規劃進行避障，如圖9所示。

圖9 rviz仿真圖

船舶根據初始位置和最終位置規劃出了一條路線，在圖中為綠色，船舶便可以根據規劃出的路線進行前進，最終到達指定位置。

6 結語

本文采用ROS作為平臺，設計并實現了船舶自主導航系統。該系統采用Gazebo 環境模擬，gps 定位，激光雷達導航，Dijkstra 全局路徑規劃，DWA 局部路徑規劃，結合航跡控制成功地模擬了船舶的導航與避障，突破了傳統的2D 船舶的Matlab模擬，實現了逼真的3D具象化立體化船舶仿真。