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

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

1 江蘇科技大學 電子信息學院，江蘇 鎮江 212003

2 江蘇科技大學 海洋裝備研究院，江蘇 鎮江 212003

0 引 言

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

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

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

1 無人艇航跡控制方法

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

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

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

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

1.2 無GPS 信號時的航跡控制

無人艇實際航行時存在過橋洞、船閘等GPS信號無效的情況，為保證航跡，本文采用慣性導航融合處理方法。如圖2 所示，以無人艇過橋洞為例，假設無人艇在O點時GPS 信號無效，此時系統需記錄最后一次GPS 信號有效時的航速v，慣性導航設備的無人艇艏向 ψX、當前無人艇位置O到目標點Pk+1的距離d。為到達Pk+1點，無人艇以目標航向 ψr=ψX、速度v、持續t=d/v時間。如果GPS 信號持續t時間后仍無效，則此時無人艇目標航向 ψr+1為慣性導航設備提供的無人艇艏向 ψ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)求得。依此類推，實現無人艇在GPS 信號無效情況下的航跡控制。

2 無人艇航跡控制算法

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

圖3 航跡控制系統框圖Fig. 3 System diagram of track control

2.1 模糊控制器

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

2.1.1 模糊化與規則庫

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

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

2.1.2 解模糊

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

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

2.2 自抗擾航向控制器

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

圖4 自抗擾控制器結構圖Fig. 4 Structure diagram of ADRC

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

3 控制系統及軟件設計

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

4 無人艇航跡控制仿真

4.1 仿真環境

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

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

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

4.2 實驗仿真

仿真航跡為三角形，仿真時式(8)中的仿真參數為：kp=0.9，kd=1.2， δ0=0.02，b0=0.3，式(11)中的參數為： ξ=0.1， ω0=0.3， σ=0.2，速度設為定值2 m/s。仿真時3 個目標點的經緯度為：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 可知，無人艇能夠沿著設定的航跡行駛且轉彎處過渡自然。轉彎角度為鈍角更能證明算法的快速適應性，從圖中可看出，無人艇在到達目標點附近時進行大幅度轉彎，此時航跡偏差與航跡偏差變化率較大，模糊控制器便會根據模糊規則輸出合適的船長比，得出可更快到達期望航線的目標航向值，使得實際航線快速貼近目標航線，且無較大超調。

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

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

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

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

5 試驗結果及分析

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

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

圖10 用戶界面Fig. 10 User interface

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

圖11 航跡跟蹤結果Fig. 11 Track tracing results

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

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

6 結 論

考慮無人艇航行時受環境的干擾，針對真實環境下無人艇航跡控制問題，設計并實現了基于模糊控制可變船長比的LOS 算法與ADRC 相結合的航跡控制方法，進行三角形航跡仿真實驗。仿真結果表明，本文算法能夠實現期望路徑的航跡控制，經過大幅度轉彎過后航跡能夠很快恢復到期望航跡，航向穩定且無較大超調，驗證了本文算法的穩定性和可靠性。

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