魏亞麗 朱大奇 褚振忠

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

0 引 言

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

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

1 水下動態(tài)目標跟蹤系統(tǒng)

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

(1)水下目標定位與軌跡預測

基于多波束前視聲納實時提供的水下動態(tài)目標圖像，通過開運算、閾值分割和特征處理等方式，完成水下目標識別與定位。考慮到擬跟蹤目標是處于動態(tài)變化中的，本文構(gòu)建無跡卡爾曼濾波器，通過時間窗滾動的方式預測下一時刻的水下動態(tài)目標軌跡信息。

(2)水下機器人動態(tài)目標跟蹤控制

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

2 水下目標定位與軌跡預測

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

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

(1)

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

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

圖1 水下動態(tài)目標聲納圖像

ck={0,|l>T1,l

(2)

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

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

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

3 水下機器人動態(tài)目標跟蹤控制

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

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

圖2 水下機器人動態(tài)目標跟蹤控制器

3.1 水下機器人模型

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

(3)

(4)

3.2 模型預測控制

為了能將該模型應用于模型預測控制器的設計，需要對式(4)進行離散化處理，得到：

(5)

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

(6)

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

(7)

(8)

為了簡化后續(xù)的分析計算，假設：

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

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

則，推導可得到預測的輸出為

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

(9)

其中，

Θt=

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

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

(10)

其中，Np為預測時域，Nc為控制時域。

式(10)中，第1項反映了水下機器人對于動態(tài)目標的跟蹤能力，第2項反映了對控制量平穩(wěn)變化的要求。Q和R是權(quán)重矩陣。另外，在實際的控制系統(tǒng)中往往還需要滿足系統(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)，可得完整形式的目標函數(shù)：

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

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

(13)

其中，

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

k=t,…,t+Hc-1

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

(14)

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

4 實驗與仿真

4.1 水下目標定位與軌跡預測實驗

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

圖3 目標機器人實物圖

圖4 校內(nèi)實驗場景圖

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

圖5 聲納圖像處理

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

表1 真實數(shù)據(jù)與預測數(shù)據(jù)

圖6 軌跡預測誤差對比

圖7 目標預測結(jié)果對比

4.2 水下機器人動態(tài)目標跟蹤控制仿真

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

在水下機器人動態(tài)目標跟蹤控制中，將動態(tài)目標的預測軌跡點作為期望跟蹤點。模型的誤差為水下機器人追蹤位置與動態(tài)目標實際位置的差值。速度控制約束為[-2 m/s, 2 m/s]，角速度約束為[-0.7 rad/s, 0.7 rad/s]。模型預測的參數(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)境下模型預測控制與反步控制的仿真對比結(jié)果，2維起始點為(9,15)，從仿真結(jié)果可以看出2種控制算法都能達到跟蹤動態(tài)目標的效果。模型預測的跟蹤效果與反步控制相比，控制精度高，更加穩(wěn)定。圖9為2種控制算法的誤差對比圖，從圖中也可以看出，反步控制的誤差要比模型預測控制的大很多，穩(wěn)定性也要差一些。由于反步控制算法中沒有加入速度變化約束，所以導致速度變化很大，造成跟蹤效果的不穩(wěn)定。與反步相比，模型預測控制算法加入了速度變化約束，使速度的變化量控制在正負2 m/s之間,進而提高了跟蹤的穩(wěn)定性。圖10為模型預測控制的控制輸出量，從圖中可以看出，模型預測控制的速度輸出量都在速度變化約束之間。

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

圖9 2維跟蹤誤差

圖10 2維模型預測控制輸出

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

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

圖12 3維跟蹤誤差

表2 跟蹤誤差均值

圖13 3維模型預測控制輸出速度

5 結(jié) 論

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