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 为底盘中心做圆弧运动的半径。设左轮速度为 vLv_{L}vL，右轮速度为vRv_{R}vR, 底盘中心的角速度为 ω, 若已知 vLv_{L}vL,vRv_{R}vR,可以由式(1)计算出底盘运动中心的角速度 ω 和线速度 v。

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

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

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

1.3 激光雷达测距模型

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

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

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

1.4 系统观测模型

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

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

2 SLAM实例

2.1 粒子滤波算法原理

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

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

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

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

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

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

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

SLAM系统原理推导相关推荐

从计算机视觉(slam)和摄影测量两个维度进行BA算法原理推导
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达摄影测量作为历史悠久的学科,在3D视觉里面很多算法发挥着重要的作用:而slam 的出现对摄影测量是某种 ...
(各种均衡算法在MIMO中的应用对比试验)最小均方误差（MMSE）原理推导以及在MIMO系统中对性能的改善。
文档和程序地址:下载地址各种均衡算法在MIMO中的应用对比试验,内附原理推导,对比实验说明和结果等.包括MMSE,ZF,ZF-SIC等.代码附有原理推导小论文.仅供参考
CamVox：一种低成本、高精度的激光雷达辅助视觉SLAM系统
标题:VCamVox: A Low-cost and Accurate Lidar-assisted Visual SLAM System 作者:Yuewen Zhu, Chunran Zheng, ...
机器视觉系统原理及学习策略
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达机器视觉系统组成从机器视觉系统字面意思就可看出主要分为三部分:机 ...
论文简述 | CamVox: 一种低成本、高精度的激光雷达辅助视觉SLAM系统
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达摘要将激光雷达与基于相机的同步定位和建图(SLAM)相结合是提高整体精度的有效方法,尤其是在大规模室 ...
视觉SLAM十四讲学习笔记——第十三讲实践：设计SLAM系统
1.如何运行示例代码首先是如何运行示例代码,这里遇到了很多问题: (1)首先要下载Kitti数据集,并在config/default.yaml文件内修改路径. (2)安装Glog.GTest.GFl ...
MIMO-OFDM系统原理及其关键技术
MIMO-OFDM系统原理及其关键技术引言未来的宽带无线通信系统,将在高稳定性和高数据传输速率的前提下,满足从语音到多媒体的多种综合业务需求.而要在有限的频谱资源上实现综合业务内容的快速传输,需 ...
经典激光雷达SLAM系统：LeGO-LOAM
作者 | 密斯特李编辑 | 汽车人原文链接:https://zhuanlan.zhihu.com/p/511968459 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨 ...
ORB-SLAM：精确多功能单目SLAM系统
ORB-SLAM: a Versatile and Accurate Monocular SLAM System Taylor Guo, 2016年3月18日-9:00 原文发表于:IEEE Tran ...
《嵌入式系统原理与接口技术》——嵌入式系统接口应用基础
本文为我负责编写的电子工业出版社出版的<嵌入式系统原理与接口技术>一书第七章部分,这里整理的仍然是修改稿,供需要的同学参考,本书为普通高等教育"十二五"规划教材,电子信 ...

SLAM系统原理推导