基于加权SDF地图的二维激光SLAM方法

夏 天 任 彧

(杭州电子科技大学计算机学院 浙江 杭州 310016)

0 引 言

在实际应用中，机器人需要在未知的环境中进行有效的路径规划并自主探索环境，这使得它必须具备自主创建环境地图并且实现准确自我定位的能力，这一问题被称为即时定位和地图构建(SLAM)。SLAM技术是一项用于解决机器人在未知环境中进行自我定位和环境地图构建问题的技术。在SLAM中，机器人的自主定位和对环境的地图构建二者相互依存，精准的地图构建需要建立在准确的定位的基础之上，同时定位需要环境地图的数据作为支撑。随着近年来人工智能和控制理论的不断发展，国内外各大学者提出了大量用于解决SLAM问题的方案。

机器人操作系统(ROS)[1]包含了很多可用的2D SLAM算法[2]，如Hector SLAM[3]、Gmapping算法[4]和CoreSLAM[5]等。Gampping是一种基于粒子滤波的SLAM方案，其在长廊等低特征场景中建图效果好，但其计算量较大，且依赖里程计，无法适用无人机及地面小车不平坦区域；CoreSLAM是一种小型的简单易懂的SLAM方法，同样使用粒子滤波方法，其方法较为简单，建图误差通常较大。Hector SLAM算法使用高斯牛顿法求解扫描匹配问题，无须依赖里程计，可适用于无人机和地面不平坦区域，要求高更新频率的激光扫描仪。由于在Hector SLAM中，占据栅格地图存在固有的精度限制，且高斯牛顿法存在可能不收敛且局部近似不够准确的缺陷。因此，本文提出了一种采用改进的符号距离函数(Signed Distance Function，SDF)地图的SLAM方法。2D-SDF地图是Fossel等[16]基于传统占据栅格地图提出的一种地图模型，该地图使用SDF值构建地图，可以低于栅格的精度构建地图。本文采用一种加权SDF地图，将SDF地图通过距离加权后可以获得更平滑的结果，容错率较高。同时使用L-M法求解扫描匹配问题，L-M算法是介于高斯牛顿法与梯度下降法之间的一种非线性优化方法，其采用信赖域的思想，在信赖域内求解下降方向的最优步长。与Hector SLAM使用高斯牛顿法相比，使目标函数陷入局部极小值的机会大大减小。最后通过将本文方法与Hector SLAM算法在ROS下进行仿真实验对比，结果表明所提方法具有较低的定位误差和较好的地图构建效果。

1 相关工作

占据栅格地图是激光SLAM中常用的一种地图表示方法，其将环境地图划分成等大的有限数量的网格，实际环境中的每个位置所在的网格只有两种状态：占据(Occupied，存在障碍物)或者空闲(Free，不存在障碍物)。环境地图可表示为：

m={mi}

(1)

式中：mi为栅格地图中的栅格。

对于栅格地图，以激光的观测数据和机器人的位姿估计每一个栅格mi在t时刻的状态：

p(mi|z1 ∶t,x1 ∶t)

(2)

式中：z1 ∶t是激光在1 ∶t时刻的观测数据；x1 ∶t是机器人在1 ∶t时刻的位姿。而栅格mi在t时刻的状态由其在1 ∶t-1时刻状态和第t时刻的观测数据和位姿决定。对于整个地图m，假设地图中的每个栅格单元mi相互独立，则可用边缘概率的乘积近似表示整个地图m的后验概率：

(3)

栅格地图的网格中存储该网格被占据的概率值，并使用二值贝叶斯滤波方法不断更新计算网格值，以此确定该网格最终的状态(p=1表示占据，p=0表示空闲)。

SLAM中的定位问题可以由扫描匹配来解决，扫描匹配方法通过最大化重叠已存在地图和当前扫描点，以求解机器人的当前位姿和上一时刻位姿间的相对平移T和旋转θ。扫描匹配可以有效计算且高精度地估计机器人位姿。为了求解出机器人两个时刻间的相对位姿，通常使用上一时刻位姿对当前扫描点进行旋转变换，然后投影到已存在地图M，并使用欧氏距离度量扫描点pi到地图M的匹配程度。该问题是一个最小化问题，可以描述为：

(4)

式中：∏(M,q⊕pi)为变换后的点到地图的欧氏投影。q⊕pi为扫描点通过位姿的旋转变换：

q⊕pi=R(θ)·pi+T

(5)

式中：R(θ)为旋转矩阵；T为平移矩阵。

为了解决上述最小化问题，研究者们提出了许多相关算法。ICP算法是最早被用作扫描匹配进行定位的算法，该算法通过搜索不同位姿下的扫描点的最近点，求出使其重合的最优刚体变换。目前已提出了众多关于ICP的变种算法[7-8]。大多数基于ICP算法的主要缺点是每次迭代过程中搜索对应点的复杂度较高。Pedrosa等提出了一种基于似然场模型[10-11]的扫描匹配方法。Hector SLAM使用了一种将当前扫描端点与地图值匹配的方案，这种方法无须搜索对应点。本文在Hector SLAM的基础上，将加权SDF地图引入到栅格地图中，并使用L-M方法改进了Hector SLAM中求解扫描匹配问题的高斯牛顿法，最后通过仿真实验对比，验证了本文方法相比Hector SLAM有较好的定位和建图效果。

2 基于加权SDF地图的SLAM方法

扫描匹配是激光扫描点和已存在地图间的匹配，目前的激光雷达具有低测量误差和较高的扫描频率，因此扫描匹配的方法对于激光传感器来说具有比较准确的效果。求解扫描匹配问题首先需要确定地图模型。传统的占据栅格地图的精度取决于栅格的大小，即地图的分辨率，存在固有的精度问题，当栅格的大小过大时，地图会呈现出不够平滑的现象，会影响到机器人定位的精度。因此本文将加权SDF地图引入栅格地图之中，该地图可以以低于栅格大小的精度捕获环境，同时具有更高的定位精度。

2.1 加权SDF地图

SDF地图是基于占据栅格地图之上的一种改进地图。SDF地图中，使用同一(或多个)栅格内的激光扫描点拟合直线来描述检测到的物体轮廓，物体轮廓以子栅格大小的精度用SDF进行描述，SDF值计算栅格到回归线的距离，并根据SDF值判断当前栅格的符号，由于回归线将穿过栅格，所计算的SDF值的绝对值将小于栅格大小，因此该地图具有低于栅格大小的精度。如图1所示，由栅格M中的三个激光点通过戴明回归拟合的直线f(x)，栅格M的SDF值为其中心到直线f(x)的距离。当机器人位置P到栅格的距离大于其到直线f(x)的距离时，栅格更新为负的SDF值，反之栅格更新为正的SDF值。SDF地图在使用符号距离函数更新物体轮廓时，使用所有时间的测量平均值。该方法可以减少高斯传感器噪声引起的误差。然而在每次扫描更新时，不仅会更新扫描点所在栅格，还会更新周围其他栅格，因此在所有栅格更新时取平均值会将噪声引入地图。本文引入三维重建中[14-15]中使用加权距离的思想，对SDF地图值的每次更新进行加权处理，减少噪声的影响，使得更新值更为平滑和有效。

图1 WSDF地图原理示意

在加权SDF地图中，每次更新栅格的SDF值，即栅格距离回归线的距离，并根据一个栅格的四个顶点所存储的SDF值的符号判断其是否被占据。具体规则如下：

若某栅格的四个顶点的值均为同符号(同为正值或同为负值)，则说明该栅格一直位于某回归线的同侧，即该栅格不存在障碍物。

若某栅格的四个顶点的值至少存在一个值的符号不同，则说明该网格位于某回归线的异侧，即该栅格存在障碍物。

加权SDF地图以距离加权每次的更新值，假设某栅格的更新值为d，该栅格当前权值为w。对于权重w，计算规则如下：

(6)

式中：dmin与dmax为距离d上限值与下限值；Wmax为权值的最大值。

当前权值的大小决定了是否进行更新，决策规则如下：若w≥r%×wlast_max，则更新；反之则不进行更新。其中wlast_max为最近最大更新的权值。上述规则集成了接近受噪声影响的最后更新的最大权值的距离值。每一时刻的更新规则如下：

(7)

为了防止W快速饱和(达到上限值Wmax)，通常令wt+1=1。

2.2 LM方法

在Hector SLAM中，使用高斯牛顿法求解扫描匹配问题[3]。假设某时刻下机器人位姿为q=(T,θ)，基于加权SDF地图的扫描匹配问题具有如下形式[13]：

(8)

式中Si=R(θ)·di+T，表示机器人当前位姿q下扫描端点di的世界坐标。式(8)的意义在于求解使得地图SDF值最小时机器人相邻时刻的位姿增量。使用高斯牛顿法可以求得扫描匹配问题的解：

Δq=H-1g

(9)

(10)

(11)

(12)

当栅格不存在障碍物时，即栅格四个顶点符号相同时，地图值及其导数可由双线性插值法求得：

M(di)≈|y(m3x+m2(1-x))+(1-y)(m1x+m0(1-x))|

(13)

(14)

当栅格存在障碍物时，此时地图梯度近似值是扫描端点di及其在回归线上的正交投影h之间的相应轴的距离，地图值近似为二者之间的欧氏距离：

(15)

由于高斯牛顿法所计算的H矩阵只有半正定性，所以在使用高斯牛顿法时可能出现H为奇异矩阵的情况，此时会造成增量的稳定性较差，导致算法不收敛。同时当所求步长Δq较大时，也会使得局部近似不够准确。因此本文采用L-M方法求解式(8)。

L-M算法[9]是一种求解非线性最优化问题的算法，该算法属于一种信赖域算法。其思想是首先从初始点假设一个可信赖的最大位移s，并在以当前点为中心、以s为半径的区域内，通过寻找目标函数的一个近似函数(二次的)的最优点来求解得到真正的位移。根据求解结果计算目标函数值，判断本次位移是否可靠，若可靠则按此规则继续迭代；反之则应减小信赖域的范围，放弃本次求解结果，并重新求解。

对于L-M方法，式(8)的解为：

Δq=(H+μI)-1g

(16)

当q>0时，此次迭代有效，并减小惩罚因子。

(17)

当q≤0时，此次迭代无效，需要增大惩罚因子。

(18)

L-M方法中，通过信赖域的方法保证了H+μI矩阵的非奇异性。当μ的值较小时，Δq接近高斯牛顿法的方向，μ的值很大时，Δq接近梯度下降的方向。L-M方法通过μ控制每次迭代的步长，是高斯牛顿法和梯度下降的充分结合，使得每一次迭代下降的步长更加精确，同时大大减小目标函数陷入局部极小值的情况。

3 仿真实验

为了验证所提出的方法的可行性，本文首先通过录制的数据集对L-M方法和高斯牛顿法的实验结果进行了对比，并对本文提出的方法和传统Hector SLAM进行对比。最后进行ROS仿真实验，通过分析算法结果的误差、生成的轨迹和地图的质量等方面对比了本文方法和它们之间的差异。本文使用在L-M方法和高斯牛顿法在相同的数据集上进行了实验，两种方法都基于占据栅格地图。通过分析扫描匹配的均方根误差rmse，可以度量二者扫描匹配的准确度。由于经过双线性插值后的栅格地图的网格值可以看作一个连续值的分布，它是该网格是否存在障碍物的概率值，因此匹配的目的即为使得式(4)的值尽可能趋近于0。图2为不同时刻两种方法的误差曲线图，可以比较两种方法在每一时刻的误差，其中误差计算方式如下：

(19)

(a) 高斯牛顿法(b) L-M方法图2 两种方法的误差箱型图

图2显示了1 500多个时刻两种方法的扫描匹配误差的箱型图，可以看出与高斯牛顿法相比，L-M方法具有较好的稳定性，且有效降低了扫描匹配误差。这证明了L-M方法的有效性。

通过将本文方法与Hector SLAM在数据集上的实验，对比两种方法对机器人的位姿估计与真实轨迹，验证了本文方法相比于Hector SLAM具有较低的定位误差。图3为实验结果对比图。

图3 轨迹对比图

图4、图5为图3轨迹放大图，可以看出，本文使用加权SDF地图的方法与真实轨迹较为接近，而Hector SLAM大约存在5 cm的误差，这在机器人发生转向时尤为明显。

图4 轨迹局部放大图1

图5 轨迹局部放大图2

为了更好地对所提方法进行验证，本文通过ROS机器人操作系统，在Gazebo中使用house仿真场景对所提方法和Hector SLAM进行实验。仿真环境中使用HLS-LFCD2激光雷达，360°激光扫描，其主要测量参数如表1所示。

表1 HLS-LFCD2激光雷达主要测量参数

通过对同一仿真环境的建图和定位，对比分析了两种方法的实验结果。图6为两种方法的建图结果。

图6 Hector SLAM(左)与所提方法(右)建图结果

可以看出，Hector SLAM在标记处出现了建图误差。圆圈标记处为场景房间中的大门，大门一侧出现了墙壁过长的现象。同时方框标记处为场景某房间门口处，其墙体出现了错位。而所提方法在圆圈标记处房间大门墙体平整，没有出现墙体过长的现象。同时方框标记处墙壁没有出现错位的情况，构建效果较好。通过仿真结果表明，在仿真场景下改进的SLAM方法能更好地构建环境地图。

同时本文对比了两种方法定位结果与真实轨迹的误差，结果如图7所示。可以看出Hector SLAM在时间大约400 s之后出现了较大的定位误差，由表1中可知其误差峰值到达了0.18 m。而所提方法的误差稳定在0.06 m以下，具有较好的定位精度。

图7 两种方法轨迹误差曲线图

表2 两种方法误差数据对比 单位：m

图8、图9为Hector SLAM与本文方法与真实轨迹的对比图。由图8可知，正如误差曲线图所示，Hector SLAM在400时刻附近进行转向时产生了较大的误差，且之后一直无法修正，从而影响到了对环境地图的构建结果。而图9中本文方法在进行多次转向时仍能保持比较准确的定位结果，与真实轨迹基本一致，具有较好的定位精度。

图8 Hector SLAM轨迹对比图

图9 本文方法轨迹对比图

上述仿真实验结果以及对实验结果的分析与对比可以证明在仿真环境下，相比于传统Hector SLAM，本文方法有效降低了机器人的定位估计误差，并且具有较好的建图结果。

4 结 语

本文提出了一种基于加权SDF的激光SLAM方法。该方法使用L-M法对Hector SLAM中的高斯牛顿法进行了改进。同时提出一种加权SDF地图，并将其引入到SLAM方法中。经仿真实验验证，与传统Hector SLAM相比，本文方法有效提高了位姿估计的精确性，从而能得到更精确的地图构建，具有一定的应用价值。