基于改進的視線導(dǎo)引算法與自抗擾航向控制器的無人艇航跡控制

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

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

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

0 引 言

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

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

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

1 無人艇航跡控制方法

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

1.1 GPS 信號有效時的航跡控制

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

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

1.2 無GPS 信號時的航跡控制

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

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

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

2 無人艇航跡控制算法

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

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

2.1 模糊控制器

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

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

表1 船長比模糊控制表Table 1 Fuzzy control table of ship length ratio

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

2.1.2 解模糊

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

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

2.2 自抗擾航向控制器

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

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

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

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

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

4 無人艇航跡控制仿真

4.1 仿真環(huán)境

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

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

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

4.2 實驗仿真

仿真航跡為三角形，仿真時式(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。仿真時3 個目標點的經(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點為起點，以正北方向為初始航向，以A點到B點再到C點的方向進行航跡仿真，無人艇航跡仿真圖如圖7 所示。

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

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

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

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

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

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

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

圖9 試驗無人艇Fig. 9 Tested USV

圖10 用戶界面Fig. 10 User interface

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

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

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

圖12 無人艇航跡誤差Fig. 12 Track error of USV

6 結(jié) 論

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

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