机器人SLAM原理及应用

段航琪 张峰源 杜啸宇 刘源 臧博

摘 要：机器人同时定位与建图技术在移动机器人、无人驾驶中占有核心地位。按照传感器不同大体分为激光SLAM和视觉SLAM两种，按照算法的不同又可以分为基于滤波器的SLAM算法和基于图优化的SLAM算法。现有的SLAM开源算法有gmapping、cartographyer、ORB-SLAM等。在二位栅格地图上，主要使用A*算法和DWA算法完成导航。

关键词：机器人SLAM；自主导航；A*算法；DWA算法

一、SLAM的起源

移动机器人在进入一个陌生环境时，要解决“我在哪”以及“我周围的环境是怎样的”这两个问题，才可以更好地进行移动和工作。而在位姿估计的同时建立环境地图，就是机器人SLAM技术。

二、SLAM的原理

SLAM主要解決位姿估计和环境地图建立这两个问题，这两个部分是相互依存的。

1.定位

机器人估计自身位姿有两种方案。一是靠里程计信息进行估计，里程计信息来源主要有轮式编码器、惯性测量单元（imu）、摄像头（视觉里程计）这几类传感器；另一种是靠观测路标点进行位姿估计，通过激光雷达扫描得到的深度信息或者摄像头拍照，对机器人当前位置周围的特征点进行提取，然后将提取到的特征点与之前的路标进行匹配，根据机器人对路标点的观测量可以得出机器人当前相对于路标点的位姿。理论上，这两种方法在没有测量误差的情况下都可以单独估计机器人位姿。但由于数据测量噪声的存在，单独使用其中一种无法得到机器人准确的位姿，因此需要将两种数据进行融合，得到效果更好的位姿估计量。融合的方法分为滤波器和图优化两种，经典的SLAM算法是基于滤波器的，而近年来的研究热点为基于图优化的视觉SLAM。

2.建图

机器人在定位过程中同时建立环境地图。最基本的，机器人在定位过程中，将自身位置周围的landmark（地标）位置记录下来，便构成一张记录地标位置的地图。机器人可以通过观测路标并与路标地图比较，从而实现两种定位方法中的观测路标法。

三、SLAM在机器人上的应用——自主导航

1.栅格地图

SLAM在机器人上的应用主要为机器人自主导航。机器人通过环境地图中的路标信息与传感器观测值来估计自身位置，并根据导航目标点和地图来进行路径规划（path planning）和轨迹规划（trajectory planning）。而前面介绍的SLAM原理中的建图环节的地图，只包含路标信息，而不包括路标之外的信息，这种地图称为“稀疏地图”。显然，稀疏地图并不能体现各路标点之间的道路状况，如果机器人使用稀疏地图进行导航，比如直奔某个路标点，那么极有可能在中途碰到障碍物。因此，导航需要的是包含路标信息以及路标之间道路状况的地图，我们称这种包含信息更多的地图为“稠密地图”。稠密地图中应用较广的一种是“占据栅格地图”， 机器人对环境地图的描述的方式最常见的为栅格地图（Grid map）或者称为Occupancy Map，如下图所示。栅格地图就是把环境划分成一系列栅格，其中每一栅格给定一个可能值，表示该栅格被占据的概率。

下面以栅格地图来介绍机器人导航算法。导航算法分为全局路径规划与局部路径规划两部分。

2.全局路径规划——A*导航算法

A*导航算法，俗称A星算法。这是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。常用于游戏中的NPC的移动计算，或网络游戏的BOT的移动计算上。如下图，考虑具有许多障碍的方格，我们给出起始单元和目标单元。我们希望尽可能快地从起始单元到达目标单元。该算法综合了最良优先搜索和Dijkstra算法的优点：在进行启发式搜索提高算法效率的同时，可以保证找到一条最优路径（基于评估函数）。

在此算法中，如果以 表示从起点到任意顶点 的实际距离， 表示任意顶点 到目标顶点的估算距离（根据所采用的评估函数的不同而变化），那么A*算法的估算函数为：

这个公式遵循以下特性：

如果 为0，即只计算任意顶点 到目标的评估函数 ，而不计算起点到顶点 的距离，则算法转化为使用贪心策略的最良优先搜索，速度最快，但可能得不出最优解；

如果 不大于顶点 到目标顶点的实际距离，则一定可以求出最优解，而且 越小，需要计算的节点越多，算法效率越低，常见的评估函数有——欧几里得距离、曼哈顿距离、切比雪夫距离；

如果 为0，即只需求出起点到任意顶点 的最短路径 ，而不计算任何评估函数 ，则转化为单源最短路径问题，即Dijkstra算法，此时需要计算最多的顶点；

3.局部导航算法——DWA

机器人局部路径规划常用DWA算法，动态窗口法主要是在速度（v，w）空间中采样多组速度，并模拟机器人在这些速度下一定时间（sim_period）内的轨迹。在得到多组轨迹后，对于这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。该算法突出点在与动态窗口这个名词，它的含义是依据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围内。该算法主要包含速度采样和评价函数两部分。

（1）速度采样

建立机器人的轨迹运动模型后，根据速度就可以推算出轨迹。因此只需采样很多速度，推算轨迹，然后评价这些轨迹优劣。速度采样的过程为：在速度（v，w）的二维空间中，存在无穷多组速度。但是根据机器人本身的限制和环境限制可以将采样速度控制在一定范围内。

（2）评价函数

在采样的速度组中，有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹进行评价。

参考文献：

作者简介：

段航琪（1999.04.23-） 男，汉族，河南省开封市，本科生，研究方向：自动化

张峰源（1998.10.26-） 男，汉族，河南省周口市，本科生，研究方向：机械工程

杜啸宇（1998.10.8-）男 汉族 河南省信阳市淮滨县 本科 研究方向：汽车工程

刘源 （1999.4.2-）男 ，汉族，河南省郑州市，本科生，研究方向：自动化

臧博（1998.6.18-）男，汉族，山东省泰安市，本科生，研究方向：机械工程