CN113532438B - 一种大初始定位误差下的改进iccp地形匹配方法 - Google Patents

Abstract

本发明提供的是一种大初始定位误差下的改进ICCP地形匹配方法。本发明是利用多波束测深系统进行实时水下高程的采集，建立水下地形匹配模板，根据预存的水下数字地形图提取等深点值，建立待匹配地形区域，通过对水下地形匹配相似性进行分析，选取最优匹配序列，求取刚性变换，对匹配对象不断地进行旋转和平移，使匹配对象逐渐逼近目标对象，直到达到某一指标或者达到迭代次数为止，从而得到水下航行器的位置信息。通过对等深值点的相似性分析，有效避免等深值点的误匹配，提高算法精度与稳定性的同时保证改进后算法的全局收敛性。

Description

一种大初始定位误差下的改进ICCP地形匹配方法

技术领域

本发明涉及大初始误差下的基于迭代最近等值点(Iterative Closest ContourPoint，ICCP)的地形匹配定位，特别是涉及一种大初始定位误差下的改进ICCP地形匹配方法。

背景技术

水下航行器在海洋资源开发、近海防御、深海/远海探测等领域有着举足轻重的作用。为保证水下航行器能够顺利地完成相关任务，要求其配备的导航系统具有水下长期自主导航定位与返航能力。由于水下应用环境复杂、具有很强的时变性和不确定因素，使得水下航行器自主、安全、高效地完成导航任务面临诸多挑战，因此，水下导航技术一直是水下航行器研制过程中必须突破和持续发展的核心技术之一。

目前已有的水下定位技术中，捷联惯性导航系统(Strapdown InertialNavigation System，SINS)因其自主性强、隐蔽性好、短时间内精度高、更新频率快及导航信息全面等特点，在水下潜器中得到广泛应用；SINS是以陀螺仪和加速度计为敏感器件的导航参数解算系统，其导航定位原理都是基于牛顿力学基础，既不需要从外部获取信息，也不向外发送信息，具有完全自主的特点。但其定位误差会随时间累积，需要其他的导航系统辅助校正。地球物理场导航是利用地球自身固有的属性特征信息来完成水下运载器的定位与导航，是一种无源定位技术。地形匹配导航是通过将水下航行器获得的当前海域实时地形信息与数据库中的基准数字地形图进行匹配运算，从而确定出水下航行器的当前位置，并利用获得的位置信息对SINS的误差进行修正和补偿，具有无源、无辐射、隐蔽性好、全天时、全天候的优良特征。从理论上讲，该方法与水下航行器的航行时间和航行距离没有关系，可以保证水下航行器在水下长时间航行之后，能够准确地到达任务部署水域，并顺利完成任务，但实际应用中其定位精度受地形数据库精度、水深测量精度及匹配算法等因素制约。

ICCP地形匹配定位算法是基于最近点迭代(Iterative Closest Point，ICP)的图配准技术改进而来的，通过SINS指示位置搜索最近等值点进行迭代计算，获得测量值和数字地图之间的最佳估计，从而获得位置信息。然而，ICCP算法的假设前提是进行地形匹配时SINS指示位置的误差较小，即SINS指示位置离载体的真实位置不远可得到良好的匹配效果。若SINS指示位置误差较大，则根据其指示位置提取的最近等值点距离载体真实位置较远，以距离指示位置最近原则寻找最优匹配点易出现匹配点的误匹配导致算法局部收敛的现象，使ICCP匹配结果失效，不能用来校正惯性导航系统。与传统ICCP方法相比，本发明提供的方法对最优匹配点提取方法进行改进，充分利用多波束系统的冗余性，结合等深值点周围地形对该点综合评价来选取最优匹配点，提升大初始误差下匹配算法的精度与可靠性。

发明内容

针以上问题，本发明提供一种大初始定位误差下的改进ICCP地形匹配方法，针对大初始误差下ICCP算法精度低、易误匹配等问题，充分利用由多波束测深仪测量地形信息的冗余性，提高ICCP算法的精度与可靠性。

本发明提供一种大初始定位误差下的改进ICCP地形匹配方法，具体步骤如下：

步骤1：载体航迹序列与高程数据获取：当水下航行器驶入地形匹配区域后，获取由SINS测量得到的载体位置指示序列{P_i(x_i，y_i)：(i＝1，2，…，N)}，其中(x_i，y_i)为第i个SINS指示位置P_i的坐标，N为载体位置指示序列的个数；与此同时，基于多波束测深系统测得实际航迹序列下的地形高程信息C_i(X_i，Y_i，H_i)(i＝1，2，…，N)，其中C_i表示第i个航迹序列点周围的地形信息量，包含实时测量的地形高程值H_i与其对应的位置坐标(X_i，Y_i)，(X_i，Y_i，H_i)均为m＊n的矩阵，m表示有m个声脉冲组合，n表示每个声脉冲下有n个测深点。其中，位置坐标基于惯性导航系统指示位置进行推算；

步骤2：航迹序列初始变换：采用随机旋转和平移的方法对SINS测量的航迹序列进行初始变换得新的{P′_i(x_i，y_i)：(i＝1，2，…，N)}，其中P′_i为新的航迹指示序列中第i个序列点，代替SINS测量得到的载体位置指示序列进行后续迭代计算，旋转和平移的大小在SINS系统误差方差的3倍范围内随意取值，取旋转偏移量为θ_rand，水平面内位移偏移量为(x_rand，y_rand)；

步骤3：水深等值线提取：根据新的航迹指示序列与其对应的地形高程序列在水下数字地形图中进行等深线提取；

步骤4：寻找最优匹配点：与传统ICCP匹配方法不同的是，不再以距离航迹指示序列点最短作为匹配点选取准则，充分利用多波束数据的冗余性，对数字地图上提取的各个等深值点周围的地形进行提取评价，选取最优匹配点序列其中，Y_i为第i个最优匹配序列点，其位置坐标为/>

步骤5：求取刚性变换：依据最优匹配点序列与刚性变换后航迹指示序列/>之间欧式距离最小原则，求取刚性变换T；

步骤6：重复迭代直到算法收敛：将TP′作为新的起始航迹集合P′转至步骤3进行下一次迭代，直至收敛，经过收敛迭代后获得的集合TP′即为最终的匹配航迹。

作为本发明进一步改进，步骤4中将多波束测量区域取内切圆作为匹配模板，同时，基于等值点在水下数字地形图中提取半径相同的圆形区域作为待匹配模板，进行相似性分析。

作为本发明进一步改进，步骤4中对当前变换后的航迹序列点附近提取的各个等深值点依次进行评价，等深值点按照当前指示位置点由近及远进行提取。

作为本发明进一步改进，步骤4中最优匹配点的选取方法为通过对地形匹配模板与待匹配地形区域的地形分布特征进行分析对比，选择匹配程度高的待匹配区域作为最优匹配点，匹配相似性算法如下：

首先依据实时扫测地形图中的高程数据特征划分统计区间：

{[H_min，H_min+ΔH)，…，[H_max-ΔH，H_max)}

其中，H_min为当前实测区域内最小高程值，H_max为当前实测区域内最大高程值，ΔH为地形高程的分类间隔，根据实时地形特征分布与多波束测深系统误差特性选取；

依据上式对实测地形信息C_i(X_i，Y_i，H_i)按照地形高程值进行分类统计，记录各个区间内测深点的个数g_j，j∈[0，d]，记区间内测深点大于5的为有效区间，其中d为有效区间的个数。利用高斯概率分布表示各有效区间内位置点的分布特征，计算水平位置的均值计算如下式所示；

其中，μ_xj，μ_yj分别为第j个区间内测深点均值，g_j为第j个区间内所有测深点的总数；

由于惯性系统存在航向误差，各区间的水平位置均值坐标不具备旋转不变性，通过建立各区间内测深点位置分布特征量来分析此地形匹配模板的地形信息分布特征：

其中，L_j为第j个区间内测深点位置分布特征量，由计算公式可看出L_j为各区间中测深点位置均值与匹配模板圆心之间的距离；(x_ci，y_ci)为第i个指示位置序列点对应地形匹配模板圆心的水平位置坐标；

其次，以上一步中提取的等值点为圆心提取与匹配模板相同大小的圆形区域作为待匹配区域，对其进行分析与评价，确定是否为最优匹配点，首先，对该地形区域地形高程值进行分类与分析，得各区间位置均值距离待匹配区域圆心的距离通过与匹配模板进行对比，建立待匹配区域的评价函数：

其中，F_ik表示第i个航迹序列点在第k次迭代过程中位置分布特征的评价值，为第k次迭代中第j个区间的权重系数，k表示第k次迭代过程。

作为本发明进一步改进，在迭代过程中，若满足(F_ik＜F_ik-1||F_ik＜α)，则选择等深值点作为最优匹配点，由于存在测量误差，待匹配区域的评价值不为0，因此对其设置阈值α，若F_ik＜α，则认为第i个指示位置序列点找到了其全局最优匹配点，不再对其进行搜索评价。依次对每个指示航迹每个点进行分析，求得最优匹配序列{Y_i：(i＝1，2，…，N)}，其中，Y_i为最优匹配序列中第i个匹配点。

作为本发明进一步改进，步骤6中算法停止迭代条件为：

(max(F_k)＜ε&|T_k-T_k-1|＜τ)||k＞N_p

其中，ε，τ为算法终止条件判断阈值，应结合实际应用中水下数字地形图、匹配精度需求等因素设定；k为迭代次数，N_p为最大迭代次数。

本发明与现有技术相比的优点在于：

依据本申请提供的改进ICCP匹配算法，可解决在大的初始位置误差下ICCP匹配精度低及误匹配的问题，提高算法匹配精度与可靠性。传统ICCP算法易出现误匹配的原因在于大初始误差下，以距离最近为准则提取等深点方法不再适用，导致等深点的提取产生误匹配而局部收敛，使得ICCP方法匹配失败。为提高算法的可靠性，本申请提供的方法对等深点的提取方法进行改进，充分利用多波束测深仪测量数据，根据所申请方法对依次对等深值点进行评价，按照评价函数逐渐递减的原则选取等深值点进行匹配，保证改进后算法的全局收敛性。

附图说明

图1为改进后ICCP算法流程图；

图2为改进前后算法仿真结果图；

图3为改进前后算法匹配误差图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明提供适用于大初始误差下的改进ICCP地形匹配方法。针对传统ICCP方法在大初始误差下精度低、误匹配及局部收敛的问题，基于多波束测深系统测量特点对等值点提取方式进行改进，提出一种适用于大初始误差下的改进ICCP地形匹配法，提高其匹配精度与可靠性。

地形辅助导航系统的核心是匹配算法。传统ICCP算法是通过不断寻找变换(选择和平移)使匹配轨迹逐渐逼近其对应等值线，最后以满足一定迭代条件的点作为测量点的修正，从而达到降低导航误差的目的。ICCP算法克服了SITAN算法对地形线性化处理与TERCOM算法对航向误差敏感的缺点，是一种高效、高精度的方法，然而，其对惯性导航指示航迹的精度要求较高。改进后的ICCP方法解决了大初始误差下误匹配的问题，算法流程如图1所示，实现方法的具体步骤如下：

1)载体航迹序列与高程数据获取

当水下航行器驶入地形匹配区域后，获取由SINS测量得到的载体位置指示序列{P_i(x_i，y_i)：(i＝1，2，…，N)}，其中(x_i，y_i)为SINS指示位置P_i的水平坐标；与此同时，基于多波束的水深测量单元测得实际航迹序列下的地形高程信息C_i(X_i，Y_i，H_i)(i＝1，2，…，N)，其中C_i表示第i个航迹序列点周围的地形信息量，包含实时测量的地形高程值H_i与其对应的位置坐标(X_i，Y_i)，均为m＊n的矩阵，m表示有m个声脉冲组合，n表示每个声脉冲下有n个测深点。其中，位置坐标基于惯性导航系统指示位置进行推算。

2)航迹序列初始变换

采用随机旋转和平移的方法对SINS测量的航迹序列进行初始变换后得航迹序列{P′_i(x_i，y_i)：(i＝1，2，…，N)}，旋转和平移的大小在SINS系统误差方差的3倍范围(3σ)内随意取值，取旋转偏移量为θ_rand，水平面内位移偏移量为(x_rand，y_rand)。

3)水深等值线提取

水下航行器航行过程中会实时地测量水深，根据测得的水深在水下数字地形图中提取等深线是ICCP算法中寻找匹配点的前提条件。将指示航迹序列投影于水下数字地形图，根据测量得到的水深值在指示航迹周围提取等深线。本申请利用双向线性内插法来建立等深线，利用双向线性插值法可以计算得到网格内任意位置的水深值，且其值是由网格的4个顶点的水深值确定。

4)寻找最优匹配点

ICCP算法收敛的关键在于等深值点的正确提取，当存在较大初始位置误差时，由于地形可能存在相似性，导致依据距离最近准则选取的等深值点存在误匹配，使得ICCP局部收敛。随着多波束测深系统的发展，水下航行器可以更有效率地得到精度更高的海底实时地形分布，为地形匹配方法提供了更多的信息，因此，本申请基于多波束测深系统提供的丰富的地形信息来寻找最优匹配点，提高大初始误差下ICCP算法的稳定性。

依据上一步中提取的等深值点的坐标位置与多波束测量特性，在数字地形图中提取与实时扫测的地形图范围相同的待匹配区域，通过对比两个地形区域的相似性，选取最优匹配点。为降低算法对航向误差的敏感性，取实时扫测地形图的内切圆作为匹配模板。首先依据实时扫测地形图中的高程数据特征划分统计区间：

{[H_min，H_min+ΔH)，…，[H_max-ΔH，H_max)}

其中，H_min为当前实测区域内最小高程值，H_max为当前实测区域内最大高程值，ΔH为地形高程的分类间隔，根据实时地形特征分布与多波束测深系统误差特性选取。

依据上式对实测地形信息C_i(X_i，Y_i，H_i)按照地形高程值进行分类统计，记录各个区间内测深点的个数g_j(j∈[0，N])，记区间内测深点大于5的为有效区间，其中N为有效区间的个数。利用高斯概率分布表示各有效区间内位置点的分布特征，水平位置的方差和标准差计算如下式所示。

其中，μ_xj，μ_yj分别为第j个区间内测深点均值，σ_xj，σ_yj分别为第j个区间内测深点标准差，M为第j个区间内所有测深点的总数。

由于惯性系统存在航向误差，各区间的水平位置均值坐标不具备旋转不变性，可通过建立各区间内测深点位置分布特征量来分析此地形匹配模板的地形信息分布特征：

其中，L_j为第j个区间内测深点位置分布特征量，由计算公式可看出L_j为各区间中测深点位置均值与匹配模板圆心之间的距离；(x_ci，y_ci)为第i个指示位置序列点对应地形匹配模板圆心的水平位置坐标。

其次，以上一步中提取的等值点为圆心提取与匹配模板相同大小的地形作为待匹配区域，对其进行分析与评价，确定是否为最优匹配点。首先，对该地形区域按照地形高程值进行分类与分析，可得各区间位置均值距离待匹配区域圆心的距离通过与匹配模板进行对比，建立待匹配区域的评价函数：

其中，F_ik表示第i个航迹序列点在第k次迭代过程中位置分布特征的评价值，为第k次迭代中第j个区间的权重系数，k表示第k次迭代过程。

理想情况下，当待匹配区域为最优匹配点时，评价函数为0，由于高程测量误差、水下数字地形图误差及匹配点选取误差等存在，使得评价函数不为0，因此，评价函数越小表示两个地形图匹配程度越高。在每次寻找最优匹配点的过程中，等深值点的选取按照当前指示位置点由近及远进行提取与评价。为保证算法的收敛性，在迭代过程中，若满足(F_ik＜F_ik-1||F_ik＜α)，则选择等深值点作为最优匹配点，由于存在测量误差，待匹配区域的评价值不为0，因此对其设置阈值α，若F_ik＜α，则认为第i个序列点找到了其全局最优匹配点，在下次迭代中不再对此序列点进行搜索匹配。依次对每个指示航迹每个点进行分析，求得最优匹配序列{Y_i：(i＝1，2，…，N)}，其中，Y_i为最优匹配序列中第i个匹配点。

5)求取刚性变换

惯性导航系统误差虽然随时间累积，但其在短期内测量精度较高，可利用此特性对相邻航迹点之间的相对位置进行约束，因此，可视为最优匹配航迹为指示航迹的刚性变换。依据最优匹配点序列{Y_i：(i＝1，2，…，N)}与刚性变换后航迹序列{TP′_i：(i＝1，2，…，N)}之间欧式距离最小原则，建立如下目标函数求取刚性变换T：

其中，D是目标函数，ω_i是各航迹序列点对应的权值，d_i为匹配序列Y_i与P′_i中第i个匹配点的距离，T是待求的刚性变换，包括旋转矩阵R与平移矩阵t，θ表示刚性旋转角度，t_x，t_y为水平位置平移量。采用四元数法求解，步骤如下。

令：

其中，分别为匹配序列Y_i与P′_i质心，/>为权值归一化值。

可计算得：

其中，B和W为求解过程构造的矩阵，B_xx，B_xy，B_yx，B_yy分别为矩阵B中各元素。

解得W的四个特征值λ₁，λ₂，λ₃，λ₄为：

旋转变换矩阵四元数q表示为：

q＝q₀+q₁i+q₂j+q₃k

其中，q₀，q₁，q₂，q₃为实数，i，j，k为虚数单位。

记最大特征值为λ_max＝max(λ₁，λ₂，λ₃，λ₄)，则有：

从而可计算得旋转矩阵R和平移向量t：

对集合{P′_i}应用刚性变换T，则有TP′＝RP′+t。

6)算法终止判断

将上一步计算得TP′作为新的指示航迹序列转至第3步进行下一次迭代，直至算法收敛。为确保算法收敛条件的准确性，可根据最优匹配点的评价函数值、刚性变换T的变化量设计收敛条件。

(max(F_k)＜ε&|T_k-T_k-1|＜τ)||k＞N_p

其中，ε，τ为算法终止条件判断阈值，应结合实际应用中水下数字地形图、匹配精度需求等因素设定；k为迭代次数，N_p为最大迭代次数。

经过收敛迭代后获得的集合TP′即为最终的匹配航迹。

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

为了验证本发明的有效性，使用MATLAB平台对该算法进行仿真验证。地形图采用某海域实测地形图，地形范围为300m＊400m，最小间距为1m＊1m，水深在30m～40m之间。载体航向轨迹仿真参数与惯性导航仿真参数设置如表1所示。轨迹匹配序列中位置采样点为15，采样步长为20m。为保证传统ICCP算法收敛，最大迭代次数设置为150；改进后ICCP算法仿真参数如表2所示。

表1仿真条件参数设置

表2改进后ICCP算法参数设置

实验结果如图2与图3所示。图2为改进前后ICCP算法仿真结果，图中★表示真实航迹，●表示INS测量轨迹，▲表示传统ICCP方法匹配轨迹，■表示改进后ICCP方法匹配轨迹，从图中可以看出，由于初始误差较大，ICCP方法陷入局部收敛，定位误差均值为20.68m，导致匹配任务失败，而改进后的方法利用多波束测深系统测量数据的冗余性，对等深值点进行分析评价，避免误匹配导致的局部收敛，定位误差均值为1.38m，实现载体位置的匹配。

以上所述，仅是本发明的较佳实施例之一，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (3)

1.一种大初始定位误差下的改进ICCP地形匹配方法，其特征在于，具体步骤如下：

步骤1：载体航迹序列与高程数据获取：当水下航行器驶入地形匹配区域后，获取由SINS测量得到的载体位置指示序列{P_i(x_i,y_i):(i＝1,2,…,N)}，其中(x_i,y_i)为第i个SINS指示位置P_i的坐标，N为载体位置指示序列的个数；与此同时，基于多波束测深系统测得实际航迹序列下的地形高程信息C_i(X_i,Y_i,H_i)(i＝1,2,…,N)，其中C_i表示第i个航迹序列点周围的地形信息量，包含实时测量的地形高程值H_i与其对应的位置坐标(X_i,Y_i)，(X_i,Y_i,H_i)均为m*n的矩阵，m表示有m个声脉冲组合，n表示每个声脉冲下有n个测深点，其中，位置坐标基于惯性导航系统指示位置进行推算；

步骤2：航迹序列初始变换：采用随机旋转和平移的方法对SINS测量的航迹序列进行初始变换得新的{P'_i(x_i,y_i):(i＝1,2,…,N)}，其中P'_i为新的航迹指示序列中第i个序列点，代替SINS测量得到的载体位置指示序列进行后续迭代计算，旋转和平移的大小在SINS系统误差方差的3倍范围内随意取值，取旋转偏移量为θ_rand，水平面内位移偏移量为(x_rand,y_rand)；

步骤3：水深等值线提取：根据新的航迹指示序列与其对应的地形高程序列在水下数字地形图中进行等深线提取；

步骤4：寻找最优匹配点：与传统ICCP匹配方法不同的是，不再以距离航迹指示序列点最短作为匹配点选取准则，充分利用多波束数据的冗余性，对数字地图上提取的各个等深值点周围的地形进行提取评价，选取最优匹配点序列其中，Y_i为第i个最优匹配序列点，其位置坐标为/>

步骤4中将多波束测量区域取内切圆作为匹配模板，同时，基于等值点在水下数字地形图中提取半径相同的圆形区域作为待匹配模板，进行相似性分析；

步骤4中对当前变换后的航迹序列点附近提取的各个等深值点依次进行评价，等深值点按照当前指示位置点由近及远进行提取；

步骤4中最优匹配点的选取方法为通过对地形匹配模板与待匹配地形区域的地形分布特征进行分析对比，选择匹配程度高的待匹配区域作为最优匹配点，匹配相似性算法如下：

首先依据实时扫测地形图中的高程数据特征划分统计区间：

{[H_min,H_min+ΔH),…,[H_max-ΔH,H_max)}

其中，H_min为当前实测区域内最小高程值，H_max为当前实测区域内最大高程值，ΔH为地形高程的分类间隔，根据实时地形特征分布与多波束测深系统误差特性选取；

依据上式对实测地形信息C_i(X_i,Y_i,H_i)按照地形高程值进行分类统计，记录各个区间内测深点的个数g_j,j∈[0,d]，记区间内测深点大于5的为有效区间，其中d为有效区间的个数，利用高斯概率分布表示各有效区间内位置点的分布特征，计算水平位置的均值计算如下式所示；

其中，μ_xj,μ_yj分别为第j个区间内测深点均值，g_j为第j个区间内所有测深点的总数；

由于惯性系统存在航向误差，各区间的水平位置均值坐标不具备旋转不变性，通过建立各区间内测深点位置分布特征量来分析此地形匹配模板的地形信息分布特征：

其中，L_j为第j个区间内测深点位置分布特征量，由计算公式可看出L_j为各区间中测深点位置均值与匹配模板圆心之间的距离；(x_ci,y_ci)为第i个指示位置序列点对应地形匹配模板圆心的水平位置坐标；

其次，以上一步中提取的等值点为圆心提取与匹配模板相同大小的圆形区域作为待匹配区域，对其进行分析与评价，确定是否为最优匹配点，首先，对该地形区域地形高程值进行分类与分析，得各区间位置均值距离待匹配区域圆心的距离通过与匹配模板进行对比，建立待匹配区域的评价函数：

其中，F_ik表示第i个航迹序列点在第k次迭代过程中位置分布特征的评价值，为第k次迭代中第j个区间的权重系数，k表示第k次迭代过程；

步骤5：求取刚性变换：依据最优匹配点序列与刚性变换后航迹指示序列/>之间欧式距离最小原则，求取刚性变换T；

步骤6：重复迭代直到算法收敛：将TP'作为新的起始航迹集合P'转至步骤3进行下一次迭代，直至收敛，经过收敛迭代后获得的集合TP'即为最终的匹配航迹。

2.根据权利要求1所述的一种大初始定位误差下的改进ICCP地形匹配方法，其特征在于：在迭代过程中，若满足(F_ik＜F_ik-1||F_ik＜α)，则选择等深值点作为最优匹配点，由于存在测量误差，待匹配区域的评价值不为0，因此对其设置阈值α，若F_ik＜α，则认为第i个指示位置序列点找到了其全局最优匹配点，不再对其进行搜索评价，依次对每个指示航迹每个点进行分析，求得最优匹配序列{Y_i:(i＝1,2,…,N)}，其中，Y_i为最优匹配序列中第i个匹配点。

3.根据权利要求1所述的一种大初始定位误差下的改进ICCP地形匹配方法，其特征在于：步骤6中算法停止迭代条件为：

(max(F_k)＜ε&|T_k-T_k-1|＜τ)||k＞N_p

其中，ε,τ为算法终止条件判断阈值，应结合实际应用中水下数字地形图、匹配精度需求等因素设定；k为迭代次数，N_p为最大迭代次数。