【文献阅读】基于地磁矢量的改进迭代最近轮廓点匹配导航算法

论文信息

论文题目：Improved Iterative Closest Contour Point Matching Navigation Algorithm Based on Geomagnetic Vector
作者：Yuan Ren ¹², Lihui Wang¹²³, Kunjie Lin², Hongtao Ma⁴ and Mingzhu Ma²

Electronics 2022, 11(5), 796; 原文链接

Submission received: 26 January 2022 / Revised: 19 February 2022 / Accepted: 1 March 2022 / Published: 3 March 2022

(This article belongs to the Special Issue Recent Advances in Intelligent Transportation Systems)

Abstract

现存问题：

• 基于迭代最近轮廓点（ICCP）算法的地磁匹配辅助定位系统可以抑制惯性导航系统的累积误差，实现车辆的精确定位。然而ICCP算法对航向误差敏感并在地磁特征相似区域容易出现匹配错误，影响了定位的准确性和效率。

改进方案：

• 本文提出了一种基于地磁矢量的改进ICCP算法。该算法结合了蚁群算法以优化大概率范围内的搜索策略，并使用地磁三维矢量特征和Hausdorff距离作为目标函数进行多次迭代，从而提高匹配效率和精度。

改进效果：

• 仿真结果表明，与传统ICCP算法相比，改进后的算法将匹配轨迹的定位误差、航向误差和匹配时间分别减少了69.6%、44.0%和39.0%。

Introduction

根据文章的Introduction部分，当前地磁匹配算法存在以下主要问题：

1. 误差累积问题：惯性导航系统（INS）的测量误差会随时间累积，导致导航精度下降，因此需要地磁导航的辅助校正。然而，现有算法的精度在特定条件下（如地磁特征相似区域）容易受到影响。

2. 航向误差敏感：基于迭代最近轮廓点（ICCP）的匹配算法对航向误差较为敏感。在存在航向偏差的情况下，算法容易产生匹配误差，从而影响导航的准确性。

3. 匹配错误问题：在地磁特征相似的区域（如地磁强度变化不明显的区域），ICCP算法难以准确区分匹配点，导致低匹配精度或匹配错误。

4. 复杂度高：ICCP算法采用全局最优搜索来实现匹配，其复杂度较高，计算效率较低，尤其是在搜索大范围匹配点时，计算资源消耗较大。

这些问题限制了地磁匹配算法在复杂环境中的应用，因此需要改进搜索策略和匹配方法以提升其效率和准确性。

因此，本文提出了一种蚁群优化（ACO）算法，以改进高概率范围内的搜索区域和搜索过程，从而提高匹配效率。为了改善算法在总地磁强度特征变化不显著区域的匹配错误现象，结合地磁场的矢量特性，算法使用三维地磁强度作为改进匹配算法的目标函数，从而更全面地描述匹配过程中相似度，显著降低了匹配结果的定位误差和航向误差，有效提升了算法的匹配效率和精度。

Materials and Methods

Navigation Algorithm for Geomagnetic Matching

地磁匹配导航算法的基本框图如图1所示。预先规划的行驶区域地磁场匹配特征量被绘制成参考地图，并存储在计算机的数据库中。然后，当载体经过该区域时，载体上的地磁测量装置实时测量这些点的匹配特征量，从而获得实时地图。通过将测量数据与参考地图进行匹配，确定载体的实时位置，并补偿惯性导航系统的累积误差，从而实现高精度的自主导航。

这张图展示了地磁匹配导航算法的基本流程，具体步骤如下：

1. 地磁检测模块：使用磁力计进行数据测量，通过数据处理得到地磁测量数据，作为后续导航匹配的输入。

2. 地磁数据库模块：地磁模型提供基准信息，经过数据提取处理后，生成用于匹配的参考地图数据。

3. 地磁匹配算法：将地磁检测模块提供的实时测量数据与数据库中的基准信息进行匹配，以确定当前载体的位置。

4. 集成导航滤波器：将地磁匹配算法生成的位置数据与惯性导航系统（INS）数据进行融合，校正惯性导航系统的累积误差。

5. 输出位置结果：经过集成导航滤波后的位置数据作为最终的导航输出，用于实现高精度的自主定位。

此流程确保实时地磁测量数据与参考地图匹配，补偿惯性导航系统的误差，从而实现高精度定位。
ICCP算法通过迭代最近点来实现测量图像与模型之间的匹配，并以欧几里得距离的平方最小化作为目标函数，从而获得测量轨迹与真实轨迹之间的最佳匹配，以校正测量轨迹。

算法的原理如图2所示，其中 $C_i$（$i=1,2,\mathrm{3...}N$）为磁场轮廓，$P_i$（$i=0,1,2,\mathrm{3...}N$）为惯性导航指示的轨迹序列，$Y_i$（$i=1,2,\mathrm{3...}N$）为匹配目标序列，$X_i$（$i=1,2,\mathrm{3...}N$）为实际轨迹序列。通过刚性变换最小化目标函数的欧几里得距离，算法在目标函数变换小于阈值或达到最大迭代次数时结束。

地磁ICCP匹配算法基于以下两个假设。

假设1：地磁传感器没有测量误差。
在实际应用中，传感器不可避免地存在误差，无法满足假设1。

假设2：载体的真实位置位于与测量的磁场值对应的轮廓上。
当车辆行驶区域中没有可选择的自适应区域时，由于刚性变换的影响，ICCP算法容易受到惯性导航系统累积误差的影响。随着导航时间的增加，每个测量点的定位误差也会增加，惯性导航指示的轨迹逐渐偏离真实轨迹。在这些情况下，如果假设2不成立，使用ICCP算法进行迭代变换的匹配误差将持续增加。

Vector Improved Algorithm for ICCP Matching

为满足ICCP匹配算法的假设2，对传统ICCP算法进行了两方面的改进。

• 首先，设计了蚁群优化（ACO）算法，以改进在高概率范围内的搜索策略。
• 其次，考虑到匹配区域的地磁总特征非常相似，ICCP算法容易出现匹配精度低或匹配错误的问题。通过使用地磁三维特征来改进目标函数，将其定义为匹配指标。

Improvement of Search Area

ICCP算法是一种在全局范围内寻找最优对齐的方法。由于在整个地图上搜索轮廓线上的最近点，因此其效率相对较低。考虑到惯性导航系统（INS）的累积误差，在惯性导航指示的轨迹附近添加了误差的上下限，以限制搜索范围，使得真实轨迹更有可能落在待匹配的区域内。建立的搜索模型如图3所示，黑色虚线椭圆区域是基于AUV指示坐标的误差椭圆。

假设惯性导航系统（INS）的定位误差服从标准正态分布，${\sigma }_x^2$、${\sigma }_y^2$和${\sigma }_{xy}$分别是东向位置方差、北向位置方差和位置协方差，${\hat{\sigma }}_0$是定位误差的标准差。误差椭圆的计算公式如下：

$$a={\hat{\sigma }}_0\sqrt{\frac{{\sigma }_x^2+{\sigma }_y^2+\sqrt{({\sigma }_x^2-{\sigma }_y^2{)}^2+4{\sigma }_{xy}^2}}{2}}$$

公式(1)解释

1. 误差椭圆：这是一个用于表示定位误差分布的椭圆，长半轴和短半轴表示误差的范围。
2. 符号说明：
   • ${\sigma }_x^2$：东向位置方差，表示东向误差的分布范围。
   • ${\sigma }_y^2$：北向位置方差，表示北向误差的分布范围。
   • ${\sigma }_{xy}$：位置协方差，表示东向和北向误差之间的相关性。
   • ${\hat{\sigma }}_0$：误差的标准差，用于缩放椭圆的大小。
3. 公式计算过程
   • 内部平方和：${\sigma }_x^2+{\sigma }_y^2$ 计算了东向和北向误差方差的总和。
   • 内部平方差和协方差：$({\sigma }_x^2-{\sigma }_y^2{)}^2+4{\sigma }_{xy}^2$ 这一部分结合了东西方和南北方误差的差异及它们的相关性。
   • 平方根运算：对上述和的结果开平方，得到一个包含协方差信息的调整值。
   • 长半轴计算：最终，将计算结果乘以 ${\hat{\sigma }}_0$ 的平方根，得到误差椭圆的长半轴 $a$，即椭圆最大方向的误差范围。
4. 公式意义
   • 文中的公式 (1) 用于计算误差椭圆的长半轴 $a$。
   • 最终用于确定误差椭圆的形状和方向，以便更精确地估计INS的定位误差范围。

$$b={\hat{\sigma }}_0\sqrt{\frac{{\sigma }_x^2+{\sigma }_y^2-\sqrt{({\sigma }_x^2-{\sigma }_y^2{)}^2+4{\sigma }_{xy}^2}}{2}}$$

公式(2)解释

1. 误差椭圆：误差椭圆的短半轴 $b$ 表示在定位误差椭圆中与长半轴垂直的方向误差范围。该半轴用于衡量误差在另一主方向上的分布。

2. 符号说明：

   • ${\sigma }_x^2$：东向位置方差，表示东向误差的分布范围。
   • ${\sigma }_y^2$：北向位置方差，表示北向误差的分布范围。
   • ${\sigma }_{xy}$：位置协方差，表示东向和北向误差之间的相关性。
   • ${\hat{\sigma }}_0$：定位误差的标准差，用于缩放椭圆的大小。

3. 公式计算过程

   • 内部平方和：${\sigma }_x^2+{\sigma }_y^2$ 计算东向和北向误差方差的总和。
   • 内部平方差和协方差：$({\sigma }_x^2-{\sigma }_y^2{)}^2+4{\sigma }_{xy}^2$ 这一部分结合了东西方向和南北方向误差的差异及它们的相关性。
   • 平方根运算：对上述和的结果取平方根，得到一个包含协方差信息的调整值。
   • 短半轴计算：将计算结果乘以 ${\hat{\sigma }}_0$ 的平方根，从而得到误差椭圆的短半轴 $b$，表示椭圆次主方向上的误差范围。

4. 公式意义

   • 文中的公式 (2) 用于计算误差椭圆的短半轴 $b$。
   • 最终用于确定误差椭圆的形状和方向，以便更精确地估计INS的定位误差范围。

$$\varphi =\frac{\pi }{2}-\frac{1}{2}\arctan \frac{2{\sigma }_{xy}}{{\sigma }_x^2-{\sigma }_y^2}$$

公式(3)解释

1. 误差椭圆方向：误差椭圆的长半轴方向与真北方向的夹角 $\varphi$ 表示误差分布的主方向。此角度用于确定误差椭圆在地理坐标系中的方向。

2. 符号说明：

   • ${\sigma }_x^2$：东向位置方差，表示东向误差的分布范围。
   • ${\sigma }_y^2$：北向位置方差，表示北向误差的分布范围。
   • ${\sigma }_{xy}$：位置协方差，表示东向和北向误差之间的相关性。

3. 公式计算过程

   • 协方差比值：$\frac{2{\sigma }_{xy}}{{\sigma }_x^2-{\sigma }_y^2}$ 这一项表示协方差与位置方差差值的比值，反映了东西和南北误差之间的相关性程度。
   • 反正切运算：对比值取反正切，用于计算长半轴与主要误差方向之间的角度。
   • 角度转换：通过减去 $\frac{1}{2}\arctan \left(\frac{2{\sigma }_{xy}}{{\sigma }_x^2-{\sigma }_y^2}\right)$ 并加上 $\frac{\pi }{2}$，得到椭圆长半轴与真北方向的夹角 $\varphi$。

4. 公式意义

   • 文中的公式 (3) 用于计算误差椭圆的长半轴与真北方向之间的夹角 $\varphi$。
   • 该公式最终用于确定误差椭圆在地理坐标中的朝向，以便更加准确地描述定位误差的分布方向。

其中，$a$ 为椭圆的长半轴，$b$ 为椭圆的短半轴，$\varphi$ 为椭圆长半轴与真北方向之间的夹角。根据 $3\sigma$ 原则，当 ${\hat{\sigma }}_0=3.03$ 时，真实轨迹点落在误差椭圆内的概率为95%，可以认为在此范围内匹配是准确的。

匹配导航算法的本质是一个函数优化问题。作为一种经典的模拟算法，遗传算法将每一个可能的解视为一个个体，并使用某种最优规则，使个体通过迭代逐步接近最优解。目标函数的计算公式为：

$$J_{\text{COR}}({\lambda }_j,{\phi }_j)=\frac{1}{N}\sum _{i=1}^N\Delta T_i\cdot \Delta T_i^K$$

公式(4)解释

1. 目标函数 $J_{\text{COR}}({\lambda }_j,{\phi }_j)$：用于评估匹配点的适应性，以便找到最优的匹配点。这里，目标函数值越高，表明该点在匹配过程中越符合目标要求。

2. 符号说明：

   • $({\lambda }_j,{\phi }_j)$：匹配点的坐标。
   • $J_{\text{COR}}({\lambda }_j,{\phi }_j)$：对应于该匹配点的目标函数值。
   • $N$：个体的数量，即在遗传算法中用于评估的候选匹配点数。
   • $\Delta T_i$：个体 $i$ 的适应度，表示匹配过程中某一候选匹配点的特定评估值。
   • $\Delta T_i^K$：点 $T_i$ 对应的权重因子，用于调整个体的适应度贡献。

3. 公式计算过程：

   • 对所有 $N$ 个候选点的适应度 $\Delta T_i$ 和其权重因子 $\Delta T_i^K$ 的乘积求和，然后除以 $N$。
   • 最终得到的 $J_{\text{COR}}({\lambda }_j,{\phi }_j)$ 表示在当前匹配点坐标下的适应度得分。
   • 其目的是找到能够最大化 $J_{\text{COR}}({\lambda }_j,{\phi }_j)$ 的 $\Delta T_i$，相应的 $({\lambda }_j,{\phi }_j)$ 为AUV位置的估计值。

4. 公式意义

   • 文中的公式 (4) 定义了用于计算匹配导航的目标函数 $J_{\text{COR}}$。
   • 其目的是找到能够最大化 $J_{\text{COR}}({\lambda }_j,{\phi }_j)$ 的 $\Delta T_i$，相应的 $({\lambda }_j,{\phi }_j)$ 为AUV位置的估计值。

交叉算子的主要功能是生成新的个体，实现算法的全局搜索能力。

• 从群体进化的角度来看，交叉概率应随着进化过程逐渐减少，最终趋于一个固定值。
• 从新个体出现的角度来看，群体中的所有个体应具有相同的状态，因此遗传算法在搜索空间中的概率相同。

因此，交叉概率的计算公式为：

$$P_c(t)=\{\begin{array}{ll}\exp \left(-\frac{t}{T_G}\right)\cdot P_{c,\min }& \text{当 }{M}_t>P_{c,\min }\\ P_{c,\max }& \text{当 }{M}_t\le P_{c,\min }\end{array}$$

公式(5)解释

1. 交叉概率 $P_c(t)$：在遗传算法第 $t$ 代时的交叉概率，用于控制算法中个体的交叉操作，从而生成新的个体。

2. 符号说明：

   • $t$：当前的进化代数，表示算法迭代的进程。
   • $T_G$：最大进化代数，表示算法允许的最大迭代次数。
   • $P_{c,\min }$：最小交叉概率，保证交叉概率不低于此值。
   • $P_{c,\max }$：最大交叉概率，用于当 $M_t$ 小于 $P_{c,\min }$ 时的固定交叉概率。
   • $M_t$：一个中间计算变量，用于判断交叉概率是否大于最小交叉概率。

3. 公式计算过程：

   • 当 $M_t>P_{c,\min }$ 时，交叉概率 $P_c(t)$ 按照指数衰减公式计算：$\exp \left(-\frac{t}{T_G}\right)\cdot P_{c,\min }$，表示交叉概率随着迭代逐渐降低。
   • 当 $M_t\le P_{c,\min }$ 时，交叉概率设为固定值 $P_{c,\max }$。

4. 公式意义

   • 公式 (5) 用于计算交叉概率 $P_c(t)$，用于控制遗传算法中每一代的交叉操作概率。
   • 通过这种设置，交叉概率在算法早期较高，以增强探索能力，随着进化过程逐渐减小，使个体更稳定，从而提高算法的收敛性。

变异算子主要用于维持种群的多样性。同一代群体中每个个体的变异概率随个体的适应度而变化。因此，应增加劣质个体的变异概率，减少优质个体的变异概率。因此，得到了以下与遗传进化代数和个体适应度相关的变异概率公式：

$$P_m(t)=\{\begin{array}{ll}{M}_t& \text{当 }{M}_t>P_{m,\min }\\ P_{m,\min }& \text{当 }{M}_t\le P_{m,\min }\end{array}$$

公式(6)解释

1. 变异概率 $P_m(t)$：第 $t$ 代中个体的变异概率，用于控制算法中各个体的变异操作概率，从而保持种群的多样性。

2. 符号说明：

   • $P_m(t)$：第 $t$ 代中个体的变异概率。
   • $M_t$：中间计算变量，用于确定是否增加或降低变异概率。
   • $P_{m,\min }$：最小变异概率，用于限制变异概率的最低值，确保变异不会完全停止。

3. 公式计算过程：

   • 当 $M_t>P_{m,\min }$ 时，变异概率 $P_m(t)$ 等于 $M_t$，即个体的变异概率由中间变量 $M_t$ 决定。
   • 当 $M_t\le P_{m,\min }$ 时，变异概率设为 $P_{m,\min }$，保证变异概率不会低于最低限制。

4. 公式意义

   • 公式 (6) 用于计算变异概率 $P_m(t)$，用来在遗传算法中控制每代个体的变异操作概率。
   • 通过这种设置，适应度较低的个体（劣质个体）会有更高的变异概率，从而增加变异的机会，以保持种群的多样性，而适应度较高的个体（优质个体）则具有较低的变异概率，有助于保护已经找到的较优解。

$$M_t=\exp \left(-\frac{f_{\max }-f(X_i)}{f_{\max }}\right)\cdot \frac{1}{1+\frac{t}{T_G}}\cdot P_{m,\max }$$

公式(7)解释

1. 中间变量 $M_t$：用于动态调整个体在每代中的变异概率。它根据个体的适应度（fitness）和当前的进化代数来决定变异概率的大小。

2. 符号说明：

   • $f_{\max }$：当前群体中的最大适应度值，表示当前最优个体的适应度。
   • $f(X_i)$：待变异个体 $X_i$ 的适应度。
   • $t$：当前的进化代数。
   • $T_G$：最大进化代数。
   • $P_{m,\max }$：最大变异概率，用于控制最高的变异概率上限。

3. 公式计算过程：

   • 适应度影响部分：$\exp \left(-\frac{f_{\max }-f(X_i)}{f_{\max }}\right)$ 这一部分基于个体适应度与当前最大适应度之间的差异来计算。差异越大，该项的值越接近于1，使 $M_t$ 接近于 $P_{m,\max }$ 的值；差异越小，结果越接近0，降低了变异概率。
   • 进化代数影响部分：$\frac{1}{1+\frac{t}{T_G}}$ 这一项随进化代数 $t$ 增加而减小，表示在进化早期个体更容易发生变异，以保持多样性，而在进化后期逐渐降低变异概率，以提高收敛性。
   • 最大变异概率 $P_{m,\max }$：控制变异概率的上限，确保个体变异的概率不会超过设定的最大值。

4. 公式意义

   • 公式 (7) 用于计算中间变量 $M_t$，它决定了个体的变异概率。
   • 通过这种设置，适应度较低的个体和进化早期的个体更容易发生变异，从而保持种群多样性；适应度较高的个体和进化后期的个体变异概率较低，增强算法的收敛性。

通过上述缩小搜索区域和更新目标点的策略，将匹配范围缩小到合适的范围，持续消除异常点，增加高概率目标点，从而提高匹配效率。

Improvements to Match Points

存在问题：在总磁场变化不大的地磁地图中，ICCP算法使用总磁场强度进行平移和旋转变换，难以清晰区分真实轨迹和惯性导航指示的轨迹，容易发生匹配错误。

解决方法：

• 在搜索最近点时，采用地磁场的三分量强度 $x$、$y$ 和 $z$ 作为评估指标。根据真实轨迹序列 $X_i$ 上的磁场强度测量值，确定地磁地图中的轮廓 $C_i$，然后计算轮廓上每个点的三分量磁场值与测量点的三分量值之间的差值平方和。
• 将目标函数差值平方和最小的点作为轮廓上的最近点 (Y_i)，最后对惯性导航指示序列 (P_i) 进行旋转和平移，使得变换后的路径与 (Y_i) 之间的距离最小。
• 重复上述过程，直到算法收敛或精度小于阈值为止。

在搜索最近点 $Y_i$ 时，考虑 $x$、$y$ 和 $z$ 的三分量强度，物理指标$B(i)$通过以下公式获得：

$$B(i)=\sqrt{(B_{cx}-B_{xx}{)}^2+(B_{cy}-B_{xy}{)}^2+(B_{cz}-B_{xz}{)}^2}$$

公式(8)解释

1. 物理指标 $B(i)$：用于量化测量点与轮廓上某点之间的磁场强度差异。计算的是测量点和轮廓点的磁场三分量之间的欧几里得距离，表示匹配误差。

2. 符号说明：

   • $B_{cx}$、$B_{cy}$、$B_{cz}$：轮廓点在 $x$、$y$ 和 $z$ 方向上的磁场强度分量。
   • $B_{xx}$、$B_{xy}$、$B_{xz}$：测量点在 $x$、$y$ 和 $z$ 方向上的磁场强度分量。

3. 公式计算过程：

   • 差异计算：分别计算测量点和轮廓点在 $x$、$y$ 和 $z$ 方向上的磁场强度差异，即 $B_{cx}-B_{xx}$、$B_{cy}-B_{xy}$、$B_{cz}-B_{xz}$。
   • 平方和：将每个方向的差异平方后求和，得出整体误差。
   • 平方根：对平方和开平方，以获得测量点和轮廓点之间的欧几里得距离。

4. 公式意义

   • 公式 (8) 用于计算最近点的物理指标 $B(i)$，表示匹配点的磁场强度三分量的差异。
   • 通过该公式，可以计算测量点与轮廓点之间的磁场强度差异，从而选择出差异最小的点 $Y_i$ 作为最近点，提高匹配精度。

Hausdorff距离描述了两组点之间的相似性。在TERCOM匹配算法中使用Hausdorff距离，验证了其比欧几里得距离更为准确。其计算公式为：

$$H(H_0,H_m)=\max (h(H_0,H_m),h(H_m,H_0))$$

公式(9)解释

1. Hausdorff 距离 $H(H_0,H_m)$：用于测量两组点 $H_0$ 和 $H_m$ 之间的最大距离差异，表示两组点的最不相似部分。距离越大，说明两组点的相似度越低。

2. 符号说明：

   • $H_0$ 和 $H_m$：表示两个点集，通常分别代表测量点集和参考点集。
   • $h(H_0,H_m)$：单向 Hausdorff 距离，表示从点集 $H_0$ 到点集 $H_m$ 的最大最小距离。
   • $h(H_m,H_0)$：从点集 $H_m$ 到点集 $H_0$ 的单向 Hausdorff 距离。

3. 公式计算过程：

   • 首先计算从点集 $H_0$ 到 $H_m$ 的单向 Hausdorff 距离 $h(H_0,H_m)$，这是 $H_0$ 中每个点到 $H_m$ 中最近点的最大距离。
   • 然后计算从点集 $H_m$ 到 $H_0$ 的单向 Hausdorff 距离 $h(H_m,H_0)$，这是 $H_m$ 中每个点到 $H_0$ 中最近点的最大距离。
   • 最后取这两个单向 Hausdorff 距离的较大值作为最终的 Hausdorff 距离 $H(H_0,H_m)$。

4. 公式意义

   • 公式 (9) 用于计算两组点之间的 Hausdorff 距离 $H(H_0,H_m)$，表示两组点的最大相似性差异。
   • 通过 Hausdorff 距离，可以衡量两组点之间的相似性或差异性，常用于需要精确匹配的场景中。较大的 Hausdorff 距离表明两组点之间的差异较大，匹配度较低。

$$h(H_0,H_m)=\underset{h_m\in H_0}{\max }\underset{h_0\in H_m}{\min }\Vert h_m-h_0\Vert$$

公式(10)解释

1. 单向 Hausdorff 距离 $h(H_0,H_m)$：表示从点集 $H_0$ 到点集 $H_m$ 的距离，即在点集 $H_0$ 中找到一个点，该点到点集 $H_m$ 中最近点的距离最大。这意味着该公式衡量的是点集 $H_0$ 中最远离点集 $H_m$ 的点到 $H_m$ 的最近距离。

2. 符号说明：

   • $H_0$ 和 $H_m$：两个点集，通常用于比较匹配的两个点集合。
   • $h_m\in H_0$：表示在 $H_0$ 集合中的某一个点。
   • $h_0\in H_m$：表示在 $H_m$ 集合中的某一个点。
   • $\Vert h_m-h_0\Vert$：表示 $H_0$ 集合中的点 $h_m$ 与 $H_m$ 集合中点 $h_0$ 之间的欧几里得距离。

3. 公式计算过程：

   • 对于 $H_0$ 集合中的每个点 $h_m$，找到它到 $H_m$ 集合中最近点的距离（即 ${\min }_{h_0\in H_m}\Vert h_m-h_0\Vert$）。
   • 在所有这些最小距离中，取最大值（即 ${\max }_{h_m\in H_0}{\min }_{h_0\in H_m}\Vert h_m-h_0\Vert$），得到从 $H_0$ 到 $H_m$ 的单向 Hausdorff 距离。

4. 公式意义

   • 公式 (10) 用于计算从点集 $H_0$ 到 $H_m$ 的单向 Hausdorff 距离 $h(H_0,H_m)$。该公式描述了在点集 $H_0$ 中的每个点到点集 $H_m$ 中最近点的最大最小距离。
   • 该公式用于测量点集 $H_0$ 中最远点与点集 $H_m$ 的最近距离，表示 $H_0$ 到 $H_m$ 的最大偏差。这对于比较两个点集的相似性或差异性非常有用。

$$h(H_m,H_0)=\underset{h_0\in H_m}{\max }\underset{h_m\in H_0}{\min }\Vert h_0-h_m\Vert$$

公式(11)解释

1. 单向 Hausdorff 距离 $h(H_m,H_0)$：表示从点集 $H_m$ 到点集 $H_0$ 的距离，即在点集 $H_m$ 中找到一个点，该点到点集 $H_0$ 中最近点的距离最大。此公式衡量的是点集 $H_m$ 中最远离点集 $H_0$ 的点到 $H_0$ 的最近距离。

2. 符号说明：

   • $H_0$ 和 $H_m$：两个点集，通常用于比较匹配的两个点集合。
   • $h_0\in H_m$：表示在 $H_m$ 集合中的某一个点。
   • $h_m\in H_0$：表示在 $H_0$ 集合中的某一个点。
   • $\Vert h_0-h_m\Vert$：表示 $H_m$ 集合中的点 $h_0$ 与 $H_0$ 集合中点 $h_m$ 之间的欧几里得距离。

3. 公式计算过程：

   • 对于 $H_m$ 集合中的每个点 $h_0$，找到它到 $H_0$ 集合中最近点的距离（即 ${\min }_{h_m\in H_0}\Vert h_0-h_m\Vert$）。
   • 在所有这些最小距离中，取最大值（即 ${\max }_{h_0\in H_m}{\min }_{h_m\in H_0}\Vert h_0-h_m\Vert$），得到从 $H_m$ 到 $H_0$ 的单向 Hausdorff 距离。

4. 公式意义

   • 公式 (11) 用于计算从点集 $H_m$ 到 $H_0$ 的单向 Hausdorff 距离 $h(H_m,H_0)$。该公式表示在点集 $H_m$ 中每个点到点集 $H_0$ 中最近点的最大最小距离。
   • 该公式用于测量点集 $H_m$ 中最远点与点集 $H_0$ 的最近距离，表示 $H_m$ 到 $H_0$ 的最大偏差。通过计算两组点的单向 Hausdorff 距离，可以更全面地比较两组点的相似性或差异性。

改进后的匹配算法流程如下：

1. 使用 $B(i)$ 作为匹配指标，Hausdorff 距离作为目标函数，利用 ICCP 算法对惯性导航指示轨迹进行迭代匹配，得到估计轨迹 $L_0$ 并确定匹配轨迹。

2. 以估计轨迹为基准，使用公式 (1)–(3) 构建误差椭圆，在可行区域内生成由 $N$ 个个体组成的初始群体，以限定初始匹配区域。

3. 计算适应度值。将每个个体的 $J_{\text{COR}}({\lambda }_j,{\phi }_j)$ 和测量数据序列作为该个体的适应度值，选择适应度较高的一半个体保留，并在群体中去除误差较大的点。

4. 遗传和变异。使用公式 (5)–(7) 更新群体的适应度，逐步向最优个体靠近。

5. 判断是否满足终止条件。如果未满足，返回步骤 2；如果满足，则输出最优解。

Experiments

实验目的

• 验证基于地磁矢量的改进ICCP匹配算法的有效性，评估其在复杂条件下的匹配精度和轨迹复原效果。

地图范围

• 选择北纬38°至38.1°、东经120°至120.15°的海域进行仿真实验，网格间距为0.00125°（约138.9米），展示该范围内的总地磁场强度。

参数设计

• 轨迹初始航向：36.6°。
• 惯性导航系统陀螺仪：具有0.04°/h的随机漂移和常值漂移。
• 加速度计：零偏为0.00005°/h，随机游走误差为0.00005°/h。
• 位置误差：测量值的±10%。
• 采样点数：沿轨迹选取10个采样点。

计算了两种ICCP匹配算法的平均定位误差、平均航向误差和匹配时间，结果如表1所示。根据匹配结果，矢量改进的ICCP算法将平均位置误差从783米降低至238米，平均航向误差从7.54°降低至4.22°。根据算法实现过程的分析，矢量改进的ICCP算法用误差椭圆代替了大范围的搜索区域，匹配时间从15.62秒缩短至9.53秒。

Algorithm	Average Position Error (m)	Average Heading Error (°)	Matching Time (s)
Traditional ICCP Algorithm	783	7.54	15.62
Vector ICCP Algorithm	238	4.22	9.53

矢量改进的ICCP算法通过改进搜索区域来减少匹配算法的搜索范围。同时，蚁群算法加快了目标点的迭代更新速度，使匹配时间减少至传统ICCP算法的61.0%，从而提高了匹配效率。此外，通过结合地磁三维特征和Hausdorff距离目标函数优化匹配指标。结果表明，位置误差降低至传统算法的30.4%，航向误差降低至55.9%，提高了匹配精度。因此，基于地磁矢量的改进ICCP算法能够获得更好的匹配效果。

Conclusions

问题：ICCP算法对航向误差敏感，并且在地磁特征相似的区域容易发生匹配错误，导致匹配精度和效率受限。

解决方法：引入蚁群算法（ACO），优化误差椭圆内的搜索范围和过程，缩小匹配区域，并去除异常点、增加高概率点，使目标快速收敛。此外，结合地磁矢量特征，对匹配点的三轴分量进行匹配，并使用Hausdorff距离评估适应度，以全面利用目标点的特征信息。

效果：仿真结果表明，改进后的算法将匹配时间缩短至原来的61.0%，显著提升了匹配效率。同时，定位误差减少至30.4%，航向误差降低至55.9%，匹配精度大幅提升。

不足与改进方向：该方法仍可能受限于较复杂环境中的动态干扰，以及高计算成本。未来可进一步优化算法以适应更多复杂场景，并提高计算效率。

References

参考文献序号	作者	参考文献标题	出版物 / 会议名称	年份	主要工作内容
[1]	Paul, A.M. 等	Autonomous Underwater Vehicle Navigation	IEEE J. Ocean. Eng.	2010	提出了水下自主导航方法，解决了惯性导航系统（INS）误差积累问题。
[2]	Tkhorenko, M.Y. 等	On integration of a strapdown inertial navigation system with modern magnetic sensors	St. Petersburg Intl. Conf. on Integrated Navigation Systems	2018	讨论了INS与磁传感器的集成，以提高导航精度。
[3]	Liu, Z. 等	Fusion of Magnetic and Visual Sensors for Indoor Localization	IEEE Trans. Multimed.	2017	提出了一种融合磁传感器和视觉传感器的室内定位方法，以提高定位精度。
[4]	Meng, W., Jian, Y.	Adaptive UKF-SLAM Based on Magnetic Gradient Inversion Method for Underwater Navigation	Springer	2015	提出了基于自适应UKF-SLAM的磁梯度辅助导航方法，提高了噪声环境下的导航能力。
[5]	Liu, Z., Chen, W.W. 等	Return Navigation Algorithm Based on Magnetic Matching	J. Mil. Commun. Technol.	2016	提出了一种基于磁场信息的回程导航算法，用于修正导航误差。
[6]	Stateczny, A. 等	Methodology for Processing of 3D Multibeam Sonar Big Data for Comparative Navigation	Remote Sens.	2019	提出了用于比较导航的3D多波束声呐大数据处理方法。
[7]	Zhang, T. 等	An autonomous underwater vehicle positioning matching method based on iterative closest contour point algorithm and affine transformation	Proc. Inst. Mech. Eng. Part M J. Eng. Marit. Environ.	2017	开发了一种用于AUV定位的两阶段匹配方法，采用ICCP和仿射变换。
[8]	Wang, H. 等	Technology of gravity aided inertial navigation system and its trial in South China Sea	IET Radar Sonar Navig.	2016	研究了重力辅助INS方法并在南海进行试验，以提高导航精度。
[9]	Kang, C. 等	Geomagnetic Navigation Region Selection Based on Geomagnetic Entropy and Geomagnetic Difference Entropy	J. Appl. Basic Eng. Sci.	2015	提出了一种基于地磁熵的导航区域选择方法，以改善INS误差估计。
[10]	Quintas, J. 等	An Integrated System for Geophysical Navigation of Autonomous Underwater Vehicles	IFAC-Pap.	2018	提出了用于AUV导航的地球物理综合导航系统。
[11]	Zhai, H.Q., Wang, L.H.	The robust residual-based adaptive estimation Kalman filter method for strap-down inertial and geomagnetic tightly integrated navigation system	Rev. Sci. Instrum.	2020	开发了一种基于鲁棒残差自适应估计的卡尔曼滤波方法，适用于紧耦合INS和地磁导航。
[12]	Chen, X. 等	Underwater Terrain Matching Positioning Method Based on MLE for AUV	Robot	2012	提出了一种基于最大似然估计（MLE）的水下地形匹配定位方法。
[13]	Wang, K. 等	Matching error of the iterative closest contour point algorithm for terrain-aided navigation	Aerosp. Sci. Technol.	2018	分析了地形辅助导航中ICCP算法的匹配误差。
[14]	Xiao, J. 等	Research Status of Magnetic Matching Algorithm and Its Improvement Strategies	Electro-Opt. Control	2018	综述了磁匹配算法的研究现状，并提出了改进策略。
[15]	Zhou, L. 等	An Effective Terrain Aided Navigation for Low-Cost Autonomous Underwater Vehicles	Sensors	2017	提出了一种有效的地形辅助导航方法，适用于低成本AUV。
[16]	Zhai, H., Wang, L. 等	Geomagnetic signal de-noising method based on improved empirical mode decomposition and morphological filtering	Proc. Inst. Mech. Eng. Part G J. Aerosp. Eng.	2020	提出了一种基于改进经验模态分解和形态学滤波的地磁信号去噪方法。
[17]	Canciani, A., John, R.	Absolute Positioning Using the Earth’s Magnetic Anomaly Field	Navig.-J. Inst. Navig.	2016	提出了一种基于地球磁异常场的绝对定位方法。
[18]	Afzal, M.H. 等	Magnetic field based heading estimation for pedestrian navigation environments	Intl. Conf. on Indoor Positioning and Indoor Navigation	2011	提出了一种基于磁场的行人导航方向估计方法。
[19]	Mo, R.	Studying on comparison of different geomagnetic matching navigation algorithms	Geomat. Spat. Inf. Technol.	2016	对各种地磁匹配导航算法进行了比较研究。
[20]	Teixeira, F.-C. 等	Experimental validation of magnetic navigation of marine robotic vehicles	IFAC Pap.	2016	通过实验验证了海洋机器人车辆的磁导航技术。

---

1. State Key Laboratory of Geo-Information Engineering, Xi’an 710054, China ↩︎ ↩︎

2. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China ↩︎ ↩︎ ↩︎ ↩︎

3. Author to whom correspondence should be addressed. ↩︎

4. China Ship Development & Design Center, Wuhan 430010, China ↩︎