基于改進視線法的欠驅動無人艇路徑跟蹤

朱騁， 莊佳園， 張磊， 蘇玉民， 袁志豪

(哈爾濱工程大學 水下智能機器人技術國防科技重點實驗室，黑龍江 哈爾濱 150001)

水面無人艇(umanned surface vehicle,USV)在大量的任務，如測深制圖、環境監測和港口監視中證明它們對科學、商業和軍事都具有重要價值應用[1]。在一個典型的預先計劃的任務場景中水面無人艇被要求精確地沿著指定的路徑。如在執行港口區域巡航時，其區域范圍不大，同時具有多障礙物，因此要求每個過航點的容許誤差足夠小，水面無人艇速度能夠隨時改變且不影響跟蹤精度，以確保區域跟蹤完成度和安全避障。因此，在執行機構(舵機)響應速度確定的情況下，如何在路徑跟蹤精度和跟蹤完成度進行取舍、如何盡可能地削減水面無人艇速度對路徑精度的影響，成為當下路徑跟蹤方面研究的熱點。

目前已有許多在工業上被廣泛使用的移動機器人的路徑跟蹤控制方法，如胡蘿卜跟蹤法(CC)[3]在期望路徑上引入虛擬目標點(virtual-target-point，VTP)，通過追逐恒定距離的虛擬目標點最終到達期望目標點；向量場法(VF)[4]在期望路徑周圍建立矢量場，每個矢量均為引導機器人走向期望路徑的最優方向；視線法(line-of-sight，LOS)[5]等。對于水面無人艇來說，目前的研究熱點為視線法。雖然視線法已經在不同的水面無人艇上得到了不同程度的成功，但事實證明，大多數評估實驗只在環境影響最小的理想環境中進行。在惡劣的天氣條件下，研究人員依舊很難確定實際的視線法控制器的性能。董早鵬等[6]采用反步法提出了基于非對稱模型的視線法改進。付悅文[7]在此基礎上提出了基于無模型自適應視線法。陳霄等[8]提出了2種改進積分視線導引策略，并利用級聯系統理論證明了當所有控制目標實現時，控制系統為全局k-指數穩定的。

視線法是基于一個沿著期望路徑移動的虛擬目標，最初起源于導彈的跟蹤。通過對運動目標的追逐，導彈最終會匯聚到路徑上。作為一種基本方法，已被水面無人艇研究人員廣泛應用于由直線和圓形路徑組成的軌跡跟蹤。本文針對水面無人艇縱向速度與橫側偏差的耦合及不同跟蹤范圍下平滑期望艏向曲線的問題提出基于自適應雙曲正切視線法導引律，通過仿真試驗比較基于自適應雙曲正切視線法與傳統視線法，分析基于自適應雙曲正切視線法的優點。最后通過外場試驗驗證該算法在實際應用中的控制性能。

1 視線法制導律

基于視線法設計路徑跟隨的制導系統，原理如圖1所示。

視線法制導律的目標在于減少橫側偏差δ，同時使得船體航向能夠沿路徑方向前進，即：

由圖1得：

δ(t)=-Δxsinφpk+Δycosφpk(3)

式中：Δx=xt-xk；Δy=yt-yk。

欠驅動水面無人艇三自由運動學和動力學模型為：

由式(5)和圖1所示幾何關系得：

由于期望路徑是由多點組成，引入安全半徑Rsafe判斷水面無人艇是否進行下一個目標點的跟隨，安全半徑的距離一般設為船長的2～3倍。

2 基于自適應雙曲正切視線法導引策略

視線法需要的參數較少，且為幾何算法，能夠高效地計算期望航向，同時期望航向的獲取只與船舶的實時位置和給定的期望航線有關，因此被廣泛的應用在水面無人艇的路徑跟隨中。但傳統視線法忽略了航線范圍和水面無人艇縱向速度u與橫側偏差δ的耦合關系，工程應用中水面無人艇在跟蹤范圍改變及變速情況下需要重新設置參數。為解決上述問題，本文基于包圍圈視線法導引策略采用自適應觀測技術以及針對包圍圈引入tanh函數提出基于自適應雙曲正切視線法導引策略。

2.1 基于雙曲正切的視線法制導律

期望路徑與水面無人艇視線圓(r=R)的交點為虛擬目標點T，視線角φlos為當前虛擬目標點與水面無人艇的連線與期望路徑之間夾角：

得期望艏向角為：

φd=φpk-φlos-β(9)

展開式(4)，得：

φd=atan2(yk+1-yk,xk+1-xk)-

式中：atan2為四象限反正切函數

從式(10)及圖3得，當δ≥R，上述傳統視線法失效。為避免此情況發生，傳統做法如下：

式中：k為正的控制參數，取k>1。

文獻[11]提出一種動態調節視線圓半徑R的方法：

在水面無人艇執行變速任務時，若采用式(12)改進制導律，雖然能夠保存制導律始終不出現奇異值，但由于水面無人艇時常變速導致控制精度不高，加之該改進的期望艏向曲線過渡段較短(見圖3)，因此往往容易造成跟蹤時的艏向震蕩。

針對上述問題，引入tanh函數，tanh函數擁有平滑的曲線，穩定性較好，并且在能夠抗飽和，相較sigmoid函數,tanh函數能夠負輸出，有：

式中：k、D為正向控制參數。

基于自適應雙曲正切視線法參數k、D對期望艏向φd的影響如圖2。

圖2 ATLOS控制參數影響因子Fig.2 The exploration of the parameter space for ATLOS

3種不同視線法制導律的期望艏向φd曲線如圖3所示。

圖3 不同LOS制導律對比Fig.3 The compare of the different guidance methods

ALOS制導律即式(12)所給出的改進。分析圖2、3可知：

1)參數k影響制導律形態，當k≤1，制導律類似傳統視線法；當k≥1，制導律類似自適應視線法，但在平滑度上勝過自適應視線法。

2)參數k與曲線的平緩度成正比，曲線隨k的增大趨向平緩，反之亦然。

3)參數D影響曲線的加速收斂的范圍，加速收斂范圍隨D的增加而增大，反之亦然。

2.2 基于自適應雙曲正切視線法導引策略

本文的水面無人艇仿真系統的航向控制模型為：

其中：

假設系統受到的隨機干擾滿足以下2個條件：

1)USV所受隨機干擾的輸入有界；

2)隨機干擾對USV水動力模型參數的影響有界。

則將式(10)化為狀態方程，設狀態向量x=[r,φ]T：

式中：n1、n2為外界產生的干擾；d1、d2為固定值。

將模型(11)中的控制律δr(t)分解為PID控制器δr1(t)和在線補償器δr2(t)進行控制，得：

引入虛擬控制量：

其中：ξ=xd-x，xd為期望運動狀態向量；x為實際輸出運動狀態向量。

為得到穩定的自適應控制參數k(ξ)，構建Lyapunov函數：

V=xTPx+ξT(x)PBBTξ(x) (21)

代入式(17)、(18)得：

基于自適應雙曲正切視線法導引策略框圖如圖4所示。

圖4 ATLOS導引策略框圖Fig.4 ATLOS guidance strategy diagram

3 結果分析

3.1 仿真實驗

本文結合C++和vega開發水面無人艇平面運動仿真平臺。在這個干擾為慢運動學的仿真平臺上，對比基于自適應雙曲正切視線法和視線法，從仿真上分析兩者差異。

速度控制為增量式PID控制器，參數選擇：Kp=10.0，KI=0.0，KD=3.0，k=1.0

船體運動學、動力學模型建立方法見文獻[13]。具體船體模型系數有：m=1 900.5，Iz=8 000，Mxx=184.57，Myy=15.338 4，Mrr=1 206，Yv=-0.475 1，Yr=0.071 6，Nv=-0.145 9，Nr=-0.055 7，Xvr=-0.002 5，Xvv=0.068 2，Xrr=0.003 9，Yvr=-0.29，Yvv=-0.4757，Yrr=-0.035，Nvv=0.014 8，Nrr=-0.04；其他未提及系數均為0。

ATLOS參數選擇：D=30，k=0.5，Rsafe=40.0，KP=3.0，KI=0.0，KD=4.0.

工況1：USV初始位置(x0,y0)=(0,0)，初始航向φ=0/π，初始速度u=0.0，安全半徑Rsafe=40.0。無外界干擾d1=0，d2=0。期望路徑與期望速度為表1所示。

表1 工況1參數Table 1 Parameter of condition 1

工況1仿真結果如圖5所示。在沒有隨機干擾的情況下，不論是視線法，還是基于自適應雙曲正切視線法，導引策略都能夠快速收斂。同時，在速度發生改變后的路徑上基于自適應雙曲正切視線法依舊保持良好的控制精度，視線法速度曲線震蕩時間延長，且艏向跟蹤精度變差。

圖5 工況1仿真結果Fig.5 Condition 1 simulation performance

工況2：水面無人艇初始位置(x0,y0)=(19.3,7)，初始航向φ=36.9/π，初始速度u=3.5，安全半徑Rsafe=40.0。無外界干擾d1=0，d2=0。期望路徑與期望速度如表2所示。

表2 工況2參數Table 2 Parameter of condition 2

工況2仿真結果如圖6所示。在施加固定的外界干擾之后，視線法在速度為u=3.5的路段中，實際路徑的超調及震蕩明顯增加，速度曲線沒有收斂。基于自適應雙曲正切視線法幾乎不受影響。

圖6 工況2仿真結果Fig.6 Condition 2 simulation performance

對比圖5、圖6可知視線法的參數不具備自適應能力，即速度范圍改變之后參數性能變差。但基于自適應的雙曲正切視線法具備較好的自適應能力，同時基于自適應的雙曲正切視線法導引的期望艏向變化幅度更加合理。基于自適應的雙曲正切視線法的自適應參數考慮到了水面無人艇縱向速度與橫側偏差和模型的耦合，有效的修正了期望艏向曲線的變化幅度，同時增加了速度跟蹤精度。其中圖(6)艏向跟蹤最后出現的躍變是由于期望艏向從180°～-180°的突變。

圖7所示為2工況的橫側偏差曲線，具體數值統計如下表3所示。

圖7 工況1、2的橫側偏差Fig.7 Cross-track error of condition 1, 2

表3 橫側偏差數據統計Table 4 Cross-track error statistics

分析圖7及表3可知，視線法和基于自適應的雙曲正切視線法產生的超調均在可容許范圍內，在給定一個恒定速度變化范圍后，兩者均能保持良好的控制精度。但在路徑曲率和速度變化范圍均發生改變的情況下，視線法控制系統震蕩較大，收斂緩慢，基于自適應的雙曲正切視線法系統基本不受影響。

2種策略比較總結如下：

1)2種策略均能引導USV在期望路徑航行；

2)在變速情況下，基于自適應的雙曲正切視線法導引的期望艏向更加合理；

3)基于自適應的雙曲正切視線法具備自適應能力，相對于視線法具有更好的魯棒性。

3.2 外場試驗

實體船與航行圖如圖8所示。

圖8 RobotX外場試驗圖Fig.8 RobotX running in lake

工況1：水流平均流速在v=0.4 m/s，東北風4級。恒定縱向速度u=0.8 m/s，初始艏向φ=-74.4°。Rsafe=5。從經緯度(45.794 63°N，126.529 55°E)出發跟蹤一條直線P1(45.794 44,126.527 34)，P2=(45.794 21,126.526 75)。

工況2：水流平均流速在v=0.1 m/s。恒定縱向速度u=1.2 m/s，初始艏向φ=13.6°。Rsafe=10。從經緯度Pt0(45.794 83°N,126.528 80°E)出發跟蹤閉合曲線P1(45.795 10,126.529 23),P2=(45.795 23,126.531 21),P3=(45.796 68,126.531 89),P4=(45.796 60,126.531 13),P5=(45.795 10,126.529 23)。

工況1試驗結果如圖9所示。在期望速度恒定ud=0.8 m/s，水流速度平均在v=0.4 m/s，加上隨機的環境干擾導致橫漂角的影響較大。圖中艏向變化曲線只截取了USV在進入期望路徑之后航行時的數據。從圖9(a)可以看出ATLOS在隨機環境干擾，橫漂角影響較大的情況下，USV依舊能夠很好的沿著期望路徑航行。當USV在期望路徑航行時，偏差在1.5 m之內，平均偏差為0.7 m。

圖9 外場試驗結果工況1Fig.9 Condition 1 the results of field experiment

工況2在600步左右，基于自適應的雙曲正切視線法策略正在改變期望路徑點，期望艏向出現62.2°的躍變，從而導致艏向跟蹤緩慢引起，產生最大偏差7.9 m。當USV在期望路徑航行時產生的平均偏差為1.0 m。如圖10所示。

圖10 外場試驗結果工況2Fig.10 Condition 2 the results of field experiment

工況3，試驗海況為三級海況，初始縱向速度u=0 m/s，初始艏向φ=-139.6°，期望縱向速度ud=12.5 m/s。Rsafe=100。從經緯度(38.791 677°N，121.123 300°E)出發跟蹤兩段直線P1(39.791 677,121.123 300)，P2=(38.791 677,121.123 300)，P3=(38.794 953,121.116 379)。

可以從圖11看出，工況3在起步加速階段由于干擾作用，橫側偏差產生了振蕩，但是在50步左右之后，收斂趨向穩定。同時從艏向圖可以進一步發現，即使艏向發生了振蕩，橫側偏差仍保持平滑的收斂狀態。本次試驗產生最大偏差5.2 m，平均偏差在3.6 m。

圖11 外場試驗結果工況3Fig.11 Condition 3 the results of field experiment

3次試驗與仿真模擬結果較吻合。可以看出ATLOS具有較好的穩定性和魯棒性。通過該導引策略，USV在隨機干擾下能夠保持良好的控制精度。

4 結論

1)兩者的收斂特性發現通過調整參數基于自適應雙曲正切視線法和視線法均能在恒定速度和曲率變化范圍內很好的收斂。

2)在速度和曲率的范圍會發生變化的情況下，基于自適應雙曲正切視線法依舊能夠保持良好的控制精度，視線法精度受損較大，其中速度震蕩時間，橫側偏差超調明顯變大。

3)實驗結果與模擬結果吻合較好，驗證了ATLOS導引策略的穩定性和魯棒性。

4)在高速航行下仍具備較好的跟蹤精度，說明ATLOS能夠運用于較高速的航行情況。

下一步將研究不同路徑跟蹤范圍下，避免期望艏向躍變產生較大橫側誤差的方法以及在高速航行下的路徑跟隨問題。