嚴(yán)浙平,郝 悅,王千一,邊信黔

(哈爾濱工程大學(xué)自動化學(xué)院,哈爾濱 150001)

?

基于變維卡爾曼濾波的UUV目標(biāo)運動優(yōu)化估計方法

嚴(yán)浙平*,郝 悅,王千一,邊信黔

(哈爾濱工程大學(xué)自動化學(xué)院,哈爾濱 150001)

為實現(xiàn)無人水下航行器精準(zhǔn)、安全的跟蹤目標(biāo)從而高效地實現(xiàn)目標(biāo)的信息采集,提出了一種針對該個體的目標(biāo)運動優(yōu)化估計方法。此方法通過建立目標(biāo)運動模型,基于卡爾曼濾波基本原理,利用變維卡爾曼濾波的方法,達(dá)到UUV個體對目標(biāo)運動狀態(tài)準(zhǔn)確估計的預(yù)期效果。為提高該方法的可行性,在利用最小化目標(biāo)位置估計協(xié)方差矩陣完成目標(biāo)運動軌跡估計及優(yōu)化的過程中,考慮實際UUV機(jī)動時的最大速度限制、與目標(biāo)最小距離的限制。文章詳細(xì)闡述該方法的實現(xiàn)和步驟,并通過仿真實驗驗證了該優(yōu)化算法的有效性。

無人水下航行器;目標(biāo)運動估計;變維卡爾曼濾波方法;協(xié)方差矩陣最小化

無人水下航行器UUV(Unmanned Underwater Vehicle)在軍民領(lǐng)域有廣泛應(yīng)用[1]。水下目標(biāo)跟蹤作業(yè)是UUV自主性和智能性的重要體現(xiàn)。利用目標(biāo)跟蹤技術(shù)[2],通過數(shù)據(jù)融合、估計、計算并決策其運動行為的算法,最終可以為指揮控制系統(tǒng)的任務(wù)決策提供有效的信息[3]。

為了保障UUV的隱蔽性,一般采取不發(fā)射信號的無源傳感器來獲取目標(biāo)信息,其相對角度和時延使得跟蹤十分復(fù)雜,成為目標(biāo)跟蹤領(lǐng)域的難題之一[4]。目前,在這方面研究的較少[5-6],傳統(tǒng)的卡爾曼濾波方法不能夠滿足UUV機(jī)動時的速度要求和與目標(biāo)的距離要求。為解決這一難題,可以從提高單UUV目標(biāo)運動估計的精準(zhǔn)性和考慮UUV機(jī)動問題兩方面實現(xiàn)目標(biāo)運動軌跡的優(yōu)化工作,提高跟蹤的準(zhǔn)確性。

本文以典型目標(biāo)運動模型為理論基礎(chǔ),利用變維的卡爾曼濾波算法實現(xiàn)單UUV對目標(biāo)運動狀態(tài)的估計工作。最后,考慮實際UUV機(jī)動時的最大速度限制、與目標(biāo)最小距離的限制,提出利用最小化目標(biāo)位置估計協(xié)方差矩陣的方法優(yōu)化目標(biāo)運動軌跡,仿真結(jié)果表明該方法有效得減小了估計誤差。

1 UUV個體估計目標(biāo)運動狀態(tài)方法

運動目標(biāo)的狀態(tài)估計是人們(或各種裝具)依靠現(xiàn)代觀測和運算手段,實現(xiàn)對目標(biāo)運動特征做連續(xù)、可靠及精確估計的過程[7]。利用這一技術(shù)可以對實際環(huán)境中的運動目標(biāo)進(jìn)行跟蹤,估計并解算其運動狀態(tài)。目前常用的濾波方法主要有卡爾曼濾波、粒子濾波和灰色預(yù)測。其中,卡爾曼濾波是一種典型的層層遞推濾波算法,在每一時刻都可以由遞推方程解算出當(dāng)前的狀態(tài)估計值。它的典型特點是計算量與存儲量較小,具有很好的穩(wěn)定性。因此,本文選擇單UUV個體利用變維卡爾曼濾波方法進(jìn)行目標(biāo)運動估計。

1.1 卡爾曼濾波基本原理

設(shè)n維系統(tǒng)狀態(tài)方程和m維觀測方程分別為:X(k+1)=Φ(k)X(k)+Ψ(k)U(k)+Γ(k)W(k)

(1)

Z(k+1)=H(k)X(k)+V(k)

(2)

其中,Φ(k)表示狀態(tài)轉(zhuǎn)移矩陣;H(k)表示觀測矩陣;X(k)為狀態(tài)變量;Γ(k)為輸入矩陣;Z(k)為觀測變量;系統(tǒng)噪聲{W(k)}及觀測噪聲{V(k)}為互不相關(guān)的高斯白噪聲:

E[W(k)]=0,Cov(W(k),W(j))=Q(k)δ(kj)

E[V(k)]=0,Cov(V(k),V(j))=R(k)δ(kj)

Cov(W(k),V(j))=0

卡爾曼濾波的基本算法為[8]:

①根據(jù)上一次的濾波值進(jìn)行狀態(tài)的預(yù)報:

(3)

②根據(jù)一次濾波的誤差值進(jìn)行其預(yù)報誤差的計算:

M(k)=Φ(k-1)P(k-1)ΦT(k-1)+
Γ(k-1)Q(k-1)ΓT(k-1)

(4)

③卡爾曼濾波增益矩陣的計算:

K(k)=M(k)HT(k)[H(k)M(k)HT(k)+R(k)]-1

(5)

④狀態(tài)估計的計算:

(6)

⑤根據(jù)狀態(tài)估計誤差進(jìn)行協(xié)方差陣的計算:

P(k)=[I-K(k)H(k)]M(k)

(7)

本文的卡爾曼濾波以運動目標(biāo)模型[9-10]為理論基礎(chǔ),將t時刻時所有運動在二維平面的目標(biāo)狀態(tài)向量統(tǒng)一用式(8)表示。

X(t)=[x(t),v(t),a(t)]T

(8)

式中,x(t)表示t時刻時目標(biāo)的位移,v(t)表示t時刻時目標(biāo)的速度,a(t)表示t時刻時目標(biāo)的加速度。

1.1.1 常速(CV)模型

常速模型的加速度值為零,但是這在實際中是不可能出現(xiàn)的情況。因此,加速度一般被學(xué)者定義為服從均值為0的高斯白噪聲,其具體的表達(dá)式為:

(9)

當(dāng)目標(biāo)的狀態(tài)向量可以用X(t)=[x(t),v(t)]T表示時,其狀態(tài)方程為:

(10)

假定采樣間隔為T,其與式(3)對應(yīng)的離散時間動態(tài)方程可以表示為:

(11)

過程噪聲W的協(xié)方差Q為:

(12)

其中,σ為高斯白噪聲的方差。

1.1.2 常加速(CA)模型

常加速模型以目標(biāo)加速度是一個獨立增量的過程為前提,相應(yīng)狀態(tài)空間表達(dá)式為:

(13)

式中,系統(tǒng)矩陣A為:

假定采樣間隔為T,其與式(11)對應(yīng)的離散時間動態(tài)方程可以表示為:

X(k+1)=F(k)X(k)+W(k)

(14)

式中

過程噪聲W的協(xié)方差Q為:

1.2 基于變維卡爾曼濾波的目標(biāo)運動估計

變維卡爾曼濾波算法的中心思想體現(xiàn)在其含有兩個模型:低階的非機(jī)動模型和高階的機(jī)動模型。是否變維則需要通過機(jī)動檢測器來監(jiān)視并做出決策。這種機(jī)動判決的方法具有簡單、易于實現(xiàn)的特點正是變維卡爾曼濾波區(qū)別于其他卡爾曼濾波算法的顯著特征[11-12]。其具體的實現(xiàn)過程為:當(dāng)檢測到機(jī)動時,模型由非機(jī)動轉(zhuǎn)向為機(jī)動,退出機(jī)動時則會返回到正常的非機(jī)動模型。

本文的低階模型采用常速模型,高階模型采用常加速模型。目標(biāo)的初始狀態(tài)定義為常速模型。

①由常速模型切換到常加速模型行為

定義常速模型的輸出新息序列為v(k),令:

(15)

其中,0<α<1,S(k)是v(k)的協(xié)方差矩陣。

定義檢測機(jī)動的有效窗口長度為:

Δ=1/(1-α)

(16)

②由常加速模型切換到常速模型行為

令加速度估計的顯著性檢測的統(tǒng)計量為:

(17)

變維卡爾曼濾波算法流程圖如圖1所示。

圖1 變維卡爾曼算法流程圖

2 目標(biāo)運動估計優(yōu)化研究

考慮UUV與目標(biāo)之間在物理學(xué)上的限制條件,針對實際問題,本文研究并設(shè)計UUV的最優(yōu)機(jī)動策略,即最小化目標(biāo)位置估計的協(xié)方差矩陣。該方法利用多UUV數(shù)量上的優(yōu)勢完成數(shù)據(jù)信息的融合,以實現(xiàn)目標(biāo)運動軌跡估計的最優(yōu)化。

2.1 目標(biāo)運動估計優(yōu)化問題描述

假定UUV個體可以直接測量到目標(biāo)所處的位置,不能直接測量到目標(biāo)的運動方向和速度,但是目標(biāo)的全部狀態(tài)信息可以通過狀態(tài)估計技術(shù)獲得。由多個UUV獨立測量目標(biāo)狀態(tài)信息,作為系統(tǒng)整體可以獲得一系列的信息。同時,這些信息在個體間是共享的,從而能夠有效地提高目標(biāo)估計的準(zhǔn)確性。與此同時,在所有這些UUV個體中,選擇哪些數(shù)據(jù)信息(估計數(shù)據(jù)信息或者測量數(shù)據(jù)信息)用來共享是一件非常有意義的工作任務(wù),因為這項工作任務(wù)決定了多UUV編隊系統(tǒng)如何進(jìn)行數(shù)據(jù)融合。相比于共享測量信息,共享估計信息可以最大限度地提高目標(biāo)下一時刻位置預(yù)測度,但是這需要傳輸更多的數(shù)據(jù)信息。盡管編隊系統(tǒng)中每個個體的測量傳感器是相對獨立的,但是在航行過程中,每個個體所受到的噪聲干擾是共同的,因此它們實現(xiàn)的狀態(tài)估計是相關(guān)聯(lián)的,這同樣增加了數(shù)據(jù)融合的復(fù)雜度[13]。

圖2 UUV目標(biāo)估計所處位置案例圖

UUV所處位置對目標(biāo)運動估計的精度有著重大影響,以下面的案例為例:即利用一個攜帶距離傳感器(受高斯白噪聲干擾)的UUV跟蹤動目標(biāo)這一實際案例,具體如圖2所示。在這個案例示意圖中,點狀線包圍的環(huán)形區(qū)域表示傳感器測量目標(biāo)位置可能存在的不確定區(qū)域,橢圓實線包圍的區(qū)域表示上一時刻UUV估計的目標(biāo)可能所處的位置區(qū)域。在圖2(a)中,說明了如果UUV持續(xù)保持這種測量距離,基于這種唯一的測量模式,UUV會在很大程度上確定目標(biāo)處于橢圓虛線包圍的區(qū)域。相反,如果改變UUV位置對目標(biāo)位置進(jìn)行再次估計,被估計的目標(biāo)所處位置的不確定區(qū)域就會大大縮小(橢圓虛線包圍的區(qū)域明顯縮小),如圖2(b)所示。

因此,通過設(shè)計有效的算法來計算每個UUV所處的最優(yōu)位置是非常具有現(xiàn)實意義的。當(dāng)擴(kuò)展到多個UUV時,這種策略仍然適用。該算法的主要目標(biāo)是在考慮UUV機(jī)動限制(最大速度限制、與目標(biāo)的最小距離限制)的前提下,提出一種有效的算法計算出跟蹤機(jī)動目標(biāo)時,每個UUV所處的最優(yōu)位置,從而實現(xiàn)目標(biāo)預(yù)測位置區(qū)域的最小化。如果實現(xiàn)了目標(biāo)預(yù)測位置區(qū)域的最小化,提高了位置估計的優(yōu)化,也能間接地實現(xiàn)了所有目標(biāo)狀態(tài)的估計優(yōu)化工作,使每個UUV處于最佳的位置采集目標(biāo)信息。

UUV實現(xiàn)動目標(biāo)運動狀態(tài)估計的過程具體如圖3所示。在二維平面上,假設(shè)存在M個UUV個體,每個UUV的速度為vi,最大速度為vimax。在k+1時刻,UUVi的位置只可能在以k時刻的位置為圓心,vimaxT(T為采樣時間)為半徑的圓上。為避免與目標(biāo)相撞,定義安全半徑ρi。

圖3 UUV與動目標(biāo)機(jī)動圖

圖3中,Si為目標(biāo)與UUVi之間的距離。θi為目標(biāo)與UUVi之間的角度。

2.2 目標(biāo)運動估計優(yōu)化算法

P(k+1)=P(k)-K(k+1)S(k+1)KT(k+1)

(18)

其中:

S(k+1)=H(k+1)P(k)HT(k+1)+R

式(18)整理后:

P-1(k+1)=P-1(k)-HT(k+1)R-1H(k+1)

也可以表述為:

(19)

(20)

(21)

其中,si=C(-φ0)pi。

由式(20)可知,實現(xiàn)si的最小化即等同協(xié)方差矩陣P(·)最小化。定義目標(biāo)函數(shù)為:

(22)

而且,目標(biāo)函數(shù)的約束條件為:

(23)

為實現(xiàn)目標(biāo)估計的優(yōu)化,最簡單、直接的方法是單獨實現(xiàn)每一個UUV個體目標(biāo)函數(shù)的最小化。需要強(qiáng)調(diào)的是,在式(23)的約束條件下每一步都需要解決以下的優(yōu)化問題。

(24)

(25)

優(yōu)化過程將會一直持續(xù)直至到達(dá)允許的最大迭代次數(shù)或者目標(biāo)函數(shù)變化值小于1%,具體算法如下:

1:循環(huán)

2:fori=1 toMdo

6:end for

7:直至到達(dá)允許的最大迭代次數(shù)或者目標(biāo)函數(shù)變化值小于1%

3 算例仿真及驗證

UUV屬于典型的水中機(jī)動目標(biāo)。因此,對UUV的運動軌跡進(jìn)行估計處理可以驗證變維卡爾曼濾波用于水中目標(biāo)運動估計是可行的。為驗證估計算法的有效性,利用某次港灣實驗中UUV的GPS值(固定坐標(biāo)系下),對其進(jìn)行濾波估計處理。UUV運動時間為t=650 s,UUV初始位置、速度、航向分別為:((106.4,8.28)m,0.44 m/s,332.11°)。在東向上、北向上分別初始化其位置、速度及加速度,則在東向的初始運動狀態(tài)為:X(0)=[106.4,0.39,0.02]T,在北向的運動初始狀態(tài)為:X(0)=[8.28,0.21,0.12]T。利用變維卡爾曼濾波算法進(jìn)行目標(biāo)運動狀態(tài)估計的仿真結(jié)果如圖4～圖6所示。

圖4 動目標(biāo)實際運動軌跡及濾波估計后軌跡

圖5 東向濾波誤差

圖6 北向濾波誤差

從圖4可以看出,單UUV個體經(jīng)過變維卡爾曼濾波方法估計后的目標(biāo)軌跡曲線與實際的目標(biāo)軌跡曲線大體上重合,能夠進(jìn)行下一步的跟蹤工作。但是從圖5和圖6的濾波誤差來看,在轉(zhuǎn)彎處,估計誤差還是比較大的。東向濾波誤差范圍為[-1.58,1.14],北向濾波誤差的范圍為[-1.53,0.81]。

為充分說明優(yōu)化算法的有效性,變維卡爾曼濾波算法的各項參數(shù)不變。設(shè)置每個UUV的安全半徑為ρ=10 m。kdi=1。圖7為經(jīng)過優(yōu)化算法后的目標(biāo)估計軌跡仿真圖。

圖7 動目標(biāo)實際運動軌跡及濾波估計優(yōu)化后軌跡

圖8和圖9分別為在東向及北向的估計誤差仿真實驗圖。

圖8 東向濾波誤差

圖9 北向濾波誤差

通過圖8和圖9與圖3和圖4的對比,直觀上可以明顯顯示了無論是在東向還是在北向,濾波誤差值都相對減小。其中,在東向的濾波誤差范圍為[-0.78,0.70],在北向的誤差范圍為[-0.69,0.49]。算法優(yōu)化前后的誤差曲線在趨勢上大致相同,但是縱向坐標(biāo)長度優(yōu)化后縮小了1倍。這能夠說明利用協(xié)方差矩陣P(·)最小化的方法有效的減小了估計誤差,實現(xiàn)了目標(biāo)軌跡估計優(yōu)化這一任務(wù)目標(biāo)。

4 結(jié)論

本文提出了一種基于變維卡爾曼濾波的UUV目標(biāo)運動優(yōu)化估計方法。以目標(biāo)運動模型為基礎(chǔ),基于卡爾曼濾波基本原理,采用常速模型和常加速模型的切換實現(xiàn)高階模型和低階模型的切換,利用變維卡爾曼濾波的方法實現(xiàn)每個UUV個體對目標(biāo)運動狀態(tài)的估計。為提高該算法的實際可行性,在考慮UUV個體的最大速度限制、與目標(biāo)的最小距離限制的前提下,通過UUV個體與目標(biāo)估計距離的最小化實現(xiàn)協(xié)方差矩陣P(·)的最小化,以此優(yōu)化目標(biāo)運動的估計狀態(tài)。通過應(yīng)用優(yōu)化算法對其進(jìn)行濾波估計仿真實驗,實驗結(jié)果驗證了該優(yōu)化算法減小了基本卡爾曼濾波的估計誤差,提高了軌跡的準(zhǔn)確性和有效性,為下一步的UUV目標(biāo)跟蹤控制系統(tǒng)研發(fā)奠定了理論基礎(chǔ)。

[1] 嚴(yán)浙平,遲冬南,趙智,等. 基于支持向量聚類的UUV障礙物孤立點惰性檢測[J]. 哈爾濱工程大學(xué)學(xué)報,2012,33(10):1-7.

[2]張輝. 海洋環(huán)境下AUV目標(biāo)跟蹤控制研究[D]. 哈爾濱:哈爾濱工程大學(xué)自動化學(xué)院,2012.

[3]許志剛. 目標(biāo)跟蹤算法研究及其應(yīng)用[D]. 無錫:江南大學(xué)計算機(jī)學(xué)院,2011.

[4]彭冬亮,郭云飛,薛安克. 變周期RP-EKF時延純方位角目標(biāo)跟蹤算法[J]. 傳感技術(shù)學(xué)報,2009,22(2):224-229.

[5]苑艷華,李四海,南江. 基于卡爾曼濾波器的航姿系統(tǒng)測姿方法研究[J]. 傳感技術(shù)學(xué)報,2011,24(12):1718-1722.

[6]Zhu Rong,Sun Dong,Zhu Zhaoying. A Linear Fusion Algorithm for Attitude Determination Using Low Cost MEMS-Based Sensors[J]. Measurement,2007,40(3):322-328.

[7]梁彥,潘泉,楊峰,等. 復(fù)雜系統(tǒng)的現(xiàn)代估計理論及應(yīng)用[M]. 北京:科學(xué)出版社,2009:1-3.

[8]David Salmond. Target Tracking:Introduction and Kalman Tracking Filters[M]. USA:QinetiQItd,2001.

[9]張強(qiáng),孫紅勝,胡澤明. 基于新息協(xié)方差的機(jī)動目標(biāo)軌跡估計算法研究[J]. 信息工程大學(xué)學(xué)報,2012,13(6):730-735.

[10]Yin X,Noguchi N,Choi J. Development of a Target Recognition and Following System for a Field Robot[J]. Computers and Electronics in Agriculture,2013,98(1):17-24.

[11]Li S,Wang X. Finite-Time Consensus and Collision Avoidance Control Algorithms for Multiple AUVs[J]. Automatica,2013,49(11):3359-3367.

[12]董文麗,孔金生. 變維卡爾曼濾波算法在車輛導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 計算技術(shù)與自動化,2006,25(4):42-45.

[13]Zhou K. Active Sensing with Applications to Mobil Robotics[D]. USA:University of Minesota,2012.

嚴(yán)浙平(1972-),男,浙江龍游人,教授,博士生導(dǎo)師,分別于1994年、1997年、2001年在哈爾濱工程大學(xué)獲得本科、碩士、博士學(xué)位?，F(xiàn)任哈工程409教研室主任。目前主要從事多傳感器數(shù)據(jù)融合理論及其應(yīng)用、潛器與水下機(jī)器人控制技術(shù)、船舶運動綜合控制等方面的研究,yanzheping@hrbeu.edu.cn;

郝悅(1989-),女,黑龍江哈爾濱人,碩士研究生,于2008年在哈爾濱工程大學(xué)獲得學(xué)士學(xué)位,研究方向為自主水下航行器,792105630@qq.com;

王千一(1988-),女,遼寧大連人,碩士研究生,目前就讀于哈爾濱工程大學(xué)自動化學(xué)院,研究方向為自主水下航行器軟件系統(tǒng)測試,335687638@qq.com;

邊信黔(1941-),男,河北唐山人,教授,博士生導(dǎo)師,國家973計劃某項目技術(shù)首席專家,自動化學(xué)會機(jī)器人專業(yè)委員會委員,哈爾濱工程大學(xué)學(xué)術(shù)委員會委員。主要研究方向為潛器與水下機(jī)器人控制技術(shù)、船舶運動綜合控制,bianxinqian@hrbeu.edu.cn。

AnOptimizedMethodofTargetMotionEstimationforUUVBasedonVariableKalmanFilter

YANZheping*,HAOYue,WANGQianyi,BIANXinqian

(College of Automation,Harbin Engineering University,Harbin 150001,China)

In order to achieve an accurate,safe and efficient way of tracking the target for Unmanned Underwater Vehicle(UUV)to collect information,it proposed an optimization of target motion estimation method for the individual. By establishing target motion model and the use of variable Kalman filtering,it achieved the expected results of accurate estimates. To improve the feasibility of the method,it adapted minimized target position estimate covariance matrix to realize target trajectory estimation and optimization. At the same time,it took the limit of maximum speed of UUV and the minimum distance to the moving target into consideration. It elaborated specific methods and steps of this method,and proved the effectiveness of the optimization algorithm by simulation.

unmanned underwater vehicle;target motion estimation;variable Kalman filter;minimization of covariance matrix

2014-07-09修改日期:2014-10-28

TP391;TP29

:A

:1004-1699(2014)12-1637-06

10.3969/j.issn.1004-1699.2014.12.010