基于磁信标的水下SLAM方法

常 帅,付晓梅,张翠翠,赵玉新,杜 雪

(1.天津大学 海洋科学与技术学院,天津,300072;2.哈尔滨工程大学 自动化学院,黑龙江 哈尔滨,150001)

0 引言

水下同步定位与构图(simultaneous localization and mapping,SLAM)是一种无人水下航行器(unmanned undersea vehicle,UUV)在未知环境中,利用传感器探测的环境特征作为导航参照,并利用距离方位等观测量实现 UUV导航状态的估计及特征地图构建的方法。SLAM导航系统相比单一惯性导航系统(inertial navigation system,INS)或航位推测(dead reckoning,DR)导航系统,可在更长的时间内实现高精度导航估计,已成为近年来UUV 自主导航的重要发展趋势之一。

受传感器精度的影响,INS或DR存在明显的误差积累效应。为减少通过卫星定位校准而带来的作业效率和能耗损失,在水下环境寻找校准导航状态估计误差的观测量是一种更加高效的解决方案。海底地形/地貌通常具有明显的空间变化,可利用水声探测捕获海底典型地形特征,作为路标信息来支撑 SLAM 的开展[1-6]。海底环境通常具有非结构化特征,很多情况下难以获取清晰、明确的特征物,在海底平原区域,这种方法甚至将失效。传统的光学探测方式在水下环境受到极大制约,探测距离通常只有米级,难以满足绝大多数场景下的水下导航需求。水声探测方式的作用距离虽然大大提升,但探测分辨率相比于光学探测方式有明显劣势,同时海流等环境干扰也会带来额外的噪声。因此,在自然环境特征制约和现有探测技术条件下,急需其他的环境特征信息来提升水下SLAM导航技术的性能和适用范围。

磁场分布受水环境、多径效应及信号时延等影响较小,其探测方式是被动的,这使得磁场探测相比于水声、光学等主动探测方式更易实现。因此,若能在 UUV 导航区域,尤其是地形/地貌特征贫乏的区域布设一些磁信标,利用实时探测的磁场信息对磁信标进行准确的相对位置反演,可为航行器SLAM系统的路标提供观测量,提升系统的导航性能。其中的关键因素是对磁信标位置的位置反演。

基于磁场梯度张量的直接反演方法最先得到研究。Wynn等[7]最早提出利用单测点的磁偶极子梯度张量和场分量,利用解析法计算场源三维位置,结合磁源位置的先验信息排除 3个干扰解得到确定位置。Brisan[8]针对动态磁性目标的跟踪问题,提出一种磁源位置、磁矩和速度的迭代贝叶斯估计方法,利用连续观测数据增强抗噪能力。基于梯度张量矩阵的特征分析法和解析信号方法也得到较多关注,特征分析法率先被 Beiki等[9]用于对重力梯度张量矩阵的分析,实现对目标体的三维位置反演,而后被 Clark[10]推广到磁场分析中,利用磁场梯度张量特征值计算归一化磁源强度,结合梯度张量来确定磁源位置的唯一解。迟铖等[11]基于磁梯度张量特征分析研究了多测点线性定位方法,但定位精度受测量系统基线大小和测量平台位移误差影响较大。万成彪等[12]以磁梯度张量作为磁测信息,先通过单点梯度张量解得位置方向矢量,再结合多点方向和辅助信息确定正解实现定位,取得与直接反演方法相当的反演定位精度。但基于多测点观测数据的定位方法对测点分布情况依赖性较强,当测点分布不理想时,容易对最小二乘解准确性产生不利影响。Zhang等[13]提出的欧拉反演法是目前采用较多的一种磁源反演定位方法,不需要磁源的磁矩信息,直接利用磁场矢量和梯度张量进行定位求解。张朝阳[14]分析了磁偶极子磁矩和磁梯度张量测量系统基线长度对欧拉反演定位精度的影响。Teixeira等[15]在传统欧拉反演方法的基础上,利用磁梯度张量矩阵最小特征值对应的特征向量与磁源和测点之间向径的正交关系,提出一种张量欧拉反褶积(tensor Euler deconvolution,TED)和磁场梯度张量特征分析(eigenanalysis of the magnetic gradient tensor,EGT)相联合的反演方法(TED+EGT),增强了反演结果的收敛性。Pei等[16]利用磁梯度场目标模式匹配迭代搜索目标磁源参数,设计了基于UUV的磁梯度仪,并开展了物理验证[17]。

基于磁场的 SLAM 方法研究较少,多数研究利用室内磁场丰富的磁异常空间特征变化及可预测性,建立了机器人 SLAM 模型[18-20]。基于文献[21]研究的解析式磁梯度反演定位方法,Wu等[22]建立了无迹卡尔曼滤波(unscented Kalman filter,UKF)-SLAM模型,但该反演方法需要将磁源磁矩作为已知条件,因而在水下环境常常难以实现。

受定位精度或实时性的影响,以上所述反演方法难以直接用于基于磁信标的 SLAM 模型构建。文中在文献[15]TED+EGT方法的基础上,提出连续反演定位结果的收敛性判断准则,准确提取UUV与磁信标的相对位置,进而建立SLAM模型,试验对文中方法的有效性进行了验证。

1 SLAM模型及工作过程

1.1 SLAM模型

用概率分布函数描述SLAM,即

式(1)表示以x0为初始位置,以为控制输入向量,以为路标观测向量的系统状态量概率分布情况,系统状态量包括tk时刻的航行器位置xk和所观测的路标集合位置向量m。

系统量测模型

式中:h（*）为系统观测量模型,即传感器观测数据表示环境特征的方法;vk为观测噪声。

以扩展卡尔曼滤波(extended Kalman filter,EKF)为例进行系统建模,主要包括运动更新(状态预测)和量测更新(状态校准)2个环节,并迭代进行。运动更新

其中

式中:表示给定状态更新、量测量和h（*）下的特征地图;∇h是h（*）的雅克比矩阵。

1.2 SLAM工作过程

SLAM工作过程如图1所示。状态向量xk表示航行器位置,在uk作用下从xk-1运动至xk。为路标位置向量,对应航行器的观测量为。

图1 水下同步定位与构图工作过程示意图Fig.1 Working process of underwater simultaneous localization and mapping(SLAM)

此过程步骤如下:1)航行器首先初始化地图z0来表示k时刻的观测量(图中为m1和mi分别为第1个和第i个路标的位置向量),传感器噪声及特征提取过程使得此时估计的地图不完全准确;2)在u1控制下运动至′(根据航行器自身的运动模型和内部传感器估计求得);3)进行下一次量测z1(z1在真实位置测得);4)航行器根据位置更新和观测到的地图特征对估计地图进行数据关联;5)更新自身位置和地图,然后开启下一次迭代。

2 磁信标磁场模型及位置反演

要将水下环境中的磁信标作为SLAM的路标特征,必须从环境磁场中准确提取磁信标磁场特征,并利用适当的反演方法估计其相对位置。

2.1 磁信标特征提取

1)磁信标磁场模型

UUV到磁信标的距离通常远大于磁信标尺寸本身,因此可将磁信标视作磁偶极子,其磁场符合磁偶极子模型。以磁信标为原点建立直角坐标系Oxyz,在远离原点的P（x,y,z）处磁场为

式中:Bx,By和Bz是信标磁场磁感应强度在x,y及z方向上的分量;μ为介质磁导率;r为P点到磁信标的距离;mx,my和mz分别为磁信标磁矩在x、y及z方向上的分量。

磁梯度张量G描述了Bx,By和Bz分别沿x,y及z方向的空间变化率,即

当航行器在磁信标磁场覆盖范围内航行时,所测量的磁场矢量和梯度张量必然是地磁场和磁信标磁场共同作用的结果。从磁场矢量和梯度场的产生原理可知,测量结果为磁信标磁场和地磁场特征的线性叠加,即

2)磁信标磁场提取

航行器航行过程中,背景地磁场可通过模型计算、航空航海测量资料、遥感等多种方式获得。根据导航系统输出的位置信息从数据库中提取Be和根据式(11)和式(12)可得Bb和Gb。设定阈值σ,若连续N个测量点位上均满足则认为当前测量磁场信息中包含磁信标磁场。由于环境噪声和传感器噪声具有比较明显的高频特性,采用滤波方法对Bb和Gb进一步进行噪声处理,保证位置反演的准确性。

2.2 磁信标位置反演

基于磁梯度 TED方法可以进行磁信标位置反演计算。在一般磁场环境中,磁异常满足欧拉齐次方程,即

式中,n表示齐次度(结构化指数),对于磁偶极子,n=3。利用梯度张量可得反演方程

式中,r为反演所得位置的位置矢量。文中方法能够实现单点反演,并且不需要将磁信标的磁矩作为已知条件,符合水下应用场景的需求。

但上述欧拉反演方法是一个明显的不适定问题,并且对观测噪声非常敏感,文献[15]寻求了进一步的求解约束条件约束提高反演计算的准确性,即 TED+EGT方法。由于磁场梯度张量矩阵是一个实对称阵,可以得到张量矩阵G的正交特征向量集 ｛b1,b2,b3｝,其中b3对应绝对值最小的特征值,并且与m-r平面相垂直,m为磁信标磁矩向量。这意味着待求的磁信标位置向量r与b3向量正交,且处于与m-b3平面相垂直的平面内。令得

综合式(14)和式(15),可得

相比于式(14),极大地缩小了位置矢量r的解空间,利用最小二乘即可进行求解。

2.3 连续测量过程中的磁信标位置确定

对于某一确定的磁信标,其磁场强度随着距离的增加而迅速减弱,噪声的干扰作用相应较强,利用TED+EGT方法所得反演结果仍具有明显的不确定性,具体表现为利用相邻观测点上的观测数据所得磁信标位置会有明显差异。而当航行器处于磁信标磁场分布较强的区域时,噪声的干扰作用相对较小,利用载体航迹上连续测量数据所得反演结果具有较强的收敛性,反演结果准确。

基于此,在航行器连续航行的过程中,设计磁信标位置反演结果提取方法如下:设航行器在连续时刻上利用 2.2节所述反演方法得到磁信标与航行器之间的相对位置序列为转化成空间几何距离为当此反演位置序列满足以下2个收敛条件时,认为反演结果准确。

条件2:m≥m0。

其中,d0表示连续2次反演所得相对位置之间允许的最大变化,需要结合采样时间间隔和航行器航行速度来确定。其物理意义在于当反演结果能够稳定跟踪某个磁信标位置时,反演所得磁信标与航行器之间的距离(由三维相对位置得到)应呈连续均匀变化,当连续 2次反演结果的变化不符合航行器自身运动规律时,则认为反演结果不准确;条件2中m0表示要求满足条件1的连续反演结果的最小数量,显然m0越大,对连续反演磁信标位置的收敛性要求越高。

3 试验分析

3.1 磁信标辅助的水下SLAM建模

定义系统状态为

航行器在航行过程中,利用文中所述方法进行磁信标相对位置的反演,构成对状态量中航行器位置和信标位置的线性观测,观测方程

3.2 磁信标反演定位试验分析

图2 磁信标反演定位结果Fig.2 Inverse positioning results of magnetic beacons

图2(a)为利用 2.2节所述方法所得的磁信标反演位置序列,图 2(b)为利用 2.3节方法进一步进行准确位置提取之后所得的反演位置结果,其中航迹上红色位置点表示对应提取反演结果的测点位置,针对磁信标1,2和3,分别可提取5,7和12个连续反演位置。显然按2.3节方法进行位置序列提取后,所得结果均收敛在真实磁信标位置附近。误差统计如表1所示。

需要指出的是,不同磁信标磁场的叠加是影响反演定位结果收敛性的主要因素之一,当航行器与某一个磁信标距离较近时,其他磁信标的磁场特征将表现为噪声。当磁信标之间距离较远时,磁场混叠效应减弱,航行器对磁信标的反演结果会有较好的收敛性。但过于稀疏的磁信标又会影响SLAM系统观测量的获取和数据关联效果,因此,磁信标应当保持适中的空间分布已达到最佳辅助导航效果。

表1 磁信标反演位置误差统计Table 1 Inversion positions errors statistics of magnetic beacons

3.3 磁信标辅助水下SLAM试验分析

设定航行器航行速度为 2 m/s,最大转向角为 30°,最大转向速率为 20°/s,期望航迹为平行往复航线,磁场采样频率为5 Hz。设定3种磁信标布局场景如下:1)磁信标分布比较稀疏,磁信标之间平均距离约为150 m;2)磁信标分布比较适中,磁信标之间平均距离约为100 m;3)磁信标分布比较密集,磁信标之间平均距离约为 50 m。磁信标反演收敛性条件为d=2 m,m0=5。设定航行器对磁信标在x,y和z方向的相对位置反演结果均包含符合均值为0,均方差为1.5 m的高斯噪声,磁信标的磁矩参数设置同3.2节。

SLAM系统仿真过程中观测量的获取方式如下:当连续反演定位结果满足收敛性条件 1和条件 2的最低要求时,将其中最后一次反演所得磁信标与航行器相对位置作为观测量对SLAM状态进行更新。在此基础上,对之后收敛性条件的连续反演所得相对位置都作为有效观测量用于SLAM 系统估计的迭代更新,直至某一个反演结果不满足收敛条件。而后重新上述过程。3种场景下系统仿真试验结果如图 3所示。图中,绿色轨迹线为期望航线,黑色轨迹为SLAM输出航线,红色椭圆为航行器SLAM过程中对每个磁信标位置估计的误差椭圆。

图3 3种场景下磁信标辅助水下SLAM仿真试验结果Fig.3 Simulation results of magnetic beacon-based underwater SLAM in three different scenes

根据试验结果可知,虽然磁信标之间距离较大可增强连续反演结果的收敛性,但路标过于稀疏会大大减少可以获取的观测量信息,降低对系统误差的校正效果。而磁信标分布过于密集也不能提升系统状态估计的准确性,甚至使其降低,这是因为各磁信标磁场的混叠效应严重降低了反演结果的收敛性,有效观测量并不会随着磁信标数量的增加而增加。因此,只有在磁信标分布较适中时,才开获得更高的导航状态估计精度。

4 结束语

文中研究了基于磁信标反演定位的水下SLAM 方法,在欧拉反演法的基础上提出连续反演结果收敛性条件,以从连续反演数据中提取准确的磁信标相对位置信息。仿真试验验证了研究方法的有效性。所提出的基于水下磁信标的SLAM 方法不要求水下磁信标位置准确已知,对于水下航行器在未知环境中开展导航工作具有较好的参考价值。但由于磁信标作用距离的限制,并且需要人工布设,因此文中方法主要适用于区域导航场景,相比于水声定位系统,具有成本低、布设方便等优势。而且磁场的空间分布特点使得它在浅水、多障碍等水声信号传播受明显限制的环境中可以表现出明显优势。下一步工作将研究磁场混叠情形下的多磁信标同步反演定位方法,以提升基于磁信标反演定位的SLAM系统性能。