[发明专利]基于多尺度模型的红外/激光雷达数据融合目标跟踪方法

权利要求书

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]。