一种融合时序约束与梯度引导的地磁—惯性粒子滤波定位方法

本发明属于地磁定位、惯性导航、多源融合定位和移动机器人导航，具体涉及一种融合时序约束与梯度引导的地磁—惯性粒子滤波定位方法。

背景技术：

1、随着移动机器人、无人平台、智能终端和应急救援装备的发展，复杂环境下的自主定位技术成为导航系统的重要基础。常见定位方式包括gnss、视觉/激光雷达 slam、无线信号定位和惯性导航等。其中，gnss 在室内、地下、峡谷、遮挡区域等环境中容易失效；视觉和激光雷达方法对光照、纹理、结构和计算资源要求较高；无线信号定位通常依赖基础设施部署；单独使用惯性导航又容易产生累积漂移。相比之下，环境磁场具有广泛可获取、无需主动发射信号、硬件成本低和功耗小等特点，因此基于磁地图的地磁定位逐渐受到关注。

2、基于磁地图的定位方法通常预先构建目标区域的磁场地图，在在线阶段将实时磁观测与磁地图进行匹配，从而估计移动载体位置。然而，实际环境中的磁场分布并非处处具有强辨识性。在磁特征弱区或局部相似区域中，不同位置可能具有相近的磁观测值，仅依赖单时刻磁观测容易产生匹配歧义。同时，惯性测量单元虽可提供连续运动先验，但存在噪声和漂移；若融合策略固定，则难以在强磁特征区和弱磁特征区之间自适应平衡局部收敛与全局搜索。因此，如何同时利用惯性运动信息、磁地图空间梯度和磁序列时序约束，提高地磁—惯性组合定位的鲁棒性，是现有技术需要解决的问题。

3、已有部分方法尝试通过地磁指纹匹配、粒子滤波、多源融合或神经网络方式提高定位性能。专利 cn103175529a“基于室内磁场特征辅助的行人惯性定位系统”通过建立磁场特征库，并结合行人航位推算和粒子滤波融合实现定位。该方法利用磁场特征辅助惯性定位，但其重点在室内行人定位和磁场特征库匹配，未针对磁特征弱区引入 soft-dtw 时序约束和磁地图梯度引导粒子细化机制。

4、专利 cn103983266a“基于地磁信息的室内定位方法及系统”根据用户移动方向和步长移动粒子，并根据地磁信息相似程度更新粒子权重，实现基于地磁信息的室内定位。该方法利用粒子滤波融合运动信息和地磁信息，但主要依赖移动前后地磁信息相似度进行权重更新，未进一步利用磁序列动态时间对齐关系和自适应似然融合策略。

5、专利 cn103925923b“一种基于自适应粒子滤波器算法的地磁室内定位系统”提出基于自适应粒子滤波算法的地磁室内定位系统。该方法关注自适应粒子滤波与地磁指纹数据采集，但对于磁地图局部梯度、粒子传播范围动态调节以及单点磁似然与序列似然的联合建模考虑不足。

6、专利 cn107421546a“一种基于空间环境磁场特征的无源组合定位方法”将惯导、地磁和地图进行组合定位，利用磁场特征提高无源定位能力。该方法强调无源组合定位和磁干扰识别处理，但未在粒子滤波框架中引入基于 soft-dtw 的时序约束，也未利用磁地图梯度对粒子进行局部修正。

7、专利 cn110300385a“一种基于自适应粒子滤波的室内定位方法”融合 wi-fi 信号、地磁信号和 pdr 信息，并采用自适应粒子滤波实现室内定位。该方法能够利用多源信息提高室内定位精度，但其依赖 wi-fi 等外部信号源，且未针对无附加基础设施条件下的地磁—惯性组合定位设计时序约束、梯度引导和自适应联合似然机制。

8、专利 cn114222240a“一种基于粒子滤波的多源融合定位方法”采用粒子滤波融合uwb、dr、vslam 和地磁信号指纹定位结果，实现多源逐步融合定位。该方法依赖多种定位源和较复杂的传感器配置，而本发明侧重于仅利用磁力计与惯性测量单元，在不增加额外硬件成本的条件下提高地磁—惯性组合定位性能。

9、综上，现有技术仍存在以下不足：一是单点磁观测在磁特征弱区和局部相似区域中容易出现匹配歧义；二是普通粒子滤波方法对磁序列时序信息利用不足，难以充分抑制短时扰动和采样节奏变化带来的影响；三是固定 proposal 或固定融合策略难以根据当前磁观测判别性动态调节搜索范围和似然权重；四是现有方法较少利用磁地图梯度信息对粒子进行局部细化。因此，亟需一种融合时序约束、梯度引导和自适应粒子滤波的地磁—惯性定位方法及系统，以提高复杂磁场环境下的定位精度、稳定性和环境适应能力。

技术实现思路

1、为解决上述技术问题，本发明提出了一种融合时序约束与梯度引导的地磁—惯性粒子滤波定位方法，用于提高磁特征弱区、局部相似磁场区域以及惯性漂移条件下的组合定位精度与鲁棒性。为实现上述目的，本发明采取的技术方案是：一种融合时序约束与梯度引导的地磁—惯性粒子滤波定位方法，具体步骤为：

2、（1）规则化磁地图构建与梯度特征提取；

3、在目标区域采集磁场数据和位置数据，通过空间插值构建规则化磁地图，并计算规则化磁地图的梯度特征图；

4、（2）定位数据采集与粒子初始化；

5、在移动载体上设置磁力计、惯性测量单元和定位计算单元，在线采集磁观测数据和惯性运动数据，并在初始位置区域生成粒子集合；

6、（3）基于惯性信息的自适应粒子传播；

7、利用惯性运动数据估计移动载体的运动先验，并根据上一时刻的观测融合状态自适应调整 proposal 分布，完成粒子状态传播；

8、（4）梯度引导粒子细化；

9、根据传播粒子在规则化磁地图中的预测磁观测、实测磁观测以及局部磁场梯度，对粒子位置进行梯度引导细化；

10、（5）融合 soft-dtw 时序约束的联合似然构造；

11、根据当前磁观测构造单点磁观测似然，根据滑动窗口内的实测磁序列和粒子预测磁序列构造 soft-dtw 序列似然，并根据当前磁观测判别性自适应融合两类似然，得到联合似然；

12、（6）粒子权重更新与状态估计；

13、利用联合似然更新粒子权重，根据有效粒子数判断是否重采样，并根据粒子状态和权重输出当前位置估计；

14、（7）连续定位结果输出；

15、循环执行步骤（3）至步骤（6），输出移动载体的连续定位轨迹。

16、进一步，步骤（1）中，规则化磁地图构建与梯度特征提取包括：采集目标区域内多个位置点的磁场样本；通过空间插值重建规则化磁地图；根据规则化磁地图计算磁场梯度特征图；将规则化磁地图用于在线磁观测查表，将梯度特征图用于粒子细化和观测不确定性建模。

17、进一步，步骤（3）中，基于惯性信息的自适应粒子传播包括：根据惯性测量单元输出的加速度、角速度或姿态信息获得运动先验；根据上一时刻融合权重调整 proposal 分布的扩散范围；当磁观测判别性强时缩小传播噪声，当磁观测判别性弱时扩大传播噪声。

18、进一步，步骤（4）中，梯度引导粒子细化包括：查询传播后粒子位置对应的预测磁观测值；计算实测磁观测与预测磁观测之间的残差；根据磁场梯度大小构造有效观测噪声；利用残差、有效观测噪声和梯度方向对粒子位置进行局部修正。

19、进一步，步骤（5）中，联合似然构造包括：根据当前磁观测构造单点磁观测似然；维护实测磁序列滑动窗口和粒子预测磁序列滑动窗口；利用 soft-dtw 计算两类序列之间的相似性并构造序列似然；根据单点磁似然最大值与次大值之间的判别间隔确定自适应融合权重；根据该权重融合单点似然和序列似然。

20、本技术与现有技术相比，具有如下有益效果：

21、1、本发明在粒子滤波框架下融合磁地图、磁地图梯度、惯性运动先验、单点磁观测和 soft-dtw 磁序列约束；

22、2、本发明通过自适应 proposal 调整，能够根据磁观测判别性动态平衡局部搜索和全局探索；

23、3、本发明通过梯度引导粒子细化，能够提高传播粒子与真实状态的一致性；

24、4、本发明通过 soft-dtw 时序约束，能够降低局部相似磁场区域和采样节奏变化对定位匹配的影响；

25、5、本发明通过自适应似然融合，能够根据观测可信度动态分配单点磁观测和序列约束的作用权重；

26、6、本发明能够在不增加额外硬件成本的前提下，提高地磁—惯性组合定位的精度、稳定性和环境适应能力。。