CN102706348B - 一种基于三角形的重力图快速匹配方法 - Google Patents

Abstract

重力辅助惯性导航系统中一种基于三角形的重力图快速匹配方法，主要有粗匹配和精匹配两个过程。根据惯导系统短时精度高的特点，首先利用载体三个连续位置点间的相对距离信息建立标准三角形，然后结合载体上重力仪实时测量的重力值，通过粗匹配和精匹配两个过程，在已知重力图中建立匹配三角形；最后将匹配三角形与标准三角形进行匹配解算获得惯导系统长时间累积的位置误差和航向角误差。本发明是一种快速的重力图匹配方法，具有实时性好、精度高、鲁棒性好的特点，可应用于重力辅助惯性导航等无源导航系统。

Description

一种基于三角形的重力图快速匹配方法

技术领域

本发明涉及了重力辅助惯性导航系统中一种基于三角形的重力图快速匹配方法，主要针对重力辅助惯性导航中重力图的快速匹配问题。

背景技术

就地球物理场辅助惯性导航的无源导航技术来说，并不是一个新鲜的课题，最早开始的是陆地上的地形辅助惯性导航研究，并于20世纪70年代末80年代初取得明显的成果。近年来，借助大地测量技术成果，惯性/重力、惯性/地磁无源组合导航的理论和方法同样引起了国内外研究机构和学者的注意，相继开展了重力匹配导航、地磁匹配导航等技术的研究。目前重力辅助惯性导航的技术常采用的方法大多是沿用地形匹配的相关技术，主要有SITAN算法、TERCOM算法等。无论是SITAN算法，还是TERCOM算法，其基本流程都是通过运载体上承载的传感器实时测量运载体运动轨迹上的地球物理场数据；同时，根据惯导系统提供的运载体位置信息，从导航用参考重力图中获取地球物理场数据；然后将这两组数据集送给匹配解算计算机，利用匹配算法确定运载体的最佳匹配位置。

数据集匹配问题最早可以追溯到二十世纪五十年代末的遥感图像分析，那时Hobrough首次对遥感模拟图像进行了相似性分析，提出了图像匹配的概念，至六七十年代，数据集匹配的研究方法大多基于对图像的特征提取，如，Anuta的快速Fourier变换(FFT)方法，但是，由于提取特征方法的限制，使得在特征提取时数据集的信息不能完全得到反映。直到八十年代初，Lucas针对视频图像匹配给出了一种迭代算法，由于该方法直接针对图像数据集进行搜索，因此Lucas的方法完全保留了图像的性质，从而确保了匹配的精确性。到了九十年代初，Chen和Medioni，Besl和McKay以及Zhang等人分别在Lucas的工作基础上将各种具体匹配问题上升到数据集之间的匹配问题，并独立提出了求解数据集匹配问题的ICP(Iterated Closest Point，简称：ICP)算法，尤其是Besl和McKey的工作已经成为数据集匹配问题的一个基础，ICP方法也已经成为求解数据集匹配问题的一般性方法而被广泛采用，而且已被广泛应用于地球物理场辅助惯性导航的重力图(地形图/地磁图)匹配。

ICP算法是一个关于全局收敛性的研究，在理论上可以在任何初始位置误差情况都能工作，而在实际应用中由于目标函数、对应关系选择、变换类型以及初始位置的选择等因素的影响，几乎所有基于ICP的现有方法都只能得到局部最优匹配，是否能取得全局匹配最优的关键在于初始匹配参数的选择。针对初始匹配的选择问题，Hugli等人提出了基于初始参数划分的SIC-range方法，Chetverikov等人通过对每一步迭代中对应点的选择范围进行“裁剪”，并随着迭代的进行逐步缩小和修正对应点的选择范围，以上方法主要源于对静态遥感图像的相似性匹配，它不能满足重力辅助惯性导航系统中对实时性的要求。

发明内容

本发明解决的技术问题：克服现有技术的不足，提供一种重力辅助惯性导航系统中重力图匹配的快速匹配方法，可有效地提高重力辅助惯性导航系统的可靠性和实时性，从而可消除和克服在惯导系统输出的位置信息存在大误差条件下重力图匹配算法失效的缺陷。

本发明采用的技术方案步骤如下：

第一步，根据导航信息得出标准三角形。

从惯导系统的导航解算结果中提取经度、纬度作为位置点的信息，进而连续获取载体的三个位置点信息，以其两两间的航行距离作为标准三角形的三条边。

第二步，结合实时重力量测值，在重力图中进行大范围搜索，获取粗匹配点集。

在已知重力图中建立块阵列进行大范围搜索，块阵列和其中的每个块都是正方形，块阵列按每边上块的个数定义其级数，块阵列的最大级数根据惯导系统的估计位置误差确定。

以第一步中的三个位置点分别为中心在重力图中建立三个块阵列，然后结合第一步三个位置点对应的实时重力值，分别在三个块阵列中以块为最小单元搜索等值点，其搜索过程按照块阵列级数从小到大的顺序进行逐级搜索，由这些搜索的等值点构成粗匹配的三组点集。

第三步，通过标准三角形的约束，建立匹配三角形集，并对其进行筛选。

⑴以第一步标准三角形的三边距离为约束条件，将第二步粗匹配的三组点集按照排列组合的原则建立一组匹配三角形集，其约束条件中三边距离的误差限由第二步块阵列的单个块大小来确定。

⑵计算第三步⑴中各个匹配三角形和标准三角形对应三边的误差之和，并将所有匹配三角形按误差之和从小到大排序；

⑶筛选误差之和最小的匹配三角形用于后续的精匹配过程。

第四步，基于粗匹配筛选结果，在重力图中进行小范围搜索，获取精匹配点集。

以第三步⑶中筛选的匹配三角形的三个点分别为搜索中心，在重力图中建立单个块进行小范围搜索，其块的大小与第二步的块大小相等，然后仍结合第一步三个位置点对应的实时重力值，分别在三个块中搜索等值点，由这些等值点直接构成精匹配的三组点集。

第五步，通过标准三角形的约束，建立新的匹配三角形集，并进行筛选。

⑴以第一步标准三角形的三边距离为约束条件，利用第四步精匹配的三组点集建立一组新的匹配三角形集，其约束条件中三边距离的误差限由重力测量误差和重力图网格精度来确定；

⑵计算第五步⑴中各个新的匹配三角形和标准三角形对应三边的误差之和，并将所有新的匹配三角形按误差之和从小到大排序；

⑶筛选误差之和最小的匹配三角形用于后续的匹配解算。

第六步，利用精匹配筛选的匹配三角形进行匹配解算。

根据两个点集序列间误差平方和最小的原理建立匹配解算的价值函数，将第一步中标准三角形对应的点序列与第五步(3)中精匹配过程筛选的匹配三角形点序列进行匹配解算，以上两个三角形间存在平移和旋转变换，可以通过匹配解算可求得以上两个三角形间的平移向量和旋转矩阵，该平移向量和旋转矩阵中包含惯导系统长时间累积的位置误差和航向角误差信息。

本发明原理说明如下：

无源重力导航就是利用局部重力异常分布的任意性，对惯导系统的累积误差进行校正。虽然惯导系统给出的导航信息存在随时间而不断增大的累积误差，但是在短时间内惯导系统能够提供高精度的相对距离信息，所以本发明充分利用惯导系统短时高精度的特点，选取惯导系统提供的三个连续位置点建立标准三角形，该三角形与载体对应的三个真实点构成的三角形是全等关系，所以他们之间可以通过旋转和平移进行相互转换；然后结合重力仪在三个对应位置点量测的实时重力值，在重力图中获取满足标准三角形约束的匹配三角形集，并通过粗匹配和精匹配过程筛选对应距离误差之和最小的匹配三角形进行最终的匹配解算；最后基于两组点集序列间误差平方和最小的原理建立价值函数，并通过微分求解获得惯导系统长时间累积的位置误差和航向角误差。

附图说明

图1为本发明一种基于三角形的重力图匹配方法流程图；

图2为本发明中标准三角形与载体真实位置点的关系图；

图3为本发明粗匹配过程中逐级搜索的四级块阵列；

图4为本发明粗匹配过程建立的三角形集；

图5为本发明匹配三角形集中存在的奇异三角形；

图6为本发明三角形的旋转和平移原理图；

具体实施方式

本发明将按照以下步骤实现重力图的匹配过程：

第一步，根据导航信息得出标准三角形。

从惯导系统的导航解算结果中提取经度、纬度作为位置点的信息，进而连续获取载体的三个位置点信息，以其两两间的航行距离作为标准三角形的三条边。如图2所示：运载体在三个连续时刻t_k、t_k+1和t_k+2对应的真实位置点分别为A'、B'、C'，而由惯导系统解算出的对应位置点分别为A(λ_A,L_A)、B(λ_B,L_B)、C(λ_C,L_C)，其中λ_A、λ_B、λ_C分别表示A、B、C三点的经度，L_A、L_B、L_C分别表示A、B、C三点的纬度，图中细实线表示载体的真实轨迹，点划线表示惯导系统指示的轨迹，则载体两两位置点间的航行距离分别为AB、BC、AC。根据球面三角形的公式：

cosS₁＝cosS₂cosS₃+sinS₂sinS₃cosA₁

其中S₁,S₂,S₃分别为单位球面上一个球面三角形的三条边对应的弧长，A₁为弧S₂和S₃的球面夹角，当时，得到S₁的计算公式为：

S₁＝arccos(sinL_AsinL_B+cosL_AcosL_Bcos(λ_B-λ_A))

于是得到边AB的长度为：

AB＝R_e·S₁＝R_e·arccos(sinL_AsinL_B+cosL_AcosL_Bcos(λ_B-λ_A))

其中R_e为地球半径，同理得到BC的长度和AC的长度计算公式为：

BC＝R_e·arccos(sinL_BsinL_C+cosL_BcosL_Ccos(λ_C-λ_B))

AC＝R_e·arccos(sinL_AsinL_C+cosL_AcosL_Ccos(λ_C-λ_A))

这样根据惯导系统提供的位置信息，计算得到了标准三角形ΔABC。

第二步，结合实时测量重力值，在重力图中进行大范围搜索，获取粗匹配点集。

在已知重力图中建立块阵列进行大范围搜索，块阵列和其中的每个块都是正方形，本发明选取10×10的重力图网格方阵做为基本块(即e＝10)，块阵列按每边上块的个数定义其级数n，块阵列的最大级数n_max根据惯导系统的估计位置误差确定。

如图3所示：(a)、(b)、(c)、(d)分别为一级、二级、三级、四级的块阵列，块阵列中央空心点表示惯导系统提供的位置点(如A、B、C)，图中曲线表示重力图中的重力等值线，粗实曲线表示与中心位置点的测量重力值相等的等值线，粗实线上的黑实点表示对应块内的粗匹配点，每一级阵列的搜索范围都只限于阵列边缘的块(阴影部分块)。

⑴根据惯导系统的误差特性确定最大的块阵列级数：

$n_{\max }=\left[2\frac{t_k\×\mathrm{\δ}}{e\×\mathrm{\δ}}\right]+1$

其中，t_k为位置点A'对应的时刻，δ为惯导系统常值漂移误差，单位是·/h，σ为重力图分辨率，单位是·/格，e为块边长，[·]为取整运算。

确定了块阵列的最大级数后，在重力图中分别以位置点A、B、C为中心建立三个块阵列。

⑵结合载体在A'、B'、C'量测的重力值g_A、g_B、g_C，以块阵列级数从小到大的顺序进行逐级搜索，如果在某一级阵列中搜索到重力值的等值点，则停止下一级的搜索。

⑶分别求取单个块中所有等值点的平均值作为该块的代表点，没有等值点的块没有代表点，由所有块的代表点组成粗匹配的点集。这样可以获取三个位置点A、B、C对应的粗匹配点集P₁,P₂,P₃分别为：

$\left\{\begin{array}{c}{P}_1=\{P_{\mathrm{1,1}},P_{\mathrm{1,2}},...P_{1,i},...\},(i_{\max }={4n}_1-4)\\ P_2=\{P_{\mathrm{2,1}},P_{\mathrm{2,2}},...P_{2,j},...\},(j_{\max }={4n}_2-4)\\ P_3=\{P_{\mathrm{3,1}},P_{\mathrm{3,2}},...P_{3,k},...\},(k_{\max }={4n}_3-4)\end{array}\right.$

其中n₁,n₂,n₃分别表示搜索A、B、C对应等值点的块阵列级数，i_max,j_max,k_max分别表示三组点集P₁,P₂,P₃中点的个数。

第三步，通过标准三角形的约束，建立匹配三角形集，并对其进行筛选，其具体步骤为：

⑴以第一步标准三角形的三边距离为约束条件，利用第二步粗匹配的三组点集建立一组匹配三角形集。如图4所示，假设位置点A在块阵列第二级搜到两个待匹配点，位置点B也在块阵列第二级搜到两个待匹配点，位置点C在块阵列第二级搜到一个待匹配点，则以位置点A、B、C(空心点)对应的三个点集(实心点)排列组合可生成粗匹配的匹配三角形集{ΔP_1,iP_2,jP_3,k}。

三角形三边距离的约束条件为：

$\left\{\begin{array}{c}|P_{1,i}{P}_{2,j}-\mathrm{AB}|<R\\ |P_{2,j}{P}_{3,k}-\mathrm{BC}|<R\\ |P_{1,i}{P}_{3,k}-\mathrm{AC}|<R\end{array}\right.$

其中，R为距离误差限。

距离误差的来源包括：块单元引入的误差、重力观测值的误差和重力图网格精度引入的误差。粗匹配过程中的主要误差是由块单元引入的误差，所以误差限R的确定如下：

$R=e\&times;\mathrm{\&sigma;}\&times;\frac{\mathrm{\&pi;}}{180}\&times;R_e$

其中，π＝3.1415926，R_e＝6378165m为地球半径。

⑵计算第三步⑴中各个匹配三角形和标准三角形ΔABC对应三边距离的误差之和，并将所有匹配三角形按误差之和从小到大排序。其误差之和M_l的计算公式为：

M_l＝|P_1,iP_2,j-AB|+|P_2,jP_3,k-BC|+|P_1,iP_3,k-AC|

其中，l表示匹配三角形点集中的第l个三角形。

⑶剔除奇异三角形，并筛选误差之和最小的匹配三角形用于精匹配过程。

针对匹配三角形集中存在的翻转奇异三角形，其剔除条件为：

$(\stackrel{\&RightArrow;}{\mathrm{AB}}-\stackrel{\&RightArrow;}{\mathrm{BC}})\&CenterDot;(\stackrel{\&RightArrow;}{P_{1,i}{P}_{2,j}}-\stackrel{\&RightArrow;}{P_{2,j}{P}_{3,k}})>0$

其中 $\stackrel{\&RightArrow;}{\mathrm{AV}},\stackrel{\&RightArrow;}{\mathrm{BC}},\stackrel{\&RightArrow;}{P_{1,i}{P}_{2,j}},\stackrel{\&RightArrow;}{P_{2,j}{P}_{3,k}}$ 均表示向量。

如图5所示，ΔP_1,1P_2,1P_3,1和ΔP_1,2P_2,2P_3,2都满足第三步⑴中的约束条件，但是ΔP_1,2P_2,2P_3,2与标准三角形ΔABC存在翻转的对应关系，所以是一个奇异三角形。

通过上式剔除了奇异三角形后，选择距离误差之和最小的三角形(定义为ΔP_1,AP_2,BP_3,C)用于后续精匹配过程。

第四步，基于粗匹配筛选结果，在重力图中进行小范围搜索，获取精匹配点集。

以第三步⑶中筛选的匹配三角形的三个点P_1,A、P_2,B、P_3,C分别为搜索中心，在重力图中建立三个独立的块进行小范围搜索，其块的大小与第二步的块大小相等，然后仍结合第一步载体在A'、B'、C'三个位置点的重力值g_A、g_B、g_C，分别在三个块中搜索等值点，由这些等值点直接构成精匹配的三组点集P₁',P₂',P₃'分别为：

$\left\{\begin{array}{c}{P^{\text{'}}}_1=\{{P_{\mathrm{1,1}}}^{\text{'}},{P_{\mathrm{1,2}}}^{\text{'}},...{P_{1,i}}^{\text{'}},...\},(i_{\max }={{4n}_1}^{\text{'}}-4)\\ {P_2}^{\text{'}}=\{{P_{\mathrm{2,1}}}^{\text{'}},{P_{\mathrm{2,2}}}^{\text{'}},...{P_{2,j}}^{\text{'}},...\},(j_{\max }={{4n}_2}^{\text{'}}-4)\\ {P_3}^{\text{'}}=\{{P_{\mathrm{3,1}}}^{\text{'}},{P_{\mathrm{3,2}}}^{\text{'}},...{P_{3,k}}^{\text{'}},...\},(k_{\max }={{4n}_3}^{\text{'}}-4)\end{array}\right.$

其中，n₁',n₂',n₃'分别表示搜索P_1,A、P_2,B、P_3,C对应等值点的块阵列级数。

第五步，通过标准三角形的约束，建立新的匹配三角形集，并进行筛选。

⑴以第一步标准三角形的三边距离为约束条件，利用第四步精匹配的三组点集建立一组新的匹配三角形集{ΔP_1,i'P_2,j'P_3,k'}。其约束条件为：

$\left\{\begin{array}{c}|{P_{1,i}}^{\text{'}}{P_{2,j}}^{\text{'}}-\mathrm{AB}|<R^{\text{'}}\\ |{P_{2,j}}^{\text{'}}{P_{3,k}}^{\text{'}}-\mathrm{BC}|<R^{\text{'}}\\ |{P_{1,i}}^{\text{'}}{P_{3,k}}^{\text{'}}-\mathrm{AC}|<R^{\text{'}}\end{array}\right.$

其中R'为精匹配三边距离的误差限。

误差限R'由重力测量误差和重力图网格精度来确定:

$R^{\text{'}}=(1+\frac{\mathrm{\&zeta;}}{\mathrm{\&tau;}})\&times;\mathrm{\&sigma;}\&times;\frac{\mathrm{\&pi;}}{180}\&times;R_e$

其中，ξ为重力仪的测量误差(单位mGal)，τ为匹配点附近重力图单位格对应重力观测值的变化量(单位mGal/格)。

⑵计算第五步⑴中各新匹配三角形和标准三角形ΔABC对应三边的误差之和，并将所有新的匹配三角形按误差之和从小到大排序。其误差之和M'_l的公式为：

${M_l}^{\text{'}}=|{P_{1,i}}^{\text{'}}{P_{2,j}}^{\text{'}}-\mathrm{AB}|+|{P_{2,j}}^{\text{'}}{P_{3,k}}^{\text{'}}-\mathrm{BC}|+|{P_{1,i}}^{\text{'}}{P_{3,k}}^{\text{'}}-\mathrm{AC}|$

⑶筛选误差之和最小的匹配三角形(定义为ΔP_1,A'P_2,B'P_3,C')作为精匹配的结果，用于后续的匹配解算。

第六步，利用精匹配筛选的匹配三角形进行匹配解算。

根据两组点集序列间误差平方和最小的原理建立匹配解算的价值函数，将第一步中惯导系统提供的标准三角形对应点序列与精匹配筛选的匹配三角形对应点序列进行匹配解算，求得两个三角形间的位置平移向量和旋转矩阵。以下给出平移向量和旋转矩阵的求解过程：

⑴定义三角形匹配序列和价值函数。

如图6所示，为方便表示，将标准三角形定义为ΔQ_AQ_BQ_C(即上述的ΔABC)，将精匹配给出的最终匹配三角形定义为ΔP_AP_BP_C(即第五步⑶中ΔP_1,A'P_2,B'P_3,C')，而标准三角形ΔQ_AQ_BQ_C通过旋转和平移后对应的三角形定义为ΔQ_A'Q_B'Q_C'。设三维矩阵R为ΔQ_AQ_BQ_C的旋转矩阵(绕ΔQ_AQ_BQ_C的重心旋转，逆时针旋转为正方向)，同时定义d＝[Δλ ΔL Δh]^T为ΔQ_AQ_BQ_C旋转后到ΔQ_A'Q_B'Q_C'的平移量，其中Δλ表示惯导系统的经度误差，ΔL表示惯导系统的纬度误差，Δh表示惯导系统的高度误差。

最佳匹配的准则是：两个三角形对应点的距离差的平方和最小，即标准三角形ΔQ_AQ_BQ_C经旋转平移后的三角形ΔQ_A'Q_B'Q_C'与ΔP_AP_BP_C对应点的距离差的平方和达到最小值。其中，定义ΔP_AP_BP_C各点的坐标向量为p_i，ΔQ_AQ_BQ_C各点的坐标向量为q_i，ΔQ_A'Q_B'Q_C'各点的坐标向量为q_i'。则价值函数T(d,R)为：

$T(d,R)=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\left|\right|p_i-{q_i}^{\text{'}}\left|\right|}^2$

同时，以点q₀为旋转中心的旋转平移公式可以为：

q_i'＝d+q₀+R(q_i-q₀)

于是，最终的价值函数为：

$T(d,R)=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\left|\right|p_i-d-q_0-R(q_i-q_0)\left|\right|}^2$

⑵求解平移向量d。

为了让目标函数取最小值，平移向量d应满足：

$\frac{\&PartialD;T(d,R)}{\&PartialD;d}=0$

即

$\frac{\&PartialD;T(d,R)}{\&PartialD;d}=-2\left[\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}(p_i-d-q_0-R(q_i-q_0))\right]=0$

所以求解平移向量d得

$d=\frac{1}{3}\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}(p_i-q_0-R(q_i-q_0))$

定义ΔP_AP_BP_C的重心坐标向量为ΔQ_AQ_BQ_C的重心坐标向量为则上式可以简化为：

d＝p₀-q₀

⑶求解旋转矩阵R。

定义 ${\tilde{p}}_i=p_i-p_0,{\tilde{q}}_i=q_i-q_0$ 则

$\begin{array}{c}T(d,R)=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{\left|\right|p_i-d{-q}_0-R\left(q_i{-q}_0\right)\left|\right|}^2\\ =\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}({\left|\right|{\tilde{p}}_i\left|\right|}^2+{\left|\right|{\tilde{q}}_i\left|\right|}^2-2{\tilde{p}}_i^{T}R{\tilde{q}}_i)\end{array}$

同时分别定义 $\mathrm{MC}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}({\left|\right|{\tilde{p}}_i\left|\right|}^2+{\left|\right|{\tilde{q}}_i\left|\right|}^2),\mathrm{MV}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}\left({\tilde{p}}_i^{T}R{\tilde{q}}_i\right),S=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}\left({\tilde{p}}_i{\tilde{q}}_i^T\right),$ 由于MV为常量，要使目标函数取最小值，即求取MV的最大值。根据矩阵分析知识有：

$\begin{array}{c}\mathrm{MV}=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}\left[{\tilde{p}}_i^{T}R{\tilde{q}}_i\right]=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}\left[\mathrm{Trace}\left({\tilde{p}}_i{\tilde{q}}_i^T{R}^T\right)\right]\\ =\mathrm{Trace}\left[\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}\left({\tilde{p}}_i{\tilde{q}}_i^T\right)R^T\right]=\mathrm{Trace}\left({\mathrm{SR}}^T\right)\end{array}$

将S奇异值分解为S＝UWV^T，其中U,V都是正交矩阵,则有

Trace(SR^T)＝Trace(UWV^TR^T)＝Trace(UWV^TR^TUU^T)＝Trace(WV^TR^TU)

令Z＝V^TR^TU，由于U,V,R都是正交矩阵，所以矩阵Z也是正交矩阵则

Trace(SR^T)＝Trace(WV^TR^TU)＝Trace(WZ)≤Trace(W)

当且仅当Z＝V^TR^TU＝I时，Trace(SR^T)取得最大值，即MV取得最大值，目标函数T(d,R)取得最小值。于是可求得旋转矩阵R的计算公式为：

R＝UV^T

这样就解算出了从标准三角形ΔQ_AQ_BQ_C到匹配三角形ΔP_AP_BP_C对应的平移向量d和旋转矩阵R。而平移向量d和旋转矩阵R与惯导系统长时间的位置误差和航向角误差Δθ存在如下关系式：

$d=\left[\begin{array}{c}\mathrm{\&Delta;\&lambda;}\\ \mathrm{\&Delta;L}\\ \mathrm{\&Delta;h}\end{array}\right],R=\left[\begin{array}{ccc}\cos \mathrm{\&Delta;\&theta;}& \sin \mathrm{\&Delta;\&theta;}& 0\\ -\sin \mathrm{\&Delta;\&theta;}& \cos \mathrm{\&Delta;\&theta;}& 0\\ 0& 0& 1\end{array}\right]$

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (2)

1.一种基于三角形的重力图快速匹配方法，其特征步骤如下：

第一步，根据导航信息得出标准三角形；

从惯导系统的导航解算结果中提取经度、纬度作为位置点的信息，进而连续获取载体的三个位置点信息，以其两两间的航行距离作为标准三角形的三条边；

第二步，结合实时重力量测值，在重力图中进行大范围搜索，获取粗匹配点集；

在已知重力图中建立块阵列进行大范围搜索，块阵列和其中的每个块都是正方形，块阵列按每边上块的个数定义其级数，块阵列的最大级数根据惯导系统的估计位置误差确定；

以第一步中的三个位置点分别为中心在重力图中建立三个块阵列，然后结合第一步三个位置点对应的实时重力值，分别在三个块阵列中以块为最小单元搜索等值点，其搜索过程按照块阵列级数从小到大的顺序进行逐级搜索，由这些搜索的等值点构成粗匹配的三组点集；

第三步，通过标准三角形的约束，建立匹配三角形集，并对其进行筛选，其具体步骤为：

⑴以第一步标准三角形的三边距离为约束条件，将第二步粗匹配的三组点集按照排列组合的原则建立一组匹配三角形集，其约束条件中三边距离的误差限由第二步块阵列的单个块大小来确定；

⑵计算第三步⑴中各个匹配三角形和标准三角形对应三边的误差之和，并将所有匹配三角形按误差之和从小到大排序；

⑶筛选误差之和最小的匹配三角形用于后续的精匹配过程；

第四步，基于粗匹配筛选结果，在重力图中进行小范围搜索，获取精匹配点集；

以第三步⑶中筛选的匹配三角形的三个点分别为搜索中心，在重力图中建立单个块进行小范围搜索，其块的大小与第二步的块大小相等，然后仍结合第一步三个位置点对应的实时重力值，分别在三个块中搜索等值点，由这些等值点直接构成精匹配的三组点集；

第五步，通过标准三角形的约束，建立新的匹配三角形集，并进行筛选，其具体步骤为：

⑴以第一步标准三角形的三边距离为约束条件，利用第四步精匹配的三组点集建立一组新的匹配三角形集，其约束条件中三边距离的误差限由重力测量误差和重力图网格精度来确定；

⑵计算第五步⑴中各个新的匹配三角形和标准三角形对应三边的误差之和，并将所有新的匹配三角形按误差之和从小到大排序；

⑶筛选误差之和最小的匹配三角形用于后续的匹配解算；

第六步，利用第五步(3)中精匹配筛选的匹配三角形进行匹配解算；

根据两个点集序列间误差平方和最小的原理建立匹配解算的价值函数，将第一步中标准三角形对应的点序列与第五步精匹配过程筛选的匹配三角形点序列进行匹配解算，可求得两个三角形间的平移向量和旋转矩阵，该平移向量和旋转矩阵中包含惯导系统长时间累积的位置误差和航向角误差信息。

2.根据权利要求1所述的一种基于三角形的重力图快速匹配方法，其特征在于：所述第二步采用块阵列逐级搜索的方法获取粗匹配的点集，其步骤为：

⑴根据惯导系统的误差特性估计出惯导系统解算位置误差，进而确定出块阵列的最大级数，以第一步中惯导系统给出的位置点为中心，在重力图中建立块阵列；

⑵以块阵列的级数从小到大的顺序进行逐级搜索，如果在某一级阵列中搜索到实时重力值的等值点，则停止下一级的搜索；

⑶分别求取单个块中所有等值点的平均值作为该块的代表点，没有等值点的块没有代表点，由所有块的代表点组成粗匹配的点集。