CN114111798A - 一种基于仿射因子补偿的改进iccp方法 - Google Patents

Abstract

本发明公开一种基于仿射因子补偿的改进ICCP方法，属于地球物理场辅助惯性导航领域。ICCP(Iterative Closest Contour Point)算法是目前地形匹配导航技术最为重要的匹配算法之一，但其对惯导航迹所作的修正仅仅是旋转和平移，没有对航迹进行缩放，因而不能修正辅助定位过程中惯导指示航迹相对于实际航迹发生的形变所造成的误差。已有的引入仿射修正以提高ICCP精度的算法，大多利用模拟退火、最小二乘等方法，计算量大且仿射因子精度有限，本发明利用单位四元数表示ICCP中的旋转，求得仿射因子的解析解，提高了ICCP算法精度。

Description

一种基于仿射因子补偿的改进ICCP方法

技术领域

本发明涉及一种基于仿射因子补偿的改进ICCP方法，属于地球物理场辅助惯性导航领域。

背景技术

惯性导航系统是一种自主式导航系统，不依靠其他任何外部信息，仅借助于运载体装配的陀螺仪、加速度计以及运载体的初始速度、位置、姿态信息实现导航功能，其优点是隐蔽性强、短期定位精度高和数据更新率高，不向外辐射能量，缺点是导航定位误差随时间累积不断增大，无法长时间保证高精度导航效果。水下航行器因在未来的军用和民用领域具有重要战略意义，其高度隐蔽性与保持长时间精确航行的需求就显得十分重要，而实现这一要求的关键技术之一就是其导航技术。随着地球物理场辅助惯性导航技术的不断发展，应用地磁、重力、地形等信息来辅助惯性导航系统，为水下航行器导航技术研究提供了一种新思路。由于地形高程信息测量较为方便，且陆地上的地形匹配技术已应用于实际，可以为水下地形匹配导航技术提供借鉴，因而应用地形高程信息来辅助水下航行器的导航技术具有高度可行性。

目前地球物理场辅助导航方法主要有地形轮廓匹配算法(Terrain ContourMatching,TERCOM)、桑迪亚惯性地形辅助导航算法(Sandia Inertia Terrain-aidedNavigation,SITAN)和迭代最近等值线算法(Iterated Closest Contour Point,ICCP)。TERCOM系统又被称为地形廓轮匹配系统，系美国公司于上世纪七十年代首先开始研究，其核心匹配算法为断续的批相关处理算法，测得一串地形高程序列后，通过无遗漏地搜索位置不确定区域内的每个网格位置的办法，在已有的地形图中找出一条路径，这条路径平行于导航系统指示的路径并且最接近于传感器的实测路径，因此TERCOM算法在地形特征独特或搜索范围足够大的情况下效果较好，但是实时性差，且对于航向误差较为敏感。SITAN系统则采用了递推的Kalman滤波技术，由惯导系统、测深传感器、数字地图和数据处理装置四部分组成，不同于相关分析法，SITAN算法把深度值直接作为系统的测量对象，Kalman滤波器在获得每个单独的深度测量信息的同时便对其进行处理，这样的递推过程赋予了SITAN算法连续的修正能力，不仅可以修正导航系统的位置误差，而且可以对速度及姿态误差进行估计于修正，但是该算法对初始位置精度要求高，且要求对地形做线性化估计，当地形斜率的正负号变化时则会导致滤波发散。

ICCP算法脱胎于ICP(Iterated Corresponding Point)算法，最早由Besl和McKay在图像对准领域使用，随后被Behzad K.P.首次应用于重力匹配辅助导航，Bishop通过大量仿真实验系统地分析了该算法的可行性及误差影响，刘承香进一步详细分析了ICCP算法在地形匹配辅助导航领域的适用性。考虑到匹配过程中因为速度误差会导致惯导指示航迹与真实航迹的形状存在不同，罗诗图等采用最小失配点准测估计仿射变换参数，此外还有模拟退火、最小二乘等最优化算法可以使用，但都需要较长的计算时间且对于参数的估计精度有限。因此，为了在不大量增加计算量的基础上尽可能得到精度高的仿射因子，本发明进行严格的数学推导，得出仿射因子的解析解，很好地改进了ICCP算法的匹配效果。

发明内容

本发明的目的：ICCP算法对惯导航迹所作的修正仅仅是旋转和平移，没有对航迹进行缩放，因而不能修正辅助定位过程中惯导指示航迹相对于实际航迹发生的形变所造成的误差。已有的引入仿射修正以提高ICCP精度的算法，大多利用模拟退火、最小二乘等方法，计算量大且仿射因子精度有限，本发明提出一种基于仿射因子补偿的改进ICCP方法，利用单位四元数表示ICCP中的旋转，求得仿射因子的解析解，在对算法实时性影响很小的前提下提高了ICCP匹配精度。

上述的目的通过以下技术方案实现：

一种基于仿射因子补偿的改进ICCP方法，该方法包括如下步骤：

(1)当运载体进入地球物理场适配区后，通过运载体携带的物理场传感器测量并存储航行轨迹点对应的物理场值得到实测物理场值，根据运载体携带的惯性导航系统提供的位置信息提取所在区域的物理场参考数据库；

(2)通过所在区域的物理场参考数据库与惯性导航系统提供的位置信息得到惯导指示航迹点，根据实测物理场值提取惯导指示航迹点附近的等值线，并搜索惯导指示航迹点到附近等值线上的对应最邻近参考点；

根据实测物理场值h_i，在物理场参考数据库中提取惯导指示航迹点p_i附近的等值线c_i，并在等值线c_i上搜索到p_i的最邻近参考点y_i。其中，i＝1,2,…,N表示第i个惯导指示航迹点序号，N为惯导指示航迹点的数量，h_i表示第i个惯导指示航迹点所对应的实测物理场值，p_i表示第i个惯导指示航迹点所对应的二维位置坐标，c_i表示第i个惯导指示航迹点附近对应实测物理场值h_i的等值线，y_i表示第i个惯导指示航迹点到等值线c_i上的最邻近参考点的二维位置坐标；

(3)根据最邻近参考点与惯导指示航迹点确定旋转矩阵R、仿射因子s和平移向量t；

求解旋转矩阵R、仿射因子s和平移向量t，使得最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和最小，公式如下：

式中，Y＝{y_i,i＝1,2,…,N}表示最邻近参考点集合，P＝{p_i,i＝1,2,…,N}表示惯导指示航迹点集合，d表示集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；

利用矩阵的特征值特征向量分解方法得到旋转矩阵R：

首先对惯导指示航迹点p_i和最邻近参考点y_i作一个变换，即令：

式中，

和

分别为惯导指示航迹点和最邻近参考点的点集质心，Δp_i和Δy_i分别为惯导指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标，且

与

均为0；

然后根据相对二维位置坐标Δp_i和Δy_i构造一个2×2矩阵M，即：

式中，M是由坐标变换后的惯导指示航迹点Δp_i和最邻近参考点Δy_i构成的2阶方阵，右上角标T表示矩阵的转置；

接着求解对称正定矩阵M^TM的正特征值λ_j和对应λ_j的单位特征向量u_j(j＝1,2)，使其满足下式：

根据2阶矩阵M和关于M^TM的特征值、特征向量，最后得到旋转矩阵R为：

接着求解仿射因子s：

最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和d(Y,sRP+t)，也即集合Y与集合P之间误差e_i的平方和，可以改写为如下形式：

其中

由此可以得到

由于

与

均为0，那么

为0，易得Δt也为0时式(8)可达到最小值，此时

根据刚性变换不改变长度模值的特性：

||RΔp_i||²＝||Δp_i||² (11)

将式(10)改写为：

其中，Y与P分别为

和

代表各点到其点集质心向量模长之和，D为惯导指示航迹点坐标与相应惯导指示航迹点旋转后坐标的点乘之和；若要令式(12)最小，应令

为0，可得仿射因子s为：

最后根据式(13)，以及惯导指示航迹点集质心坐标

最邻近参考点集质心坐标

和旋转矩阵R、仿射因子s，可求得平移向量t为：

(4)利用步骤(3)得到的旋转矩阵R、仿射因子s和平移向量t更新惯导指示航迹点的位置，得到当前匹配航迹，判断迭代次数是否超限或满足局部最优条件，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(2)，否则进行下一步骤；

(5)对最后得到的匹配航迹进行精度评估，判断结果是否可靠，如果可靠则将结果用以校正惯导指示航迹点组成的航迹，否则认为匹配失败。

有益效果：

本发明与现有技术相比，在传统ICCP算法针对惯导指示航迹进行平移与旋转修正的基础上，分析了辅助定位过程中航迹形变造成的仿射误差，且不同于常见的模拟退火、最小二乘等近似求解仿射因子的手段，基于数学分析给出了仿射因子解析形式，不仅很好地保证了原有算法的实时性，而且提高了传统ICCP算法的匹配精度。

附图说明

图1为地球物理场辅助惯性导航系统基本原理框图；

图2为本发明的流程示意图；

图3为水下航行器航迹匹配图，图中四条曲线分别为理论航迹，INS待匹配航迹，ICCP匹配航迹，以及本发明提出的改进ICCP匹配航迹。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

实施例1：

如图1所示，地球物理场辅助惯性导航系统主要由地球物理场传感器、惯性导航系统、地球物理场参考数据库以及物理场数据匹配算法四个部分组成。地球物理场传感器被装配与运载体上，用以实时测量运载体每个航行轨迹点所对应的地球物理场数据，可以是重力、地磁、深度等；惯性导航系统则为整套系统的基石，根据自身的陀螺仪、加速度计信息与初始的速度、位置、姿态进行自主导航；地球物理场数据库包含现有的数据信息，根据惯导指示航迹的不同提取不同区域的数字地图，以供后续进行对准、匹配；物理场数据匹配算法作为整个系统的软件核心，主要功能是进行匹配解算，在对地球物理场传感器得到的数据进行相关处理后，计算出最佳定位估计值并为惯性导航系统提供所需的修正信息。

本发明主要是针对传统ICCP算法未考虑航迹缩放的不足，改进ICCP算法航迹修正与匹配的模型，提供一种基于仿射因子补偿的改进ICCP方法，在对于航迹进行平移与旋转修正的基础上，引入仿射因子，并严格推导得出仿射因子解析表达式，达到了更好的轨迹匹配效果，如图2所示，包括如下具体步骤：

(1)当运载体进入地球物理场适配区后，通过运载体携带的物理场传感器测量并存储航行轨迹点对应的物理场值得到实测物理场值，根据运载体携带的惯性导航系统提供的位置信息提取所在区域的物理场参考数据库；

(2)通过所在区域的物理场参考数据库与惯性导航系统提供的位置信息得到惯导指示航迹点，根据实测物理场值提取惯导指示航迹点附近的等值线，并搜索惯导指示航迹点到附近等值线上的对应最邻近参考点；

根据实测物理场值h_i，在物理场参考数据库中提取惯导指示航迹点p_i附近的等值线c_i，并在等值线c_i上搜索到p_i的最邻近参考点y_i。其中，i＝1,2,…,N表示第i个惯导指示航迹点序号，N为惯导指示航迹点的数量，h_i表示第i个惯导指示航迹点所对应的实测物理场值，p_i表示第i个惯导指示航迹点所对应的二维位置坐标，c_i表示第i个惯导指示航迹点附近对应实测物理场值h_i的等值线，y_i表示第i个惯导指示航迹点到等值线c_i上的最邻近参考点的二维位置坐标；

(3)根据最邻近参考点与惯导指示航迹点确定旋转矩阵R、仿射因子s和平移向量t；

求解旋转矩阵R、仿射因子s和平移向量t，使得最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和最小，公式如下：

式中，Y＝{y_i,i＝1,2,…,N}表示最邻近参考点集合，P＝{p_i,i＝1,2,…,N}表示惯导指示航迹点集合，d表示集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；

利用矩阵的特征值特征向量分解方法得到旋转矩阵R：

首先对惯导指示航迹点p_i和最邻近参考点y_i作一个变换，即令：

式中，

和

分别为惯导指示航迹点和最邻近参考点的点集质心，Δp_i和Δy_i分别为惯导指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标，且

与

均为0；

然后根据相对二维位置坐标Δp_i和Δy_i构造一个2×2矩阵M，即：

式中，M是由坐标变换后的惯导指示航迹点Δp_i和最邻近参考点Δy_i构成的2阶方阵，右上角标T表示矩阵的转置；

接着求解对称正定矩阵M^TM的正特征值λ_j和对应λ_j的单位特征向量u_j(j＝1,2)，使其满足下式：

根据2阶矩阵M和关于M^TM的特征值、特征向量，最后得到旋转矩阵R为：

接着求解仿射因子s：

最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和d(Y,sRP+t)，也即集合Y与集合P之间误差e_i的平方和，可以改写为如下形式：

其中

由此可以得到

由于

与

均为0，那么

为0，易得Δt也为0时式(8)可达到最小值，此时

根据刚性变换不改变长度模值的特性：

||RΔp_i||²＝||Δp_i||² (11)

可将式(10)改写为：

其中，Y与P分别为

和

代表各点到其点集质心向量模长之和，D为惯导指示航迹点坐标与相应惯导指示航迹点旋转后坐标的点乘之和；若要令式(12)最小，应令

为0，可得仿射因子s为：

最后根据式(13)，以及惯导指示航迹点集质心坐标

最邻近参考点集质心坐标

和旋转矩阵R、仿射因子s，可求得平移向量t为：

(4)利用步骤(3)得到的旋转矩阵R、仿射因子s和平移向量t更新惯导指示航迹点的位置，得到当前匹配航迹，判断迭代次数是否超限或满足局部最优条件，此处迭代允许的最大次数由匹配过程的实时性要求以及实际操作经验确定，一般可取10～20次，局部最优条件是当前匹配航迹相较于惯导指示航迹点组成的航迹的欧几里得距离平方和的变化率小于某特定值ε_sop，ε_sop的具体数值根据地形图网格精度、惯导器件误差、初始位置误差等实际情况确定，下述仿真中将ε_sop设为5％，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(2)，否则进行下一步骤；

(5)对最后得到的匹配航迹进行精度评估，判断结果是否可靠，如果最终的匹配航迹相较于上一次迭代得到的匹配航迹(若为第一次迭代，则与惯导指示航迹点组成的航迹比对)的欧几里得距离平方和小于某特定值ε_op则认为结果可靠，ε_op的具体数值根据地形图网格精度、惯导器件误差、初始位置误差等实际情况确定，下述仿真中将ε_op设为2m，最后得到的匹配航迹可用以校正惯导指示航迹点组成的航迹，否则认为匹配失败。

本发明的可行性通过仿真实验加以验证：

(1)实验选用某地区的航测地形数据，以此建立待匹配的基准地形图。实验中的惯性导航系统与地形图的相关仿真参数如表1。

表1仿真参数

(2)仿真结果可以由曲线图呈现，如图3所示。传统ICCP算法以及本发明提出的基于仿射因子补偿的改进ICCP算法的精度，可由对应航迹上的匹配点与理论航迹上的点之间的平均欧几里得距离衡量，易得在本次仿真实验中，传统ICCP算法的匹配位置误差为99.2691m，本发明提出的基于仿射因子补偿的改进ICCP算法的匹配位置误差为17.1535m，本发明提出的算法很好地利用仿射变换的解析形式修正了传统ICCP算法的误差，提高了匹配精度与鲁棒性，并且本发明对传统ICCP算法的复杂度增加较少，保证了改进算法的实时性。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (3)

1.一种基于仿射因子补偿的改进ICCP方法，其特征在于，该方法包括如下步骤：

(1)当运载体进入地球物理场适配区后，通过运载体携带的物理场传感器测量并存储航行轨迹点对应的物理场值得到实测物理场值，根据运载体携带的惯性导航系统提供的位置信息提取所在区域的物理场参考数据库；

(2)通过所在区域的物理场参考数据库与惯性导航系统提供的位置信息得到惯导指示航迹点，根据实测物理场值提取惯导指示航迹点附近的等值线，并搜索惯导指示航迹点到附近等值线上的对应最邻近参考点；

(3)根据最邻近参考点与惯导指示航迹点确定旋转矩阵R、仿射因子s和平移向量t；

(4)利用步骤(3)得到的旋转矩阵R、仿射因子s和平移向量t更新惯导指示航迹点的位置，得到当前匹配航迹，判断迭代次数是否超限或满足局部最优条件，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(2)，否则进行下一步骤；

(5)对最后得到的匹配航迹进行精度评估，判断结果是否可靠，如果可靠则将结果用以校正惯导指示航迹点组成的航迹，否则认为匹配失败。

2.根据权利要求1所述的一种基于仿射因子补偿的改进ICCP方法，其特征在于，步骤(2)的具体方法是：

根据实测物理场值h_i，在物理场参考数据库中提取惯导指示航迹点p_i附近的等值线c_i，并在等值线c_i上搜索到p_i的最邻近参考点y_i；

其中，i＝1,2,…,N表示第i个惯导指示航迹点序号，N为惯导指示航迹点的数量，h_i表示第i个惯导指示航迹点所对应的实测物理场值，p_i表示第i个惯导指示航迹点所对应的二维位置坐标，c_i表示第i个惯导指示航迹点附近对应实测物理场值h_i的等值线，y_i表示第i个惯导指示航迹点到等值线c_i上的最邻近参考点的二维位置坐标。

3.根据权利要求1所述的一种基于仿射因子补偿的改进ICCP方法，其特征在于，步骤(3)的具体方法是：

求解旋转矩阵R、仿射因子s和平移向量t，使得最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和最小，公式如下：

式中，Y＝{y_i,i＝1,2,…,N}表示最邻近参考点集合，P＝{p_i,i＝1,2,…,N}表示惯导指示航迹点集合，d表示集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；

利用矩阵的特征值特征向量分解方法得到旋转矩阵R：

首先对惯导指示航迹点p_i和最邻近参考点y_i作一个变换，即令：

式中，

和

分别为惯导指示航迹点和最邻近参考点的点集质心，Δp_i和Δy_i分别为惯导指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标，且

与

均为0；

然后根据相对二维位置坐标Δp_i和Δy_i构造一个2×2矩阵M，即：

式中，M是由坐标变换后的惯导指示航迹点Δp_i和最邻近参考点Δy_i构成的2阶方阵，右上角标T表示矩阵的转置；

接着求解对称正定矩阵M^TM的正特征值λ_j和对应λ_j的单位特征向量u_j(j＝1,2)，使其满足下式：

根据2阶矩阵M和关于M^TM的特征值、特征向量，最后得到旋转矩阵R为：

接着求解仿射因子s：

最邻近参考点集合Y与惯导指示航迹点集合P之间的欧几里得距离平方和d(Y,sRP+t)，也即集合Y与集合P之间误差e_i的平方和：

其中

由此得到

由于

与

均为0，那么

为0，易得Δt也为0时式(8)可达到最小值，此时

根据刚性变换不改变长度模值的特性：

||RΔp_i||²＝||Δp_i||² (11)

将式(10)改写为：

其中，Y与P分别为

和

代表各点到其点集质心向量模长之和，D为惯导指示航迹点坐标与相应惯导指示航迹点旋转后坐标的点乘之和；若要令式(12)最小，应令

为0，得仿射因子s为：

最后根据式(13)，以及惯导指示航迹点集质心坐标

最邻近参考点集质心坐标

和旋转矩阵R、仿射因子s，求得平移向量t为：

Images