CN111397599A - 基于三角形匹配算法改进的iccp水下地磁匹配方法 - Google Patents

Abstract

本发明公开了一种基于三角形匹配算法改进的ICCP水下地磁匹配方法，提出一种利用约束粒子群算法优化估计航路改进的ICCP算法。构建三角形匹配模型，确定估计点的匹配搜索范围，得到约束粒子群算法的一个约束条件；利用不同三角形之间刚性旋转和平移变换得到的矩阵函数，将导航系统累积误差对匹配精度的影响数学化，同时定义不同三角形之间的匹配度原则，得到约束粒子群算法的另一个约束条件；利用约束粒子群优化算法得到匹配三角形和惯性导航指示三角形两质心点之间的最小欧式距离，得到更新点作为下一次循环的确定点，依次迭代，规划出最终航行路径。提高了水下地磁信息的利用率，降低ICCP匹配算法的原理缺陷和惯导累积误差对匹配精度的影响。

Description

基于三角形匹配算法改进的ICCP水下地磁匹配方法

技术领域

本发明涉及一种地磁导航匹配算法，尤其涉及一种基于三角形匹配算法改进 的ICCP水下地磁匹配方法。

背景技术

地磁导航是一种基于地理信息的导航方式，相比卫星导航和地形导航等方式， 地磁导航具有无源、无辐射、隐蔽性强、误差不随时间累积等特点，是修正惯性 导航系统累积误差的一种有效方式。最近等值线迭代(ICCP)匹配算法作为一 种水下地磁辅助导航，具有较高的隐蔽性、自主性和全天候性。ICCP匹配算法 仅通过对惯性导航系统指示航迹进行刚性旋转和平移变换来实现最近等深点的 迭代配准，匹配误差将随匹配段惯性导航误差的累积而增加已于实现，且具有较 好的初始航向容错性。在地磁环境稀疏的情况下，足够数量的地磁点可能导致更 长的匹配距离，从而导致轨道误差增加和匹配不准确，但是，距离较短时，匹配 点可能不足以进行精确匹配。因此提出了一种基于三角形匹配算法改进的ICCP 水下地磁匹配方法，减少稀疏地磁环境下惯性导航误差对地磁导航匹配精度带来 的影响，提高地磁导航的精确性和鲁棒性。另外，在最优化问题领域内，传统的 最小二乘算法存在较大的弊端，对于求解非线性问题存在局限，但是广泛应用的 CPSO(约束粒子群算法)从随机解出发，通过迭代寻找最优解，具有实现容易、 精度高、收敛快的优点。如何改进ICCP算法精度，有效减少惯性导航系统造成 误差地影响，是本发明研究的重点。

发明内容

发明目的：为了克服现有技术的不足，提出一种降低ICCP算法精度影响， 提高水下地磁信息利用率，具有更好定位精度和鲁棒性的ICCP水下地磁匹配方 法。

技术方案：一种基于三角形匹配算法改进的ICCP水下地磁匹配方法，包括 以下步骤：

(1)构建三角形匹配模型，根据参考区域中的地磁信息，确定参考地图中 的搜索范围，给出三角形匹配适配度算法，构建三角匹配模型具体包括：

(1.1)磁力计在参考区域中获得三个连续的地磁信息点；

(1.2)在搜索范围内，在地磁等磁线上寻找连续三个合适的匹配点，构成 三角形对应于惯性导航系统指示路径三个连续点构成的三角形；

(1.3)提出匹配三角形适配度算法；

(2)依据磁力计的测量误差和惯性导航系统的导航误差，提出用于解决匹 配三角形的容错能力的三角形刚性变换和旋转方程，给出惯性导航系统指示路径H_i三角形的质心N和校正后的路径X_i三角形的质心M之间的欧式距离，用约束 基于粒子群优化算法在各搜索区域中依次取位置形成参考航迹，解算出适应度航 迹，确定地磁导航路径；

(2.1)构建刚性平移和旋转方程；

(2.2)给出惯性导航系统指示路径H_i三角形的质心N和校正后的路径X_i三 角形的质心M，并给出两点之间的距离，并把航迹上所有对应三角形质心之间 的距离的和作为适应度函数；

(2.3)在各搜索区域中依次取位置点形成数条航路，将每条航路上的位置 点序列作为一个粒子，基于粒子群优化算法使适应度函数的值最小，从而确定地 磁导航航迹。

进一步地，步骤(1.2)中根据地磁传感器测量精度和惯性导航定位精度， 匹配三角形满足以下约束条件：

式(1)中,H₁ H₂ H₃代表参考区域中获得三个连续的地磁信息点，构建成 ΔH₁H₂H₃三角形的图形，其中X_1,i表示H₁周围的第i个匹配点，R代表搜索范围。

进一步地，步骤(1.3)根据如下确定最佳匹配三角形：

x＝a/c,y＝b/c (2)

式(2)中,在ΔH₁H₂H₃中，定义H₁H₂＝a，H₂H₃＝b，H₁H₃＝c，其中定义 H₁H₃为ΔH₁H₂H₃中最长的一条边，x与y分别代表剩余两边H₂H₃和H₁H₂与最长边 H₁H₃之间的数值比；因此，P(x,y)代表一组ΔH₁H₂H₃三角形的特征值；式(3) 中P′(x,y)代表一组ΔX₁X₂X₃三角形的特征值，D_PP′代表直角坐标系上点P和P′之间 的欧式距离，则ε_XY代表ΔH₁H₂H₃和ΔX₁X₂X₃之间的匹配度，ε_XY越接近于0，匹配 度越高。

进一步地，步骤(2)中，依据磁力计的测量误差，匹配三角形有变化，模 型变化见如下函数：

式(4)中，(t_xt_y)分别平移矩阵的水平位移量和垂直位移量，R表示旋转 矩阵，其中θ代表关于原点逆时针旋转的角度，H_i′代表实际的路径；H_i代表水下航行器指示轨迹。

进一步地，步骤(2.2)中，给出对应三角形质心之间的距离的和作为适应 度函数如下:

H_i″＝t+H_i-1+(H_i-H_i-1)R (5)

式(5)中，H_i-1为指示航迹三角形ΔH_iH_i+1H_i+2的质心，H_i″(i＝1,2,…,n)为匹 配三角形ΔH_i+1′H_i+2′H_i+3′的质心；

式(6)中，e代表估计轨迹和容错后的匹配三角形ΔH_i+1′H_i+2′H_i+3′的质心之 间的距离，逐步迭代出X_i+1的位置。

进一步地，步骤(2.3)中粒子群算法模型如下：

v_id(t+1)＝ωv_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t)) (7)

x_id(t+1)＝v_id(t)+v_id(t+1) (8)

式(5)(6)中，ω为惯性权值；r₁、r₂是均匀分布于[0,1]上的随机数；c₁、 c₂为学习因子，也称加速常数，根据经验，通常取c₁＝c₂＝2；v_id代表第i个D 维向量的粒子的速度，v_id∈[-vmax_max]，x_id代表第i个D维向量的粒子的位置； p_id代表第i个D维向量的粒子迄今为止搜索到的最优位置，称为个体极值，即 为目前的航迹；p_gd代表整个粒子群迄今为止搜索到的位置为全局极值，也为地 磁导航航迹。

有益效果：本发明与现有技术相比，其显著优点是：(1)将三角形匹配法 和CPSO算法相结合，使水下航行器基于三角形匹配算法在稀疏的地磁环境进行 地磁导航，提高了水下地磁信息的利用率，减少了惯性导航误差对地磁导航的影 响；(2)基于约束粒子群算法的优化，具有更好的定位精度和鲁棒性，合理地减 少了惯性系统导航带来的测量误差；(3)通过三角形地磁匹配法和约束粒子群算 法相结合，可以有效地提高水下航行器地磁导航的精确性和鲁棒性。

附图说明

图1本发明基于三角形匹配算法改进的ICCP水下地磁匹配方法的总体框图；

图2(a)地磁导航水下匹配地图；

图2(b)三角形匹配点图；

图2(c)匹配三角形的平移和旋转图；

图3是约束粒子群算法的流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，由于水下稀疏的地磁环境和惯性导航误差的影响，水下航行器 携带的磁力计难以收集到地磁测量值与参考地图进行比较，基于三角形匹配算法 改进的ICCP水下地磁匹配方法，将三角形地磁匹配方法和约束粒子群算法结合 起来，逐步迭代得到目标点，减少惯性导航系统带来的测量误差，提高了水下稀 疏地磁的利用率，提高地磁导航的精确性和稳定性，完成地磁导航。如图2(a)， 图中C_i(i＝1,2,…,n())表示匹配区域磁场等值线，L_i(i＝1,2,…,n())表示水下航 行器的实际真实路径，H_i(i＝1,2,…,n())表示惯性导航系统指示路径， X_i(i＝1,2,…,n())表示经过算法校正后的路径,n代表采样的点数。如图1，为 基于三角形匹配算法改进的ICCP水下地磁匹配方法的总体框图，可以看出，首 先采用三角形匹配算法和刚性平移旋转方程，得到变换后三角形，确定校正后的 路径三角形的质心，从而得到惯性导航系统指示路径H_i三角形的质心N和校正后 的路径X_i三角形的质心M之间的欧式距离表达式。如图3，依次取位置点形成数 条参考航路作为粒子，确定各粒子的适应度，给出以三角形搜索范围和三角形匹 配度作为约束条件，以质心之间的欧式距离的最小值作为适应度函数，并采用约 束粒子群算法得出最优解，找到最佳的地磁导航轨迹。

具体步骤包括：

(1)由于水下航行器携带的惯性导航存在测量误差，通过三角形匹配算法 和刚性平移旋转变化方程，建立数学模型，得到惯性导航系统指示路径H_i三角形 的质心和校正后的路径X_i三角形的质心之间的欧式距离表达式。

三角形匹配算法是一种将匹配点距离作为匹配特征的算法，并通过匹配地磁 等高线上的最近点距离来完成三角形匹配。

如图2(b)时三角匹配图所示，在某一时刻，确定当前水下航行器地磁匹配 的搜索范围ΔH₁H₂H₃。其次，在H₁附近周围并且在对应的磁场等值线C₁上，寻找 可待定的三角形匹配点X(i＝1,2,…,n)_1,i。由于地磁传感器测量精度和惯性导航 定位精度的限制，实际匹配中需要允许一定的匹配容差。因此水下航行器在进行 三角形匹配时，所提出的匹配三角形的搜索范围需要满足以下约束条件：

式(1)中，匹配过程中的主要距离误差R定义为搜索范围，可以根据水下航 行器携带的惯性导航的测量误差实时调整。

如图2(c)匹配三角形的平移和旋转图，选取对应的匹配点后组成匹配点三 角形ΔX₁X₂X₃。在惯性导航指示航迹三角形ΔH₁H₂H₃中，H₁H₂＝a，H₂H₃＝b， H₁H₃＝c，其中c定义为ΔH₁H₂H₃中最长的一条边；因此，P(x_H,y_H)代表一组 ΔH₁H₂H₃三角形的特征值。式(3)中P′(x_X,y_X)代表一组ΔX₁X₂X₃三角形的特征值，D_PP′代表点P和P′之间的欧式距离，ε_XY代表ΔH₁H₂H₃和ΔX₁X₂X₃之间的匹配度，ε_XY越接近于0，匹配三角形ΔX₁X₂X₃匹配度越高,水下航行器实际地磁导航信息与参 考地图提供的地磁信息越接近。

x＝a/c,y＝b/c (2)

如图2(c)，由于水下航行器的测量误差，在搜索匹配三角形时需要一定程 度的容错能力，但是该容差将导致匹配三角形和由水下航行器航迹形成的三角形 之间的形状略有变化。为了解决这个问题，需要调整匹配三角形的参数。水下航 行器指示轨迹H_i和实际位置H_i′可通过平移和旋转如公式(4)转换：

如图2(c)，t表示平移矩阵，R表示旋转矩阵，H_i′代表实际的路径。定义 ΔH_iH_i+1H_i+2的质心为H₀,定义ΔH_i+1′H_i+2′H_i+3′的质心为H_i″(i＝1,2,…,n)。得出式 (5)：

H_i″＝t+H₀+(H_i-H₀)R (5)

找到最合适的位置，将建立的地磁匹配三角形和水下航行器指示航迹三角形 之间的对应点的距离的平方和设置为最小，如式(6)：

式(6)中，e代表估计轨迹和容错后的匹配三角形ΔH_i+1′H_i+2′H_i+3′的质心之 间的距离。通过式(6)，可以逐步迭代出X_i+1的位置。

(2)充分利用稀疏的水下地磁信息，给出水下航行器的估计航迹，然后采 用约束粒子群算法得出最优解，为防止陷入局部最优的问题，给出适应度函数来 达到全局最优。

CPSO算法的基本思想是在空间的某个区域内随机初始化一群粒子，将每个 粒子的状态即水下航行器的每条估计航迹视为空间优化问题的一个可行解，航迹 的匹配优良由预先设定的适应度函数来确定。每条航迹将在提出的约束条件下运 动，并由一个速度向量决定其运动的方向和距离。通常粒子群中的粒子根据自身 经验信息和群体经验信息更新自身状态，即可行解在更新规则下追随当前的最优 粒子，不断逐代搜索得到最优解。

在各搜索区域中依次取位置点形成数条参考航路，并对粒子速度进行初始 化,目标函数的定义为

得出式(7)(8)：

v_id(t+1)＝ωv_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t)) (7)

x_id(t+1)＝v_id(t)+v_id(t+1) (8)

式(7)(8)中，ω为惯性权值；r₁、r₂是均匀分布于[0,1]上的随机数；c₁、c₂为 学习因子，也称加速常数，根据经验，通常取c₁＝c₂＝2。v_id代表第i个D维向 量的粒子的速度，v_id∈[-vmax_max]，x_id代表第i个D维向量的粒子的位置；p_id代 表第i个D维向量的粒子迄今为止搜索到的最优位置，也称为个体极值，p_gd代 表整个粒子群迄今为止搜索到的最优位置，也称为全局极值。

由于X_i受到匹配三角形得搜索范围和匹配三角形匹配度的限制，约束粒子群 的适应度函数为式(1)和式(3)，在地磁地图的信息空间内搜索最优N个最优 姿态，使得优化函数

取得最小值，使校正后的地磁导航路径与惯导系统指示的 路径最拟合。

Claims (6)

1.基于三角形匹配算法改进的ICCP水下地磁匹配方法，其特征在于，包括以下步骤：

(1)构建三角形匹配模型，根据参考区域中的地磁信息，确定参考地图中的搜索范围，给出三角形匹配适配度算法，构建三角匹配模型具体包括：

(1.1)磁力计在参考区域中获得三个连续的地磁信息点；

(1.2)在搜索范围内，在地磁等磁线上寻找连续三个合适的匹配点，构成三角形对应于惯性导航系统指示路径三个连续点构成的三角形；

(1.3)提出匹配三角形适配度算法；

(2)依据磁力计的测量误差和惯性导航系统的导航误差，提出用于解决匹配三角形的容错能力的三角形刚性变换和旋转方程，给出惯性导航系统指示路径H_i三角形的质心N和校正后的路径X_i三角形的质心M之间的欧式距离，用约束基于粒子群优化算法在各搜索区域中依次取位置形成参考航迹，解算出适应度航迹，确定地磁导航路径；

(2.1)构建刚性平移和旋转方程；

(2.2)给出惯性导航系统指示路径H_i三角形的质心N和校正后的路径X_i三角形的质心M，并给出两点之间的距离，并把航迹上所有对应三角形质心之间的距离的和作为适应度函数；

(2.3)在各搜索区域中依次取位置点形成数条航路，将每条航路上的位置点序列作为一个粒子，基于粒子群优化算法使适应度函数的值最小，从而确定地磁导航航迹。

2.根据权利要求1所述的基于三角形匹配算法改进的ICCP水下地磁匹配方法，其特征在于，步骤(1.2)中根据地磁传感器测量精度和惯性导航定位精度，匹配三角形满足以下约束条件：

式(1)中,H₁ H₂ H₃代表参考区域中获得三个连续的地磁信息点，构建成ΔH₁H₂H₃三角形的图形，其中X_1,i表示H₁周围的第i个匹配点，R代表搜索范围。

3.根据权利要求1所述的基于三角形匹配算法改进的ICCP水下地磁匹配方法，其特征在于，步骤(1.3)根据如下确定最佳匹配三角形：

x＝a/c,y＝b/c (2)

式(2)中,在ΔH₁H₂H₃中，定义H₁H₂＝a，H₂H₃＝b，H₁H₃＝c，其中定义H₁H₃为ΔH₁H₂H₃中最长的一条边，x与y分别代表剩余两边H₂H₃和H₁H₂与最长边H₁H₃之间的数值比；因此，P(x,y)代表一组ΔH₁H₂H₃三角形的特征值；式(3)中P′(x,y)代表一组ΔX₁X₂X₃三角形的特征值，D_PP′代表直角坐标系上点P和P′之间的欧式距离，则ε_XY代表ΔH₁H₂H₃和ΔX₁X₂X₃之间的匹配度，ε_XY越接近于0，匹配度越高。

4.根据权利要求1所述的基于三角形匹配算法改进的ICCP水下地磁匹配方法，其特征在于，步骤(2)中，依据磁力计的测量误差，匹配三角形有变化，模型变化见如下函数：

式(4)中，(t_x t_y)分别平移矩阵的水平位移量和垂直位移量，R表示旋转矩阵，其中θ代表关于原点逆时针旋转的角度，H_i′代表实际的路径；H_i代表水下航行器指示轨迹。

5.根据权利要求1所述的基于三角形匹配算法改进的ICCP水下地磁匹配方法，其特征在于，步骤(2.2)中，给出对应三角形质心之间的距离的和作为适应度函数如下:

H_i″＝t+H_i-1+(H_i-H_i-1)R (5)

式(5)中，H_i-1为指示航迹三角形ΔH_iH_i+1H_i+2的质心，H_i″(i＝1,2,…,n)为匹配三角形ΔH_i+1′H_i+2′H_i+3′的质心；

式(6)中，e代表估计轨迹和容错后的匹配三角形ΔH_i+1′H_i+2′H_i+3′的质心之间的距离，逐步迭代出X_i+1的位置。

6.根据权利要求1所述的基于三角形匹配算法改进的ICCP水下地磁匹配方法，其特征在于，步骤(2.3)中粒子群算法模型如下：

v_id(t+1)＝ωv_id(t)+c₁r₁(p_id(t)-x_id(t))+c₂r₂(p_gd(t)-x_id(t)) (7)

x_id(t+1)＝v_id(t)+v_id(t+1) (8)

式(5)(6)中，ω为惯性权值；r₁、r₂是均匀分布于[0,1]上的随机数；c₁、c₂为学习因子，也称加速常数，根据经验，通常取c₁＝c₂＝2；v_id代表第i个D维向量的粒子的速度，v_id∈[-vmax_max]，x_id代表第i个D维向量的粒子的位置；p_id代表第i个D维向量的粒子迄今为止搜索到的最优位置，称为个体极值，即为目前的航迹；p_gd代表整个粒子群迄今为止搜索到的位置为全局极值，也为地磁导航航迹。。

Images