[发明专利]一种基于马氏距离的等值线迭代重力匹配算法

说明书

本发明公开了一种基于马氏距离的等值线迭代重力匹配算法。本发明考虑了重力测量的实时误差，利用导航误差协方差矩阵来表示重力异常背景图的精度和重力实时测量精度，并采用马氏距离作为迭代目标函数，引入协方差矩阵来表征点集的疏密程度，使得同一个点集内具有较高的相似度，保留了更多的正确匹配点对，提升了算法的匹配精度，进一步提高了系统导航定位精度。同时，还基于测量误差和目标函数值提出条形搜索区域范围改进策略，使得搜索面积会随着迭代次数的增加而减小，体现出ICCP算法的收敛性，也使得算法在寻找等值线上的最近点时的计算量逐步减小，并增加了搜索到全局最优点的可能。

技术领域

本发明涉及导航、制导与控制技术领域，具体涉及一种基于马氏距离的等值线迭代重力匹配算法。

背景技术

惯性导航系统由于能够实时地为载体提供位置、速度、姿态等导航定位信息，在陆、海、空、天各类载体中广泛得到应用。但是大型潜器在水下长时间的连续工作过程中，如果单一使用惯性导航系统进行导航，其误差将随时间而积累，严重影响导航定位的精度。所以需要借助外界其他信息对其进行辅助导航。而海洋重力场具有特征稳定性和位置相关性，为辅助长航时惯性导航系统奠定了基础。重力匹配是研究重力辅助导航的核心和焦点。基于等值线的最近点迭代(Iterative Closet Contour Point，ICCP)算法是最常用的序列重力匹配算法。ICCP算法基本原理为根据惯性导航系统的输出轨迹，在重力等值线图上找到距离惯性导航采样点最近的等值线点，在最近等值线点集合与惯性导航系统采样点集合之间进行刚性变换，迭代计算，从而获得逼近真实轨迹的匹配航迹，修正惯性导航系统的航迹。传统ICCP算法采用欧式距离作为目标函数，没有综合考虑重力场背景图误差和重力实时测量误差对迭代过程产生的影响，而在实际中，二者对匹配算法造成的影响会随着重力场背景图的分辨率、重力测量仪器精度的不同而变化，这可能会使传统ICCP算法的匹配效果变差，从而影响导航精度。

发明内容

有鉴于此，本发明提供了一种基于马氏距离的等值线迭代重力匹配算法，能够有效减少传统ICCP算法的误匹配率，提高惯性导航系统的导航定位精度。

本发明公开一种基于马氏距离的等值线迭代重力匹配算法，该算法包括如下步骤：

步骤1，在预先已知的重力场背景图中，根据重力仪实际测量信息，提取重力异常值，得到导航区域的重力异常等值线图，记为C＝{c_i，i＝1,2,…,N}，惯性导航系统输出的位置记为P＝{p_i，i＝1,2,…,N}，本发明改进的ICCP算法假设水中运载体的真实航迹点必然会落在某条等值线上，其值对应着重力异常测量值。

步骤2，在重力等值线图上，根据惯性导航系统的输出信息标示出纯惯性导航的轨迹，并在重力场背景图中对应于重力异常测量值等值线上，将惯性导航系统输出的位置点向等值线上做投影，找到距离惯性导航采样点距离最近的点，称为最近点集，记为Y＝{y_i，i＝1,2,…,N}。。

步骤3，在惯性导航系统采样点集P与最近点集Y之间进行迭代刚性变换T，刚性变换通常包括两部分：旋转变换(其矩阵为R)，平移变换(其向量为t)。本发明采用先旋转、后平移的变换顺序。本发明中，采用四元数的方法来进行旋转矩阵的计算，只需要考虑旋转角θ的取值即可得到旋转矩阵R所有的信息。

迭代过程中采用马氏距离目标函数M：

其中，Σ为重力测量和重力基准图误差造成的匹配误差协方差矩阵，R为旋转矩阵，t为平移向量，T＝(R,t)为刚性变换，Y＝{y_i，i＝1,2,…,N}表示每一个惯性导航系统的采样点经过搜索，得到的对应等值线上的最近点的集合，P＝{p_i，i＝1,2,…,N}表示惯性导航系统的输出航迹点。

协方差矩阵Σ的第i，j项的计算由如下公式给出: