CN113624227B - 一种基于马氏距离的等值线迭代重力匹配算法 - Google Patents

Abstract

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

Description

一种基于马氏距离的等值线迭代重力匹配算法

技术领域

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

背景技术

惯性导航系统由于能够实时地为载体提供位置、速度、姿态等导航定位信息，在陆、海、空、天各类载体中广泛得到应用。但是大型潜器在水下长时间的连续工作过程中，如果单一使用惯性导航系统进行导航，其误差将随时间而积累，严重影响导航定位的精度。所以需要借助外界其他信息对其进行辅助导航。而海洋重力场具有特征稳定性和位置相关性，为辅助长航时惯性导航系统奠定了基础。重力匹配是研究重力辅助导航的核心和焦点。基于等值线的最近点迭代(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项的计算由如下公式给出:

其中，为样本的中心点。

迭代结束需要一定的条件，本发明采用如下两式作为迭代中止条件：

|R^(k+1)-R^(k)|<ε_r,|t^(k+1)-t^(k)|<ε_t (4)

或

k>k_max (5)

其中，式(4)表示基于本次迭代计算相对上次迭代计算的刚性变换的变化量而言，刚性变换T＝(R,t)没有发生显著变化，此时算法终止迭代；式(5)则是为了防止迭代陷入死循环的终止条件，当算法执行次数达到设定上限k_max时，便会强制停止迭代过程。经过迭代后，得到逐渐向真实轨迹靠拢的ICCP匹配航迹，作为最佳的匹配航迹，修正惯性导航系统输出的航迹。

步骤4，根据重力测量误差方差和迭代过程中得到的马氏距离目标函数值，确定等值线最近点条形搜索区域。

其中，确定条形区域法向宽度d的方法为，以当前采样点所在等值线为基准，沿等值线法线方向搜索，直到等值线上的重力异常数值满足下列条件之一：

式中g(C_i+j)为等值线C_i+j的重力异常值，g(C_i-k)为等值线C_i-k的重力异常值。

而条形区域等值线方向长度l不超过上次迭代的马氏距离目标函数值：

步骤5，重复上述采样、迭代步骤，得到的结果为匹配算法求解出的最佳匹配航迹。

有益效果：

(1)本发明将传统ICCP算法中欧式距离目标函数推广为马氏距离目标函数，克服了欧氏距离存在的各指标计量单位的选择有一定的人为性和随意性问题，且没有考虑各个变量之间的相关性和重要性的问题。本发明利用马氏距离具有仿射不变性的特点，不受指标量纲的影响，将其作为迭代目标函数，降低了误匹配率，提高了系统导航定位精度。

(2)本发明考虑了重力测量的实时误差，利用导航误差协方差矩阵来表示重力异常背景图的精度和重力实时测量精度，当航行器航行时间较长时，惯性导航系统的输出误差变大，使得重力异常背景图的精度和重力实时测量精度不可忽略，此时采用马氏距离作为迭代目标函数，重力测量和重力基准图误差造成的导航误差协方差矩阵发挥作用，放宽了传统ICCP算法对重力传感器测量数据无噪声的假设。马氏距离引入了协方差矩阵来表征点集的疏密程度，使得同一个点集内具有较高的相似度，保留了更多的正确匹配点对，提升了算法的匹配精度，进一步提高了系统导航定位精度。

(3)本发明提出一种基于测量误差和目标函数值的条形搜索区域范围改进策略，使得条形搜索区域的搜索面积会随着迭代次数的增加而减小，体现出ICCP算法的收敛性，也使得算法在寻找等值线上的最近点时的计算量逐步减小；相比于其他用圆形区域或者正方形区域来缩小搜索区域的方法，条形搜索区域在一定程度上考虑到了重力异常图中等值线的凹凸性，搜索区域会沿着等值线的弯曲而弯曲，进而增加了搜索到全局最优点的可能。

(4)本发明使用的基于马氏距离的重力匹配算法，收敛速度和传统ICCP算法相当，易于在现有条件下的水下自主航行器上使用，无需额外增加成本，实用性强。

附图说明

图1为本发明方法的原理图；

图2为本发明方法的流程图；

图3为基于测量误差和目标函数值的条形搜索区域原理图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明提供了一种基于马氏距离的等值线迭代重力匹配算法，通过利用马氏距离替代传统ICCP算法中的欧式距离作为迭代目标函数，有效地提高了惯性导航系统的精度。

本发明的等值线迭代重力匹配算法原理是：在预先已知的重力场背景图中，根据重力仪实际测量信息，提取到重力异常值，得到导航区域的重力异常等值线图，然后提出假设：水中运载体的真实航迹点必然会落在某条等值线上，其值对应着重力异常测量值。在重力等值线图上，根据惯性导航系统INS的输出信息标示出纯惯性导航的轨迹，并且在背景图中某条对应着重力异常测量值等值线上，找到距离惯性导航采样点距离最近的点，称为最近点集，在惯性导航采样点的集合与最近点的集合之间利用马氏距离最优函数，进行迭代刚性变换，包括平移和旋转，直到满足迭代中止条件，得到逐渐向真实轨迹靠拢的ICCP匹配航迹，作为最佳的匹配航迹，来修正惯性导航系统输出的航迹。系统框图如图1所示。

具体导航流程如图2所示，包括以下步骤：

步骤1，采样重力数据，利用预存重力数据得到等值线：在导航区域内，重力数据由重力仪实时测量得到，接着提取预先已知的重力基准图中信息，计算并画出重力异常等值线，记为C＝{c_i，i＝1,2,…,N}，惯性导航系统输出的位置记为P＝{p_i，i＝1,2,…,N}，ICCP算法基本原理如图2所示。

步骤2，最近等值线点选取是ICCP算法的关键步骤之一。最近点搜索的前提假设是惯性导航系统的位置误差较小，即满足惯性导航系统所指示的航迹在真实航迹附近。在经过上一步插值后得到的重力异常值图中，找到二者之间马氏距离最小的交点，即为最近等值点，记为Y＝{y_i，i＝1,2,…,N}。

采用马氏距离代替欧式距离，建立新的目标函数，从而对ICCP算法进行改进，改进后的最优目标函数M为式(2)。协方差矩阵Σ能够使得重力测量序列P＝{p_i(i＝1,2,…,n)}和在重力基准图中找到的最近等值点集Y＝{y_i(i＝1,2,…,n)}的数据去相关，更为有效地进行刚性变换，从而使得算法的导航精度提高。

步骤3，考虑到从几何意义上来看，平移变换只是集合质心之间的平移，平移量只是对惯性导航系统指示航迹P＝p_i(i＝1,2,…,N)做旋转变换后的点集的质心与最近等值线点集Y＝y_i(i＝1,2,…,N)的质心之间的距离，因此在求解刚性变换时，本发明采用先旋转，后平移的顺序进行。本发明中，采用四元数的方法来进行旋转矩阵的计算，只需要考虑旋转角θ的取值即可得到旋转矩阵R所有的信息。

ICCP算法应用于二维环境，故其旋转变换只在平面X-Y内完成，与垂直Z方向无关，因此可以用S矩阵将矩阵W展开为：

其中，S矩阵定义为：

令x₁,x₂,x₃,x₄为W矩阵的特征向量，λ₁,λ₂,λ₃,λ₄为对应特征向量的特征值。使取得最大值的四元数q为矩阵W的最大特征值对应的特征向量。因此，在ICCP算法中，矩阵W的最大特征值所对应的特征向量，对应的q^TWq最大值。通过q^TWq的最大值，能够计算出目标函数M的最大值，完成旋转变换。

矩阵W的特征值为：

四个特征值中，最大为λ_max，对应的特征向量为：

(S₁₁+S₂₂-λ_max)q₀+(S₂₁-S₁₂)q₃＝0 (11)

如此，完成了旋转角θ的求解：

通过求出来的旋转角θ，根据式(1)即可求得旋转矩阵R。

步骤4，求解出旋转矩阵R后，接下来需要求取平移向量t_n。求取平移向量的关键部分为当/>时，最优目标函数M最小，即

步骤5，上述步骤完成了利用四元数分别对刚性变换T＝(R,t)中的旋转矩阵R和平移向量t的求取。刚性变换是一个迭代的过程，迭代结束需要一定的条件，本发明采用式(4)和式(5)作为迭代中止条件。

步骤6，根据重力测量误差方差和迭代过程中得到的马氏距离目标函数值，确定等值线最近点条形搜索区域。

其中，确定条形区域法向宽度d的方法为，以当前采样点所在等值线为基准，沿等值线法线方向搜索，直到等值线上的重力异常数值满足下列条件之一：

式中g(C_i+j)为等值线C_i+j的重力异常值，g(C_i-k)为等值线C_i-k的重力异常值。

而条形区域等值线方向长度l的具体表达形式则需要从马氏距离目标函数进行推导。设ICCP算法的目标函数为M，最小的刚性变换T＝(R,t)，迭代过程的目标函数为：

在第k次迭代的过程中，计算的最小值相当于计算每一个惯性导航系统INS采样点的目标函数/>的最小值。

设匹配后残差平方和为其中i表示惯性导航系统第i个采样点。同时将式(15)代入不等式(16)，得到：

假设不等式(17)不满足，那么相当于对点集P的刚体变换将会产生一个比原始点集更大的均方误差，这就意味着迭代没有意义，这种情况显然不可能发生。

又根据迭代的基本过程：

代入最优目标函数得到：

而两次迭代匹配之间的误差设为因为/>为根据新一轮的采样序列/>寻找到的等值线上的最近点，所以：

根据式(20)，同时进行到下一代k＝k+1，可得：

综上所述，结合式(19)、(20)、(21)可得:

提出进而：

则根据式(23)可以得到条形区域沿着等值线的搜索长度l的公式：

即第k+1次条形搜索的长度，不超过第k次迭代的目标函数值。以上为用马氏距离推导得出的结论，可以验证，在欧氏距离下，结论依然成立。

经过上面的推导，可以得到在第k次迭代时，每一个惯性导航系统的采样点所对应的等值线上最近点都会分布在以采样点为中心，宽度为d，长度为l的条形区域内，宽度和长度分别由式(14)、式(24)决定。基于测量误差和目标函数值的条形搜索区域范围改进策略的原理图如图3所示。

故按照上述步骤进行的基于马氏距离的等值线最近点迭代的重力匹配算法即可使匹配最近点的精度提高，从而达到减小导航误差的目的，实现更精确地组合导航。

步骤7，重复上述迭代步骤，得到的结果即可作为最佳匹配航迹。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于马氏距离的等值线迭代重力匹配算法，其特征在于，包括：

步骤1，根据重力仪的重力测量值，从重力场背景图中提取重力异常值，并绘制重力异常等值线图；

步骤2，在重力等值线图上绘制惯性导航系统输出的惯导位置点，并将惯导位置点向重力异常等值线上做投影，找到与惯导位置点马氏距离最近的投影点，称为最近投影点；

步骤3，采用改进的ICCP算法对惯导位置点集P与最近投影点集Y进行匹配；其中，改进的ICCP算法在迭代过程中的迭代目标函数为马氏距离目标函数值，且搜索区域为一个宽度为d、长度为l的条形区域，其中，d的获取方法如下：以当前载体通过重力测量仪获得的测量点所在重力异常等值线C_i为基准，沿等值线法线方向搜索，直到等值线上的重力异常值满足下列条件之一：

g(C_i+j)≤g_i+g_noise

g(C_i-k)≥g_i-g_noise

其中，g(C_i+j)为等值线C_i+j上的重力异常值，g(C_i-k)为等值线C_i-k上的重力异常值；g_i为当前采样点的重力异常值，g_noise为重力测量仪的先验噪声；则条形搜索区域的宽度d即为满足以上条件的两条等值线C_i+j与C_i-k间的距离d＝||C_i+j-C_i-k||；

条形区域等值线方向长度l不超过上次迭代的马氏距离目标函数值；

所述步骤3中，改进的ICCP算法刚性变换采用先旋转、后平移的变换顺序。

2.如权利要求1所述的基于马氏距离的等值线迭代重力匹配算法，其特征在于，所述步骤3中，马氏距离目标函数M为：

其中，θ为旋转变换的旋转角；y_i表示第i个最近投影点，p_i表示第i个惯导位置点；Σ为重力测量值和重力场背景图之间的误差造成的匹配误差协方差矩阵，t为平移变换的平移向量。

3.如权利要求2所述的基于马氏距离的等值线迭代重力匹配算法，其特征在于，所述步骤3中，协方差矩阵Σ的第i，j项的计算如下：

其中，分别为最近投影点集Y与惯导位置点集P的中心点，cov表示求取协方差，E表示求取期望。

4.如权利要求2或3所述的基于马氏距离的等值线迭代重力匹配算法，其特征在于，所述步骤3中，终止迭代的条件为：

|R^(k+1)-R^(k)|<ε_r,|t^(k+1)-t^(k)|<ε_t

或

k>k_max

其中，上标k和k+1表示第k和第k+1次迭代；ε_r和ε_t分别为对应的设定值；k_max为设定的最大迭代次数。