CN103616027A - 一种基于改进msd的重力匹配方法 - Google Patents

Abstract

本发明属于重力辅助惯性导航系统的技术领域，尤其涉及一种基于改进MSD的重力匹配方法。本发明包括：确定参考位置信息；引入位置误差向量；确定代价函数；确定使代价函数E最小的位置误差向量；得到最终位置。针对传统的重力匹配方法计算量大、耗时多的问题，本发明在MSD的基础上，引入位置误差向量，确定代价函数，简化匹配计算机中的匹配过程，在保证匹配精度的基础上，提高了匹配计算机的工作效率。本发明引入SOR迭代方法，获取位置误差向量，其收敛速度更快。一般的匹配方法都会用到重力数据库，现有重力数据库的精度还有待进一步提高，本发明的匹配过程中仅用到参考轨迹上点处的重力异常值及其变化梯度，减轻对重力数据库的依赖。

Description

一种基于改进MSD的重力匹配方法

技术领域

本发明属于重力辅助惯性导航系统的技术领域，尤其涉及一种基于改进MSD的重力匹配方法。

背景技术

惯性导航系统是一种利用自身的陀螺仪来建立和保持空间基准，并通过加速度计测量载体加速度，解算出载体的航向、航速、位置和水平姿态等数据的导航系统。惯性导航系统可以不接收外界信息，进行自主导航，因此在军事上具有非常重要的地位，但由于其核心惯性器件—陀螺仪的固有随机漂移造成系统误差随时间积累。利用外界信息对惯导系统进行重调，可以消除系统误差积累，但这降低了载体的隐蔽性。地球重力场的变化是地球固有的物理信息，利用这种变化信息，可以修正惯性导航系统的位置误差或限制其增长。重力辅助导航具有精度高，不受时间限制，对外无辐射的特性，可最终解决潜艇的隐蔽性问题，因此常作为惯性导航的辅助手段。

匹配算法是重力匹配技术的核心，重力匹配算法的作用是将重力仪的实时测量数据与计算机中预存的重力基准数据在空间位置上进行匹配，获得最佳的匹配位置。匹配算法的精度决定了重力匹配系统的精度。与地形导航算法类似，现有的基于重力匹配算法也可分为两大类，即序列相关匹配方法和递推滤波方法。实际中，由于重力模型很难用解析表达式表示，递推滤波的精度受到极大的限制。序列相关匹配方法主要是将实际测量序列与数据库中所有可能基准序列按一定算法进行相关分析，计算实测数据和基准序列的相关度，将具有相关峰值的基准序列对应的位置作为匹配位置，进而修正主导航系统的导航参数。目前相关分析方法有三种较常用的性能指标：交叉相关算法(COR)、平均绝对差算法(MAD)和均方差算法(MSD)，其中MSD精度最高。但是这些相关分析方法计算量较大，耗时长，对重力数据库的分辨率有较高要求，且匹配精度不高。

发明内容

本发明的目的在于提供一种减少计算量，提高匹配精度的基于改进MSD的重力匹配方法。本发明的目的是这样实现的：

基于改进MSD的重力匹配方法，包括：

（1）确定惯性导航系统输出的参考位置信息（经度r_x和纬度r_y），根据计算机中预存的重力异常基准图（EGM2008重力数据库）获取该位置的重力异常值Δg和重力异常变化梯度和

（2）引入位置误差向量e＝(e_x,e_y)，建立参考位置(r_x,r_y)与匹配位置(p_x,p_y)之间的等式，在等式两边作用重力异常函数，并进行一阶泰勒展开；

（3）忽略参考轨迹和匹配轨迹在采样间隔内航向和航速的变化，在均方差算法MSD的基础上，确定代价函数E；

（4）确定使代价函数E最小的位置误差向量e，利用SOR迭代方法对位置误差向量e进行迭代更新，即在已知第k次SOR迭代后位置误差向量e(k)的基础上，计算第k+1次SOR迭代时的位置误差向量e(k+1)；

（5）每次迭代后进行不等式判断，若不等式成立，迭代终止，所得结果即为匹配位置误差，否则，继续进行迭代匹配；

（6）在参考位置的基础上，加上匹配得到的位置误差，即可得到最终位置。

步骤（2）中，C_r表示参考轨迹，C_m表示匹配轨迹，参考轨迹上的点的坐标可表示为(r_xi,r_yi)^T，r_xi和r_yi分别表示参考点i的经度和纬度信息，其中i＝1,2，…,N，N是参考轨迹点总数，根据计算机中预存的重力异常基准图，可知该点的重力异常值Δg(r_xi,r_yi)，(p_xi,p_yi)^T表示与参考点(r_xi,r_yi)^T对应的匹配点，p_xi和p_yi分别表示对应匹配点的经度和纬度信息：

Δg(r_xi+e_xi,r_yi+e_yi)＝Δg(p_xi,p_yi)

其中，e_xi＝p_xi-r_xi表示匹配点与参考点之间的经度误差，e_yi＝p_yi-r_yi表示匹配点与参考点之间的纬度误差，进行一阶泰勒展开，即

$\mathrm{\Δg}(r_{\mathrm{xi}},r_{\mathrm{yi}})+\frac{\∂\mathrm{\Δg}}{{\∂x}_i}{e}_{\mathrm{xi}}+\frac{\∂\mathrm{\Δg}}{{\∂y}_i}{e}_{\mathrm{yi}}+R_{2i}=\mathrm{\Δg}(p_{\mathrm{xi}},p_{\mathrm{yi}})$

式中，

表示参考点(r_xi,r_yi)^T处得重力异常变化梯度向量，R_2i表示高阶泰勒展开项，忽略R_2i：

▽(Δg)_i·e_i+(Δg_i-l_i)＝0

式中，e_i＝[e_xi,e_yi]^T为参考点与匹配点之间的位置误差向量，l_i＝Δg(p_xi,p_yi)为利用重力仪测得参考点(r_xi,r_yi)^T处的重力异常值。

代价函数

$E=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}(c_i+\mathrm{\λ}{s}_i)$

其中，

$c_i={(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}{e}_{\mathrm{xi}}+\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{e}_{\mathrm{yi}}+(\mathrm{\Δ}{g}_i-l_u))}^2$

$s_i=\frac{1}{2}[{(e_{x(i+1)}-e_{\mathrm{xi}})}^2+{(e_{\mathrm{xi}}-e_{x(i-1)})}^2+{(e_{y(i+1)}-e_{\mathrm{yi}})}^2+{(e_{\mathrm{yi}}-e_{y(i-1)})}^2]$

式中，i＝1,2，…,N，N是参考轨迹点总数；校正参数λ＞0。

确定使代价函数E最小的位置误差向量e为代价函数E对e_xi和e_yi求一阶偏导数，即

$\frac{\∂E}{\∂e_{\mathrm{xi}}}=2\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}{e}_{\mathrm{xi}}+\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{e}_{\mathrm{yi}}+(\mathrm{\Δ}{g}_i-l_i))+2\mathrm{\λ}(e_{\mathrm{xi}}-{\stackrel{\‾}{e}}_{\mathrm{xi}})$

$\frac{\∂E}{\∂e_{\mathrm{yi}}}=2\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}{e}_{\mathrm{xi}}+\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{e}_{\mathrm{yi}}+(\mathrm{\Δ}{g}_i-l_i))+2\mathrm{\λ}(e_{\mathrm{yi}}-{\stackrel{\‾}{e}}_{\mathrm{yi}})$

其中，平均位置误差可表示为

${\stackrel{\‾}{e}}_{\mathrm{xi}}=\frac{1}{2}(e_{x(i-1)}+e_{x(i+1)})$

${\stackrel{\‾}{e}}_{\mathrm{yi}}=\frac{1}{2}(e_{y(i-1)}+e_{y(i+1)})$

进一步，获取代价函数E对e_xi和e_yi的二阶偏导数

$\frac{{\∂}^{2}E}{\∂e_{\mathrm{xi}}^2}=2{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\right)}^2+2\mathrm{\λ}$

$\frac{{\∂}^{2}E}{\∂e_{\mathrm{yi}}^2}=2{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}\right)}^2+2\mathrm{\λ}$

和均为正数，存在位置误差向量e_i＝(e_xi,e_yi)，使得代价函数等于0，即

$(\mathrm{\λ}+{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\right)}^2)e_{\mathrm{xi}}+\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{e}_{\mathrm{yi}}=\mathrm{\λ}{\stackrel{\‾}{e}}_{\mathrm{xi}}-\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}(\mathrm{\Δ}{g}_i-l_i)$

$\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{e}_{\mathrm{xi}}+(\mathrm{\λ}+{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}\right)}^2)e_{\mathrm{yi}}=\mathrm{\λ}{\stackrel{\‾}{e}}_{\mathrm{yi}}-\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}(\mathrm{\Δ}{g}_i-l_i)$

式中，i＝1,2，…,N，N是参考轨迹点总数，

利用SOR迭代进行求解，即

$e_{\mathrm{xi}}(k+1)={\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)-\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\frac{\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}{\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)+(\mathrm{\Δ}{g}_i-l_i)}{\mathrm{\λ}+{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\right)}^2+{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}\right)}^2}$

$e_{\mathrm{yi}}(k+1)={\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)-\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}\frac{\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}{\stackrel{\‾}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}{\stackrel{\‾}{e}}_{\mathrm{yi}}\left(k\right)+(\mathrm{\Δ}{g}_i-l_i)}{\mathrm{\λ}+{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂x_i}\right)}^2+{\left(\frac{\∂\mathrm{\Δ}{g}_i}{\∂y_i}\right)}^2}$

式中，k为迭代的次数。

步骤（5）每次迭代后按下面的不等式进行判断

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({(e_{\mathrm{xi}}(k+1)-e_{\mathrm{xi}}\left(k\right))}^2+{(e_{\mathrm{yi}}(k+1)-e_{\mathrm{yi}}\left(k\right))}^2)<\mathrm{\&epsiv;}$

其中，判断阈值ε为给定值，且满足ε≥0，若不等式成立，迭代终止，所得结果即为匹配位置误差，若不等式成立，继续进行迭代匹配。

步骤（6）在参考位置(r_xi,r_yi)^T的基础上，加上匹配得到的位置误差e_i＝[e_xi,e_yi]^T，即可得到最终位置，即

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right..$

本发明的有益效果在于：针对传统的重力匹配方法，如MSD，MAD等，计算量大、耗时多的问题，本发明在MSD的基础上，引入位置误差向量，确定代价函数，简化匹配计算机中的匹配过程，在保证匹配精度的基础上，提高了匹配计算机的工作效率。

本发明引入SOR迭代方法，获取位置误差向量，SOR迭代方法是对Jacobi迭代方法和Gauss-Seidel迭代方法的进一步改进，其收敛速度更快，综合性更强。

一般的匹配方法都会用到重力数据库，现有重力数据库的精度还有待进一步提高，与传统的MSD匹配方法相比，本发明的匹配过程中仅用到参考轨迹上点处的重力异常值及其变化梯度，减轻对重力数据库的依赖。

附图说明

图1是本发明的方法流程图；

图2是本发明提供的匹配效果对比图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

基于改进MSD的重力匹配方法，包括：

（1）确定惯性导航系统输出的参考位置信息：经度r_x和纬度r_y，根据计算机中预存的重力异常基准图即EGM2008重力数据库，获取该位置的重力异常值Δg和重力异常变化梯度

和

（2）引入位置误差向量e＝(e_x,e_y)，建立参考位置(r_x,r_y)与匹配位置(p_x,p_y)之间的等式，在等式两边作用重力异常函数，并进行一阶泰勒展开；

（3）忽略参考轨迹和匹配轨迹在采样间隔内航向和航速的变化，在均方差算法MSD的基础上，确定代价函数E；

（4）确定使代价函数E最小的位置误差向量e，利用SOR迭代方法对位置误差向量e进行迭代更新，即在已知第k次SOR迭代后位置误差向量e(k)的基础上，计算第k+1次SOR迭代时的位置误差向量e(k+1)；

（5）每次迭代后进行不等式判断，若不等式成立，迭代终止，所得结果即为匹配位置误差，否则，继续进行迭代匹配。

（6）在参考位置的基础上，加上匹配得到的位置误差，即可得到最终位置。

在步骤一中，确定惯性导航系统输出的参考位置信息（经度r_x和纬度r_y），利用计算机中预存的重力异常基准图（EGM2008重力数据库）可提取参考位置的重力异常值

而不能直接从数据库读取参考位置的重力异常变化梯度，需要利用重力数据库对参考位置处得重力数据进行局部线性化，得到在经度和纬度方向的重力异常变化梯度

和

在步骤二中，用C_r表示参考轨迹，C_m表示匹配轨迹。参考轨迹上的点的坐标可表示为(r_xi,r_yi)^T，r_xi和r_yi分别表示参考点i的经度和纬度信息，其中i＝1,2，…,N，N是参考轨迹点总数。根据计算机中预存的重力异常基准图，可知该点的重力异常值Δg(r_xi,r_yi)，可以理解成经度和纬度的函数。(p_xi,p_yi)^T表示与参考点(r_xi,r_yi)^T对应的匹配点，p_xi和p_yi分别表示对应匹配点的经度和纬度信息。于是，我们得到

Δg(r_xi+e_xi,r_yi+e_yi)＝Δg(p_xi,p_yi)

其中，e_xi＝p_xi-r_xi表示匹配点与参考点之间的经度误差，e_yi＝p_yi-r_yi表示匹配点与参考点之间的纬度误差。

考虑到参考点位于匹配点附近区域，于是对上式进行一阶泰勒展开，即

$\mathrm{\&Delta;g}(r_{\mathrm{xi}},r_{\mathrm{yi}})+\frac{\&PartialD;\mathrm{\&Delta;g}}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;g}}{\&PartialD;y_i}{e}_{\mathrm{yi}}+R_{2i}=\mathrm{\&Delta;g}(p_{\mathrm{xi}},p_{\mathrm{yi}})$

式中，

表示参考点(r_xi,r_yi)^T处得重力异常变化梯度向量，R_2i表示高阶泰勒展开项。在忽略R_2i的情况下，可以得到

▽(Δg)_i·e_i+(Δg_i-l_i)＝0

式中，e_i＝[e_xi,e_yi]^T为参考点与匹配点之间的位置误差向量，l_i＝Δg(p_xi,p_yi)为利用重力仪测得参考点(r_xi,r_yi)^T处的重力异常值。

在步骤三中，假设在采样间隔内参考点与匹配点之间的航向和航速相差不大，即参考轨迹与匹配轨迹近似平行。于是，我们按下面的表达式确定代价函数

$E=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(c_i+\mathrm{\&lambda;}{s}_i)$

其中，

$c_i={(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))}^2$

$s_i=\frac{1}{2}[{(e_{x(i+1)}-e_{\mathrm{xi}})}^2+{(e_{\mathrm{xi}}-e_{x(i-1)})}^2+{(e_{y(i+1)}-e_{\mathrm{yi}})}^2+{(e_{\mathrm{yi}}-e_{y(i-1)})}^2]$

式中，i＝1,2，…,N，N是参考轨迹点总数；校正参数λ＞0，如果可以精确计算得到▽(Δg)_i和(Δg_i-l_i)，λ取值应适当增大，否则适当减少λ取值。

在步骤四中，将

代价函数E对e_xi和e_yi求一阶偏导数，即

$\frac{\&PartialD;E}{\&PartialD;e_{\mathrm{xi}}}=2\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))+2\mathrm{\&lambda;}(e_{\mathrm{xi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}})$

$\frac{\&PartialD;E}{\&PartialD;e_{\mathrm{yi}}}=2\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))+2\mathrm{\&lambda;}(e_{\mathrm{yi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}})$

其中，平均位置误差可表示为

${\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}=\frac{1}{2}(e_{x(i-1)}+e_{x(i+1)})$

${\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}=\frac{1}{2}(e_{y(i-1)}+e_{y(i+1)})$

进一步，获取代价函数E对e_xi和e_yi的二阶偏导数

$\frac{{\&PartialD;}^{2}E}{\&PartialD;e_{\mathrm{xi}}^2}=2{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2+2\mathrm{\&lambda;}$

$\frac{{\&PartialD;}^{2}E}{\&PartialD;e_{\mathrm{yi}}^2}=2{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2+2\mathrm{\&lambda;}$

从上式可以看出，

均为正数，那么存在位置误差向量e_i＝(e_xi,e_yi)，使得代价函数等于0。于是，我们可以得到

$(\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2)e_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}-\frac{\&PartialD;g_i}{\&PartialD;x_i}(\mathrm{\&Delta;}{g}_i-l_i)$

$\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{xi}}+(\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2)e_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}(\mathrm{\&Delta;}{g}_i-l_i)$

式中，i＝1,2，…,N，N是参考轨迹点总数。

利用SOR迭代对上面的方程进行求解，即

$e_{\mathrm{xi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\frac{\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+(\mathrm{\&Delta;}{g}_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2}$

$e_{\mathrm{yi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\frac{\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+(\mathrm{\&Delta;}{g}_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2}$

式中，k为迭代的次数。

在步骤五中，

每次迭代后按下面的不等式进行判断

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({(e_{\mathrm{xi}}(k+1)-e_{\mathrm{xi}}\left(k\right))}^2+{(e_{\mathrm{yi}}(k+1)-e_{\mathrm{yi}}\left(k\right))}^2)<\mathrm{\&epsiv;}$

其中，判断阈值ε为给定值，且满足ε≥0。

若不等式成立，迭代终止，所得结果即为匹配位置误差，若不等式成立，继续进行迭代匹配。

在步骤六中，

在参考位置(r_xi,r_yi)^T的基础上，加上匹配得到的位置误差e_i＝[e_xi,e_yi]^T，即可得到最终位置，即

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right.$

根据步骤二可知，在进行一阶泰勒展开时，忽略了二阶以及二阶以上的泰勒展开项，随着初始位置误差e(0)的增大，匹配误差e也会越大。针对这一问题，可以通过有限次重复迭代来改善匹配的精度，即每次匹配的参考轨迹为上一次匹配后的匹配轨迹。

本发明包括以下几个步骤：

步骤一：确定惯性导航系统输出的参考位置信息（经度r_x和纬度r_y），并利用计算机中预存的重力异常基准图（EGM2008重力数据库）获取该位置的重力异常值Δg和重力异常变化梯度

和

具体为，确定惯性导航系统输出的参考位置信息（经度r_x和纬度r_y），利用计算机中预存的重力异常基准图（EGM2008重力数据库）可提取参考位置的重力异常值Δg(r_x,r_y)，而不能直接从数据库读取参考位置的重力异常变化梯度，需要利用重力数据库对参考位置处得重力数据进行局部线性化，得到在经度和纬度方向的重力异常变化梯度

和

步骤二：通过引入位置误差向量e＝(e_x,e_y)，建立参考位置(r_x,r_y)与对应匹配位置(p_x,p_y)之间的等式，在等式两边作用重力异常函数，并进行一阶泰勒展开。

具体为，用C_r表示参考轨迹，C_m表示匹配轨迹。参考轨迹上的点的坐标可

表示为(r_xi,r_yi)^T，r_xi和r_yi分别表示参考点i的经度和纬度信息，其中i＝1,2，…,N，N是参考轨迹点总数。根据计算机中预存的重力异常基准图，可知该点的重力异常值Δg(r_xi,r_yi)，可以理解成该点处经度和纬度的函数。(p_xi,p_yi)^T表示与参考点(r_xi,r_yi)^T对应的匹配点，p_xi和p_yi分别表示对应匹配点的经度和纬度信息。于是，我们得到

Δg(r_xi+e_xi,r_yi+e_yi)＝Δg(p_xi,p_yi)

其中，e_xi＝p_xi-r_xi表示匹配点与参考点之间的经度误差，e_yi＝p_yi-r_yi表示匹配点与参考点之间的纬度误差。

考虑到参考点位于匹配点附近区域，于是对上式进行一阶泰勒展开，即

$\mathrm{\&Delta;g}(r_{\mathrm{xi}},r_{\mathrm{yi}})+\frac{\&PartialD;\mathrm{\&Delta;g}}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;g}}{\&PartialD;y_i}{e}_{\mathrm{yi}}+R_{2i}=\mathrm{\&Delta;g}(p_{\mathrm{xi}},p_{\mathrm{yi}})$

式中，

表示参考点(r_xi,r_yi)^T处得重力异常在经度和纬度方向的变化梯度向量，R_2i表示二阶及二阶以上泰勒展开项。在忽略R_2i的情况下，可以得到

▽(Δg)_i·e_i+(Δg_i-l_i)＝0

式中，e_i＝[e_xi,e_yi]^T为参考点与匹配点之间的位置误差向量，l_i＝Δg(p_xi,p_yi)为利用重力仪测得参考点(r_xi,r_yi)^T处的重力异常值。

步骤三：忽略参考轨迹和匹配轨迹在采样间隔内航向和航速变化的情况下，在均方差算法（MSD）的基础上，引入位置误差向量e＝(e_x,e_y)，确定代价函数E。

具体为，假设在采样间隔内参考点与匹配点之间的航向和航速相差不大，也假设参考轨迹与匹配轨迹近似平行。在此基础上忽略上式中的R_2i，我们可确定代价函数为

$E=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(c_i+\mathrm{\&lambda;}{s}_i)$

其中，

$c_i={(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))}^2$

$s_i=\frac{1}{2}[{(e_{x(i+1)}-e_{\mathrm{xi}})}^2+{(e_{\mathrm{xi}}-e_{x(i-1)})}^2+{(e_{y(i+1)}-e_{\mathrm{yi}})}^2+{(e_{\mathrm{yi}}-e_{y(i-1)})}^2]$

式中，i＝1,2，…,N，N是参考轨迹点总数；λ为校正参数，且满足λ＞0，如果可以精确计算得到▽(Δg)_i和(Δg_i-l_i)，λ取值应适当增大，否则，适当减少λ取值。

步骤四：确定使代价函数E最小的位置误差向量e，过程中利用SOR迭代方法对位置误差向量e进行迭代更新，即在已知第k次SOR迭代后位置误差向量e(k)的基础上，计算第k+1次SOR迭代后的位置误差向量e(k+1)。

具体为，将代价函数E对e_xi和e_yi求一阶偏导数，即

$\frac{\&PartialD;E}{\&PartialD;e_{\mathrm{xi}}}=2\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))+2\mathrm{\&lambda;}(e_{\mathrm{xi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}})$

$\frac{\&PartialD;E}{\&PartialD;e_{\mathrm{yi}}}=2\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))+2\mathrm{\&lambda;}(e_{\mathrm{yi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}})$

其中，

和表示平均位置误差：

${\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}=\frac{e_{x(i-1)}+e_{x(i+1)}}{2}$

${\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}=\frac{e_{y(i-1)}+e_{y(i+1)}}{2}$

进一步，获取代价函数E对e_xi和e_yi的二阶偏导数：

$\frac{{\&PartialD;}^{2}E}{\&PartialD;e_{\mathrm{xi}}^2}=2{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2+2\mathrm{\&lambda;}$

$\frac{{\&PartialD;}^{2}E}{\&PartialD;e_{\mathrm{yi}}^2}=2{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2+2\mathrm{\&lambda;}$

从上式可以看出，

和

均为正数，那么存在位置误差向量e_i＝(e_xi,e_yi)，使得代价函数等于0，这里的e_i即为方程的解。于是，我们可以得到

$(\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2)e_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}(\mathrm{\&Delta;}{g}_i-l_i)$

$\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{e}_{\mathrm{xi}}+(\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2)e_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}(\mathrm{\&Delta;}{g}_i-l_i)$

式中，i＝1,2，…,N，N是参考轨迹点总数。

在此，引入SOR迭代方法获取位置误差向量，SOR迭代方法是在Jacobi和Gauss-Seidel迭代方法的改进。经过计算可知Gauss-Seidel迭代方法比Jacobi迭代方法省点计算量，也是Jacobi迭代方法的改进。可是精确度低，计算量高，费时间，需要改进。SOR迭代方法是进一步改进Gauss-Seidel迭代方法而得到的，比Jacobi和Gauss-Seidel迭代方法收敛速度快，综合性强。

便于SOR迭代，经整理，得到

$e_{\mathrm{xi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\frac{\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+(\mathrm{\&Delta;}{g}_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2}$

$e_{\mathrm{yi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)-\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\frac{\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+(\mathrm{\&Delta;}{g}_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;x_i}\right)}^2+{\left(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{\&PartialD;y_i}\right)}^2}$

式中，k为迭代的次数。

步骤五：每次迭代后进行不等式判断，若不等式成立，则迭代终止，所得结果即为匹配位置误差，若不等式不成立，则继续进行迭代匹配。

具体为，每次迭代后按下面的不等式进行判断

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({(e_{\mathrm{xi}}(k+1)-e_{\mathrm{xi}}\left(k\right))}^2+{(e_{\mathrm{yi}}(k+1)-e_{\mathrm{yi}}\left(k\right))}^2)<\mathrm{\&epsiv;}$

其中，判断阈值ε为给定值，且满足ε≥0。

若不等式成立，迭代终止，所得结果即为匹配位置误差，若不等式成立，继续进行迭代匹配。

步骤六：在参考位置(r_xi,r_yi)^T的基础上，加上匹配得到的位置误差(e_xi,e_yi)^T，即可得到最终位置(p_xi,p_yi)^T。

具体为，在参考位置(r_xi,r_yi)^T的基础上，加上匹配得到的位置误差(e_xi,e_yi)^T，即可得到最终位置，即

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right.$

根据步骤二可知，在进行一阶泰勒展开时，忽略了二阶以及二阶以上的泰勒展开项，随着初始位置误差e(0)的增大，匹配误差e也会越大。针对这一问题，可以通过有限次重复迭代来改善匹配的精度，即每次匹配的参考轨迹为上一次匹配后的匹配轨迹。

实施过程：采用EGM2008重力异常数据库，其网格精度为2′×2′，设惯导系统的初始位置误差为经度-0、01°和纬度0、01°，陀螺仪三个轴的常值漂移ε_x、ε_y和ε_z均为0、01°/h，加速度计零偏ΔA_x、ΔA_y和ΔA_z均为1×10^-5g，以45°航向匀速直航，忽略重力仪的测量误差，仿真时间为4500s。在上述条件下分别对传统MSD匹配方法和改进MSD匹配方法进行仿真。仿真结果见图2，传统MSD匹配后的位置误差为平均经度误差0.0052°和平均纬度误差0.0048°，匹配时间4.37s；改进MSD匹配方法的匹配误差为平均经度误差0.0034°和平均纬度误差0.0029°，匹配时间3.25s。仿真结果表明：比传统MSD匹配方法，改进MSD匹配方法在匹配精度和匹配效率都上有较大提高。

从以上实施例不难看出，针对传统的重力匹配方法，如MSD、MAD等，计算量大，匹配精度不高的问题。本发明引入位置误差向量，忽略参考轨迹和匹配轨迹在采样间隔内航向和航速的变化，并在MSD的基础上确定代价函数，简化匹配计算机中的匹配过程，提高了匹配计算机的工作效率，利用SOR迭代方法获取位置误差向量进而得到匹配轨迹，有效地提高了匹配的精度。

Claims (6)

1.一种基于改进MSD的重力匹配方法，其特征在于：

（1）确定惯性导航系统输出的参考位置信息：经度r_x和纬度r_y，根据计算机中预存的重力异常基准图即EGM2008重力数据库，获取该位置的重力异常值Δg和重力异常变化梯度

和

（2）引入位置误差向量e＝(e_x,e_y)，建立参考位置(r_x,r_y)与匹配位置(p_x,p_y)之间的等式，在等式两边作用重力异常函数，并进行一阶泰勒展开；

（3）忽略参考轨迹和匹配轨迹在采样间隔内航向和航速的变化，在均方差算法MSD的基础上，确定代价函数E；

（4）确定使代价函数E最小的位置误差向量e，利用SOR迭代方法对位置误差向量e进行迭代更新，即在已知第k次SOR迭代后位置误差向量e(k)的基础上，计算第k+1次SOR迭代时的位置误差向量e(k+1)；

（5）每次迭代后进行不等式判断，若不等式成立，迭代终止，所得结果即为匹配位置误差，否则，继续进行迭代匹配；

（6）在参考位置的基础上，加上匹配得到的位置误差，即可得到最终位置。

2.根据权利要求1所述的一种基于改进MSD的重力匹配方法，其特征在于：所述步骤（2）中，C_r表示参考轨迹，C_m表示匹配轨迹，参考轨迹上的点的坐标可表示为(r_xi,r_yi)^T，r_xi和r_yi分别表示参考点i的经度和纬度信息，其中i＝1,2，…,N，N是参考轨迹点总数，根据计算机中预存的重力异常基准图，可知该点的重力异常值Δg(r_xi,r_yi)，(p_xi,p_yi)^T表示与参考点(r_xi,r_yi)^T对应的匹配点，p_xi和p_yi分别表示对应匹配点的经度和纬度信息：

Δg(r_xi+e_xi,r_yi+e_yi)＝Δg(p_xi,p_yi)

其中，e_xi＝p_xi-r_xi表示匹配点与参考点之间的经度误差，e_yi＝p_yi-r_yi表示匹配点与参考点之间的纬度误差，进行一阶泰勒展开，即

$\mathrm{\&Delta;g}(r_{\mathrm{xi}},r_{\mathrm{yi}})+\frac{\&PartialD;\mathrm{\&Delta;g}}{\&PartialD;x_i}{e}_{\mathrm{xi}}+\frac{\&PartialD;\mathrm{\&Delta;g}}{\&PartialD;y_i}{e}_{\mathrm{yi}}+R_{2i}=\mathrm{\&Delta;g}(p_{\mathrm{xi}},p_{\mathrm{yi}})$

式中，

表示参考点(r_xi,r_yi)^T处得重力异常变化梯度向量，R_2i表示高阶泰勒展开项，忽略R_2i：

▽(Δg)_i·e_i+(Δg_i-l_i)＝0

式中，e_i＝[e_xi,e_yi]^T为参考点与匹配点之间的位置误差向量，l_i＝Δg(p_xi,p_yi)为利用重力仪测得参考点(r_xi,r_yi)^T处的重力异常值。

3.根据权利要求1所述的一种基于改进MSD的重力匹配方法，其特征在于：所述的代价函数

$E=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(c_i+{\mathrm{\&lambda;s}}_i)$

其中，

$c_i={(\frac{\&PartialD;\mathrm{\&Delta;}{g}_i}{{\&PartialD;x}_i}{e}_{\mathrm{xi}}+\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{e}_{\mathrm{yi}}+({\mathrm{\&Delta;g}}_i-l_i))}^2$

$s_i=\frac{1}{2}[{(e_{x(i+1)}-e_{\mathrm{xi}})}^2+{(e_{\mathrm{xi}}-e_{x(i-1)})}^2+{(e_{y(i+1)}-e_{\mathrm{yi}})}^2+{(e_{\mathrm{yi}}-e_{y(i-1)})}^2]$

式中，i＝1,2，…,N，N是参考轨迹点总数；校正参数λ＞0。

4.根据权利要求1所述的一种基于改进MSD的重力匹配方法，其特征在于：所述确定使代价函数E最小的位置误差向量e为代价函数E对e_xi和e_yi求一阶偏导数，即

$\frac{\&PartialD;E}{{\&PartialD;e}_{\mathrm{xi}}}=2\frac{\&PartialD;{\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}{e}_{\mathrm{xi}}+\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{e}_{\mathrm{yi}}+({\mathrm{\&Delta;g}}_i-l_i))+2\mathrm{\&lambda;}(e_{\mathrm{xi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}})$

$\frac{\&PartialD;E}{{\&PartialD;e}_{\mathrm{yi}}}=2\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}{e}_{\mathrm{xi}}+\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{e}_{\mathrm{yi}}+(\mathrm{\&Delta;}{g}_i-l_i))+2\mathrm{\&lambda;}(e_{\mathrm{yi}}-{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}})$

其中，平均位置误差可表示为

${\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}=\frac{1}{2}(e_{x(i-1)}+e_{x(i+1)})$

${\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}=\frac{1}{2}(e_{y(i-1)}+e_{y(i+1)})$

进一步，获取代价函数E对e_xi和e_yi的二阶偏导数

$\frac{{\&PartialD;}^{2}E}{{\&PartialD;e}_{\mathrm{xi}}^2}=2{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\right)}^2+2\mathrm{\&lambda;}$

$\frac{{\&PartialD;}^{2}E}{{\&PartialD;e}_{\mathrm{yi}}^2}=2{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}\right)}^2+2\mathrm{\&lambda;}$

和

均为正数，存在位置误差向量e_i＝(e_xi,e_yi)，使得代价函数等于0，即

$(\mathrm{\&lambda;}+{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\right)}^2)e_{\mathrm{xi}}+\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{e}_{\mathrm{yi}}={\mathrm{\&lambda;}\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}-\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}({\mathrm{\&Delta;g}}_i-l_i)$

$\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{e}_{\mathrm{xi}}+(\mathrm{\&lambda;}+{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}\right)}^2)e_{\mathrm{yi}}=\mathrm{\&lambda;}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}-\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}({\mathrm{\&Delta;g}}_i-l_i)$

式中，i＝1,2，…,N，N是参考轨迹点总数，

利用SOR迭代进行求解，即

$e_{\mathrm{xi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)-\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\frac{\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+({\mathrm{\&Delta;g}}_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\right)}^2+{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}\right)}^2}$

$e_{\mathrm{yi}}(k+1)={\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)-\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}\frac{\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{xi}}\left(k\right)+\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}{\stackrel{\&OverBar;}{e}}_{\mathrm{yi}}\left(k\right)+({\mathrm{\&Delta;g}}_i-l_i)}{\mathrm{\&lambda;}+{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;x}_i}\right)}^2+{\left(\frac{{\&PartialD;\mathrm{\&Delta;g}}_i}{{\&PartialD;y}_i}\right)}^2}$

式中，k为迭代的次数。

5.根据权利要求1所述的一种基于改进MSD的重力匹配方法，其特征在于：所述步骤（5）每次迭代后按下面的不等式进行判断

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({(e_{\mathrm{xi}}(k+1)-e_{\mathrm{xi}}\left(k\right))}^2+{(e_{\mathrm{yi}}(k+1)-e_{\mathrm{yi}}\left(k\right))}^2)<\mathrm{\&epsiv;}$

其中，判断阈值ε为给定值，且满足ε≥0，若不等式成立，迭代终止，所得结果即为匹配位置误差，若不等式成立，继续进行迭代匹配。

6.根据权利要求1所述的一种基于改进MSD的重力匹配方法，其特征在于：所述步骤（6）在参考位置(r_xi,r_yi)^T的基础上，加上匹配得到的位置误差e_i＝[e_xi,e_yi]^T，即可得到最终位置，即

$\left\{\begin{array}{c}{p}_{\mathrm{xi}}=r_{\mathrm{xi}}+e_{\mathrm{xi}}\\ p_{\mathrm{yi}}=r_{\mathrm{yi}}+e_{\mathrm{yi}}\end{array}\right..$

Images