文元橋，陶威，周杰，周陽，楊吉，肖長詩

(1.武漢理工大學(xué) 智能交通系統(tǒng)研究中心，湖北 武漢 430063；2.國家水運(yùn)安全工程技術(shù)研究中心，湖北 武漢 430063;3.內(nèi)河航運(yùn)技術(shù)湖北省重點(diǎn)實(shí)驗(yàn)室，湖北 武漢 430063;4.武漢理工大學(xué) 能源與動力工程學(xué)院，湖北 武漢 430063;5.武漢理工大學(xué) 航運(yùn)學(xué)院，湖北 武漢 430063)

近年來無人水面艇(unmanned surface vehicles，USV)作為一種無人水上運(yùn)載平臺在海洋學(xué)科領(lǐng)域得到了廣泛的研究與應(yīng)用，如海洋監(jiān)測[1]、海洋測深[2]、通信中繼[3]、橋梁測繪[4]、水質(zhì)監(jiān)測[5]和水上搜救[6]等。當(dāng)USV在執(zhí)行上述任務(wù)時，需要有精確的、可靠的和適應(yīng)性強(qiáng)的模型、制導(dǎo)方法和控制系統(tǒng)作為支撐[7]。在建模過程中，由于USV的運(yùn)動受到風(fēng)和水流以及流體阻力的影響，導(dǎo)致使用傳統(tǒng)方法建立USV精確動力學(xué)模型時難度較大，并且所建模型在不同的水域和工作環(huán)境中缺乏普適性[8]。因此，采用特征建模方法建立更精準(zhǔn)的USV模型。特征建模根據(jù)被控對象動力學(xué)特征、環(huán)境特征與控制目標(biāo)和性能要求相結(jié)合,克服只考慮動力學(xué)分析建模的不足,建立以利于控制器設(shè)計(jì)的模型[9]。在艏向和速度控制方法中，PID算法最為經(jīng)典并較早地應(yīng)用于實(shí)際，但傳統(tǒng)PID算法不能很好的適用于復(fù)雜多變的環(huán)境[10]；滑模算法[11]雖然有較快的響應(yīng)速度，但其本身對系統(tǒng)模型的精確性有較高的要求；模糊控制算法[12]針對一個控制對象需要眾多的模糊系統(tǒng)參數(shù)與之對應(yīng)，整定參數(shù)的過程繁瑣，容易出現(xiàn)頻繁的超調(diào)。針對上述不足，結(jié)合所建立的特征模型，設(shè)計(jì)一種調(diào)節(jié)簡單、魯棒性強(qiáng)的基于全系數(shù)自適應(yīng)控制方法[13]的艏向和速度協(xié)同控制器。在路徑跟蹤控制中的制導(dǎo)律主要是常規(guī)的瞄準(zhǔn)線(line-of-sight,LOS)制導(dǎo)算法、針對外界環(huán)境干擾的積分瞄準(zhǔn)線(integral line-of-sight,ILOS)制導(dǎo)算法以及L1制導(dǎo)算法[14-17]。但是多數(shù)制導(dǎo)算法只適用于對直線參考路徑的跟蹤。而實(shí)際應(yīng)用中的USV因航行和作業(yè)任務(wù)的需要，應(yīng)能同時完成對直線路徑和曲線路徑路徑跟蹤的任務(wù)。

本文以武漢理工大學(xué)iNav-IV型USV為研究對象針對USV在實(shí)際航行中的路徑跟蹤控制問題進(jìn)行研究，利用雙推進(jìn)器USV的特征模型，設(shè)計(jì)一種基于全系數(shù)自適應(yīng)控制算法的自適應(yīng)艏向和速度協(xié)同控制器，并由矢量場(vectro field,VF)制導(dǎo)算法[18]構(gòu)成路徑跟蹤控制器外環(huán)制導(dǎo)環(huán)節(jié)。將所設(shè)計(jì)的自適應(yīng)路徑跟蹤控制器部署到iNav-IV型USV上，并在武漢理工大學(xué)實(shí)驗(yàn)水池進(jìn)行實(shí)船實(shí)驗(yàn)。

1 無人水面艇特征模型和參數(shù)估計(jì)

1.1 USV的特征模型

USV在風(fēng)浪流及外界干擾下，其運(yùn)動數(shù)學(xué)模型是一個非線性高階方程，考慮對于整體型模型結(jié)構(gòu)下的船舶運(yùn)動數(shù)學(xué)模型，常見的處理方法是從整體的角度把作用在船體上的流體動力對每個運(yùn)動變量按泰勒級數(shù)展開，進(jìn)行線性化處理，忽略高階小量，簡化至2階或3階所得的降階模型。無論是簡化后的2階模型還是3階模型，都會使原高階模型里包含的信息有所丟失，而特征模型與高階系統(tǒng)的降階模型不同，它是把高階模型有關(guān)信息都壓縮到幾個特征參量之中，并不丟失信息[19-21]。雙推進(jìn)器型USV從整體模型結(jié)構(gòu)建模時進(jìn)行線性化處理后可以看作一個多輸入多輸出線性時變系統(tǒng)，考慮對象動力學(xué)特征、環(huán)境特征和控制性能要求，本文采用特征模型[9]來代替原有的船舶運(yùn)動數(shù)學(xué)模型進(jìn)行控制器設(shè)計(jì)。

定理1對于多輸入多輸出線性時變系統(tǒng)如式，當(dāng)要實(shí)現(xiàn)位置保持或位置跟蹤控制時，在采樣周期和狀態(tài)參數(shù)滿足一定條件下，其特征模型可用輸出解耦型二階差分方程組描述：

X(k+1)=F1×X(k)+F2×X(k-1)+

G0×u(k)+G1×u(k-1)

(1)

其中：

特征模型與對象動力學(xué)模型不同，特征模型主要體現(xiàn)的是控制量與要求輸出量之間的特征關(guān)系，是由特征變量與參量組成的模型。

1.2 USV特征模型的建立

圖1為iNav-IV型USV平面運(yùn)動示意圖，ψ為USV艏向角，u和v分別為在USV附體坐標(biāo)系下船體bx軸和by軸上的速度,f1和f2分別為USV左右電機(jī)的推力。

圖1 iNav-IV型USV平面運(yùn)動示意Fig.1 Schematic depiction of iNav-IV USV

雙輸入雙輸出USV的特征模型方程表示為：

(2)

其中：

式中:F1、F2、G0和G1為可辨識特征模型參數(shù);x1(k)表示k時刻艏向角;x2(k)表示k時刻速度;u1(k)表示k時刻左推進(jìn)器電機(jī)指令;u2(k)表示k時刻右推進(jìn)器電機(jī)指令。

1.3 在線參數(shù)估計(jì)

首先，根據(jù)從USV上GPS得到輸出的經(jīng)緯度數(shù)據(jù)計(jì)算出USV在NED坐標(biāo)系下正東方向和正北方向上的線速度分量Ev和Nv[22]。然后，結(jié)合電子羅經(jīng)輸出的USV艏向角ψ，可得到在USV附體坐標(biāo)系下船體bx軸上的速度u和by軸上的速度v：

(3)

將上面得到的USV縱向速度u和艏向角ψ以及左右電機(jī)指令u1(k)、u2(k)數(shù)據(jù)代到式(4)中。盡管是多變量，對于USV特征模型按照每路獨(dú)立進(jìn)行在線參數(shù)辨識，辨識方法采用帶遺忘因子λ的遞推最小二乘法[23]的方法。從而在線辨識出F1、F2、G0和G1項(xiàng)的值，即可實(shí)現(xiàn)對USV特征模型參數(shù)的在線參數(shù)估計(jì)。

(4)

2 基于特征模型的全系數(shù)自適應(yīng)控制器

全系數(shù)自適應(yīng)控制結(jié)構(gòu)如圖2所示，虛線框內(nèi)的為全系數(shù)自適應(yīng)控制器。

圖2 基于特征模型的全系數(shù)自適應(yīng)控制結(jié)構(gòu)Fig.2 The all-coefficient adaptive control based on characteristic model

建立的USVWM是維持跟蹤控制器；WH是黃金分割自適應(yīng)控制器[24]；LI是積分控制器；u(k)是控制量總輸入；WD表示被控對象；Y(k)是期望輸出與實(shí)際輸出的誤差。二階特征模型、總控制量由黃金分割自適應(yīng)控制器、維持跟蹤控制器和積分控制器組成：

X(k+1)=F1X(k)+F2X(k-1)+

G0U(k)+G1U(k-1)

(5)

u(k)=uh(k)+uw(k)+ui(k)

(6)

式中：設(shè)期望輸出為Xr(k)，實(shí)際輸出為X(k)；參數(shù)估計(jì)值為F1、F2、G0和G1。

1)黃金分割自適應(yīng)控制器。

黃金分割自適應(yīng)控制器是針對構(gòu)建了特征模型的系統(tǒng)，構(gòu)成一個黃金分割比例與最小方差相結(jié)合的黃金分割自適應(yīng)控制：

uh(k)=-[G0+λ]-1·[l1F1Y(k)+

l2F2Y(k-1)]

(7)

式中：Y(k)=Xr(k)-X(k)；l1=0.382l2=0.618，F(xiàn)1、F2是特征模型中的待辨識參數(shù)，λ為可調(diào)參數(shù)，其作用是防止矩陣G0奇異。

2)維持跟蹤控制器。

控制量uw(k)是為了保持輸出X(k)為一定值或跟蹤某一理想輸出曲線。維持控制量中的可變參數(shù)是由估計(jì)參數(shù)計(jì)算所得?？刂坡桑?/p>

uw(k)=[Xr(k+1)-F1·Xr(k)-

F2·Xr(k-1)]×[G0]-1

(8)

3)積分控制器。

ui(k)保證系統(tǒng)在參數(shù)估計(jì)有偏差時有較好的動態(tài)品質(zhì)，各回路采用各自的積分控制有助于調(diào)節(jié)：

ui(k)=kiY(k)+cui(k-1)

(9)

式中ki、c為可調(diào)參數(shù)。

3 自適應(yīng)路徑跟蹤控制器的設(shè)計(jì)

如圖3所示，USV路徑跟蹤控制器采用間接控制方法，即制導(dǎo)環(huán)節(jié)與控制環(huán)節(jié)分離的結(jié)構(gòu)，由內(nèi)環(huán)基于特征模型的USV艏向角和速度控制器和外環(huán)VF制導(dǎo)律2部分組成。基于矢量場制導(dǎo)律根據(jù)參考路徑信息和USV的運(yùn)動狀態(tài)數(shù)據(jù)輸出USV跟蹤參考路徑所需要的參考艏向角指令ψcmd以及提前設(shè)定好的速度值v。VF制導(dǎo)環(huán)節(jié)如圖4所示。

圖3 自適應(yīng)路徑跟蹤控制器結(jié)構(gòu)Fig.3 Adaptive PF controller structure

圖4 VF制導(dǎo)算法Fig.4 VF guidance law for USV

3.1 VF直線制導(dǎo)

VF直線制導(dǎo)算法如式(11)中所示：

(10)

式中:ds為USV到參考直線的垂直距離;χp為船艏向與USV和參考點(diǎn)連線與正北方向的夾角。通過設(shè)定χms、ks值可計(jì)算出USV從當(dāng)前船位跟蹤參考路徑所需要的艏向角度ψcmd(即式中χd),從而實(shí)現(xiàn)USV路徑跟蹤過程中的直線制導(dǎo)環(huán)節(jié)。

3.2 VF圓形制導(dǎo)

VF圓形制導(dǎo)算法為：

(11)

式中：dc為USV到參考圓的距離誤差，χOrbit為參考圓心點(diǎn)USV當(dāng)前位置點(diǎn)的連線與正北方向夾角的角度。通過設(shè)定χmc、kc值可計(jì)算出USV從當(dāng)前船位跟蹤參考路徑所需要的艏向角度Ψcmd(即式中χd)，從而實(shí)現(xiàn)USV路徑跟蹤過程中的圓形制導(dǎo)環(huán)節(jié)。

4 實(shí)驗(yàn)與結(jié)果分析

4.1 iNav-IV型USV系統(tǒng)架構(gòu)和實(shí)船實(shí)驗(yàn)環(huán)境

iNav-IV型USV是一艘純電動的雙推進(jìn)型USV，長1.75 m、寬0.75 m、吃水0.12～0.13 m，推進(jìn)器間距0.4 m。該型USV的推進(jìn)器為2臺直流電機(jī)推進(jìn)器，配置有慣性測量與導(dǎo)航系統(tǒng)、差分GPS系統(tǒng)和船岸無線通信系統(tǒng)。

實(shí)驗(yàn)當(dāng)天風(fēng)向?yàn)闁|北、最大風(fēng)速約為3.18 m/s。實(shí)船實(shí)驗(yàn)中最小二乘算法中的遺忘因子取0.98；全系數(shù)自適應(yīng)算法中，在速度和艏向2個控制回路中ki分別取值1、1，c分別取值-0.2、-0.1；VF制導(dǎo)律中的χms設(shè)為90,ks設(shè)為0.8，χmc設(shè)為90，ks設(shè)為0.7。

4.2 艏向和速度協(xié)同控制實(shí)驗(yàn)及結(jié)果分析

為檢驗(yàn)USV特征模型和全系數(shù)自適應(yīng)控制器參數(shù)在線估計(jì)和艏向與速度協(xié)同控制的實(shí)際效果。設(shè)計(jì)一組期望速度為1.0 m/s、轉(zhuǎn)艏幅度為100°左右的大角度轉(zhuǎn)艏艏向與速度協(xié)同控制實(shí)驗(yàn)。

如圖5所示，實(shí)驗(yàn)結(jié)果證明基于特征模型的全系數(shù)自適應(yīng)艏向和速度協(xié)同控制器能夠較好的實(shí)現(xiàn)艏向和速度的協(xié)同控制。

圖5 船速1.0 m/s的USV艏向控制實(shí)驗(yàn)Fig.5 The heading control results of vessel speed at 1.0 m/s

4.3 路徑跟蹤實(shí)驗(yàn)及結(jié)果分析

將所設(shè)計(jì)的USV自適應(yīng)路徑跟蹤控制器部署到iNav-IV型USV上。USV在實(shí)際執(zhí)行任務(wù)時的參考路徑通常是由多端直徑路徑和曲線路徑組成的復(fù)雜路徑組成。因此，設(shè)計(jì)以0.8 m/s的航速對如圖6所示的復(fù)雜路徑進(jìn)行跟蹤的實(shí)驗(yàn)，包括大角度機(jī)動、連續(xù)弧形路徑等，參考路徑包括6段直線路徑、3段曲線路徑和1個圓形路徑。

圖6所示的復(fù)雜路徑跟蹤實(shí)驗(yàn)的結(jié)果表明iNav-IV型USV在各路徑段銜接處的航跡誤差較大，跟蹤復(fù)雜路經(jīng)的航跡誤差在±2 m以內(nèi)，可以較好地完成復(fù)雜路徑跟蹤任務(wù)。

圖6 復(fù)雜路徑徑跟蹤實(shí)驗(yàn)結(jié)果Fig.6 The results of complex-path guidance

4.4 相對惡劣環(huán)境下的仿真驗(yàn)證

實(shí)船實(shí)驗(yàn)船只為小型測繪無人艇，由于其自身?xiàng)l件限制(最大船速2 m/s)只進(jìn)行低速條件下的實(shí)船實(shí)驗(yàn)。ROS系統(tǒng)中的Gazebo仿真軟件是一款可以為機(jī)器人增加現(xiàn)實(shí)世界的物理屬性、構(gòu)建傳感器模型甚至可以為傳感器添加噪聲的物理仿真軟件，利用如圖7所示的Gazebo軟件進(jìn)行仿真實(shí)驗(yàn)，以檢驗(yàn)在航速更高，實(shí)驗(yàn)環(huán)境干擾較大的條件下驗(yàn)證路徑跟蹤控制器的控制效果。

圖7 Gazebo仿真環(huán)境Fig.7 The environments of Gazebo simulation

仿真環(huán)境中船長4.05 m，通過修改仿真環(huán)境中的usv_gazebo_thrust_plugin插件中的推力函數(shù)從而使仿真環(huán)境中的無人艇航行速度增大，通過修改wind_coeff_vector插件中的風(fēng)的力矩從而增大環(huán)境干擾:

(12)

式中：T為推力，N；x為推理指令x∈[-1,1]，其余變量為廣義邏輯函數(shù)中的參數(shù)，在這種模式下，通過2個廣義邏輯函數(shù)將指令轉(zhuǎn)換為推力：

(13)

式中：Cx、Cy、CN為3個經(jīng)驗(yàn)參數(shù)；V為風(fēng)速；

在仿真環(huán)境中，設(shè)計(jì)一組多段直線路徑跟蹤實(shí)驗(yàn)，風(fēng)速約為6 m/s，風(fēng)向西南，直線路徑總長約為840 m,轉(zhuǎn)向點(diǎn)半徑設(shè)為20 m，實(shí)驗(yàn)結(jié)果如圖8所示。

圖8 仿真實(shí)驗(yàn)結(jié)果Fig.8 The results of simulation

仿真實(shí)驗(yàn)結(jié)果表明：所設(shè)計(jì)的自適應(yīng)路徑跟蹤控制器在速度更快，環(huán)境干擾更大的情況下控制器依然有效，且穩(wěn)定后航跡誤差在一倍船長以內(nèi)。

5 結(jié)論

1)無人艇艏向和速度協(xié)同控制系統(tǒng)可通過特征模型體現(xiàn)控制量與要求輸出量之間的特征關(guān)系，與傳統(tǒng)建模方法相比簡化了模型結(jié)構(gòu)并減少了計(jì)算量。

2)引入VF制導(dǎo)算法，結(jié)合基于特征模型的全系數(shù)自適應(yīng)艏向和速度協(xié)同控制器設(shè)計(jì)了路徑跟蹤控制器，分析表明本方法可以實(shí)現(xiàn)復(fù)雜路徑的跟蹤。

3)通過仿真及實(shí)船實(shí)驗(yàn)表明，該USV自適應(yīng)路徑跟蹤控制器在真實(shí)航行環(huán)境和不同期望航速下的控制性能和路徑跟蹤效果，具有較好的自適應(yīng)性。

考慮到本文采用的特征模型對控制器參數(shù)初值范圍要求嚴(yán)格和VF制導(dǎo)算法參數(shù)不可在線調(diào)節(jié)的局限性，下一步將研究特征模型參數(shù)初值問題并采用改進(jìn)的變參數(shù)VF制導(dǎo)算法來設(shè)計(jì)無人艇路徑跟蹤控制器，以提高控制器的適應(yīng)能力和精度。