基于雙偶極向量場的欠驅(qū)動(dòng)無人船目標(biāo)跟蹤制導(dǎo)方法

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

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

0 引 言

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

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

1 問題描述

1.1 欠驅(qū)動(dòng)無人船

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

圖1 無人船運(yùn)動(dòng)的地球坐標(biāo)系和船體坐標(biāo)系Fig. 1 Earth-fixed frame and body-fixed frame of AUV motion

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

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

1.2 雙偶極向量場

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

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

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

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

2.1 導(dǎo)引律設(shè)計(jì)

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

航向誤差可表示為

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

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

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

圖3 所示為基于雙偶極向量場USV 的目標(biāo)跟蹤制導(dǎo)控制框圖。

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

2.2 穩(wěn)定性分析

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

分別對(duì)等號(hào)兩側(cè)進(jìn)行求導(dǎo)，可得

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

化簡后，可得

此時(shí)，說明USV 正在接近目標(biāo)位置參考點(diǎn)，故將其代入式（11），可得

3 仿真結(jié)果

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

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

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

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

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

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

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

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

圖6 和圖7 分別給出了向量場制導(dǎo)和目標(biāo)跟蹤制導(dǎo)時(shí)的USV 速度對(duì)比。由圖可見，USV 均可以在一段時(shí)間后跟蹤到制導(dǎo)速度。

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

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

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

圖8 目標(biāo)點(diǎn)跟蹤無人船實(shí)驗(yàn)配置Fig. 8 Experimental configuration of USV for target point tracking

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

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

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

圖9 目標(biāo)跟蹤制導(dǎo)時(shí)的實(shí)驗(yàn)軌跡Fig. 9 Trajectory in experiment with target tracking guidance

圖10 向量場制導(dǎo)時(shí)的實(shí)驗(yàn)軌跡Fig. 10 Trajectory in experiment with vector field guidance

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

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

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

圖12 向量場制導(dǎo)時(shí)的制導(dǎo)速度和跟蹤船速度Fig. 12 Velocities of guiding and follower with vector field guidance

圖13 目標(biāo)跟蹤制導(dǎo)時(shí)的制導(dǎo)速度和跟蹤船速度Fig. 13 Velocities of guiding and follower with target tracking guidance

5 結(jié) 語

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