基于雙偶極向量場的欠驅動無人船目標跟蹤制導方法

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

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

0 引 言

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

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

1 問題描述

1.1 欠驅動無人船

本文的研究對象為一種雙槳推進的欠驅動USV。USV 的運動方程一般是基于地球坐標系和船體坐標系來表示，如圖1 所示。欠驅動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 的速度狀態信息，分別表示USV 在船體坐標系（XBYB)中的前向速度、側向速度、艏搖角速度。

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

1.2 雙偶極向量場

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

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

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

2 導引律的設計和穩定性分析

2.1 導引律設計

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

航向誤差可表示為

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

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

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

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

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

2.2 穩定性分析

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

分別對等號兩側進行求導，可得

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

化簡后，可得

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

3 仿真結果

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

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

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

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

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

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

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

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

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

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

4 實驗結果

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

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

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

圖9～圖13 所示為根據實驗采集的數據繪制成的結果圖。

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

圖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，隨后開始減小，最終穩定在-0.15 rad/s 附近，且始終小于0，即USV會按照固定方向旋轉，航向不會振蕩；與實驗采用目標跟蹤制導時的艏搖角速度進行對比，可見USV 的艏搖角速度波動較大，在4 s 時刻達到最大值-0.96 rad/s，隨后開始振蕩，當艏搖角速度的符號改變時，USV 的旋轉方向也會隨之改變，并對應于圖9 中的軌跡超調部分。經計算，在有向量場制導時，USV 的艏搖角速度超調量為目標跟蹤制導時超調量的58%。

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

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

圖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 結 語

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