CN114061591A - 一种基于滑动窗数据回溯的等值线匹配方法 - Google Patents

Abstract

本发明公开一种基于滑动窗数据回溯的等值线匹配方法，属于地球物理场辅助惯性导航领域。对于基于地形/地磁/重力的地球物理场辅助惯性导航系统，当运载体进入到物理场适配区后，传统的迭代最近等值线方法受匹配点数量限制而容易陷入局部最优解，为了获得全局最优航行轨迹，本发明根据实时多任务操作系统的计算性能和辅助导航精度要求确定滑动窗口参数，利用历史物理场值匹配数据构建滑动窗数据回溯框架，采用矩阵的特征值特征向量分解方法获得指示航迹点集至最邻近参考点集的旋转变换矩阵，通过移动滑动窗并进行正向和逆向循环匹配以达到全局航迹约束，可提高匹配精度和鲁棒性。

Description

一种基于滑动窗数据回溯的等值线匹配方法

技术领域

本发明涉及一种基于滑动窗数据回溯的等值线匹配方法，属于地球物理场辅助惯性导航领域。

背景技术

惯性导航系统是一种自主式导航系统，不依靠其他任何外部信息，也不向外辐射能量，其优点是隐蔽性强、短期定位精度高和数据更新率高，缺点是导航定位误差随时间累积，这就使得水下运载体无法实现长时间的深远海航行，为控制定位误差范围，运载体通常需要定期上浮接收卫星导航信号并校正导航误差，这就导致运载体的隐蔽性下降和能耗大大增大。近十几年来，由于地球物理特征技术(包括测量传感器、制图精度等)的发展，使得利用地球物理特征进行辅助导航成为可能，地球物理场(地形/地磁/重力)辅助导航系统与惯性导航系统相结合可以弥补惯导系统误差随时间积累的不足，具有精度高、隐蔽性好、节约能耗等优点，因此受到了广大学者的关注青睐。地球物理场辅助导航系统主要由地球物理场参考图、地球物理场传感器和导航匹配算法三部分组成，在运载体进入到地球物理场适配区域后，首先通过测量的地球物理量与存储在计算机中的参考图进行匹配，进而获得运载体的绝对定位信息，最后再与惯性导航系统输出的导航参数进行信息融合并修正惯导系统的定位误差。

目前地球物理场辅助导航方法主要有地形轮廓匹配算法(Terrain ContourMatching,TERCOM)、迭代最近等值线算法(Iterated Closest Contour Point,ICCP)和桑迪亚惯性地形辅助导航算法(Sandia Inertia Terrain-aided Navigation,SITAN)。TERCOM算法是将运载体测量的一系列地球物理量数据累积起来处理，根据物理量测量序列和参考数据库序列的相关度，确定运载体的位置，累积的物理量测量值越多，匹配定位越准确，但该方法对航向误差比较敏感且不适用于大机动航行。SITAN算法利用Kalman滤波技术对实时采集的测量数据进行递归处理，从而得到惯导系统误差的最优估计，但运载体位置误差与地球物理场特征值变化量成线性关系的假设只有当真实位置距惯导指示位置不远处时才成立，在惯导位置误差较大的情况下会引起误匹配，最终导致匹配失败。ICCP算法是使用刚性变换不断接近等值线上的最近点并进行配准的迭代算法，是以ICP(IterativeClose Point)算法为基础来开发的，最初此算法在图像配准领域广泛应用，而在1999年Behzed将ICP中的迭代最近点算法提取出来，再用于水下航行器的导航算法中，提出了迭代最近等值线算法。ICCP算法实现的基础思想是认为运载体的真实位置是物理测量值所对应的等值线上距惯导指示位置最近的点，因此该算法的应用受到惯导指示位置与运载体真实位置之间的误差必须足够小的假设条件的约束，但该约束可先通过TERCOM算法进行粗略搜索后得到满足。实际应用中，ICCP算法工作的前提是物理场传感器的测量值要足够精确，这样在等值线上搜索最近点才有意义，而ICCP算法每次利用的是局部航迹下的实测物理场值进行匹配，由于每次匹配的数据点数量有限，导致匹配的航迹结果是局部最优解，并未得到真实航迹的最优解。因此，为了获得全局最优航行轨迹，本发明利用历史匹配数据构建滑动窗数据回溯框架以提高等值线匹配的鲁棒性和精度性能。

发明内容

本发明的目的：为了克服现有技术的不足，有效解决ICCP算法受匹配点数量限制而易陷入局部最优解的问题，提供一种基于滑动窗数据回溯的等值线匹配方法，对历史时刻所有滑动窗内的指示航迹点与其对应等值线上的最近点进行匹配迭代，找到全局的最优轨迹，并采用矩阵的特征值特征向量分解方法获得指示航迹点集至最邻近参考点集的旋转变换矩阵。

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

一种基于滑动窗数据回溯的等值线匹配方法，该方法包括如下步骤：

(1)当运载体进入地球物理场适配区后，通过运载体携带的物理场传感器测量并存储航迹点对应的实测物理场值，同时存储惯性导航系统输出的指示航迹点的经纬度坐标，根据惯性导航系统提供的经纬度坐标信息提取所在区域的物理场参考数据库；

(2)提取步骤(1)得到的物理场参考数据库中起点经纬度坐标(λ₀,L₀)，将惯性导航系统输出的指示航迹点经纬度坐标(λ,L)转换成以起点经纬度坐标(λ₀,L₀)为原点的相对位置坐标(p_E,p_N)，即：

式中，λ₀和L₀分别为物理场参考数据库中起点经度和纬度坐标，单位为°；λ和L分别为惯性导航系统输出的指示航迹点经度和纬度坐标，单位为°；p_E和p_N分别为惯性导航系统输出的指示航迹点相对东向坐标和相对北向坐标，单位为m；

(3)根据实时多任务操作系统的计算性能和辅助导航精度要求确定滑动窗口长度N、滑动窗口移动步长L、滑动窗正向和逆向循环匹配次数IC；

其中，N表示滑动窗口长度，L表示滑动窗口移动步长，IC表示滑动窗正向和逆向循环匹配次数，N、L、IC均为正整数，且满足N≥L；

(4)根据步骤(1)测量得到的航迹点对应的实测物理场值h_i和步骤(2)得到的指示航迹点相对位置坐标提取步骤(3)确定的滑动窗口内惯导指示航迹点p_i附近的等值线c_i并搜索该滑动窗口内指示航迹点到附近等值线上的对应最邻近参考点y_i；

其中，i表示滑动窗内第i个指示航迹点(i＝1,2,…,N)，h_i表示第i个航迹点所对应的实测物理场值，p_i表示第i个指示航迹点的相对位置坐标，c_i表示在物理场参考数据库中提取对应于实测物理场值h_i的第i个指示航迹点附近的等值线，y_i表示第i个指示航迹点到等值线c_i上的最邻近参考点的二维位置坐标；

(5)根据步骤(4)搜索得到的最邻近参考点与滑动窗口内的指示航迹点构造一个关联矩阵M并计算2阶对称正定矩阵S＝M^TM的特征值和单位特征向量，进而确定旋转矩阵R和平移向量t；

首先对滑动窗内的指示航迹点p_i(i＝1,2,…,N)和最邻近参考点y_i(i＝1,2,…,N)作一个新变换，即令：

式中，

为滑动窗内的指示航迹点集质心坐标，

为最邻近参考点集质心坐标，

p′_i(i＝1,2,…,N)和y′_i(i＝1,2,…,N)分别为滑动窗口内指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标；

接着利用相对点集质心的位置坐标p′_i和y′_i构造一个关联矩阵M为：

式中，M是由相对点集质心坐标变换后的指示航迹点p′_i(i＝1,2,…,N)和最邻近参考点y′_i(i＝1,2,…,N)构成的2阶方阵；矩阵右上角标T表示矩阵的转置；

然后利用M构造一个2阶对称正定矩阵S＝M^TM并求解2阶对称正定矩阵S的正特征值和对应正特征值的单位特征向量，使其满足下式：

式中，λ₁和λ₂为2阶对称正定矩阵S的特征值，u₁为S的对应于特征值λ₁的单位特征向量，u₂为S的对应于特征值λ₂的单位特征向量；

最后根据关联矩阵M和2阶对称正定矩阵S的特征值、特征向量，得到指示航迹点集至最邻近参考点集的旋转矩阵R为：

根据滑动窗内的指示航迹点集质心坐标

最邻近参考点集质心坐标

和旋转矩阵R求得平移向量t为：

(6)利用步骤(5)得到的旋转矩阵R和平移向量t更新滑动窗口内指示航迹点的位置，判断滑动窗口内迭代次数是否超限或满足收敛条件，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(4)，否则进行下一步骤；

滑动窗口内最邻近参考点集合Y与指示航迹点集合P之间的欧几里得距离平方和为：

式中，Y＝{y_i,i＝1,2,…,N}表示滑动窗内最邻近参考点集合，P＝{p_i,i＝1,2,…,N}表示滑动窗内指示航迹点集合，d_k表示第k(k＝1,2,…)次变换后集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；

收敛条件需同时满足：

(1)根据匹配精度要求给定一个常数ε，使得

(2)根据匹配精度要求给定一个常数δ，使得d_k≤δ；

其中，d_k-1表示第k-1(k＝2,3,…)次变换后集合Y和集合P的欧几里得距离平方和；

(7)判断是否达到滑动窗正向和逆向循环匹配次数，若未达到，则根据步骤(3)中的滑动窗口移动步长对滑动窗进行移动并转到步骤(4)；若达到，则完成全局最优航迹点匹配。

有益效果：

本发明与现有技术相比，克服了现有ICCP算法在原理上的缺陷，在现有ICCP算法基础上，构建适当窗口长度的滑动窗数据回溯等值线匹配方法，对历史时刻所有滑动窗内的指示航迹点与其对应等值线上的最近点进行匹配迭代，找到全局的最优轨迹，提供的利用矩阵特征值特征向量分解法计算旋转变换矩阵的方式简单易懂，有效解决了ICCP算法受匹配点数量限制而易陷入局部最优解的问题，提高了匹配定位的精度和鲁棒性。

附图说明

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

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

图3为对历史时刻数据进行滑动窗数据回溯的示意图；

图4为本发明实施例中所述方法的水下地形匹配仿真结果图。

具体实施方式

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

实施例1：

如图1所示，地球物理场辅助惯性导航系统主要由地球物理场传感器、惯性导航系统、地球物理场参考图和匹配算法四个部分组成。地球物理场传感器的作用是实时测量运载体航迹点所对应的地球物理场数据，经过数据预处理和误差补偿后可作为待匹配的实际物理场测量值；惯性导航系统作为主要导航系统可实时提供载体的导航参数信息，数据输出更新率高；地球物理场参考图是根据惯导指示位置从计算机中提取所在区域的数字地图，其呈现形式是储存在二维数组中的地球物理场参考数据库或数字高程模型；匹配算法的作用是将测量的物理场序列值与参考数据库进行匹配，最终得到运载体的最优位置信息并将其用于惯导系统误差修正。

本发明主要是针对匹配算法，为了克服现有技术的不足，有效解决ICCP算法受匹配点数量限制而易陷入局部最优解的问题，提供一种基于滑动窗数据回溯的等值线匹配方法，对历史时刻所有滑动窗内的等值线上的最近点进行匹配迭代，找到全局的最优轨迹，并采用矩阵的特征值特征向量分解方法获得旋转变换矩阵，如图2所示，包括如下具体步骤：

(1)以水下运载体中地形辅助导航为例，当运载体进入地形适配区后，通过运载体携带的深度计和压力计测量并存储航迹点对应的水深值，根据惯性导航系统提供的位置提取所在区域的水深参考数据库；

(2)提取步骤(1)得到的水深参考数据库中起点经纬度坐标，将惯性导航系统输出的指示航迹点经纬度坐标转换成以起点经纬度坐标为原点的相对位置坐标；

(3)根据实时多任务操作系统的计算性能和辅助导航精度要求确定滑动窗口长度为15个水深值采样点、滑动窗口移动步长为5个水深值采样点、滑动窗正向和逆向循环匹配次数为4次；

(4)根据滑动窗内的实测水深值h_i，在水深参考数据库中提取滑动窗内惯导指示航迹点p_i附近的水深等值线c_i，并在水深等值线c_i上搜索到p_i的最邻近参考点y_i；

其中，i表示滑动窗内第i个指示航迹点(i＝1,2,…,15)，h_i表示第i个航迹点所对应的实测物理场值，p_i表示第i个指示航迹点的相对位置坐标，c_i表示在物理场参考数据库中提取对应于实测物理场值h_i的第i个指示航迹点附近的等值线，y_i表示第i个指示航迹点到等值线c_i上的最邻近参考点的二维位置坐标；

(5)根据步骤(4)搜索得到的最邻近参考点与滑动窗口内的指示航迹点构造一个关联矩阵M并计算2阶对称正定矩阵S＝M^TM的特征值和单位特征向量，进而确定指示航迹点集至最邻近参考点集的旋转矩阵R和平移向量t；

首先对滑动窗内的指示航迹点p_i(i＝1,2,…,15)和最邻近参考点y_i(i＝1,2,…,15)作一个新变换，即令：

式中，

为滑动窗内的指示航迹点集质心坐标，

为最邻近参考点集质心坐标，

p′_i(i＝1,2,…,15)和y′_i(i＝1,2,…,15)分别为滑动窗口内指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标；

接着利用相对点集质心的位置坐标p′_i和y′_i构造一个关联矩阵M为：

式中，M是由相对点集质心坐标变换后的指示航迹点p′_i(i＝1,2,…,15)和最邻近参考点y′_i(i＝1,2,…,15)构成的2阶方阵；矩阵右上角标T表示矩阵的转置；

然后利用M构造一个2阶对称正定矩阵S＝M^TM并求解2阶对称正定矩阵S的正特征值和对应正特征值的单位特征向量，使其满足下式：

式中，λ₁和λ₂为2阶对称正定矩阵S的特征值，u₁为S的对应于特征值λ₁的单位特征向量，u₂为S的对应于特征值λ₂的单位特征向量；

最后根据关联矩阵M和2阶对称正定矩阵S的特征值、特征向量，得到旋转矩阵R为：

根据滑动窗内的指示航迹点集质心坐标

最邻近参考点集质心坐标

和旋转矩阵R求得平移向量t为：

(6)利用步骤(5)得到的旋转矩阵R和平移向量t更新滑动窗口内指示航迹点的位置，判断滑动窗口内8次迭代次数是否超限或满足收敛条件，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(4)，否则进行下一步骤；

滑动窗口内最邻近参考点集合Y与指示航迹点集合P之间的欧几里得距离平方和为：

式中，Y＝{y_i,i＝1,2,…,15}表示滑动窗内最邻近参考点集合，P＝{p_i,i＝1,2,…,15}表示滑动窗内指示航迹点集合，d_k表示第k(k＝1,2,…)次变换后集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；

收敛条件需同时满足：

(1)根据匹配精度要求给定一个常数ε，使得

(2)根据匹配精度要求给定一个常数δ，使得d_k≤δ；

其中，d_k-1表示第k-1(k＝2,3,…)次变换后集合Y和集合P的欧几里得距离平方和；

(7)判断是否达到滑动窗正向和逆向循环匹配次数，若未达到，则根据步骤(3)中的滑动窗口移动步长对滑动窗进行移动并转到步骤(4)；若达到，则完成全局最优航迹点匹配。

仿真实验：

以水下运载体中地形辅助导航为例，在一个分辨率为10米的水深地图参考数据库中仿真一段运载体航行轨迹，设置惯导系统初始东向位置误差和北向位置误差分别为180m和100m，初始航向误差为3°，陀螺零漂为0.01°/h，加速度计零偏为0.001mg，陀螺和加速度计的采样频率为200Hz，压力计和深度计的采样频率为1Hz，运载体航行速度为10m/s，初始航向角为北偏东30°，仿真时间为20s，对应的水深采样点共有20个，仿真结果如图4所示。

由图4可以看出，惯导系统指示航迹与运载体真实的理论航迹相差较大；相比于惯导指示航迹，经过传统ICCP算法匹配后的航迹与理论航迹更接近；而基于本发明提供的一种基于滑动窗数据回溯的等值线匹配方法的匹配航迹效果明显优于传统ICCP算法，数据回溯匹配航迹误差比传统ICCP算法的匹配误差更小，更接近理论航迹。

为了进一步验证本发明的实用价值，下面比较惯导指示航迹、传统ICCP匹配航迹、本发明提供的数据回溯匹配航迹三种航迹相对于理论航迹的均方根误差。

表1三种航迹的均方根误差对比

由表1可以看出，本发明提供的一种基于滑动窗数据回溯的等值线匹配方法的匹配航迹下的均方根定位误差最小，为12.79m，其精度接近于仿真所用的水深地图参考数据库的10米分辨率，若将匹配结果用于校正惯导系统误差，可满足水下运载体的导航定位需求。

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

Claims (10)

1.一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，该方法包括如下步骤：

(1)当运载体进入地球物理场适配区后，通过运载体携带的物理场传感器测量并存储航迹点对应的实测物理场值，同时存储惯性导航系统输出的指示航迹点的经纬度坐标，根据惯性导航系统提供的经纬度坐标信息提取所在区域的物理场参考数据库；

(2)提取步骤(1)得到的物理场参考数据库中起点经纬度坐标，将惯性导航系统输出的指示航迹点经纬度坐标转换成以起点经纬度坐标为原点的相对位置坐标；

(3)根据实时多任务操作系统的计算性能和辅助导航精度要求确定滑动窗口参数；

(4)根据步骤(1)得到的航迹点对应的实测物理场值和步骤(2)得到的指示航迹点相对位置坐标提取步骤(3)确定的滑动窗口内惯导指示航迹点附近的等值线并搜索该滑动窗口内指示航迹点到附近等值线上的对应最邻近参考点；

(5)根据步骤(4)搜索得到的最邻近参考点与滑动窗口内的指示航迹点构造一个关联矩阵M并计算2阶对称正定矩阵S＝M^TM的特征值和单位特征向量，进而确定指示航迹点集至最邻近参考点集的旋转矩阵R和平移向量t；

(6)利用步骤(5)得到的旋转矩阵R和平移向量t更新滑动窗口内指示航迹点的位置，判断滑动窗口内迭代次数是否超限或满足收敛条件，如果没有达到迭代次数或没有满足收敛条件，则转到步骤(4)，否则进行下一步骤；

(7)判断是否达到滑动窗正向和逆向循环匹配次数，若未达到，则根据步骤(3)中的滑动窗口移动步长对滑动窗进行移动并转到步骤(4)；若达到，则完成全局最优航迹点匹配。

2.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(2)的具体方法是：

提取步骤(1)得到的物理场参考数据库中起点经纬度坐标(λ₀,L₀)，将惯性导航系统输出的指示航迹点经纬度坐标(λ,L)转换成以起点经纬度坐标(λ₀,L₀)为原点的相对位置坐标(p_E,p_N)，即：

式中，λ₀和L₀分别为物理场参考数据库中起点经度和纬度坐标，单位为°；λ和L分别为惯性导航系统输出的指示航迹点经度和纬度坐标，单位为°；p_E和p_N分别为惯性导航系统输出的指示航迹点相对东向坐标和相对北向坐标，单位为m。

3.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(3)所述的滑动窗口参数包括滑动窗口长度N、滑动窗口移动步长L、滑动窗正向和逆向循环匹配次数IC；其中，N表示滑动窗口长度，L表示滑动窗口移动步长，IC表示滑动窗正向和逆向循环匹配次数，N、L、IC均为正整数，且满足N≥L。

4.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(4)的具体方法是：

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

其中，i表示滑动窗内第i个指示航迹点(i＝1,2,…,N)，h_i表示第i个航迹点所对应的实测物理场值，p_i表示第i个指示航迹点的相对位置坐标，c_i表示在物理场参考数据库中提取对应于实测物理场值h_i的第i个指示航迹点附近的等值线，y_i表示第i个指示航迹点到等值线c_i上的最邻近参考点的二维位置坐标。

5.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(5)中所述构造一个关联矩阵M的具体方法是：

首先对滑动窗内的指示航迹点p_i(i＝1,2,…,N)和最邻近参考点y_i(i＝1,2,…,N)作一个新变换，即令：

式中，

为滑动窗内的指示航迹点集质心坐标，

为最邻近参考点集质心坐标，

和y′_i(i＝1,2,…,N)分别为滑动窗口内指示航迹点和最邻近参考点相对它们各自点集质心的位置坐标；

然后利用相对点集质心的位置坐标p′_i和y′_i构造一个关联矩阵M为：

式中，M是由相对点集质心坐标变换后的指示航迹点p′_i(i＝1,2,…,N)和最邻近参考点y′_i(i＝1,2,…,N)构成的2阶方阵；矩阵右上角标T表示矩阵的转置。

6.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(5)中所述计算2阶对称正定矩阵S＝M^TM的特征值和单位特征向量的具体方法是：

先利用M构造一个2阶对称正定矩阵S，有S＝M^TM；

再求解2阶对称正定矩阵S的正特征值和对应正特征值的单位特征向量，使其满足下式：

式中，λ₁和λ₂为2阶对称正定矩阵S的特征值，u₁为S的对应于特征值λ₁的单位特征向量，u₂为S的对应于特征值λ₂的单位特征向量。

7.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(5)中所述确定指示航迹点集至最邻近参考点集的旋转矩阵R的具体方法是：

根据关联矩阵M和2阶对称正定矩阵S的特征值、特征向量，得到旋转矩阵R为：

8.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(5)中所述确定平移向量t的具体方法是：

根据滑动窗内的指示航迹点集质心坐标

最邻近参考点集质心坐标

和旋转矩阵R求得平移向量t为：

9.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(6)中所述判断是否满足收敛条件的具体方法是：

首先利用步骤(5)得到的旋转矩阵R和平移向量t更新滑动窗口内指示航迹点的位置，并计算滑动窗口内最邻近参考点集合Y与指示航迹点集合P之间的欧几里得距离平方和，公式如下：

式中，Y＝{y_i,i＝1,2,…,N}表示滑动窗内最邻近参考点集合，P＝{p_i,i＝1,2,…,N}表示滑动窗内指示航迹点集合，d_k表示第k(k＝1,2,…)次变换后集合Y和集合P的欧几里得距离平方和，||X||表示向量X的2范数；

收敛条件需同时满足：

(1)根据匹配精度要求给定一个常数ε，使得

(2)根据匹配精度要求给定一个常数δ，使得d_k≤δ；

其中，d_k-1表示第k-1(k＝2,3,…)次变换后集合Y和集合P的欧几里得距离平方和。

10.根据权利要求1所述的一种基于滑动窗数据回溯的等值线匹配方法，其特征在于，步骤(6)所述的滑动窗口内迭代次数为8次。

Images