1 引言

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

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

2.1 算法粗匹配

根据重力导航基本理论，数据预处理得到等值点.

第一步：固定搜索区域.由于惯性导航系统存在漂移，所以实际位置应该在以惯导输出为中心，惯性导航系统的漂移误差的三倍为半径的圆形搜索范围内.这里分别以惯导进入适配区后惯导系统指示的三个位置点 A_m(x₁，y₁)，B_m(x₂，y₂)，C_m(x₃，y₃) (其中坐标值表示测量点的经纬度信息)为中心，在重力图中下述经纬度范围内进行搜索：

上述(1)—(4)式中i=1，2，3，round表示取整数运算. δ=3(δ₀+t_kδ₁+δ₂)，δ₀ 表示惯导系统的初始误差(°)， δ₁ 表示随机常值漂移误差((°)/h)， δ₂ 表示随机噪声(°).

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

第三步：建立径、向匹配模型.如图 2所示，由二维笛卡儿坐标平面上的三个不共线点 P₁(x₁，y₁)，P₂(x₂，y₂)，P₃(x₃，y₃)， 可以求出其外接圆圆心 o(x₀，y₀)、外接圆半径r， 而后以该外接圆圆心 o 为坐标原点，以 P₁点与o 点的连线为极轴，可以建立极坐标系 (ρ，β). 这样可定义：外接圆半径 r 为三点 P₁，P₂，P₃ 对应的匹配半径长度；点 P₂(r，β₂)和P₃(r，β₃) 在定义极坐标系中的角度值 β₂、β₃， 分别为三点 P₁，P₂，P₃ 对应的匹配方向角 θ、φ，于是有：x_i=x₀+rcosβ_i，y_i=x₀+rsinβ_i(i=1，2，3)；β₁=0，β₂=θ，β₃=φ.

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

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

用(7)—(6)，(8)—(6)消去r，并且简化得：

对(9)式进一步分析化简，为求 x₀，y₀， 令

则(9)式可写为二元一次形式

解(11)式知：

所以 P₁，P₂，P₃ 对应的匹配半径长度为：

接着，设二维笛卡儿坐标平面上的三点 P₁(x₁，y₁)，P₂(x₂，y₂)，P₃(x₃，y₃) 的横、纵坐标误差均为Δ，由此横、纵坐标误差引起的匹配半径 r 的系统误差设为Δr，引起的匹配方向角 θ、φ 的系统误差分别设为 Δ_θ、Δ_φ， 根据误差理论：

如图 2所示方法建立极坐标系后， P₁，P₂，P₃ 三点的极坐标分别为： P₁(r，0)，P₂(r，θ)和P₃(r，φ)，所以P₁，P₂，P₃ 三点对应的原二维笛卡儿坐标平面上的坐标值为： P₁(x₀+r，y₀)，P₂(x₀+rcosβ，y₀+rsinβ)，P₃(x₀+rcosφ，y₀+rsinφ)，因而将x₀，y₀ 看成是关于 x₁、x₂、x₃、y₁、y₂、y₃ 的复合函数，分别求(13)式中三个式子对 x₁、x₂、x₃、y₁、y₂、y₃ 的各偏导数，而后结合(14)式以及(12)式两个分式分别对 x₁、x₂、x₃、y₁、y₂、y₃ 的各偏导数，并将笛卡儿坐标值代入，可得：

同理根据误差理论得：

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

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

将 R，r 看成是关于 x₁、x₂、x₃、y₁、y₂、y₃ 的复合函数，分别求(17)式对 x₁、x₂、x₃、y₁、y₂、y₃ 的偏导数，并将结果代入(17)式，而后进一步求 R 关于 x₁，x₂，y₂ 的偏导数，并最终将点 P₁(x₀+r，y₀)，P₂(x₀+rcosβ，y₀+rsinβ)，P₃(x₀+rcosφ，y₀+rsinφ) 对应坐标值代入化简得：

结合(15)式即：

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

按照相似推导过程可得：

设运载体进入适配区后依时间先后顺序指示的三个不共线的实际轨迹位置点为A，B，C，按上述依时间先后顺序将其分别视为笛卡儿坐标平面上的三点 P₁(x₁，y₁)，P₂(x₂，y₂)，P₃(x₃，y₃)， 其中横纵坐标值表示经纬度信息值，而后结合式(13)、(17)、(23)求解其对应的匹配半径记为r_p，匹配方向角记为θ_p，φ_p，另外设A，B，C三点依次对应的测量点轨迹为：A_m，B_m，C_m，同理按上述依时间先后顺序进行处理，结合式(13)、(17)、(23)得到的测量点轨迹A_m，B_m，C_m对应的匹配半径记为r_m，匹配方向角记为θ_m，φ_m.由于惯性导航系统短时间内精度较高，所 以可以认为惯导系统短时间内给出的测量位置的相对信息和惯导系统真实位置的相对信息无误差，所以可利用测量位置的相对信息来代替描述载体真实位置的相对信息量，同时由于笛卡儿坐标平面上的匹配半径、匹配方向角均代表了空间位置点的相对信息，所以有：

于是，设由A_m、B_m、C_m三点按重力导航数据预处理方法计算得到的等值点集为 A _e、B_e、C_e， 在该三点集中依次取一点，按上述利用(13)、(17)、(23)式求其对应的匹配半径集合记为 r_e (该集合元素记为 re)，匹配方向角集合记为 θ_e (该集合元素记为 θe)、φ_e(该集合元素记为 φe)； 在等值点集 A_e、B_e、C_e 中分别距离A、B、C三点最近的等值点，也就是最佳粗匹配点记为A_ebest、B_ebest、C_ebest，按上述利用(13)、(17)、(23)式求其对应的匹配圆半径，记为r_ebest，匹配方向角记为θ_ebest、φ_ebest.

对于每次重力导航匹配解算，由2.2节分析知最佳粗匹配点A_ebest，B_ebest，C_ebest与载体真实位置点A，B，C在同一个重力网格内，也就是说与载体真实位置最近的等值点距离误差最大值不超过(σ为重力图分辨率)，所以说最佳粗匹配点的相对实际轨迹点经纬度坐标误差Δ_errbest最大值为， 该值小于一个重力分辨率，这个精度与目前一般重力导航算法研究的精度相当，所以可以将误差Δ_errbest看成是重力辅助惯导系统的导航系统误差进行处理，而将最佳粗匹配点与实际轨迹点分别结合式(13)、(17)、(23)求得的半径误差，方向角误差都看成是由该系统误差Δ_errbest引起的结果，显然Δ_errbest小于. 所以由(15)、(22)、(24)式知最佳粗匹配点A_ebest，B_ebest，C_ebest相对于实际轨迹点确定的半径和两个方向角误差分别为(26)—(28)式：

又令：(1)匹配半径约束条件：

(2)匹配方向角约束条件1：

(3)匹配方向角约束条件2：

由于 rm≈rp，θm≈θp，φm≈φp， Δ_errbest小于； 所以(29)、(30)、(31)式中定义的：Δ_maxr>Δ_ebestr，Δ_maxθ>Δ_ebestθ，Δ_maxφ>Δ_ebestφ，也就是利用(29)、(30)、(31)式所示的匹配约束条件进行粗匹配，可确保得到的点集包含了最佳粗匹配点A_ebest，B_ebest，C_ebest.

2.2 算法精匹配

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

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

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

其中 g_<sub>mA，g_<sub>mB，g_<sub>mC 分别表示导航系统在 A，B，C 三点重力传感器输出重力值， g_A，g_B，g_C 表示连续场搜索区域内由连续背景场解析式 G(x，y) 计算得到的重力值，其他参数定义如前述.

2.3 算法跟踪匹配

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

这里采用二维高斯基函数对局部离散格网重力异常基准图进行逼近获取其解析.设： G(x)=e^－x2/a2， 设局部重力异常基准图等分格网点集合为 {(x_i，y_i，z_i，j)|i=1，2，…，m;j=1，2，…，n}. 则其 x 方向一维高斯基函数解析式为 L_x=Span{G_x((x－x_i)/σx)}，y 方向一维高斯基函数解析式为 L_y=Span{G_y((y－y_i)/σy)}， 其中 σx、σy 为格网点分辨率，则 x，y 平面的二维高斯基函数可以写成 L_x与L_y 的张量积形式：

即得：

令

则(35)式可以简化为矩阵形式：

根据插值条件：

有如下线性方程：

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

3.2 优化模型求解

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

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

第二步：检验是否满足终止条件计算 ‖ Δ f(x^k)‖＜ε； 若成立则迭代结束，取 x^*=x^k， 否则转第三步；

第三步：构造牛顿方向，计算[Δ ²f(x^k)]^-1和d_k=-[ Δ ²f(x^k)]^-1· Δ f(x^k)；

第四步：进行一维搜索求 t_k和x^k+1， 使得： f(x^k+t_k·d_k)=min t≥0 f(x^k+t·d_k)， 求下一迭代点，令： x^k+1=x^k+t·d_k=x^k+t_k[ Δ ²f(x^k)]^-1· Δ f(x^k)，k=k+1 返回第二步.

4 仿真实验与分析

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

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

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

5 结论

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