王跃钢，文超斌，2＊，郭志斌，杨家胜，左朝阳

1第二炮兵工程大学304教研室，西安 710025

2中国人民解放军96124部队，通化 134000

1 引言

现有海洋重力场的分辨率已经达到2′×2′，进一步插值精细化处理可达1′×1′，重力异常精度达到3～8mGal，同时海洋重力仪的测量精度达到1mGal，这给高精度下实施重力辅助导航提供了可能性（王虎彪等，2012）.重力匹配是重力辅助惯性导航的核心技术，是重力辅助导航定位算法能否成功的关键，其基本思路就是通过某种策略实现由惯性导航系统指示的导航系统位置到导航系统真实位置的最优逼近.目前重力匹配算法主要是借鉴较为成熟的地形匹配算法.按照算法设计原理可分为序列相关匹配方法和递推滤波方法两种，序列相关匹配方法主要包括最近等值线迭代算法（the Iterated Closest Contour Point，ICCP）、相关极值分析算法两大类，而递推滤波方法以桑迪亚惯性地形辅助导航（SITAN）算法为典型代表，它们都有一定的局限性.基于相关极值分析的匹配算法往往采用全局遍历搜索的搜索策略，在搜索区域较大的情况下运算量较大，导致算法实时性较差，匹配时间较长，不利于与惯导进行组合（闫利等，2009；李姗姗等，2011；Yuan etal.，2011）.基于最近等值线迭代算法和递推滤波方法的匹配算法要求惯导测量位置与目标对象已经很接近，加之反复的全局变换运算会大大提高计算量，影响匹配的实时性（Jiang etal.，2009；孙枫等，2009；Cheng etal.，2009）.所以本文针对实际惯导初始误差较大的情况，提出一种基于径、向分析的重力辅助导航算法，该算法充分利用惯导短时精度高的特点给出相对运动的半径和方向角匹配约束模型，然后通过对待匹配点相似性进行度量，筛选求取最优匹配参数.算法精度、稳定性、实时性大幅增强.

2 基于径、向分析的实时重力辅助导航匹配算法基本原理

径、向分析匹配算法的流程分为：粗匹配、精匹配和跟踪匹配.粗匹配的主要任务是在惯导误差的估计范围内快速建立径、向模型，从而获取粗略的匹配结果，为精匹配提供可靠的初始匹配参数；精匹配是为了获取更高精度的匹配结果；而跟踪匹配是后续的实时匹配.其匹配流程如图1所示.

2.1 算法粗匹配

图1 径、向匹配算法流程图Fig.1 Flow chart of the R-D matching algorithm

根据重力导航基本理论，数据预处理得到等值点.第一步：固定搜索区域.由于惯性导航系统存在漂移，所以实际位置应该在以惯导输出为中心，惯性导航系统的漂移误差的三倍为半径的圆形搜索范围内.这里分别以惯导进入适配区后惯导系统指示的三个位置点（其中坐标值表示测量点的经纬度信息）为中心，在重力图中下述经纬度范围内进行搜索：

上述（1）—（4）式中i＝1，2，3，round表示取整数运算表示惯导系统的初始误差（°），δ1表示随机常值漂移误差（（°）／h），δ2表示随机噪声（°）.

第二步：寻找等值点.利用导航系统重力传感器输出的重力实测值g（x，y）与重力图分辨率为σ的搜索区域内每个网格点上的重力数据g（xk，yk）进行比较确定等值点：同一个重力网格单元上的四个重力值都小于或者大于g（x，y）时，取网格中心点为等值点坐标值；同一个重力网格单元边上的重力值介于g（x，y）时线性插值得到等值点的坐标.这样，每个搜索区域内都可以找到多个重力等值点构成相应的等值点集，由于重力场在空间上具有连续性，所以在该等值点集中相对载体真实位置最近的等值点与载体真实位置点在同一个重力网格内，也就是说与载体真实位置最近的等值点距离误差最大值不超过（σ为重力图分辨率），用算式表示为：

第三步：建立径、向匹配模型.如图2所示，由二维笛卡儿坐标平面上的三个不共线点可以求出其外接圆圆心、外接圆半径r，而后以该外接圆圆心o为坐标原点，以P1点与o点的连线为极轴，可以建立极坐标系（ρ，β）.这样可定义：外接圆半径r为三点对应的匹配半径长度；点和）在定义极坐标系中的角度值分别为三点对应的匹配方向角θ、φ，于是有：

图2 径、向匹配模型图Fig.2 Model of R-D matching

下面首先给出一种适合计算机编程，且可使程序结构优化、提高运算效率的匹配半径长度r的求解方法.具体叙述如下：

利用点坐标结合圆方程可得：

用（7）—（6），（8）—（6）消去r，并且简化得：

对（9）式进一步分析化简，为求x0，y0，令

则（9）式可写为二元一次形式

解（11）式知：

所以P1，P2，P3对应的匹配半径长度为：

接着，设二维笛卡儿坐标平面上的三点P1（x1，y1），P2（x2，y2），P3（x3，y3）的横、纵坐标误差均为Δ，由此横、纵坐标误差引起的匹配半径r的系统误差设为Δr，引起的匹配方向角θ、φ的系统误差分别设为Δθ、Δφ，根据误差理论：

如图2所示方法建立极坐标系后，P1，P2，P3三点的极坐标分别为：P1（r，0），P2（r，θ）和P3（r，φ），所以P1，P2，P3三点对应的原二维笛卡儿坐标平面上的坐标值为：P1（x0+r，y0），P2（x0+rcosβ，y0+rsinβ），P3（x0+rcosφ，y0+rsinφ），因而将x0，y0看成是关于x1、x2、x3、y1、y2、y3的复合函数，分别求（13）式中三个式子对x1、x2、x3、y1、y2、y3的各偏导数，而后结合（14）式以及（12）式两个分式分别对x1、x2、x3、y1、y2、y3的各偏导数，并将笛卡儿坐标值代入，可得：

同理根据误差理论得：

由几何关系，及余弦定理得：

先考虑π≤θ≤2π的情况，为公式推导方便，令

将R，r看 成 是 关 于x1、x2、x3、y1、y2、y3的 复 合 函数，分别求（17）式对x1、x2、x3、y1、y2、y3的偏导数，并将结果代入（17）式，而后进一步求R关于x1，x2，y2的偏导数，并最终将点P1（x0+r，y0），P2（x0+rcosβ，y0+rsinβ），P3（x0+rcosφ，y0+rsinφ）对应坐标值代入化简得：

同理知π≤θ≤2π时的情况：

结合（20）、（21）式知：

结合（15）式即：

同理由几何关系，及余弦定理得：

按照相似推导过程可得：

设运载体进入适配区后依时间先后顺序指示的三个不共线的实际轨迹位置点为A，B，C，按上述依时间先后顺序将其分别视为笛卡儿坐标平面上的三点P1（x1，y1），P2（x2，y2），P3（x3，y3），其中横纵坐标值表示经纬度信息值，而后结合式（13）、（17）、（23）求解其对应的匹配半径记为rp，匹配方向角记为θp，φp，另外设A，B，C三点依次对应的测量点轨迹为：Am，Bm，Cm，同理按上述依时间先后顺序进行处理，结合式（13）、（17）、（23）得到的测量点轨迹Am，Bm，Cm对应的匹配半径记为rm，匹配方向角记为θm，φm.由于惯性导航系统短时间内精度较高，所以可以认为惯导系统短时间内给出的测量位置的相对信息和惯导系统真实位置的相对信息无误差，所以可利用测量位置的相对信息来代替描述载体真实位置的相对信息量，同时由于笛卡儿坐标平面上的匹配半径、匹配方向角均代表了空间位置点的相对信息，所以有：

于是，设由Am、Bm、Cm三点按重力导航数据预处理方法计算得到的等值点集为Ae、Be、Ce，在该三点集中依次取一点，按上述利用（13）、（17）、（23）式求其对应的匹配半径集合记为re（该集合元素记为re），匹配方向角集合记为θe（该集合元素记为θe）、φe（该集合元素记为φe）；在等值点集Ae、Be、Ce中分别距离A、B、C三点最近的等值点，也就是最佳粗匹配点记为、、，按上述利用（13）、（17）、（23）式求其对应的匹配圆半径，记为，匹配方向角记为、.

又令：（1）匹配半径约束条件：

（2）匹配方向角约束条件1：

（3）匹配方向角约束条件2：

2.2 算法精匹配

第一步：筛选最优匹配点.将经过2.1节所述得到的粗匹配点代入（32）式所示离散相似度模型函数，寻求相似度值最大的匹配等值点Aec，Bec，Cec作为本次匹配的结果.同时，相似度可以用来评价匹配算法的效果，从而决定继续匹配还是返回粗匹配：

第二步：局部重力图优化.利用插值算法进行重力图局部精确化处理，建立局部连续重力场结构，消除由于重力图量化误差引起的整个重力辅助惯性导航系统匹配误差.具体的做法是在以匹配等值点Aec，Bec，Cec为中心，以一个重力图分辨率大小σ为半径的区间上通过高斯样条函数二维逼近局部离散背景场，获得连续背景场解析式G（x，y）.

第三步：建立优化匹配模型.分别在以等值匹配点Aec，Bec，Cec为中心，以一个重力图分辨率大小σ为半径的连续场搜索区域内寻找，使下述（33）式所示相似度优化匹配模型最大的匹配点作为最终的运载体最优匹配轨迹点.由此将局部连续重力异常场的最优匹配点搜索过程转化为在非线性约束条件下的非线性多维函数寻优过程.并且可以指标函数的最大值大小判断是否返回粗匹配还是继续进行下一步的跟踪匹配.

其中gmA，gmB，gmC分别表示导航系统在A，B，C三点重力传感器输出重力值，gA，gB，gC表示连续场搜索区域内由连续背景场解析式G（x，y）计算得到的重力值，其他参数定义如前述.

2.3 算法跟踪匹配

跟踪匹配阶段，每一步都沿用之前已经确定的两个匹配点的信息，只需要对新的惯导系统指示点进行块内搜索匹配，最终寻找到使（33）式所示相似度优化匹配模型最大的匹配点，作为本次最优匹配轨迹点即可，具体搜索匹配方法和上述粗匹配、精匹的过程相同.同时在匹配过程中利用（33）式相似度函数，研究匹配是否继续进行还是返回粗匹配.

3 重力图局部连续和优化模型求解

3.1 重力图局部连续

这里采用二维高斯基函数对局部离散格网重力异常基准图进行逼近获取其解析.设：G（x）＝e-x2／a2，设局部重力异常基准图等分格网点集合为｛（xi，yi，zi，j）｜i＝1，2，…，m；j＝1，2，…，n｝.则其x方向一维高斯基函数解析式为Lx＝Span｛Gx（（xxi）／σx）｝，y方向一维高斯基函数解析式为Ly＝Span｛Gy（（y-yi）／σy）｝，其中σx、σy为格网点分辨率，则x，y平面的二维高斯基函数可以写成Lx与Ly的张量积形式：

即得：

则（35）式可以简化为矩阵形式：

根据插值条件：

有如下线性方程：

已知X，Y均为非奇异矩阵，即（37）式有唯一解，解方程即得到系数矩阵C，将系数矩阵代入式（36）便得到该局部重力异常基准图二维高斯基函数逼近解析式.需要注意的是，参数a的取值对高斯基函数逼近效果影响很大，a过大或过小都将导致很大的逼近误差，需要通过建立调优模型并进行解算获取参数a的最优值.限于篇幅，本文对参数a调优过程不作介绍，最后仿真试算取a为2即可获得较高的逼近精度（Xiao and Bian，2010；童余德等，2011；童余德等，2012）.

3.2 优化模型求解

约束条件下最优化模型求解问题通常称为无约束最优化方法（Unconstrained Optimization Method）.一般来说无约束最优化问题的求解是通过一系列的迭代算法来实现的.具体的迭代收敛算法很多：如牛顿法、梯度法、共轭方向法、拟牛顿法、方向加速法、步长加速法等.从本质上各种迭代收敛算法仅存在迭代方向矢量sk与长度tk≥0的差别.本文采用拟牛顿法编程解决（33）式非线性模型求解问题.拟牛顿法的迭代步骤为：

第一步：选取初始值x0，给出允许误差ε＞0；并令k＝0；

第四步：进行一维搜索求tk和xk+1，使得：求下一迭代点，令：返回第二步.

4 仿真实验与分析

本文采用我校惯性制导实验室研制开发的轨迹生成器软件生成仿真验证需要的真实航迹和惯导指示航迹.而重力异常数据使用渤海地区经度范围118°—119°，纬度范围37°—38°的真实数据，该重力异常场的三维图如图4所示.重力图分辨率为1′×1′，重力仪的实时量测数据是利用真实航迹在重力数据库中的采样获取，运载体质量为5000kg，速度为60km／h，航迹的初始经度为118.02°，纬度为37.24°，航行时长60min，采样时间为3min.惯导指示航迹的误差源有惯导初始位置误差、常值漂移和随机噪声3种，常值漂移误差为0.01（°）／h；随机噪声为均值为0，标准差为0.001的高斯分布白噪声；本文分别分析了惯导初始误差分别为0.01°和0.05°两种情况下进行的仿真，并对比分析了传统ICCP算法、相关极值算法和本文新算法的匹配效果.

图4为初始误差为0.01°时经纬度的匹配误差.图5为初始误差为0.05°时经纬度的匹配误差.由于新算法充分利用了导航系统的相对位置信息，在匹配过程当中加入了判断机制，每步匹配过程中分粗、精匹配两个步骤进行匹配参数寻优，在粗匹配阶段利用位置信息结合误差理论建立门限值进行参数寻优，而在精匹配阶段融入了重力图局部插值连续化理论和优化模型求解理论.由图可见新算法显著提高了导航的精度.在初始误差较小时新算法的精度高于ICCP算法和相关极值算法，三类算法都能够正常工作；初始误差较大时新算法的精度仍然高于ICCP算法和相关极值算法，在这种情况下，ICCP算法误差明显增大，新算法的精度高于传统匹配算法的特点明显显露出来，说明新算法的粗匹配、精匹配在整个重力匹配过程中发挥了非常大的作用，说明了上述理论结果的正确性.

图3 重力异常场三维图Fig.3 3Dmap of gravity anomaly field

图4 较小误差时算法仿真结果（a）经度误差；（b）纬度误差.Fig.4 Simulation results of the algorithm for small initial errors（a）Longitudinal errors；（b）Latitudinal errors.

图5 较大误差时算法仿真结果（a）经度误差；（b）纬度误差.Fig.5 Simulation results of the algorithm for large initial errors（a）Longitudinal errors；（b）Latitudinal errors.

表1 三种算法仿真误差对比Table 1 Comparison of simulation errors of three algorithms

表1给出了2次仿真对应三种算法的平均匹配误差，由表可知无论惯导初始为0.01°还是0.05°，径、向匹配算法的匹配误差都能够保持在0.001°～0.003°（约110～330m）左右；相关极值能够保持在0.01°～0.025°（约1100～2750m）左右；ICCP算法只有在惯导初始误差为0.01°的情况下匹配误差接近0.016°（大约一个重力图分辨率），在惯导初始误差为0.05°的情况下经度匹配误差超过0.1°（大约11000m）.总体来看，新算法可以在0.01°到0.05°的初始误差情况下将惯导经纬度误差降至随机漂移（0.01°）的20%左右.

5 结论

本文研究传统ICCP、相关极值、（SITAN）等重力辅助惯性导航匹配算法基础上，提出了基于径向分析的重力辅助导航新算法，给出了一种重力辅助惯性导航匹配算法中寻求等值点的方法，基于该方法的研究成果从误差分析角度入手进行建模，得出了新匹配算法所应遵循的匹配门限条件，定义了新算法匹配相似度函数，为了进一步提高和完善新算法，从重力图局部优化和模型优化求解两个方面进行了相关研究.实验结果表明，基于径向分析的重力辅助导航新算法，可以在惯导初始误差较大的情况下进行精确、快速、高效的导航匹配，极大地克服了惯性导航设备由于时间推移误差积累的问题.

Cheng Y，Xiu C B，Luo J.2009.The simulation of ICCP algorithm in the gravity aided navigation.∥Proceedings of the Second International Conference on Intelligent Networks and Intelligent Systems.Tianjin：IEEE：62-65.

Jiang D F，Tong Y D，Bian S F，etal.2012.The study on ICCP algorithm for gravity matching based on local continuous field.Geomatics and Information Science of Wuhan University（in Chinese），37（10）：1203-1206.

Jiang F，Deng X T，Zhang Z S，etal.2009.Seabed terrain matching algorithm basing on correction factor of tide and probability data associate filtering.∥Proceedings of International Colloquium on Computing，Communication，Control，and Management.Sanya：IEEE，2：63-67.

Li S S，Wu X P，Ma B.2011.Correlative extremum matching algorithm using underwater gravity anomalies.Acta Geodaetica et Cartographica Sinica（in Chinese），40（4）：464-469，476.

Raty M，Kangas A.2012.Comparison of k-MSN and Kriging in local prediction.Forest Ecology and Management，263（1）：47-56.

Sun F，Wang W J，Gao W，etal.2009.Contour matching algorithm for passive gravity navigation.Chinese Journal of Scientific Instrument（in Chinese），30（4）：817-822.

Tong Y D，Bian S F，Jiang D F，etal.2011.Gravity matching aided navigation based on local continuous field.Journal of Chinese Inertial Technology（in Chinese），19（6）：681-685.

Tong Y D，Bian S F，Jiang D F，etal.2012.A new integrated gravity matching algorithm based on approximated local gravity map.Chinese Journal of Geophysics（in Chinese），55（9）：2917-2924.

Wang H B，Wang Y，Fang J，etal.2012.Simulation research on a minimum root-mean-square error rotation-fitting algorithm for gravity matching navigation.Science China Earth Sciences，55（1）：90-97.

Wang Z G，Bian S F.2008.A local geopotential model for implementation of underwater passive navigation.Progress in Natural Science，18（9）：1139-1145.

Wu X P，Li S S.2010.Progress of underwater gravity-aided inertial navigation.IEEE，The International Union of Geodesy and Geophysics，Beijing，79-86.

Xia B，Wang H，Wang S W，etal.2010.Gravity matching algorithm based on intrinsic features threshold of gravitational field.∥Proceedings of the Sixth International Symposium on Precision Engineering Measurements and Instrumentation，7544：1-8.

Xiao S H，Bian S F.2010.Research on regional model of continuous Fourier series of marine magnetic anomaly field using for the geomagnetic navigation.∥Proceedings of the 2nd International Conference on Industrial and Information System.Dalian：IEEE，2：437-440.

Yan L，Cui C F，Wu H L.2009.A gravity matching algorithm based on TERCOM.Geomatics and Information Science of Wuhan University（in Chinese），34（3）：261-264.

Yuan G N，Zhang H W，Yuan K F，etal.2011.A combinational underwater aided navigation algorithm based on TERCOM／ICCP and Kalman filter.∥Proceedings of the 4th International Joint Conference on Computational Sciences and Optimization.Yunnan：IEEE：952-955.

附中文参考文献

蒋东方，童余德，边少锋等.2012.ICCP重力匹配算法在局部连续背景场中的实现.武汉大学学报，37（10）：1203-1206.

李姗姗，吴晓平，马彪.2011.水下重力异常相关极值匹配算法.测绘学报，40（4）：464-469，476.

孙枫，王文晶，高伟等.2009.用于无源重力导航的等值线匹配算法.仪器仪表学报，30（4）：817-822.

童余德，边少锋，蒋东方等.2011.基于局部连续场的重力匹配辅助导航.中国惯性技术学报，19（6）：681-685.

童余德，边少锋，蒋东方等.2012.一种新的基于局部重力图逼近的组合匹配算法.地球物理学报，55（9）：2917-2924.

王虎彪，王勇，方剑等.2012.“最小均方误差旋转拟合法”重力辅助导航仿真研究.中国科学，42（7）：1055-1062.

闫利，崔晨风，吴华玲.2009.基于TERCOM算法的重力匹配.武汉大学学报，34（3）：261-264.