基于視線導(dǎo)引法的AUV軌跡跟蹤控制技術(shù)研究

郭亦平 王益民 任元洲

（中船重工第七〇七研究所九江分部，九江，332007）

隨著AUV及相關(guān)技術(shù)的發(fā)展，AUV已經(jīng)被用于執(zhí)行掃雷、偵察、情報(bào)搜集及海洋探測(cè)等任務(wù)，未來(lái)還可作為水下武器平臺(tái)、后勤支持等裝備使用[1]。軌跡跟蹤是AUV執(zhí)行任務(wù)時(shí)最基本的運(yùn)動(dòng)模式之一，該控制方法已大量應(yīng)用于航天、兵器、無(wú)人機(jī)（Unmanned aerial vehicle，UAV）、無(wú)人水面艦（Unmanned Surface vehicle，USV）等領(lǐng)域。陳霄、劉忠等針對(duì)欠驅(qū)動(dòng)無(wú)人艇，研究了改進(jìn)視線導(dǎo)引算法，實(shí)現(xiàn)了 USV的自適應(yīng)滑模航跡跟蹤控制[2]；Lionel Lapierre等基于LOS原理，提出了一種AUV反步自適應(yīng)軌跡跟蹤控制算法[3]；Morten Breivik 和 Thor I Fossen在文獻(xiàn)[3]的基礎(chǔ)上，改進(jìn)了 LOS法，基于AUV動(dòng)態(tài)特性提出了一種改進(jìn)的反步自適應(yīng)控制算法[4-5]。本文在學(xué)習(xí)借鑒上述研究成果的基礎(chǔ)上，提出了一種基于 LOS法的運(yùn)動(dòng)控制算法與航向 MPC控制算法串接的AUV軌跡跟蹤控制算法。

1 基于三次樣條插值曲線的路徑規(guī)劃[5]

1.1 操縱任務(wù)概述

一條參數(shù)化軌跡定義為一被連續(xù)軌跡參數(shù)?參數(shù)化的幾何曲線η(?)∈Rq,q≥1，?為連續(xù)路徑變量。對(duì)于定深運(yùn)動(dòng)的AUV而言，可使用一2-D描述：

式中，pn(?)對(duì)?的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)分別記為p′和p′′。

軌跡跟蹤問(wèn)題的一個(gè)經(jīng)常用到的解決方案是把其當(dāng)做一個(gè)操縱問(wèn)題的幾何任務(wù)，由下面的定義給定。操縱問(wèn)題包含兩個(gè)任務(wù)：

（1）幾何任務(wù)：對(duì)于任意連續(xù)函數(shù)?(t)，驅(qū)使位置pn(t)收斂到一期望軌跡，

（2）動(dòng)力學(xué)任務(wù)：根據(jù)下式使速度?˙收斂到期望速度Ud

式中，[xd,yd]為目標(biāo)軌跡的水平面坐標(biāo)位置。動(dòng)力學(xué)任務(wù)服從

式中(t)為沿軌跡切線的指令軌跡跟蹤速度，而(t)表示沿軌跡法向的指令軌跡偏移速度。

1.2 使用3次樣條插值法的路徑產(chǎn)生方法

可使用樣條插值法產(chǎn)生一曲線軌跡(xd(?),yd(?))以通過(guò)事先定義的航路點(diǎn)集合。軌跡(xd(?),yd(?))可通過(guò)選擇=k,k∈R，以致?=kt來(lái)得到，即在一段規(guī)劃路徑軌跡中，連續(xù)路徑變量?對(duì)時(shí)間的導(dǎo)數(shù)為常值。。

考慮下述三次多項(xiàng)式，使三次樣條可內(nèi)插通過(guò)航路點(diǎn)集合

式中，xd(?)、yd(?)是AUV計(jì)劃航路點(diǎn)，而?是由下式給定的軌跡變量

利用樣條插值法的目的之一就是將上式軌跡變量函數(shù)用分段=k函數(shù)進(jìn)行表述。

xd(?)和yd(?)對(duì)?的偏導(dǎo)數(shù)分別為：

AUV規(guī)劃軌跡的指向角即為

因此，AUV的期望速度可計(jì)算如下

上式中的未知參數(shù)a0、a1、a2、a3，b0、b1、b2、b3能夠使用三次樣條算法計(jì)算。

1.3 三次樣條插值路徑產(chǎn)生算法

通過(guò)航路點(diǎn) (xk-1,yk-1)和(xk,yk)的軌跡必須滿(mǎn)足

通過(guò)使下式成立，方可得到平滑曲線

對(duì)x和y，增加兩份邊界條件（速度和加速度）是可能的

對(duì)于n個(gè)航路點(diǎn)，可將未知參數(shù)收集為一個(gè)向量

因此，三次插值問(wèn)題就可被寫(xiě)做如下線性方程：

式中

對(duì)于bk=[b3k，b2k，b1k，b0k]也可用相似方法求得。

1.4 本文算法設(shè)計(jì)

圖1為Serret-Frenet 坐標(biāo)系中AUV的軌跡跟蹤運(yùn)動(dòng)描述，對(duì)于一個(gè)二維參數(shù)化路徑{n}

通過(guò)指定一隨軌跡移動(dòng)的坐標(biāo)系，基于船舶動(dòng)態(tài)模型可設(shè)計(jì)該運(yùn)動(dòng)控制算法。該坐標(biāo)系通常被選作Serret-Frenet 坐標(biāo)系。在軌跡跟蹤過(guò)程中，船速為U，設(shè)計(jì)運(yùn)動(dòng)控制算法的目的為：（1）調(diào)節(jié) AUV與路徑間的距離e和s到 0；（2）調(diào)節(jié)AUV的航速與軌跡變化速度差到0。

由圖1可知，在軌跡跟蹤過(guò)程中，實(shí)時(shí)軌跡位置與AUV間的距離為：

圖1 Serret-Frenet 坐標(biāo)系

為了調(diào)節(jié)AUV與規(guī)劃軌跡間的距離e到0，可采用LOS法求得AUV的實(shí)時(shí)指令航向角為

式中，Δ＞0是一個(gè)常值參數(shù)，φd為式（7）所示的規(guī)劃軌跡方向角。

為了克服洋流等常值干擾影響，可在上式基礎(chǔ)上增加積分環(huán)節(jié)，實(shí)時(shí)指令航向角可修正為：

為了調(diào)節(jié)AUV切向跟蹤偏差到0，可設(shè)計(jì)指令航速運(yùn)動(dòng)控制律為：

式中，χSF的求取就成為AUV軌跡跟蹤的關(guān)鍵。由圖1可知

故對(duì)χSF的求取可轉(zhuǎn)為求AUV漂角問(wèn)題。根據(jù)船舶操縱性可知，AUV的漂角定義為：

因此，可設(shè)計(jì)一水平面狀態(tài)觀測(cè)器對(duì)AUV的橫蕩速度v進(jìn)行估計(jì)。觀測(cè)器的設(shè)計(jì)可參照文獻(xiàn)[6]。

2 帶有約束的航向MPC控制器設(shè)計(jì)

AUV水平面運(yùn)動(dòng)線性方程可用狀態(tài)空間方程描述如下：

式（25）和式（26）簡(jiǎn)記為

利用后向差分法離散化式（27）后，可得

由式（28）可得增量式狀態(tài)方程和狀態(tài)輸出方程：

假設(shè)在k時(shí)刻的m個(gè)采樣間隔后解算的預(yù)測(cè)指令舵角增量為

式中，L（m）T為擬合航向 MPC控制器的離散化Laguerre函數(shù)向量，η為L(zhǎng)aguerre參數(shù)[7]。

假設(shè)在k時(shí)刻，式（29）的狀態(tài)為x（k），則在此時(shí)刻的m個(gè)采樣間隔的預(yù)測(cè)狀態(tài)為

為解決航向預(yù)測(cè)控制律，設(shè)代價(jià)函數(shù)為

將式（30）代入上述代價(jià)函數(shù)后，可得

考慮到 AUV航行控制系統(tǒng)存在下述物理限制：（1）舵速限制：Δumin≤Δu≤Δumax；（2）舵角限制：umin≤u(k)≤umax，可用下述不等式描述舵速約束：

舵角約束可用下述不等式表示為：

航向MPC控制器的求解問(wèn)題轉(zhuǎn)換為二次規(guī)劃問(wèn)題：

該二次規(guī)劃問(wèn)題采用活動(dòng)集法求解，可利用Hildreth二次規(guī)劃方法求解。故航向MPC控制器可設(shè)計(jì)為：

上述二次規(guī)劃問(wèn)題中的AUV運(yùn)動(dòng)狀態(tài)可利用狀態(tài)觀測(cè)器進(jìn)行估計(jì)，可利用離散時(shí)間kalman估計(jì)理論設(shè)計(jì)[8]：

3 仿真分析

本文研究的AUV完整6自由度模型可參照文獻(xiàn)[9]，該 AUV 是 The Naval Postgraduate School（NPS）設(shè)計(jì)的第二代AUV。NPS AUVⅡ的主尺度為：L=5.3 m，W=534 kN。其最高航速為4 kn。根據(jù)前文提出的AUV軌跡跟蹤控制方法，設(shè)計(jì)如下軌跡跟蹤試驗(yàn)：AUV初始位置為（–810，110）m，航向0°，航速3 kn，潛深50 m，航路點(diǎn)共計(jì)13個(gè)，設(shè)置為：(–800，100)m、(–500，1 800)m、(–200，2 000)m、(–50，1 500)m、(–200，1 000)m、(–250，100)m、(200，1 000)m、（500，300）m、（500，–250）m、（300，–200）m、（0，100）m、（–300，–600）m、（–600，–1 000）m。13個(gè)計(jì)劃航路點(diǎn)見(jiàn)圖2中A～M所示。三次樣條插值軌跡產(chǎn)生方法中設(shè)置Ud(t)=2.5 kn、ω(tk)=0.001，計(jì)劃航路見(jiàn)圖2，軌跡跟蹤變化曲線見(jiàn)圖3，軌跡跟蹤偏差變化曲線見(jiàn)圖4，航速變化曲線見(jiàn)圖5，指令航向和實(shí)際航向變化曲線見(jiàn)圖6，操舵變化曲線見(jiàn)圖7。

圖2 計(jì)劃航路

圖3 軌跡跟蹤變化曲線

圖4 軌跡偏差變化曲線

圖5 航速變化曲線

圖6 航向角變化曲線

圖7 舵角變化曲線

由圖可以看出，利用三次樣條插值法規(guī)劃的軌跡連續(xù)平滑，有利于 AUV的軌跡跟蹤控制。AUV可實(shí)時(shí)跟蹤規(guī)劃軌跡航行，表明提出的軌跡跟蹤算法可以實(shí)現(xiàn)軌跡跟蹤。在軌跡跟蹤過(guò)程中，AUV的法向跟蹤偏差最大不超過(guò)12 m，表明利用LOS導(dǎo)引法的運(yùn)動(dòng)控制算法合理可行。

軌跡跟蹤過(guò)程中，軌跡縱向跟蹤偏差有較大的負(fù)值，即AUV沿軌跡航行時(shí)可超過(guò)實(shí)時(shí)軌跡，而有時(shí)AUV也會(huì)落后于實(shí)時(shí)規(guī)劃軌跡位置。由于AUV的航速控制算法采用了基于軌跡偏差的比例控制算法，前期逼近并追蹤規(guī)劃軌跡的過(guò)程中，航速調(diào)節(jié)過(guò)高，使得AUV超越了規(guī)劃軌跡達(dá)190 m。此后，隨著航速的不斷調(diào)節(jié)，AUV與實(shí)時(shí)規(guī)劃軌跡間的縱向距離逐漸縮小，并保持在可接受的范圍內(nèi)。這表明提出的航速控制算法雖可滿(mǎn)足軌跡跟蹤要求，但仍有較大的改進(jìn)空間。仿真過(guò)程中，實(shí)際航向可實(shí)時(shí)跟蹤指令航向角。

4 結(jié)論

本文針對(duì)AUV水下定深軌跡跟蹤問(wèn)題，首先基于航路點(diǎn)計(jì)劃，利用三次樣條插值方法規(guī)劃了預(yù)定航行軌跡，隨后基于LOS導(dǎo)引法，將軌跡跟蹤問(wèn)題轉(zhuǎn)換為AUV的航速和航向跟蹤問(wèn)題，設(shè)計(jì)了一種航速、航向跟蹤控制算法，然后，基于AUV水平面運(yùn)動(dòng)方程，設(shè)計(jì)了一種帶有舵角舵速約束的離散時(shí)間航向模型預(yù)測(cè)控制方法，實(shí)現(xiàn)了AUV的軌跡跟蹤控制。仿真分析表明：（1）提出的軌跡跟蹤控制方法可實(shí)現(xiàn)AUV軌跡精確跟蹤控制，控制效果好，操控品質(zhì)高；（2）文中設(shè)計(jì)的LOS制導(dǎo)算法計(jì)算簡(jiǎn)潔，有較高的工程實(shí)用價(jià)值；（3）文中提出的基于 Laguerre函數(shù)的帶有舵角舵速約束的航向MPC控制算法，利用Laguerre網(wǎng)絡(luò)逼近方法和 Hildreth二次規(guī)劃方法，大幅降低了一般MPC算法的沉重計(jì)算負(fù)擔(dān)，有利于工程實(shí)現(xiàn)。