[发明专利]基于矢量匹配算法的重力匹配定位误差抑制方法

说明书

本发明提供一种基于矢量匹配算法的重力匹配定位误差抑制方法，能够使得匹配的轨迹更加逼近于真实的轨迹。一种基于矢量匹配算法的重力匹配定位误差抑制方法，根据矢量匹配算法判断运动载体的轨迹形状，分为直线轨迹、折线轨迹、曲线轨迹，通过对直线轨迹、折线轨迹和曲线轨迹三种情况分析矢量匹配算法的误差范围，计算出真实载体位置所在的区域。

技术领域

本发明涉及导航、制导与控制技术领域，具体涉及一种基于矢量匹配算法的重力匹配定位误差抑制方法。

背景技术

重力匹配算法直接决定重力辅助惯导系统的精度，是水下重力无源辅助导航的核心技术。其本质是利用各种合适的信息处理融合方法，将实际测量的水下重力数据和存储好的重力基准图中的相应的重力数据进行相关性分析，根据一定的准则判断这两类数据的相似程度，最终得到对惯导位置的最佳估计结果。

传统的重力匹配算法按照采样方式可以分为序列匹配算法和单点匹配算法。序列匹配算法主要有ICCP和TERCOM算法。但是序列匹配算法的实时性比较差。单点匹配算法实时性较好，但是需要较高精度的初始位置，线性化方法选用不当或者线性化的误差较大的话，会出现滤波发散的情况。最近出现了矢量匹配算法。矢量匹配算法是一种单点粒子滤波匹配算法基础上的二次匹配算法，是在单点粒子滤波匹配结果的基础上加入惯导相邻点之间的位置相关性，对匹配结果进行二次估计，并借鉴贪心算法对多个校正结果利用加权最小二乘法得到最终匹配结果。矢量匹配算法相比较传统的重力匹配算法来说具有较好的精度和鲁棒性。但是该方法的精度经过理论的分析是可以进一步提高的。也就是说，该方法的误差是可以继续缩小的。

发明内容

有鉴于此，本发明提供一种基于矢量匹配算法的重力匹配定位误差抑制方法，能够使得匹配的轨迹更加逼近于真实的轨迹。

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

一种基于矢量匹配算法的重力匹配定位误差抑制方法，根据矢量匹配算法判断运动载体的轨迹形状，分为直线轨迹、折线轨迹、曲线轨迹，通过对直线轨迹、折线轨迹和曲线轨迹三种情况分析矢量匹配算法的误差范围，计算出真实载体位置所在的区域。

当矢量匹配算法判断运动载体的轨迹是直线轨迹时，所述运动载体的真实位置是在以匹配点为圆心，d为半径的圆内；当重力基准图上匹配点位置对应的重力异常值与真实重力异常值之差大于预设阈值的时候，则在以匹配点为圆心，d为半径的圆内进行粒子滤波。

当矢量匹配算法判断运动载体的轨迹是折线轨迹时，即运动载体从S点出发，行驶到O₁点，再从O₁点行驶到O₂点，O₂点对应的真实位置是在以O₂为圆心，以r₁为半径的圆内，其中其中，L₁是从S到O₁这一段航迹的长度；θ表示矢量匹配算法角度误差，l₁表示矢量匹配算法第一段长度的误差；当重力基准图上匹配点位置对应的重力异常值与真实重力异常值之差大于预设阈值时，则在以O₂为圆心，以r₁为半径的圆内进行粒子滤波。

当矢量匹配算法判断运动载体的轨迹是曲线轨迹时，即运动载体从A点出发，经过ABCD曲线轨迹到达D点，则D点对应的真实位置是以D点为圆心，kL为半径的圆内；其中其中α为惯导的长度误差系数，L是整个曲线轨迹的长度；当重力基准图上匹配点位置对应的重力异常值与真实重力异常值之差大于预设阈值时，则在D点为圆心，kL为半径的圆内进行粒子滤波。

所述粒子滤波采用以下方式：在误差范围内随机撒粒子，计算每个粒子的权值，并进行归一化处理；然后对粒子集进行重采样，重采样后的粒子是其中，是第i个粒子经过重采样得到的粒子，是归一化后的权值，计算为最终匹配点，其中n代表粒子的个数。

本发明的有益效果：