宋吉廣，李德隆，林 揚(yáng)，劉肖宇，谷海濤

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

0 引 言

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

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

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

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

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

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

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

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

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

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

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

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

圖1 實(shí)際曲線與擬合曲線對(duì)比Fig. 1 Comparison between the fitting curve and practical curve

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

2 勢(shì)點(diǎn)跟蹤算法原理

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

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

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

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

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

2.1 勢(shì)點(diǎn)跟蹤策略設(shè)計(jì)

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

圖3 勢(shì)點(diǎn)跟蹤算法基本原理圖Fig. 3 Basic schematic diagram of potential point tracking algorithm

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

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

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

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

基于視線導(dǎo)引方法設(shè)計(jì)路徑跟隨制導(dǎo)系統(tǒng)，期望路徑由一系列點(diǎn)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為收斂半徑，一般為艇長(zhǎng)的2～3倍，Δ為超前距離，ψd為視線角[17-19]，如圖5所示。

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

其中：

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

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

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

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

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

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

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

3 實(shí)驗(yàn)仿真及外場(chǎng)試驗(yàn)

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

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

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

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

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

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

4 結(jié) 語

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