深入理解如何不费吹灰之力搭建一个无人驾驶车（七）建图算法总结

七、slam常用建图算法总结

7.1、概述
建图是SLAM最重要的一环，有了地图我们就可以进行路径规划并通过已建地图的信息去矫正位姿，其大致可以分为高斯栅格地图，光线投影栅格地图两种。由此进行介绍

7.2.1、2D高斯栅格地图
在建图中，我们常把地图划分为一个的栅格，每个栅格有一个占用概率（0-100%），这个概率可以通过贝叶斯滤波求得：如下
先上贝叶斯滤波：

及离散贝叶斯滤波（直方图滤波）：

以最简单的考虑为例：拿连续的情况来说，假设起始空间没有障碍物，我位姿分布为一维的N(1,1),观测到一个点在目前位姿的左边距离为2的位置，那么我们产生一个障碍物分布，记做N(-1,1) 。具体来说，见离散贝叶斯滤波，在栅格情况下，我想求t时刻某一个格子被占用的概率，其t-1时刻被占用的概率p（k,t-1）是0.8,那么根据t-1时刻周围格子的占用置信度及运动模型累加求和，我们可以得到t时刻该格子的占用预测（上图第3项）（不同的是建图时需要设定周围格子的范围及取概率大于某值的格子，位姿估计是取概率大于某值的格子就可以）。之后由t时刻观测结果，能生成t时刻的观测障碍物分布,就是我上文说的，比如你位姿分布是N(1,1),观测一个点在左边距离2，那么其分布为N(-1,1)，在离散贝叶斯滤波不过是用栅格概率表示罢了，这样，将这两项相乘取积，然后对周围每个格子做同样计算后归一化即得t时刻该格子被占用的概率，其他格子也一样计算。
那么，为什么障碍物分布一定是N(-1,1)呢？这种假设默认测量的不确定性是因为位姿的不确定产生的，那我们其实也可以认为测量的不确定性由传感器本身产生，不由位姿的协方差决定。来张直观的图：

越深表示概率越大。下图圆右下两点因为概率叠加可以即形成墙。

实际上在粒子滤波中，这个测量分布的协方差只能是传感器测量误差产生，你也可以用广为人知的波束模型生成其分布，其组成为四大部分：如下：

叠加起来就是：

具体可见概率机器人波束模型。

7.2.2、静态二值贝叶斯滤波方法
其框图如下：

l为对数概率，避免了0,1附近的不稳定性，我们用t时观测结果更新某栅格的概率，这样只要概率大于50，占用概率增加，其可以通过波束模型表述。
7.2.3、MAP方法
为了解决标准占用栅格建图的冲突问题，如下：

我们采用MAP方法建图，如下

其原理就是考虑了多个测量的结果（各个栅格的依赖性）从而使后验概率最大。用一个公式表述就是： 也就是
推导可见概率机器人。
引用自概率机器人：效果如下图：

7.3、光线投影栅格地图
其基本表述如下二图：


由光线投影去建图，其考虑激光雷达圆锥面方程，实现可见pythonrobotics代码。

7.4、3D 高斯cube地图
其原理是把3维空间划分为一个个cube，看我第4章介绍，然后用2D高斯栅格一样的方法或静态二值贝叶斯滤波一样的方法去建图，在ros中可以以点云表示栅格，以达到显示的效果及用于导航定位。

7.5、3D 点云地图
这种直接记录点云，四舍五入，相当于111的直接cube建图，最后以点云显示即可。

7.6、3D 射线地图
根据激光雷达圆锥体方程进行建图。不过是3D的情况

好了，本章就讲完了，下一章是slam经典体系讲解（icp-slam，hector-slam，graph-slam，fast-slam，ekf-slam，ukf-slam，SEIF-SLAM），下下章便是主流体系讲解（gmapping，loam，cartographer）。