一种基于磁信标的同步定位与构图系统的故障恢复方法转让专利
申请号 : CN202110669283.9
文献号 : CN113124881B
文献日 : 2021-10-08
发明人 : 常帅 , 林叶 , 万程程
申请人 : 天津大学
摘要 :
权利要求 :
1.一种基于磁信标的同步定位与构图系统的故障恢复方法，其特征在于，包括以下步骤：
步骤1，在随机分布了磁信标的工作区域内，航行器在SLAM模式下运动，并且，航行器的航行轨迹覆盖整个工作区域，利用航行器在SLAM模式下采集的数据集建立磁场图和磁信标位置图，包括：
航行器在工作区域内运动，导航系统工作在SLAM模式，如下：选取航行轨迹上的Nm个航迹点作为测点序列其中，Pi表示第i个测点，Nm表示测点序列中包含的航迹点总数，Nm≥15；基于测量得到的测点序列上的磁场序列，利用非线性最小二乘方法实时解算磁信标的相对位置；设磁信标置于笛卡儿坐标系的原点，则该磁信标在任一测点Pi(x，y，z)处产生的磁场强度为Bi＝[Bx，i By，i Bz，i]，数学模型如式(1)所示：
2 2 2 2
式中，r为测点Pi与该磁信标的距离，r＝x+y +z；Bx，i，By，i和Bz，i分别为该磁信标在第iT
个测点处产生的磁场强度在x、y、z方向的分量；[mx，i my，i mz，i]为磁信标的三维磁矩，μ为介质磁导率；如果一个区域内存在R个磁信标，则某一测点测得的磁场矢量 即为所有单个磁信标在该测点产生的磁场信息之和，即 其中 为第r个磁信标在第i个测点处产生的磁场强度，R＞1；
以航行器测得的磁场强度作为已知量，以式(2)为目标函数，采用非线性优化算法估计磁信标的相对位置：
式中， 为包含所有待解算参数的未知向量；BR为实测的磁场矢量数据序列，如式(3)所示；Bb为由多项式计算而得的背景磁场矢量值，如式(4)所示； 为根据磁信标参数算得的磁场矢量估计值，如式(5)所示；
式中， 为第i个测点处实测的磁场矢量序列，且 1≤i≤Nm，其中， 分别为实测磁场在第i个测点处x、y、z方向的分量；
式中， 为第i个测点处估计的背景磁场矢量序列，且 其中，分别为第i个测点处背景磁场在x、y、z方向上的分量；
式中， 表示所有可能存在的磁信标磁源在第i个测点上产生的磁场矢量，其中， 分别表示第k个可能存在的磁信标磁源在第i个测点处x、y、z方向上产生的磁场矢量，1≤k≤K，K表示可能存在的磁信标磁源数量，f(ck)为对应第k个可能存在的磁信标磁源的待定系数函数，f(ck)的表达式如式(6)所示
式中，ck为有效性判定系数中的待定系数参数，为唯一变量，取值范围为(‑∞，+∞)；f(ck)的函数值范围为(0，1)；
如果Bb可以预先测量或使用现有模型计算，则在优化时只需对磁信标进行估计，否则，将背景磁场视为未知，并与磁信标的位置一起估计，此时，用多项式对连续测点序列进行背景磁场建模，如式(7)所示：
式中， 表示背景磁场信息Bb在第i个测点处的某个分量；q是多项式的阶数，q≥2；a0至aq为多项式中待求的未知系数；
基于建立的目标函数，进行磁信标反演的初始化，初始向量为：式中，Xini为初始向量，其中，表示第k个可能存在的磁信标的位置， 表示第k个可能存在的磁信标的三维磁矩，cini，k表示第k个可能存在的磁信标对应的待定系数参数，并且， 和 在设定范围内随机取值；cini，k取值为10，即认为所有磁信标在初始时刻默认都存在；a＝[a0 … aq]表示该段航行轨迹对应的背景场多项式系数，a0至aq的初始值随机赋值；
利用非线性最小二乘方法对所建立的目标函数 进行反演解算，对优化结果当中对应判定系数值大于有效阈值的磁信标认为是真实存在的磁信标，也就是SLAM工作过程中观测到的路标，将这些磁信标相对于航行器的位置作为观测量，进行SLAM解算；
随着航行器的运动，航行器的航行轨迹实现了对整个工作区域的覆盖，所有磁信标均被观测到，制作并保存包含所有磁信标的磁信标位置图 如式(9)所示：式中，Xl＝[Xl，x Xl，y Xl，z]表示第l个被探测到的磁信标的三维位置信息，L为航行器工作区域内被探测到的磁信标总数；
根据航行器在每个航迹点出测得的磁场信息，结合对应的航行器位置，利用改进谢别德法构建工作区域的磁场图
步骤2，由于SLAM系统故障或外界干扰，航行器丢失自身位置；而后系统从故障中恢复，需从一个未知位置开始继续工作；航行器利用接下来运动过程中测得的磁场信息，与步骤1中建立的磁场图进行匹配，估计航行器的大致位置；
步骤3，航行器利用观测的磁场信息进行磁信标反演定位，并使用迭代最近点方法对反演所得信标组合与步骤1中得到的磁信标位置图进行匹配，以修正航行器位置，完成对航行器位置的在线初始化；
步骤4，将迭代最近点方法的修正结果作为航行器的初始位置，基于磁信标的SLAM系统继续工作。
2.根据权利要求1所述的基于磁信标的同步定位与构图系统的故障恢复方法，其特征在于，步骤2进一步包括：
航行器在外力作用下突然移动到另一个位置，或SLAM从系统临时故障中恢复，航行器SLAM系统必须在一个未知的点重新启动；
首先，航行器从一个未知的点开始以纯推位模式运行，在N个连续测点上收集一组磁场矢量数据集M，如式(10)所示：M＝[B′1…B′j…B′N]，1≤j≤N，N≥Nm (10)式中，B′j为SLAM系统重新启动后第j个测点处实测的磁场矢量序列，且B′j＝[B′x，j B′y，j B′z，j]，其中，B′x，j、B′y，j、B′z，j分别为SLAM系统重新启动后实测磁场在第j个测点处x、y、z方向的分量；
执行磁场轮廓匹配算法，将对M在 上进行匹配，得到该段纯推位航迹在工作区域全局地图上的大致位置，将最后一个航迹点作为匹配结果，即为航行器当前的估计位置CM。
3.根据权利要求1所述的基于磁信标的同步定位与构图系统的故障恢复方法，其特征在于，步骤3进一步包括：
为了避免错误的匹配结果，为航行器提供一个更为准确的初始化位置信息，令步骤2推位阶段中，每Nm个点组成一个测点序列，并利用步骤1中建立磁场图和磁信标位置图时所采用的方法，得到该推位航迹周围的磁信标分布情况 如式(11)：式中，Bq表示该推位段上所探测到的第q个磁信标的三维坐标，Q为该推位段上所探测到的所有磁信标的数量；
使用迭代最近点方法对 与 进行匹配，利用迭代最近点方法匹配结果中的旋转矩阵和平移向量对航位推算路径进行修正，将修正后的航行器位置记为CI，作为航行器恢复SLAM系统时的初始位置。
说明书 :
一种基于磁信标的同步定位与构图系统的故障恢复方法
技术领域
背景技术
在预定工作海域布设导航基阵，只适用于局部作业且成本较高，惯性导航误差随时间积累，
无法保证结果的收敛。基于地球物理场的匹配导航技术稳定性强、自主性高，可以实现位置
的收敛，但需要以高精度、高分辨率的先验地图为基础，受地球物理场空间特征的丰富性以
及数据来源的限制。而同步定位与构图方法(simultaneous localization and mapping,
SLAM)无需先验环境信息，在导航过程中，利用实时观测信息辅助导航定位，提高系统精度，
是弥补地球物理场先验信息不足的有效手段。
间的相对位置并作为观测量，实现SLAM系统状态的解算。但是当航行器在此过程中由于软
件、硬件等故障原因，或外界干扰下，临时丢失自己的位置，对磁信标的观测不再连续，导致
SLAM系统中断后，导航系统需要具备在线重新获取自身位置的能力，并继续开展SLAM估计。
但目前并没有相关研究成果解决此问题。
通过对匹配定位结果进行校准，实现SLAM系统的恢复运行。该方法能够有效应对基于磁信
标的SLAM系统的故障状况，在全局地图下准确初始化航行器位置，大大提高了基于磁信标
的同步定位与构图方法在实际应用中针对潜在故障的鲁棒性。
发明内容
过工作区域后，对工作区域进行磁场建模，并用磁场匹配方法初始化航行器的位置，进而通
过基于磁源反演的位置校准，实现SLAM系统故障恢复。
信标位置图；
步骤1中建立的磁场图进行匹配，估计航行器的大致位置；
航行器位置的在线初始化；
序列上的磁场序列，利用非线性最小二乘方法实时解算磁信标的相对位置；设磁信标置于
笛卡儿坐标系的原点，则该磁信标在任一测点Pi(x,y,z)处产生的磁场强度为Bi＝[Bx,i By,i
Bz,i]，数学模型如式(1)所示：
在第i个测点处产生的磁场强度在x、y、z方向的分量；[mx,imy,imz,i] 为磁信标的三维磁矩，μ
为介质磁导率；如果一个区域内存在R个磁信标，则某一测点测得的磁场矢量 即为所有单
个磁信标在该测点产生的磁场信息之和，即 其中 为第r个磁信标在第i个
测点处产生的磁场强度，R>1；
算得的磁场矢量估计值，如式(5)所示；
信标磁源在第i个测点处x、y、z方向上产生的磁场矢量，1≤k≤K，K表示可能存在的磁信标
磁源数量；f(ck)为对应第k个可能存在的磁信标磁源的待定系数函数，f(ck)的表达式如式
(6)所示：
进行背景磁场建模，如式(7)所示：
可能存在的磁信标的三维磁矩，cini,k表示第k个可能存在的磁信标对应的待定系数参数，并
且， 和 在设定范围内随机取值；cini,k取值为10，即认为
所有磁信标在初始时刻默认都存在；a＝[a0 … aq]表示该段航行轨迹对应的背景场多项式
系数，a0至aq的初始值随机赋值；
观测到的路标，将这些磁信标相对于航行器的位置作为观测量，进行SLAM解算；
x、y、z方向的分量；
得到该推位航迹周围的磁信标分布情况 如式(11)：
复SLAM系统时的初始位置。
本方法提出工作区域被航行器在SLAM模式下实现航迹覆盖的情形下，利用匹配定位方法快
速实现位置初始化，并结合磁信标反演定位，利用ICP方法实现初始位置校准，进而实现
SLAM系统恢复。该方法能够有效应对SLAM系统的故障状况，准确初始化航行器位置，大大提
高了实际应用中基于磁信标的同步定位与构图方法面对潜在故障的鲁棒性。
附图说明
具体实施方式
对未知背景磁场下的磁信标的精确检测，以及对整个工作区域内磁场分布情况的准确建
模。该方法主要应用于航行器的工作区域被SLAM路径基本覆盖后，SLAM系统失效或航行器
丢失自身位置信息的情况。根据航行器测得的磁场信息，可以用匹配方法代替SLAM系统或
匹配得到准确的航行器初始位置，以重新启动SLAM工作模式。
标(磁信标)位置图。具体包括：
点序列上的磁场序列，利用非线性最小二乘方法实时解算磁信标的相对位置；设磁信标置
于笛卡儿坐标系的原点，则该磁信标在任一测点Pi(x,y,z)处产生的磁场强度为Bi＝[Bx,i
By,i Bz,i]，数学模型如式(1)所示：
在x、y、z方向的分量；[mx,i my,i mz,i]为磁信标的三维磁矩，μ为介质磁导率；如果一个区域
内存在R(R>1)个磁信标，则某一测点测得的磁场矢量 即为所有单个磁信标在该测点产
生的磁场信息之和，即 其中 为第r个磁信标在第i个测点处产生的磁场强
度。
算得的磁场矢量估计值，如式(5)所示；
信标磁源在第i个测点处x、y、z方向上产生的磁场矢量，1≤k≤K，K表示可能存在的磁信标
磁源数量；f(ck)为对应第k个可能存在的磁信标磁源的待定系数函数，f(ck)的表达式如式
(6)所示：
进行背景磁场建模，如式(7)所示：
到。
在的磁信标的位置， 表示第k个可能存在的磁信标的三维磁矩，cini,k
表示第k个可能存在的磁信标对应的待定系数参数，并且，
和 在设定范围内随机取值；cini,k取值为10，即认为所有磁信标在初始时刻默认都存
在；a＝[a0 … aq]表示该段航行轨迹对应的背景场多项式系数，a0至aq的初始值随机赋值；
磁信标，也就是SLAM工作过程中观测到的路标，将这些磁信标相对于航行器的位置作为观
测量，进行SLAM解算。
步骤1中建立的磁场图进行匹配，估计航行器的大致位置。具体包括：
y、z方向的分量；
计位置CM。
航行器位置的在线初始化。具体包括：
得到该推位航迹周围的磁信标分布情况 如式(11)：
系统时的初始位置。
对其中包含的匹配误差进行精细校准。利用本发明介绍的磁信标探测方法，反演得到每段
测点序列周围的磁信标分布情况，使用迭代最近点(ICP)方法对将其与地标图进行匹配，利
用匹配结果中的旋转矩阵和平移向量，能够有效地将匹配位置中的误差进行修正，并作为
航行器SLAM系统恢复后的初始坐标。
技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可
以做出很多形式，这些均属于本发明的保护范围之内。