CN102176220B - 一种同平台配置的雷达与红外传感器进行空间配准的方法 - Google Patents

Abstract

本发明适用于数据融合技术领域，提供了一种同平台配置的雷达与红外传感器进行空间配准的方法，所述方法包括：建立以极坐标位置、直角坐标速度和极坐标配准偏差为状态向量的系统状态方程和量测方程；用前两个时刻雷达与红外传感器的量测通过UT变换得到系统状态向量的初始化值；根据系统的状态方程、量测方程和状态向量的初始化值，对量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差本发明克服了现有雷达与红外传感器空间配准的局限性，提高了配准的精度。

Description

一种同平台配置的雷达与红外传感器进行空间配准的方法

技术领域

本发明属于数据融合技术领域，尤其涉及一种同平台配置的雷达与红外传感器进行空间配准的方法。

背景技术

随着科学技术的发展，传感器的性能得到了很大的提高，各种面向复杂应用背景的多传感器系统大量涌现。特别是进入20世纪70年代以后，高技术兵器尤其是精确制导武器和远程打击武器大量出现，使战场范围扩大到海、陆、空、天、电磁五维空间中。依靠单一传感器提供信息已无法满足需要，而采用数据融合技术处理来自于微波、毫米波、电视、红外、激光以及电子情报技术等覆盖宽广频段的有源和无源探测器在内的观测数据，不仅可以实现对多个传感器探测信息的综合、分析和处理，达到资源共享，功能、性能相互弥补，还可以使系统具有良好的鲁棒性，扩展的系统时空覆盖范围，良好的检测能力等诸多优点。雷达与红外传感器属于异类传感器，具有上述多传感器的优势，它们在机载系统、舰载系统、反导弹、近程目标跟踪中得到了广泛的使用，然而来自于这两个不同传感器的数据具有不同的采样间隔，所采用的坐标系也不尽相同，要完成对这些数据的融合处理，只有对这些数据进行空间配准的情况下才能进行融合处理，可见数据的空间配准过程是完成后续融合、跟踪的基础，没有经过空间配准的数据将无法完成数据的融合。空间配准在整个数据融合中占有基础性的地位，对传感器数据融合系统的性能起着非常重要的作用。

基于空间配准的重要性，国内外众多的专家学者在此领域作出了大量的研究。目前常用的空间配准算法主要有离线配准方法和在线配准方法，其中：

离线配准方法主要有：实时质量控制法采用对每个传感器所测得的数据进行平均处理，然后取平均值作为传感器的观测值；最小二乘法采用对每个传感器所测得的数据运用最小二乘法进行运算处理，取其运算结果作为传感器最终观测值；加权最小二乘法(GLS)是对最小二乘法的一种推广，它根据传感器所测数据的方差为每个测量值赋予不同的权值,然后运用最小二乘法则进行计算，此外还有极大似然法等。

在线配准方法主要有各种滤波算法：Helmick利用卡尔曼滤波来估计传感器偏差参数，该算法以一个传感器为参考，利用多个传感器对目标的位置观测值的微分估算出传感器的偏差参数，然后把其余各传感器对准到该传感器的参考坐标系中，消除传感器偏差；Nabaa利用扩展的卡尔曼滤波算法来综合考虑传感器系统偏差和相对于公共参考坐标的位置、方位误差，此外还有UKF等滤波算法在空间配准中也得到了应用。

由于红外传感器的量测值缺乏目标的距离信息，目前用于雷达与红外传感器空间配准的算法主要是各种在线配准方法，系统的状态方程普遍采用了直角坐标系形式，然而雷达的测量是基于极坐标系的，许多实际系统也只需要目标的极坐标参数，需要进行直角坐标与极坐标之间的数据转换，而且现有的空间配准算法对系统状态初始值的取值很敏感，对预测协方差矩阵的初始值有太多的限制，配准的精度比较低。

发明内容

本发明实施例的目的在于提供一种同平台配置的雷达与红外传感器进行空间配准的方法，旨在解决现有技术提供的雷达与红外传感器的空间配准算法目标状态与实际需求坐标系不一致，现有的空间配准算法对系统状态初始值敏感，对预测协方差矩阵的初始值有限制，配准的精度比较低的问题。

本发明实施例是这样实现的，一种同平台配置的雷达与红外传感器进行空间配准的方法，所述方法包括下述步骤：

根据系统的状态向量，建立系统的状态方程，即：

X(t_k+1)=[h₁(t_k),h₂(t_k),h₃(t_k),h₄(t_k),h₅(t_k),h₆(t_k),h₇(t_k),h₈(t_k)]^T+W(t_k)，其中：

$h_1\left(t_k\right)=\mathrm{sqrt}({({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T)}^2),$

$h_2\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T}{{\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T}\right),$

$h_3\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T}{\mathrm{sqrt}{(({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2)}\right),$

$h_4\left(t_k\right)=\stackrel{\·}{x}\left(t_k\right),$

$h_5\left(t_k\right)=\stackrel{\·}{y}\left(t_k\right),$

$h_6\left(t_k\right)=\stackrel{\·}{z}\left(t_k\right),$

h₇(t_k)=Δθ(t_k)，

h₈(t_k)=Δη(t_k)，

而且，T为雷达与红外传感器经时间配准过后的采样周期，ξ_x(t_k)=r(t_k)cosη(t_k)sinθ(t_k)、ξ_y(t_k)=r(t_k)cosη(t_k)cosθ(t_k)、ξ_z(t_k)=r(t_k)sinη(t_k)、

分别表示k时刻直角坐标系下目标位置、速度在三个坐标轴上的分量，W(t_k)是过程噪声，

它的协方差矩阵为 $Q=\mathrm{diag}\left(\right[{\mathrm{\δ}}_1^2,{\mathrm{\δ}}_2^2,...,{\mathrm{\δ}}_{L_1}^2\left]\right);$

根据状态变量和雷达与红外传感器的测量特性，建立系统的量测方程，即：

其中，(ρ_r(t_k),θ_r(t_k)，η_r(t_k))为雷达的量测值，(θ_i(t_k),η_i(t_k))为红外传感器的量测值，n_j(t_k),j=1,2,...,L₂是系统的量测噪声，它的协方差矩阵为(r(t_k),θ(t_k),η(t_k))为k时刻目标距离、方位角、俯仰角的状态值，(Δθ(t_k),Δη(t_k))为k时刻红外方位角与俯仰角偏差的状态值；

将前两个时刻雷达与红外传感器的量测通过UT变换得到系统状态向量的初始化值：

$X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),\stackrel{\·}{x}\left(t_0\right),\stackrel{\·}{y}\left(t_0\right),\stackrel{\·}{z}\left(t_0\right),\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T;$

根据系统的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差

所述根据系统的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差

的步骤如下：

（a）、对状态向量和协方差进行初始化操作，即：

${\hat{x}}_1\left(0\right)=X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),{\stackrel{\·}{x}}_0,{\stackrel{\·}{y}}_0,{\stackrel{\·}{z}}_0,\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T,$ $P_a^{*}\left(0\right)=E[(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))$

${(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))}^T]=\left[\begin{array}{ccc}{P}_1\left(0\right)& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right],$

其中，P₁(0)是X(t₀)的协方差矩阵， ${\hat{x}}_a^{*}\left(0\right)={\left[\begin{array}{cc}{\hat{x}}_1^T\left(0\right),& 0^T\end{array}\right]}^T$ 是扩维后的状态向量；

（b）、对扩维后的状态向量

用2L+1个对称的σ点来近似；

（c）、计算σ点通过状态方程的传播，生成得到状态预测估计和预测协方差；

（d）、计算状态向量和量测之间的交互协方差；

（e）、对新获取得到的量测进行滤波更新，得到系统偏差值；

重复步骤(b)～(e)，得到系统偏差值，直到配准偏差不再明显变化。

在本发明实施例中，以极坐标位置、直角坐标速度和极坐标配准偏差为状态向量建立系统的状态方程和量测方程，将雷达与红外传感器的前两个量测通过UT变换得到系统状态向量的初始化值；最后用UKF对后续量测数据进行滤波配准，得到红外传感器相对于雷达的空间配准偏差克服了现有雷达与红外传感器配准方法的局限性，提高了配准的精度。

附图说明

图1是本发明实施例提供的雷达与红外传感器进行空间配准的方法的实现流程图；

图2是本发明实施例提供的将雷达与红外传感器的前两个量测通过UT变换得到系统状态向量的初始化值的实现流程图；

图3是本发明实施例提供的根据系统的状态方程、量测方程和状态向量的初始化值，对量测数据进行滤波配准，得到红外传感器相对于雷达的空间配准偏差的实现流程图；

图4至图6是本发明第一实施例提供的配准结果示意图；

图7至图9是本发明第二实施例提供的配准结果示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在本发明实施例提供的同平台的雷达与红外传感器空间配准算法，将目标的距离、方位角、俯仰角作为系统状态向量的一部分，更符合目标的真实状态，有利于得到更好的配准精度，与传统方法相比，本发明实施例对系统状态初始值的取值不敏感，对预测协方差矩阵的初始值亦不做太多的限制，有利于算法在实际中的应用

本发明实施例的目的在于提供一种同平台配置的雷达与红外传感器进行空间配准的方法，所述方法包括下述步骤：

根据系统的状态向量，建立系统的状态方程，即：

X(t_k+1)=[h₁(t_k),h₂(t_k),h₃(t_k),h₄(t_k),h₅(t_k),h₆(t_k),h₇(t_k),h₈(t_k)]^T+W(t_k)，其中：

$h_1\left(t_k\right)=\mathrm{sqrt}({({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T)}^2),$

$h_2\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T}{{\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T}\right),$

$h_3\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T}{\mathrm{sqrt}{(({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2)}\right),$

$h_4\left(t_k\right)=\stackrel{\·}{x}\left(t_k\right),$

$h_5\left(t_k\right)=\stackrel{\·}{y}\left(t_k\right),$

$h_6\left(t_k\right)=\stackrel{\·}{z}\left(t_k\right),$

h₇(t_k)=Δθ(t_k)，

h₈(t_k)=Δη(t_k)，

而且，T为雷达与红外传感器经时间配准过后的采样周期，ξ_x(t_k)=r(t_k)cosη(t_k)sinθ(t_k)、ξ_y(t_k)=r(t_k)cosη(t_k)cosθ(t_k)、ξ_z(t_k)=r(t_k)sinη(t_k)、

分别表示k时刻直角坐标系下目标位置、速度在三个坐标轴上的分量，W(t_k)是过程噪声，

它的协方差矩阵为 $Q=\mathrm{diag}\left(\right[{\mathrm{\δ}}_1^2,{\mathrm{\δ}}_2^2,...,{\mathrm{\δ}}_{L_1}^2\left]\right);$

根据状态变量和雷达与红外传感器的测量特性，建立系统的量测方程，即：

其中，(ρ_r(t_k),θ_r(t_k)，η_r(t_k))为雷达的量测值，(θ_i(t_k),η_i(t_k))为红外传感器的量测值，n_j(t_k),j=1,2,...,L₂是系统的量测噪声，它的协方差矩阵为(r(t_k),θ(t_k),η(t_k))为k时刻目标距离、方位角、俯仰角的状态值，(Δθ(t_k),Δη(t_k))为k时刻红外方位角与俯仰角偏差的状态值；

将前两个时刻雷达与红外传感器的量测通过UT变换得到系统状态向量的初始化值：

$X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),\stackrel{\·}{x}\left(t_0\right),\stackrel{\·}{y}\left(t_0\right),\stackrel{\·}{z}\left(t_0\right),\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T;$

根据系统的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差

所述根据系统的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差

的步骤如下：

（a）、对状态向量和协方差进行初始化操作，即：

${\hat{x}}_1\left(0\right)=X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),{\stackrel{\·}{x}}_0,{\stackrel{\·}{y}}_0,{\stackrel{\·}{z}}_0,\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T,$ $P_a^{*}\left(0\right)=E[(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))$

${(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))}^T]=\left[\begin{array}{ccc}{P}_1\left(0\right)& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right],$

其中，P₁(0)是X(t₀)的协方差矩阵， ${\hat{x}}_a^{*}\left(0\right)={\left[\begin{array}{cc}{\hat{x}}_1^T\left(0\right),& 0^T\end{array}\right]}^T$ 是扩维后的状态向量；

（b）、对扩维后的状态向量

用2L+1个对称的σ点来近似；

（c）、计算σ点通过状态方程的传播，生成得到状态预测估计和预测协方差；

（d）、计算状态向量和量测之间的交互协方差；

（e）、对新获取得到的量测进行滤波更新，得到系统偏差值；

重复步骤(b)～(e)，得到系统偏差值，直到配准偏差不再明显变化。

实施例一

图1示出了本发明实施例提供的同平台配置的雷达与红外传感器进行空间配准的方法的实现流程，其具体的步骤如下所述：

在步骤S101中，根据系统的状态向量，建立系统的状态方程，即：

X(t_k+1)=[h₁(t_k),h₂(t_k),h₃(t_k),h₄(t_k),h₅(t_k),h₆(t_k),h₇(t_k),h₈(t_k)]^T+W(t_k)，其中：

$h_1\left(t_k\right)=\mathrm{sqrt}({({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T)}^2),$

$h_2\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T}{{\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T}\right),$

$h_3\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T}{\mathrm{sqrt}{(({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2)}\right),$

$h_4\left(t_k\right)=\stackrel{\·}{x}\left(t_k\right),$

$h_5\left(t_k\right)=\stackrel{\·}{y}\left(t_k\right),$

$h_6\left(t_k\right)=\stackrel{\·}{z}\left(t_k\right),$

h₇(t_k)=Δθ(t_k)，

h₈(t_k)=Δη(t_k)，

这里，T为雷达与红外传感器经时间配准过后的采样周期，ξ_x(t_k)=r(t_k)cosη(t_k)sinθ(t_k)、ξ_y(t_k)=r(t_k)cosη(t_k)cosθ(t_k)、ξ_z(t_k)=r(t_k)sinη(t_k)、

分别表示k时刻直角坐标系下目标位置、速度在三个坐标轴上的分量，W(t_k)是过程噪声，

它的协方差矩阵为 $Q=\mathrm{diag}\left(\right[{\mathrm{\δ}}_1^2,{\mathrm{\δ}}_2^2,...,{\mathrm{\δ}}_{L_1}^2\left]\right).$

在本发明实施例中，系统的状态向量为：

$X\left(t_k\right)={[r\left(t_k\right),\mathrm{\θ}\left(t_k\right),\mathrm{\η}\left(t_k\right),\stackrel{\·}{x}\left(t_k\right),\stackrel{\·}{y}\left(t_k\right),\stackrel{\·}{z}\left(t_k\right),\mathrm{\Δ\θ}\left(t_k\right),\mathrm{\Δ\η}\left(t_k\right)]}^T,$ (r(t_k),θ(t_k),η(t_k))是是k时刻目标在极坐标系下的坐标；(Δθ,Δη)表示红外传感器的方位角与俯仰角偏差，W(t_k)是过程噪声。

在步骤S102中，根据状态变量和雷达与红外传感器的测量特性，建立系统的量测方程，即：

其中，(ρ_r(t_k),θ_r(t_k)，η_r(t_k))为雷达的量测值，(θ_i(t_k),η_i(t_k))为红外传感器的量测值，n_j(t_k),j=1,2,...,L₂是系统的量测噪声，L₂为量测向量的维数，它的噪声方差为

(r(t_k),θ(t_k),η(t_k))为k时刻目标距离、方位角、俯仰角的状态值，(Δθ(t_k),Δη(t_k))为k时刻红外方位角与俯仰角偏差的状态值。

在本发明实施例中，假设系统的量测向量为Z=[ρ_r(t_k),θ_r(t_k),η_r(t_k),θ_i(t_k)，η_i(t_k)]。

在步骤S103中，将前两个时刻雷达与红外传感器的量测通过UT变换得到系统状态向量的初始化值：

$X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),\stackrel{\·}{x}\left(t_0\right),\stackrel{\·}{y}\left(t_0\right),\stackrel{\·}{z}\left(t_0\right),\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T.$

在本发明实施例中，令系统的状态向量为：

$X\left(t_k\right)={[r\left(t_k\right),\mathrm{\θ}\left(t_k\right),\mathrm{\η}\left(t_k\right),\stackrel{\·}{x}\left(t_k\right),\stackrel{\·}{y}\left(t_k\right),\stackrel{\·}{z}\left(t_k\right),\mathrm{\Δ\θ}\left(t_k\right),\mathrm{\Δ\η}\left(t_k\right)]}^T,$ 雷达的前两个测量数据分别为：(ρ_r(t₀),θ_r(t₀),η_r(t₀))，(ρ_r(t₁),θ_r(t₁),η_r(t₁))，红外传感器的前两个测量数据：(θ_i(t₀),η_i(t₀))，(θ_i(t₁),η_i(t₁))，

表示k时刻直角坐标系下目标速度在三个坐标轴上的分量。初始化

必须将雷达与红外传感器的前两个量测向直角坐标系转化，这是一个非线性转换，为了尽可能减小转换偏差，结合UT变换可以得到目标速度在直角坐标系下三个坐标轴上的初始化分量为

(Δθ(t₀),Δη(t₀))是将雷达与红外前两个量测中方位角与俯仰角求其差然后平均求得，因此，状态向量的初始化值为：

$X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),\stackrel{\·}{x}\left(t_0\right),\stackrel{\·}{y}\left(t_0\right),\stackrel{\·}{z}\left(t_0\right),\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T.$

下述有具体的实施例进行描述，在此不再赘述。

在步骤S104中，根据系统到的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差 $\mathrm{\Δ}={[\mathrm{\Δ}\widehat{\mathrm{\θ}},\mathrm{\Δ}\widehat{\mathrm{\η}}]}^T.$

在本发明实施例中，由状态方程、量测方程、状态初始值以及雷达与红外传感器对目标的量测数据利用UKF算法可得到最终状态估值为 因此，

就是所要求的红外传感器相对于雷达的空间配准偏差。

下述有具体的实施例进行描述，在此不再赘述。

在本发明实施例中，以极坐标位置、直角坐标速度和极坐标配准偏差为状态向量建立系统的状态方程和量测方程，将雷达与红外传感器的前两个量测通过UT变换得到系统状态向量的初始化值；最后用UKF对后续量测数据进行滤波配准，得到红外传感器相对于雷达的空间配准偏差

克服了现有雷达与红外传感器配准方法的局限性，提高了配准的精度。

实施例二

图2示出了本发明实施例提供的将前两个时刻雷达与红外传感器的量测通过UT变换得到系统状态向量的初始化值的实现流程，其具体的步骤如下所述：

在步骤S201中，估计配准偏差的初始值。

在本发明实施例中，设雷达的前两个测量数据分别为：(ρ_r(t₀)，θ_r(t₀)，η_r(t₀))，(ρ_r(t₁)，θ_r(t₁)，η_r(t₁))，红外传感器的前两个数据为：(θ_i(t₀)，η_i(t₀))，(θ_i(t₁)，η_i(t₁))，令红外传感器方位角偏差与俯仰角偏差为(Δθ_i(t_k)，Δη_i(t_k))， ${[r\left(t_k\right),\mathrm{\θ}\left(t_k\right),\mathrm{\η}\left(t_k\right),\stackrel{\·}{x}\left(t_k\right),\stackrel{\·}{y}\left(t_k\right),\stackrel{\·}{z}\left(t_k\right),\mathrm{\Δ\θ}\left(t_k\right),\mathrm{\Δ\η}\left(t_k\right)]}^T$ 为系统的状态向量。

红外传感器的方位角偏差、俯仰角初始化偏差可认为是：

$\mathrm{\Δ\θ}\left(t_0\right)=\frac{({\mathrm{\θ}}_r\left(t_0\right)-{\mathrm{\θ}}_i\left(t_0\right))+({\mathrm{\θ}}_r\left(t_1\right)-{\mathrm{\θ}}_i\left(t_1\right))}{2};$

$\mathrm{\Δ\η}\left(t_0\right)=\frac{({\mathrm{\η}}_r\left(t_0\right)-{\mathrm{\η}}_i\left(t_0\right))+({\mathrm{\η}}_r\left(t_1\right)-{\mathrm{\η}}_i\left(t_1\right))}{2};$

它的噪声方差为

是量测噪声方差。

令向量Y(t_k)＝[ρ_r(t_k)，θ_r(t_k)，η_r(t_k)，θ_i(t_k)，η_i(t_k)，Δθ(t_k)，Δη(t_k)]^T，(ρ_r(t_k)，θ_r(t_k)，η_r(t_k))表示k时刻雷达的量测值，(θ_i(t_k)，η_i(t_k))表示k时刻红外的量测值，(Δθ(t_k)，Δη(t_k))表示k时刻红外相对于雷达的偏差，其初始协方差矩阵P_Y＝diag([R，P_d])。又设Z(t_k)＝[x(t_k)，y(t_k)，z(t_k)]^T表示直角坐标系下目标在三个坐标轴上的分量。由于两传感器处于同一平台上，可认为目标到红外传感器与到雷达的距离一致，则有：

Z(t_k)＝F(Y(t_k))；

其中：

$F\left(Y\left(t_k\right)\right)=\left\{\begin{array}{c}\frac{{\mathrm{\ρ}}_r\left(t_k\right)\cos \left({\mathrm{\θ}}_r\left(t_k\right)\right)\cos \left({\mathrm{\η}}_r\left(t_k\right)\right)+{\mathrm{\ρ}}_r\left(t_k\right)\cos ({\mathrm{\θ}}_i\left(t_k\right)+\mathrm{\Δ\θ}\left(t_k\right))\cos ({\mathrm{\η}}_i\left(t_k\right)+\mathrm{\Δ\η}\left(t_k\right))}{2}\\ \frac{{\mathrm{\ρ}}_r\left(t_k\right)\sin \left({\mathrm{\θ}}_r\left(t_k\right)\right)\cos \left({\mathrm{\η}}_r\left(t_k\right)\right)+{\mathrm{\ρ}}_r\left(t_k\right)\sin ({\mathrm{\θ}}_i\left(t_k\right)+\mathrm{\Δ\θ}\left(t_k\right))\cos ({\mathrm{\η}}_i\left(t_k\right)+\mathrm{\Δ\η}\left(t_k\right))}{2}\\ \frac{{\mathrm{\ρ}}_r\left(t_k\right)\sin \left({\mathrm{\η}}_r\left(t_k\right)\right)+{\mathrm{\ρ}}_r\left(t_k\right)\sin \cos ({\mathrm{\η}}_i\left(t_k\right)+\mathrm{\Δ\η}\left(t_k\right))}{2}\end{array}\right..$

因此，对其进行UT变换，该方案给出了具体的实现流程。

在步骤S202中，根据输入的Y(t_k)和P_Y，选择对称采样策略，生成输入变量的Sigma点集{χ_i},i＝1,2,....,2L，及对应的权值

和其中，L＝8为状态向量的维数，

为均值加权值，

为协方差加权值。

在本发明实施例中，其具体的实现如下：

$\stackrel{\‾}{x}=Y\left(t_k\right),$ ${\mathrm{\χ}}_0=\stackrel{\‾}{x};$

${\mathrm{\χ}}_i=\stackrel{\‾}{x}+{\left(\sqrt{(L+\mathrm{\λ})P_Y}\right)}_i,i=1,\·\·\·,L;$

${\mathrm{\χ}}_i=\stackrel{\‾}{x}-{\left(\sqrt{(L+\mathrm{\λ})P_Y}\right)}_i,i=L+1,\·\·\·,2L;$

$W_0^m=\mathrm{\λ}/(L+\mathrm{\λ});$

$W_0^c=\mathrm{\λ}/(L+\mathrm{\λ})+(1-{\mathrm{\α}}^2+\mathrm{\β});$

$W_i^m=W_i^c=1/(2L+2\mathrm{\λ}),i=1,\·\·\·,2L;$

其中，扩散因子λ=α²(L+k)-L，α为sigma向量围绕

分布的扩展因子（一般为较小的正值），k为另一个扩展因子（通常设为0），β为表示Y(t_k)先验分布的因子（对于高斯分布来说，通常设为2），

是矩阵均方根的第i行或列，矩阵均方根的求解可以利用QR分解或Cholesky分解得到。

在步骤S203中，对所采样的输入变量Sigma点集{χ_i}中的每个Sigma点进行非线性变换F(·)，生成Sigma点集{Z_i}，其中，Z_i=F(χ_i)。

在步骤S204中，对变换后的Sigma点集{Z_i}进行加权处理，生成输出变量Z的统计量

和P_Z，其中， $\stackrel{\‾}{Z}=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^m{Z}_i,$ $P_Z=\underset{i=0}{\overset{L-1}{\mathrm{\Σ}}}{W}_i^c(Z_i-\stackrel{\‾}{Z}){(Z_i-\stackrel{\‾}{Z})}^T.$

在步骤S205中，根据Y(t₀)=[ρ_r(t₀),θ_r(t₀),η_r(t₀),θ_i(t₀),η_i(t₀),Δθ_i0,Δη_i0]^T和Y(t₁)=[ρ_r(t₁),θ_r(t₁),η_r(t₁),θ_i(t₁),η_i(t₁),Δθ_i0,Δη_i0]^T，计算得到目标在直角坐标系下三个坐标轴方向的速度初始化值。

在本发明实施例中，将Y(t₀)=[ρ_r(t₀),θ_r(t₀),η_r(t₀),θ_i(t₀),η_i(t₀),Δθ_i0,Δη_i0]^T代入Z_i=F(χ_i)，可得Z₀=[x₀,y₀,z₀]；同理Y(t₁)=[ρ_r(t₁),θ_r(t₁),η_r(t₁),θ_i(t₁),η_i(t₁),Δθ_i0,Δη_i0]^T代入可得Z₁=[x₁,y₁,z₁]；因此，目标在直角坐标系下三个坐标轴方向的速度初始化值为：

${\stackrel{\·}{x}}_0=\frac{x_1-x_0}{T};$

${\stackrel{\·}{y}}_0=\frac{y_1-y_0}{T};$

${\stackrel{\·}{z}}_0=\frac{z_1-z_0}{T};$

其中，T表示传感器的采样间隔(上述方式是在雷达与红外传感器量测数据时间对准已经完成的基础上实现的)。

在步骤S206中，根据所述速度初始化值，雷达的测量值和配准偏差的初始化值得到状态变量的初始化结果：

$X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),\stackrel{\·}{x}\left(t_0\right),\stackrel{\·}{y}\left(t_0\right),\stackrel{\·}{z}\left(t_0\right),...\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T.$

在本发明实施例中，在计算得到状态变量的初始化结果时，还可以得到直角坐标系下坐标轴方向三个速度分量的初始化方差P_Z0，

实施例三

图3示出了本发明实施例提供的根据系统的状态方程、量测方程和状态向量的初始化值，对量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差的实现流程，其具体的步骤如下所述：

在步骤S301中，对状态向量和协方差进行初始化操作，即：

${\hat{x}}_1\left(0\right)=X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),{\stackrel{\·}{x}}_0,{\stackrel{\·}{y}}_0,{\stackrel{\·}{z}}_0,\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T,$

$P_a^{*}\left(0\right)=E\left[(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right)){(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))}^T\right]=\left[\begin{array}{ccc}{P}_1\left(0\right)& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right],$ 其中， $P_1\left(0\right)=\left[\begin{array}{ccc}{R}_r& 0& 0\\ 0& P_{Z0}& 0\\ 0& 0& P_d\end{array}\right],$

${\hat{x}}_a^{*}\left(0\right)={\left[\begin{array}{cc}{\hat{x}}_1^T\left(0\right),& 0^T\end{array}\right]}^T.$

在本发明实施例中，

表示雷达的量测噪声方差，p_z0为状态初始化时求得的协方差，P_d为红外系统偏差的初始协方差，Q为系统过程噪声方差，R为系统量测噪声方差，

为扩维后的状态向量，维数为L=2L₁+L₂，

为扩维后的协方差。

在步骤S302中，对扩维后的状态向量

将由2L+1个对称的点σ来近似。

在本发明实施例中，扩维后的状态向量将由2L+1个对称的点σ来近似：

${\mathrm{\χ}}_{a,0}^{*}(k-1)={\hat{x}}_a^{*}(k-1);$

${\mathrm{\χ}}_{a,i}^{*}={\widehat{\mathrm{\χ}}}_a^{*}(k-1)+{\left(\sqrt{(L+\mathrm{\λ})P_a^{*}(k-1)}\right)}_i,i=1,\·\·\·,L;$

${\mathrm{\χ}}_{a,i+L}^{*}={\widehat{\mathrm{\χ}}}_a^{*}(k-1)+{\left(\sqrt{(L+\mathrm{\λ})P_a^{*}(k-1)}\right)}_i,i=1,\·\·\·,L;$ 其中，L为扩维状态向量

的维数，λ为复合尺度参数；

为矩阵平方根的第i列。

在步骤S303中，计算σ点通过状态方程的传播，生成得到状态预测估计和预测协方差。

在本发明实施例中，计算σ点通过状态方程的传播，利用权值和状态方程，生成得到状态预测估计和预测协方差：

${\mathrm{\χ}}_a^{*}\left(k\right|k-1)=f_a\left({\mathrm{\χ}}_a^{*}(k-1)\right);$

${\hat{x}}_a^{*}\left(k\right|k-1)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{w}_i^m{\mathrm{\χ}}_{a,i}^{*}\left(k\right|k-1);$

$P_a^{*}\left(k\right|k-1)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{w}_i^c\left(\mathrm{\Δ}{\mathrm{\χ}}_{a,i}^{*}\right)\left(\mathrm{\Δ}{\mathrm{\χ}}_{a,i}^{*}\right);$

其中， $\mathrm{\Δ}{\mathrm{\χ}}_{a,i}^{*}\left(k\right|\mathrm{lk}-1)={\mathrm{\χ}}_{a,i}^{*}\left(k\right|k-1)-{\hat{x}}_a^{*}\left(k\right|k-1);$ $w_0^m=\mathrm{\λ}/(L+\mathrm{\λ});$ $w_i^m=1/2(L+\mathrm{\λ}),i=1,\·\·\·,2L.$

在步骤S304中，计算状态向量和量测之间的交互协方差。

在本发明实施例中，计算输出的一步预测，得到预测量测和预测协方差，同时得到状态向量和量测之间的交互协方差阵：

$\mathrm{\ζ}\left(k\right|k-1)=H\left({\mathrm{\χ}}_a^{*}\left(k\right|k-1)\right);$

$\hat{z}\left(k\right|k-1)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{w}_i^m{\mathrm{\ζ}}_i\left(k\right|k-1);$

$P_{\mathrm{zz}}\left(k\right)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{w}_i^c\left({\mathrm{\Δ\ζ}}_i\left(k\right|k-1)\right){\left({\mathrm{\Δ\ζ}}_i\left(k\right|k-1)\right)}^T+R;$

$P_{x^{a}z}\left(k\right)=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{w}_i^c\left(\mathrm{\Δ}{\mathrm{\χ}}_{a,i}^{*}\left(k\right|k-1)\right){\left({\mathrm{\Δ\ζ}}_i\left(k\right|k-1)\right)}^T;$

其中， ${\mathrm{\Δ\ζ}}_i\left(k\right|k-1)={\mathrm{\ζ}}_i\left(k\right|k-1)-\hat{z}\left(k\right|k-1).$

在步骤S305中，对新获取得到的量测进行滤波更新，得到系统偏差值。

在本发明实施例中，其具体的滤波过程为：

${\hat{x}}_a^{*}\left(k\right|k)={\hat{x}}_a^{*}\left(k\right|k-1)+G\left(k\right)(z\left(k\right)-\hat{z}\left(k\right|k-1));$

$G\left(k\right)=P_{x^{a}z}\left(k\right)P_{\mathrm{zz}}^{-1}\left(k\right);$

$P_a^{*}\left(k\right|k)=P_a^{*}\left(k\right|k-1)-G\left(k\right)P_{\mathrm{zz}}^{-1}{G}_k^T\left(k\right);$

从上述

可以得到k时刻系统偏差值。

在本发明实施例中，重复步骤S302～S305得到系统偏差值，直到配准偏差不再明显变化。

作为本发明的一个具体实施例，下述给出两个具体的实验实例进行说明：

实施例三

令目标的状态方程为：

$\left\{\begin{array}{c}x=13+15\×\sin (0.06\×(i+1))\\ y=9+5\×(i+1)\\ z=10\×(i+1)\end{array}\right.$

采样周期T＝1秒，雷达的测量噪声为(0.15km,0.00175rad,0.00175rad)，红外传感器的测量噪声为(0.00035rad,0.00035rad)，红外传感器的系统偏差为(0.0035rad,0.0025rad)，图4-图6给出了在以上仿真情况下的配准结果。

实施例四

令目标的状态方程为：

$\left\{\begin{array}{c}x=2.1+0.1i\\ y=1.8+0.15i\\ z=1.5+0.08i\end{array}\right.$

采样周期T＝1秒，雷达的测量噪声为(0.10km,0.00275rad,0.00275rad)，红外传感器的测量噪声为(0.00045rad,0.00045rad)，红外传感器的系统偏差为(0.0030rad,0.0025rad)，图7-图9给出了在以上情况下仿真的配准结果。

在本发明实施例中，以极坐标位置、直角坐标速度和极坐标配准偏差为状态向量建立系统的状态方程和量测方程，将雷达与红外传感器的前两个量测通过UT变换得到系统状态向量的初始化值；最后用UKF对后续量测数据进行滤波配准，得到红外传感器相对于雷达的空间配准偏差

克服了现有雷达与红外传感器配准方法的局限性，提高了配准的精度。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种同平台配置的雷达与红外传感器进行空间配准的方法，其特征在于，所述方法包括下述步骤：

根据系统的状态向量，建立系统的状态方程，即：

X(t_k+1)=[h₁(t_k),h₂(t_k),h₃(t_k),h₄(t_k),h₅(t_k),h₆(t_k),h₇(t_k),h₈(t_k)]^T+W(t_k)，其中：

$h_1\left(t_k\right)=\mathrm{sqrt}({({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T)}^2),$

$h_2\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T}{{\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T}\right),$

$h_3\left(t_k\right)=\arctan \left(\frac{{\mathrm{\ξ}}_z\left(t_k\right)+\stackrel{\·}{z}\left(t_k\right)\·T}{\mathrm{sqrt}{(({\mathrm{\ξ}}_x\left(t_k\right)+\stackrel{\·}{x}\left(t_k\right)\·T)}^2+{({\mathrm{\ξ}}_y\left(t_k\right)+\stackrel{\·}{y}\left(t_k\right)\·T)}^2)}\right),$

$h_4\left(t_k\right)=\stackrel{\·}{x}\left(t_k\right),$

$h_5\left(t_k\right)=\stackrel{\·}{y}\left(t_k\right),$

$h_6\left(t_k\right)=\stackrel{\·}{z}\left(t_k\right),$

h₇(t_k)=Δθ(t_k)，

h₈(t_k)=Δη(t_k)，

而且，T为雷达与红外传感器经时间配准过后的采样周期，ξ_x(t_k)=r(t_k)cosη(t_k)sinθ(t_k)、ξ_y(t_k)=r(t_k)cosη(t_k)cosθ(t_k)、ξ_z(t_k)=r(t_k)sinη(t_k)、

分别表示k时刻直角坐标系下目标位置、速度在三个坐标轴上的分量，W(t_k)是过程噪声，

它的协方差矩阵为 $Q=\mathrm{diag}\left(\right[{\mathrm{\δ}}_1^2,{\mathrm{\δ}}_2^2,...,{\mathrm{\δ}}_{L_1}^2\left]\right);$

根据状态变量和雷达与红外传感器的测量特性，建立系统的量测方程，即：

其中，(ρ_r(t_k),θ_r(t_k)，η_r(t_k))为雷达的量测值，(θ_i(t_k),η_i(t_k))为红外传感器的量测值，n_j(t_k),j=1,2,...,L₂是系统的量测噪声，它的协方差矩阵为

(r(t_k),θ(t_k),η(t_k))为k时刻目标距离、方位角、俯仰角的状态值，(Δθ(t_k),Δη(t_k))为k时刻红外方位角与俯仰角偏差的状态值；

将前两个时刻雷达与红外传感器的量测通过UT变换得到系统状态向量的初始化值： $X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),\stackrel{\·}{x}\left(t_0\right),\stackrel{\·}{y}\left(t_0\right),\stackrel{\·}{z}\left(t_0\right),\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T;$

根据系统的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差

所述根据系统的状态方程、量测方程和状态向量的初始化值，对后续量测数据进行UKF滤波配准，得到红外传感器相对于雷达的空间配准偏差

的步骤如下：

（a）、对状态向量和协方差进行初始化操作，即：

${\hat{x}}_1\left(0\right)=X\left(t_0\right)={[{\mathrm{\ρ}}_r\left(t_0\right),{\mathrm{\θ}}_r\left(t_0\right),{\mathrm{\η}}_r\left(t_0\right),{\stackrel{\·}{x}}_0,{\stackrel{\·}{y}}_0,{\stackrel{\·}{z}}_0,\mathrm{\Δ\θ}\left(t_0\right),\mathrm{\Δ\η}\left(t_0\right)]}^T,$ $P_a^{*}\left(0\right)=E[(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))$

${(x_a^{*}\left(0\right)-{\hat{x}}_a^{*}\left(0\right))}^T]=\left[\begin{array}{ccc}{P}_1\left(0\right)& 0& 0\\ 0& Q& 0\\ 0& 0& R\end{array}\right],$

其中，P₁(0)是X(t₀)的协方差矩阵， ${\hat{x}}_a^{*}\left(0\right)={\left[\begin{array}{cc}{\hat{x}}_1^T\left(0\right),& 0^T\end{array}\right]}^T$ 是扩维后的状态向量；

（b）、对扩维后的状态向量

用2L+1个对称的σ点来近似；

（c）、计算σ点通过状态方程的传播，生成得到状态预测估计和预测协方差；

（d）、计算状态向量和量测之间的交互协方差；

（e）、对新获取得到的量测进行滤波更新，得到系统偏差值；

重复步骤(b)～(e)，得到系统偏差值，直到配准偏差不再明显变化。

2.如权利要求1所述的方法，其特征在于，所述系统的状态向量为 $X\left(t_k\right)={[r\left(t_k\right),\mathrm{\θ}\left(t_k\right),\mathrm{\η}\left(t_k\right),\stackrel{\·}{x}\left(t_k\right),\stackrel{\·}{y}\left(t_k\right),\stackrel{\·}{z}\left(t_k\right),\mathrm{\Δ\θ}\left(t_k\right),\mathrm{\Δ\η}\left(t_k\right)]}^T,$ 其中，(r(t_k),θ(t_k),η(t_k))是k时刻目标在极坐标系下的位置坐标，

表示k时刻直角坐标系下目标速度在三个坐标轴上的分量，(Δθ,Δη)表示红外传感器相对于雷达的方位角与俯仰角配准偏差。

3.如权利要求1所述的方法，其特征在于，所述系统的量测向量为：Z=[ρ_r(t_k),θ_r(t_k),η_r(t_k),θ_i(t_k),η_i(t_k)]。

Images