楊忠凱，仲偉波*,2，馮友兵，孫彬

1 江蘇科技大學(xué) 電子信息學(xué)院，江蘇 鎮(zhèn)江 212003

2 江蘇科技大學(xué) 海洋裝備研究院，江蘇 鎮(zhèn)江 212003

0 引 言

無(wú)人艇（USV）是一種能夠自主航行并完成任務(wù)的水面艦艇，在軍事作戰(zhàn)、維護(hù)海域安全、海洋資源勘測(cè)、環(huán)境監(jiān)測(cè)等領(lǐng)域具有廣泛的應(yīng)用前景[1]。無(wú)人艇的運(yùn)動(dòng)控制主要包括航速與航向控制、航跡控制、智能規(guī)劃、避障與導(dǎo)航、編隊(duì)協(xié)同等，其中航跡控制是無(wú)人艇能夠自主完成任務(wù)的前提，也是無(wú)人艇智能控制的核心之一[2-3]。

無(wú)人艇航跡控制的主要方法有視線導(dǎo)引(line-of-sight, LOS)算法[4]、反步滑模技術(shù)[5]、串級(jí)PID[6]、神經(jīng)網(wǎng)絡(luò)控制[7]、魯棒自適應(yīng)控制[8]、模糊控制[9]等。田勇等[10]利用LOS 及抗飽和PID 算法設(shè)計(jì)并實(shí)現(xiàn)了直線航跡跟蹤，解決了航向超調(diào)和振蕩問題，但外界擾動(dòng)對(duì)航向精度和穩(wěn)定性具有較大影響。陳霄等[11]利用改進(jìn)的LOS 算法與自適應(yīng)滑模航向控制算法結(jié)合，引入自適應(yīng)觀測(cè)器對(duì)漂角進(jìn)行實(shí)時(shí)估計(jì)和補(bǔ)償，取得了較好的航跡跟蹤效果，但在實(shí)艇驗(yàn)證的期望航跡拐點(diǎn)處，無(wú)人艇航跡跟蹤誤差較大。祝鏡等[12]基于LOS 算法對(duì)增量式PID、模糊PID 以及可變船長(zhǎng)比模糊PD 這3 種控制算法進(jìn)行仿真分析，結(jié)果表明可變船長(zhǎng)比模糊PD 控制更具抗干擾優(yōu)勢(shì)。范云生等[13]設(shè)計(jì)了一種LOS 與模糊自適應(yīng)PID 結(jié)合的航跡控制算法并進(jìn)行實(shí)艇試驗(yàn)，結(jié)果表明該算法降低了時(shí)變漂角對(duì)航跡控制的影響，但在路徑拐點(diǎn)處會(huì)出現(xiàn)一定的轉(zhuǎn)向超調(diào)及較大的位置偏差。Fossen等[14]將非線性積分自適應(yīng)LOS 算法與PID 航向控制器結(jié)合，結(jié)果表明該航跡控制方法適用于水面無(wú)人艇航跡控制，但在路徑曲率改變時(shí)，需要較長(zhǎng)的時(shí)間跟隨期望路徑。

國(guó)內(nèi)外學(xué)者對(duì)無(wú)人艇航跡控制理論、模型和實(shí)現(xiàn)方法等進(jìn)行了研究，但無(wú)人艇實(shí)際工作環(huán)境復(fù)雜多變。鑒于此，本文將根據(jù)無(wú)人艇實(shí)際航行時(shí)會(huì)受到的導(dǎo)航信號(hào)及環(huán)境的影響，對(duì)無(wú)人艇航跡控制進(jìn)行研究。采用基于模糊控制可變船長(zhǎng)比的LOS 算法與自抗擾航向控制器（ADRC）結(jié)合的航跡控制方法，首先進(jìn)行三角形航跡仿真驗(yàn)證，驗(yàn)證算法的可行性。隨后進(jìn)行湖上試驗(yàn)，驗(yàn)證無(wú)人艇航跡控制的效果。

1 無(wú)人艇航跡控制方法

無(wú)人艇在實(shí)際航行時(shí)，存在風(fēng)、浪、流、遮擋等環(huán)境干擾，會(huì)出現(xiàn)偏離目標(biāo)航線的情況。針對(duì)這些問題，需對(duì)無(wú)人艇航跡控制進(jìn)行細(xì)分研究。本文將無(wú)人艇航跡控制分為GPS 信號(hào)有效和無(wú)效2 種情況，分別介紹航跡控制方法。

1.1 GPS 信號(hào)有效時(shí)的航跡控制

LOS 算法在水面船舶航跡控制中應(yīng)用廣泛，其實(shí)質(zhì)是控制船舶實(shí)際航向?qū)?zhǔn)目標(biāo)航向的LOS角，如圖1 所示。圖中，Pk?1點(diǎn)為航跡起點(diǎn)，Pk點(diǎn)為設(shè)定目標(biāo)點(diǎn)，O點(diǎn)為無(wú)人艇實(shí)時(shí)位置，設(shè)O點(diǎn)經(jīng)緯度為（Oj，Ow），Pk?1點(diǎn)經(jīng)緯度為（P(k?1)j，P(k?1)w），Pk點(diǎn)經(jīng)緯度為（Pkj，Pkw），PLOS點(diǎn)經(jīng)緯度為（Pj，Pw）。以無(wú)人艇O點(diǎn)為圓心，以可調(diào)參數(shù)R為可視半徑得到一扇形區(qū)域。假設(shè)船長(zhǎng)為L(zhǎng)，船長(zhǎng)比為n，則R=nL[12]，Δh為航跡偏差。圖1 中扇形區(qū)域交直線Pk?1Pk于點(diǎn)PLOS，O點(diǎn)與實(shí)時(shí)目標(biāo)點(diǎn)PLOS的連線方向?yàn)橄鄬?duì)于真北方向的實(shí)時(shí)目標(biāo)航向Ψr。無(wú)人艇實(shí)際航向Ψ 由GNSS 設(shè)備獲取，Ψr和 Ψ 的差為消除航跡偏差的角度ΔΨ 。當(dāng)無(wú)人艇駛?cè)隤k點(diǎn)圓域內(nèi)時(shí)，則切換新的目標(biāo)點(diǎn)，以此類推，最終完成無(wú)人艇的航跡控制。

圖1 無(wú)人艇LOS 航跡控制原理圖Fig. 1 Schematic diagram of LOS track control for USV

1.2 無(wú)GPS 信號(hào)時(shí)的航跡控制

無(wú)人艇實(shí)際航行時(shí)存在過橋洞、船閘等GPS信號(hào)無(wú)效的情況，為保證航跡，本文采用慣性導(dǎo)航融合處理方法。如圖2 所示，以無(wú)人艇過橋洞為例，假設(shè)無(wú)人艇在O點(diǎn)時(shí)GPS 信號(hào)無(wú)效，此時(shí)系統(tǒng)需記錄最后一次GPS 信號(hào)有效時(shí)的航速v，慣性導(dǎo)航設(shè)備的無(wú)人艇艏向 ψX、當(dāng)前無(wú)人艇位置O到目標(biāo)點(diǎn)Pk+1的距離d。為到達(dá)Pk+1點(diǎn)，無(wú)人艇以目標(biāo)航向 ψr=ψX、速度v、持續(xù)t=d/v時(shí)間。如果GPS 信號(hào)持續(xù)t時(shí)間后仍無(wú)效，則此時(shí)無(wú)人艇目標(biāo)航向 ψr+1為慣性導(dǎo)航設(shè)備提供的無(wú)人艇艏向 ψX加 上 下 一 點(diǎn) 的 航 向 偏 差 ?θ， ?θ由 式(4)確定：

圖2 無(wú)人艇有遮擋航跡控制示意圖Fig. 2 Schematic of covered track control for USV

式中， ψk+1與 ψk+2分 別為Pk+1點(diǎn)相對(duì)于Pk點(diǎn)的目標(biāo)航向和Pk+2點(diǎn)相對(duì)于Pk+1點(diǎn)的目標(biāo)航向，由式(5)和式(6)求得。依此類推，實(shí)現(xiàn)無(wú)人艇在GPS 信號(hào)無(wú)效情況下的航跡控制。

2 無(wú)人艇航跡控制算法

對(duì)于相同的目標(biāo)航跡，采用不同的可視半徑R具有不同的航跡控制效果，為了能夠在不同的環(huán)境影響下得到最合適的船長(zhǎng)比n，采用模糊控制方法，實(shí)時(shí)調(diào)節(jié)n，航向控制采用自抗擾控制器；傳統(tǒng)的PID 控制算法參數(shù)固定，很難選取合適的控制參數(shù)，自抗擾算法具有過渡過程，可實(shí)時(shí)給予補(bǔ)償，并可對(duì)狀態(tài)誤差進(jìn)行非線性組合。航跡控制系統(tǒng)結(jié)構(gòu)圖如圖3 所示，圖中，δr為控制舵機(jī)的輸入量，δ 為作用在無(wú)人艇模型上的控制量。

圖3 航跡控制系統(tǒng)框圖Fig. 3 System diagram of track control

2.1 模糊控制器

圖3 中模糊控制器的輸入為實(shí)時(shí)位置距目標(biāo)航線的航跡偏差?h和航跡偏差變化率dh，輸出為可變船長(zhǎng)比n。該系統(tǒng)可根據(jù)無(wú)人艇環(huán)境變化產(chǎn)生的 ?h和dh實(shí)時(shí)調(diào)節(jié)n，從而改變LOS 航跡算法中的R，得出最佳目標(biāo)航向 ψr。

2.1.1 模糊化與規(guī)則庫(kù)

表1 船長(zhǎng)比模糊控制表Table 1 Fuzzy control table of ship length ratio

可變船長(zhǎng)比n的作用為：選用較小的n時(shí)，當(dāng)受到風(fēng)浪流影響，無(wú)人艇能夠更快地到達(dá)目標(biāo)航線，但容易產(chǎn)生超調(diào)；選用較大的n時(shí)，無(wú)人艇能夠平穩(wěn)地保持目標(biāo)航向，但受到風(fēng)浪流干擾，不能快速貼近目標(biāo)航線。本文模糊規(guī)則表的主要思路為：當(dāng) ?h較大時(shí)，無(wú)人艇偏離目標(biāo)航線較為嚴(yán)重，應(yīng)采用較小的n，盡快地將無(wú)人艇的位置調(diào)節(jié)到目標(biāo)航線附近；當(dāng) ?h較小、dh較大時(shí)，說明此時(shí)正在受到環(huán)境影響，應(yīng)采用較小的n；當(dāng) ?h較小、dh較小時(shí)，說明此時(shí)航行比較穩(wěn)定，應(yīng)采用較大的n。無(wú)人艇實(shí)際航行過程中通過 ?h和dh實(shí)時(shí)調(diào)節(jié)可n，可快速適應(yīng)環(huán)境變化。

2.1.2 解模糊

模糊控制器的輸出為精確的數(shù)值，因此需要進(jìn)行解模糊運(yùn)算，解模糊有很多種求法，常見的有加權(quán)平均法、中值法、面積重心法等[15]，本文選用面積重心法，具體公式為

式中：k為變量；m為模糊集元素個(gè)數(shù)；uk為模糊子集元素值；uv(uk)為模糊子集對(duì)應(yīng)元素的隸屬度值；∑為輸出模糊子集中各元素的隸屬度值的求和；u為輸出的精確數(shù)值。

2.2 自抗擾航向控制器

自抗擾控制器由3 個(gè)部分組成：跟蹤微分器、擴(kuò)張狀態(tài)觀測(cè)器和非線性狀態(tài)誤差反饋控制律。跟蹤微分器安排過渡過程并提取微分信號(hào)，解決由于航向突變導(dǎo)致的超調(diào)問題；擴(kuò)張狀態(tài)觀測(cè)器不僅對(duì)航向值進(jìn)行觀測(cè)，還可實(shí)現(xiàn)對(duì)不確定擾動(dòng)的估計(jì)；非線性狀態(tài)誤差反饋控制律根據(jù)過渡過程與狀態(tài)估計(jì)之間誤差的非線性組合和擾動(dòng)估計(jì)量的補(bǔ)償來(lái)生成控制信號(hào)[3]，自抗擾結(jié)構(gòu)如圖4所示。圖中，V0(k)為目標(biāo)航向，V1(k)為V0(k)的過渡過程，V2(k)為過渡過程的微分，e1(k)和e2(k)為過渡過程信號(hào)與觀測(cè)信號(hào)的誤差函數(shù)，U0(k)為中間控制量，U(k)為輸入舵角值，Y(k)為實(shí)際航向，Z1(k)～Z3(k)為系統(tǒng)狀態(tài)的觀測(cè)量，b0為增益參數(shù)。

圖4 自抗擾控制器結(jié)構(gòu)圖Fig. 4 Structure diagram of ADRC

實(shí)驗(yàn)中采用的跟蹤微分器離散表達(dá)式及擴(kuò)張狀態(tài)觀測(cè)器表達(dá)式與文獻(xiàn)[16]中的表達(dá)式相同，非線性狀態(tài)誤差反饋控制律采用非線性狀態(tài)誤差的PD 控制律，如式(8)所示。式(8)中非線性函數(shù)fal與文獻(xiàn)[16]中的相同，kp，kd， δ0，b0為可調(diào)參數(shù)。

3 控制系統(tǒng)及軟件設(shè)計(jì)

采用國(guó)產(chǎn)芯片龍芯1C 和國(guó)產(chǎn)嵌入式實(shí)時(shí)操作系統(tǒng)RT-Thread 作為無(wú)人艇控制的自主可控平臺(tái)，研制的數(shù)據(jù)采集控制板卡接口豐富，電源、傳感器、電機(jī)舵機(jī)控制信號(hào)、通信模塊等均接入到數(shù)據(jù)采集控制板卡，由數(shù)據(jù)采集控制板卡解析出時(shí)間、速度、方位角、GPS 狀態(tài)、經(jīng)緯度、姿態(tài)角和障礙物距離等。為完成航跡控制任務(wù)，采用RT-Thread 國(guó)產(chǎn)嵌入式實(shí)時(shí)操作系統(tǒng)多線程設(shè)計(jì)。主要分為傳感器數(shù)據(jù)接收處理線程、航跡算法線程、航向控制線程、電機(jī)舵機(jī)控制線程和上位機(jī)數(shù)據(jù)交互線程，航跡流程圖如圖5 所示。系統(tǒng)首先接收各傳感器發(fā)送的數(shù)據(jù)，解析并提取有效數(shù)據(jù)，根據(jù)GPS 狀態(tài)選擇航跡控制方法，并將計(jì)算得到的脈沖寬度調(diào)制(PWM)值傳給舵機(jī)，舵機(jī)接收到信號(hào)后執(zhí)行命令。

4 無(wú)人艇航跡控制仿真

4.1 仿真環(huán)境

根據(jù)設(shè)計(jì)的航跡控制算法使用Simulink 工具搭建航跡仿真環(huán)境，Simulink 航跡仿真框圖如圖6所示。仿真框圖主要分為3 個(gè)部分：航跡控制環(huán)、航向控制環(huán)和更新航跡環(huán)。

航跡控制環(huán)采用基于模糊控制可變船長(zhǎng)比的LOS 算法，輸入為當(dāng)前位置、目標(biāo)點(diǎn)位置和根據(jù)模糊控制可變船長(zhǎng)比算法求得的可視半徑，輸出為目標(biāo)航向。航向控制環(huán)采用自抗擾控制器，航向控制環(huán)中仿真的無(wú)人艇數(shù)學(xué)運(yùn)動(dòng)模型采用Nomoto 模型[17]，表達(dá)式如式(9)所示；舵機(jī)模型采用一階慣性模型，表達(dá)式如式(10)所示；無(wú)人艇航行中受到風(fēng)浪對(duì)航行的干擾可等效到舵角干擾，通過加入白噪聲來(lái)模擬風(fēng)浪流的干擾，表達(dá)式如式(11)所示。更新航跡環(huán)可根據(jù)無(wú)人艇當(dāng)前位置、速度和實(shí)際航向角求得下一點(diǎn)位置。仿真算法中無(wú)人艇位置均采用經(jīng)緯度坐標(biāo)表示。

圖5 航跡控制軟件框圖Fig. 5 Software block diagram of track control

4.2 實(shí)驗(yàn)仿真

仿真航跡為三角形，仿真時(shí)式(8)中的仿真參數(shù)為：kp=0.9，kd=1.2， δ0=0.02，b0=0.3，式(11)中的參數(shù)為： ξ=0.1， ω0=0.3， σ=0.2，速度設(shè)為定值2 m/s。仿真時(shí)3 個(gè)目標(biāo)點(diǎn)的經(jīng)緯度為：A（119.460 934 101 7°,32.198 867 838 3°） ，B（ 119.461 125 181 7°，32.198 695 635 0°） ，C（ 119.460 876 234 6°,32.198 674 479 7°）。以A點(diǎn)為起點(diǎn)，以正北方向?yàn)槌跏己较?，以A點(diǎn)到B點(diǎn)再到C點(diǎn)的方向進(jìn)行航跡仿真，無(wú)人艇航跡仿真圖如圖7 所示。

由圖7 可知，無(wú)人艇能夠沿著設(shè)定的航跡行駛且轉(zhuǎn)彎處過渡自然。轉(zhuǎn)彎角度為鈍角更能證明算法的快速適應(yīng)性，從圖中可看出，無(wú)人艇在到達(dá)目標(biāo)點(diǎn)附近時(shí)進(jìn)行大幅度轉(zhuǎn)彎，此時(shí)航跡偏差與航跡偏差變化率較大，模糊控制器便會(huì)根據(jù)模糊規(guī)則輸出合適的船長(zhǎng)比，得出可更快到達(dá)期望航線的目標(biāo)航向值，使得實(shí)際航線快速貼近目標(biāo)航線，且無(wú)較大超調(diào)。

圖6 Simulink 航跡仿真框圖Fig. 6 System diagram of track control using Simulink

圖7 航跡仿真圖Fig. 7 Diagram of track

圖8 為無(wú)人艇航向與舵角仿真圖。無(wú)人艇以正北方向?yàn)槌跏己较?，? s 內(nèi)達(dá)到目標(biāo)航向，并在有環(huán)境干擾的情況下保持航向穩(wěn)定且航向曲線平滑，轉(zhuǎn)彎后能夠快速達(dá)到目標(biāo)航向且基本無(wú)超調(diào)現(xiàn)象。無(wú)人艇實(shí)際航向?yàn)?°～360°，在90 s 的時(shí)刻無(wú)人艇目標(biāo)航向由264°變?yōu)?6°時(shí)，無(wú)人艇先從264°增加到360°之后再?gòu)?°增加到16°，滿足無(wú)人艇實(shí)際航行的要求。圖8 中舵角值0°表示初始時(shí)的中間位置，大于0°表示向右打舵，小于0°表示向左打舵。無(wú)人艇在切換目標(biāo)點(diǎn)時(shí)，會(huì)先打出比較大的舵角，當(dāng)達(dá)到目標(biāo)航向時(shí)，無(wú)人艇舵角逐漸趨于0°，這也證明了本文自抗擾航向控制器的有效性。

圖8 無(wú)人艇航向與舵角仿真結(jié)果Fig. 8 Simulation result of headings and rudder angle of USV

5 試驗(yàn)結(jié)果及分析

試驗(yàn)無(wú)人艇為雙槳雙舵，艇長(zhǎng)1.4 m，艇寬0.35 m，外接高精度差分GPS 設(shè)備的定位精度可達(dá)10 cm，組合慣性導(dǎo)航經(jīng)濾波融合算法后輸出航向角度誤差達(dá)0.5°。采用2 組定制鋰電池，每組電池容量為13 600 mA 時(shí)，最大負(fù)載電流80 A。試驗(yàn)地點(diǎn)為山東日照萬(wàn)平口瀉湖，試驗(yàn)天氣條件為：氣溫18 ℃，東北風(fēng)4 級(jí)，路徑全程5 km，要求通過4 個(gè)橋洞，最窄橋洞寬度為3 m 左右。試驗(yàn)無(wú)人艇如圖9 所示。在自主航行過程中實(shí)時(shí)上傳無(wú)人艇內(nèi)數(shù)據(jù)、視頻等信息，并在用戶界面進(jìn)行顯示，如圖10 所示。

圖9 試驗(yàn)無(wú)人艇Fig. 9 Tested USV

圖10 用戶界面Fig. 10 User interface

無(wú)人艇航行過程中GPS 位置數(shù)據(jù)都有記錄，以經(jīng)度為橫坐標(biāo)，緯度為縱坐標(biāo)，將無(wú)人艇實(shí)際航行5 km 的目標(biāo)航線和實(shí)際航線進(jìn)行對(duì)比，試驗(yàn)中目標(biāo)航路點(diǎn)有92 個(gè)，實(shí)際GPS 點(diǎn)有5 000 個(gè)。途中穿過4 個(gè)橋洞，如圖11 所示。

圖11 航跡跟蹤結(jié)果Fig. 11 Track tracing results

由圖11 可知，無(wú)人艇能夠根據(jù)設(shè)定的目標(biāo)航線完成自主航行任務(wù)，并在無(wú)GPS 信號(hào)的情況下穿過橋洞。為驗(yàn)證航跡保持精度，對(duì)5 km 的航跡點(diǎn)的航跡偏差進(jìn)行計(jì)算，結(jié)果如圖12 所示，航跡誤差均值約為0.1 m，方差約為0.03，最大航跡偏差在1.5 m 左右，是由于此時(shí)正在切換航跡點(diǎn)，但無(wú)人艇能根據(jù)基于模糊控制可變船長(zhǎng)比的LOS算法和自抗擾控制器較快地將航跡偏差降到1 m以內(nèi)。文獻(xiàn)[13]“海鱘03 號(hào)”無(wú)人艇在青島至連云港之間航行時(shí)最大誤差在20 m 左右。由此可見，本文給出的基于自主可控平臺(tái)的水面無(wú)人艇航跡控制算法控制精度高，可根據(jù)環(huán)境情況實(shí)時(shí)調(diào)整，能夠滿足無(wú)人艇實(shí)際航行的要求。

圖12 無(wú)人艇航跡誤差Fig. 12 Track error of USV

6 結(jié) 論

考慮無(wú)人艇航行時(shí)受環(huán)境的干擾，針對(duì)真實(shí)環(huán)境下無(wú)人艇航跡控制問題，設(shè)計(jì)并實(shí)現(xiàn)了基于模糊控制可變船長(zhǎng)比的LOS 算法與ADRC 相結(jié)合的航跡控制方法，進(jìn)行三角形航跡仿真實(shí)驗(yàn)。仿真結(jié)果表明，本文算法能夠?qū)崿F(xiàn)期望路徑的航跡控制，經(jīng)過大幅度轉(zhuǎn)彎過后航跡能夠很快恢復(fù)到期望航跡，航向穩(wěn)定且無(wú)較大超調(diào)，驗(yàn)證了本文算法的穩(wěn)定性和可靠性。

基于自主可控平臺(tái)實(shí)現(xiàn)了本文給出的控制策略和算法，利用雙槳雙舵無(wú)人艇進(jìn)行實(shí)航驗(yàn)證。試驗(yàn)結(jié)果表明，本文策略和方法能夠很好地完成真實(shí)環(huán)境下無(wú)人艇的航跡控制，試驗(yàn)航跡誤差均值約為0.1 m，方差約為0.03。