初慶棟，尹羿博，龔小旋，劉陸，王丹，彭周華

大連海事大學 船舶電氣工程學院，遼寧 大連 116026

0 引 言

近20 年以來，海洋航行器已廣泛應(yīng)用于水文監(jiān)測、生物檢測、航道疏通以及搜索和救援等領(lǐng)域[1-4]，其制導和控制已經(jīng)成為研究的熱點問題。根據(jù)控制目標的不同，對單一無人船（USV）的運動控制可以分為3 類主要問題[5-6]：目標跟蹤、軌跡跟蹤和路徑跟蹤。其中，目標跟蹤技術(shù)是提高其自主性面臨的挑戰(zhàn)之一。文獻[7]提出了一種直線目標跟蹤控制方法；文獻[8]針對目標跟蹤問題，結(jié)合李雅普諾夫穩(wěn)定性理論和反步法使USV 跟蹤到了目標點；文獻[9-10]結(jié)合A 星路徑規(guī)劃方法，為USV 提供到達目標點的最優(yōu)路徑，實現(xiàn)了目標跟蹤；文獻[11]基于視覺檢測，獲得了USV 與目標間的方向及距離，進而跟蹤到目標點。然而，傳統(tǒng)的USV 目標跟蹤技術(shù)也存在不足，原因在于：其一，現(xiàn)有技術(shù)是根據(jù)角度誤差來調(diào)節(jié)制導角速度的，若初始的角速度誤差較大，制導角速度會產(chǎn)生較大的暫態(tài)振蕩和超調(diào)，從而影響系統(tǒng)暫態(tài)性能[12-13]，導致USV 的跟蹤軌跡不夠光滑；其二，現(xiàn)有制導方法在跟蹤到目標點時會出現(xiàn)奇異點，即穩(wěn)態(tài)時航向不能指定[14-15]。

鑒于上述不足，本文擬提出一種基于雙偶極向量場的USV 目標跟蹤制導方法。根據(jù)雙偶極向量場原理，設(shè)計一種欠驅(qū)動USV 的前向速度和艏搖角速度的導引律。然后，通過穩(wěn)定性分析，以證明控制系統(tǒng)的有界穩(wěn)定性。最后，通過仿真與實驗驗證所設(shè)計的目標跟蹤制導方法的有效性。

1 問題描述

1.1 欠驅(qū)動無人船

本文的研究對象為一種雙槳推進的欠驅(qū)動USV。USV 的運動方程一般是基于地球坐標系和船體坐標系來表示，如圖1 所示。欠驅(qū)動USV的運動方程由式（1）[1]描述。

圖1 無人船運動的地球坐標系和船體坐標系Fig. 1 Earth-fixed frame and body-fixed frame of AUV motion

式中： (x,y)表示在地球坐標系（XE-YE）下的位置；ψ ∈(-π,π]， 表示USV 的航向；u,v,r為USV 的速度狀態(tài)信息，分別表示USV 在船體坐標系（XBYB)中的前向速度、側(cè)向速度、艏搖角速度。

定義USV 的實際運動速度U=(u2+ν2)1/2＞0，實際的運動方向 ψw=ψ+β（ β表示側(cè)滑角），則其運動方程可表示為

1.2 雙偶極向量場

采用雙偶極向量場的概念來設(shè)計導引律可以使處于向量場中的USV 跟蹤到給定的目標點。圖2 所示為USV 跟蹤給定目標的示意圖。F(p*)：R2→R2，采用如下形式表示：

圖2 雙偶極向量場Fig. 2 Two-dimensional dipolar vector field

雙偶極向量場的主要特點是：處于向量場F(p*)內(nèi)任意一條場線均為一條至目標的路徑，內(nèi)部場線也都是目標點開始至目標點結(jié)束。處于雙偶極向量場中的USV 都會沿著向量場內(nèi)部場線到達雙偶極向量場的目標點，即沿著光滑的場線跟蹤到目標。如圖2 所示雙偶極向量場，其中，藍線表示雙偶極向量場，紅線表示向量場的鏡像線，USV 按照向量場的場線跟蹤至目標點。

2 導引律的設(shè)計和穩(wěn)定性分析

2.1 導引律設(shè)計

為使處于當前位置p的USV 跟蹤到目標位置p*，需要設(shè)計基于雙偶極向量場的導引律。雙偶極向量場根據(jù)式（3）由下式表示。

航向誤差可表示為

式中：USV 當前位置p的 航向 ψc=atan2(Fy,Fx)，即表示向量場在 (x,y)位置的方向，這里規(guī)定ψc=atan2(0,0)=ψ。

基于雙偶極向量場的USV 前向速度與艏搖角速度，可設(shè)計如下導引律。

式中：uc，rc分別為前向速度和艏搖角速度的制導信號；ku,kr∈R+，為導引律參數(shù)，分別表示前向速度和艏搖角速度的制導增益； ψe為無人船航向與向量場方向間的航向誤差； εu,εr為可避免制導信號飽和的參數(shù)。

圖3 所示為基于雙偶極向量場USV 的目標跟蹤制導控制框圖。

圖3 整體控制框圖Fig. 3 System structure

2.2 穩(wěn)定性分析

本節(jié)將證明USV 沿著向量場的場線移動并與目標間的跟蹤誤差最終是一致有界的。

分別對等號兩側(cè)進行求導，可得

將航向誤差和位置誤差代入，可得

化簡后，可得

此時，說明USV 正在接近目標位置參考點，故將其代入式（11），可得

3 仿真結(jié)果

本節(jié)將給出仿真結(jié)果以驗證所提方法的有效性。仿真使用Ardupilot 軟件在環(huán)仿真（SITL+MAVProxy），所用程序即為最終實驗程序。Ardupilot 固件本身已很完善，故未對其修改，而是直接作為底層使用，上層則為前文所述基于向量場的制導算法。在ubuntu16.04 系統(tǒng)上進行編譯，加入對比仿真試驗以驗證所提方法的有效性。對比仿真的目標跟蹤導引律如式（17）所示。

式中： ψa表 示無人船航向與實際方向間的誤差，ψa=ψ-ψc，ψc=atan2(pey,pex)，pe=[xe,ye]T=p-p*。

仿真參數(shù)的取值說明如下：向量場參數(shù) κ=2；參考點在半徑為15 m 的圓形上移動，速度1.2 m/s；導引律參數(shù)ku=0.9，kr=1， εu=1 ， εr=1。仿真結(jié)果如圖4～圖7 所示。

圖7 目標跟蹤制導時制導速度和跟蹤船速度仿真結(jié)果Fig. 7 Simulations of velocity of guiding and follower with target tracking guidance

圖4 所示為向量場制導和目標跟蹤制導時的USV（即跟蹤船）軌跡對比。由圖可見，經(jīng)過一段時間后USV 都可以跟蹤到給定的目標位置點，但紅線軌跡（即有向量場制導時的跟蹤軌跡）更光滑，跟蹤效果較好。

圖4 USV 的仿真軌跡對比Fig. 4 Comparison of simulated trajectories of USV

圖5 所示為向量場制導和目標跟蹤制導時的USV 艏搖角速度對比。由圖可見，在有向量場制導下，2 s 時刻USV 的角速度值達到最大-0.4 rad/s，隨后逐漸減小，最終穩(wěn)定在-0.1 rad/s，此時的USV按照固定的方向旋轉(zhuǎn)，航向不會出現(xiàn)振蕩； 在對比實驗仿真中，目標跟蹤制導時在1 s 時刻USV的角速度達到最大值0.9 rad/s，隨后出現(xiàn)振蕩，在5 s 時刻達到最小值-0.9 rad/s，在角速度振蕩時，USV 的航向也會振蕩，對應(yīng)于圖中USV 的軌跡不夠光滑。經(jīng)計算，有向量場制導時的超調(diào)量是目標跟蹤制導時超調(diào)量的30%；

圖5 USV 的仿真艏搖角速度對比Fig. 5 Comparison of simulated yaw angular velocity of USV

圖6 和圖7 分別給出了向量場制導和目標跟蹤制導時的USV 速度對比。由圖可見，USV 均可以在一段時間后跟蹤到制導速度。

圖6 向量場制導時制導速度和跟蹤船速度的仿真結(jié)果Fig. 6 Simulations of velocity of guiding and follower with vector field guidance

4 實驗結(jié)果

本文采用實驗室的一艘USV 和一臺中央監(jiān)控計算機來驗證所提方法的有效性，如圖8 所示。USV（即圖中跟蹤船）的控制器采用的是Raspberry 4B 和Pixhawk，配 有GPS 接 收 器、2 個推進器、2 個電子調(diào)速器和WiFi 網(wǎng)橋等通信設(shè)備。Pixhawk 內(nèi)置的姿態(tài)傳感器可以獲得USV 的艏搖角速度和偏航角信息，GPS 傳感器可以獲取USV 的位置信息和速度信息。2 個推進器用于提供動力。通過網(wǎng)橋組建局域網(wǎng)，與岸基監(jiān)控系統(tǒng)進行通信，網(wǎng)絡(luò)的工作頻率是2.4 GHz。

圖8 目標點跟蹤無人船實驗配置Fig. 8 Experimental configuration of USV for target point tracking

以下對前文參數(shù)在實驗中的取值進行說明。由于算法是在軟件在環(huán)（software-in-the-loop，SITL）中進行仿真，實驗參數(shù)與仿真參數(shù)取值大致相同。向量場參數(shù) κ=2； 導引律參數(shù)ku=1.2，kr=1，εu=1 ， εr=1。實驗中，虛擬目標點由本地坐標系產(chǎn)生，不在地圖上顯示。虛擬目標點在半徑為10 m的圓形軌跡上運動，速度為1.2 m/s。

圖9～圖13 所示為根據(jù)實驗采集的數(shù)據(jù)繪制成的結(jié)果圖。

圖9 和圖10 為目標跟蹤制導和向量場制導時的USV 軌跡對比。由兩圖可見，經(jīng)過一段時間后USV 都可以跟蹤到目標點，但向量場制導的跟蹤軌跡沒有超調(diào)，跟蹤效果較好。

圖9 目標跟蹤制導時的實驗軌跡Fig. 9 Trajectory in experiment with target tracking guidance

圖10 向量場制導時的實驗軌跡Fig. 10 Trajectory in experiment with vector field guidance

圖11 所示為USV 的艏搖角速度對比。由圖可見，在有向量場制導時，在3 s 時刻USV 的艏搖角速度值達到最大的-0.7 rad/s，隨后開始減小，最終穩(wěn)定在-0.15 rad/s 附近，且始終小于0，即USV會按照固定方向旋轉(zhuǎn)，航向不會振蕩；與實驗采用目標跟蹤制導時的艏搖角速度進行對比，可見USV 的艏搖角速度波動較大，在4 s 時刻達到最大值-0.96 rad/s，隨后開始振蕩，當艏搖角速度的符號改變時，USV 的旋轉(zhuǎn)方向也會隨之改變，并對應(yīng)于圖9 中的軌跡超調(diào)部分。經(jīng)計算，在有向量場制導時，USV 的艏搖角速度超調(diào)量為目標跟蹤制導時超調(diào)量的58%。

圖11 無人船的艏搖角速度Fig. 11 Yaw angular velocities of USV

圖12 和圖13 所示為向量場制導及目標跟蹤制導時的制導速度與USV 跟隨速度（跟蹤船速度）對比。由兩個圖可見，USV 的跟隨速度均可以跟蹤到給定速度，最終給定速度逐漸減小到約1.2 m/s，與目標位置點的速度相近。由于實驗所在湖泊存在水草及分布不均勻的情況，在USV 經(jīng)過水草密集區(qū)時，水草會纏繞在推進器上，影響了USV 的航行，這是產(chǎn)生誤差的原因之一。總體上，誤差是在可接受的控制范圍內(nèi)。

圖12 向量場制導時的制導速度和跟蹤船速度Fig. 12 Velocities of guiding and follower with vector field guidance

圖13 目標跟蹤制導時的制導速度和跟蹤船速度Fig. 13 Velocities of guiding and follower with target tracking guidance

5 結(jié) 語

本文研究了欠驅(qū)動USV 的目標跟蹤制導問題。首先，根據(jù)USV 的運動學模型，設(shè)計了一種基于雙偶極向量場的艏搖角速度和前向速度導引律； 然后，通過穩(wěn)定性分析證明了控制系統(tǒng)的有界穩(wěn)定性； 最后，通過仿真和實驗驗證了所提設(shè)計方法的有效性。結(jié)果表明：與傳統(tǒng)的目標點跟蹤制導方法相比，所提方法因引入了雙偶極向量場，USV 可以直接跟蹤向量場方向，避免了初始的角度跟蹤誤差帶來的USV 跟蹤過程中的振蕩和超調(diào)，減小了操縱過程中的抖動現(xiàn)象，顯著提高了系統(tǒng)的暫態(tài)控制性能及航行安全性。同時，在跟蹤目標時，能夠保持期望的航向，使跟蹤目標的軌跡曲線更為光滑，克服了現(xiàn)有方法中穩(wěn)態(tài)航向不能指定的局限性。