CN104614751A - 基于约束信息的目标定位方法 - Google Patents

Abstract

本发明公开了一种基于约束信息的目标定位方法，用于对机动目标实时跟踪定位，包括以下步骤：首先进行约束模型建模；接着进行系统降维和关联测量值与新变量；其次，获取测量信息，并设计递归滤波器；然后重构状态值和误差协方差的当前估计值；重复上述步骤对机动目标进行实时跟踪定位。本发明通过系统降维和关联测量值与新变量，简化了原始数学模型和计算复杂度，降低计算量，利于得到更优的估计效果，提高了定位精度；通过设计递归滤波器，根据新变量和误差协方差上一时刻的估计值以及测量值的当前值，求出新变量和误差协方差的当前估计值，有效实现对机动目标的实时跟踪定位，且无需保留更前面的测量值与约束信息，节省了存储空间。

Description

基于约束信息的目标定位方法

技术领域

本发明涉及目标跟踪技术领域，具体涉及一种基于约束信息的目标定位方法。

背景技术

机动目标跟踪是一门涉及多学科交叉的技术，目标定位是指利用来自若干个传感器的目标观测数据集，对目标的运行状态进行估计和预测的过程。随着科技的不断进步，机动目标的类型越来越多，运行速度越来越快，而仅凭传感器的输出很难实现快速跟踪与精准定位，因此必须充分利用一切有用信息以提高目标定位的精度。

事实上，通过分析目标所处的环境及运行的轨迹往往可挖掘出有用的先验信息，将先验信息进行合理利用可达到提高估计精度的目的。如，当车辆沿直线运动时，车辆的运行轨迹与道路的走向保持一致，若道路的走向已预先获知，则可间接得到有关状态的先验信息；目标在做匀速圆周运动时，满足速度与加速度点乘为零的约束关系，利用该约束关系可以得到比直线运动时更好的跟踪结果；在组合导航定位系统中，通过在不同的位置分别安装GPS，惯性测量单元(IMU)和CCD相机等传感器进行距离测量，并根据各个传感器之间的相对位置以精确定位。以上这些均称为约束条件，合理利用约束条件蕴含的约束信息，并结合传感器的输出，可有效的提高状态的估计精度，实现对机动目标的精准定位。

近年来，国内外基于约束条件的状态估计与目标定位方法的研究有很多，且取得了一些研究成果。C.Rao等人在《Constrained linear state estimation―amoving horizon approach》(Automatica,2001,37,pp1619–1628)一文中以恒速运行的机动目标为对象，提取出目标速度与加速度向量始终保持正交这一关系作为已知的先验信息，同时结合卡尔曼滤波的计算过程，提出了更优的滤波算法，并论证了滤波器的渐进无偏收敛性，然而该方法不具有递归功能，不适合对机动目标的跟踪定位。之后，D.Simon等人在《Kalman filtering with state equalityconstraints》(Trans.AES,2002,38,pp.128-136)一文中提出了基于投影(PJ)的方法，通过将卡尔曼滤波结果与约束空间相结合得到新的约束估计，具体表现为解一个拉格朗日方程实现目标定位，然而该方法的定位精度仍然有待提高。Ko等人在《State estimation for linear systems with state equality constraints》(Automatica,2007,43:1363–1368)一文中对约束系统的一些参数做了假设，并在此基础上推导出一个约束预测器，验证了该预测器比投影方法具有更精确的估计结果，为舰船定位和导航提供了可靠的辅助监控数据，然而该方法中所做的几个假设条件限制了该方法的应用范围。之后，Hewett等人在《A robust null spacemethod for linear equality constrained state estimation》(Trans.SP 2010,58,3961–3971)一文中提出了基于零空间分解(NS)的定位方法，认为系统的状态向量由固定部分和随机部分共同构成，首先分别得到二者的估计值，接着通过重构得到原约束系统状态的估计值，然而该方法在降阶的过程中损失了部分信息，导致估计精度不高，无法实现目标的精准定位。

发明内容

本发明为了克服以上不足，提供了一种不仅能实现机动目标实时跟踪定位，且可以有效提高定位精度的基于约束信息的目标定位方法。

为了解决上述技术问题，本发明的技术方案是：一种基于约束信息的目标定位方法，用于对机动目标实时跟踪定位，包括以下步骤：

S1：约束模型建模，分别建立状态值x_k、测量值z_k以及约束方程的数学表达式；

S2：系统降维，根据约束方程，寻找一个新变量用以表示其他变量，得到关于新变量的降维方程，对状态值x_k的表达式进行降维；

S3：关联测量值z_k与新变量，建立测量值z_k与新变量的表达关系；

S4：获取测量信息，通过传感器实时获取机动目标的状态信息的测量值z_k；

S5：设计递归滤波器，根据新变量和误差协方差上一时刻的估计值以及测量值z_k的当前值，设计递归滤波器，求出新变量和误差协方差的当前估计值；

S6：重构状态值x_k和误差协方差的当前估计值，根据新变量的当前值求出原始状态值x_k和误差协方差的当前估计值；

S7：重复步骤S2-S6，对机动目标进行实时跟踪定位。

进一步的，所述步骤S1中，状态值x_k和测量值z_k满足数学表达式：

x_k＝A_kx_k-1+Γ_ku_k-1+w_k-1

z_k＝H_kx_k+v_k

约束方程：D_kx_k＝d_k

其中，k为采样时刻值；状态值 $x_k={\left[\begin{array}{cccc}{x}_k^1& x_k^2& x_k^3& x_k^4\end{array}\right]}^T,$ 包含四个状态元素；u_k-1为已知输入项；过程噪声w_k和v_k为均值为零的高斯白噪声；w_k和v_k的协方差分别为U_k和R_k；A_k是n×n的状态转移矩阵，Γ_k是n×p的输入矩阵，H_k是m×n的测量矩阵，D_k是s×n的行满秩约束矩阵，d_k是s×1的约束向量，m、n、p、s均为正整数，且s<n。

进一步的，所述步骤S2具体包括以下步骤：

S21：将约束矩阵D_k进行分解，表示成 $D_k=\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]{E_k}^{-1},$ 其中E_k为可逆矩阵，为由D_k的其中s列构成的满秩方阵，D_k(·,k_j)表示矩阵D_k的第k_j列，j＝1,2,…,s；为D_k余下的n-s列；

S22：寻找新变量y_2,k，对状态值x_k的数学表达式进行降维，令对应D_k的分块将y_k进行分块表示成 $y_k=\left[\begin{array}{c}{y}_{1,k}\\ y_{2,k}\end{array}\right],$ 并根据约束方程用y_2,k表示y_1,k，得到关于新变量y_2,k的降维方程。

进一步的，所述降维方程为：

${\stackrel{\&OverBar;}{D}}_k{y}_{2,k}={\stackrel{\&OverBar;}{A}}_k{y}_{2,k-1}+{\stackrel{\&OverBar;}{d}}_k+E_k^{-1}{w}_{k-1}$

其中， ${\stackrel{\&OverBar;}{A}}_k={\tilde{A}}_k{\stackrel{\&OverBar;}{D}}_{k-1};$

${\tilde{A}}_k=E_k^{-1}{A}_k{E}_{k-1};$

${\stackrel{\&OverBar;}{d}}_k={\tilde{A}}_k{\stackrel{\&OverBar;}{D}}_{k-1}^1{d}_{k-1}+E_k^{-1}{\mathrm{\&Gamma;}}_k{u}_{k-1}-{\stackrel{\&OverBar;}{D}}_k^1{d}_k;$

${\stackrel{\&OverBar;}{D}}_k=\left[\begin{array}{c}-{\left({\stackrel{\&OverBar;}{D}}_k^1\right)}^{-1}{\stackrel{\&OverBar;}{D}}_k^2\\ I\end{array}\right];$

${\stackrel{\&OverBar;}{D}}_k^1=\left[\begin{array}{c}-{\left(D_k^1\right)}^{-1}\\ 0\end{array}\right].$

进一步的，所述步骤S3中，测量值z_k与新变量y_2,k的表达关系为：

$z_k={\stackrel{\&OverBar;}{H}}_k{y}_{2,k}+{\tilde{d}}_k+v_k$

其中， ${\stackrel{\&OverBar;}{H}}_k={\tilde{H}}_k{\stackrel{\&OverBar;}{D}}_k;$ ${\tilde{H}}_k=H_k{E}_k;$ ${\tilde{d}}_k={\tilde{H}}_k{\stackrel{\&OverBar;}{D}}_k{d}_k.$

进一步的，所述步骤S5具体包括以下步骤：

S51：获得k-1时刻，即上一时刻y_2,k-1的估计值与误差协方差的估计值P_2,k-1|k-1；

S52：根据最小二乘算法获得y_2,k k时刻，即当前时刻的预测值与误差协方差当前时刻的预测值P_2,k|k-1；

S53：结合当前时刻的测量值z_k，得到y_2,k的当前估计值和误差协方差的当前估计值P_2,k|k。

进一步的，所述步骤S52中根据最小二乘算法获得的和P_2,k|k-1分别为：

${\hat{y}}_{2,k|k-1}={\left({\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{\stackrel{\&OverBar;}{D}}_k\right)}^{-1}{\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{b}_k$

$P_{2,k|k-1}={\left({\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{\stackrel{\&OverBar;}{D}}_k\right)}^{-1}$

其中，

$b_k={\stackrel{\&OverBar;}{A}}_k{\hat{y}}_{2,k-1|k-1}+{\stackrel{\&OverBar;}{d}}_k;$

${\stackrel{\&OverBar;}{U}}_{k-1}={\stackrel{\&OverBar;}{E}}_k^{-1}{U}_{k-1}{E}_k^{-T}+{\stackrel{\&OverBar;}{A}}_k{P}_{2,k-1|k-1}{\stackrel{\&OverBar;}{A}}_{k-1}^T.$

进一步的，所述步骤S53中和P_2,k|k分别为：

${\hat{y}}_{2,k|k}={\hat{y}}_{2,k|k-1}+K_k{\tilde{z}}_{k|k-1}$

$P_{2,k|k}=(I-K_k{\stackrel{\&OverBar;}{H}}_k)P_{2,k|k-1}$

其中，

$K_k=P_{2.k|k-1}{\stackrel{\&OverBar;}{H}}_k^T{({\stackrel{\&OverBar;}{H}}_k{P}_{2,k|k-1}{\stackrel{\&OverBar;}{H}}_k^T+R_k)}^{-1};$

${\tilde{z}}_{k|k-1}=z_k-{\stackrel{\&OverBar;}{H}}_k{\hat{y}}_{2,k|k-1}-{\tilde{d}}_k.$

进一步的，所述步骤S6具体包括：

S61：重构得到y_k的当前估计值和误差协方差的当前估计值

S62：根据和得到原始状态值x_k的当前估计值和原始误差协方差的当前估计值P_k|k。

进一步的，所述步骤S61中和分别为：

${\hat{y}}_{k|k}={\stackrel{\&OverBar;}{D}}_k{\hat{y}}_{2,k|k}+{\stackrel{\&OverBar;}{D}}_k^1{d}_k$

$P_{k|k}^{\left(y\right)}={\stackrel{\&OverBar;}{D}}_k{P}_{2,k|k}{\stackrel{\&OverBar;}{D}}_k^T$

本发明提供的基于约束信息的目标定位方法，通过系统降维和关联测量值z_k与新变量，利用约束方程消去了状态值x_k和测量值z_k中的部分变量，简化了原始数学模型和计算复杂度，降低计算量，利于得到更优的估计效果，提高了定位精度；通过设计递归滤波器，根据新变量和误差协方差上一时刻的估计值以及测量值z_k的当前值，递归求出新变量和误差协方差的当前估计值，有效实现对机动目标的实时跟踪定位，且无需保留更前面的测量值与约束信息，节省了存储空间；此外本发明最大程度上利用了约束信息，且估计过程中未损失约束信息，进一步提高了定位精度。

附图说明

图1是本发明基于约束信息的目标定位方法的流程图；

图2是本发明机动目标的运动模拟图；

图3是本发明与现有三种方法针对正北方向位移进行估计的均方根误差比较图；

图4是本发明与现有三种方法针对正北方向速度进行估计的均方根误差比较图。

具体实施方式

下面结合附图对本发明作详细描述：

如图1所示，本发明提供一种基于约束信息的目标定位方法，用于对机动目标实时跟踪定位，包括以下步骤：

S1：约束模型建模，分别建立状态值x_k、测量值z_k以及约束方程的数学表达式；具体的，状态值x_k和测量值z_k分别满足数学表达式(1)和(2)：

x_k＝A_kx_k-1+Γ_ku_k-1+w_k-1     (1)

z_k＝H_kx_k+v_k        (2)

约束方程：D_kx_k＝d_k     (3)

其中，k为采样时刻值；状态值 $x_k={\left[\begin{array}{cccc}{x}_k^1& x_k^2& x_k^3& x_k^4\end{array}\right]}^T,$ 包含四个状态元素；u_k-1为已知输入项；过程噪声w_k和v_k为均值为零的高斯白噪声；w_k和v_k的协方差分别为U_k和R_k；A_k是n×n的状态转移矩阵，Γ_k是n×p的输入矩阵，H_k是m×n的测量矩阵，D_k是s×n的行满秩约束矩阵，d_k是s×1的约束向量，m、n、p、s均为正整数，且s<n。

S2：系统降维，根据约束方程，寻找一个新变量用以表示其他变量，得到关于新变量的降维方程，对状态值x_k的表达式进行降维；具体包括以下步骤：

S21：将约束矩阵D_k进行分解，表示成 $D_k=\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]{E_k}^{-1},$ 其中E_k为可逆矩阵，为由D_k的其中s列构成的满秩方阵，D_k(·,k_j)表示矩阵D_k的第k_j列，j＝1,2,…,s；为D_k余下的n-s列，需要说明的是，由于D_k行满秩，则必然存在其中的s列可构成一个满秩方阵由于矩阵 $\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]$ 可以由D_k经过最多s次初等变换得到，则必然可找到一个可逆矩阵E_k使得 $D_k{E}_k=\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]$ 成立。

S22：寻找新变量y_2,k，对状态值x_k的数学表达式进行降维，令对应D_k的分块将y_k进行分块表示成 $y_k=\left[\begin{array}{c}{y}_{1,k}\\ y_{2,k}\end{array}\right],$ 并根据约束方程用y_2,k表示y_1,k，得到关于新变量y_2,k的降维方程。具体的，约束方程(3)可写成：

$d_k=D_k{x}_k=D_k{E}_k{E}_k^{-1}{x}_k=\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]y_k---\left(4\right)$

其中即为新的状态值。

与D_k的分块相对应，将y_k进行分块表示成 $y_k=\left[\begin{array}{c}{y}_{1,k}\\ y_{2,k}\end{array}\right],$ 其中y_1,k由y_k的前s个元素组成，y_2,k由y_k的后n-s个元素组成，则约束方程(3)可表示为：

$d_k=\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]\left[\begin{array}{c}{y}_{1,k}\\ y_{2,k}\end{array}\right]=D_k^1{y}_{1,k}+D_k^2{y}_{2,k}---\left(5\right)$

将(1)式左右两端同时左乘矩阵可得

$y_k={\tilde{A}}_k{y}_{k-1}+E_k^{-1}{\mathrm{\&Gamma;}}_k{u}_{k-1}+E_k^{-1}{w}_{k-1}---\left(6\right)$

其中

${\tilde{A}}_k=E_k^{-1}{A}_k{E}_{k-1}$

由于可逆，根据(5)式和 $y_k=\left[\begin{array}{c}{y}_{1,k}\\ y_{2,k}\end{array}\right],$ 可将y_1,k和y_k分别表示成y_2,k的关系式：

$y_{1,k}={\left(D_k^1\right)}^{-1}(d_k-D_k^2{y}_{2,k})---\left(7\right)$

$y_k={\stackrel{\&OverBar;}{D}}_k{y}_{2,k}+{\stackrel{\&OverBar;}{D}}_k^1{d}_k---\left(8\right)$

其中

${\stackrel{\&OverBar;}{D}}_k=\left[\begin{array}{c}-{\left({\stackrel{\&OverBar;}{D}}_k^1\right)}^{-1}{\stackrel{\&OverBar;}{D}}_k^2\\ I\end{array}\right]$

${\stackrel{\&OverBar;}{D}}_k^1=\left[\begin{array}{c}-{\left(D_k^1\right)}^{-1}\\ 0\end{array}\right]$

结合方程(4)可得(1)式的降维方程为：

${\stackrel{\&OverBar;}{D}}_k{y}_{2,k}={\stackrel{\&OverBar;}{A}}_k{y}_{2,k-1}+{\stackrel{\&OverBar;}{d}}_k+E_k^{-1}{w}_{k-1}---\left(9\right)$

其中

${\stackrel{\&OverBar;}{A}}_k={\tilde{A}}_k{\stackrel{\&OverBar;}{D}}_{k-1};$

${\stackrel{\&OverBar;}{d}}_k={\tilde{A}}_k{\stackrel{\&OverBar;}{D}}_{k-1}^1{d}_{k-1}+E_k^{-1}{\mathrm{\&Gamma;}}_k{u}_{k-1}-{\stackrel{\&OverBar;}{D}}_k^1{d}_k.$

S3：关联测量值z_k与新变量，建立测量值z_k与新变量y_2,k的表达关系，具体的，根据(3)式、(8)式以及可得到测量值z_k与新变量y_2,k的表达关系如下：

$\begin{array}{c}{z}_k={\tilde{H}}_k{y}_k+v_k\\ ={\stackrel{\&OverBar;}{H}}_k{y}_{2,k}+{\tilde{d}}_k+v_k\end{array}---\left(10\right)$

其中

${\stackrel{\&OverBar;}{H}}_k=H_k{E}_k;$

${\stackrel{\&OverBar;}{H}}_k={\tilde{H}}_k{\stackrel{\&OverBar;}{D}}_k;$

${\tilde{d}}_k={\tilde{H}}_k{\stackrel{\&OverBar;}{D}}_k{d}_k.$

S4：获取测量信息，通过传感器实时获取机动目标的状态信息的测量值z_k，根据式(2)可知，该传感器的测量值z_k除了包含机动目标当前的状态信息外，还不可避免的受环境等噪声的污染，这里假设噪声v_k的统计特性已知。

S5：设计递归滤波器，根据新变量和误差协方差上一时刻的估计值以及测量值z_k的当前值，设计递归滤波器，求出新变量和误差协方差的当前估计值；具体包括以下步骤：

S51：获得k-1时刻，即上一时刻y_2,k-1的估计值与误差协方差的估计值P_2,k-1|k-1；

S52：根据最小二乘算法获得y_2,k k时刻即当前时刻的预测值与误差协方差当前时刻的预测值P_2,k|k-1，具体的，根据最小二乘算法获得的和P_2,k|k-1分别为：

${\hat{y}}_{2.k|k-1}={\left({\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{\stackrel{\&OverBar;}{D}}_k\right)}^{-1}{\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{b}_k---\left(11\right)$

$P_{2,k|k-1}={\left({\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{\stackrel{\&OverBar;}{D}}_k\right)}^{-1}---\left(12\right)$

其中，

$b_k={\stackrel{\&OverBar;}{A}}_k{\hat{y}}_{2,k-1|k-1}+{\stackrel{\&OverBar;}{d}}_k;$

${\stackrel{\&OverBar;}{U}}_{k-1}={\stackrel{\&OverBar;}{E}}_k^{-1}{U}_{k-1}{E}_k^{-T}+{\stackrel{\&OverBar;}{A}}_k{P}_{2,k-1|k-1}{\stackrel{\&OverBar;}{A}}_{k-1}^T.$

S53：结合当前时刻的测量值z_k，得到y_2,k的当前估计值和误差协方差的当前估计值P_2,k|k分别为：

$P_{2,k|k}=(I-K_k{\stackrel{\&OverBar;}{H}}_k)P_{2,k|k-1}---\left(14\right)$

其中，

$K_k=P_{2,k|k-1}{\stackrel{\&OverBar;}{H}}_k^T{({\stackrel{\&OverBar;}{H}}_k{P}_{2,k|k-1}{\stackrel{\&OverBar;}{H}}_k^T+R_k)}^{-1}$

${\tilde{z}}_{k|k-1}=z_k-{\stackrel{\&OverBar;}{H}}_k{\hat{y}}_{2,k|k-1}-{\tilde{d}}_k.$

S6：重构状态值x_k和误差协方差的当前估计值，根据新变量的当前值求出原始状态值x_k和误差协方差的当前估计值，具体包括：

S61：重构得到y_k的当前估计值和误差协方差的当前估计值分别满足：

${\hat{y}}_{k|k}={\stackrel{\&OverBar;}{D}}_k{\hat{y}}_{2,k|k}+{\stackrel{\&OverBar;}{D}}_k^1{d}_k---\left(15\right)$

$P_{k|k}^{\left(y\right)}={\stackrel{\&OverBar;}{D}}_k{P}_{2,k|k}{{\stackrel{\&OverBar;}{D}}_k}^T;---\left(16\right)$

S62：根据和得到原始状态值x_k的当前估计值和原始误差协方差的当前估计值P_k|k分别为：

${\hat{x}}_{k|k}=E_k{\hat{y}}_{k|k}---\left(17\right)$

$P_{k|k}=E_k{P}_{k|k}^{\left(y\right)}{E}_k^T---\left(18\right)$

其中

S7：重复步骤S2-S6，对机动目标进行实时跟踪定位，即估计出目标的运行状态，从而实现目标的定位，其中，估计误差协方差代表定位的精准度。

为了说明本发明基于约束信息的目标定位方法想比现有定位方法的优势，通过实验对其进行比较：

如图2所示，假设机动目标沿笔直马路运行的车辆，设 $A_k=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],$ ${\mathrm{\&Gamma;}}_k=\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 0\\ 0& 1\end{array}\right],$ $H_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right],$ $D_k=\left[\begin{array}{cccc}1& 0& -\tan \mathrm{\&theta;}& 0\\ 0& 1& 0& -\tan \mathrm{\&theta;}\end{array}\right],$ $d_k=\left[\begin{array}{c}0\\ 0\end{array}\right],$ $x_k={\left[\begin{array}{cccc}{x}_k^1& x_k^2& x_k^3& x_k^4\end{array}\right]}^T,$ 其中四个变量分别表示正北方向位移、正北方向速度、正东方向位移和正东方向速度。输入 $u_k=\left[\begin{array}{c}1\\ 1.2\end{array}\right],$ 采样时间T＝1秒，移动方向为东偏北θ＝30°。系统噪声w_k的协方差 $U_k=\left[\begin{array}{cccc}3& 0.3& 0& 0.1\\ 0.3& 2& 0& 0.5\\ 0& 0& 3& 0\\ 0.1& 0.5& 0& 1\end{array}\right],$ 测量噪声v_k的协方差 $R_k=\left[\begin{array}{ccc}2& 0.2& 0.1\\ 0.2& 2& 0.3\\ 0.1& 0.3& 1\end{array}\right],$ 初值 $x_0={\left[\begin{array}{cccc}1& \sqrt{3}& 0& 0\end{array}\right]}^T,$ P₀＝diag{2 1 2 0.5}。取 $D_k^1=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ 则 $D_k^2=\left[\begin{array}{cc}-\tan \mathrm{\&theta;}& 0\\ 0& -\tan \mathrm{\&theta;}\end{array}\right],$ 状态值x_k分块为 $x_{1,k}=\left[\begin{array}{c}{x}_k^1\\ x_k^2\end{array}\right],$ $x_{2,k}=\left[\begin{array}{c}{x}_k^3\\ x_k^4\end{array}\right].$

如图3、4所示，分别为本发明基于约束信息的目标定位方法，与无约束滤波方法(UK)、投影方法(PJ，其中加权矩阵W_k分别取W_k＝I和 为基于无约束滤波方法的误差协方差)和零空间方法(NS)针对正北方向位移和正北方向速度进行估计的均方根误差比较图，其中均方根误差的计算公式为：

${\mathrm{RMSE}}_k^i=\sqrt{\underset{j=1}{\overset{M}{\mathrm{\&Sigma;}}}{(x_{k,j}^i-{\hat{x}}_{k,j}^i)}^2}---\left(19\right)$

其中k为采样时刻值；i＝1、2、3、4分别代表正北方向位移、正北方向速度、正东方向位移和正东方向速度四个状态元素；M为正整数，代表采用同一种方法对同一个时间点的状态值x_k进行估计的次数，为k时刻第j次估计时对应的真实值，为k时刻第j次估计的估计值。

从图3和图4中可以看出，不论针对正北方向的位移还是速度，采用本发明基于约束信息的目标定位方法的估计误差都明显低于其他三类方法，由于正北向和正东向的位移与速度满足约束关系，故采用以上几类方法对正东方向位移和正东方向速度的估计误差分别与针对正北向位移和正北方向速度的估计误差相似。

综上所述，本发明提供的基于约束信息的目标定位方法，通过系统降维和关联测量值z_k与新变量，利用约束方程消去了状态值x_k和测量值z_k中的部分变量，简化了原始数学模型和计算复杂度，降低计算量，利于得到更优的估计效果，提高了定位精度；通过设计递归滤波器，根据新变量和误差协方差上一时刻的估计值以及测量值z_k的当前值，设计递归滤波器，求出新变量和误差协方差的当前估计值，有效实现对机动目标实时跟踪定位，且无需保留更前面的测量值与约束信息，节省了存储空间；此外本发明最大程度上利用了约束信息，且估计过程中未损失约束信息，进一步提高了定位精度。

虽然说明书中对本发明的实施方式进行了说明，但这些实施方式只是作为提示，不应限定本发明的保护范围。在不脱离本发明宗旨的范围内进行各种省略、置换和变更均应包含在本发明的保护范围内。

Claims (10)

1.一种基于约束信息的目标定位方法，用于对机动目标实时跟踪定位，其特征在于，包括以下步骤：

S1：约束模型建模，分别建立状态值x_k、测量值z_k以及约束方程的数学表达式；

S2：系统降维，根据约束方程，寻找一个新变量用以表示其他变量，得到关于新变量的降维方程，对状态值x_k的表达式进行降维；

S3：关联测量值z_k与新变量，建立测量值z_k与新变量的表达关系；

S4：获取测量信息，通过传感器实时获取机动目标的状态信息的测量值z_k；

S5：设计递归滤波器，根据新变量和误差协方差上一时刻的估计值以及测量值z_k的当前值，设计递归滤波器，求出新变量和误差协方差的当前估计值；

S6：重构状态值x_k和误差协方差的当前估计值，根据新变量的当前值求出原始状态值x_k和误差协方差的当前估计值；

S7：重复步骤S2-S6，对机动目标进行实时跟踪定位。

2.根据权利要求1所述的基于约束信息的目标定位方法，其特征在于，所述步骤S1中，状态值x_k和测量值z_k满足数学表达式：

x_k＝A_kx_k-1+Γ_ku_k-1+w_k-1

z_k＝H_kx_k+v_k

约束方程：D_kx_k＝d_k

其中，k为采样时刻值；状态值 $x_k={\left[\begin{array}{cccc}{x}_k^1& x_k^2& x_k^3& x_k^4\end{array}\right]}^T\text{,}$ 包含四个状态元素；u_k-1为已知输入项；过程噪声w_k和v_k为均值为零的高斯白噪声；w_k和v_k的协方差分别为U_k和R_k；A_k是n×n的状态转移矩阵，Γ_k是n×p的输入矩阵，H_k是m×n的测量矩阵，D_k是s×n的行满秩约束矩阵，d_k是s×1的约束向量，m、n、p、s均为正整数，且s<n。

3.根据权利要求2所述的基于约束信息的目标定位方法，其特征在于，所述步骤S2具体包括以下步骤：

S21：将约束矩阵D_k进行分解，表示成 $D_k=\left[\begin{array}{cc}{D}_k^1& D_k^2\end{array}\right]{E_k}^{-1},$ 其中E_k为可逆矩阵，为由D_k的其中s列构成的满秩方阵，D_k(·,k_j)表示矩阵D_k的第k_j列，j＝1,2,…,s；为D_k余下的n-s列；

S22：寻找新变量y_2,k，对状态值x_k的数学表达式进行降维，令对应D_k的分块将y_k进行分块表示成 $y_k=\left[\begin{array}{c}{y}_{1,k}\\ y_{2,k}\end{array}\right],$ 并根据约束方程用y_2,k表示y_1,k，得到关于新变量y_2,k的降维方程。

4.根据权利要求3所述的基于约束信息的目标定位方法，其特征在于，所述降维方程为：

${\stackrel{\&OverBar;}{D}}_k{y}_{2,k}={\stackrel{\&OverBar;}{A}}_k{y}_{2,k-1}+{\stackrel{\&OverBar;}{d}}_k+E_k^{-1}{w}_{k-1}$

其中， ${\stackrel{\&OverBar;}{A}}_k={\tilde{A}}_k{\stackrel{\&OverBar;}{D}}_{k-1};$

${\tilde{A}}_k=E_k^{-1}{A}_k{E}_{k-1};$

${\stackrel{\&OverBar;}{d}}_k={\tilde{A}}_k{\stackrel{\&OverBar;}{D}}_{k-1}^1{d}_{k-1}+E_k^{-1}{\mathrm{\&Gamma;}}_k{u}_{k-1}-{\stackrel{\&OverBar;}{D}}_k^1{d}_k;$

${\stackrel{\&OverBar;}{D}}_k=\left[\begin{array}{c}-{\left({\stackrel{\&OverBar;}{D}}_k^1\right)}^{-1}{\stackrel{\&OverBar;}{D}}_k^2\\ I\end{array}\right];$

${\stackrel{\&OverBar;}{D}}_k^1=\left[\begin{array}{c}-{\left(D_k^1\right)}^{-1}\\ 0\end{array}\right].$

5.根据权利要求4所述的基于约束信息的目标定位方法，其特征在于，所述步骤S3中，测量值z_k与新变量y_2,k的表达关系为：

$z_k={\stackrel{\&OverBar;}{H}}_k{y}_{2,k}+{\tilde{d}}_k+v_k$

其中， ${\stackrel{\&OverBar;}{H}}_k={\tilde{H}}_k{\stackrel{\&OverBar;}{D}}_k;$ ${\tilde{H}}_k=H_k{E}_k;$ ${\tilde{d}}_k={\tilde{H}}_k{\stackrel{\&OverBar;}{D}}_k{d}_k.$

6.根据权利要求5所述的基于约束信息的目标定位方法，其特征在于，所述步骤S5具体包括以下步骤：

S51：获得k-1时刻，即上一时刻y_2,k-1的估计值与误差协方差的估计值P_2,k-1|k-1；

S52：根据最小二乘算法获得y_2,kk时刻，即当前时刻的预测值与误差协方差当前时刻的预测值P_2,k|k-1；

S53：结合当前时刻的测量值z_k，得到y_2,k的当前估计值和误差协方差的当前估计值P_2,k|k。

7.根据权利要求6所述的基于约束信息的目标定位方法，其特征在于，所述步骤S52中根据最小二乘算法获得的和P_2,k|k-1分别为：

${\hat{y}}_{2,k|k-1}={\left({\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{\stackrel{\&OverBar;}{D}}_k\right)}^{-1}{\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{b}_k$

$P_{2,k|k-1}={\left({\stackrel{\&OverBar;}{D}}_k^T{\stackrel{\&OverBar;}{U}}_{k-1}^{-1}{\stackrel{\&OverBar;}{D}}_k\right)}^{-1}$

其中，

$b_k={\stackrel{\&OverBar;}{A}}_k{\hat{y}}_{2,k-1|k-1}+{\stackrel{\&OverBar;}{d}}_k;$

${\stackrel{\&OverBar;}{U}}_{k-1}={\stackrel{\&OverBar;}{E}}_k^{-1}{U}_{k-1}{E}_k^{-T}+{\stackrel{\&OverBar;}{A}}_k{P}_{2,k-1|k-1}{\stackrel{\&OverBar;}{A}}_{k-1}^T.$

8.根据权利要求7所述的基于约束信息的目标定位方法，其特征在于，所述步骤S53中和P_2,k|k分别为：

${\tilde{y}}_{2,k|k}={\hat{y}}_{2,k|k-1}+K_k{\tilde{z}}_{k|k-1}$

$P_{2,k|k}=(I-K_k{\stackrel{\&OverBar;}{H}}_k)P_{2,k|k-1}$

其中，

$K_k=P_{2,k|k-1}{\stackrel{\&OverBar;}{H}}_k^T{({\stackrel{\&OverBar;}{H}}_k{P}_{2,k|k-1}{\stackrel{\&OverBar;}{H}}_k^T+R_k)}^{-1};$

${\tilde{z}}_{k|k-1}=z_k-{\stackrel{\&OverBar;}{H}}_k{\hat{y}}_{2,k|k-1}-{\tilde{d}}_k.$

9.根据权利要求7所述的基于约束信息的目标定位方法，其特征在于，所述步骤S6具体包括：

S61：重构得到y_k的当前估计值和误差协方差的当前估计值

S62：根据和得到原始状态值x_k的当前估计值和原始误差协方差的当前估计值P_k|k。

10.根据权利要求9所述的基于约束信息的目标定位方法，其特征在于，所述步骤S61中和分别为：

${\hat{y}}_{k|k}={\stackrel{\&OverBar;}{D}}_k{\hat{y}}_{2,k|k}+{\stackrel{\&OverBar;}{D}}_k^1{d}_k$

$P_{k|k}^{\left(y\right)}={\stackrel{\&OverBar;}{D}}_k{P}_{2,k|k}{{\stackrel{\&OverBar;}{D}}_k}^T.$