水面無人艇環(huán)形軌跡跟蹤方法研究與實現(xiàn)

宋吉廣，李德隆，林 揚，劉肖宇，谷海濤

(1. 中國科學(xué)院沈陽自動化研究所 機(jī)器人學(xué)國家重點實驗室，遼寧 沈陽 110016；2. 中國科學(xué)院 機(jī)器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110169)

0 引 言

水面/水下自主航行器，作為搭載探測儀器與工程設(shè)備的自主航行載體，可有效地輔助人類進(jìn)行海洋數(shù)據(jù)采集、資源探測和海洋工程實施，對海洋資源探索技術(shù)的發(fā)展具有重大意義[1-3]。良好的航行控制能力是實現(xiàn)不同需求任務(wù)的根本，而對典型航跡的跟蹤精度是航行控制效果的良好體現(xiàn)。而目前在USV軌跡跟蹤控制的研究中，主要存在以下難點：1）USV系統(tǒng)水動力系數(shù)隨USV航行速度的變化而改變，因此該系統(tǒng)具有不確定性、時變性和非線性等特點；2）USV具有明顯的欠驅(qū)動特性，因此很多非線性控制算法不能直接進(jìn)行應(yīng)用；3）目前所研究的軌跡跟蹤方法多數(shù)為基于仿真或無人艇低速狀態(tài)下的試驗，卻無法確保在復(fù)雜環(huán)境、無人艇高速狀態(tài)下對軌跡的精確跟蹤[4]。

無人艇對典型軌跡直線和環(huán)形的跟蹤能力，是其執(zhí)行任務(wù)應(yīng)當(dāng)具備的基本能力。環(huán)形軌跡對無人艇的航行控制方法具有較強(qiáng)的能力要求，本文針對環(huán)形軌跡跟蹤方法進(jìn)行研究及實現(xiàn)。由于系統(tǒng)模型的不確定性，選用基于擬合偏差的最小二乘航向辨識法建立模型集，實時計算選擇最優(yōu)模型參與控制計算，并以此搭建仿真平臺對航向控制、控制策略及制導(dǎo)律等進(jìn)行設(shè)計仿真。本文方法以USV航向控制為核心，結(jié)合控制策略的強(qiáng)抗干擾性，以“勇士”號無人水面艇為驗證載體，對算法有效性和適用性進(jìn)行實航驗證。

1 基于擬合偏差的最小二乘航向辨識法

“勇士”號USV航向和速度控制執(zhí)行機(jī)構(gòu)分別為尾部噴嘴和噴泵，通過尾部噴嘴的左右調(diào)節(jié)改變航向，噴泵向后噴水產(chǎn)生反作用力控制縱向速度。

USV系統(tǒng)航向控制模型采用一般的船舶首搖響應(yīng)線性方程[5]：

其中：T1，T2，T3為應(yīng)舵指數(shù)參數(shù)；K為回轉(zhuǎn)性能參數(shù)；r為航向角速度；δ為噴嘴角度。

1957年野本謙使用自動調(diào)節(jié)原理方法分析式（1），在執(zhí)行操作非頻繁情況下，略去2階以上的小量，并設(shè)T1+T2－KT3=T，基于此簡化可近似由下式代替[6-7]：

式中：K為回轉(zhuǎn)性能指數(shù)，T為應(yīng)舵指數(shù)，r為航向角速度，δ為噴嘴角度。由式（2）得：

式中，a和b為待估計參數(shù)，由此建立USV航向控制模型，設(shè)航向角為ψ，如下式：

基于S形開環(huán)試驗法，獲取辨識所需數(shù)據(jù)，根據(jù)最小二乘法辨識參數(shù)[8]，構(gòu)建USV SIMO（sigle input multiple output）模型。設(shè)辨識模型為：

式中：U(k) ，y1(k+1)，y2(k) 分 別為第k次輸入、輸出序列。則其最小二乘格式為：

根據(jù)極大值原理，則其取最小值時，應(yīng)滿足

輸入用于參數(shù)驗證的數(shù)據(jù)，由原值YiM和辨識輸出值 Φ獲 得 擬 合 偏 差 為：

選擇多組數(shù)據(jù)進(jìn)行辨識，所得結(jié)果在取得擬合偏差后，計算多組結(jié)果偏差均值得到閾值將大于閾值的模型剔除并構(gòu)建模型集，用于運動控制的最優(yōu)調(diào)用。

根據(jù)以上原理，通過對“勇士”號USV實航數(shù)據(jù)進(jìn)行模型辨識，得到多組模型后選擇低于閾值的模型進(jìn)行驗證，結(jié)果如圖1所示。

圖1 實際曲線與擬合曲線對比Fig. 1 Comparison between the fitting curve and practical curve

經(jīng)過多組數(shù)據(jù)驗證，算法擬合輸出與實際輸入基本吻合，且其平均擬合偏差較小，符合構(gòu)建模型條件。

2 勢點跟蹤算法原理

設(shè)定環(huán)形軌跡中心和半徑，輸入當(dāng)前USV的位置、速度、航向信息，完成勢點的劃分、首勢點選擇及環(huán)繞方向判定，實時將有序勢點進(jìn)行坐標(biāo)轉(zhuǎn)換，以獲得USV在所跟蹤勢點的運動坐標(biāo)系下坐標(biāo)，通過勢點切換判據(jù)更新所跟蹤的勢點，并實時計算期望航向角。根據(jù)USV辨識模型所設(shè)計的控制策略對航向進(jìn)行控制，實現(xiàn)對勢點的穩(wěn)定跟蹤[9-11]。算法實現(xiàn)流程如圖2所示。

圖2 勢點跟蹤算法實現(xiàn)流程Fig. 2 Implementation process of potential point tracking algorithm

根據(jù)水面無人艇動力學(xué)模型及運動學(xué)模型，將系統(tǒng)航向控制模型式（4）轉(zhuǎn)換為狀態(tài)空間表達(dá)式[12]：

首先，預(yù)設(shè)閉環(huán)系統(tǒng)矩陣極點為λ1，λ2， 且－1＜λ1,λ2＜0，根據(jù)現(xiàn)代控制理論中極點配置問題，構(gòu)建動態(tài)反饋控制律[13]：

其中，a和b為辨識所獲得參數(shù)。

2.1 勢點跟蹤策略設(shè)計

依托于水平面航向控制器[14-16]，采用勢點跟蹤算法實現(xiàn)環(huán)形軌跡跟蹤，如圖3所示。首先設(shè)定環(huán)形軌跡中心位置以及環(huán)形半徑，根據(jù)中心位置建立運動坐標(biāo)系，并預(yù)置有序勢點集合。對航行器位置進(jìn)行坐標(biāo)變換，轉(zhuǎn)至運動坐標(biāo)系下，依次跟蹤每一個勢點，進(jìn)而形成環(huán)形軌跡。

圖3 勢點跟蹤算法基本原理圖Fig. 3 Basic schematic diagram of potential point tracking algorithm

根據(jù)軌跡半徑、航行速度，計算勢點數(shù)量，并將所得到的勢點存入Paim[]中：

圖4 勢點切換判據(jù)原理圖Fig. 4 Principle diagram of potential point switching criterion

當(dāng)滿足如下切換條件，切換至下一勢點繼續(xù)跟蹤。

2.2 制導(dǎo)律設(shè)計

基于視線導(dǎo)引方法設(shè)計路徑跟隨制導(dǎo)系統(tǒng)，期望路徑由一系列點Paim[]=[Paim[1],···,Paim[n]]組成。設(shè)Paim[t]=(xnt,ynt) ，艇位置 (xt,yt) ，d為艇中心到直線Paim[t]Paim[t+1]的距離，即制導(dǎo)系統(tǒng)使該值趨于0。β為側(cè)漂角，ψ為USV航向角，ψpt為直線與北向夾角角度，Rt為收斂半徑，一般為艇長的2～3倍，Δ為超前距離，ψd為視線角[17-19]，如圖5所示。

圖5 航向控制計算原理圖Fig. 5 Course control calculation schematic digram

其中：

則可求橫向距離d、超前距離Δ 和視線角ψd，

由此可得USV航向期望角度為：

當(dāng)橫向誤差d絕對值大于Rt時，制導(dǎo)方法失效，故Rt應(yīng)實時適應(yīng)d變化，并使d快速收斂，則

通過調(diào)節(jié)參正數(shù)μ 以 保證 |Rt|＞|d|，同時，橫向誤差d增大，視線角ψd增大，d將快速收斂，橫向誤差d減小，視線角ψd減 小，d將較慢收斂，軌跡將趨于圓滑。

為減小因水流干擾產(chǎn)生的穩(wěn)態(tài)誤差，計算視線角時將橫向誤差進(jìn)行積分，其方式為：

其中，ki為積分參數(shù)，所以軌跡跟蹤制導(dǎo)律為：

該無人艇制導(dǎo)律可通過航向調(diào)節(jié)快速有效的收斂到環(huán)形軌跡上，同時，消除外界干擾導(dǎo)致的穩(wěn)態(tài)誤差，增強(qiáng)系統(tǒng)的魯棒性[20]。

3 實驗仿真及外場試驗

經(jīng)由辨識方法得到的USV數(shù)學(xué)模型，搭建仿真平臺，在此平臺下進(jìn)行多工況環(huán)形軌跡跟蹤方法驗證。設(shè)定環(huán)形軌跡半徑100 m，USV速度9 m/s，進(jìn)行環(huán)形軌跡跟蹤。仿真結(jié)果如圖6所示：

圖6 仿真平臺下的環(huán)形軌跡跟蹤Fig. 6 Circular trajectory tracking in simulation platform

由圖6（b）中間距穩(wěn)定狀態(tài)可知，USV對環(huán)形軌跡的跟蹤效果良好。經(jīng)過計算，在該仿真狀態(tài)下其實航軌跡RMS為0.8 m，具備良好的軌跡跟蹤能力。

為提高算法的實用性，以“勇士”號USV為驗證平臺，進(jìn)行典型海況下（3級海況）海上環(huán)形軌跡跟蹤多條次試驗。設(shè)定環(huán)形軌跡半徑為100，航行速度為11 m/s，USV對所設(shè)定的環(huán)形軌跡進(jìn)行跟蹤，經(jīng)對數(shù)據(jù)整合處理，其實航軌跡效果如圖7所示。

在海上環(huán)境中，由于風(fēng)浪流等環(huán)境干擾，將對環(huán)形軌跡跟蹤方法的抗干擾能力進(jìn)行檢驗。由圖7（b）可知，USV按照所設(shè)計的勢點跟蹤算法及制導(dǎo)律進(jìn)行運行，其能夠按照預(yù)定航向、航跡航行，且通過對實航數(shù)據(jù)的計算得出實航軌跡RMS為1.2 m，表明環(huán)形軌跡跟蹤方法具有較強(qiáng)的抗干擾能力，即在較強(qiáng)環(huán)境干擾、高速狀態(tài)下亦可實現(xiàn)對環(huán)形軌跡的穩(wěn)定跟蹤。

圖7 海試中環(huán)形軌跡跟蹤Fig. 7 Circular trajectory tracking in sea trial

4 結(jié) 語

本文針對USV在實航中模型參數(shù)不確定、高速狀態(tài)下對環(huán)形軌跡的準(zhǔn)確跟蹤能力方面研究薄弱問題，提出了基于勢點跟蹤算法的環(huán)形軌跡跟蹤方法。算法以“勇士”號USV作為驗證平臺，經(jīng)過模型參數(shù)辨識得到系統(tǒng)數(shù)學(xué)模型集，進(jìn)而建立USV仿真平臺，基于系統(tǒng)模型辨識構(gòu)建跟蹤策略以及制導(dǎo)律，對環(huán)形軌跡跟蹤方法進(jìn)行仿真驗證，并在典型海況下多條次環(huán)形軌跡跟蹤試驗進(jìn)行驗證。實航數(shù)據(jù)中軌跡的RMS較小，可良好的在環(huán)境干擾、高速狀態(tài)下實現(xiàn)對環(huán)形軌跡的精確跟蹤，表明在USV平臺下該方法對環(huán)形軌跡穩(wěn)定跟蹤的有效性、適用性以及魯棒性。