一种基于投影与约束补偿的纯距离目标运动状态估计方法及系统

本发明属于目标跟踪与定位，具体涉及一种利用纯距离测量值进行目标运动状态（位置和速度）估计的方法及系统。

背景技术：

1、纯距离目标运动分析（range-only target motion analysis, rotma）指仅利用传感器与目标之间的径向距离测量值来估计目标的运动参数（如初始位置和速度）。该技术在水下声纳定位、无人机自主导航、超宽带定位及无线传感器网络等领域具有广泛应用。由于距离测量与目标运动状态之间存在高度非线性的平方根关系，现有技术主要分为以下两类：

2、（1）迭代类方法：以最大似然估计（mle）为代表，直接最大化似然函数虽然能够获得渐近最优性能，但通常需要借助高斯-牛顿法等迭代优化算法。这类方法在每一步迭代中都需要重新计算雅可比矩阵并求解线性最小二乘问题，导致整体计算成本较高。

3、（2）伪线性闭式解法：以平方-伪线性模型为代表的方法，虽能避免迭代，但会引入关于运动参数的辅助变量。若忽略这些变量须满足的二次约束，则会造成信息损失；若精确保留该约束，则求解过程将变得极为复杂（例如需解算高达16阶的多项式），导致计算量急剧攀升。现有技术难以在估计精度与计算效率之间取得良好平衡。因此，亟需一种既能逼近理论最优性能（即克拉美-罗下界），又能以非迭代、低计算复杂度实现的目标运动状态估计方法。

4、现有技术的核心矛盾在于：忽略约束的方法精度低、信息损失大；精确保留约束的方法计算复杂、无法实时应用；迭代类方法计算复杂度高、依赖初值。

技术实现思路

1、针对现有rotma技术中忽略约束的投影法信息损失大、精度低，而精确保留约束的解析法计算爆炸、无法实用的矛盾，本发明旨在提供一种基于投影与约束补偿的纯距离目标运动状态估计方法及系统。

2、本发明构建了一种融合正交投影和约束线性化补偿策略的两阶段非迭代估计框架，能够在显著降低运算开销的同时，有效逼近克拉美-罗下界（crlb）所对应的理论性能极限。首次在rotma问题中构建了“投影消元+约束线性化补偿”的两步非迭代解析框架。先利用正交投影获得无偏但信息损失的初始估计，再通过一阶泰勒展开将丢失的二次约束信息显式地建模为一个线性最小二乘补偿项，从而在一次非迭代更新后，使最终估计精度渐进达到crlb，彻底解决了精度与复杂度之间的矛盾。

3、本发明一方面，提供一种基于投影与约束补偿的纯距离目标运动状态估计方法，区别现有的单纯投影法或复杂约束耦合法，本发明采用了一种先投影消元，后线性化补偿的策略，具体包括以下步骤：

4、步骤1：构建伪线性系统模型，将非线性的距离测量关系转化为线性形式，并引入辅助变量。

5、已知传感器轨迹点，，其中和分别为第

k时刻传感器的

x轴和

y轴方向的位置分量；带噪声的纯距离测量值；采样周期。

6、设目标在时间的初始位置为,其中和分别为

x轴和

y轴方向的位置分量，其速度向量为，其中和分别为

x轴和

y轴方向的速度分量。在采样周期下，目标在时刻的位置为。

7、传感器和目标之间的真实距离为；在实际应用中，距离测量值会受到加性零均值高斯白噪声的干扰，该噪声方差为，且在不同时间点上相互独立，表示为：

8、

9、为测量噪声方差，将，代入上式，得到如下等式：

10、

11、复合噪声项。对所有时刻合并所述等式得到伪线性模型：

12、

13、其中表示为，里面包含待估计参数，以及：

14、

15、

16、噪声项的协方差矩阵近似为，三个辅助变量。

17、步骤2：通过正交投影消去辅助变量，获得目标运动参数初始估计。

18、将系数矩阵a按列分割：前四列为子矩阵b，后三列为子矩阵c。于是伪线性方程重写为：

19、

20、其中为真实待估计参数，。

21、构造正交投影矩阵，为单位矩阵，满足，则等式两边同时乘以后消除辅助变量项，则对伪线性方程重投影得到：

22、

23、其中，，对上述投影后的方程应用最小二乘准则，求解得到目标运动参数的初始估计：

24、

25、此步骤得到投影初始估计。

26、步骤3：利用获得的初始估计作为线性化点，通过一阶泰勒展开将丢失的二次约束信息显示建模为一个线性最小二乘补偿项，计算得到偏差补偿项。

27、根据目标运动参数初始估计，计算其对应的辅助变量近似值：

28、

29、定义待补偿的偏差为：

30、

31、将真实的辅助变量围绕进行一阶泰勒展开：

32、

33、其中，是对的雅可比矩阵在处的取值，

34、将和线性化约束代入步骤2的伪线性方程：

35、

36、得到关于偏差的线性模型：

37、

38、其中：

39、

40、

41、其中分别对应矩阵c的每一列，对应的每一行。

42、对上述线性方程应用加权最小二乘解，根据步骤1提供的噪声协方差矩阵，权重矩阵可以表示为，得到：

43、

44、步骤4：输出最终估计结果，将步骤2的初始估计与步骤3的偏差补偿项相加，得到最终的约束补偿估计结果。

45、最终估计为，最终输出的目标运动状态估计结果包括：初始位置，其中和分别为对应

x和

y轴位置分量；初始速度，其中和分别为对应

x和

y轴速度分量。

46、本发明另一方面，提供一种基于投影与约束补偿的纯距离目标运动状态估计系统，包括以下模块：

47、伪线性系统模块，用于建立带辅助变量的伪线性系统模型，并引入辅助变量。

48、目标运动参数初始估计模块，用于通过正交投影消去辅助变量，获得目标运动参数初始估计。

49、偏差补偿模块，用于利用目标运动参数初始估计作为线性化点，通过一阶泰勒展开，计算得到偏差补偿项。

50、约束补偿估计模块，用于将目标运动参数初始估计与偏差补偿项相加，得到最终的约束补偿估计结果。

51、本发明有益效果：

52、本发明通过“投影消元+约束线性化补偿”的两步非迭代策略，在保留解析求解效率的同时，利用一阶泰勒展开将投影过程中丢失的二次约束信息显式建模为线性最小二乘补偿项，从而有效恢复了被丢弃的约束信息。理论分析和仿真实验均表明，在高信噪比条件下，本发明对目标初始位置和速度的最终估计误差协方差矩阵渐进等于克拉美-罗下界（crlb）。

53、本发明全程无需迭代求解，整个运动状态估计过程仅需计算一次正交投影矩阵和一次加权最小二乘更新，即可同时完成对目标初始位置和速度的估计。相比依赖高斯-牛顿等迭代算法的最大似然估计方法，本发明的计算开销极低且不随噪声水平变化而显著增加，能够满足无人机自主导航、水下声纳定位等场景对实时性的严苛要求。

54、本发明采用正交投影获得目标位置和速度的初始估计，该初始估计在统计意义上是无偏的，不依赖于任何外部初始值或先验信息，完全由测量数据驱动。在此基础上，约束补偿步骤进一步压缩估计方差，使最终的位置和速度估计精度逼近理论极限。这一特性使得本发明在不同噪声水平、采样点数以及传感器观测几何条件下均能稳定工作，工程部署简便可靠。