现有海洋重力场的分辨率已经达到2′×2′，进一步插值精细化处理可达1′×1′，重力异常精度达到3~8 mGal，同时海洋重力仪的测量精度达到1 mGal，这给高精度下实施重力辅助导航提供了可能性().重力匹配是重力辅助惯性导航的核心技术，是重力辅助导航定位算法能否成功的关键，其基本思路就是通过某种策略实现由惯性导航系统指示的导航系统位置到导航系统真实位置的最优逼近.目前重力匹配算法主要是借鉴较为成熟的地形匹配算法.按照算法设计原理可分为序列相关匹配方法和递推滤波方法两种，序列相关匹配方 法主要包括最近等值线迭代算法(the Iterated Closest Contour Point，ICCP)、相关极值分析算法两大类，而递推滤波方法以桑迪亚惯性地形辅助导航(SITAN)算法为典型代表，它们都有一定的局限性.基于相关极值分析的匹配算法往往采用全局遍历搜索的搜索策略，在搜索区域较大的情况下运算量较大，导致算法实时性较差，匹配时间较长，不利于与惯导进行组合(；；).基于最近等值线迭代算法和递推滤波方法的匹配算法要求惯导测量位置与目标对象已经很接近，加之反复的全局变换运算会大大提高计算量，影响匹配的实时性(; ；).所以本文针对实际惯导初始误差较大的情况，提出一种基于径、向分析的重力辅助导航算法，该算法充分利用惯导短时精度高的特点给出相对运动的半径和方向角匹配约 束模型，然后通过对待匹配点相似性进行度量，筛选 求取最优匹配参数.算法精度、稳定性、实时性大幅增强.
2 基于径、向分析的实时重力辅助导航匹配算法基本原理
径、向分析匹配算法的流程分为：粗匹配、精匹配和跟踪匹配.粗匹配的主要任务是在惯导误差的估计范围内快速建立径、向模型，从而获取粗略的匹配结果，为精匹配提供可靠的初始匹配参数；精匹配是为了获取更高精度的匹配结果；而跟踪匹配是后续的实时匹配.其匹配流程如图 1所示.
图 1 径、向匹配算法流程图
Fig. 1 Flow chart of the R-D matching algorithm
2.1 算法粗匹配
根据重力导航基本理论，数据预处理得到等值点. 第一步：固定搜索区域.由于惯性导航系统存在漂移，所以实际位置应该在以惯导输出为中心，惯性导航系统的漂移误差的三倍为半径的圆形搜索范围内.这里分别以惯导进入适配区后惯导系统指示的三个位置点 Am(x1，y1)，Bm(x2，y2)，Cm(x3，y3) (其中坐标值表示测量点的经纬度信息)为中心，在重力图中下述经纬度范围内进行搜索：
上述(1)—(4)式中i=1，2，3，round表示取整数运算. δ=3(δ0+tkδ1+δ2)，δ0 表示惯导系统的初始误差(°)， δ1 表示随机常值漂移误差((°)/h)， δ2 表示随机噪声(°).
第二步：寻找等值点.利用导航系统重力传感器输出的重力实测值 g(x，y) 与重力图分辨率为 σ 的搜索区域内每个网格点上的重力数据 g(xk，yk) 进行比较确定等值点：同一个重力网格单元上的四个重力值都小于或者大于 g(x，y) 时，取网格中心点为等值点坐标值；同一个重力网格单元边上的重力值介于 g(x，y) 时线性插值得到等值点的坐标.这样，每个搜索区域内都可以找到多个重力等值点构成相应的等值点集，由于重力场在空间上具有连续性，所以在该等值点集中相对载体真实位置最近的等值点与载体真实位置点在同一个重力网格内，也就是说与载体真实位置最近的等值点距离误差最大 值不超过(σ 为重力图分辨率)，用算式表示为：
第三步：建立径、向匹配模型.如图 2所示，由二维笛卡儿坐标平面上的三个不共线点 P1(x1，y1)，P2(x2，y2)，P3(x3，y3)， 可以求出其外接圆圆心 o(x0，y0)、外接圆半径r， 而后以该外接圆圆心 o 为坐标原点，以 P1点与o 点的连线为极轴，可以建立极坐标系 (ρ，β). 这样可定义：外接圆半径 r 为三点 P1，P2，P3 对应的匹配半径长度；点 P2(r，β2)和P3(r，β3) 在定义极坐标系中的角度值 β2、β3， 分别为三点 P1，P2，P3 对应的匹配方向角 θ、φ，于是有：xi=x0+rcosβi，yi=x0+rsinβi(i=1，2，3)；β1=0，β2=θ，β3=φ.
图 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φ) 对应坐标值代入化简得：
结合(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三点按重力导航数据预处理方法计算得到的等值点集为 A e、Be、Ce， 在该三点集中依次取一点，按上述利用(13)、(17)、(23)式求其对应的匹配半径集合记为 re (该集合元素记为 re)，匹配方向角集合记为 θe (该集合元素记为 θe)、φe(该集合元素记为 φe)； 在等值点集 Ae、Be、Ce 中分别距离A、B、C三点最近的等值点，也就是最佳粗匹配点记为Aebest、Bebest、Cebest，按上述利用(13)、(17)、(23)式求其对应的匹配圆半径，记为rebest，匹配方向角记为θebest、φebest.
对于每次重力导航匹配解算，由2.2节分析知最佳粗匹配点Aebest，Bebest，Cebest与载体真实位置点A，B，C在同一个重力网格内，也就是说与载体真实位置最近的等值点距离误差最大值不超过(σ为重力图分辨率)，所以说最佳粗匹配点的相对实际轨迹点经纬度坐标误差Δerrbest最大值为， 该值小于一个重力分辨率，这个精度与目前一般重力导航算法研究的精度相当，所以可以将误差Δerrbest看成是重力辅助惯导系统的导航系统误差进行处理，而将最佳粗匹配点与实际轨迹点分别结合式(13)、(17)、(23)求得的半径误差，方向角误差都看成是由该系统误差Δerrbest引起的结果，显然Δerrbest小于. 所以由(15)、(22)、(24)式知最佳粗匹配点Aebest，Bebest，Cebest相对于实际轨迹点确定的半径和两个方向角误差分别为(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)式所示的匹配约束条件进行粗匹配，可确保得到的点集包含了最佳粗匹配点Aebest，Bebest，Cebest.
2.2 算法精匹配
第一步：筛选最优匹配点.将经过2.1节所述得到的粗匹配点代入(32)式所示离散相似度模型函 数，寻求相似度值最大的匹配等值点 Aec，Bec，Cec
作为本次匹配的结果.同时，相似度可以用来评价 匹配算法的效果，从而决定继续匹配还是返回粗匹配：
第二步：局部重力图优化.利用插值算法进行重力图局部精确化处理，建立局部连续重力场结构，消除由于重力图量化误差引起的整个重力辅助惯性导航系统匹配误差.具体的做法是在以匹配等值点 Aec，Bec，Cec 为中心，以一个重力图分辨率大小 σ 为半径的区间上通过高斯样条函数二维逼近局部离散背景场，获得连续背景场解析式 G(x，y).
第三步：建立优化匹配模型.分别在以等值匹配点 Aec，Bec，Cec 为中心，以一个重力图分辨率大小 σ 为半径的连续场搜索区域内寻找，使下述(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， 设局部重力异常基准图等分格网点集合为 {(xi，yi，zi，j)|i=1，2，…，m;j=1，2，…，n}. 则其 x 方向一维高斯基函数解析式为 Lx=Span{Gx((x－xi)/σ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即可获得较高的逼近精度(; ；).
3.2 优化模型求解
约束条件下最优化模型求解问题通常称为无约 束最优化方法(Unconstrained Optimization Method). 一般来说无约束最优化问题的求解是通过一系列的迭代算法来实现的.具体的迭代收敛算法很多：如牛顿法、梯度法、共轭方向法、拟牛顿法、方向加速法、步长加速法等.从本质上各种迭代收敛算法仅存在迭代方向矢量 s k与长度tk≥0 的差别.本文采用拟牛顿法编程解决(33)式非线性模型求解问题.拟牛顿法的迭代步骤为：
第一步：选取初始值x0，给出允许误差ε＞0；并令k=0；
第二步：检验是否满足终止条件计算 ‖ Δ f(xk)‖＜ε； 若成立则迭代结束，取 x*=xk， 否则转第三步；
第三步：构造牛顿方向，计算[Δ 2f(xk)]-1和dk=-[ Δ 2f(xk)]-1· Δ f(xk)；
第四步：进行一维搜索求 tk和xk+1， 使得： f(xk+tk·dk)=min t≥0 f(xk+t·dk)， 求下一迭代点，令： xk+1=xk+t·dk=xk+tk[ Δ 2f(xk)]-1· Δ f(xk)，k=k+1 返回第二步.
图 3 重力异常场三维图
Fig. 3 3D map of gravity anomaly field
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 较小误差时算法仿真结果
Fig. 4 Simulation results of the algorithm for small initial errors
图 4为初始误差为0.01°时经纬度的匹配误差.图 5为初始误差为0.05°时经纬度的匹配误差.由于新算法充分利用了导航系统的相对位置信息，在匹配过程当中加入了判断机制，每步匹配过程中分粗、精匹配两个步骤进行匹配参数寻优，在粗匹配阶段利用位置信息结合误差理论建立门限值进行参数寻优，而在精匹配阶段融入了重力图局部插值连续化理论和优化模型求解理论.由图可见新算法显著提 高了导航的精度.在初始误差较小时新算法的精度高于ICCP算法和相关极值算法，三类算法都能够正常工作；初始误差较大时新算法的精度仍然高于ICCP算法和相关极值算法，在这种情况下，ICCP算法误差明显增大，新算法的精度高于传统匹配算法的特点明显显露出来，说明新算法的粗匹配、精匹配在整个重力匹配过程中发挥了非常大的作用，说明了上述理论结果的正确性.
图 5 较大误差时算法仿真结果
Fig. 5 Simulation results of the algorithm for large initial errors
表 1给出了2次仿真对应三种算法的平均匹配误差，由表可知无论惯导初始为0.01°还是0.05°，径、向匹配算法的匹配误差都能够保持在0.001°～0.003°(约110～330 m)左右；相关极值能够保持在0.01°～0.025°(约 m)左右；ICCP算法只有在惯导初始误差为0.01°的情况下匹配误差接近0.016°(大约一个重力图分辨率)，在惯导初始误差为0.05°的情况下经度匹配误差超过0.1°(大约11000 m).总体来看，新算法可以在0.01°到0.05°的初始误差情况下将惯导经纬度误差降至随机漂移(0.01°)的20% 左右.
表 1(Table 1)
表 1 三种算法仿真误差对比
Table 1 Comparison of simulation errors of three algorithms
匹配算法初始误差(0.01°)初始误差(0.05°)经度(°)纬度(°)经度(°)纬度(°)相关极值0.01670.01100.02420.0173 ICCP0.01390.014710.11290.1319 新算法0.00180.00150.00250.0039
表 1 三种算法仿真误差对比
Table 1 Comparison of simulation errors of three algorithms
本文研究传统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.&&.
Jiang D F, Tong Y D, Bian S F, et al. 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): .
Jiang F, Deng X T, Zhang Z S, et al. 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.&&.
Li S S, Wu X P, Ma B. 2011. Correlative extremum matching algorithm using underwater gravity anomalies.&&.
Raty M, Kangas A. 2012. Comparison of k-MSN and Kriging in local prediction.&&.
Sun F, Wang W J, Gao W, et al. 2009. Contour matching algorithm for passive gravity navigation.&&.
Tong Y D, Bian S F, Jiang D F, et al. 2011. Gravity matching aided navigation based on local continuous field.&&.
Tong Y D, Bian S F, Jiang D F, et al. 2012. A new integrated gravity matching algorithm based on approximated local gravity map. Chinese Journal of Geophysics (in Chinese), 55(9): .
Wang H B, Wang Y, Fang J, et al. 2012. Simulation research on a minimum root-mean-square error rotation-fitting algorithm for gravity matching navigation.&&.
Wang Z G, Bian S F. 2008. A local geopotential model for implementation of underwater passive navigation.&&.
Wu X P, Li S S. 2010. Progress of underwater gravity-aided inertial navigation.&&.
Xia B, Wang H, Wang S W, et al. 2010. Gravity matching algorithm based on intrinsic features threshold of gravitational field.&&.
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.&&.
Yan L, Cui C F, Wu H L. 2009. A gravity matching algorithm based on TERCOM.&&.
Yuan G N, Zhang H W, Yuan K F, et al. 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.&&.
蒋东方, 童余德, 边少锋等. 2012. ICCP重力匹配算法在局部连续背景场中的实现.&&. ?
李姗姗, 吴晓平, 马彪. 2011. 水下重力异常相关极值匹配算法.&&. ?
孙枫, 王文晶, 高伟等. 2009. 用于无源重力导航的等值线匹配算法.&&. ?
童余德, 边少锋, 蒋东方等. 2011. 基于局部连续场的重力匹配辅助导航.&&. ?
童余德, 边少锋, 蒋东方等. 2012. 一种新的基于局部重力图逼近的组合匹配算法.&&. ?
王虎彪, 王勇, 方剑等. 2012. “最小均方误差旋转拟合法”重力辅助导航仿真研究.&&. ?
闫利, 崔晨风, 吴华玲. 2009. 基于TERCOM算法的重力匹配.&&.求大神给一个MATLAB编程，或者思路。我要把一个极坐标矩阵转换为直角坐_百度知道
求大神给一个MATLAB编程，或者思路。我要把一个极坐标矩阵转换为直角坐
hiphotos://f，第一行是极坐标的角度，直角坐标的x＝a[i][0]*sin(/zhidao/pic/item/0b46f21fbe096b63d3744ebf8ac04.baidu.hiphotos。如表格标.jpg" esrc="http！.com/zhidao/wh%3D450%2C600/sign=ca7efb973a022c8caf21fbe096b63d3744ebf8ac04.jpg" target="_blank" title="点击查看大图" class="ikqb_img_alink"><img class="ikqb_img" src="http。拜托了.baidu://f！<a href="/zhidao/wh%3D600%2C800/sign=beda27a0df5afe/0b46f21fbe096b63d3744ebf8ac04.5°)。就相当于，下面每一行都是距离.hiphotos://f！
提问者采纳
y]&=&R(2:);整理到与r等大[x;%&pol2cart(t;%&&nbsp.;&nbsp,;&角度转弧度t&&=&取出角度数据t&=&nbsp,r);=&%&&&nbsp,;R(1;&所有点到原点的距离数据t&&t*pi/180;=&&%&&nbsp,size(r.;&R&&&&nbsp,1);repmat(t;&nbsp.];&=&[:end,1);&%&原始数据r&nbsp:);&&nbsp%&nbsp
提问者评价
来自团队：
其他类似问题
为您推荐：
其他1条回答
y);)θ=arctan (y&#47：ρ=√(x&#178,θ)转换到直接坐标(x;+y&#178：x=ρ cos θy=ρ sin θ反过来极坐标(ρ
极坐标的相关知识
等待您来回答
下载知道APP
随时随地咨询
出门在外也不愁系统发生错误
系统发生错误
您可以选择 [
[ 错误信息 ]
您浏览的页面暂时发生了错误！请稍后再试～
ThinkPHP 2.0 { Fast & Simple OOP PHP Framework } -- [ WE CAN DO IT JUST THINK IT ]系统发生错误
系统发生错误
您可以选择 [
[ 错误信息 ]
您浏览的页面暂时发生了错误！请稍后再试～
ThinkPHP 2.0 { Fast & Simple OOP PHP Framework } -- [ WE CAN DO IT JUST THINK IT ]系统发生错误
系统发生错误
您可以选择 [
[ 错误信息 ]
您浏览的页面暂时发生了错误！请稍后再试～
ThinkPHP 2.0 { Fast & Simple OOP PHP Framework } -- [ WE CAN DO IT JUST THINK IT ]}