基于高斯牛顿的局部优化SLAM系统

北京信息科技大学自动化学院，北京 100192

一、前言

同时定位与建图（Simultaneous Localization and Mapping, SLAM[1]）作为一种基础技术，是指搭载特定传感器的主体在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自己的运动。从最早的军事用途（核潜艇海底定位就有了SLAM的雏形）到今天，SLAM已经逐步走入人们的视野。由于计算机视觉技术的快速发展，摄像头和激光雷达成本的降低，SLAM正在AR、机器人、无人机、无人驾驶等领域发挥重要作用。

近些年来，随着传感器和相关SLAM算法的发展，SLAM研究取得了长足的进展。研究的热点根据传感器的不同大体上可分为基于激光的SLAM，以及基于视觉的SLAM。视觉SLAM有很丰富的信息量，能够很好地识别环境，但这种方法制出来的地图不够精确，而且由于需要处理的数据过多，无法保证算法的实时性。激光SLAM制图较为准确，且有很高的实时性，很适合室内小环境的SLAM。

激光传感器抗干扰性好，所得信息准确。相较于其他主动传感器，激光测距仪在获得某一帧的数据时，能够测量更大的角度和更多的数据点，而且测量的距离信息也更加准确。虽然利用激光雷达作为外部传感器的机器人SLAM成本高昂，但却是目前为止最稳定、最可靠的SLAM方式。

现如今，SLAM技术已经展示了广阔的发展空间，而且在各个领域占据着举足轻重的地位，这使得SLAM问题的研究极具意义。近年来，在SLAM问题上已经有了大量的研究，目前应用最广的2D SLAM方法是Hector和Gmapping[2]。Rao-Blackwellized 粒子滤波算法（Rao-Blackwellized Particle Filter, RBPF）[3]是目前对于室内同时定位与建图的典型解决方案。

SLAM系统可以分为前端和后端两部分。SLAM前端用于估计机器人实时在线运动，最常用的方法是扫描匹配[4-5]方法。而后端根据前端已经生成的位姿图来进行优化。主流方案就两种，基于概率学理论的贝叶斯滤波器（EKF[6-7], PF）以及基于优化的方法。

随着移动机器人研究的兴起，扫描匹配方法被迅速地应用于机器人定位和环境建图之中。其中，迭代最近点（ICP）[8-9]方法是实现扫描匹配中最常用的一种方法。ICP的主要缺点是需要反复寻找对应点，在应用中收敛速度慢，计算成本很大。

本文采用Sobel算子来计算栅格地图的梯度，再对其进行双线性插值获得地图上任意点的梯度，通过高斯-牛顿方法来寻找局部最优值，充分利用激光雷达的高更新率，采用较低的计算成本的方法，可以实现可靠精度的建图和定位的功能。

二、SLAM系统的构成

该SLAM系统主要分为栅格地图的构造、激光数据匹配求取位姿、更新地图，最终建立整体地图。

1、地图构造

栅格地图作为一种激光雷达在实际环境中进行定位的已证实方法，可以呈现任意环境。但栅格地图的离散属性限制了地图精度，也不允许直接计算插值或梯度值。这里采用一种双线性过滤的方案来估计栅格概率值和梯度值，这样一来栅格地图单元格值就可以被视为一个连续概率分布。

给定一个连续地图坐标Pm，其栅格概率值和梯度值可以近似通过距离Pm周围最近的4个整数坐标P00，P10，P01，P11来计算，Pm地图概率值M(Pm)具体实现按式（1）进行插值[10]：

这样我们已经获得的近似地图的表达式，对其求导就可以进一步求取地图的梯度，具体的求梯度的方法是对式（1）分别求偏导，如式（2）和式（3）：

2、激光匹配

扫描匹配是将激光扫描数据与现有的地图进行对齐的过程。现在的激光雷达具备测量噪声低和扫描频率高的特性，所获取的激光数据比较准确，因此扫描匹配方法可以产生一个很准确的结果。

采用的方法是优化对齐激光数据到现有的地图中，我们试图找到激光雷达的位姿ξ=(px,py,φ)T，使以下目标函数式（4）达到最小：

其中，M(Si(ξ))—表示Si(ξ)坐标下为障碍物的概率，1表示100%障碍物，0表示100%空闲区域。

我们想要找到最好的位姿转换来把激光扫描数据校准到现有的地图中。其中根据给定机器人的位姿，激光点束的端点在世界坐标下的坐标计算如式（5）所示：

给定一个初值ξ，我们想通过估计Δξ来优化以下测量误差，使得式（6）趋于零：

通过泰勒展开M(Si(ξ+Δξ))得到式（7）：

这个方程通过设置式（8）关于Δξ的偏导数为零来求最小值：

通过解出Δξ来实现高斯-牛顿最小化问题：

这样我们就能求出将激光数据对齐到现有地图中的估计位姿。

3、地图的更新

在求解出机器人姿态以后，我们已经能够根据现在的姿态更新地图了，更新地图的具体方法为，对于地图的网格的原始数据：

激光束的顶点（hit），存在障碍物的可能性变高，所以增加一个权重；

激光束上的其他点（miss），存在障碍物的可能性变小，所以减少一个权重；

激光束的始点（original point），存在障碍物的可能性变小，所以减少一个权重。

根据以下公式（12）可以求出障碍物存在的概率：

其中，x—代表网格的原始数据的值；

M—代表障碍物存在的概率。

三、改进获取占据栅格地图梯度的近似方法

原来获取占据栅格地图概率值和地图梯度的近似方法是先通过双线性插值得到栅格地图的概率值，然后对其求导获取地图梯度。栅格地图的概率值本身就是线性插值近似的，再对其求导得到的地图梯度的误差就会更大。于是，参照获取地图概率值的方式，通过图像获取梯度的方法先获取离散的梯度值矩阵，再对其进行插值计算。

这里采用Sobel算子来求取栅格地图的梯度，在x，y两个方向上的Sobel算子模板为：

通过对栅格地图M的二维矩阵用Sobel算子作为相关核进行滤波，可以获取栅格地图梯度的离散值，gx和gy分别为x，y两个方向上的地图梯度矩阵。然后和获取地图概率值一样，再对栅格地图梯度值进行插值：

原有方法对栅格地图概率值M直接求导获取地图梯度。改进获取地图梯度的方法后，利用Sobel算子作为相关核和栅格地图概率值M进行相关运算，求出地图梯度的离散矩阵gx和gy，再对其进行双线性插值。采用原有方法和改进方法分别对模拟实验数据进行建图测试，效果图见图1和图2。

通过对照图1和图2的效果，可以明显看出改进后的建图精度得到了提升。

四、实验

为了进一步验证改进方法，在实验室环境下利用机器人采集真实激光雷达扫描数据，进行建图实验。

依照上述SLAM系统的框架逻辑，首先建立采用一种可通过双线性过滤的方案来估计栅格概率值和梯度值的近似连续分布的栅格地图，然后根据高斯-牛顿优化方法来寻求能获得当前观测值的栅格地图上的最优位姿，从而确定两帧数据的位姿关系，最后把当前的观测数据更新到栅格地图里，为下一次激光数据的匹配做准备，实现了同时定位与建图的功能。

为了测试编写算法的效果，在实验室搭建环境，机器人采集数据，进行建图效果的测试。机器人的激光雷达采用的是HOKUYO公司的UTM-30LX激光扫描测距仪，该产品拥有30m，270°测量范围，DC12V输入，采用ToF（Time of Flight）激光时间飞行原理进行距离测量。

在实验室搭建如图3所示环境，利用手柄控制机器人的运动，使机器人的激光雷达能够逐渐扫描到整个周围环境，寻求把每一帧新的扫描数据对齐到地图中的最优位姿，根据位姿更新现有地图。当机器人的激光雷达获取了环境中的所有数据信息，就可以完整地构建出室内环境的二维地图。建图效果如图4所示。

五、结论

本文充分利用激光雷达的高更新率，采用较低的计算成本的SLAM方法。改进了获取栅格地图梯度的方法，利用Sobel算子和栅格地图概率值矩阵进行互相关运算，求出地图梯度的离散矩阵，再对其进行双线性插值，获得地图上任意点的梯度。通过高斯-牛顿方法来寻求每一帧新的观测数据对齐到现有地图的最优位姿，确定了机器人的位姿就可以根据位姿把观测数据更新到地图中，从而建立环境地图。