基于改進(jìn)視線法的欠驅(qū)動(dòng)無人艇路徑跟蹤

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

(哈爾濱工程大學(xué) 水下智能機(jī)器人技術(shù)國防科技重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001)

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

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

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

1 視線法制導(dǎo)律

基于視線法設(shè)計(jì)路徑跟隨的制導(dǎo)系統(tǒng)，原理如圖1所示。

視線法制導(dǎo)律的目標(biāo)在于減少橫側(cè)偏差δ，同時(shí)使得船體航向能夠沿路徑方向前進(jìn)，即：

由圖1得：

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

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

欠驅(qū)動(dòng)水面無人艇三自由運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型為：

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

由于期望路徑是由多點(diǎn)組成，引入安全半徑Rsafe判斷水面無人艇是否進(jìn)行下一個(gè)目標(biāo)點(diǎn)的跟隨，安全半徑的距離一般設(shè)為船長的2～3倍。

2 基于自適應(yīng)雙曲正切視線法導(dǎo)引策略

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

2.1 基于雙曲正切的視線法制導(dǎo)律

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

得期望艏向角為：

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

展開式(4)，得：

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

式中：atan2為四象限反正切函數(shù)

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

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

文獻(xiàn)[11]提出一種動(dòng)態(tài)調(diào)節(jié)視線圓半徑R的方法：

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

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

式中：k、D為正向控制參數(shù)。

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

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

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

圖3 不同LOS制導(dǎo)律對比Fig.3 The compare of the different guidance methods

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

1)參數(shù)k影響制導(dǎo)律形態(tài)，當(dāng)k≤1，制導(dǎo)律類似傳統(tǒng)視線法；當(dāng)k≥1，制導(dǎo)律類似自適應(yīng)視線法，但在平滑度上勝過自適應(yīng)視線法。

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

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

2.2 基于自適應(yīng)雙曲正切視線法導(dǎo)引策略

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

其中：

假設(shè)系統(tǒng)受到的隨機(jī)干擾滿足以下2個(gè)條件：

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

2)隨機(jī)干擾對USV水動(dòng)力模型參數(shù)的影響有界。

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

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

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

引入虛擬控制量：

其中：ξ=xd-x，xd為期望運(yùn)動(dòng)狀態(tài)向量；x為實(shí)際輸出運(yùn)動(dòng)狀態(tài)向量。

為得到穩(wěn)定的自適應(yīng)控制參數(shù)k(ξ)，構(gòu)建Lyapunov函數(shù)：

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

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

基于自適應(yīng)雙曲正切視線法導(dǎo)引策略框圖如圖4所示。

圖4 ATLOS導(dǎo)引策略框圖Fig.4 ATLOS guidance strategy diagram

3 結(jié)果分析

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

本文結(jié)合C++和vega開發(fā)水面無人艇平面運(yùn)動(dòng)仿真平臺(tái)。在這個(gè)干擾為慢運(yùn)動(dòng)學(xué)的仿真平臺(tái)上，對比基于自適應(yīng)雙曲正切視線法和視線法，從仿真上分析兩者差異。

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

船體運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)模型建立方法見文獻(xiàn)[13]。具體船體模型系數(shù)有：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；其他未提及系數(shù)均為0。

ATLOS參數(shù)選擇：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參數(shù)Table 1 Parameter of condition 1

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

圖5 工況1仿真結(jié)果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參數(shù)Table 2 Parameter of condition 2

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

圖6 工況2仿真結(jié)果Fig.6 Condition 2 simulation performance

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

圖7所示為2工況的橫側(cè)偏差曲線，具體數(shù)值統(tǒng)計(jì)如下表3所示。

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

表3 橫側(cè)偏差數(shù)據(jù)統(tǒng)計(jì)Table 4 Cross-track error statistics

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

2種策略比較總結(jié)如下：

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

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

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

3.2 外場試驗(yàn)

實(shí)體船與航行圖如圖8所示。

圖8 RobotX外場試驗(yàn)圖Fig.8 RobotX running in lake

工況1：水流平均流速在v=0.4 m/s，東北風(fēng)4級(jí)。恒定縱向速度u=0.8 m/s，初始艏向φ=-74.4°。Rsafe=5。從經(jīng)緯度(45.794 63°N，126.529 55°E)出發(fā)跟蹤一條直線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。從經(jīng)緯度Pt0(45.794 83°N,126.528 80°E)出發(fā)跟蹤閉合曲線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試驗(yàn)結(jié)果如圖9所示。在期望速度恒定ud=0.8 m/s，水流速度平均在v=0.4 m/s，加上隨機(jī)的環(huán)境干擾導(dǎo)致橫漂角的影響較大。圖中艏向變化曲線只截取了USV在進(jìn)入期望路徑之后航行時(shí)的數(shù)據(jù)。從圖9(a)可以看出ATLOS在隨機(jī)環(huán)境干擾，橫漂角影響較大的情況下，USV依舊能夠很好的沿著期望路徑航行。當(dāng)USV在期望路徑航行時(shí)，偏差在1.5 m之內(nèi)，平均偏差為0.7 m。

圖9 外場試驗(yàn)結(jié)果工況1Fig.9 Condition 1 the results of field experiment

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

圖10 外場試驗(yàn)結(jié)果工況2Fig.10 Condition 2 the results of field experiment

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

可以從圖11看出，工況3在起步加速階段由于干擾作用，橫側(cè)偏差產(chǎn)生了振蕩，但是在50步左右之后，收斂趨向穩(wěn)定。同時(shí)從艏向圖可以進(jìn)一步發(fā)現(xiàn)，即使艏向發(fā)生了振蕩，橫側(cè)偏差仍保持平滑的收斂狀態(tài)。本次試驗(yàn)產(chǎn)生最大偏差5.2 m，平均偏差在3.6 m。

圖11 外場試驗(yàn)結(jié)果工況3Fig.11 Condition 3 the results of field experiment

3次試驗(yàn)與仿真模擬結(jié)果較吻合?？梢钥闯鯝TLOS具有較好的穩(wěn)定性和魯棒性。通過該導(dǎo)引策略，USV在隨機(jī)干擾下能夠保持良好的控制精度。

4 結(jié)論

1)兩者的收斂特性發(fā)現(xiàn)通過調(diào)整參數(shù)基于自適應(yīng)雙曲正切視線法和視線法均能在恒定速度和曲率變化范圍內(nèi)很好的收斂。

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

3)實(shí)驗(yàn)結(jié)果與模擬結(jié)果吻合較好，驗(yàn)證了ATLOS導(dǎo)引策略的穩(wěn)定性和魯棒性。

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

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