秦有成 陳紅衛(wèi) 袁 偉

（江蘇科技大學(xué)電子信息學(xué)院 鎮(zhèn)江 212100）

1 引言

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

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

2 系統(tǒng)總體結(jié)構(gòu)

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

圖1 系統(tǒng)總體結(jié)構(gòu)

3 Gazebo的船體建模模塊

3.1 船舶動力學(xué)模型設(shè)計

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

船舶六自由度運動學(xué)動力學(xué)模型公式為

3.2 傳感器配置

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

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

4 智能航行控制系統(tǒng)設(shè)計

4.1 感知系統(tǒng)設(shè)計

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

4.2 路徑規(guī)劃設(shè)計

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

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

圖2 Dijkstra算法流程圖

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

圖3 全局規(guī)劃算法實際程序流程圖

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

圖4 動態(tài)窗口法算法流程

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

圖5 局部規(guī)劃算法實際程序流程圖

4.3 航跡控制設(shè)計

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

圖6 航跡控制流程圖

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

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

當受到環(huán)境干擾時，狀態(tài)量會存在靜差，因此采用增量形式的狀態(tài)空間模型，以實現(xiàn)無靜差控制［12］。另外，對于跟蹤問題，需要在狀態(tài)空間中加入?yún)⒖紶顟B(tài)［13］，改寫的狀態(tài)空間模型如下所示：

定義基于Δu的性能指標函數(shù)為

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

根據(jù)多參數(shù)二次規(guī)劃算法，可以求解該性能指標函數(shù)，以此來求取航跡控制的最優(yōu)控制律。

5 仿真平臺實現(xiàn)與運行

在Gazebo 的船體建模和ROS 的算法導(dǎo)航的基礎(chǔ)上，添加模塊與模塊之間的數(shù)據(jù)處理和slam 建圖，再進行整體的數(shù)據(jù)流結(jié)構(gòu)分析與設(shè)計如圖7，進而實現(xiàn)仿真平臺的設(shè)計。

圖7 仿真平臺設(shè)計圖

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

圖8 Gazebo 仿真圖

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

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

圖9 rviz仿真圖

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

6 結(jié)語

本文采用ROS作為平臺，設(shè)計并實現(xiàn)了船舶自主導(dǎo)航系統(tǒng)。該系統(tǒng)采用Gazebo 環(huán)境模擬，gps 定位，激光雷達導(dǎo)航，Dijkstra 全局路徑規(guī)劃，DWA 局部路徑規(guī)劃，結(jié)合航跡控制成功地模擬了船舶的導(dǎo)航與避障，突破了傳統(tǒng)的2D 船舶的Matlab模擬，實現(xiàn)了逼真的3D具象化立體化船舶仿真。