[發(fā)明專利]基于3D-UKF的目標多象限運動跟蹤方法

在審

• 申請?zhí)枺?/td>: 202011579950.6
• 申請日: 2020-12-28
• 公開（公告）號: CN112598696A
• 公開（公告）日: 2021-04-02
• 發(fā)明（設(shè)計）人: 申屠晗;林俊浩;韓闊業(yè);郭云飛;陳霄
• 申請（專利權(quán)）人: 杭州電子科技大學(xué)
• 主分類號: G06T7/20
• 分類號: G06T7/20;G06T7/277
• 代理公司: 杭州君度專利代理事務(wù)所(特殊普通合伙) 33240
• 代理人: 朱亞冠

1.基于3D-UKF的目標多象限運動跟蹤方法，其特征在于包括以下步驟：

步驟(1)、構(gòu)建單傳感器單目標跟蹤場景；

步驟(2)、仿真生成目標運動軌跡；

步驟(3)、仿真生成傳感器對目標的測量集；

步驟(4)、初始化UKF濾波器；

步驟(5)、利用3D-UKF-MQT濾波算法獲得目標當前時刻的狀態(tài)估計和協(xié)方差，具體如下：

5-1獲取一組采樣點及其對應(yīng)的權(quán)值ω⁽ⁱ⁾：

式(3)中，i表示第i個采樣點，表示k-1時刻的狀態(tài)估計，X⁽ⁱ⁾(k-1|k-1)表示k-1時刻的第i個采樣點的狀態(tài)，P(k-1|k-1)表示k-1時刻的狀態(tài)估計協(xié)方差，n表示狀態(tài)維數(shù)；

式(4)中，為第i個采樣點均值對應(yīng)的權(quán)值，為第i個采樣點協(xié)方差對應(yīng)的權(quán)值；λ＝α²(n+κ)-n，表示比例因子；α、κ、β表示參數(shù)；

5-2利用步驟5-1中X⁽ⁱ⁾(k-1|k-1)計算2n+1個采樣點的一步預(yù)測：

X⁽ⁱ⁾(k|k-1)＝f[k-1,X⁽ⁱ⁾(k-1|k-1)](i＝1,2,…,2n+1) (5)

其中f是非線性狀態(tài)方程函數(shù)；X⁽ⁱ⁾(k|k-1)表示k-1時刻第i個采樣點的狀態(tài)預(yù)測值；

5-3利用步驟5-2中X⁽ⁱ⁾(k|k-1)和步驟5-1中ω⁽ⁱ⁾，得到系統(tǒng)狀態(tài)量的一步預(yù)測：

其中表示k-1時刻預(yù)測的目標狀態(tài)估計，P(k|k-1)表示k-1時刻預(yù)測的狀態(tài)估計協(xié)方差，Q表示過程噪聲協(xié)方差，T表示矩陣的轉(zhuǎn)置；

5-4利用步驟5-3中的和P(k|k-1)計算獲得新的2n+1個采樣點的狀態(tài)：

5-5將步驟5-4中預(yù)測采樣點的狀態(tài)代入觀測方程，得到觀測預(yù)測值：

其中xx_θ(k)表示目標方位角所在象限，xx_γ(k)表示目標仰角所在象限，h是非線性觀測方程函數(shù)，Z⁽ⁱ⁾(k|k-1)表示第i個采樣點預(yù)測的觀測值，Z_θ(k)、Z_γ(k)分別表示k時刻傳感器得到的目標的方位角和仰角；

上述觀測量的角度所在象限根據(jù)公式(11)可知：

其中ζ表示目標的角度(即方位角或仰角)，MOD表示求余函數(shù)，xx表示目標所對應(yīng)的象限；

5-6將步驟5-5中得到的觀測預(yù)測值，通過加權(quán)求和得到系統(tǒng)預(yù)測的均值及協(xié)方差和

其中包括和分別表示仰角和方位角的觀測預(yù)測均值，根據(jù)公式(12)可推導(dǎo)得到：

5-7利用步驟5-6中公式(13)-(14)計算k時刻的Kalman增益矩陣K(k)：

5-8利用公式(18)、(19)判斷目標是否存在跨象限情況，若均認為不存在跨象限情況，則進行步驟5-9，反之進行步驟5-10：

其中flag為跨象限的標志，1表示跨象限，0表示未跨象限；

5-9當flag＝0時根據(jù)公式(20)更新系統(tǒng)的狀態(tài)和協(xié)方差：

5-10當flag＝1時根據(jù)公式(21)更新系統(tǒng)的狀態(tài)和協(xié)方差：

其中F為狀態(tài)轉(zhuǎn)移矩陣，表示k時刻的狀態(tài)估計，P(k|k)表示k時刻的狀態(tài)估計協(xié)方差，R_d表示測量噪聲協(xié)方差R中對應(yīng)距離的數(shù)值，Z_xyz(k)表示Z(k)轉(zhuǎn)化的x,y,z軸數(shù)值。