魏亞麗 朱大奇 褚振忠

(上海海事大學(xué)智能海事搜救與水下機(jī)器人上海工程技術(shù)研究中心 上海 201306)

0 引 言

水下動(dòng)態(tài)目標(biāo)跟蹤技術(shù)在海洋資源勘探、水下工程作業(yè)、海戰(zhàn)場(chǎng)監(jiān)視及水下精確制導(dǎo)等方面具有廣泛的應(yīng)用前景。近年來(lái)，水下動(dòng)態(tài)目標(biāo)跟蹤問(wèn)題研究得到了一定的發(fā)展，在聲納圖像處理及動(dòng)態(tài)目標(biāo)定位方面提出了很多方法。文獻(xiàn)[1]提出了基于聲納圖像的高斯混合基數(shù)化概率假設(shè)密度(Gaussian mixture cardinality probability hypothesis density，GM-CPHD)濾波算法，更有效地實(shí)現(xiàn)了目標(biāo)狀態(tài)及數(shù)目的估計(jì)。文獻(xiàn)[2]提出了基于魯棒粒子濾波的被動(dòng)聲納多目標(biāo)跟蹤算法，該算法能夠在保持多目標(biāo)連續(xù)跟蹤的同時(shí)跟蹤未知的時(shí)變多目標(biāo)，提高了水下目標(biāo)跟蹤的準(zhǔn)確性。文獻(xiàn)[3]針對(duì)主動(dòng)聲納測(cè)量誤差引入的目標(biāo)機(jī)動(dòng)決策模糊問(wèn)題，提出了一種基于機(jī)動(dòng)檢測(cè)延遲的跟蹤濾波算法，提高了聲納圖像目標(biāo)定位的精度。文獻(xiàn)[4]研究了基于交互式多模型的水下動(dòng)態(tài)目標(biāo)跟蹤問(wèn)題，在動(dòng)態(tài)目標(biāo)軌跡預(yù)測(cè)中具有較好的實(shí)時(shí)性與可靠性。以上相關(guān)研究成果大多關(guān)注于水下動(dòng)態(tài)目標(biāo)識(shí)別與預(yù)測(cè)問(wèn)題，而對(duì)動(dòng)態(tài)目標(biāo)軌跡跟蹤控制這一核心問(wèn)題較少研究。

本文研究基于水下機(jī)器人的動(dòng)態(tài)目標(biāo)跟蹤控制問(wèn)題，利用前視聲納圖像對(duì)動(dòng)態(tài)目標(biāo)進(jìn)行實(shí)時(shí)定位，并通過(guò)無(wú)跡卡爾曼濾波器預(yù)測(cè)動(dòng)態(tài)目標(biāo)軌跡；在此基礎(chǔ)上，提出一種基于模型預(yù)測(cè)控制(model predictive control，MPC)的水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制方法。最后，通過(guò)水池實(shí)驗(yàn)與仿真分析對(duì)所提方法進(jìn)行有效性驗(yàn)證。

1 水下動(dòng)態(tài)目標(biāo)跟蹤系統(tǒng)

本文探討結(jié)合多波束前視聲納的水下機(jī)器人的動(dòng)態(tài)目標(biāo)跟蹤控制問(wèn)題，所構(gòu)建的水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤系統(tǒng)主要由水下目標(biāo)定位與軌跡預(yù)測(cè)以及水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制2個(gè)部分組成。

(1)水下目標(biāo)定位與軌跡預(yù)測(cè)

基于多波束前視聲納實(shí)時(shí)提供的水下動(dòng)態(tài)目標(biāo)圖像，通過(guò)開(kāi)運(yùn)算、閾值分割和特征處理等方式，完成水下目標(biāo)識(shí)別與定位?？紤]到擬跟蹤目標(biāo)是處于動(dòng)態(tài)變化中的，本文構(gòu)建無(wú)跡卡爾曼濾波器，通過(guò)時(shí)間窗滾動(dòng)的方式預(yù)測(cè)下一時(shí)刻的水下動(dòng)態(tài)目標(biāo)軌跡信息。

(2)水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制

在預(yù)測(cè)得到下一時(shí)刻的目標(biāo)位置后，設(shè)計(jì)控制器使水下機(jī)器人跟蹤預(yù)測(cè)目標(biāo)位置運(yùn)動(dòng)。為此，本文利用模型預(yù)測(cè)控制算法，經(jīng)過(guò)在線(xiàn)滾動(dòng)優(yōu)化和反饋校正，產(chǎn)生水下機(jī)器人期望的速度與期望航向角信息。通過(guò)跟蹤該期望速度與航向角，最終完成水下動(dòng)態(tài)目標(biāo)的跟蹤控制。

2 水下目標(biāo)定位與軌跡預(yù)測(cè)

如圖1所示，多波速聲納采集到的水下動(dòng)態(tài)目標(biāo)圖像是以亮斑形態(tài)出現(xiàn)的[5]。除擬跟蹤的動(dòng)態(tài)目標(biāo)之外，還存在魚(yú)、海藻、垃圾等其他物體。在濾波去噪的基礎(chǔ)上，還要進(jìn)行形態(tài)學(xué)處理以去除小的干擾物。本文利用開(kāi)運(yùn)算的效果除去圖像中的小目標(biāo)而保持大目標(biāo)的形狀和大小不變[6]。其定義如下

(fοq)(x,y)=(fΘq)⊕q

(1)

其中，f為二值圖像原圖，q為結(jié)構(gòu)元素，(x，y)為結(jié)構(gòu)元素所在的二值圖像中的連通域，即對(duì)圖像先進(jìn)行腐蝕操作，然后進(jìn)行膨脹操作的運(yùn)算過(guò)程[7]。

圖1為一幅水下目標(biāo)的聲納圖像，圖像中除擬跟蹤動(dòng)態(tài)目標(biāo)之外還存在較多小的亮斑區(qū)域。當(dāng)外部干擾較多時(shí)，亮斑會(huì)更多[8]。本文結(jié)合所采用的多波束聲納特性，提出了一種閾值分割和特征提取相結(jié)合的方法。針對(duì)聲納的閾值分割問(wèn)題，設(shè)置閾值T1、T2、T3，閾值分割方法如式(2)所示。若閾值小于T3這個(gè)大小的區(qū)域則自動(dòng)填充為黑色，而閾值在T1到T2之間的區(qū)域則保留下來(lái)。

圖1 水下動(dòng)態(tài)目標(biāo)聲納圖像

ck={0,|l>T1,l

(2)

其中ck為閾值分割后的保留區(qū)域，l為被檢測(cè)位置的閾值大小。

考慮到聲納可視區(qū)域內(nèi)可能會(huì)存在除目標(biāo)之外的雜物和其他干擾信息，本文還引入了特征提取，其目的在于對(duì)亮斑形狀進(jìn)行提取。對(duì)于滿(mǎn)足擬跟蹤目標(biāo)形狀的亮斑，在滿(mǎn)足閾值范圍的前提下選擇出來(lái)并標(biāo)記出重心位置。標(biāo)記出目標(biāo)位置后，計(jì)算目標(biāo)在聲納二值圖像中的坐標(biāo)位置。假設(shè)目標(biāo)位置為(X,Y)，同時(shí)計(jì)算聲納二值圖像的大小，設(shè)其為(N,M)。根據(jù)聲納反饋數(shù)據(jù)可知實(shí)際探測(cè)最大距離為r，而圖像中點(diǎn)和扇形中點(diǎn)是同一位置。在二值圖像中點(diǎn)的縱向坐標(biāo)值為M/2，實(shí)際位置縱向坐標(biāo)為r/2，因此實(shí)際的位置坐標(biāo)與聲納二值圖的坐標(biāo)比例為r/M。于是，可知任意位置二值圖像坐標(biāo)(X,Y)轉(zhuǎn)換到實(shí)際坐標(biāo)系中的大小為(X×r/M,Y×r/M)。

由于擬跟蹤目標(biāo)位置是動(dòng)態(tài)變化的，若直接以當(dāng)前時(shí)刻的目標(biāo)位置為參考點(diǎn)進(jìn)行跟蹤控制，則很難完成。為此，在獲得水下動(dòng)態(tài)目標(biāo)當(dāng)前時(shí)刻的位置信息后，本文利用無(wú)跡卡爾曼濾波器進(jìn)行目標(biāo)位置預(yù)測(cè)。由于目標(biāo)運(yùn)動(dòng)是非線(xiàn)性的，普通卡爾曼濾波是對(duì)非線(xiàn)性的系統(tǒng)方程或者觀測(cè)方程進(jìn)行泰勒展開(kāi)，這樣不可避免地引入了線(xiàn)性化誤差[9]。而無(wú)跡卡爾曼濾波摒棄了對(duì)非線(xiàn)性函數(shù)線(xiàn)性化的傳統(tǒng)做法，對(duì)于一步預(yù)測(cè)方程使用無(wú)跡變換來(lái)處理均值與協(xié)方差的非線(xiàn)性傳遞問(wèn)題[10]，提高了估計(jì)的精度，穩(wěn)定性更好。本文所構(gòu)建的動(dòng)態(tài)目標(biāo)軌跡預(yù)測(cè)系統(tǒng)的輸入是每個(gè)當(dāng)前時(shí)刻的動(dòng)態(tài)目標(biāo)的位置信息，其輸出是經(jīng)過(guò)無(wú)跡卡爾曼濾波器預(yù)測(cè)得到的下一時(shí)刻的動(dòng)態(tài)目標(biāo)的軌跡信息，用來(lái)作為水下機(jī)器人動(dòng)態(tài)跟蹤的設(shè)定值。

3 水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制

模型預(yù)測(cè)控制算法是現(xiàn)在應(yīng)用范圍廣泛的控制算法。該算法對(duì)模型的精度要求不高，建模方便，采用了非最小化描述模型，系統(tǒng)的魯棒性、穩(wěn)定性較好。此外該算法能有效地處理多變量多約束的問(wèn)題，這些特別適合航行速度較慢、模型精確度不高的水下機(jī)器人系統(tǒng)[10]。模型預(yù)測(cè)控制算法實(shí)現(xiàn)時(shí)有3個(gè)關(guān)鍵步驟，分別是預(yù)測(cè)模型、滾動(dòng)優(yōu)化和反饋校正[11]。

通過(guò)無(wú)跡卡爾曼預(yù)測(cè)得到水下動(dòng)態(tài)目標(biāo)下一時(shí)刻的軌跡信息后，采用模型預(yù)測(cè)控制算法設(shè)計(jì)水下機(jī)器人的期望速度與期望航向角信息以實(shí)現(xiàn)動(dòng)態(tài)目標(biāo)跟蹤控制。本文構(gòu)建的水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制器如圖2所示?？刂破髦饕烧`差模型、系統(tǒng)約束以及目標(biāo)函數(shù)組成。誤差模型就是目標(biāo)跟蹤控制系統(tǒng)的數(shù)學(xué)描述，也是構(gòu)建控制算法的基礎(chǔ)[12]。本文中的誤差模型就是實(shí)時(shí)控制的水下機(jī)器人的位置與動(dòng)態(tài)目標(biāo)位置的差值。為了防止模型失配引起控制對(duì)理想追蹤的偏離，在新的采樣時(shí)刻，會(huì)先檢測(cè)誤差大小，并利用這一信息對(duì)模型的預(yù)測(cè)結(jié)果進(jìn)行修正然后再進(jìn)行新的優(yōu)化，這也就是反饋校正的過(guò)程[13]。系統(tǒng)約束包括水下機(jī)器人的速度約束、控制量平滑約束以及穩(wěn)定性約束。目標(biāo)函數(shù)的設(shè)計(jì)則綜合考慮目標(biāo)跟蹤的快速性與平穩(wěn)性。并且模型預(yù)測(cè)的控制優(yōu)化不是一次離線(xiàn)進(jìn)行的，而是反復(fù)在線(xiàn)進(jìn)行的滾動(dòng)優(yōu)化[14]。

圖2 水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制器

3.1 水下機(jī)器人模型

假設(shè)水下機(jī)器人的重心與浮心距離夠遠(yuǎn)，則可忽略水下機(jī)器人的縱傾運(yùn)動(dòng)與橫傾運(yùn)動(dòng)。水下機(jī)器人慣性坐標(biāo)系與載體坐標(biāo)系下運(yùn)動(dòng)狀態(tài)向量分別為η=[x,y,z,ψ]T和v=[u,v,w,r]T，其中x、y、z、ψ分別表示水下機(jī)器人在大地坐標(biāo)系下的x方向位置、y方向位置、z方向位置和艏向角，u、v、w是線(xiàn)速度矢量在載體坐標(biāo)系中的3個(gè)分量。r是角速度矢量在載體坐標(biāo)系中的一個(gè)分量。水下機(jī)器人的運(yùn)動(dòng)學(xué)方程可表示為

(3)

(4)

3.2 模型預(yù)測(cè)控制

為了能將該模型應(yīng)用于模型預(yù)測(cè)控制器的設(shè)計(jì)，需要對(duì)式(4)進(jìn)行離散化處理，得到：

(5)

在模型預(yù)測(cè)控制構(gòu)建的目標(biāo)函數(shù)中，需要計(jì)算未來(lái)一段時(shí)間系統(tǒng)的輸出[15]。因此，對(duì)式(5)進(jìn)行變換，得到：

(6)

從而，可進(jìn)一步得到新的狀態(tài)空間表達(dá)式為

(7)

(8)

為了簡(jiǎn)化后續(xù)的分析計(jì)算，假設(shè)：

Ak,τ=At,t,k=1,…,t+Ν-1

Bk,τ=Bt,t,k=1,…,t+Ν-1

則，推導(dǎo)可得到預(yù)測(cè)的輸出為

Y(t)=Ψtξ(t|t)+ΘtΔV(t)

(9)

其中，

Θt=

式(9)中，預(yù)測(cè)的狀態(tài)量和輸出量都可以通過(guò)當(dāng)前狀態(tài)量和控制增量計(jì)算得到。為實(shí)施模型預(yù)測(cè)控制，需構(gòu)建二次規(guī)劃優(yōu)化問(wèn)題[16]。

在實(shí)際的水下動(dòng)態(tài)目標(biāo)跟蹤過(guò)程中，由于水下機(jī)器人動(dòng)力系統(tǒng)對(duì)于控制量跳變要求比較嚴(yán)格，因此本文將控制增量作為目標(biāo)函數(shù)的狀態(tài)量，所構(gòu)建的優(yōu)化目標(biāo)函數(shù)為

(10)

其中，Np為預(yù)測(cè)時(shí)域，Nc為控制時(shí)域。

式(10)中，第1項(xiàng)反映了水下機(jī)器人對(duì)于動(dòng)態(tài)目標(biāo)的跟蹤能力，第2項(xiàng)反映了對(duì)控制量平穩(wěn)變化的要求。Q和R是權(quán)重矩陣。另外，在實(shí)際的控制系統(tǒng)中往往還需要滿(mǎn)足系統(tǒng)一定的約束條件，具體為

控制量約束：

νmin(t+k)≤ν(t+k)≤νmax(t+k)

(11)

控制增量約束：

Δνmin(t+k)≤Δν(t+k)≤Δνmax(t+k)

(12)

其中，k=0,…,Nc-1。

將式(7)代入式(10)，可得完整形式的目標(biāo)函數(shù)：

J(ξ(t),ν(t-1),ΔV(t))

=ΔV(t)HtΔV(t)T+GtΔV(t)T

(13)

其中，

因此，模型預(yù)測(cè)控制在每一步的帶約束優(yōu)化求解問(wèn)題都等價(jià)于求解如下的二次規(guī)劃問(wèn)題：

k=t,…,t+Hc-1

ΔVmin≤ΔV(k)≤ΔVmax，k=t,…,t+Hc-1

(14)

在每個(gè)控制周期完成對(duì)式(14)的求解后，得到了控制時(shí)域內(nèi)的一系列控制輸入增量。本文所構(gòu)建的目標(biāo)函數(shù)是一個(gè)典型的二次規(guī)劃優(yōu)化問(wèn)題，且具有線(xiàn)性與非線(xiàn)性約束條件。在每個(gè)控制周期內(nèi)完成對(duì)目標(biāo)函數(shù)的求解后，利用得到的控制時(shí)域內(nèi)的一系列控制輸入增量。根據(jù)模型預(yù)測(cè)控制的基本原理，將該控制序列中第一個(gè)元素作為實(shí)際的控制輸入增量作用于系統(tǒng)，系統(tǒng)執(zhí)行這一控制量直到下一時(shí)刻，循環(huán)往復(fù)，直至系統(tǒng)完成動(dòng)態(tài)目標(biāo)跟蹤控制。

4 實(shí)驗(yàn)與仿真

4.1 水下目標(biāo)定位與軌跡預(yù)測(cè)實(shí)驗(yàn)

為驗(yàn)證本文所設(shè)計(jì)的水下動(dòng)態(tài)目標(biāo)定位與軌跡預(yù)測(cè)系統(tǒng)的有效性，利用多波束前視聲納在校內(nèi)河流中進(jìn)行了實(shí)驗(yàn)。實(shí)驗(yàn)中水下動(dòng)態(tài)目標(biāo)采用遙控水下機(jī)器人進(jìn)行模擬，機(jī)器人outland1000(ROV)尺寸為0.65 m×0.37 m×0.26 m。圖3是outland1000的外觀圖，圖4為校內(nèi)實(shí)驗(yàn)場(chǎng)景圖。

圖3 目標(biāo)機(jī)器人實(shí)物圖

圖4 校內(nèi)實(shí)驗(yàn)場(chǎng)景圖

多波束前視聲納獲取圖像后上傳至計(jì)算機(jī)。圖5(a)為聲納原始圖像，可以看出圖中有很多的噪聲點(diǎn)和其他干擾物。圖5(b)是灰度化和濾波后的圖像。圖5(c)是形態(tài)學(xué)操作和閾值處理后的圖像，可以看出小的物體已經(jīng)被去除，只有較大的亮斑被保留。根據(jù)跟蹤目標(biāo)的亮斑區(qū)域大小，將閾值分別設(shè)置為2 000(T1)、4900(T2)和420(T3)。對(duì)于小于閾值T3的自動(dòng)填充為黑色，同時(shí)把介于T1和T2間的區(qū)域保留下來(lái)。圖5(d)是特征提取部分。由于模擬的水下動(dòng)態(tài)目標(biāo)(ROV水下機(jī)器人)為一長(zhǎng)方形物體，因此通過(guò)判斷亮斑區(qū)域是否為長(zhǎng)方形，即可完成目標(biāo)識(shí)別。在找到長(zhǎng)方形亮斑區(qū)域后，標(biāo)記出輪廓重心在圖像區(qū)域中的位置。結(jié)合多波束前視聲納所反饋的探測(cè)范圍數(shù)據(jù)，可解算出動(dòng)態(tài)目標(biāo)相對(duì)于水下機(jī)器人的位置坐標(biāo)，即實(shí)現(xiàn)水下動(dòng)態(tài)目標(biāo)定位。

圖5 聲納圖像處理

本文的動(dòng)態(tài)目標(biāo)運(yùn)動(dòng)是非線(xiàn)性的，無(wú)跡卡爾曼使用無(wú)跡變換來(lái)處理均值和協(xié)方差的非線(xiàn)性傳遞，是對(duì)非線(xiàn)性函數(shù)的概率密度分布進(jìn)行近似，用一系列確定樣本來(lái)逼近狀態(tài)的后驗(yàn)概率密度，而不是對(duì)非線(xiàn)性函數(shù)進(jìn)行近似，不需要求導(dǎo)計(jì)算雅可比矩陣，沒(méi)有線(xiàn)性化忽略高階項(xiàng)，因此無(wú)跡卡爾曼非線(xiàn)性分布統(tǒng)計(jì)量的計(jì)算精度較高。表1是無(wú)跡卡爾曼濾波及卡爾曼濾波預(yù)測(cè)出的動(dòng)態(tài)目標(biāo)位置和真實(shí)位置的坐標(biāo)，真實(shí)位置數(shù)據(jù)是在水池實(shí)驗(yàn)中采集的。圖6是2種預(yù)測(cè)算法的軌跡誤差對(duì)比圖，其中虛線(xiàn)為卡爾曼預(yù)測(cè)結(jié)果，實(shí)線(xiàn)為無(wú)跡卡爾曼預(yù)測(cè)結(jié)果。從數(shù)據(jù)和誤差圖中可以明顯看出無(wú)跡卡爾曼的預(yù)測(cè)更加準(zhǔn)確。其中卡爾曼預(yù)測(cè)x軸的最大誤差值是2.03 m，平均誤差值為0.995 m。而無(wú)跡卡爾曼預(yù)測(cè)的x軸最大誤差為0.62 m，平均誤差為0.31 m。卡爾曼預(yù)測(cè)y軸坐標(biāo)最大誤差為1.96 m，平均誤差為1.063 m。無(wú)跡卡爾曼預(yù)測(cè)y軸最大誤差為0.37 m，平均誤差為0.22 m?？柭A(yù)測(cè)z軸最大誤差值為2.87 m，平均誤差值為1.356 m。無(wú)跡卡爾曼預(yù)測(cè)z軸最大誤差值為1.27 m，平均誤差為0.39 m。圖7是目標(biāo)預(yù)測(cè)結(jié)果對(duì)比圖，標(biāo)注△的曲線(xiàn)為實(shí)際坐標(biāo)位置，標(biāo)注?的曲線(xiàn)為卡爾曼預(yù)測(cè)位置，標(biāo)注▽的曲線(xiàn)為無(wú)跡卡爾曼預(yù)測(cè)位置。從圖中可以證明出無(wú)跡卡爾曼預(yù)測(cè)數(shù)據(jù)比卡爾曼預(yù)測(cè)穩(wěn)定性好，估計(jì)精度高。上述實(shí)驗(yàn)中，雖然遙控水下機(jī)器人模擬的動(dòng)態(tài)目標(biāo)運(yùn)動(dòng)具有很大的不確定性，但所設(shè)計(jì)的目標(biāo)定位與軌跡預(yù)測(cè)系統(tǒng)能夠獲得較好的精度，為水下動(dòng)態(tài)目標(biāo)的跟蹤控制提供了保證。

表1 真實(shí)數(shù)據(jù)與預(yù)測(cè)數(shù)據(jù)

圖6 軌跡預(yù)測(cè)誤差對(duì)比

圖7 目標(biāo)預(yù)測(cè)結(jié)果對(duì)比

4.2 水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制仿真

為驗(yàn)證所提基于模型預(yù)測(cè)控制的水下機(jī)器人動(dòng)態(tài)跟蹤控制方法的有效性，分別在2維環(huán)境及3維環(huán)境下進(jìn)行了仿真驗(yàn)證。

在水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制中，將動(dòng)態(tài)目標(biāo)的預(yù)測(cè)軌跡點(diǎn)作為期望跟蹤點(diǎn)。模型的誤差為水下機(jī)器人追蹤位置與動(dòng)態(tài)目標(biāo)實(shí)際位置的差值。速度控制約束為[-2 m/s, 2 m/s]，角速度約束為[-0.7 rad/s, 0.7 rad/s]。模型預(yù)測(cè)的參數(shù)Q=[1 0 0 0;0 1 0 0,0 0 1 0;0 0 0 0.5]，R=[0.1 0 0 0;0 0.1 0 0;0 0 0.1 0;0 0 0 0.1]。

圖8為2維環(huán)境下模型預(yù)測(cè)控制與反步控制的仿真對(duì)比結(jié)果，2維起始點(diǎn)為(9,15)，從仿真結(jié)果可以看出2種控制算法都能達(dá)到跟蹤動(dòng)態(tài)目標(biāo)的效果。模型預(yù)測(cè)的跟蹤效果與反步控制相比，控制精度高，更加穩(wěn)定。圖9為2種控制算法的誤差對(duì)比圖，從圖中也可以看出，反步控制的誤差要比模型預(yù)測(cè)控制的大很多，穩(wěn)定性也要差一些。由于反步控制算法中沒(méi)有加入速度變化約束，所以導(dǎo)致速度變化很大，造成跟蹤效果的不穩(wěn)定。與反步相比，模型預(yù)測(cè)控制算法加入了速度變化約束，使速度的變化量控制在正負(fù)2 m/s之間,進(jìn)而提高了跟蹤的穩(wěn)定性。圖10為模型預(yù)測(cè)控制的控制輸出量，從圖中可以看出，模型預(yù)測(cè)控制的速度輸出量都在速度變化約束之間。

圖8 2維跟蹤對(duì)比結(jié)果

圖9 2維跟蹤誤差

圖10 2維模型預(yù)測(cè)控制輸出

圖11為3維環(huán)境下基于模型預(yù)測(cè)控制與反步控制的仿真對(duì)比結(jié)果。3維起始點(diǎn)為(5,13,9)，從仿真圖可以看出，無(wú)論是模型預(yù)測(cè)控制還是反步控制的跟蹤趨勢(shì)都是符合動(dòng)態(tài)目標(biāo)的運(yùn)動(dòng)趨勢(shì)的。但是模型預(yù)測(cè)控制的跟蹤效果與2維情況相同，明顯比反步控制的跟蹤控制精度高，更加穩(wěn)定。圖12為3維環(huán)境下的動(dòng)態(tài)目標(biāo)跟蹤誤差，即水下機(jī)器人實(shí)際軌跡與動(dòng)態(tài)目標(biāo)預(yù)測(cè)軌跡的偏差。表2是2種控制算法在3維情況下不同坐標(biāo)軸的誤差數(shù)據(jù)。從表中可以看出，模型預(yù)測(cè)控制算法在每個(gè)坐標(biāo)軸的誤差均值都要小于反步控制算法。由于反步控制算法中沒(méi)有加入速度變化約束，所以導(dǎo)致速度變化很大，造成跟蹤效果的不穩(wěn)定。與反步相比，模型預(yù)測(cè)控制算法加入了速度變化約束，使速度的變化量控制在±2 m/s之間，進(jìn)而提高了跟蹤的穩(wěn)定性。圖13為3維環(huán)境下模型預(yù)測(cè)控制算法產(chǎn)生的控制量，從圖中可以看出水下機(jī)器人的期望速度與期望角速度均在設(shè)定的約束范圍內(nèi)。正因如此，模型預(yù)測(cè)控制算法的速度變化不會(huì)像反步那么劇烈，并且模型預(yù)測(cè)控制對(duì)于水下機(jī)器人的模型要求不高，所以模型預(yù)測(cè)控制算法的跟蹤精確度和穩(wěn)定性比反步算法高。以下的仿真結(jié)果均能證明，所提出的模型預(yù)測(cè)控制算法具有較好的跟蹤控制能力，在水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制中是可行的。

圖11 3維跟蹤對(duì)比結(jié)果

圖12 3維跟蹤誤差

表2 跟蹤誤差均值

圖13 3維模型預(yù)測(cè)控制輸出速度

5 結(jié) 論

本文探討了水下動(dòng)態(tài)目標(biāo)的定位與軌跡預(yù)測(cè)方法，提出了一種基于模型預(yù)測(cè)控制的水下機(jī)器人動(dòng)態(tài)目標(biāo)跟蹤控制方法?；诙嗖ㄊ耙暵暭{的實(shí)驗(yàn)結(jié)果表明，所提方法在水下動(dòng)態(tài)目標(biāo)的識(shí)別、定位與軌跡預(yù)測(cè)中具有較高的精度。但是本文只在運(yùn)動(dòng)學(xué)模型中進(jìn)行了仿真實(shí)驗(yàn)，還未擴(kuò)展到動(dòng)力學(xué)模型中進(jìn)行實(shí)驗(yàn)，模型預(yù)測(cè)控制的優(yōu)化算法還可以作進(jìn)一步的改進(jìn)，使跟蹤效果更加精確，這也是下一步需要研究的工作。