自主巡航無人船控制系統(tǒng)設(shè)計與實現(xiàn)

劉雨青，楊喜清，楚慧勇，黃璐瑤

（上海海洋大學(xué) 工程學(xué)院，上海 201306）

0 引言

近年來，隨著池塘養(yǎng)殖產(chǎn)業(yè)的迅速發(fā)展,各式各樣的自動化投餌設(shè)備也相繼出現(xiàn)，越來越多的無人投餌船被用到養(yǎng)殖當(dāng)中，王真亮[1]設(shè)計了一種自主巡航式太陽能充電投餌船，但陰雨天氣時無人船就會受到影響。高潔等[2]設(shè)計了一種可以實現(xiàn)兩種方式進行無線充電的無人船充電系統(tǒng)。一方面可以實現(xiàn)無線充電，另一方面實現(xiàn)太陽能供電的功能,但是無人船的自主返航也將成為一個難點?；诖搜芯恳环N可以自主巡航無人船控制系統(tǒng)，實現(xiàn)無人船無法順利返航的問題。

無人船自主巡航的關(guān)鍵技術(shù)就是航向的控制、航跡的選擇、動態(tài)避障等方面。王丹等[3]人通過無人船的航跡跟蹤設(shè)計，設(shè)計航向以及航向控制器，將模糊控制與傳統(tǒng)的PID控制進行結(jié)合的方式進行無人船的控制。薛飛[4]提出了一種將無人船的路徑分為全局路徑以及局部路徑兩個部分完成無人船的航行任務(wù)。通過傳感器感知周圍的環(huán)境實現(xiàn)動態(tài)避障。霍星星等[5]人基于PID控制、模糊控制、神經(jīng)網(wǎng)絡(luò)三種控制算法方式設(shè)計了一種船舶的自動舵。以航向的偏差作為輸入，通過PID算法對無人船進行控制。J.M.Larrazabal等[6]采用遺傳算法對傳統(tǒng)的PID控制進行優(yōu)化調(diào)節(jié)，無人船實際航行中的各種未知條件使用模糊控制進行調(diào)節(jié)，實現(xiàn)無人船的路徑跟蹤。

本文的研究目的就是為了實現(xiàn)無人船高效快速自主返航，將返航的過程分為兩個部分：全局路徑規(guī)劃與局部路徑規(guī)劃。全局路徑規(guī)劃通過模糊PID實現(xiàn)無人船的航向控制，航跡控制算法實現(xiàn)無人船從一點航行至另一點。局部路徑規(guī)劃通過激光雷達(dá)與圖像識別技術(shù)進行融合來實現(xiàn)無人船的動態(tài)避障。

1 無人船系統(tǒng)方案設(shè)計

1.1 系統(tǒng)構(gòu)成

無人船系統(tǒng)構(gòu)成整體分為三個部分：主控制器模塊、電機驅(qū)動模塊、避障模塊、航向與定位模塊、電源管理模塊等組成[7]。如圖1所示。

圖1 無人船的系統(tǒng)構(gòu)成

避障模塊：激光雷達(dá)可以對周圍的環(huán)境進行感知，獲取障礙物的距離和角度信息。攝像頭通過YOLOV4目標(biāo)檢測算法可以對障礙物進行識別。同時將信息通過串口3傳輸給STM32進行避障處理。當(dāng)遇到的較大障礙物時由激光雷達(dá)進行處理，當(dāng)障礙物較小激光雷達(dá)不能檢測時，使用圖像識別技術(shù)進行避障。

主控制器模塊：主控制器模塊主要是航向控制算、航跡選擇算法與各傳感器進行結(jié)合使得無人船具有自主巡航功能。主控制器將樹莓派傳輸過來的信息與相關(guān)控制算法進行結(jié)合，進而控制電機驅(qū)動模塊，使無人船進行動態(tài)避障。

電源管理模塊：對無人船的電池進行管理，當(dāng)無人船電池電量不足時，發(fā)出提示信息，通過網(wǎng)絡(luò)通訊模塊傳輸?shù)缴衔粰C。

1.2 無人船控制方案

由于差分GPS可以實時獲取經(jīng)緯度、航向角等信息，所以將GPS獲取的位置信息以及航向角信息作為控制算法的輸入?yún)?shù)。通過計算當(dāng)前點坐標(biāo)與目標(biāo)點的坐標(biāo)的偏差，獲取無人船的期望航向角，在通過主控芯片控制兩個電機的PWM調(diào)整兩電機的轉(zhuǎn)向，控制無人船進行實時調(diào)整方向，實現(xiàn)閉環(huán)控制。無人船控制策略如圖2所示。

圖2 無人船控制策略

2 無人船全局路徑規(guī)劃

無人船全局路徑規(guī)劃主要是通過對航跡的選擇實現(xiàn)無人船點到點的航行，通過模糊PID控制實現(xiàn)無人船的航向控制，通過仿真實驗驗證航向控制策略的可行性。航向與航跡控制的結(jié)合使得無人船自主航行。

2.1 航跡選擇設(shè)計

航跡的選擇首先要進行航跡數(shù)學(xué)模型的建立，航跡數(shù)學(xué)模型如圖3所示。無人船起始點A(xA,yA),目標(biāo)點坐標(biāo)B(xB,yB),CN(xN,yN)為無人船的當(dāng)前航向目標(biāo)點CN-1(xN-1,yN-1)為無人船GPS的上一坐標(biāo)點。根據(jù)無人船的起始點位置坐標(biāo)以及目標(biāo)點坐標(biāo)可以確認(rèn)無人船的實際航跡，同時也可以確認(rèn)無人船實際偏離航跡的垂直距離E,θ3為無人船與理論航向的允許偏差角度值，在調(diào)整時需要在此范圍內(nèi)調(diào)整。其中θ1指的是無人船的實際航向與當(dāng)前坐標(biāo)點與目標(biāo)點的夾角，θAB為無人船的實際航向與x軸的夾角，θACN為上一個目標(biāo)點到當(dāng)前點與x軸的夾角。?為無人船在規(guī)定航跡的最大偏差角度范圍。

圖3 航跡數(shù)學(xué)模型

由已知條件可得：

由式(1)和式(2)根據(jù)勾股定理可得軌跡的垂直位移E為：

E為無人船在正確的航跡偏差范圍。保持直線行駛。當(dāng)E＞0航跡偏理論路線的左側(cè)，反之則偏離理論路線的右側(cè)，θAB-φ≤θ4≤θAB+φ無人船在正常的航線上。

通過相關(guān)算法可以計算出起始點與目標(biāo)點的距離、方位角信息[8]。

2.2 航向控制策略

為了提高無人船的航向控制器性能，在PID控制的基礎(chǔ)上采用模糊PID控制完成無人船控制，其控制原理如圖3所示。

圖4 模糊PID控制

首先進行Nomoto模型[9]建立，得出航向角與推進器偏轉(zhuǎn)角之間的傳遞函數(shù)。其次分析模糊PID控制原理，目標(biāo)點的航向角φ0與當(dāng)前的航向角φ的偏差e以及偏差變化率ec作為模糊控制的輸入，經(jīng)過相關(guān)模糊處理輸出相應(yīng)的參數(shù)作為PID控制器的輸入實時調(diào)整PID參數(shù)，PID控制算法得到的推進器偏轉(zhuǎn)角δ輸入Nomoto模型，即可得到無人船的航向，實現(xiàn)閉環(huán)控制。

仿真實驗需要將輸入的兩個實際值的變量進行模糊化處理得到相應(yīng)的論域EC、E，并將EC、E輸出論域控制在[-3,3],控制在[0,3]?？紤]到模糊子集對無人船的航向影響，將論域量化為{NB、NM、NS、ZO、PS、PM、PB}7個級別，采用隸屬度函數(shù)為三角形函數(shù)，得出隸屬度函數(shù)。該設(shè)計采用加權(quán)平均值法將模糊規(guī)則進行推理得到模糊集，然后將模糊集轉(zhuǎn)換為精確地ΔKP、ΔKI、ΔKD值。具體計算公式如式1所示，其中u為輸出值，xi為輸出模糊論子集，ki為輸出子集對應(yīng)的隸屬度[10]。

2.3 航向控制仿真

無干擾的情況下，將無人船航速設(shè)置為2m/s，初始角度為0°起始點位置坐標(biāo)(0,0),無人船的目標(biāo)點為(60,50)，得出無人船的方位為50°，使用Simulink模型搭建進行仿真如圖2-2所示，仿真結(jié)果對比圖如圖5所示：

圖5 模糊PID控制Simulink仿真圖

從仿真結(jié)果可以看出，PID控制下，大概第6s時無人船的航向與目標(biāo)點一致。模糊PID控制下大概第7s航向與目標(biāo)點方向一致，如圖6所示。

圖6 兩種控制策略下的航向角仿真結(jié)果

存在干擾的情況下，為了驗證模糊PID控制在實際的控制過程中的可靠性，仿真更接近真實情況，假定無人船受到一個自南向北的1m/s水流干擾，無風(fēng)的情況下的進行仿真,將無干擾與有干擾的情況進行仿真結(jié)果對比，如圖6所示。綜合分析在有外界干擾的情況下，無人船達(dá)到目標(biāo)方向的時間會變長，但是依然具有更高的穩(wěn)定性。

圖7 航向控制仿真結(jié)果

3 無人船局部路徑規(guī)劃

無人船動態(tài)避障在不偏離全局路線的情況下，將激光雷達(dá)對環(huán)境的感知信息與圖像識別的信息融合進行無人船的避障。避開障礙物后可以重新回到規(guī)定航線進行航行，在某種程度上保證了無人船的順利返航。圖像識別部分利用OpenCV對障礙物的信息進行采集，采用YOLOV4目標(biāo)檢測算法實現(xiàn)障礙物的識別。激光雷達(dá)環(huán)境感知部分采用的是官方提供的SDK進行開發(fā)調(diào)試。

3.1 激光雷達(dá)環(huán)境感知

激光雷達(dá)通過數(shù)據(jù)的讀取，得到相對應(yīng)的數(shù)據(jù)報文，再經(jīng)過相關(guān)公式算法就可以測出實際船與障礙物的實際夾角，從得到的角度和距離進行判斷，進而控制電機螺旋槳M1和電機螺旋槳M2的轉(zhuǎn)動。并且還可以通過串口1以無線通訊的方式發(fā)送給上位機來進行顯示障礙物的位置距離，也可以進行上位機的控制，進而控制無人船的電機轉(zhuǎn)動。無人船行駛過程中激光雷達(dá)的掃描圖如圖8所示。

圖8 激光雷達(dá)掃描圖

3.2 障礙物識別

在樹莓派上這種計算資源相對有限的開發(fā)板上做目標(biāo)檢測使用YOLOV4相對較好。YOLOV4網(wǎng)絡(luò)架構(gòu)可以將YOLOV4目標(biāo)檢測算法分為4個部分，具體包括：輸入端、基準(zhǔn)網(wǎng)絡(luò)、Neck網(wǎng)絡(luò)與Head輸出端[10]，將YOLOV4模型應(yīng)用到數(shù)據(jù)采集、網(wǎng)絡(luò)設(shè)計、模型參數(shù)訓(xùn)練：

數(shù)據(jù)采集，圖9為USB免驅(qū)采集障礙物部分樣本，障礙種類有：塑料瓶、易拉罐、打撈的船只等常規(guī)障礙物。采集圖片樣本后，將VOC格式數(shù)據(jù)借助LabelImg軟件進行標(biāo)注，是一個在Python下運行的軟件，可以高效地捕捉到目標(biāo)物，提高效率。圖10為LablImg軟件進行標(biāo)注圖。

圖9 障礙物采集部分樣本圖

圖10 LablImg標(biāo)注軟件

目標(biāo)識別實驗通過對障礙物數(shù)據(jù)的采集、標(biāo)注、訓(xùn)練等操作，通過實驗驗證目標(biāo)障礙物識別效果良好，通過YOLOV4目標(biāo)檢測算法可以實現(xiàn)對物體的精準(zhǔn)識別，符合無人船目標(biāo)識別的技術(shù)需求。圖11為無人船的部分障礙物識別。

圖11 障礙物識別實驗

4 自主巡航控制

自主巡航的實現(xiàn)首要要對整體的框架進行設(shè)計，然后將依靠的幾個主干分支程序填寫進去。自主巡航實先主要依托的幾大分支：主程序、自主巡航程序、地圖程序、動態(tài)避障程序[11]。

自主巡航程序主要用于起始點至目標(biāo)點的設(shè)定，將地圖設(shè)定信息傳輸給主控芯片。

地圖程序主要是獲取設(shè)定無人船的起始點，目標(biāo)點的坐標(biāo)信息。

動態(tài)避障程序主要是通過激光雷達(dá)來感知周圍環(huán)境，攝像頭用于圖像識別障礙物，兩者進行結(jié)合達(dá)到避障目的。

無人船進行自主巡航流程圖如圖12所示。

圖12 自主巡航流程圖

實現(xiàn)自主巡航具體分為以下幾個步驟：

1）上位機主控界面中點擊自主巡航，此刻無人船開啟了自主巡航功能。無人船控制程序初始化，在上位機界面地圖上標(biāo)注一點中心點作為無人船的起始出發(fā)點，緊接著就是在無人船起始點經(jīng)緯度編輯框中輸入經(jīng)緯度坐標(biāo)信息，作為無人船的起始點經(jīng)緯度坐標(biāo)，并進行設(shè)定，接下來響應(yīng)函數(shù)將獲取并存儲起始點緯度坐標(biāo)。同時利用UM_StartPosition消息將無人船起始點地圖坐標(biāo)發(fā)送給地圖程序，此刻地圖程序中的響應(yīng)函數(shù)接收到地圖坐標(biāo)以后，將起始點位置顯示在地圖上。

2）在上位機地圖界面中設(shè)定四個點作為無人船航行點，其中一點為最終目標(biāo)點。地圖程序中的鼠標(biāo)點擊后，相應(yīng)的響應(yīng)函數(shù)OnButtonDown依次獲取目標(biāo)點地圖坐標(biāo)，并利用UM_TargetPoint消息將目標(biāo)點坐標(biāo)發(fā)送給主程序。當(dāng)相應(yīng)的響應(yīng)函數(shù)接收到目標(biāo)點坐標(biāo)后，存在數(shù)組M_TargetArray中，根據(jù)無人船的起始點與目標(biāo)點地圖坐標(biāo)信息，根據(jù)先關(guān)算法算出實際的經(jīng)緯度坐標(biāo)并將數(shù)值存儲到相應(yīng)的數(shù)組中，將無人船切換位自主巡航模式后，響應(yīng)函數(shù)Wm_AutoNaviStart將消息傳遞給主控制程序，對應(yīng)的響應(yīng)函數(shù)OnStartAuto將無人船航行起始點設(shè)置為1。

3）當(dāng)主程序接收到GPS信息后，開始標(biāo)志位才置1，表示無人船進入自主航行狀態(tài)，將接收到的GPS信息通過串口發(fā)送給主控制程序。然后主控制程序?qū)⒔邮盏降姆较騻鞲衅髂K、激光雷達(dá)、圖像識別模塊的信息進行解析融合，通過相關(guān)算法計算航向角、距離等信息，從而完成無人船的自主巡航。

5 實船測試

無人船的自主巡航、定點返航實驗，選取上海海洋大學(xué)的一個人工湖作為測試水域，忽略無人船與水體的摩擦，無風(fēng)速，1m/s水流速情況下進行測試。

在上位機界面完成航跡點的設(shè)定，將坐標(biāo)（30.885188N，121.898611E）作為起始點,無人船的起始航向為180度。由MATLAB得出擬合曲線，由網(wǎng)絡(luò)通訊模塊將無人船軌跡信息上傳上位機界面，圖13為無人船的模擬軌跡與實際航行軌跡圖。

圖13 無人船試驗航跡圖

圖14為無人船遇到障礙物進行自主避障的圖片。當(dāng)無人船航行到理論的目標(biāo)點時，通過傳統(tǒng)目標(biāo)識別技術(shù)去緩慢的逼近目標(biāo)點，可以避免由于外界不可抗力因素以及GPS定位不精準(zhǔn)帶來的誤差。當(dāng)無人船航行至理論的無線充電裝置坐標(biāo)時，通過目標(biāo)識別技術(shù)去緩慢的逼近目標(biāo)點，圖15為無人船識別到無線充電裝置，正在慢慢逼近目標(biāo)點。

圖14 無人船自主避障

圖15 目標(biāo)裝置圖像識別

6 結(jié)語

本文主要研究主要是實現(xiàn)無人船自主巡航進行返航的問題。通過模糊PID控制實現(xiàn)對無人船航向控制；通過航跡路線的選擇、路徑控制算法實現(xiàn)無人船點到點的自主航行；通過激光雷達(dá)與圖像識別技術(shù)進行融合實現(xiàn)無人船的動態(tài)避障；但是在實際的環(huán)境中，大水流風(fēng)速影響較大，對無人船的返航產(chǎn)生影響，所以在后面研究中針對不同的水流、風(fēng)速展開研究，真正意義上的無人船的自主精準(zhǔn)返航。