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

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

1 引言

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

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

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

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

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

3 Gazebo的船體建模模塊

3.1 船舶動(dòng)力學(xué)模型設(shè)計(jì)

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

船舶六自由度運(yùn)動(dòng)學(xué)動(dòng)力學(xué)模型公式為

3.2 傳感器配置

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

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

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

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

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

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

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

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

圖2 Dijkstra算法流程圖

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

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

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

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

在代碼加入動(dòng)態(tài)窗口法時(shí)，整個(gè)的程序流程如圖5。

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

4.3 航跡控制設(shè)計(jì)

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

圖6 航跡控制流程圖

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

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

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

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

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

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

5 仿真平臺(tái)實(shí)現(xiàn)與運(yùn)行

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

圖7 仿真平臺(tái)設(shè)計(jì)圖

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

圖8 Gazebo 仿真圖

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

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

圖9 rviz仿真圖

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

6 結(jié)語(yǔ)

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