[发明专利]一种基于磁信标的同步定位与构图系统的故障恢复方法

说明书

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

技术领域

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

背景技术

导航技术是自主水下航行器实际应用中的一项关键技术。传统的导航方法有卫星导航、水声定位和惯性导航技术技术等，但水介质对无线电波有较强的吸收，水声定位需要在预定工作海域布设导航基阵，只适用于局部作业且成本较高，惯性导航误差随时间积累，无法保证结果的收敛。基于地球物理场的匹配导航技术稳定性强、自主性高，可以实现位置的收敛，但需要以高精度、高分辨率的先验地图为基础，受地球物理场空间特征的丰富性以及数据来源的限制。而同步定位与构图方法(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进一步包括：