一种基于马尔可夫切换的双主平台鲁棒协同定位方法

本发明属于多平台协同定位，具体地，涉及一种基于马尔可夫切换的双主平台鲁棒协同定位方法。

背景技术：

1、随着无人系统与多传感器融合技术的发展，多平台协同光电测量因可扩大测量范围并提高目标状态估计的连续性与可靠性，已成为定位领域的研究热点。对于双主平台—单从平台协同系统，两主平台可利用自身已知位置与从平台的相对距离形成双基准约束，为从平台位置校正提供外部观测信息，但如何有效融合平台运动信息与多源光电距离信息，是提高协同测量和定位精度的关键问题。

2、从平台常用的航位推算（dr）方法不依赖外部观测，更新频率高，但速度零偏、随机噪声及运动模型误差会随时间累积导致位置误差增大；光电测距具有非接触、响应速度快和测量精度高等优点，可用于校正dr结果，但距离与目标位置之间存在非线性映射关系，且双平台协同定位精度受观测噪声、平台几何构型及测量信息可靠性的共同影响，需采用非线性信息融合算法。

3、现有非线性信息融合算法如扩展卡尔曼滤波（ekf）、无迹卡尔曼滤波（ukf）等，通常假设测量噪声为统计特性已知的零均值高斯白噪声，但实际光电测量过程中，受目标遮挡、视场边缘效应、光电探测器饱和等因素影响，距离观测可能出现噪声方差突变、离散粗差、持续偏置或短时失效。异常观测会使估计结果偏离真实轨迹，甚至导致系统丧失有效的外部位置约束。

4、针对异常观测问题，现有研究主要采用信息检验、阈值剔除、测量噪声自适应估计等方法，但这些方法存在不足：基于信息阈值的观测剔除方法对阈值设置敏感，易造成有效信息损失；鲁棒滤波方法性能依赖调节参数的合理选取；自适应滤波难以准确区分异常来源；多模型方法仅采用单一正常模型与单一异常模型，无法对两路观测实施差异化加权。

技术实现思路

1、本发明旨在解决双主平台协同测量中，测距异常导致定位精度下降的问题，提供一种基于马尔可夫切换的鲁棒无迹卡尔曼滤波方法，能够在线判断两路测距的可靠状态，并依据观测状态动态调整不同测量信息的融合权重，提高协同定位的精度与鲁棒性。

2、本发明通过以下技术方案实现：

3、一种基于马尔可夫切换的双主平台鲁棒协同定位方法：

4、所述方法具体包括以下步骤：

5、步骤1、搭建包含两艘主平台与一艘从平台的协同光电测量系统；主平台搭载非接触式光电测距传感器与高精度导航设备，分别获取与从平台的相对距离观测值及自身位置信息，并将所述位置信息与距离观测值同步传输至从平台；从平台搭载速度测量装置或组合导航设备，以获取自身东向和北向的速度测量值；

6、步骤2、建立从平台的离散运动模型，基于上一时刻的位置估计结果与当前速度测量值进行航位推算，得到当前时刻从平台位置的先验估计值；

7、步骤3、主平台将自身位置及相对距离观测信息传输至从平台，从平台根据双主平台的空间几何关系建立非线性观测模型；

8、步骤4、构建马尔可夫切换观测模式体系，根据两路测距的正常、异常状态组合划分多种观测模式，为每种观测模式配置对应的测量噪声协方差矩阵，利用一阶马尔可夫过程描述两路光电测量状态随时间的转移关系，执行基于马尔可夫切换的无迹卡尔曼滤波计算；

9、步骤5、根据各模式的后验概率，动态调节相应距离观测的权重，对不同模式下的位置估计结果进行概率加权融合，输出从平台的位置估计结果。

10、进一步地，在步骤2中，

11、从平台上一时刻的位置估计包含东向位置与北向位置；从平台当前速度测量值包括东向和北向速度测量值；

12、采用一阶离散运动模型描述从平台位置变化过程，结合噪声矩阵得到从平台的位置先验估计。

13、进一步地，在步骤3中，

14、将两个主平台和从平台的真实距离值叠加对应的光电测量噪声，得到两路实际距离观测值并构成观测向量；结合观测向量与测量噪声向量，构建非线性观测模型。

15、进一步地，在步骤4中，

16、两艘主平台各自与从平台具有独立的光电测距链路；则有4中观测模式，分别为两路测距均正常、任一一路测距异常、两路测距均异常；

17、为不同模式配置相应的测量噪声协方差矩阵，对于判定为异常的测距链路，通过增大其等效测量噪声方差降低该观测在滤波更新中的权重。

18、进一步地，在步骤4中，

19、依次进行模式先验概率推算、多模型交互、各观测模式下的无迹卡尔曼滤波预测与更新、观测似然计算与模式后验概率更新。

20、进一步地，在步骤4中，

21、各观测模式下的无迹卡尔曼滤波预测过程具体为：基于各模式的混合初始状态生成sigma采样点，经状态转移函数传递后，计算得到预测状态均值与预测协方差矩阵；

22、滤波更新过程具体为：将预测sigma点代入观测函数得到观测预测值，计算观测预测均值、观测预测协方差与状态-观测互协方差，求解滤波增益并更新状态估计值与协方差矩阵。

23、进一步地，在步骤5中，

24、在输出最终位置估计结果的同时，基于各模式的后验概率分别计算每一路光电距离的异常概率，用于在线评估两路测距链路的可靠状态。

25、一种基于马尔可夫切换的双主平台鲁棒协同定位系统；

26、所述系统包括定位系统构建模块、状态预测模块、观测建模模块、滤波结算模块和输出模块；

27、所述定位系统构建模块，用于搭建包含两艘主平台与一艘从平台的协同光电测量系统；主平台搭载非接触式光电测距传感器与高精度导航设备，分别获取与从平台的相对距离观测值及自身位置信息，并将所述位置信息与距离观测值同步传输至从平台；从平台搭载速度测量装置或组合导航设备，以获取自身东向和北向的速度测量值；

28、所述状态预测模块，用于建立从平台的离散运动模型，基于上一时刻的位置估计结果与当前速度测量值进行航位推算，得到当前时刻从平台位置的先验估计值；

29、所述观测建模模块，主平台将自身位置及相对距离观测信息传输至从平台，从平台根据双主平台的空间几何关系建立非线性观测模型；

30、所述滤波结算模块、构建马尔可夫切换观测模式体系，根据两路测距的正常、异常状态组合划分多种观测模式，为每种观测模式配置对应的测量噪声协方差矩阵，利用一阶马尔可夫过程描述两路光电测量状态随时间的转移关系，执行基于马尔可夫切换的无迹卡尔曼滤波计算；

31、所述输出模块，根据各模式的后验概率，动态调节相应距离观测的权重，对不同模式下的位置估计结果进行概率加权融合，输出从平台的位置估计结果。

32、一种计算机装置设备系统，包括存储器、处理器及存储在存储器上的计算机程序，所述处理器执行所述计算机程序以实现上述方法的步骤

33、一种计算机可读存储介质，其上存储有计算机程序/指令，该计算机程序/指令被处理器执行时实现上述方法的步骤。

34、一种计算机程序产品，包括计算机程序/指令，该计算机程序指令被处理器执行时实现上述方法的步骤。

35、相较于现有技术，本发明的有益效果为：

36、本发明通过融合从平台的运动信息和两路光电距离观测，实现从平台二维位置的连续估计；解决了双主平台协同测量中，测距异常导致定位精度下降的问题，依据观测状态动态调整不同测量信息的融合权重，提高协同定位的精度与鲁棒性。