激光SLAM导航系列(一)SLAM与导航基本原理

激光SLAM基本原理

基本原理
关于机器人运动控制系统架构，在《ros by example》 chapter 7一章第二节中介绍了控制机器人的5个层次，从低到高依次是：motor controllers anddrivers-> ROS base controller ->Frame-Base Motion（move_base）->Frame-Base Motion（gmapping + amcl）->Semantic Goals。总结起来如下图所示：

可简单的分为三个层面，最底层，中间通信层和决策层。最底层就是机器人本身的电机驱动和控制部分，中间通信层是底层控制部分和决策层的通信通路，决策层就是负责机器人的建图定位以及导航。本文主要研究激光SLAM(构建2D地图和导航)，所以只探讨决策层这一层的实现。我们在已有机器人最底层的前提下，采用ROS提供的Gmapping包和Navigation栈作为机器人的决策层。
1、占据栅格地图基本原理
Gmapping包是在ROS里对开源社区openslam下gmapping算法的C++实现，该算法采用一种高效的Rao-Blackwellized粒子滤波将收取到的激光测距数据最终转换为栅格地图。
机器人定位与建图通常被认为是“鸡与鸡蛋”的问题，因为这个原因才会将这个过程命名为SLAM（Simultaneous localization and mapping），所以即时定位与地图构建（SLAM）是这样一个概念：把两方面的进程都捆绑在一个循环之中，以此支持双方在各自进程中都求得连续解；不同进程中相互迭代的反馈对双方的连续解有改进作用。
占据栅格地图的构建主要采取粒子滤波的方法，粒子滤波是目前一种可以代替高斯滤波器的广为流行的滤波器是非参数化滤波器。[这句话的描述不清晰]非参数化滤波器不需要满足扩展卡尔曼滤波算法所要求的非线性滤波随机量必须满足高斯分布的条件，它也不依赖于一个固定的后验方程去估计后验状态，而是从后验概率中抽取随机状态粒子来表达其分布。粒子滤波就是一种非参数化滤波器的实现算法，粒子滤波的关键是从后验分布中产生一组随机状态样本来表示后验概率分布。
粒子滤波的思想基于蒙特卡洛方法来表示概率[粒子滤波的思想是基于蒙特卡洛方法来表示概率]，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率（观测方程）中抽取的随机状态粒子来表达其分布，是一种循序重要性采样法。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算(状态方程)，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率分布。虽然在粒子滤波算法中，其概率分布仅仅是真实分布的一种近似，但由于粒子滤波是非参数化的，它解决了非线性滤波问题中随机量必须满足高斯分布的缺陷，能表达相较于高斯分布模型而言更为广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。
在粒子滤波中，后验分布的样本，我们称之为“粒子”，每一个粒子都是在时刻t的一个状态的实例化，这个实例化就是在t时刻的真实状态的假设。这里M代表粒子集中粒子的总数。在实际环境中，粒子总数M通常是一个较大的数字，在一些实现方法中，M是与时间t或者其它与状态的概率密度相关的函数。
粒子滤波的思想是通过一组粒子来估计近似状态概率。理论上，粒子集中一个假设状态粒子的概率应该与贝叶斯滤波器t时刻后验概率成一定比例关系。

粒子滤波主要步骤如下：
（1）初始化阶段：
规定粒子数量，将粒子平均的分布在规划区域，规划区域需要人为或者通过特征算法计算得出，比如人脸追踪，初始化阶段需要人为标出图片中人脸范围或者使用人脸识别算法识别出人脸区域。对于SLAM来说，规划区域一般为用来进行定位的地图，在初始化时，将需要设置的特定数量粒子均匀的撒满整张地图。
（2）转移阶段：
这个阶段所做的任务就是对每个粒子根据状态转移方程进行状态估计，每个粒子将会产生一个与之相对应的预测粒子。这一步同卡尔曼滤波方法相同，只是卡尔曼是对一个状态进行状态估计，粒子滤波是对大量样本（每个粒子即是一个样本）进行状态估计。
（3）决策阶段：
决策阶段也称校正阶段。在这一阶段中，算法需要对预测粒子进行评价，越接近于真实状态的粒子，其权重越大，反之，与真实值相差较大的粒子，其权重越小。此步骤是为重采样做准备。在SLAM中权重计算方式有很多，比如机器人行走过程中，激光雷达或者深度摄像头会返回周围位置信息，如果这些信息与期望值相差较大，亦或者在运动中某些粒子本应该没有碰到障碍或者边界，然而在运算中却到达甚至穿过了障碍点或边界，那么这种粒子就是坏点粒子，这样的粒子权重也就比较低一些。
（4）重采样阶段：
根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；权重小的粒子有些会被淘汰，为了保证粒子总数不变，一般会在权值较高的粒子附近加入一些新的粒子。
（5）滤波：
将重采样后的粒子带入状态转移方程得到新的预测粒子，然后将它们继续进行上述转移、决策、重采样过程，经过这种循环迭代，最终绝大部分粒子会聚集在与真实值最接近的区域内，从而得到机器人准确的位置，实现定位。
（6）地图生成：
每个粒子都携带一个路径地图，整个过程下来，我们选取最优的粒子，即可获得规划区域的栅格地图。
2、导航基本原理
Navigation栈[是否为Navigation包]是一个获取里程计信息、传感器数据和目标位姿并输出安全的速度命令到运动平台的2D导航包的集合。
(1) 定位
机器人在导航的过程中需要时刻确定自身当前的位置，Navigation 栈中使用amcl包来定位。amcl是一种概率定位系统，以2D方式对移动机器人定位，它实现了自适应（或者KLD-采样）蒙特卡洛定位法，使用粒子滤波跟踪机器人在已知地图中的位姿。下面的图片显示用里程计和AMCL定位的不同之处，AMCL估计base结构（机器人）相当于global结构(世界地图)TF转换（ROS中的坐标系转换）。从本质上，这种转换利用航位推算来处理漂移，所发布的转换是远期的。

(2)路径规划
路径导航部分则使用move_base包，move_base能够获取机器人周围信息（如激光雷达扫描结果）并生成全局与局部的代价地图，根据这些代价地图可以使机器人绕开障碍物安全到达指定的位置。move_base的路径规划主要分为全局规划和局部规划，分别采用的是A*算法和DWA（Dynamic Window Approach)）算法。