劉 廠，趙俊翔，胡 海

(1.哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001； 2.空間物理重點實驗室，北京 100076；

3.中國核動力研究設計院，四川 雙流 610213)

基于雙貝葉斯估計的動態(tài)威脅運動狀態(tài)估計

劉 廠1，趙俊翔2，胡 海3

(1.哈爾濱工程大學 自動化學院，黑龍江 哈爾濱 150001； 2.空間物理重點實驗室，北京 100076；

3.中國核動力研究設計院，四川 雙流 610213)

現(xiàn)有的水下動態(tài)威脅運動狀態(tài)估計主要采用單級估計方法，該方法將動態(tài)威脅的速度分解到三個坐標軸分別進行估計，沒有考慮運動體速度的整體性.本文提出了一種雙貝葉斯估計方法，首先利用自適應貝葉斯估計方法對水下動態(tài)威脅的速度大小和方向進行估計，再以此為輸入，利用無跡貝葉斯估計方法估計其位置.與傳統(tǒng)方法相比，雙貝葉斯估計方法充分考慮了水下動態(tài)威脅速度的整體性，其有效性和精確性在仿真試驗中得到驗證.

雙貝葉斯估計；水下動態(tài)威脅；運動狀態(tài)

0 引言

水下潛器在航行過程中不可避免地遇到一些動態(tài)威脅，對動態(tài)威脅的運動狀態(tài)進行準確估計才能實現(xiàn)有效規(guī)避[1].現(xiàn)有的運動狀態(tài)估計往往采用單級估計方法，通過無跡卡爾曼、容積卡爾曼、自適應卡爾曼等方法完成目標跟蹤的任務.這些方法將水下動態(tài)威脅的運動分解為三個坐標軸上的運動并分別進行估計[2-3].該分解方法對水下動態(tài)威脅的運動模型適應性不佳，不能完全把握水下動態(tài)威脅的運動趨勢[4-5]，無法適應快速機動的威脅狀態(tài)變化.因此，筆者提出了一種雙貝葉斯運動狀態(tài)估計方法，該方法針對動態(tài)威脅的運動模型，首先利用自適應貝葉斯估計方法[3，6]對水下動態(tài)威脅的速度大小和方向進行估計，進而利用無跡貝葉斯估計方法[7-8]估計動態(tài)威脅的位置.仿真試驗表明，相對于傳統(tǒng)的估計方法，該方法更加貼近水下動態(tài)威脅的運動模型[9-10]，具有較高的精確性.

1 動態(tài)威脅運動方程和觀測方程

為了使水下動態(tài)威脅的狀態(tài)估計更加貼近真實的運動狀態(tài)首先根據(jù)實際運動情況建立水下動態(tài)威脅的運動方程和觀測方程.對于水下動態(tài)威脅的觀測一般通過聲學傳感器來完成，因此得到的觀測數(shù)據(jù)大多為極坐標下的數(shù)據(jù)，即水下動態(tài)威脅相對于觀測點的距離R、平面偏向角A和垂直偏向角E.結合觀測點自身坐標，可以得到觀測方程：

(1)

式中：(ξ,η,ζ)為水下動態(tài)威脅的世界坐標系坐標；(x,y,z)為觀測點位置坐標.

建立水下動態(tài)威脅的運動模型應當結合觀測方程進行.由觀測方程可以看出，對水下動態(tài)威脅的觀測信息僅限于動態(tài)威脅與觀測點的相對位置，通過觀測方程無法獲得水下動態(tài)威脅自身的姿態(tài)角.因此，由于觀測信息不足，水下動態(tài)威脅的轉動在建立運動方程時應該被忽略.但是由于動態(tài)威脅運動的連續(xù)性，在某一時刻的位置信息不能完全反應其運動情況，為了更全面地把握動態(tài)威脅的運動狀態(tài)，通過相鄰兩個時刻的觀測信息可以解算得到動態(tài)威脅在兩個時刻間速度大小和方向的平均值.在雙貝葉斯運動狀態(tài)估計方法中，將速度作為整體看待，不將其投影至三個坐標軸.

記水下動態(tài)威脅的速度大小為v，速度方向與ηEξ平面的夾角為φ，速度方向與Eζ軸正方向夾角為θ，可以得到水下動態(tài)威脅的運動方程

(2)

2 水下動態(tài)威脅速度和位置的處理

為了實現(xiàn)對水下動態(tài)威脅的狀態(tài)估計，需要解算水下動態(tài)威脅的速度大小及其方向，速度方向包括水下動態(tài)威脅的縱傾角和偏航角.文中的縱傾角和偏航角指的是水下動態(tài)威脅的速度方向與水平面ηEξ和Eζ軸正方向的夾角，而不是水下動態(tài)威脅的姿態(tài)角.解算方程如下：

(3)

式中：Δξ=ξk-ξk-1,Δη=ηk-ηk-1,Δζ=ζk-ζk-1；φk-1為其k-1時刻的縱傾角；θk-1為其k-1時刻的偏航角；vk-1為其k-1時刻的速度大小.

3 對速度的自適應貝葉斯估計

在實際運動過程中，水下動態(tài)威脅的速度和加速度均會發(fā)生變化，通常采用的CT模型和CV模型無法完全匹配其運動，而模型失配往往會導致估計發(fā)散.自適應估計方法在經(jīng)典估計方法的基礎上，能夠充分發(fā)揮觀測信息的作用，對自身模型進行校正，從而連續(xù)校正預測值.因此，采用自適應估計的方法，通過觀測結果對模型進行調整，以適應加速度變化的運動模型.筆者將加速度的變化量視作噪聲，通過Sage-husa噪聲估計器對其均值進行估計.

根據(jù)前兩節(jié)的運動分析，得到水下動態(tài)威脅速度大小和方向的狀態(tài)轉移方程和觀測方程：

(4)

(5)

記：

(6)

(7)

(8)

基于以上方程，使用自適應貝葉斯估計方法，設計自適應貝葉斯估計器.

預測

(9)

在獲得k+1時刻的數(shù)據(jù)后，對參數(shù)進行更新，需要注意的是，在k+1時刻，由于觀測值為水下動態(tài)威脅的坐標，所以解算出的水下動態(tài)威脅速度信息是k到k+1時刻之間的平均值，由于間隔時間較小，將此平均值作為k時刻水下動態(tài)威脅的速度信息.

(10)

(11)

(12)

(13)

Pk=[I-KkHk]Pk,k-1.

(14)

噪聲估計：

(15)

(16)

(17)

(18)

在Sage-husa自適應貝葉斯估計方法中，遺忘因子b是十分關鍵的參數(shù)，估計精度是否發(fā)散與其相關.本文中，使用預報殘差法求取遺忘因子.量測殘差：

(19)

(20)

又有：

(21)

代入，有：

(22)

通過以上各式，即可得到對水下動態(tài)威脅速度大小和方向的估計結果.

4 對位置的無跡貝葉斯估計

獲得水下動態(tài)威脅的速度大小和方向的估計信息后，下一步是對運動體的未來位置進行估計.不可避免地，由于水下動態(tài)威脅的機械誤差和環(huán)境影響，水下動態(tài)威脅的行進過程仍是一個存在過程噪聲的動態(tài)過程，因此需要使用貝葉斯估計方法對運動體位置進行進一步的預測.由于此時狀態(tài)方程仍為非線性，傳統(tǒng)估計方法無法滿足要求.無跡貝葉斯估計方法根據(jù)“利用一組確定性的點對概率分布函數(shù)的近似要比直接近似非線性函數(shù)更容易”的思想，通過無跡變換獲得確定性采樣點，在非線性估計中能夠取得良好的效果.因此，采用無跡貝葉斯估計方法進行運動體位置的估計.建立水下動態(tài)威脅的狀態(tài)方程和觀測方程

Xk+1=f(Xk,φ,θ,v)+Wk;

(23)

yk=HXk+Vk,

(24)

式中：

步態(tài)選擇方面，選取穩(wěn)定性高的三角步態(tài)作為四足機器人在斜面上的運動步態(tài)，并采用LF-RH-RF-LH的腿部運動順序以獲得最優(yōu)的穩(wěn)定裕度[6]。本文的運動步態(tài)時序圖如圖3所示。其中，黑色矩形表示腿部處于支撐相，白色矩形表示腿部處于擺動相。

(25)

預測：

(26)

(27)

(28)

這樣，我們就得到了水下動態(tài)威脅位置的最終估計值，在k+1時刻，獲取運動體位置的觀測值之后，根據(jù)觀測量對參數(shù)進行更新

(29)

(30)

(31)

(32)

(33)

(34)

(35)

式中:λ為點集產(chǎn)生的尺度參數(shù)，每個點的權值因采樣策略的不同而稍有差異.筆者采用標度采樣：

(36)

5 仿真試驗

為了驗證上述方法的有效性，設計水下動態(tài)威脅在三維空間中進行變加速運動，模擬觀測數(shù)據(jù)作為估計算法的輸入.分別利用雙貝葉斯估計方法和單級貝葉斯估計方法對動態(tài)威脅的運動路徑進行估計，通過對比估計結果與水下動態(tài)威脅實際航路的誤差驗證雙貝葉斯估計方法的有效性。其中，單級貝葉斯估計采用自適應的無跡貝葉斯估計方法，將速度投影至三個坐標軸，直接對水下動態(tài)威脅的世界坐標系坐標進行估計.其狀態(tài)方程為：

(37)

5.1 水下動態(tài)威脅運動路徑設計

設計動態(tài)威脅從(200，250，1 000)點出發(fā)，先進行5 s的勻速直線運動，在隨后的95 s中，做速度大小和方向不斷變化的變速曲線運動.具體運動情況如下.

1～5 s.做速度為5 m/s的勻速直線運動.

5～10 s.速度俯仰角突變?yōu)?.429 7，角加速度為-0.05 rad/s，航向角突變?yōu)?.707 1，角加速度為0.05 rad/s，加速度為0.05 m/s2.

10～25 s.水下動態(tài)威脅改變航向，速度俯仰角突變?yōu)?.729 7，角加速度為0.05 rad/s，航向角突變?yōu)?.407 1，角加速度為-0.07 rad/s，加速度為-0.05 m/s2.

25～40 s.速度俯仰角突變?yōu)?.829 7，角加速度為-0.01 rad/s，航向角突變?yōu)?.507 1，角加速度為0.05 rad/s，加速度為-0.05 m/s2.

40～60 s.水下動態(tài)威脅突然回轉并加速行進，速度俯仰角突變?yōu)?.829 7，角加速度為-0.01 rad/s，航向角突變?yōu)?.607 1，角加速度為0.05 rad/s，加速度為0.1 m/s2.

60～80 s.再次改變航向，速度俯仰角突變?yōu)?.329 7，角加速度為-0.03 rad/s，航向角突變?yōu)?.607 1，角加速度為-0.03 rad/s，加速度為0.05 m/s2.

80～100 s.水下動態(tài)威脅速度俯仰角突變?yōu)?.929 7，角加速度為0.01 rad/s，航向角突變?yōu)?.007 1，角加速度為0.01 rad/s，加速度為-0.05 m/s2.

在100 s后運動體消失.在此過程中，水下動態(tài)威脅的最高速度達到11 m/s，最大角加速度為0.07 rad/s，最大加速度為0.1 m/s2.

水下動態(tài)威脅運動路徑如圖1所示.

圖1 水下動態(tài)威脅的模擬航路

5.2 仿真結果

設觀測誤差為1(單位：m)，方差為1，取無跡貝葉斯濾波中λ為1，分別采用單級貝葉斯方法和筆者設計的雙貝葉斯估計方法(取無跡貝葉斯濾波中λ為1)得到的仿真結果如圖2～5所示.

通過比較可以發(fā)現(xiàn)，在觀測噪聲較大的條件下，將速度投影至三個坐標軸分別進行估計的單貝葉斯估計方法不能很好地把握運動特征，估計誤差較大，在航路附近有較大波動.由于雙貝葉斯估計充分考慮到水下動態(tài)威脅的運動狀態(tài)，先對水下動態(tài)威脅的速度大小及其方向進行估計，更充分地利用了觀測信息，因此，雙貝葉斯估計能夠更好地消除觀測噪聲對估計的影響，在觀測噪聲均值較大的情況下，即使對動態(tài)目標的位置估計出現(xiàn)了誤差，對于動態(tài)目標的運動趨勢估計依舊較為精確，也得到了較高精度的估計值.

圖2 水下動態(tài)威脅航路估計結果

圖3 x方向估計誤差

圖4 y方向估計誤差

圖5 z方向估計誤差

6 結論

提出了一種基于雙貝葉斯估計的水下動態(tài)威脅運動狀態(tài)估計方法.首先建立了水下運載體的運動模型，通過自適應貝葉斯估計對動態(tài)威脅的速度大小和方向進行估計，再以此為輸入，通過第二級無跡貝葉斯估計進行運動體位置估計.仿真試驗證明，相對于傳統(tǒng)的單級估計方法，雙貝葉斯估計方法在進行水下動態(tài)威脅運動狀態(tài)估計時能夠更好地貼合運動模型，得到較高的估計精度，并能準確地給出水下動態(tài)威脅的運動趨勢.

[1] 朱大奇, 顏明重. 移動機器人路徑規(guī)劃技術綜述 [J]. 控制與決策, 2010, 25(7): 961-967.

[2] 高曉光, 李青原, 邸若海. 基于DBN威脅評估的MPC無人機三維動態(tài)路徑規(guī)劃[J]. 系統(tǒng)工程與電子技術, 2014, 36(11)：2199-2205.

[3] 李峰. 自主水下航行器目標跟蹤方法研究[D]. 哈爾濱：哈爾濱工程大學自動化學院, 2013.

[4] 邸若海, 高曉光. 模糊離散動態(tài)貝葉斯網(wǎng)絡的目標威脅等級評估[J]. 火力與指揮控制, 2012, 37(4):14-17.

[5] GAO M, JIANG J, MING N K, et al. Cooperative path planning for UAVs with UAV loss considerations[C]//Proc. of computationall intelligence for security and defense applications, 2013: 38-44.

[6] 張召友. 非線性 Bayesian 濾波及其在 SINS/GPS 緊耦合導航中的應用研究[D]. 哈爾濱：哈爾濱工程大學自動化學院, 2013.

[7] 張宏啟. 新型自適應 kalman 濾波算法在組合導航中的應用研究[D]. 哈爾濱：哈爾濱工程大學自動化學院, 2012.

[8] 王克文, 蔣德瓏, 孫栗. 電力系統(tǒng)狀態(tài)估計算法的模糊綜合評價與分析[J]. 鄭州大學學報(工學版), 2011, 32(3):85-89.

[9] 張恒艷, 高中文, 李文龍，等. 不確定T-S模糊系統(tǒng)的跟蹤控制器設計[J]. 鄭州大學學報(工學版), 2016, 37(2):15-19.

[10]范文兵, 郭云, 王振義,等. 一種MIMO時間相干信道下的有限反饋新方法[J]. 鄭州大學學報(工學版), 2014, 35(3):43-46.

Research on Motion Status Estimation of Dynamic Threat Base on Double Bayes Estimation

LIU Chang1, ZHAO Junxiang2, HU Hai3

(1.Collage of Automatic, Harbin Engineering University, Harbin 150001, China; 2.Key Laboratory of space physics,Beijing 100076, China; 3.Nuclear Power Institute of China , Shuangliu 610213, China)

For the issue of motion estimation of underwater dynamic threat, single-stage estimation method is utilized extensively in which velocity is decomposed to three coordinate axes for independent calculation. In order to consider the integrity of the speed, a double Bayesian estimation method is proposed in this paper. In the first stage of the improved approach, the velocity and direction of underwater dynamic threat are estimated by the adaptive Bayesian estimation, and then the position is estimated by the unscented Bayesian estimation in the second stage with the joint efforts of the estimated results in the first stage. Simulation results are provided to verify the feasibility and effectiveness of the proposed method.

double Bayes estimation; underwater dynamic threat; motion status

2016-10-07；

2016-12-05

國家自然科學基金資助項目(51379069)；中央高?；A科研業(yè)務費支持項目(HEUCF160410)

劉廠(1982— )，男，山東濟寧人，哈爾濱工程大學講師，博士，主要從事水下運載器智能導航技術研究，E-mail:liuchang407@hrbeu.edu.cn.

1671-6833(2017)02-0055-06

TP181

A

10.13705/j.issn.1671-6833.2017.02.013