[發明專利]基于多尺度模型的紅外/激光雷達數據融合目標跟蹤方法

有效

• 申請號: 201510080015.8
• 申請日: 2015-02-13
• 公開（公告）號: CN104730537B
• 公開（公告）日: 2017-04-26
• 發明（設計）人: 王炳健;郝靜雅;張高翔;李敏;易翔;吳飛紅;秦翰林;周慧鑫
• 申請（專利權）人: 西安電子科技大學
• 主分類號: G01S17/66
• 分類號: G01S17/66
• 代理公司: 廣東朗乾律師事務所44291
• 代理人: 楊煥軍

1.基于多尺度模型的紅外/激光雷達數據融合目標跟蹤方法，其特征在于，包括以下步驟：

步驟1、根據紅外探測系統和激光雷達探測系統的采樣頻率建立多尺度模型，所述多尺度模型為：

x(i,k_i+1)＝F(i,k_i)x(i,k_i)+w(i,k_i)

z(i,k_i)＝H(i,k_i)x(i,k_i)+v(i,k_i)

其中，x(i,k_i+1)為k_i+1時刻在尺度i上的狀態向量，F(i,k_i)為k_i時刻在尺度i上的系統狀態轉移矩陣，x(i,k_i)為k_i時刻在尺度i上的狀態向量，w(i,k_i)為系統噪聲，z(i,k_i)為k_i時刻在尺度i上的觀測向量，H(i,k_i)為k_i時刻在尺度i上的觀測矩陣，v(i,k_i)為觀測噪聲，k_i為尺度i上的采樣時刻，i＝1,2；

步驟2、紅外探測系統和激光雷達探測系統分別對目標進行數據采集，得到目標的方位角測量值α_m、目標的俯仰角測量值β_m和目標的距離測量值r_m；

步驟3、在尺度1上對紅外探測系統獲取的角度信息采用無跡卡爾曼濾波法進行估計，濾波過程中利用兩點遞推法進行初始化，得到目標的方位角濾波估計值α′_m和俯仰角濾波估計值β′_m；

步驟4、在尺度2上將步驟3得到角度估計信息和激光雷達探測系統獲取的距離信息進行融合，計算出目標在直角坐標系下的位置坐標(x_m，y_m，z_m)：

$\left\{\begin{array}{c}{x}_m=r_m\×{\mathrm{cos\β}}_m^{\′}\×{\mathrm{cos\α}}_m^{\′}\\ y_m=r_m\×{\mathrm{cos\β}}_m^{\′}\×{\mathrm{sin\α}}_m^{\′}\\ z_m=r_m\×{\mathrm{sin\β}}_m^{\′}\end{array}\right.;$

其中，r_m為目標的距離測量值，α′_m為目標的方位角濾波估計值，β′_m為目標的俯仰角濾波估計值；

步驟5、在尺度2上采用卡爾曼濾波法對目標狀態進行估計；

步驟5-1、計算目標的無偏觀測值得到直角坐標系下的轉換測量值

$\left\{\begin{array}{c}{x}_m^u={\mathrm{\λ}}_{\mathrm{\α}}^{-1}{\mathrm{\λ}}_{\mathrm{\β}}^{-1}{x}_m\\ y_m^u={\mathrm{\λ}}_{\mathrm{\α}}^{-1}{\mathrm{\λ}}_{\mathrm{\β}}^{-1}{y}_m\\ z_m^u={\mathrm{\λ}}_{\mathrm{\β}}^{-1}{z}_m\end{array}\right.;$

其中，x_m、y_m、z_m為直角坐標系下目標的位置坐標，λ_α和λ_β為無偏系數，為方位角測量誤差，為俯仰角測量誤差，分別是和的方差，E[·]表示求期望值，[·]^T表示轉置操作；

步驟5-2、計算誤差方差矩陣R_p：

$R_p=\mathrm{cov}\left\{{\[x_m^u,y_m^u,z_m^u\]}^T\right|r_m,{\mathrm{\α}}_m^{\′},{\mathrm{\β}}_m^{\′}\}=\left[\begin{array}{ccc}{R}_p^{11}& R_p^{12}& R_p^{13}\\ R_p^{21}& R_p^{22}& R_p^{23}\\ R_p^{31}& R_p^{32}& R_p^{33}\end{array}\right];$

$R_p^{11}=\frac{1}{4}(r_m^2+{\mathrm{\σ}}_r^2)\[1+{{\mathrm{\λ}}_{\mathrm{\α}}}^{\′}cos\left(2{\mathrm{\α}}_m^{\′}\right)\]\[1+{{\mathrm{\λ}}_{\mathrm{\β}}}^{\′}cos\left(2{\mathrm{\β}}_m^{\′}\right)\]+r_m^2{\cos }^2{\mathrm{\β}}_m^{\′}{\cos }^2{\mathrm{\α}}_m^{\′}\[{\left({\mathrm{\λ}}_{\mathrm{\α}}{\mathrm{\λ}}_{\mathrm{\β}}\right)}^{-2}-2\]$

$R_p^{22}=\frac{1}{4}(r_m^2+{\mathrm{\σ}}_r^2)\[1-{{\mathrm{\λ}}_{\mathrm{\α}}}^{\′}cos\left(2{\mathrm{\α}}_m^{\′}\right)\]\[1+{{\mathrm{\λ}}_{\mathrm{\β}}}^{\′}cos\left(2{\mathrm{\β}}_m^{\′}\right)\]+r_m^2{\cos }^2{\mathrm{\β}}_m^{\′}{\sin }^2{\mathrm{\α}}_m^{\′}\[{\left({\mathrm{\λ}}_{\mathrm{\α}}{\mathrm{\λ}}_{\mathrm{\β}}\right)}^{-2}-2\]$

$R_p^{33}=\frac{1}{2}(r_m^2+{\mathrm{\σ}}_r^2)\[1+{{\mathrm{\λ}}_{\mathrm{\β}}}^{\′}cos\left(2{\mathrm{\β}}_m^{\′}\right)\]+r_m^2{\sin }^2{\mathrm{\β}}_m^{\′}({{\mathrm{\λ}}_{\mathrm{\β}}}^{-2}-2)$

$R_p^{12}=\frac{1}{4}(r_m^2+{\mathrm{\σ}}_r^2){{\mathrm{\λ}}_{\mathrm{\α}}}^{\′}sin\left(2{\mathrm{\α}}_m^{\′}\right)\[1+{{\mathrm{\λ}}_{\mathrm{\β}}}^{\′}cos\left(2{\mathrm{\β}}_m^{\′}\right)\]+r_m^2{\cos }^2{\mathrm{\β}}_m^{\′}{\mathrm{sin\α}}_m^{\′}{\mathrm{cos\α}}_m^{\′}\[{\left({\mathrm{\λ}}_{\mathrm{\α}}{\mathrm{\λ}}_{\mathrm{\β}}\right)}^{-2}-2\]$

$R_p^{13}=\frac{1}{2}(r_m^2+{\mathrm{\σ}}_r^2){\mathrm{\λ}}_{\mathrm{\α}}{{\mathrm{\λ}}_{\mathrm{\β}}}^{\′}{\mathrm{cos\α}}_m^{\′}sin\left(2{\mathrm{\β}}_m^{\′}\right)+r_m^2{\mathrm{cos\β}}_m^{\′}{\mathrm{sin\β}}_m^{\′}{\mathrm{cos\α}}_m^{\′}({{\mathrm{\λ}}_{\mathrm{\α}}}^{-1}{{\mathrm{\λ}}_{\mathrm{\β}}}^{-2}-{{\mathrm{\λ}}_{\mathrm{\α}}}^{-1}-{\mathrm{\λ}}_{\mathrm{\α}})$

$R_p^{23}=\frac{1}{2}(r_m^2+{\mathrm{\σ}}_r^2){\mathrm{\λ}}_{\mathrm{\α}}{{\mathrm{\λ}}_{\mathrm{\β}}}^{\′}{\mathrm{sin\α}}_m^{\′}sin\left(2{\mathrm{\β}}_m^{\′}\right)+r_m^2{\mathrm{cos\β}}_m^{\′}{\mathrm{sin\β}}_m^{\′}{\mathrm{sin\α}}_m^{\′}({{\mathrm{\λ}}_{\mathrm{\α}}}^{-1}{{\mathrm{\λ}}_{\mathrm{\β}}}^{-1}-{{\mathrm{\λ}}_{\mathrm{\α}}}^{-1}-{\mathrm{\λ}}_{\mathrm{\α}})$

式中的為距離測量誤差的方差；

步驟5-3、建立目標狀態方程和測量方程；

融合后目標的狀態方程為：x^u(2,k₂+1)＝Ax^u(2,k₂)+w^u(2,k₂)；

其中，A為去偏后的系統狀態轉移矩陣，x^u(2,k₂)為去偏后的狀態向量，x^u(2,k₂)＝[x′,v_x,y′,v_y,z′,v_z]^T，x′、y′和z′是目標在直角坐標系下的位置坐標的濾波值，v_x、v_y和v_z分別為x′、y′和z′方向的速度，w^u(2,k₂)是去偏后的過程噪聲；

融合后目標的測量方程為：z^u(2,k₂)＝Bx^u(2,k₂)+v^u(2,k₂)；

其中，B為去偏后的觀測矩陣，v^u(2,k₂)為去偏后的觀測噪聲；

步驟5-4、濾波更新，采用卡爾曼濾波法進行濾波：

k₂-1時刻的濾波值和濾波協方差分別為x^u(2,k₂-1|k₂-1)和P(k₂-1|k₂-1)，則k₂時刻的預測值x^u(2,k₂|k₂-1)＝Ax^u(2,k₂-1|k₂-1)，k₂時刻的預測協方差P(2,k₂︱k₂-1)＝AP(k₂-1︱k₂-1)A^T+Q(2,k₂)，Q(2,k₂)為去偏后的過程噪聲的方差矩陣；

卡爾曼增益矩陣K(2,k₂)＝P(2,k₂|k₂-1)B^T/(R_p+BP(2,k₂|k₂-1)B^T)，

濾波后的狀態值x^u(2,k₂|k₂)＝x^u(2,k₂|k₂-1)+K(2,k₂)(z^u-Bx^u(2,k₂|k₂-1))，

濾波后的協方差矩陣P(2,k₂|k₂)＝P(2,k₂|k₂-1)-K(2,k₂)AP(2,k₂|k₂-1)，

經過融合估計，獲得目標在直角坐標系下的位置坐標的濾波值x′、y′和z′；

步驟6、將融合估計的信息轉換到尺度1上，進一步優化角度估計結果；

利用步驟5獲得的位置信息，計算目標的方位角精確估計值和俯仰角精確估計值

${\mathrm{\α}}_m^u=arctan\frac{y^{\′}}{x^{\′}},$

${\mathrm{\β}}_m^u=arctan\frac{z^{\′}}{\sqrt{x^{\′2}+y^{\′2}}};$

再將方位角精確估計值和俯仰角精確估計值返回到尺度1上，以尺度2上的采樣時刻n(k₁-1)+1作為當前時刻，在尺度1上采用卡爾曼濾波法進行濾波估計，得到精確濾波估計值，紅外探測系統在尺度1的采樣率q₁和激光雷達探測系統在尺度2的采樣率q₂滿足：q₁/q₂＝n，n為正整數，紅外探測系統在尺度1上的采樣時刻k₁與激光雷達探測系統在尺度2上的采樣時刻k₂之間關系為：k₂＝n(k₁-1)+1；

步驟7、重復步驟2至步驟6，直至目標離開探測系統的探測范圍，獲得目標運動軌跡。

2.如權利要求1所述的基于多尺度模型的紅外/激光雷達數據融合目標跟蹤方法，其特征在于：

尺度1為紅外探測系統的采樣頻率，尺度2為激光雷達探測系統的采樣頻率；

k₁時刻在尺度1上的系統狀態轉移矩陣

狀態向量x(1,k₁)＝[α’_m,v_α,α_α,β’_m,v_β,α_β]，觀測向量z(1,k₁)＝[α_m,β_m]，觀測矩陣H(1,k₁)＝[1,0,0,0,0,0；0,0,0,1,0,0]，其中，T為紅外探測系統的采樣周期，α_m為目標的方位角測量值，β_m為目標的俯仰角測量值，α′_m為尺度1上目標的方位角濾波估計值，v_α為α′_m的速度，α_α為α′_m的加速度，β′_m為尺度1上目標的俯仰角濾波估計值，v_β為β′_m的速度，α_β為β′_m的加速度；

k₂時刻在尺度2上的系統狀態轉移矩陣F(2,k₂)＝[1,0,0；0,1,0；0,0,1]，狀態向量x(2,k₂)＝[r_m,α′_m,β′_m]，觀測向量z(2,k₂)＝[r_m,α′_m,β′_m]，觀測矩陣H(2,k₂)＝[1,0,0；0,1,0；0,0,1]。