CN113124881B - 一种基于磁信标的同步定位与构图系统的故障恢复方法 - Google Patents

Abstract

本发明公开了一种基于磁信标的同步定位与构图系统的故障恢复方法：（1）航行器在SLAM模式下运动，并建立磁场图和磁信标位置图；（2）航行器丢失自身位置后再次从一个未知位置开始继续工作，航行器利用接下来运动过程中测得的磁场信息，与（1）中建立的磁场图进行匹配，估计航行器的大致位置；（3）航行器利用观测的磁场信息进行磁信标反演定位，并对反演所得信标组合与（1）中得到的磁信标位置图进行匹配，以修正航行器位置；（4）将修正结果作为航行器的初始位置，基于磁信标的SLAM系统继续工作。本发明能够有效应对SLAM系统的故障状况，准确初始化航行器位置，大大提高了实际应用中基于磁信标的同步定位与构图方法面对潜在故障的鲁棒性。

Description

一种基于磁信标的同步定位与构图系统的故障恢复方法

技术领域

本发明涉及同步定位与构图系统，特别涉及一种基于磁信标的同步定位与构图系统的故障恢复方法。

背景技术

导航技术是自主水下航行器实际应用中的一项关键技术。传统的导航方法有卫星导航、水声定位和惯性导航技术技术等，但水介质对无线电波有较强的吸收，水声定位需要在预定工作海域布设导航基阵，只适用于局部作业且成本较高，惯性导航误差随时间积累，无法保证结果的收敛。基于地球物理场的匹配导航技术稳定性强、自主性高，可以实现位置的收敛，但需要以高精度、高分辨率的先验地图为基础，受地球物理场空间特征的丰富性以及数据来源的限制。而同步定位与构图方法(simultaneous localization and mapping,SLAM)无需先验环境信息，在导航过程中，利用实时观测信息辅助导航定位，提高系统精度，是弥补地球物理场先验信息不足的有效手段。

基于磁信标的同步定位与构图技术是一种新型导航技术，它以导航环境中随机布置的磁信标为导航参照，通过在航行过程中实时测量磁场信息，准确反演自身与磁信标之间的相对位置并作为观测量，实现SLAM系统状态的解算。但是当航行器在此过程中由于软件、硬件等故障原因，或外界干扰下，临时丢失自己的位置，对磁信标的观测不再连续，导致SLAM系统中断后，导航系统需要具备在线重新获取自身位置的能力，并继续开展SLAM估计。但目前并没有相关研究成果解决此问题。

因此本发明提出一种基于磁信标的同步定位与构图系统的故障恢复方法，面向航行器临时丢失自身位置的场景，用匹配方法代替中断的SLAM模式以重新捕获自身位置，并通过对匹配定位结果进行校准，实现SLAM系统的恢复运行。该方法能够有效应对基于磁信标的SLAM系统的故障状况，在全局地图下准确初始化航行器位置，大大提高了基于磁信标的同步定位与构图方法在实际应用中针对潜在故障的鲁棒性。

发明内容

本发明的目的是面向基于磁信标的SLAM系统突然丢失自身位置的场景，提供一种基于磁信标的同步定位与构图系统的故障恢复方法。在航行器轨迹于SLAM模式下充分覆盖过工作区域后，对工作区域进行磁场建模，并用磁场匹配方法初始化航行器的位置，进而通过基于磁源反演的位置校准，实现SLAM系统故障恢复。

本发明所采用的技术方案是：一种基于磁信标的同步定位与构图系统的故障恢复方法，包括以下步骤：

步骤1，在随机分布了磁信标的工作区域内，航行器在SLAM模式下运动，并且，航行器的航行轨迹覆盖整个工作区域，利用航行器在SLAM模式下采集的数据集建立磁场图和磁信标位置图；

步骤2，由于SLAM系统故障或外界干扰，航行器丢失自身位置；而后系统从故障中恢复，需从一个未知位置开始继续工作；航行器利用接下来运动过程中测得的磁场信息，与步骤1中建立的磁场图进行匹配，估计航行器的大致位置；

步骤3，航行器利用观测的磁场信息进行磁信标反演定位，并使用迭代最近点方法对反演所得信标组合与步骤1中得到的磁信标位置图进行匹配，以修正航行器位置，完成对航行器位置的在线初始化；

步骤4，将迭代最近点方法的修正结果作为航行器的初始位置，基于磁信标的SLAM系统继续工作。

其中，步骤1进一步包括：

航行器在工作区域内运动，导航系统工作在SLAM模式，如下：

选取航行轨迹上的N_m个航迹点作为测点序列

其中，P_i表示第i个测点，N_m表示测点序列中包含的航迹点总数，N_m≥15；基于测量得到的测点序列上的磁场序列，利用非线性最小二乘方法实时解算磁信标的相对位置；设磁信标置于笛卡儿坐标系的原点，则该磁信标在任一测点P_i(x,y,z)处产生的磁场强度为B_i＝[B_x,i B_y,iB_z,i]，数学模型如式(1)所示：

式中，r为测点P_i与该磁信标的距离，r²＝x²+y²+z²；B_x,i,B_y,i和B_z,i分别为该磁信标在第i个测点处产生的磁场强度在x、y、z方向的分量；[m_x,im_y,im_z,i]^T为磁信标的三维磁矩，μ为介质磁导率；如果一个区域内存在R个磁信标，则某一测点测得的磁场矢量

即为所有单个磁信标在该测点产生的磁场信息之和，即

其中

为第r个磁信标在第i个测点处产生的磁场强度，R>1；

以航行器测得的磁场强度作为已知量，以式(2)为目标函数，采用非线性优化算法估计磁信标的相对位置：

式中，

为包含所有待解算参数的未知向量；B_R为实测的磁场矢量数据序列，如式(3)所示；B_b为由多项式计算而得的背景磁场矢量值，如式(4)所示；

为根据磁信标参数算得的磁场矢量估计值，如式(5)所示；

式中，

为第i个测点处实测的磁场矢量序列，且

其中，

分别为实测磁场在第i个测点处x、y、z方向的分量

式中，

为第i个测点处估计的背景磁场矢量序列，且

其中，

分别为第i个测点处背景磁场在x、y、z方向上的分量；

式中，

表示所有可能存在的磁信标磁源在第i个测点上产生的磁场矢量，

其中，

分别表示第k个可能存在的磁信标磁源在第i个测点处x、y、z方向上产生的磁场矢量，1≤k≤K，K表示可能存在的磁信标磁源数量；f(c_k)为对应第k个可能存在的磁信标磁源的待定系数函数，f(c_k)的表达式如式(6)所示：

式中，c_k为有效性判定系数中的待定系数参数，为唯一变量，取值范围为(-∞，+∞)；f(c_k)的函数值范围为(0，1)；

如果B_b可以预先测量或使用现有模型计算，则在优化时只需对磁信标进行估计，否则，将背景磁场视为未知，并与磁信标的位置一起估计，此时，用多项式对连续测点序列进行背景磁场建模，如式(7)所示：

式中，

表示背景磁场信息B_b在第i个测点处的某个分量；q是多项式的阶数，q≥2；a₀至a_q为多项式中待求的未知系数；

基于建立的目标函数，进行磁信标反演的初始化，初始向量为：

式中，X_ini为初始向量，其中，

表示第k个可能存在的磁信标的位置，

表示第k个可能存在的磁信标的三维磁矩，c_ini,k表示第k个可能存在的磁信标对应的待定系数参数，并且，

和

在设定范围内随机取值；c_ini,k取值为10，即认为所有磁信标在初始时刻默认都存在；a＝[a₀ … a_q]表示该段航行轨迹对应的背景场多项式系数，a₀至a_q的初始值随机赋值；

利用非线性最小二乘方法对所建立的目标函数

进行反演解算，对优化结果当中对应判定系数值大于有效阈值的磁信标认为是真实存在的磁信标，也就是SLAM工作过程中观测到的路标，将这些磁信标相对于航行器的位置作为观测量，进行SLAM解算；

随着航行器的运动，航行器的航行轨迹实现了对整个工作区域的覆盖，所有磁信标均被观测到，制作并保存包含所有磁信标的磁信标位置图

如式(9)所示：

式中，X_l＝[X_l,x X_l,y X_l,z]表示第l个被探测到的磁信标的三维位置信息，L为航行器工作区域内被探测到的磁信标总数；

根据航行器在每个航迹点出测得的磁场信息，结合对应的航行器位置，利用改进谢别德法构建工作区域的磁场图

其中，步骤2进一步包括：

航行器在外力作用下突然移动到另一个位置，或SLAM从系统临时故障中恢复，航行器SLAM系统必须在一个未知的点重新启动；

首先，航行器从一个未知的点开始以纯推位模式运行，在N个连续测点上收集一组磁场矢量数据集M，如式(10)所示：

M＝[B′₁…B′_j…B′_N]，1≤j≤N，N≥N_m (10)

式中，B′_j为SLAM系统重新启动后第j个测点处实测的磁场矢量序列，且B′_j＝[B′_x,jB′_y,j B′_z,j]，其中，B′_x,j、B′y,_j、B′_z,j分别为SLAM系统重新启动后实测磁场在第j个测点处x、y、z方向的分量；

执行磁场轮廓匹配算法，将对M在

上进行匹配，得到该段纯推位航迹在工作区域全局地图上的大致位置，将最后一个航迹点作为匹配结果，即为航行器当前的估计位置C_M。

其中，步骤3进一步包括：

为了避免错误的匹配结果，为航行器提供一个更为准确的初始化位置信息，令步骤2推位阶段中，每N_m个点组成一个测点序列，并利用步骤1中所示的磁信标反演定位方法，得到该推位航迹周围的磁信标分布情况

如式(11)：

式中，B_q表示该推位段上所探测到的第q个磁信标的三维坐标，Q为该推位段上所探测到的所有磁信标的数量；

使用迭代最近点方法对

与

进行匹配，利用迭代最近点方法匹配结果中的旋转矩阵和平移向量对航位推算路径进行修正，将修正后的航行器位置记为C_I，作为航行器恢复SLAM系统时的初始位置。

本发明的有益效果是：本发明提出了一种基于磁信标的同步定位与构图系统的故障恢复方法。面对SLAM系统的临时失效或由于外界因素导致航行器位置突然丢失的情况，本方法提出工作区域被航行器在SLAM模式下实现航迹覆盖的情形下，利用匹配定位方法快速实现位置初始化，并结合磁信标反演定位，利用ICP方法实现初始位置校准，进而实现SLAM系统恢复。该方法能够有效应对SLAM系统的故障状况，准确初始化航行器位置，大大提高了实际应用中基于磁信标的同步定位与构图方法面对潜在故障的鲁棒性。

附图说明

图1为本发明一种基于磁信标的同步定位与构图系统的故障恢复方法流程图；

图2为应用本发明的航行器故障恢复中的初始位置校正示意图。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹例举以下实施例，并配合附图详细说明如下：

本发明为基于磁信标探测的SLAM系统提供了一种有效的系统故障恢复方法。通过测量航行器航迹点上的磁场信息，利用多项式对背景磁场建模，并进行联合估计，可以实现对未知背景磁场下的磁信标的精确检测，以及对整个工作区域内磁场分布情况的准确建模。该方法主要应用于航行器的工作区域被SLAM路径基本覆盖后，SLAM系统失效或航行器丢失自身位置信息的情况。根据航行器测得的磁场信息，可以用匹配方法代替SLAM系统或匹配得到准确的航行器初始位置，以重新启动SLAM工作模式。

如图1所示，一种基于磁信标的同步定位与构图系统的故障恢复方法，包括以下步骤：

步骤1，在随机分布了磁信标的工作区域内，航行器在SLAM模式下运动，并且，航行器的航行轨迹覆盖整个工作区域，利用航行器在SLAM模式下采集的数据集建立磁场图和路标(磁信标)位置图。具体包括：

航行器在工作区域内运动，导航系统工作在SLAM模式，如下：

选取航行轨迹上的N_m个航迹点作为测点序列

其中，P_i表示第i个测点，N_m表示测点序列中包含的航迹点总数，N_m≥15；基于测量得到的测点序列上的磁场序列，利用非线性最小二乘方法实时解算磁信标的相对位置；设磁信标置于笛卡儿坐标系的原点，则该磁信标在任一测点P_i(x,y,z)处产生的磁场强度为B_i＝[B_x,iB_y,i B_z,i]，数学模型如式(1)所示：

式中，r为测点P_i与该磁信标的距离，r²＝x²+y²+z²；B_x,i,B_y,i和B_z,i分别为磁场强度在x、y、z方向的分量；[m_x,i m_y,i m_z,i]^T为磁信标的三维磁矩，μ为介质磁导率；如果一个区域内存在R(R>1)个磁信标，则某一测点测得的磁场矢量

即为所有单个磁信标在该测点产生的磁场信息之和，即

其中

为第r个磁信标在第i个测点处产生的磁场强度。

以航行器测得的磁场强度作为已知量，以式(2)为目标函数，采用非线性优化算法估计磁信标的相对位置。

式中，

为包含所有待解算参数的未知向量；B_R为实测的磁场矢量数据序列，如式(3)所示；B_b为由多项式计算而得的背景磁场矢量值，如式(4)所示；

为根据磁信标参数算得的磁场矢量估计值，如式(5)所示；

式中，

为第i个测点处实测的磁场矢量序列，且

其中，

分别为实测磁场在第i个测点处x、y、z方向的分量；

式中，

为第i个测点处估计的背景磁场矢量序列，且

其中，

分别为第i个测点处背景磁场在x、y、z方向上的分量；

式中，

表示所有可能存在的磁信标磁源在第i个测点上产生的磁场矢量，

其中，

分别表示第k个可能存在的磁信标磁源在第i个测点处x、y、z方向上产生的磁场矢量，1≤k≤K，K表示可能存在的磁信标磁源数量；f(c_k)为对应第k个可能存在的磁信标磁源的待定系数函数，f(c_k)的表达式如式(6)所示：

式中，c_k为有效性判定系数中的待定系数参数，为唯一变量，取值范围为(-∞，+∞)；f(c_k)的函数值范围为(0，1)。

如果B_b可以预先测量或使用现有模型计算，则在优化时只需对磁信标进行估计，否则，将背景磁场视为未知，并与磁信标的位置一起估计，此时，用多项式对连续测点序列进行背景磁场建模，如式(7)所示：

式中，

表示背景磁场信息B_b在第i个测点处的某个分量；q是多项式的阶数，q≥2；a₀至a_q为多项式中待求的未知系数，将在之后的优化过程中，与磁源信息被一同解算得到。

基于建立的目标函数，进行磁信标反演的初始化，初始向量为：

式中，X_ini为初始向量，其中，

表示第k个可能存在的磁信标的位置，

表示第k个可能存在的磁信标的三维磁矩，c_ini,k表示第k个可能存在的磁信标对应的待定系数参数，并且，

和

在设定范围内随机取值；c_ini,k取值为10，即认为所有磁信标在初始时刻默认都存在；a＝[a₀ … a_q]表示该段航行轨迹对应的背景场多项式系数，a₀至a_q的初始值随机赋值；

利用非线性最小二乘方法对所建立的目标函数

进行反演解算，对优化结果当中对应判定系数值大于有效阈值(应设置为大于0.5，且，小于1)的磁信标认为是真实存在的磁信标，也就是SLAM工作过程中观测到的路标，将这些磁信标相对于航行器的位置作为观测量，进行SLAM解算。

随着航行器的运动，航行器的航行轨迹实现了对整个工作区域的覆盖，所有磁信标均被观测到，制作并保存包含所有磁信标的磁信标位置图

如式(9)所示：

式中，X_l＝[X_l,x X_l,y X_l,z]表示第l个被探测到的磁信标的三维位置信息，L为航行器工作区域内被探测到的磁信标总数。

根据航行器在每个航迹点出测得的磁场信息，结合对应的航行器位置，利用改进谢别德法构建工作区域的磁场图

步骤2，由于SLAM系统故障或外界干扰，航行器丢失自身位置；而后系统从故障中恢复，需从一个未知位置开始继续工作；航行器利用接下来运动过程中测得的磁场信息，与步骤1中建立的磁场图进行匹配，估计航行器的大致位置。具体包括：

航行器在外力作用下突然移动到另一个位置，或SLAM从系统临时故障中恢复，航行器SLAM系统必须在一个未知的点重新启动。

首先，航行器从一个未知的点开始以纯推位模式运行，在N个连续测点上收集一组磁场矢量数据集M，如式(10)所示：

M＝[B′₁…B′_j…B′_N]，1≤j≤N，N≥N_m (10)

式中，B′_j为SLAM系统重新启动后第j个测点处实测的磁场矢量序列，且B′_j＝[B′_x,jB′_y,j B′_z,j]，其中，B′_x,j、B′_y,j、B′_z,j分别为SLAM系统重新启动后实测磁场在第j个测点处x、y、z方向的分量；

执行磁场轮廓匹配算法(MAGCOM)，将对M在

上进行匹配，得到该段纯推位航迹在工作区域全局地图上的大致位置，将最后一个航迹点作为匹配结果，即为航行器当前的估计位置C_M。

步骤3，航行器利用观测的磁场信息进行磁信标反演定位，并使用迭代最近点方法对反演所得信标组合与步骤1中得到的磁信标位置图进行匹配，以修正航行器位置，完成对航行器位置的在线初始化。具体包括：

为了避免错误的匹配结果，为航行器提供一个更为准确的初始化位置信息，令步骤2推位阶段中，每N_m个点组成一个测点序列，并利用步骤1中所示的磁信标反演定位方法，得到该推位航迹周围的磁信标分布情况

如式(11)：

式中，B_q表示该推位段上所探测到的第q个磁信标的三维坐标，Q为该推位段上所探测到的所有磁信标的数量；

使用迭代最近点(ICP)方法对

与

进行匹配，利用ICP匹配结果中的旋转矩阵和平移向量对航位推算路径进行修正，将修正后的航行器位置记为C_I，作为航行器恢复SLAM系统时的初始位置。

步骤4，将ICP匹配结果作为航行器的初始位置，基于磁信标的SLAM系统继续工作。

图2为应用本发明的航行器初始位置校正示意图。当航行器通过磁场轮廓匹配算法(MAGCOM)在全局地图中找到自身的概略位置后，为了能够顺利重新启动SLAM系统，需要对其中包含的匹配误差进行精细校准。利用本发明介绍的磁信标探测方法，反演得到每段测点序列周围的磁信标分布情况，使用迭代最近点(ICP)方法对将其与地标图进行匹配，利用匹配结果中的旋转矩阵和平移向量，能够有效地将匹配位置中的误差进行修正，并作为航行器SLAM系统恢复后的初始坐标。

尽管上面结合附图对本发明的优选实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，并不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可以做出很多形式，这些均属于本发明的保护范围之内。

Claims (3)

1.一种基于磁信标的同步定位与构图系统的故障恢复方法，其特征在于，包括以下步骤：

步骤1，在随机分布了磁信标的工作区域内，航行器在SLAM模式下运动，并且，航行器的航行轨迹覆盖整个工作区域，利用航行器在SLAM模式下采集的数据集建立磁场图和磁信标位置图，包括：

航行器在工作区域内运动，导航系统工作在SLAM模式，如下：

选取航行轨迹上的N_m个航迹点作为测点序列

其中，P_i表示第i个测点，N_m表示测点序列中包含的航迹点总数，N_m≥15；基于测量得到的测点序列上的磁场序列，利用非线性最小二乘方法实时解算磁信标的相对位置；设磁信标置于笛卡儿坐标系的原点，则该磁信标在任一测点P_i(x，y，z)处产生的磁场强度为B_i＝[B_x，iB_y，i B_z，i]，数学模型如式(1)所示：

式中，r为测点P_i与该磁信标的距离，r²＝x²+y²+z²；B_x，i，B_y，i和B_z，i分别为该磁信标在第i个测点处产生的磁场强度在x、y、z方向的分量；[m_x，i m_y，i m_z，i]^T为磁信标的三维磁矩，μ为介质磁导率；如果一个区域内存在R个磁信标，则某一测点测得的磁场矢量

即为所有单个磁信标在该测点产生的磁场信息之和，即

其中

为第r个磁信标在第i个测点处产生的磁场强度，R＞1；

以航行器测得的磁场强度作为已知量，以式(2)为目标函数，采用非线性优化算法估计磁信标的相对位置：

式中，

为包含所有待解算参数的未知向量；B_R为实测的磁场矢量数据序列，如式(3)所示；B_b为由多项式计算而得的背景磁场矢量值，如式(4)所示；

为根据磁信标参数算得的磁场矢量估计值，如式(5)所示；

式中，

为第i个测点处实测的磁场矢量序列，且

1≤i≤N_m，其中，

分别为实测磁场在第i个测点处x、y、z方向的分量；

式中，

为第i个测点处估计的背景磁场矢量序列，且

其中，

分别为第i个测点处背景磁场在x、y、z方向上的分量；

式中，

表示所有可能存在的磁信标磁源在第i个测点上产生的磁场矢量，

其中，

分别表示第k个可能存在的磁信标磁源在第i个测点处x、y、z方向上产生的磁场矢量，1≤k≤K，K表示可能存在的磁信标磁源数量，f(c_k)为对应第k个可能存在的磁信标磁源的待定系数函数，f(c_k)的表达式如式(6)所示

式中，c_k为有效性判定系数中的待定系数参数，为唯一变量，取值范围为(-∞，+∞)；f(c_k)的函数值范围为(0，1)；

如果B_b可以预先测量或使用现有模型计算，则在优化时只需对磁信标进行估计，否则，将背景磁场视为未知，并与磁信标的位置一起估计，此时，用多项式对连续测点序列进行背景磁场建模，如式(7)所示：

式中，

表示背景磁场信息B_b在第i个测点处的某个分量；q是多项式的阶数，q≥2；a₀至a_q为多项式中待求的未知系数；

基于建立的目标函数，进行磁信标反演的初始化，初始向量为：

式中，X_ini为初始向量，其中，

表示第k个可能存在的磁信标的位置，

表示第k个可能存在的磁信标的三维磁矩，c_ini，k表示第k个可能存在的磁信标对应的待定系数参数，并且，

和

在设定范围内随机取值；c_ini，k取值为10，即认为所有磁信标在初始时刻默认都存在；a＝[a₀ … a_q]表示该段航行轨迹对应的背景场多项式系数，a₀至a_q的初始值随机赋值；

利用非线性最小二乘方法对所建立的目标函数

进行反演解算，对优化结果当中对应判定系数值大于有效阈值的磁信标认为是真实存在的磁信标，也就是SLAM工作过程中观测到的路标，将这些磁信标相对于航行器的位置作为观测量，进行SLAM解算；

随着航行器的运动，航行器的航行轨迹实现了对整个工作区域的覆盖，所有磁信标均被观测到，制作并保存包含所有磁信标的磁信标位置图

如式(9)所示：

式中，X_l＝[X_l，x X_l，y X_l，z]表示第l个被探测到的磁信标的三维位置信息，L为航行器工作区域内被探测到的磁信标总数；

根据航行器在每个航迹点出测得的磁场信息，结合对应的航行器位置，利用改进谢别德法构建工作区域的磁场图

步骤2，由于SLAM系统故障或外界干扰，航行器丢失自身位置；而后系统从故障中恢复，需从一个未知位置开始继续工作；航行器利用接下来运动过程中测得的磁场信息，与步骤1中建立的磁场图进行匹配，估计航行器的大致位置；

步骤3，航行器利用观测的磁场信息进行磁信标反演定位，并使用迭代最近点方法对反演所得信标组合与步骤1中得到的磁信标位置图进行匹配，以修正航行器位置，完成对航行器位置的在线初始化；

步骤4，将迭代最近点方法的修正结果作为航行器的初始位置，基于磁信标的SLAM系统继续工作。

2.根据权利要求1所述的基于磁信标的同步定位与构图系统的故障恢复方法，其特征在于，步骤2进一步包括：

航行器在外力作用下突然移动到另一个位置，或SLAM从系统临时故障中恢复，航行器SLAM系统必须在一个未知的点重新启动；

首先，航行器从一个未知的点开始以纯推位模式运行，在N个连续测点上收集一组磁场矢量数据集M，如式(10)所示：

M＝[B′₁…B′_j…B′_N]，1≤j≤N，N≥N_m (10)

式中，B′_j为SLAM系统重新启动后第j个测点处实测的磁场矢量序列，且B′_j＝[B′_x，jB′_y，j B′_z，j]，其中，B′_x，j、B′_y，j、B′_z，j分别为SLAM系统重新启动后实测磁场在第j个测点处x、y、z方向的分量；

执行磁场轮廓匹配算法，将对M在

上进行匹配，得到该段纯推位航迹在工作区域全局地图上的大致位置，将最后一个航迹点作为匹配结果，即为航行器当前的估计位置C_M。

3.根据权利要求1所述的基于磁信标的同步定位与构图系统的故障恢复方法，其特征在于，步骤3进一步包括：

为了避免错误的匹配结果，为航行器提供一个更为准确的初始化位置信息，令步骤2推位阶段中，每N_m个点组成一个测点序列，并利用步骤1中建立磁场图和磁信标位置图时所采用的方法，得到该推位航迹周围的磁信标分布情况

如式(11)：

式中，B_q表示该推位段上所探测到的第q个磁信标的三维坐标，Q为该推位段上所探测到的所有磁信标的数量；

使用迭代最近点方法对

与

进行匹配，利用迭代最近点方法匹配结果中的旋转矩阵和平移向量对航位推算路径进行修正，将修正后的航行器位置记为C_I，作为航行器恢复SLAM系统时的初始位置。

Images