SLAM初探-SLAM for Dummies

　　SLAM综述性特别是原理讲述比较浅显易懂的的资料比较少，相对比较知名的是《SLAM for Dummies》，但中文资料相对较少，这里就简单概述一下《SLAM for Dummies》的核心内容。

　　（一） SLAM for Dummies中SLAM的基本模块

　　SLAM的基本组成包括：地标（Landmark）抽取、数据关联、系统状态变量估计、基于观测值得系统状态变量更新，以及地标更新。SLAM for Dummies主要描述了2D场景下的地图构建与机器人定位，这里的状态变量主要是指机器人x，y坐标位置与方向角以及地标x，y坐标位置，而系统状态变量估计、基于观测值的系统状态变量更新主要使用EKF算法。

　　（二） SLAM for Dummies中SLAM硬件组成

机器人采用了轮式室内机器人Evolution Robotics ER1 robot，使用里程计（odometry ）预测下一时刻机器人的系统状态变量，里程计的误差要求是每移动1米的误差小于2cm，每转动45度的误差小于2度。测距模块采用Sick公司的激光雷达，其参数为角度分辨率为0.25度或0.5度，扫描范围100度或180度，测距误差正负5厘米。

　　（三） SLAM for Dummies中SLAM流程

　　SLAM流程如下图所示，基于里程计的变化使用EKF更新系统状态变量，基于激光扫描数据抽取地标，并进行数据关联完成本次观测的地标与以前观察到地标的关联，根据数据关联的结果，即观测过程，使用EKF更新和修订基于里程计预测的系统状态变量，同时更新地标信息，增加以前没有观测到地标。

　　（四） SLAM for Dummies中地标的抽取

　　地标的选取原则：（1）地标容易重新观测到；（2）地标相互之间区分度高；（3）环境中地标数量充足；（4）地标是稳定不动的。

　　基于激光雷达观测数据进行地标抽取的算法：

　　（1）Spike地标：基于激光雷达测距结果，抽取出“点”型地标。如果2个相邻的扫描点的测距距离大于一个距离门限，认为是一个Spike地标。在平滑的环境中，Spike无法正常工作，也无法处理像人这样在环境中移动的情况

　　（2）RANSAC地标：基于雷达测距结果，抽取出“线”型地标。通过随机采样角度差在D度以内的激光测距结果，并通过最小均方LS近似拟合成一条直线，如果距离该直线很近的激光测距结果的样点数量大于一个指定的门限，则认为该直线为一个地标。

　　在EKF算法中，地标有一个相对机器人位置的距离和方向角，对于“线”型地标，通常以机器人位置到“线”型地标的垂直线长度作为地标相对机器人的距离，垂直线的夹角作为方向角。

　　相对而言，以上两种基于地标的算法在准确性和稳定性方面差于基于scan匹配的算法。

　　（五） SLAM for Dummies中地标的关联

　　地标关联是指完成两个不同扫描观测到的地标之间的匹配。在《SLAM for Dummies》中，使用数据库维护以前观测到的地标及其被观测到的次数。只有该地标被观测到N次，才认为是合格的地标。 对于新的扫描，基于最近邻居规则，与数据库中的一个地标关联，并计算两者之间的欧式距离或者Mahalanobis距离，当距离小于一个门限（该门限是EKF不确定参数的函数）时，认为两者是同一个地标，否则认为是新地标，被观测到的次数设为1。

　　（六） SLAM for Dummies中核心算法EKF的设计

　　EKF的主要流程如下：（1）使用里程计数据更新当前系统状态；（2）基于地标关联或者说是地标的重观察修订更新的系统状态；（3）在当前系统状态中增加新的地标。

　　EKF算法的核心是系统状态变量（通常是由机器人位置的X，Y坐标、方向角，各个地标位置的X，Y坐标等3+2N个元素组成的列向量，N为合格地标的数量）、系统状态协方差（(3+2N)*(3+2N)矩阵，初始化时，因为没有地标信息，也没有里程计数据，协方差矩阵仅为3*3的对角矩阵，对角矩阵的值为接近0 的一个非0值）的维护和更新，以及Kalman增益的计算。

　　（1）基于里程计数据更新当前系统状态

　　　　（a）更新由机器人位置的X，Y坐标、方向角，即系统状态变量的前三个元素

　　　　（b）更新预测模型（方程）的雅克布矩阵

　　　　（c）更新预测模型（方程）的高斯噪声方差

　　　　（d）计算系统状态协方差

　　（2）基于地标关联修订系统状态

　　针对每一个匹配的地标，执行以下处理过程：

　　　　（a）计算观测模型（方程）的雅克布矩阵

　　　　（b）更新观测模型（方程）的高斯噪声方差

　　　　（c）基于观测模型高斯噪声方差、观测模型（方程）的雅克布矩阵，系统状态协方差矩阵，计算Kalman增益

　　　　（d）基于Kalman增益，更新系统状态变量

　　（3）在当前系统状态中增加新的地标

　　针对每一个匹配的地标，执行以下处理过程：

　　　　（a）在系统状态变量中增加新的地标，元素个数从3+2N拓展到3+2N+2M，M为新增地标个数

　　　　（b）在系统状态变量协方差矩阵中增加新地标对应的项