SLAM系统原理推导

1.SLAM 基础知识

  SLAM 系统框架如图1 所示。包括两个主要传感器：激光雷达与轮式里程计。其中激光雷达用来获取环境信息，通过激光扫描可以测量周围 360°环境的障碍物信息。轮式里程计则通过移动底盘的电机编码器持续获取机器人的里程信息。

  激光雷达的作用主要有两个方面：一方面可以为构图算法提供点云数据。当构图算法获取足够的点云数据后，即可构建出以机器人为中心，以雷达射程为半径的局部地图。另一方面可以通过系统观测模型修正贝叶斯滤波器的预测位姿，提高滤波估计的机器人位姿精度。轮式里程计主要作用则是为 SLAM 系统提供里程信息。通过轮式里程计运动学模型，可以将里程信息转化为机器人位姿变化信息，送入贝叶斯滤波器初步计算出预测位姿。 预测精度完全依赖于轮式里程计提供的数据精度。而轮式里程计的测量精度对于获取精确的机器人位姿来讲远远不够。因此，需要用到系统观测模型对预测结果进行修正。经过修正并加权求和得到的机器人位姿即为当前估计的最佳位姿。在已知最佳位姿的条件下，局部地图即可更新至全局地图中。全局地图进一步进行高斯模糊，构建新的观测模型。至此整个 SLAM系统形成完整的闭环系统。

1.1 SLAM 系统概率模型

  在 SLAM 系统框架中， 采用贝叶斯滤波器为 SLAM 系统的后端优化算法提供概率模型。 贝叶斯滤波的原理是基于已经发生的事件对未发生事件进行估计，求解未发生事件的最可能概率分布。贝叶斯滤波的推导如图 2 所示。

  在贝叶斯滤波器构建的 SLAM 概率模型基础上， 发展出了两大分支： 基于卡尔曼滤波的 SLAM 算法， 主要代表有 EKF-SLAM， UKF-SLAM 等； 以及基于粒子滤波的 SLAM 算法， 主要代表有 RBPF-SLAM， FastSLAM 等。 其中基于卡尔曼滤波的 SLAM 算法要求噪声必须服从高斯分布且系统非线性不能太大；相反基于粒子滤波的 SLAM 算法适用于非线性较大的系统， 但受限于粒子退化与粒子耗散这一对耦合问题。

1.2 里程计运动模型

  机器人底盘分为两轮差速底盘和三轮全向底盘。两轮差速底盘其结构简单，造价较低，并且建立的模型相对简单，因此被应用于各种类型的机器人生产中。两轮差速底盘的运动学模型如图 3 所示。

该模型将机器人的任意移动视为某一圆心做圆弧运动，其中 d 为两轮距离的二分之一， r 为底盘中心做圆弧运动的半径。设左轮速度为 $v_L$，右轮速度为$v_R$, 底盘中心的角速度为 ω, 若已知 $v_L$,$v_R$,可以由式(1)计算出底盘运动中心的角速度 ω 和线速度 v。

  假设机器人在里程计数据更新的两时刻之间的极短时间 $\Delta t$内作匀速运动，则相邻两时刻之间机器人位移由式(2)给出。

式中： dx ,dy ,$d\theta$ 分别为机器人坐标系下沿 x 轴方向位移，沿 y 轴方向位移，以及以 z 轴为旋转轴的逆时针旋转角度。式(2)给出的是在机器人坐标系下的位移。机器人坐标系与世界坐标系的关系是由机器人位姿联系起来的，如图 2-4 所示。 图中$\left(x,y,\theta \right)$为机器人在世界坐标系下的位姿表示。

  可以通过机器人位姿 $\left(x,y,\theta \right)$将机器人在机器人坐标系的位移转换到世界坐标系中。最终计算的机器人运动模型由式(3)给出：

1.3 激光雷达测距模型

  激光雷达的测距模型主要有以下两种：三角测距与 TOF（ Time of Flight）测距。其中基于三角测距模型的激光雷达实物图如图 5 a 所示。图中左边小孔为激光发射器，右边稍大的孔为接受激光的光学摄像头。 三角测距模型主要原理为三角形的角边角定理，激光发射器位置与摄像头光心以及扫描点构成的三角形如图 5 b 所示。

  图 5b 中 $O_1$点为激光发射器， $O_2$点为摄像头光心轴，光心轴旁边的三角形代表用来捕捉反射光斑的相机模型。激光发射器发射的激光由红色虚线表示，相机接收的反射激光由蓝色虚线表示。 A 点表示激光击中的障碍物位置,障碍物A 在成像平面上的位置用 $A_1$ 表示。其中线段$O_1$ $O_2$长度已知；由于激光垂直于直线 $O_1$ $O_2$ 发射，$\angle A{O}_1{O}_2$ 为 90°；$\angle O_1{O}_{2}A$ 可以通过成像点 $A_1$ 的位置获取。在三角形 $O_1{O}_{2}A$中已知两内角与一条边长，即可通过正弦定理求出三角形另外一边 $O_{1}A$的长度，见式(4)。 线段 $O_{1}A$ 即为障碍物 A 到雷达的距离。

单线激光雷达通过电机驱动雷达测距核心进行顺时针旋转，最高可达每秒10Hz。旋转过程中不断发射激光束，基于三角测距模型获取 360°方向的障碍物距离信息，完成激光扫描数据的封装。

1.4 系统观测模型

  激光雷达的观测模型包括光束模型和似然场模型。其中似然场模型的基本原理是对地图进行高斯模糊（如图6 所示），利用 SLAM算法输出的环境地图， 对于地图上存在障碍物的位置， 以该点为中心， 以地图上其他点到该点的欧氏距离为变量来构造高斯分布函数，设 d 表示激光测量终点坐标$\left(x_{zt},y_{zt}\right)$与地图 m 上最近的障碍物之间的欧式距离，那么激光雷达测量的概率分布可以由一个以 0 为中心，方差为 d 的高斯函数$\epsilon (d)$给出，见式(2-5)：

  依据地图提供的障碍物信息可以获取地图上全部点对应的高斯分布函数值，并将该值作为地图上的似然评分，评分越高则代表扫描点与障碍物点越接近，其测量结果也就越准确。
  地图经高斯模糊后， 环境$z_t$的期望值相对于位姿来讲都是连续的，相比于光束模型，似然场模型不会因为环境形状变化不规律导致观测期望突变，因此似然场模型适合结构化与非结构化的环境，增加了该模型的适用场景。此外，对于似然场每一点的概率都可以预先计算，在实际 SLAM 算法执行过程中计算式仅需要对预先计算的似然场概率分布进行查表即可，计算量可以忽略不计，提高了算法的实时性。

2 SLAM实例

2.1 粒子滤波算法原理

  粒子滤波算法使用贝叶斯估计的思想， 用粒子分布去近似机器人位姿的概率分布，如图7 所示。
  首先将一定数量的粒子均匀撒在可能分布的空间中，作为粒子滤波的初始状态$x_0$ 的概率分布。按照贝叶斯估计的流程，可以通过里程计运动模型求得t时刻的机器人位姿 $x_t$ ，作为预测粒子集。 再根据激光雷达观测模型对预测粒子集中每一个粒子赋予权值， 粒子权值越高， 则代表该粒子与实际位置的误差越小。最终对所有粒子根据其权值进行加权平均，即可得到滤波后的位姿估计结果。
  SLAM 中主要任务是机器人的自主定位与环境地图的建立。 其中在已知机器人的位姿信息的条件下计算环境地图存在闭式解。 因此 SLAM 问题的主要难点在于如何获取机器人位姿信息。通过粒子滤波得到的机器人位姿$x_{1:t}$，与激光雷达的历史观测信息$z_{1:t}$可以直接计算得到环境的栅格地图，下面主要介绍基于粒子滤波器获取机器人位姿的算法步骤。
  粒子滤波算法的基本流程如下：

1. 用粒子集表示机器人的位姿信息： $X_t=\left\{\left(x_t^i,w_t^i\right)|i=1,...,n\right\}$， 其中每一个粒子包含两个参数： 机器人位姿 $x_t^i$ ，粒子权重 $w_t^i$。
2. 将总数为 N 的粒子集随机且均匀地撒在所有可能的位姿空间内，作为粒子滤波算法的初始状态 。
3. 已知前一时刻的粒子集状态 $X_{t-1}=\left\{\left(x_{t-1}^i,w_{t-1}^i\right)|i=1,...,n\right\}$， 每一个粒子通过建议分布 $x_t^i\sim p(x_t|u_t,x_{t-1}^i)$ 计算当前时刻的位姿信息。 这里建议分布采用里程计运动模型。其中 Δx, Δy, Δθ 为里程计信息 $u_t$；$N_x$, $N_y$, $N_{\theta }$为里程计的测量噪声，噪声服从均值为 0 的高斯分布。最后粒子在建议分布进行随机采样，见式(6)。

4. 评估每一个粒子的权重：
  机器人实际位姿分布未知，粒子集内每一个位姿都可能是机器人的实际位姿。定义粒子权重$w_t^i$，用来描述粒子的预测分布与实际分布的匹配程度，粒子权重定义见式(7)，权重越大，表明粒子的预测分布实际分布匹配程度越高。

  经贝叶斯公式推导，可以得到权重的计算式，见式(8):

  由于粒子权重与机器人在$x_t$位姿下获取观测数据$z_t$的置信度成正比，其系数$\eta$为归一化因子。因此首先将每个粒子对应的$p(z_t|x_t^i)$ 求出，计算出归一化因子$\eta$ （见式）即可通过式()求出归一化后的粒子权重。

5. 计算出机器人位姿估计结果，见式(10)。

6. 将得到的当前时刻粒子集$X_t$ 替换前一时刻粒子集$X_{t-1}$ ，返回步骤（3）进行迭代计算。
7. 通过上述步骤可以迭代计算出机器人在每一时刻的位姿分布情况，并通过式(11)计算出每一时刻下的机器人位姿估计结果。通过此结果可以进一步计算出环境地图，完成即时定位与地图构建的任务。