戴天， 缪玲娟， 邵海俊

(北京理工大学 自动化学院, 北京 100081)

0 引言

自主水下潜航器(AUV)是探索海洋的重要工具之一，导航系统是AUV的重要组成部分，高精度、高可靠性的导航系统可以持续为AUV提供准确的位置与姿态信息，是AUV长时间水下作业的重要保障[1-2]。大多数AUV均配备惯性导航系统(INS)作为主导航系统[2]，但是INS的误差随时间累积而发散。提高加速度计和陀螺仪的精度可以延缓误差累积的速度，但是无法从根本上消除累积误差[3-4]。通常情况下，会使用其他导航系统作为辅助导航系统对INS累积误差进行修正。水下环境中，使用重力场、地磁场和地形等地球物理信息作为信息源的数据库辅助导航系统(DBRNS)，能够很好地解决INS累积误差的校正问题[1,5-7]。但是DBRNS的有效运行需要预先获取相应信息的高分辨率数字基准图，否则DBRNS无法作为辅助导航系统校正INS的累积误差。

同步定位与地图构建(SLAM)技术不依赖基准图，可以在无基准图时作为辅助导航手段来校正INS的累积误差。SLAM最早于1986年由美国旧金山IEEE机器人与自动化会议提出[8]，SLAM方法通过实时感知周围的环境特征定位自身的位置与姿态，再根据自身位置与姿态构建周围环境的地图，从而达到SLAM的目的。根据算法的不同，SLAM可以分为基于贝叶斯滤波的SLAM和图优化SLAM[9-10].

基于贝叶斯滤波的SLAM多见于早期的研究，该类方法根据机器人运动模型和传感器量测模型对问题进行建模，在隐形马尔可夫假设下，实现系统的状态更新和观测更新[11]。基于贝叶斯滤波的SLAM中，基于扩展卡尔曼滤波器的SLAM(EKF-SLAM)算法和基于Rao-Blackwellized粒子滤波器的SLAM(RBPF-SLAM)算法是研究热点。EKF-SLAM算法适用于弱非线性系统，但是该类算法在运行过程中需要不断添加新的路标，在大规模环境下难以使用；RBPF-SLAM算法适用于强非线性及非高斯系统，但是该类算法计算量很大。基于贝叶斯滤波的SLAM假定下一时刻的状态只与前一时刻有关，不考虑前一时刻之前的历史记录，长时间运行情况下由传感器噪声不确定性引起的误差不断累积，最终将导致地图的不一致[12]。

图优化SLAM采用全局优化方式解决SLAM问题，该类方法将所有状态看成变量，将运动方程和观测方程看成变量间的约束，然后构造误差函数并最小化该误差函数的二次型[13]。该技术被越来越多地应用于大规模、非结构化环境中，在此背景下基于贝叶斯滤波的SLAM逐渐被图优化SLAM取代。

基于视觉传感器的图优化SLAM方法是目前最受关注的SLAM方法之一，如果将基于视觉传感器的图优化SLAM方法用于水下导航任务，则需要满足3个条件才能保证导航精度：1)载体贴近海底航行；2)水域有足够高的清晰度；3)海底地形为结构化环境[14]。但是这3个条件很难长时间同时保持，因此基于视觉传感器的图优化SLAM方法在水下导航任务中难以取得很好的定位精度。多波束测深仪探测距离远，且对水域的清晰度与海底环境的结构性特征几乎没有要求，非常适用于水下导航任务。

本文采用多波束测深仪作为传感器，提出一种基于多波束测深仪的图优化SLAM方法。为了减少错误闭环检测对优化结果所造成的不良影响，该方法在通用图优化SLAM方法的前端部分增加了误匹配检测模块。此外，为了提升闭环检测的精度，该方法将局部灰度值编码算法[15]用于闭环检测模块。

本文简要介绍通用图优化SLAM方法及其组成部分，指出通用SLAM方法在处理水下导航问题时的不足，并给出基于多波束测深仪的图优化SLAM方法；对新方法中前端部分的闭环检测模块和误匹配检测模块进行详细阐述；简要介绍了后端部分的优化原理；基于真实海底地形图进行仿真实验，展示两种SLAM方法对INS指示轨迹的校正结果，并对结果进行分析。

1 通用图优化SLAM方法

图优化SLAM方法利用图模型对SLAM问题建模，将SLAM问题划分为前端和后端两个部分，前端负责图的构建，后端负责图的优化。通用图优化SLAM方法如图1所示[12]。

图1 通用图优化SLAM方法Fig.1 General graph SLAM method

图优化SLAM方法在前端部分构建的图由节点和边组成，节点表示载体离散的位置与姿态以及对应传感器的实测值，边表示节点之间的约束，约束关系一般由两个节点之间的位置与姿态变换关系构成。如图1中前端部分所示，图优化SLAM方法通常采用两种方式构建约束关系，一种是顺序数据关联，另一种是闭环检测。

顺序数据关联是建立连续节点之间的相对位置变换，该类约束关系的建立需要传感器能够不断地重复观测到地图特征，大部分视觉SLAM方法均可以构建该类约束，而基于多波束测深仪的SLAM方法难以通过顺序数据关联构建约束关系。一般情况下，水下多波束测深仪可以获得垂直于船体下方的条带区域内多个测量点的地形数据，但是连续的观测数据之间很少包含重复信息，因此通过顺序数据关联建立约束难以实现。

闭环检测是检测载体是否回到先前经过的位置，该类约束是一种强约束关系，一旦检测到闭环，将对整个位置与姿态图进行较大程度的修正。本文所提SLAM方法在每一个节点存储该点对应的子图，通过设定S形航线，检测不同节点所对应的子图是否包含重叠区域，进而完成闭环检测。

基于多波束测深仪的图优化SLAM方法构建的图模型如图2所示，图2中节点{x0，x1，…，xi+1}表示载体的位置与姿态以及子图。

图2 基于多波束测深仪的图优化SLAM方法中图结构Fig.2 Graph structure of multi-beam sonar-based graph SLAM method

2 基于多波束测深仪的图优化SLAM方法

采用通用图优化SLAM方法处理基于多波束测深仪的水下SLAM问题，存在如下两个缺陷：

1)水下长航导航任务中，难以通过顺序数据关联建立连续节点之间的约束关系；

2)闭环检测对优化结果影响很大，现有方法难以保证检测精度。

为了解决上述两个问题，本文对通用图优化SLAM方法进行改进，提出一种基于多波束测深仪的图优化SLAM方法，如图3所示。新方法在通用图优化SLAM方法的基础上，移除了顺序数据关联模块，并引入误匹配检测模块以剔除无效闭环。

图3 基于多波束测深仪的图优化SLAM方法Fig.3 Multi-beam sonar-based graph SLAM method

2.1 闭环检测

2.1.1 问题描述

将航行过程中多波束测深仪传回的测深数据转换至大地坐标系可构建测深图，以图优化SLAM方法中节点所在位置为中心，按指定的长度和宽度将测深图划分为若干个子图。在构造新子图时，可以使用模板匹配方法检测新子图与历史子图之间是否包含闭环(重叠部分)。

(1)

图4 子图匹配过程示意图Fig.4 Matching process of subgraph

(2)

(3)

Xp+lp-Xq-lq=0.

(4)

由于地形异常图与灰度图相似，本文借鉴灰度匹配算法中的局部灰度值编码算法[15]，将该算法用于基于多波束测深仪图优化SLAM方法中的闭环检测模块。相对于其他常用的灰度匹配算法，局部灰度值编码算法时间复杂度更低，且对图像尺寸不敏感，鲁棒性更高。

2.1.2 局部灰度值编码算法

局部灰度值编码算法的匹配过程分为粗匹配和精匹配[15]。在粗匹配过程中，对模板和搜索图根据灰度值进行编码，然后将模板在搜索图上按照固定步长遍历，寻找搜索图中与模板编码最接近的图块，得到初始匹配参数。在精匹配过程中，根据初始匹配结果，在粗匹配位置裁剪出一个大小和模板一样的子图，然后使用相位相关法进行精确匹配。

智能仓储机器人一旦出现问题，停止运作后，想去维修它就会变得非常困难，一方面是机器人本身就很重，一台机器人就有上百斤重，所以很难去移动它。另一方面是机器人内部比较复杂，一般员工根本不懂，必须要研发的专业人员来维修，这样就会对产品的售后造成很大困难。

粗匹配首先提取图像的分块编码特征，如图5(a)所示，将搜索图像划分为K×K个互不重叠的方块，称该方块为R块，K可根据问题任意选择。分块完毕后，将图像中多余的行和列裁减掉。R块与其周围8个相邻的R块组成R块的邻域，将R块的邻域分成D1、D2、D3、D4共4个部分，称为R块的4个D邻域。R块R5的邻域和4个D邻域如图5(b)所示。对每个D邻域中4个R块的灰度值进行排序(升序或降序)，将结果转换成5位二进制码，记作P(D)。如图5所示，将R块R5所在的4个D邻域编码拼接起来，得到F(R5)=P(D1)P(D2)·P(D3)P(D4)，F(R5)即为R5的编码。因为图像外围的一圈R块不存在8个邻域，所以只有图5中的阴影部分可以编码。将图像中所有R块的编码特征转换成十进制数，将其按照行列顺序组成1个一维向量，该向量称作图像的特征向量。将模板特征向量与子图特征向量进行比较，相似度最高的子图即为粗匹配结果。

图5 图像分块及编码Fig.5 Image segmentation and coding

粗匹配可以找到与模板相似度最高的子图，但是该子图与模板存在一定的偏差，并非完全重叠，精匹配使用相位相关法修正粗匹配结果的偏差。根据二维傅里叶变换的性质：空间域上的平移等价于频域相位的平移。两幅图的平移矢量可以通过它们互功率谱的相位直接计算。假设图像f1和f2之间的平移关系为

f1(x,y)=f2(x-x0,y-y0),

(5)

式中：(x0,y0)为空间(x,y)的1个点。

相应的傅里叶变换如(6)式所示：

F1(u,v)=F2(u,v)e-j2π(ux0+vy0)，

(6)

式中：(u,v)为(x,y)对应的频域坐标；F1(u,v)为f1(x,y)的傅里叶变换结果；F2(u,v)为f2(x-x0,y-y0)的傅里叶变换结果。

定义图像f1与f2之间的归一化互功率谱为

(7)

2.1.3 采用局部灰度值编码算法执行闭环检测

灰度图与测深图结构类似，灰度图存储像素灰度，测深图存储地形深度，用于灰度图匹配的算法也可用于测深图匹配。但是，SLAM中的闭环检测问题与图像模板匹配问题并不完全相同。模板匹配是在一幅较大图像上定位一幅给定的子图像；闭环检测是寻找与新构建子图有重叠部分的已构建子图。

在图优化SLAM问题中，假设hn(n为当前采样点数)为新构建的子图，如果将hn与先前构建的n-1幅子图分别当作模板和搜索图的子块，则局部灰度值编码算法就可用于闭环检测。

2.2 误匹配检测

对全部采样点执行完闭环检测后即可构建约束条件，完成图的构建。但是错误的闭环检测结果会对后续图优化造成干扰，因此本文提出基于多波束测深仪的图优化SLAM方法，剔除错误闭环。

三重约束条件下的误匹配实时检测算法[16]分为模型拟合检测、空间结构检测和距离比检测3个检测模块，且3个检测模块均采用逐点实时检测的方案。本文与文献[16]处理的误匹配检测问题，有如下两点不同：

1)文献[16]中假设AUV的轨迹接近直线，但是在基于多波束测深仪的图优化SLAM方法中，为了使航行过程中包含闭环，需要设定AUV的轨迹为S形轨迹。

2)图优化SLAM方法是一种先建图再优化的方法，该类方法对误匹配检测方法的实时性要求较低。

基于不同点1，由于载体呈S形前进，轨迹中会出现很多拐点，很难用某个单一模型拟合真实轨迹。基于不同点2，由于本文所提图优化SLAM方法对误匹配检测的实时性要求不高，可在图构建完毕后一次性检测所有的误匹配点。

基于上述原因，本文采用误匹配检测算法，在三重约束条件下的误匹配实时检测算法基础上取消模型拟合检测模块，只包含空间结构检测模块和距离比检测模块，且每一个检测模块均采取批处理的方式。

2.2.1 空间结构检测

设XINS={xINS1,xINS2,…,xINSN}为INS指示轨迹，如果XINS上的两个位置点xINSi和xINSj之间被检测到包含闭环，且没有检测到xINSi之前的点与xINSi之间包含闭环，则将xINSi与校正后的xINSj视为匹配点。如果xINSj之后检测到新的位置点xINSk与xINSj之间包含闭环，则需对xINSj进行两次校正，再将其添加进匹配轨迹。假设有N′个匹配点，通过上述方法可以构建匹配轨迹Xm={xm1,xm2,…,xmN′}，记Xm，INS为Xm对应的INS指示轨迹。

空间结构检测的执行步骤如下：

1)构建Xm，INS和Xm的K最近邻(KNN)图Gm，INS={Vm，INS,Em，INS}和Gm={Vm,Em}，其中Vm，INS和Vm分别为Gm，INS和Gm中顶点的集合，Em，INS和Em分别为Gm，INS和Gm中边的集合。如果xj是xi的KNN域中的点，且‖xi-xj‖≤η，则xj和xi之间存在一条有向边〈i,j〉，其中η为相应顶点集中所有顶点之间距离的中值。KNN图构建完毕后可以得到每一个图的邻接矩阵，记为Am，INS和Am，其中Am，INS可由(8)式计算得到，同理可得Am.

(8)

2)对于连接顶点vmi至vmm的边，m≠i，使用(9)式计算权值W(i,m)：

(9)

式中：xm，INSm为Xm，INS上第m个位置点；xm，INSi为Xm，INS上第i个位置点；xmm为Xm上第m个位置点；xmi为Xm上第i个位置点；Rτ为旋转矩阵。

4)使用(10)式计算Gm中每个顶点的权值w(i)：

(10)

5)使用(11)式计算μo，

(11)

式中：μo为顶点权值的均值。

然后删除权值最大的点xmax，重新生成Xm，INS和Xm. 重复步骤1～步骤4，使用(11)式计算新生成顶点权值的均值μn. 如果|μn-μo|<ε(ε为需要手动设置的参数)且w(i)的最大值小于π，则检测完毕，否则xmax是误匹配点，算法进入下一次迭代。本文在仿真实验部分设定ε值为0.02.

2.2.2 距离比检测

假设dm，INSk-1,k和dmk-1,k分别表示INS指示轨迹和匹配轨迹上第k-1和第k个采样点之间的距离。距离比检测的执行步骤如下：

3)如果Smax和Smin均满足由INS误差传播模型得到的范围[17]则算法结束，否则存在误匹配。假设Smax对应的点为xmkmax-1和xmkmax(kmax表示Smax对应的采样点)，Smin对应的点为xmkmin-1和xmkmin(kmin表示Smin对应的采样点)，如果上述4个点包含重复位置，则该重复点为误匹配点；如果上述4个点互不相同，则xmkmax和xmkmin均为误匹配点。删除误匹配点后开始下一轮迭代。

2.3 后端图优化

通过前端完成图的构建后，建立图的节点和边，在后端需要对该图进行优化。设XSLAM={xSLAM1，xSLAM2，…，xSLAMN}是待优化的节点，定义zij为前端构建的第i个节点和第j个节点之间的位置与姿态变换关系，定义向量误差函数e(xSLAMi,xSLAMj,zij)表示xSLAMi和xSLAMj之间的关系与zij的偏离程度。则求SLAM最优轨迹可以表示成求解节点位置，使得下述总体误差函数最小[18]：

(12)

式中：Ωij为误差的信息矩阵，表示误差e(xSLAMi,xSLAMj,zij)所占的权重，本文设定Ωij为单位矩阵；Fij为[e(xSLAMi,xSLAMj,zij)]TΩije(xSLAMi,xSLAMj,zij)的简化表达式。

后端优化的目标是找到最优的节点配置XSLAM*，使得F(XSLAM)值最小。

(13)

图优化SLAM常采用高斯- 牛顿法或Levenberg- Marquardt法对(13)式进行求解，本文采用高斯- 牛顿迭代策略求解(13)式的数值解。该数值解用于校正INS的误差，校正后的导航信息即为下一轮SLAM前端部分的输入。

3 仿真实验结果与分析

3.1 仿真参数设置

本节规划的真实轨迹起点为(东经153°，北纬21°)，在此基础上仿真产生一条71.7 min的INS指示轨迹，INS的仿真参数如表1所示。真实轨迹与INS指示轨迹如图6所示。

表1 INS仿真参数

图6 真实轨迹与INS指示轨迹Fig.6 Actual trajectory and INS indicated trajectory

图7 三维地形图Fig.7 3D terrain base map

本文使用美国地球物理中心发布的模型[19]计算从(东经152.4°，北纬20.5°)到(东经153.1°，北纬21.2°)一片矩形区域的地形深度数据，对该区域内的地形数据进行插值，得到仿真实验用的真实地形图，插值后网格分辨率为0.05′×0.05′，三维地形图如图7所示。地形图的相关参数如表2所示。

表2 地形相关参数

仿真中所用的多波束测深仪固定于载体正下方，扫描长度为12个网格，在真实地形图上重采样并叠加一零均值高斯白噪声，作为由多波束测深仪测量的地形实测值，噪声方差为1 m. SLAM方法涉及的相关参数如表3所示。

表3 SLAM方法相关参数

3.2 通用图优化SLAM方法仿真

第1组仿真实验采用通用图优化SLAM方法校正INS指示轨迹，由于顺序关联约束难以建立，故该方法的前端部分只包含闭环检测模块。本组仿真实验采用局部灰度值编码算法进行闭环检测，采用高斯- 牛顿法进行后端图优化，实验结果如图8所示。

图8 通用图优化SLAM方法生成的校正轨迹Fig.8 Corrected trajectory generated by general SLAM method

从图8中可以直观地看出，SLAM方法校正轨迹比INS指示轨迹更接近真实轨迹的形状，但是在部分区域，尤其是轨迹的前半部分，SLAM方法校正轨迹的位置误差反而比INS指示轨迹的位置误差大。图9给出了SLAM方法校正轨迹及INS指示轨迹上每一个采样点的经度和纬度误差变化曲线，从图9中可以看出，在航行后半程SLAM方法校正轨迹的位置误差要明显小于INS指示轨迹，但是在航行的前40 min时间段，SLAM方法校正轨迹的纬度误差在多个采样点处要明显大于INS指示轨迹。导致这一现象的原因是闭环检测的结果包含了错误的闭环，错误的闭环检测结果会影响到轨迹上的每一个采样点。

图9 通用图优化SLAM方法生成的校正轨迹及INS指示轨迹的经度和纬度误差Fig.9 Longitude and latitude errors of corrected trajectories generated by general SLAM method and INS indicated trajectory

3.3 基于多波束测深仪的图优化SLAM方法仿真

第2组仿真实验在第1组仿真基础上引入了误匹配检测模块，即采用本文所提基于多波束测深仪的图优化SLAM方法校正INS指示轨迹，仿真结果如图10所示。

图10 基于多波束测深仪的图优化SLAM方法生成的校正轨迹Fig.10 Corrected trajectory generated by multi-beam sonar-based graph SLAM method

对比图8和图10中的SLAM校正轨迹可以明显看到，采用基于多波束测深仪图优化SLAM方法生成的校正轨迹更接近真实轨迹。

图11 基于多波束测深仪的图优化SLAM方法生成的校正轨迹及INS指示轨迹的经度和纬度误差Fig.11 Longitude and latitude errors of corrected trajectories generated by multi-beam sonar-based graph SLAM method and INS indicated trajectory

图11给出了SLAM方法校正轨迹及INS指示轨迹上每一个采样点的经度和纬度误差变化曲线。从图11中可以看到，SLAM方法校正轨迹的位置误差在全部航行时段均小于INS指示轨迹的误差，基于多波束测深仪的图优化SLAM方法具有更高的定位精度。

4 结论

本文提出一种基于多波束测深仪的水下图优化SLAM方法，在无基准图的情形下校正INS的累积误差。该方法在通用图优化SLAM方法基础上增加了误匹配检测模块，剔除了闭环检测模块输出的错误闭环。此外，该方法针对多波束测深仪数据的特性，采用局部灰度值编码算法进行闭环检测。基于真实海底地形图的仿真实验表明，在无基准图情况下采用该方法对INS轨迹进行校正可以取得更高的定位精度。

需要指出的是，本文提出的图优化SLAM方法对初始位置误差敏感且需要设定S形轨迹，同时还需要地形深度有明显的起伏。本文方法应与DBRNS相结合，共同在水下环境校正INS误差。在可以获得高分辨率基准图的前提下，采用DBRNS的定位效果往往优于本文提出的方法，当单一信息源特征明显时，应采用单信息源DBRNS进行水下自主导航；当单一信息源特征不明显时，应增加传感器种类，构建多信息源联合DBRNS进行水下自主导航；如果无法获得基准图或基准图分辨率低，则应采用本文提出的方法校正INS误差。