一种约束条件下的机动目标跟踪方法

摘要

本发明公开了一种约束条件下的机动目标跟踪方法，所述方法包括以下步骤：A、获得机动目标的位置量测信息；B、将所述位置量测信息转换为笛卡尔坐标量测；C、在一维空间建立约束坐标静态模型,即建立机动目标在道路x，y方向上的位置关于里程数s的表达式；D、依据所述机动目标的运动特点设置运动模型,根据所建立的表达式建立状态方程和量测方程；E、分别针对各所述运动模型，对转换得到的笛卡尔坐标量测进行滤波，得到一维变量里程数s及其速度的滤波结果。根据本发明的实施方式，可以提高机动目标跟踪的性能。

说明书

技术领域

本发明涉及约束条件下的机动目标跟踪，尤其涉及一种将二维约束转到一维空间下进行处理的机动目标跟踪方法。

背景技术

在雷达目标跟踪领域，存在一些场景，目标的运动轨迹发展不由其本身的运动速度决定，而是受到外界环境的严重影响甚至严格约束。例如，在公路上行驶的车辆受到地形和公路形状的约束，离开公路的可能性极小，这就是典型的约束目标跟踪问题。

然而，在处理约束目标跟踪问题的时候，常规目标跟踪方法存在明显的缺陷：一、约束轨迹形状复杂，传统运动模型失配严重；二、常规方法不考虑约束条件先验信息，造成信息浪费；三、跟踪结果难以满足约束条件，存在性能损失。因此，如何合理利用先验约束信息，建立准确的约束目标运动模型并提出对应的跟踪方法，对于提高约束目标跟踪精度具有重要的意义。

目前比较常见的约束目标跟踪方法包括：

一、模型降阶法，如W.Wen and H.F.Durrant-Whyte,“Model-based multi-sensor data fusion,”Proceedings of IEEE International Conference on Robotics and Automation,Nice,France,pp.1720--1726,1992(基于模型的多传感器数据融合)，提取系统模型中状态分量之间的约束关系式，利用约束关系式将状态分量互相表示来减少状态向量的维数；R.J.Hewett,M.T.Health,M.D.Butala and F.Kamalabadi,“A robust null space method for linear equality constrained state estimation,”IEEE Transactions on Signal Processing,vol.58,pp.3961--3971,2010(针对线性等式约束状态估计的鲁棒零空间方法)，通过零空间分解获得降阶模型，这类方法简化了系统并获得了简单模型约束状态的最优估计，但缺点是模型降阶后物理意义不清晰，且只适用于简单问题难以推广。

二、完美量测法，如L.S.Wang,Y.T.Chiang and F.R.Chang,“Filtering method for nonlinear systems with constraints,”IEEE Proceedings of Control Theory and Applications,vol.149(6),pp.525--531,2002(包含约束条件的非线性系统滤波方法)，即将等式约束作为没有噪声的伪量测引入，把约束估计问题便转化为观测扩维的常规估计问题来解决。但由于观测噪声协方差矩阵是奇异矩阵，因此在卡尔曼滤波求解时可能引起数值计算问题。

三、估计后投影法，如D.Simon and T.L.Chia,“Kalman filtering with state equality constraints,”IEEE Transactions on Aerospace and Electronic Systems,vol.38(1),pp.128--136,2002(状态等式约束下的卡尔曼滤波)，利用约束优化技术将无约束估计向约束子空间投影，寻找约束子空间中距离无约束估计最近的一点作为最终估计。这样得到的估计结果必定满足约束条件，但不是约束状态的最优估计。

四、模型修正法，主要有两类方法，一种如S.Ko and R.Bitmead,“State estimation for linear systems with state equality constraints,”Automatica,vol.43(8),pp.1363-1368,2007(状态等式约束下的线性系统状态估计)，通过左乘投影矩阵实现无约束动态模型和约束条件的融合。另一种如Z.S.Duan,X.R.Li and J.F.Ru,“Design and Analysis of Linear Equality Constrained Dynamic Systems,”Proceedings of the 15th International Conference on Information Fusion,Singapore,pp.2537--2544,2012(线性等式约束动态系统的设计与分析)，采用直接消除法得到满足约束条件的修正模型。这类方法仍然无法解决常规模型与复杂约束之间的失配问题。

上述几种方法都是在常规无约束模型的基础上研究约束跟踪问题，并没有系统的解决模型失配等问题，且大多是基于目标无机动的假设开展研究。然而实际场景中，目标可能会有变速机动。以道路场景为例，目标可能在转弯处减速，超车时加速等。因此，提出一种更加贴近物理实际的机动目标运动建模和状态估计方法具有重要的实践意义。

发明内容

为了克服上述现有技术存在的一项或更多的缺陷，本发明提供一种基于一维约束坐标的机动目标跟踪方法。

为实现上述目的，本发明提供了一种约束条件下的机动目标跟踪方法，该方法包括以下步骤：A、获得机动目标的位置量测信息；B、将所述位置量测信息转换为笛卡尔坐标量测；C、在一维空间建立约束坐标静态模型,即建立机动目标在道路x，y方向上的位置关于一维变量里程数s的表达式；D、依据所述机动目标的运动特点设置运动模型,根据所建立的表达式建立状态方程和量测方程；E、分别针对各所述运动模型，对转换得到的笛卡尔坐标量测进行滤波，得到一维变量里程数s及其速度的滤波结果。

根据本发明的一种实施方式，所述机动目标为道路上行驶的车辆，所述机动目标的位置量测信息为车辆相对观测雷达坐标系原点的距离和方位角，在所述步骤B中，利用去偏量测转换方法将所述位置量测信息转换为笛卡尔坐标量测。

根据本发明的一种实施方式，在所述步骤C中，建立机动目标在道路x，y方向上的位置关于一维变量里程数s的表达式：

C1：当道路段为直线段时，直线段上的任意点位置满足表达式：

其中，方向角(x₁,y₁)为直线段起始点，(x₂,y₂)为直线段终点；

C2：当道路段为圆弧段时，圆弧段上的任意点位置满足表达式：

其中，(x₁,y₁)为圆弧段起始点，(x₂,y₂)为圆弧段终点，(x₀,y₀)为圆心，圆弧起始角和曲率半径

根据本发明的一种实施方式，所述步骤E包括计算各运动模型对应的模型概率的步骤，以及按模型概率针对各所述运动的滤波结果进行加权融合的步骤。

根据本发明的一种实施方式，所述步骤E滤波包括如下的步骤：

E1：将k-1时刻滤波结果相互作用初值及其方差与转换量测值一起作为k时刻第j个模型的输入值，利用公式进行状态一步预测，其中，Φ_k-1为状态转移矩阵，

E2、计算状态一步预测协方差

$P_{k-1|k}^j={\Phi }_{k-1}{P}_{k-1|k-1}^{j0}{\Phi }_{k-1}^T+Q_{k-1}$

其中Q_k-1过程噪声协方差矩阵；

E3、计算无迹变换δ采样点状态预测

$x_{k|k-1}^{j,i}=\{{\hat{x}}_{k|k-1}^j,{\hat{x}}_{k|k-1}^j\pm {\left(\sqrt{(n+\kappa )P_{k|k-1}^j}\right)}_l\},(i=0,...,2n,l=0,...,n)$

其中κ是尺度参数，其数值满足是均方根矩阵的第l行或第l列，n是状态向量的维数，对应对应

E4、计算δ采样点量测预测

$z_{k|k-1}^{j,i}=h_k\left(x_{k|k-1}^{j,i}\right)$

其中h_k为量测函数，由所述一维坐标约束建模给出；

E5、计算量测预测

${\hat{z}}_{k|k-1}^j=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i^m{z}_{k|k-1}^{j,i}$

其中是无迹变换时对均值加权计算使用的权值

E6、计算新息指量测值与量测预测值的差

$v_k^j=z_k^c-{\hat{z}}_{k|k-1}^j$

E7、计算量测预测协方差即新息协方差

$P_{k,zz}^j=R_k^a+\underset{i=0}{\overset{2n}{\Sigma }}{W}_i^c(z_{k|k-1}^{j,i}-{\hat{z}}_{k|k-1}^j){(z_{k|k-1}^{j,i}-{\hat{z}}_{k|k-1}^j)}^T$

其中，是无迹变换时对均值加权计算使用的权值，为量测噪声协方差矩阵

E8、计算量测和状态向量的交互协方差

$P_{k,xz}^j=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i^c(x_{k|k-1}^{j,i}-{\hat{x}}_{k|k-1}^j){(x_{k|k-1}^{j,i}-{\hat{x}}_{k|k-1}^j)}^T$

E9、确定滤波增益

$K_k^j=P_{k,xz}^j{\left(P_{k,zz}^j\right)}^{-1}$

E10、进行状态更新，获得k时刻j模型滤波结果

${\hat{x}}_{k|k}^j={\hat{x}}_{k|k-1}^j+K_k^j{v}_k^j$

E11、进行协方差更新，获得k时刻j模型滤波误差协方差

$P_{k|k}^j=P_{k|k-1}^j-K_k^j{P}_{k,zz}^j{\left(K_k^j\right)}^T$

E12、模型概率更新：

在得到各个模型k时刻的输出和后，更新该时刻各模型对应的概率计算对应模型j的可能性为

${\Lambda }_k^j=\frac{1}{\sqrt{\left|2{\mathrm{\pi P}}_{k,zz}^j\right|}}\exp [-\frac{1}{2}{\left(v_k^j\right)}^{\prime }{\left(P_{k,zz}^j\right)}^{-1}\left(v_k^j\right)]$

且对运动模型j的模型概率进行更新：

$u_k^j=\frac{{\Lambda }_k^j\underset{i=1}{\overset{N}{\Sigma }}{\rho }_{ij}{u}_{k-1}^i}{\underset{j=1}{\overset{N}{\Sigma }}\left({\Lambda }_k^j\underset{i=1}{\overset{N}{\Sigma }}{\rho }_{ij}{u}_{k-1}^i\right)}$

E13、k时刻交互式多模型滤波的融合输出为

${\hat{x}}_{k|k}=\underset{j=1}{\overset{N}{\Sigma }}{\hat{x}}_{k|k}^j{u}_k^j$

$P_{k|k}=\underset{j=0}{\overset{N}{\Sigma }}{u}_k^j\{P_{k|k}^j+[{\hat{x}}_{k|k}^j-{\hat{x}}_{k|k}\left]{[{\hat{x}}_{k|k}^j-{\hat{x}}_{k|k}]}^T\right\}$

根据本发明的一种实施方式，所述方法还包括基于k时刻针对各运动模型的滤波结果外推下一时刻预测结果，并根据k时刻模型概率对针对各运动模型的预测结果加权融合，得到k+1时刻融合预测结果的步骤。

本发明一些实施方式的一维坐标机动目标建模方法保证了过程噪声的自由度为1，即只在沿道路方向上变化，比起传统的在二维自由空间建模的方法更加贴近物理实际。

根据本发明的实施方式，能够提高跟踪性能。

附图说明

结合附图，可以更好地理解本发明，但是附图仅仅是示例性的，不是对本发明的限制。

图1示出了本发明的机动目标跟踪方法的一种实施方式的流程图；

图2示出了本发明的机动目标跟踪方法的滤波流程图；

图3示出了本发明的一个实施例中道路段参数示意图；

图4和图5示出了本发明线性约束仿真实验的200次蒙特卡洛仿真结果；

图6示出了本发明非线性约束仿真实验的单次轨迹仿真结果；

图7和图8示出了本发明非线性约束仿真实验的200次蒙特卡洛仿真结果。

具体实施方式

下面结合附图和实施例详细说明本发明的实施方式。附图和实施方式中的描述仅仅是示例性的，不是对本发明的保护范围的限制。

图1示出了依据本发明的一种实施方式的流程图。如图1所示，

首先在步骤S210，从观测雷达处获得雷达位置量测信息。在一种实施方式中，雷达位置量测信息为机动目标(该实施例以道路上行驶的车辆为例)相对观测雷达坐标系原点的距离和方位角。至于雷达如何获得这些信息、以及本发明实施方式的方法如何从雷达处获得这些信息，本领域技术人员可以采取各种方式来实现，无论采用哪种方法都在本发明的保护范围内。

接着，在步骤S220，将来自观测雷达的雷达位置量测信息(如上文所示，即机动目标相对观测雷达坐标系原点的距离和方位角)转换为笛卡尔坐标量测。可以采用多种方法进行这种转换，无论采用哪种方法都在本发明的范围内。在一种实施方式中，优选地，采用去偏量测转换方法。

去偏量测转换方法如下：

$z_k^c=\left(\begin{array}{c}{x}_k^c\\ y_k^c\end{array}\right)=\left(\begin{array}{c}{r}_k^{m}cos{\theta }_k^m\\ r_k^m{\mathrm{sin\theta }}_k^m\end{array}\right)-{\mu }_k^a$

其中，代表转换量测向量，代表转换后x方向位置量测，代表转换后y方向位置量测，和是从雷达获取的距离和方位角量测，是基于目标量测位置得到的平均真实偏差。

${\mu }_k^a=\left(\begin{array}{c}{\mu }_{k,x}^a\\ {\mu }_{k,y}^a\end{array}\right)=\left(\begin{array}{c}{r}_k^{m}cos{\theta }_k^m(e^{-{\sigma }_{\theta }^2}-e^{-{\sigma }_{\theta }^2/2})\\ r_k^{m}sin{\theta }_k^m(e^{-{\sigma }_{\theta }^2}-e^{-{\sigma }_{\theta }^2/2})\end{array}\right)$

对应的协方差矩阵为

$R_k^a=\left(\begin{array}{cc}{R}_{k,xx}^a& R_{k,xy}^a\\ R_{k,yx}^a& R_{k,yy}^a\end{array}\right)$

其中

$\left(\begin{array}{c}{R}_{k,xx}^a=r_m^2{e}^{-2{\sigma }_{\theta }^2}\left\{{\cos }^2{\theta }_m\right[\cosh \left(2{\sigma }_{\theta }^2\right)-\cosh \left({\sigma }_{\theta }^2\right)]+{\sin }^2{\theta }_m[\sinh \left(2{\sigma }_{\theta }^2\right)-\sinh \left({\sigma }_{\theta }^2\right)\left]\right\}\\ +{\sigma }_r^2{e}^{-2{\sigma }_{\theta }^2}\left\{{\cos }^2{\theta }_m\right[2\cosh \left(2{\sigma }_{\theta }^2\right)-\cosh \left({\sigma }_{\theta }^2\right)]+{\sin }^2{\theta }_m[\sinh \left(2{\sigma }_{\theta }^2\right)-\sinh \left({\sigma }_{\theta }^2\right)\left]\right\}\end{array}\right)$

$\left(\begin{array}{c}{R}_{k,xx}^a=r_m^2{e}^{-2{\sigma }_{\theta }^2}\left\{{\sin }^2{\theta }_m\right[\cosh \left(2{\sigma }_{\theta }^2\right)-\cosh \left({\sigma }_{\theta }^2\right)]+{\cos }^2{\theta }_m[\sinh \left(2{\sigma }_{\theta }^2\right)-\sinh \left({\sigma }_{\theta }^2\right)\left]\right\}\\ +{\sigma }_r^2{e}^{-2{\sigma }_{\theta }^2}\left\{{\sin }^2{\theta }_m\right[2\cosh \left(2{\sigma }_{\theta }^2\right)-\cosh \left({\sigma }_{\theta }^2\right)]+{\cos }^2{\theta }_m[\sinh \left(2{\sigma }_{\theta }^2\right)-\sinh \left({\sigma }_{\theta }^2\right)\left]\right\}\end{array}\right)$

$R_{k,xy}^a=R_{k,yx}^a={\mathrm{sin\theta }}_m{\mathrm{cos\theta }}_m{e}^{-4{\sigma }_{\theta }^2}[{\sigma }_r^2+(r_m^2+{\sigma }_r^2)(1-e^{{\sigma }_{\theta }^2})]$

然后，在步骤S230，获得车辆的运动轨迹的约束条件，即一维坐标运动建模。这里的一维坐标是以目标所在道路为坐标轴，目标在道路上行驶的里程数为坐标。将道路近似为直线段和曲线段来考虑，参考图3。

对于直线段来说，必备的参数是起始点(x₁,y₁)和终点(x₂,y₂)。有了这两个点，道路直线段的长度S和方向角θ可以表示为：

$S=\sqrt{{(x_2-x_1)}^2+{(y_2-y_1)}^2}$

$\theta ={\tan }^{-1}\left(\frac{y_2-y_1}{x_2-x_1}\right)$

被估计的状态变量是弧长或者称其为路程，包括它对应的速度和加速度，用s来表示。那么直线段上的任意点可以表示为：

对于圆弧段来说，除了起始点(x₁,y₁)和终点(x₂,y₂)这两个先验信息外，我们还需要圆心(x₀,y₀)来确定圆弧起始角θ₀和曲率半径R，具体如下：

${\theta }_0={\tan }^{-1}\left(\frac{y_1-y_0}{x_1-x_0}\right)$

$\left(\begin{array}{c}R=\sqrt{{(x_0-x_1)}^2+{(y_0-y_1)}^2}\\ =\sqrt{{(x_0-x_2)}^2+{(y_0-y_2)}^2}\end{array}\right)$

应该注意，在该式中，两个等号分别对应利用圆弧段起点和终点计算曲率半径的形式，不是递进关系而是并列关系。

同样以s作为变量，圆弧段上的任意点可以表示为：

由此得到目标x，y方向位置关于里程数s的表达式，该表达式可以用于量测方程构建和滤波结果转换中。

接着，在步骤S240，根据目标运动特点设置运动模型。针对不同应用场景采用不同运动模型来描述目标运动过程，运动模型主要由目标运动特性决定，影响状态方程的建立。在本发明的一些实施方式中，道路上行驶的车辆主要做近匀速或近匀加速运动，因此，运动模型分为近匀速模型NCV和近匀加速模型NCA。

之后，在步骤S250，进行模型滤波器初始化。

为进行滤波，需要建立状态方程和系统观测方程。状态方程和系统观测方程应该在滤波器初始化前进行，即建立方程后再初始化，然后进行滤波。

状态方程是目标运动规律的假设，根据选定的运动模型写出目标状态的递推表达式x_k＝Φ_kx_k-1+Γ_kv_k-1，其中Φ_k是状态转移矩阵，Γ_k是过程噪声分布矩阵，与运动模型有关。

${\Phi }_k=\left(\begin{array}{cc}1& T\\ 0& 1\end{array}\right),{\Gamma }_k=\left(\begin{array}{c}0.5T^2\\ T\end{array}\right)$

或

分别对应近匀速NCV模型和近匀加速NCA模型，状态变量分别为和采用NCA模型时需要先将状态扩维，引入加速度分量，滤波后再降维，T为雷达扫描周期，v_k-1为零均值高斯白噪声，其协方差为

其中q_s是沿道路方向的过程噪声标准差；

系统观测方程为

$z_k^c=\left(\begin{array}{c}{x}_k^c\\ y_k^c\end{array}\right)=h_k\left(x_k\right)+w_k$

其中h_k(x_k)由所述S1中推导出的里程s与x，y坐标间关系给出，w_k为去偏转换测量噪声。

尽管为了描述的方便，将模型滤波器的初始化设为单独的一个步骤，但是根据上下文，其也可以认为是后面滤波步骤的一部分。

尽管滤波初始化可以通过一维观测独立完成，但这样会造成信息浪费，初始化精度也不理想。根据本发明的一种实施方式，采用最小均方误差融合的方法将多维观测融合进行滤波初始化，既避免了信息浪费又提高了初始化精度。具体过程如下：

对于随机变量s＝f(x,y)，其方差与x,y方差之间的关系为：

${\sigma }_s^2={\left(\frac{\partial f}{\partial x}\right)}^2{\sigma }_x^2+{\left(\frac{\partial f}{\partial y}\right)}^2{\sigma }_y^2$

其中和是随机变量x，y的方差。

若s可分别由x，y单独表示，即s＝f₁(x)，s＝f₂(y)，则可分别求出s对x，y的方差：

$\left(\begin{array}{c}{\left({\sigma }_s^x\right)}^2={\left(\frac{\partial f_1}{\partial x}\right)}^2{\sigma }_x^2\\ {\left({\sigma }_s^y\right)}^2={\left(\frac{\partial f_2}{\partial y}\right)}^2{\sigma }_y^2\end{array}\right)$

此时估计的融合公式为：

$s_k^m=\frac{{\sigma }_s^2}{{\left({\sigma }_s^x\right)}^2}{\hat{s}}_{k,x}^m+\frac{{\sigma }_s^2}{{\left({\sigma }_s^y\right)}^2}{\hat{s}}_{k,y}^m$

其中和是分别是采用x,y一维观测进行初始化的结果，一维观测初始化方法随轨迹形状不同而不同，以直线段道路轨迹为例，一维观测初始化方法为为融合后的方差：

${\sigma }_s^2=\frac{{\left({\sigma }_s^x\right)}^2{\left({\sigma }_s^y\right)}^2}{{\left({\sigma }_s^x\right)}^2+{\left({\sigma }_s^y\right)}^2}.$

之后，在步骤S260进行滤波。

执行交互式多模型滤波，此时已知上一时刻各模型滤波器状态估计结果状态估计误差协方差上一时刻模型概率(上一时刻滤波结果)，本时刻转换量测(雷达得到)，过程噪声协方差矩阵Q_k-1，量测噪声协方差矩阵状态转移矩阵Φ_k-1，量测函数h_k(先验已知)。

首先是进行状态估计值之间的相互作用，目的是得到对应j模型滤波器的混合初值及其协方差

子滤波器模型M_j在k-1时刻的混合状态初值由模型M_i的输出相应的模型概率以及过渡概率ρ_ij来计算，即

$u_{k-1|k-1}\left(i\right|j)=\frac{{\rho }_{ij}{u}_{k-1}^i}{\underset{i=1}{\overset{N}{\Sigma }}{\rho }_{ij}{u}_{k-1}^i}$

${\hat{x}}_{k-1|k-1}^{j0}=\underset{i=1}{\overset{N}{\Sigma }}{\hat{x}}_{k-1|k-1}^i·u_{k-1|k-1}\left(i\right|j)$

在k-1时刻，子滤波器模型M_j的状态误差协方差阵为

$P_{k-1|k-1}^{j0}=\underset{i=0}{\overset{N}{\Sigma }}\{P_{k-1|k-1}^i+[{\hat{x}}_{k-1|k-1}^i-{\hat{x}}_{k-1|k-1}^{j0}\left]{[{\hat{x}}_{k-1|k-1}^i-{\hat{x}}_{k-1|k-1}^{j0}]}^T\right\}{u}_{k-1|k-1}\left(i\right|j)$

其中，为k-1时刻第i个子滤波器的状态估计，为相应的状态协方差矩阵，N为模型数目，

将状态向量及其方差与转换量测值一起作为k时刻第j个模型的输入值，由于状态变量与量测间存在非线性关系，选用无迹卡尔曼滤波器(UKF)进行滤波。

各模型滤波器单独滤波。由于状态向量和量测向量之间是非线性关系，需要非线性滤波方法来处理。可以采用多种非线性滤波方法，无论采用哪种方法都在本发明的范围内。在一种实施方式中，优选地，采用无迹卡尔曼滤波方法。该方法通过一系列精确选择的δ采样点经过非线性模型的映射来传递随机量的统计特性，是一种有效的非线性滤波方法。

单独滤波目的是得到对应j模型滤波器的滤波结果及其协方差滤波过程参考图2。

计算状态一步预测

${\hat{x}}_{k|k-1}^j={\Phi }_{k-1}{\hat{x}}_{k-1|k-1}^{j0}$

其中，Φ_k-1为状态转移矩阵，为相互作用得到的混合初值。

计算协方差一步预测

$P_{k-1|k}^j={\Phi }_{k-1}{P}_{k-1|k-1}^{j0}{\Phi }_{k-1}^T+Q_{k-1}$

其中，Q_k-1为过程噪声协方差矩阵

计算无迹变换δ采样点状态预测

$x_{k|k-1}^{j,i}=\{{\hat{x}}_{k|k-1}^j,{\hat{x}}_{k|k-1}^j\pm {\left(\sqrt{(n+\kappa )P_{k|k-1}^j}\right)}_l\},(i=0,...,2n,l=0,...,n)$

其中κ是尺度参数，可以为任何数值，只要(n+κ)≠0。是均方根矩阵的第l行或第l列，n是状态向量的维数，对应n＝2；对应

计算δ采样点量测预测

$z_{k|k-1}^{j,i}=h_k\left(x_{k|k-1}^{j,i}\right)$

其中h_k为量测函数，由一维坐标约束建模给出

计算量测预测

${\hat{z}}_{k|k-1}^j=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i^m{z}_{k|k-1}^{j,i}$

其中是无迹变换时对均值加权计算使用的权值

计算新息指量测值与量测预测值的差

$v_k^j=z_k^c-{\hat{z}}_{k|k-1}^j$

计算量测预测协方差即新息协方差

$P_{k,zz}^j=R_k^a+\underset{i=0}{\overset{2n}{\Sigma }}{W}_i^c(z_{k|k-1}^{j,i}-{\hat{z}}_{k|k-1}^j){(z_{k|k-1}^{j,i}-{\hat{z}}_{k|k-1}^j)}^T$

其中是无迹变换时对均值加权计算使用的权值，为量测噪声协方差矩阵

量测和状态向量的交互协方差

$P_{k,xz}^j=\underset{i=0}{\overset{2n}{\Sigma }}{W}_i^c(x_{k|k-1}^{j,i}-{\hat{x}}_{k|k-1}^j){(x_{k|k-1}^{j,i}-{\hat{x}}_{k|k-1}^j)}^T$

确定滤波增益

$K_k^j=P_{k,xz}^j{\left(P_{k,zz}^j\right)}^{-1}$

进行状态更新，获得k时刻j模型滤波结果

${\hat{x}}_{k|k}^j={\hat{x}}_{k|k-1}^j+K_k^j{v}_k^j$

进行协方差更新，获得k时刻j模型滤波误差协方差

$P_{k|k}^j=P_{k|k-1}^j-K_k^j{P}_{k,zz}^j{\left(K_k^j\right)}^T$

其中和分别是对均值和协方差加权计算时使用的权值

$\left(\begin{array}{c}{W}_0^m=\frac{\kappa }{n+\kappa },W_0^c=\frac{\kappa }{n+\kappa }+(1-{\alpha }^2+\beta )\\ W_i^m=W_i^c=\frac{\kappa }{2(n+\kappa )}\\ W_{i+n}^m=W_{i+n}^c=\frac{\kappa }{2(n+\kappa )}\end{array}\right),(i=1,...,n)$

同样其中κ是尺度参数，可以为任何数值，只要(n+κ)≠0；n是状态向量的维数，对应对应a决定采样点在均值周围的散布，优选设定为比较小的正值，β用于引入变量的先验分布信息，优选高斯分布下设定β＝2。

模型概率更新，目的是得到j模型对应的模型概率

在得到各个模型k时刻的输出和后，更新该时刻各模型对应的概率计算对应模型j的可能性为

${\Lambda }_k^j=\frac{1}{\sqrt{\left|2{\mathrm{\pi P}}_{k,zz}^j\right|}}\exp [-\frac{1}{2}{\left(v_k^j\right)}^{\prime }{\left(P_{k,zz}^j\right)}^{-1}\left(v_k^j\right)]$

模型j的模型更新为

$u_k^j=\frac{{\Lambda }_k^j\underset{i=1}{\overset{N}{\Sigma }}{\rho }_{ij}{u}_{k-1}^i}{\underset{j=1}{\overset{N}{\Sigma }}\left({\Lambda }_k^j\underset{i=1}{\overset{N}{\Sigma }}{\rho }_{ij}{u}_{k-1}^i\right)}$

状态估计融合，目的是得到融合滤波结果及其协方差P_k|k

k时刻交互式多模型滤波的融合输出为

${\hat{x}}_{k|k}=\underset{j=1}{\overset{N}{\Sigma }}{\hat{x}}_{k|k}^j{u}_k^j$

$P_{k|k}=\underset{j=0}{\overset{N}{\Sigma }}{u}_k^j\{P_{k|k}^j+[{\hat{x}}_{k|k}^j-{\hat{x}}_{k|k}\left]{[{\hat{x}}_{k|k}^j-{\hat{x}}_{k|k}]}^T\right\}$

于是便得到了对量测值的滤波结果及其协方差，接着对目标状态(包括位置和速度)进行外推预测。

最后，在步骤S270，将滤波结果转换到笛卡尔坐标系下

利用步骤S230中推导出的里程s与x，y坐标间关系将交互式多模型滤波结果转换到笛卡尔坐标系下，得到目标位置和速度滤波结果然后转第一步等待下一时刻k+1时刻雷达观测数据。

本发明所述的公路坐标运动建模方法和跟踪方法为约束目标跟踪提供了一条新途径，合理利用等式约束条件的同时有效避免了信息浪费。本发明的一些实施方式的公路坐标运动建模方法的一个优点在于保证了过程噪声的自由度为1，即只在沿道路方向上变化，比起传统的在二维自由空间建模的方法更加贴近物理实际。这种建模方法还使得目标动态模型与其运动轨迹独立，在保证滤波结果是满足约束条件的同时大大降低了复杂道路约束对滤波精度的影响，有效提高滤波性能。在跟踪方法中引入交互式多模型方法，提高了应对目标可能存在的变速机动的能力，多个模型同时作用有效匹配目标实际运动模型，减少模型失配导致的性能恶化。

为展示本发明的实施方式的效果，设置两组仿真场景分别对应线性等式约束和非线性等式约束。首先在线性等式约束场景中验证方法性能，跟踪一个在道路直线段上匀速行驶的目标。将本发明提出的约束坐标卡尔曼滤波方法(CCKF)与几种经典的约束估计算法进行比较，包括完美量测法(PMKF)，估计后投影法(EPKF)，线性等式约束卡尔曼滤波方法(LECKF)以及模型降维法(MRKF)。此外还选取一种经典的无约束算法，转换量测法(CMKF)作为对比。选取均方根误差(RMSE)作为性能的量度，进行200次蒙特卡洛仿真。

仿真结果参见图4、5所示,可以明显看出CCKF方法成功将约束条件引入跟踪系统中，性能明显优于非约束的CMKF方法。与其他约束估计算法相比，CCKF也展现出一定的性能优势。这是由于其采用了不同的目标运动建模方法，运动模型更加贴近物理实际。

接着在非线性场景中验证交互式多模型约束坐标卡尔曼滤波方法(IMM-CCKF)跟踪机动目标的能力，跟踪一个在近似田径跑道形状道路段上变速运动的目标。将IMM-CCKF与包含匀速(CV)、匀加速(CA)和坐标转弯(CT)模型的交互式多模型算法(IMM-CVCACT、IMM-CVCA)对比。同样选取均方根误差(RMSE)作为性能的量度。单次轨迹仿真结果如图6所示，可以看出，滤波结果是满足约束条件的，证明IMM-CCKF可以有效引入约束条件并保证滤波结果满足约束条件。

200次蒙特卡洛仿真结果如图7、8所示，可以明显看出IMM-CCKF算法性能整体优于其他两种交互式多模型算法，尤其是在模型转换的时候，其他两种算法均出现了明显的滤波发散现象，而IMM-CCKF的曲线波动相对较为平缓，基本保持在相近的水平上。这是由于IMM-CCKF算法运动模型更加贴近物理实际，同时目标的动态模型与其运动轨迹独立，非线性约束对滤波过程的影响很小，因此得到了比较好的结果。

因而，本发明的一些实施方式将目标所在道路作为一维坐标系，于是道路上目标的运动便可以由其行驶里程数和速度随时间的变化来描述。再将整个公路分成多个道路段分别用直线段和圆弧段来近似，建立目标位置和行驶里程数之间的关系，在一维空间内对行驶里程数进行滤波。滤波跟踪过程利用无迹卡尔曼滤波(UKF)来处理一维状态变量和笛卡尔坐标量测之间存在的非线性关系，并结合交互式多模型(IMM)方法，引入近匀速(NCV)和近匀加速(NCA)模型应对目标存在机动的情况，得到一维滤波结果，最后将一维滤波结果转换到笛卡尔坐标系，得到目标位置的滤波结果。

另外，本发明所述的建模方法使得目标动态模型与其运动轨迹独立，在保证滤波结果是满足约束条件的同时大大降低了复杂道路约束对滤波精度的影响，有效提高滤波性能。

本发明所述的公路坐标机动目标跟踪方法中引入交互式多模型方法，提高了应对目标可能存在的变速机动的能力，多个模型同时作用有效匹配目标实际运动模型，减少模型失配导致的性能恶化。

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