从零学会XR-ROS SLAM自主导航机器人开发之第五章 SLAM及导航算法解析

lina

一、常见SLAM算法讲解

SLAM是Simultaneous localization and mapping缩写，意为“同步定位与建图”，主要用于解决机器人在未知环境运动时的定位与地图构建问题，为了让大家更多的了解SLAM，以下将从SLAM的应用领域、SLAM框架、SLAM分类（基于传感器的SLAM分类）来进行全面阐述。

1、SLAM的典型应用领域

机器人定位导航领域：地图建模。SLAM可以辅助机器人执行路径规划、自主探索、导航等任务。

无人机领域：地图建模。SLAM可以快速构建局部3D地图，并与地理信息系统（GIS）、视觉对象识别技术相结合，可以辅助无人机识别路障并自动避障规划路径，如大疆测绘无人机，就应用到了SLAM技术。

无人驾驶领域：视觉里程计。SLAM技术可以提供视觉里程计功能，并与GPS等其他定位方式相融合，从而满足无人驾驶精准定位的需求。例如，应用了基于激光雷达技术Google无人驾驶车以及牛津大学MobileRoboticsGroup11年改装的无人驾驶汽车野猫(Wildcat)均已成功路测。

2、SLAM框架

SLAM系统框架如图所示，一般分为五个模块，包括传感器数据、视觉里程计、后端、建图及回环检测。

传感器数据：主要用于采集实际环境中的各类型原始数据。包括激光扫描数据、视频图像数据、点云数据等。

里程计：主要用于不同时刻间移动目标相对位置的估算。包括特征匹配、直接配准等算法的应用。

算法：优化里程计带来的累计误差。包括滤波器、图优化等算法应用。

建图：用于地图构建

回环检测：主要用于空间累积误差消除

3、SLAM分类（基于传感器的SLAM分类）

目前用在SLAM上的传感器主要分为这两类，一种是基于激光雷达的激光SLAM(Lidar SLAM)和基于视觉的VSLAM(Visual SLAM)。

3.1.激光SLAM

激光SLAM采用2D或3D激光雷达（也叫单线或多线激光雷达），2D激光雷达一般用于室内机器人上（如扫地机器人），而3D激光雷达一般使用于无人驾驶领域。激光雷达的出现和普及使得测量更快更准，信息更丰富。激光雷达采集到的物体信息呈现出一系列分散的、具有准确角度和距离信息的点，被称为点云。通常，激光SLAM系统通过对不同时刻两片点云的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，也就完成了对机器人自身的定位。

激光雷达测距比较准确，误差模型简单，在强光直射以外的环境中运行稳定，点云的处理也比较容易。同时，点云信息本身包含直接的几何关系，使得机器人的路径规划和导航变得直观。激光SLAM理论研究也相对成熟，落地产品更丰富。

3.2、视觉SLAM

眼睛是人类获取外界信息的主要来源。视觉SLAM也具有类似特点，它可以从环境中获取海量的、富于冗余的纹理信息，拥有超强的场景辨识能力。早期的视觉SLAM基于滤波理论，其非线性的误差模型和巨大的计算量成为了它实用落地的障碍。近年来，随着具有稀疏性的非线性优化理论(Bundle Adjustment)以及相机技术、计算性能的进步，实时运行的视觉SLAM已经不再是梦想。

视觉SLAM的优点是它所利用的丰富纹理信息。例如两块尺寸相同内容却不同的广告牌，基于点云的激光SLAM算法无法区别他们，而视觉则可以轻易分辨。这带来了重定位、场景分类上无可比拟的巨大优势。同时，视觉信息可以较为容易的被用来跟踪和预测场景中的动态目标，如行人、车辆等，对于在复杂动态场景中的应用这是至关重要的。

通过对比我们发现，激光SLAM和视觉SLAM各擅胜场，单独使用都有其局限性，而融合使用则可能具有巨大的取长补短的潜力。例如，视觉在纹理丰富的动态环境中稳定工作，并能为激光SLAM提供非常准确的点云匹配，而激光雷达提供的精确方向和距离信息在正确匹配的点云上会发挥更大的威力。而在光照严重不足或纹理缺失的环境中，激光SLAM的定位工作使得视觉可以借助不多的信息进行场景记录。

二、常见SLAM算法

1、HectorSLAM

HectorSLAM是一种结合了鲁棒性较好的扫描匹方法2D SLAM方法和使用惯性传感系统的导航技术。传感器的要求：高更新频率小测量噪声的激光扫描仪,使空中无人机与地面小车在不平坦区域运行存在运用的可能性.作者利用现代激光雷达的高更新率和低距离测量噪声，通过扫描匹配实时地对机器人运动进行估计。所以当只有低更新率的激光传感器时，即便测距估计很精确，对该系统都会出现一定的问题，如下图是该系统生成的二维地图。

利用已经获得的地图对激光束点阵进行优化, 估计激光点在地图的表示,和占据网格的概率.，其中扫描匹配利用的是高斯牛顿的方法进行求解. 找到激光点集映射到已有地图的刚体转换(x,y,theta).具体的公式如下：

下图二维地图是hectorSLAM 多分辨率地图的表示。 网格单元格长度（从左到右）：20厘米，10厘米和5厘米 ，使用多分辨率地图表示，以避免陷入局部最小化。 这个解决方案背后的想法是在内存中有不同的地图，同时使用之前估计的姿势来更新它们。 计算成本 仍然很低，所以地图总是一致的

最后，3D空间导航状态估计是基于EKF滤波器。 但是，这种情况是仅在IMU存在时才需要，例如在空中机器人的情况下。 因此，这里我们只讨论二维的SLAM

它会将不再这项工作中使用。如下图显示了HectorSLAM方法的概述。

2、Gmapping

Gmapping是一种基于激光的SLAM算法，它已经集成在ROS中，是移动机器人中使用最多的SLAM算法。这个算法已经由Grisetti等人提出是一种基于 Rao-Blackwellized的粒子滤波的 SLAM方法。基于粒子滤波的算法用许多加权粒子表示路径的后验概率，每个粒子都给出一个重要性因子。但是，它们通常需要大量的粒子才能获得比较好的的结果，从而增加该算法的的计算复杂性。此外，与PF重采样过程相关的粒子退化耗尽问题也降低了算法的准确性。粒子退化问题包括在重采样阶段从样本集粒子中消除大量的粒子。发生这种情况是因为它们的重要性权重可能变得微不足道。因此，这意味着有一定的小概率时间会消除正确的假设的粒子。为了避免粒子的退化问题，已经开发了自适应重采样技术。

作者还提出了一种计算精确分布的方法，不仅考虑机器人平台的运动，还考虑最近的观察。提出了在使用测距运动模型时结合观测值提出建议分布。然而，当一个移动机器人装备一个非常精确的传感器LRF时，可以使用该传感器的模型，因为它可以实现了极高的似然函数。 基于此，作者整合了最近的传感器观测zt，并且他们计算高斯分布近似有效地获得下一代粒子分布。

这种自适应重采样降低了PF预测步骤中机器人姿态的不确定性。 结果，由于扫描匹配过程，所需的粒子数量减少，因为不确定性较低。 实验中，Gmapping使用的粒子数量是30，与普通PF方法相比，这是非常低的。

3、LagoSLAM

LagoSLAM 是线性近似图优化,不需要初始假设。基本的图优化slam的方法就是利用最小化非线性非凸代价函数.每次迭代, 解决局部凸近似的初始问题来更新图配置,过程迭代一定次数直到局部最小代价函数达到. (假设起始点经过多次迭代使得局部代价函数最小). 。假设图中每个节点的相对位置和方向都是独立的，作者求解了一个等价于非凸代价函数的方程组。为此，提出了一套基于图论的程序，通过线性定位和线性位置估计，得到非线性系统的一阶近似。

4、KartoSLAM

KartoSLAM是基于图优化的方法，用高度优化和非迭代 cholesky矩阵进行稀疏系统解耦作为解．图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新．

KartoSLAM的ROS版本，其中采用的稀疏点调整（the Spare Pose Adjustment(SPA)）与扫描匹配和闭环检测相关．landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大.在某些情况下KartoSLAM更有效,因为他仅包含点的图(robot pose),求得位置后再求map.

5、CoreSLAM

COreSLAM最小化性能损失的一种slam算法.将算法简化为距离计算与地图更新的两个过程,  第一步,每次扫描输入,基于简单的粒子滤波算法计算距离,粒子滤波的匹配器用于激光与地图的匹配,每个滤波器粒子代表机器人可能的位置和相应的概率权重,这些都依赖于之前的迭代计算. 选择好最好的假设分布,即低权重粒子消失,新粒子生成..在更新步骤,扫描得到的线加入地图中,当障碍出现时,围绕障碍点绘制调整点集,而非仅一个孤立点。

6、cartographer

cartographer是Google的实时室内建图项目，传感器安装在背包上面，可以生成分辨率为5cm的2D格网地图。获得的每一帧laser scan数据，利用scan match在最佳估计位置处插入子图（submap）中，且scan matching只跟当前submap有关。在生成一个submap后，会进行一次局部的回环（loop close），利用分支定位和预先计算的网格，所有submap完成后，会进行全局的回环。

三、move_base路径规划算法

1、在总体框架图中可以看到

move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：

（1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划；

（2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。

2、数据结构

ROS中定义了MoveBaseActionGoal数据结构来存储导航的目标位置数据，其中最重要的就是位置坐标（position）和方向（orientation）。

rosmsg show MoveBaseActionGoal

[move_base_msgs/MoveBaseActionGoal]:

std_msgs/Header header

uint32 seq

time stamp

string frame_id

actionlib_msgs/GoalID goal_id

time stamp

string id

move_base_msgs/MoveBaseGoal goal

geometry_msgs/PoseStamped target_pose

std_msgs/Header header

uint32 seq

time stamp

string frame_id

geometry_msgs/Pose pose

geometry_msgs/Point position

float64 x

float64 y

float64 z

geometry_msgs/Quaternion orientation

float64 x

float64 y

float64 z

float64 w

3、配置文件

move_base使用前需要配置一些参数：运行成本、机器人半径、到达目标位置的距离，机器人移动的速度，这些参数都在rbx1_nav包的以下几个配置文件中：

base_local_planner_params.yaml

costmap_common_params.yaml

global_costmap_params.yaml

local_costmap_params.yaml

4、全局路径规划（global planner）

在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。这一功能是navfn这个包实现的。

navfn通过Dijkstra最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。将来在算法上应该还会加入A*算法。

具体见：http://www.ros.org/wiki/navfn?distro=fuerte

5、本地实时规划（local planner）

本地的实时规划是利用base_local_planner包实现的。该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。

base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。

其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：

（1） 采样机器人当前的状态（dx,dy,dtheta）；

（2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。

（3） 利用一些评价标准为多条路线打分。

（4） 根据打分，选择最优路径。

（5） 重复上面过程。

蒙特卡洛：是一种思想或方法。举例：一个矩形里面有个不规则形状，怎么计算不规则形状的面积？不好算。但我们可以近似。拿一堆豆子，均匀的撒在矩形上，然后统计不规则形状里的豆子的个数和剩余地方的豆子个数。矩形面积知道的呀，所以就通过估计得到了不规则形状的面积。拿机器人定位来讲，它处在地图中的任何一个位置都有可能，这种情况我们怎么表达一个位置的置信度呢？我们也使用粒子，哪里的粒子多，就代表机器人在哪里的可能性高。

AMCL是移动机器人在2D环境中的概率定位系统。它实现了自适应（或KLD采样）蒙特卡罗定位方法，其使用粒子滤波器来针对已知的地图跟踪机器人的位姿。目前情况下，该节点仅能使用激光扫描数据和激光雷达地图来工作。它可以通过修改代码以扩展到其他传感器数据。

AMCL接收基于激光的地图，激光扫描和tf变换消息，并输出位姿估计。在启动时，amcl根据提供的参数初始化其粒子滤波器。注意，如果没有设置初始化参数，则根据默认值初始滤波器状态将是以（0,0,0）为中心的中等大小的粒子云。在rviz中可以通过2D Pose Estimate按钮来初始化位姿。

但是，从图中可以看出，激光数据不是很准确，所以需要在teleop_twist_keyboard的终端小范围前后左右移动小车，小车逐步确定它自身的位置。 判断小车是否定位成功，主要看小车周围红色的点是否收缩到一起。如图为收敛后的粒子图: