無人艇航跡跟蹤控制系統設計與驗證

劉志強， 卞濤,2,*， 葉曦， 劉彬

(1.江漢大學智能制造學院， 武漢 430056； 2.廣東華中科技大學工業技術研究院， 東莞 523808；3.華中科技大學人工智能與自動化學院， 武漢 430074)

近年來，無人艇(unmanned surface vehicle，USV)的智能化水平逐步提高，應用領域逐漸擴展。在軍事領域，USV承擔著反潛偵察、國土安全、情報收集等使命[1-2]；在民用領域，USV在環境檢測、水質監測、水上救援等方面得到應用[3-4]。USV依托其小型輕便、成本低、無人化等特點[5]，受到中外學者的廣泛關注。無人艇航跡跟蹤定義為從任意初始位置出發，在滿足空間和時間要求的前提下，實現跟蹤或遵循預定航跡的目的。航跡跟蹤不僅需要保持正確航向以達到沿預定航線高速行駛，還需克服各種惡劣環境。當前，無人艇航跡跟蹤算法的理論研究較多，實際平臺搭建較少，而且無人艇系統包含了導航、制導以及控制三個部分，它們之間存在相互作用，每一個部分都直接關系到無人艇執行水面任務的效率和安全。因此，開發一種能夠應用于內湖環境中的無人艇航跡跟蹤控制系統具有很大的價值和意義。

許多USV裝備有兩個獨立的推進器或一個推進器和一個方向舵，它們屬于一類欠驅動系統。這類系統的控制輸入數量比自身所需控制的自由度數量少，所以會存在不可積的加速度約束，從而增加了系統的復雜性[6]。另外，為全驅動USV所提出的許多運動控制方案可能不會直接推廣到欠驅動USV上，這也為解決欠驅動USV航跡跟蹤控制研究帶來了困難。

USV的欠驅動問題要求在某種制導策略的作用下，將對USV浪涌、橫漂以及首搖三個自由度的運動控制轉化為一個航向控制問題。針對這一問題，當前解決策略中一個重要的方法就是視線(line of sight，LOS)制導法，許多學者對此開展了研究。文獻[7]將LOS制導算法應用于無人艇直線航跡跟蹤，通過實船實驗進行了驗證；文獻[8]設計了一種基于LOS算法和級聯定理的路徑跟蹤控制策略，使得交叉誤差均勻半指數穩定，然而導航定律容易受到環境干擾，如風、浪和洋流的影響。如果USV受到環境干擾的影響，會出現收斂問題和路徑偏差；為補償這種干擾，在文獻[9]提出了一種考慮外部環境干擾的自適應LOS制導算法，通過仿真分析驗證了算法收斂速度快，路徑跟蹤誤差小，然而未將無人艇應用于室外內湖環境中執行水面任務。總的來說，由于LOS制導算法的簡單性和實用性，它在無人艇航跡跟蹤控制系統設計中發揮了關鍵作用。

在無人艇的航向控制問題中，PID控制、滑模控制、神經網絡控制等方法的應用已有諸多文獻做了研究。文獻[10]應用粒子群算法改進PID控制器，設計了無人艇智能航向控制系統；文獻[11]將基于指數滑模面的滑模同反演法結合，設計了無人艇航向控制器，通過仿真分析和實物實驗驗證了算法的有效性；文獻[12]基于雙體結構的無人艇，利用RBF神經網絡改進PID算法，在數值仿真分析中驗證了該控制器具有較好的性能。雖然，智能算法結合PID控制能夠大幅提高控制性能，但可能會遇到復雜的計算問題，在實際應用中難以實現。另外，精確的無人艇數學模型是難以建立的，致使許多控制算法在實際工程應用中效果欠佳。近年來，無模型自適應控制方法在工業控制領域得到了廣泛應用[13-14]，它是一種數據驅動控制，具有自適應特性，更適合處理系統的不確定性[15]。由于無模型自適應控制具有計算簡單、魯棒性強、模型要求低等優點，它更有利于將無人艇航跡控制系統應用到實艇中。

綜合上述分析，目前中外在無人艇航跡跟蹤控制上所取得的研究成果大多處于數值仿真或者半物理仿真階段，實際應用較少。因此，本文的研究目標是設計一個由控制算法、數據采集與處理、上位機監控三部分組成的無人艇航跡跟蹤控制系統，實現無人艇在內湖穩定地執行水面任務。首先，在數據采集與處理部分采用卡爾曼濾波算法；其次，在控制算法部分設計基于自適應LOS算法的制導系統和基于無模型自適應PID算法的航向控制系統；然后，為實時獲取無人艇航行的位置和姿態信息，開發一種上位機監控系統；最后，基于“江豚-16”全自主無人艇開放實驗平臺測試航跡跟蹤控制系統的效果，在室外內湖環境下進行實艇驗證。

1 無人艇系統總體結構

1.1 系統模型

以江漢大學“江豚-16”無人艇實驗平臺為研究對象，設計航跡跟蹤控制系統以實現無人艇的自主航行。該艇長1.6 m，最大載荷35 kg，配備有噴水推進器、直流無刷電機、倒車舵機、差分GPS等，同時搭配多種傳感器，包括激光雷達、慣導、視覺等。其外部結構如圖1所示。

圖1 無人艇外部結構Fig.1 USV external structure

“江豚-16”無人艇的數學模型為

(1)

式(1)中：

(2)

(3)

(4)

(5)

(6)

(7)

式中：x、y和ψ分別為慣性坐標系下無人艇的位置和方向；u、v和r分別為艇體坐標系下的前進速度、橫漂速度和航向角速度；τu和τr分別為前進力和首搖力矩；τwind和τwave分別為風、浪引起的力和力矩。

圖3 系統整體框架Fig.3 Overall system framework

1.2 運動控制系統總體設計方案

圖2 無人艇運動控制系統總體結構Fig.2 Overall structure of USV motion control system

無人艇運動控制系統總體分為三個部分，分別是任務層、控制層和執行層，如圖2所示。在各層協同下控制無人艇自主航行，當遭遇突發狀況或不明障礙物時，任務層可以直接對執行層發送控制指令，從而避免系統響應的延遲。控制層根據任務層所獲取到的目標指令以及環境信息，采用一些算法來求解出應施加在無人艇各個自由度上執行機構的推力和力矩，再將這部分指令發送到執行層。執行層又根據控制層所獲取到的信息將推力和力矩合理施加到執行機構上，通過驅動執行機構來作出相應的運動，從而讓無人艇按照預設航跡自主航行。

2 航跡跟蹤控制系統設計

本文研究中開發了一種無人艇航跡跟蹤控制系統，如圖3所示。該系統分為航跡跟蹤、數據采集與處理、上位機監控三個子系統。在航跡跟蹤子系統中，搭建了基于自適應LOS制導算法和無模型自適應PID的航跡控制器；在數據采集與處理子系統中，將采集到的導航信息和狀態環境信息經過卡爾曼濾波器融合處理；在上位機監控子系統中，根據用戶指令設定目標航跡，無人艇跟蹤目標航跡的位置信息和狀態信息被實時監控和保存。

2.1 數據采集與處理

無人艇航跡跟蹤控制系統的數據采集與處理任務主要是獲取全部傳感器數據信息，包括無人艇的位置、姿態、速度、舵角、轉速以及各種障礙物等信息。為準確估計無人艇航行過程中的位置和姿態信息，本文采用卡爾曼濾波器對傳感器信息進行融合處理。

將卡爾曼濾波器的輸入作為組合導航系統的量測值，輸出作為慣導系統的估計誤差值，其計算步驟如下。

首先定義狀態預測方程和預測方差陣[16]為

Xk,k-1=Φk,k-1Xk-1

(8)

(9)

式中：Xk,k-1為前一個狀態的預測值；Xk-1為前一個狀態的最優值；Φk,k-1和Ek-1為系統參數；Mk,k-1為對應Xk,k-1的協方差；Mk-1為對應Xk-1的協方差；Nk-1為系統過程噪聲的協方差。

然后對系統狀態變量進行估計，如式(10)所示，以Xk作為當前最優狀態估計值，Zk為系統的測量值。

Xk=Xk,k-1+Kk[Zk-HkXk,k-1]

(10)

由式(7)和式(8)計算卡爾曼增益矩陣Kk：

(11)

式(11)中：Hk為量測矩陣；Rk為測量噪聲的協方差。

最后估計均方差矩陣Mk，有

ΜMk=(I-KkHk)Mk,k-1

(12)

2.2 LOS制導系統

圖4 LOS制導算法航跡跟蹤原理Fig.4 LOS guidance algorithm track tracking principle

無人艇航跡跟蹤的準確性很大程度上取決于制導算法，LOS制導算法在水面船舶航跡跟蹤控制中應用廣泛。其原理是在預先設定的一段航跡上選取視線點來求解無人艇在每個航跡點處的期望航向角[17-19]。在制導系統中，采用自適應LOS算法計算目標轉向角，作為航向控制系統的輸入。LOS航跡跟蹤原理如圖4所示。在圖4中，Pn點為路徑起點，Pn+1點為目標航跡點，O點為無人艇實時位置，以O點為圓心，以可調參數RLOS為可視半徑，可得到一片扇形區域。假設無人艇長為L，船長比為n，則R=nL，Δh為前視距離，e為航跡誤差。O點正北方向與OPLOS直線之間的夾角為實時目標航向ψr，而實際航向ψ由北斗定位系統獲取，ψr-ψ為偏差角度Δψ。當無人艇進入Pn+1點圓形區域內時，將切換到新的航跡點，循環往復，最終實現無人艇對目標航跡的跟蹤。

應用式(13)可求得PLOS：

(13)

根據無人艇的當前位置和期望航跡獲取目標航向ψr為

(14)

無人艇空間固定速度到慣性速度的轉換關系為

(15)

(16)

(17)

式中：e為交叉跟蹤誤差；浪涌速度假定為常數u0，且u0>0；橫漂速度通常很小，可以假定為v≈0。故，式(16)簡化為

(18)

(19)

(20)

這里采用Lyapunov第二法證明e收斂到0，設置Lyapunov函數為V(e)=e2，則有

(21)

上述所設計的基本LOS算法的視線圓半徑是固定的，在跟蹤預設航跡時收斂速度較慢，難以保證較高的跟蹤精度。故提出一種自適應LOS算法，基于過渡層自適應地調整視線圓半徑，具體方案為

(22)

式(22)中：a=Rmin-μ；b=Rmin+μ；Rmin為最小可視半徑；μ為過渡層系數；k3為可調參數。調整方案概述為：當|e|≤a時，無人艇以最小內切圓可視半徑Rmin收斂到目標航跡；當|e|>b時，可視半徑RLOS為當前航跡誤差|e|；當a<|e|≤b時，可視半徑RLOS在區間[Rmin,b]內光順過渡，使得視線角抖動變小，無人艇能夠平穩地趨向于目標航跡。

2.3 航向控制系統

采用無模型自適應PID(model free adaptive PID，MFA-PID)算法設計航向控制器，通過北斗定位系統獲取無人艇實時位置和航向信息，以LOS制導算法求解的目標航向ψr為輸入，控制量為指令舵角δr，系統輸出為位置信息，即當前坐標(x,y)和實際航向ψ，從而形成雙閉環結構的航跡跟蹤系統，如圖5所示。

圖5 航跡跟蹤控制結構Fig.5 Trajectory tracking control structure

無人艇航向控制系統屬于SISO非線性離散系統，計算公式[20-22]為

y(k+1)=f[y(k),…,y(k-ny),

u(k),…,u(k-nu)]

(23)

式(23)中：y(k)∈R和u(k)∈R分別為k時刻系統的輸入輸出；R為實數集；ny和nu為系統的階次。

對式(23)提出如下假設[23]。

假設1f(…)關于第(ny+2)個變量的偏導數存在且連續。

假設2式(23)滿足廣義Lipschitz條件，即對任意k1≠k2，k2≥0和u(k1)≠u(k2)有

|y(k1+1)-y(k2+1)|≤b|u(k1)-u(k2)|

(24)

式(24)中：y(ki+1)=f[y(ki),…,y(ki-ny),u(ki),…,u(ki-nu)]；i=1,2；b>0為常數。

假設3是在控制系統的設計中對一般非線性系統設定的一種典型的約束條件；假設2是對系統輸出變化率上界的一種限制，也即有界的輸入能量變化產生有界的輸出能量變化，顯然它包括一類非線性系統。

定理1對滿足假設1和假設2的非線性系統[式(23)]，當|Δu(k)|≠0時，必存在一個被稱為“偽偏導數”的時變參數φ(k)∈R，使得系統[式(23)]可轉化為如下CFDL數據模型：

Δy(k+1)=φ(k)Δu(k)

(25)

且φ(k)在任意k時刻有界，Δy(k+1)=y(k+1)-y(k)，Δu(k)=u(k)-u(k-1)。定理1的證明參見文獻[24]。

因此，可得到式(23)的動態線性化模型，即泛模型為

y(k+1)-y(k)=φ(k)u(k)-u(k-1)

(26)

根據式(26)進行無模型自適應PID控制律設計。若系統期望輸出為yd(k)，系統控制目標是找到一個控制序列u(k)使得在k趨于無窮時航向誤差e(k)=yd(k)-y(k)收斂于0。

MFA-PID設計方案如下：

kp[e(k)-e(k-1)]+kie(k)+

kd[e(k)-2e(k-1)+e(k-2)]

(27)

(28)

(29)

2.4 上位機監控系統

為了實時獲取無人艇航行過程中的位置和姿態信息，實現對無人艇數據融合處理、任務設定分配以及實時地圖顯示，建立了一款無人艇航跡跟蹤上位機監控系統，系統框架如圖6所示。

圖6 上位機監控系統框架Fig.6 Framework of upper computer monitoring system

上位機監控系統主要功能如下。

(1)顯示電子地圖。該軟件設計了地圖顯示區，能夠實現2D地圖和衛星地圖的切換，并添加了谷歌地圖應用。地圖中可以顯示無人艇的航行狀態，包括坐標、任務點、航跡等信息，如圖7所示。

(2)接收處理無人艇狀態數據。由遙控器傳輸無人艇航行的狀態信息，上位機軟件通過232總線實時接受。所接受的數據信息會經過卡爾曼濾波器處理，再對數據進行過濾轉化。

(3)設置航跡點位置。用戶可通過鼠標左鍵在地圖區域設置預定航跡點，第一個航跡點作為起點，最后一個航跡點作為終點。通過任務設置功能將所有航跡點作為任務信息發送給無人艇，點擊“主控板運行”，無人艇會按照預設航跡自主航行。

(4)執行航道跟蹤任務。在航道跟蹤模式下，地圖上會顯示出預設航跡和無人艇實際航行的航跡，并能實時顯示無人艇航行姿態。無人艇在執行航跡跟蹤任務時，用戶可隨時取消任務。

圖7 上位機監控界面Fig.7 Upper computermonitoring interface

3 實艇驗證

利用所設計的無人艇航跡跟蹤控制系統進行實艇實驗，以驗證該控制系統的有效性和可靠性。實驗對象為“江豚-16”無人艇實驗平臺，實驗地點在江漢大學三角湖龍舟碼頭，實驗時天氣晴朗，氣溫18 ℃，風浪較小。制導系統參數設置為：Rmin=3，μ=1，k3=0.02；航向控制系統參數設置為：kp=0.653，ki=0.001，kd=1.125，λ=5，μ=100，ρ=1，η=0.6。圖8為實驗過程的工作場景，圖9為無人艇在湖面的運行情況。本實驗分為三組，第一組在不同航速下測試無人艇跟蹤直線航跡的效果；第二組在同一航速下測試無人艇跟蹤不同軌跡的效果；第三組測試無人艇航跡跟蹤控制系統面對隨機強干擾情況的效果。通過這三組實驗來綜合驗證無人艇的航跡跟蹤能力和系統的實用性。

圖8 實驗工作場景Fig.8 Experimental working scene

圖9 無人艇湖面航行情況Fig.9 USV sailing on the lake

3.1 不同航速下跟蹤直線航跡

為驗證“江豚-16”無人艇在不同航速下的航跡跟蹤能力，設定4種航速測試實驗，分別為1、2、3、5 m/s。圖10為4種航速下無人艇跟蹤直線航跡效果。從圖10中可以看出，無人艇能夠準確地從航跡點1跟蹤行駛到航跡點2。圖10(a)～圖10(c)反映了無人艇以較低速度航行的情況，跟蹤航跡與預設航跡基本吻合，但速度越快，跟蹤誤差也隨之增大。當速度提升到5 m/s時，從圖10(d)中可以看到，無人艇明顯偏離了預設航跡，跟蹤誤差較大。這是由于航速太快，算法收斂速度跟不上，另外航向航速具有耦合作用，較快的航速也影響了航向，使得無人艇偏離了預設航跡，但只是增大了誤差并未對無人艇的穩定行駛產生太大影響，說明該系統的穩定性還是可靠的。

3.2 同一航速下跟蹤不同航跡

無人艇在實際應用中需要在水面到達不同地點，執行各種任務。為驗證“江豚-16”無人艇執行不同任務的能力，統一設定航速為1.8 m/s，跟蹤任務設置兩段航跡、三段航跡、四段航跡、五段航跡，分別對應于4組水面任務。圖11為無人艇執行不同水面跟蹤任務的情況。從圖11中可以看出，無人艇可以順利完成4種不同情況的水面任務。當任務點出現拐角時，無人艇會產生較大幅度的轉向，這個轉向角來自LOS制導算法的解算，圖中無人艇的轉向效果良好，驗證了制導系統的合理性，證明本文所設計的無人艇航跡跟蹤控制系統具有較好的實際應用價值和意義。

圖12 隨機強干擾下航跡跟蹤Fig.12 Path tracking under random strong interference

3.3 湖面隨機強干擾情況

復雜湖面環境下無人艇自主航行不僅會受到風、浪、流等因素的影響，還會遇到水面垃圾、“魚躍水”以及不明障礙物等隨機強干擾情況。應對隨機強干擾的能力是評價無人艇航跡跟蹤控制系統的重要指標，因此本文研究在湖面反復進行航跡跟蹤實驗，在多組實驗中找出突發隨機強干擾的例子，如圖12所示。

從圖12(b)中可以看到無人艇航跡跟蹤軌跡在第一段直線路徑上出現了較大偏離，從圖12(c)和圖12(d)中反映出航向角和航速在31 s左右受到了隨機強干擾作用，這一作用使得航向角最大偏離了9°左右，跟蹤誤差也隨之變大。但經過約5 s的調整后，無人艇又回到了穩定地航行狀態，并沒有對后續的跟蹤任務造成太大影響，這說明本文所設計的航跡跟蹤控制系統具備應對隨機強干擾的能力，具有較好的魯棒性和穩定性。

4 結論

無人艇作為一類智能機器人，未來在江河湖海上的應用具有極大的潛力。針對無人艇在內湖執行水面任務的難點，設計了一種無人艇航跡跟蹤控制系統，并將其應用于“江豚-16”無人艇實驗平臺。該系統利用卡爾曼濾波器進行數據采集和處理，基于自適應LOS制導算法對預設航跡進行跟蹤，通過設計無模型自適應PID航向控制器，保證了航跡跟蹤系統的穩定性。在實艇驗證過程中，利用“江豚-16”無人艇在江漢大學三角湖水域分別測試了不同航速下跟蹤直線航跡、同一航速下跟蹤不同航跡以及面對隨機強干擾的效果，結果表明，該航跡跟蹤控制系統能夠實現無人艇在內湖中穩定地執行各種水面任務，跟蹤效果良好，具有較強的魯棒性和實用性。

不過，本文所設計的無人艇航跡跟蹤控制系統僅在內湖環境下進行了驗證，接下來還需要在河流、海洋等更加復雜多變的環境下進行測試，另外設計考慮漂角、風、浪、流等因素影響的航跡跟蹤算法也有待進一步研究。