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_{1} O