基于图优化的二维激光SLAM 研究

翁潇文，李 迪，柳俊城

（华南理工大学 机械与汽车工程学院，广州510640）

生产环境的平面图是移动机器人进行自主导航的依据，通常由人工进行测量，然后利用计算机辅助设计CAD（computer aided design）整理而成。人工测量通常会将障碍物边缘视为直线进行计算，存在一定的误差，还有耗费时间长，测量难度大，后续处理困难等缺点。 移动机器人运动过程中的同时定位与地图构建SLAM 算法的提出正是为了解决这一问题，为移动机器人自主导航奠定基础。 SLAM问题可以描述为：在未知环境中移动机器人从随机起点开始运动，根据运动过程中得到的传感器数据预估推测机器人本体的位姿， 构建出环境地图，同时标示出机器人在地图中的位姿[1]。

1 SLAM 及其问题

1.1 SLAM 分类

目前，主流的同时定位与地图构建SLAM 算法可根据主要传感器的不同而分为2 种，一种是基于激光传感器的SLAM 算法， 其特点是距离测量准确，数据处理比较简单。 常见的激光传感器有机械式激光雷达、固态雷达等。 另一种是基于视觉传感器的SLAM 算法，具有应用场景丰富，适用于复杂动态场景，适合多机协作，等特点。 常见的传感器有单目摄像头、双目摄像头、红外摄像头等[2]。

根据算法原理的不同，SLAM 算法可分为滤波法和图优化法。 滤波法是根据滤波算法，如扩展卡尔曼滤波、粒子滤波、无损卡尔曼滤波等，对每一帧观测数据进行处理后推测出机器人位姿，进而将观测数据拼合而成环境地图。 图优化法则是根据历史所有观测数据，以机器人的位姿作为点，位姿之间的关系作为边构建图，而后调整机器人位姿尽量满足边的约束，最后得出环境地图。 随着待建地图尺寸的扩大， 图优化法比滤波法的计算量增长更慢，内存占用少，并且更容易达成闭环[3-4]。

在此， 以虚拟机器人试验平台V-REP（virtual robot experimentation platform） 为基础搭建SLAM算法仿真验证平台，提出一种基于图优化理论的激光SLAM 算法，并在仿真环境和实际环境中验证算法可行性。

1.2 SLAM 问题

在二维地图中，机器人的位姿可以用向量（x，y，θ）（其中，x 和y 分别为机器人的横、 纵坐标；θ 为机器人转过的角度）表示，建立观测方程和运动方程，即

式（1）中：xt，yj，zt，j分别为t 时刻时，机器人的位姿、第j 个路标的位置、 传感器所获得的数据；vt，j为本次观测中的噪声。 观测方程（1）描述了机器人观测到路标的过程。 式（2）中：ut为t 时刻机器人运动传感器的输入， 也可以理解为机器人的位移；xt和xt-1分别为t 时刻及其上一时刻机器人的位姿；wt为噪声。 运动方程（2）描述了机器人运动的过程。

采用观测方程和运动方程将SLAM 问题以数学的形式表达。SLAM 问题可以归结为：当知道运动传感器的数据u 及激光传感器的读数z 时， 同时求解定位问题（即求解x 值）和建图问题（即求解y值）。

因此，可以把SLAM 问题建模成一个状态估计问题，即通过带有噪声的传感器测量数据，估计动态系统内部隐藏的状态变量，如图1 所示。

图1 SLAM 算法模型Fig.1 SLAM algorithm model

2 SLAM 算法

在图1 所示SLAM 问题模型中， 已知量u，z 的值与真实值间存在一定误差，具体表现为在闭环的运动建图过程中：

根据式（3）、式（4）所示的SLAM 模型，需要求出机器人的位姿x 和路标的位置y， 并使测量值尽可能地接近真实值，所以其本质为求解一个最小二乘问题。

这种将待求解的量作为节点，已知量作为约束建立成边，节点和边构造成图，通过求解非线性最小二乘问题使图的总误差最小的方法称为图优化法。 在SLAM 问题中，图优化法相比于传统的滤波法，具有计算速度快，能构造回环，地图累积误差较小等优点。

算法将输入的激光传感器数据依据数据量和测量范围分割为多个子数据集，在每个子数据集中采用激光数据配准的方法求解SLAM 问题，得到一个局部地图，称为子地图。 子地图具有范围小、累积误差小、计算量少等特点。 子地图之间存在一定的累积误差，依据图优化的理论，将子地图数据（云数据和在全局地图中的位姿）作为图的节点，观测数据和回环约束作为边构造图，经优化后使子地图集与真实环境的总误差最小， 然后更新子地图数据，得出环境地图。

2.1 点云预处理

子地图的尺寸需要根据待测环境进行调整。 子地图尺寸过小可能导致子地图之间累积误差过大，会导致子地图数目过多，使得回环检测难度上升，甚至无法完成回环；子地图尺寸过大则可能导致单帧子地图内的位姿估算难度上升，子地图的准确性下降。 在确定子地图的尺寸后，首先对激光点云数据进行尺寸滤波，舍弃超出子地图范围的点云数据。

经过初次滤波后的激光雷达点云数据，因其具有数据量大、存在随机误差、重复数据多等特点，需要再次进行点云滤波。 此次滤波采用体素滤波思想对点云数据进行处理。

体素滤波可以使点云在保持形状特征的基础上大幅度减少点云数据，减轻后续计算负荷。 其原理为，在点云数据中创建大小一致的虚拟三维体素栅格，以每个体素中点云的重心近似表示体素中所有的点云数据。 虽然以重心点表示比采用体素中心表示的计算量大，但对点云的形状特征保存更加准确。 具体操作如下：

定义最大边长和最小体素数，以最大体素边长开始进行体素滤波，比较滤波后的体素数是否大于等于最小体素数， 没有则体素边长减半进行滤波，直至滤波结果大于等于最小体素数。 最大体素边长和最小体素数的选取， 需要根据待测环境进行调整，体素数过少则点云丧失形状特征，后续难以检测回环；体素过多则无法减轻后续计算压力，失去滤波的意义。

2.2 构建子地图

首先，定义“激光数据配准”，以求解2 帧雷达数据或者雷达数据与已有地图之间的位移矩阵（平移和旋转）。 激光数据配准后，可使机器人在不同时刻、不同位姿下测得的点云数据统一到一个坐标系中，这就是构建地图的基本原理。 子地图的构建就是一个重复迭代配准激光数据的过程，根据计算出的最佳位姿估计将点云数据插入地图相应的位置，从而构建基础模型，即

式中：xt和xt-1为当前时刻和上一时刻机器人的位姿；zt为当前时刻所有点云数据的集合，每个点云数据是一个矢量， 代表该点相对机器人的距离和角度；mt-1为添加上一时刻数据后的地图即现有地图；ut-1为上一时刻到这一时刻的运动量；p（zt|xt，mt-1）为观测概率，即求解在已知地图下，获得某一观测值zt时，可能性最大的机器人位姿；p（xt|xt-1，ut-1）为运动概率， 即已知机器人上一时刻位姿和位移量，求解当前时刻可能性最大的机器人位姿。 最后，根据当前位姿， 将对应的点云数据经平移旋转后，添加到子地图相应位置。

在子地图得到一定数量的点云数据后，将子地图栅格化。 根据点云的位置，不可通行区域（或障碍物）以黑色表示，灰度为0；可通行区域以白色表示，灰度为255；未知区域灰度为128。 至此子地图构建完毕。

2.3 构建全局地图

由于每一帧子地图都是单独构建的，相互之间存在误差，为消除这一误差，需要对子地图进行匹配。 在检测到匹配度足够高的子地图后，可以认为在这2 帧子地图中机器人观测到了同一路标， 即2帧地图描述了同一场景，机器人经过这段时间的位移后回到之前的坐标附近，即为回环检测。 匹配度采用迭代最近点算法ICP（iterated closest points）进行衡量，即

式中：p 和p′为匹配的点云对；R 为旋转矩阵；t 为平移矩阵；J 为误差平方和。 当J 足够小时认为完成匹配，根据J 值衡量匹配度，J 值越小匹配度越高，J 值为0 时2 帧点云数据完全一致。 然后，计算最小误差下2 帧激光点云图的协方差矩阵， 其逆记为Ω，称为信息矩阵。 信息矩阵也可以用于衡量匹配度。

检测到回环后，根据图优化的思想，构建待优化的图，以子地图相对于全局地图的位姿xt作为点，在不同子地图中的观测数据所产生的约束作为边，在整个过程中所产生的点与边组成图。 其中，图的边可分为运动模型约束和观测模型约束2 种，分别为

式中：xt为机器人的位姿；m 为静态的环境地图；ut和zt分别为机器人的运动量（速度、转向等）、观测量（由外部传感器获取）。 它们都是SLAM 问题中的已知量。 运动模型约束可以理解为子地图之间的相对位姿关系，观测模型约束可以理解为观测到同一路标时子地图的相对关系。 在此仅采用激光传感器，故不存在运动模型约束，只需考虑观测模型约束构造优化方程式。

根据点和边约束构建图优化目标方程，即

这是一个非线性最小二乘问题， 其求解过程为：步骤1 根据目标方程求解点（位姿）初始的估计值u0；步骤2 对信息矩阵Ω 降维，求优化的梯度方向；步骤3 梯度方向迭代，直至收敛，返回优化结果。 在程序中，采用通用图优化算法G2O（general graph optimization）， 对这一图优化问题进行求解。 G2O 是一个开源的C++算法框架，用于求解用图表示的优化问题，其具有计算高效、代码简单、通用性好等优点。

经过优化后，全局地图与环境的误差降低到了理论上的最小值，并且全局地图构成回环，提高了全局地图的准确性。

3 试验过程

试验分别在V-REP（如图2 所示）和现实环境（如图3 所示）中进行。

图2 仿真环境Fig.2 Simulation environment

在现实环境中， 以全向轮AGV 为移动机器人平台，使用基于三角测距原理的激光传感器，频率为10 Hz， 传感器通过串口传输数据。 机器人通过TCP/IP 的方式与ROS 进行通信， 操作系统为Ubuntu 16.04，ROS 版本为kinetic。 试验中，待测环境为摆放有不规则障碍物的室内环境，房间面积为20 m×10 m。

VREP 是由Coppelia Robotics GmbH 开发的三维机器人集成开发环境。VREP 可跨平台使用，支持7 种编程语言，每个模块可单独控制，自带通用计算模块，提供与机器人操作系统的通信接口。在VREP中搭建算法验证环境，能够缩短开发周期，节约开发成本，方便后续代码移植。

4 结果分析

环境地图和试验结果分别如图4，图5 所示。

图4 仿真环境算法结果Fig.4 Algorithm result in the simulation environment

在现实环境和仿真环境中，以图中5 段待测距离，分别为左侧桌子长度、左下实验台长度和宽度、房间总长度和总宽度，计算地图中对应距离与实际距离的误差，试验结果见表1。

试验结果表明，该算法在仿真试验和实际试验中，均可较为准确地完成地图建立以及推算自身位姿的工作，误差波动小，因此仿真环境可以作为现实环境的代替品对算法进行验证。 由于现实环境存在地面不平、车轮打滑等随机影响，致使现实环境下结果的误差要大于仿真环境。

表1 测量误差Tab.1 Measurement error

试验过程由于受激光传感器的限制，与传感器不在同一平面的障碍物无法检测。 对此，可以通过调整传感器高度，重复扫描环境以建立更准确的地图。

5 结语

所提出的基于图优化的移动机器人同步定位与地图构建算法，在仿真环境和实际环境中对算法进行了验证。 传统粒子滤波优化方法需要维护全局地图中的所有粒子，随着地图范围的增大，需要维护的粒子数增多， 对计算机的性能有较高的要求。而图优化方法不需要地图中每一个粒子的信息，减少了累积误差，提高了有效性和鲁棒性。