机器人学习

转自： 占据栅格地图构建（Occupancy Grid Map） - 知乎

占据栅格地图构建（Occupancy Grid Map）_「小白学移动机器人」一个专注分享移动机器人相关知识的公众号！-CSDN博客_构建栅格地图

用于学习参考用。

懂了占据栅格地图构建算法，就意味着SLAM问题不再是抽象的理论公式，它变成了浮现在你我脑海里的动态构建过程。这将对我们完整理解各种激光SLAM算法起着十分重要的作用。

移动机器人常见的地图有三种：尺度地图、拓扑地图、语义地图。

尺度地图：具有真实的物理尺寸，如栅格地图、特征地图、点云地图；常用于地图构建、定位、SLAM、小规模路径规划。

拓扑地图：不具备真实的物理尺寸，只表示不同地点的连通关系和距离，如铁路网，常用于超大规模的机器人路径规划。

语义地图：加标签的尺度地图，公认SLAM的未来--SLAM和深度学习的结合，常用于人机交互。

其中对尺度地图进行补充说明，如下图所示。

顾名思义，占据栅格地图构建算法当然构建的是栅格地图。

在开始讲解之前，我们要明确一些事情。

第一，构建栅格地图需要使用激光雷达传感器。

第二，激光雷达传感器是有噪声存在的，通俗的说，"不一定准"。

举个例子，机器人在同一位姿下的不同时刻，通过激光雷达对一个固定的障碍物的探测距离不一致，一帧为5m，一帧为5.1m，我们难道要把5m和5.1m的位置都标记为障碍物？这也就是使用占据栅格地图构建算法的原因。

为了解决这一问题，我们引入占据栅格地图（Occupancy Grid Map）的概念。我们将地图栅格化，对于每一个栅格的状态要么占用，要么空闲，要么未知（即初始化状态）。

关于占据栅格地图构建算法的引出、推导、演化，从下面的图片得出。（哈哈，图省事，在word里推导的）

首先，我们假设 looccu = 0.9，lofree = -0.7。那么，显而易见，一个栅格状态的数值越大，就越表示该栅格为占据状态，相反数值越小，就表示改栅格为空闲状态。（这也就解决了此前文中提出的激光雷达观测值"不一定准"的问题）

上图是用两次激光扫描数据更新地图的过程。在结果中，颜色越深越表示栅格是空闲的，颜色越浅越表示是占据的。这里要区分常用的激光SLAM算法中的地图，只是表述方式的不同，没有对错之分。

通过上述的讲解，你是否了抓住算法实现的的重要依据是什么？要是没有，你就要反思一下自己是否仔细读了这篇文章？

显然，整篇文章得出的一个结论就是下图所示，这里假设lofree和looccu为确定的数值，一般情况下一正一负。

然后，我们通过激光雷达数据栅格进行判断，如果判定栅格是空闲，就执行上面公式；如果判定栅格是占据，就执行下面的公式。在经过许多帧激光雷达数据的洗礼之后，每一个栅格都存储了一个值，此时我们可以自己设定阈值与该值比较，来做栅格最终状态的判定。

激光雷达数据包（每个扫描点包含角度（逆时针为正方向）和距离，每帧激光数据包含若干扫描点，激光雷达数据包包含若干帧激光雷达数据）

机器人位姿数据包（每一个位姿包含世界坐标系下的机器人位置和航向角，初始航向角与世界坐标系X轴正方向重合，逆时针为正方向）

地图参数（需要构建地图的高度和宽度，构建地图的起始点，lofree和looccu的设定值，地图的分辨率）

假设激光雷达坐标系和机器人坐标系重合

这里以处理第一帧激光雷达为例，从上向下依次介绍。

（1）读取一帧激光雷达数据和该帧对应的机器人位姿

（2）计算该帧机器人位置的栅格序号

即从世界坐标系转入栅格坐标系，每个栅格序号有x，y两个数字。这里与地图分辨率有关，比如说：地图分辨率为0.05，也就是1m用20个栅格表示。

例如：世界坐标系下机器人位置（1,1）对应栅格坐标系的（20,20）。

注意：世界坐标系与像素坐标系区分开来，他们之间的y轴方向相反，其他都一致，地图的显示使用的像素坐标系（栅格坐标系）。

（3）遍历该帧激光雷达数据的所有扫描点执行以下操作

计算每一个激光点击中栅格在像素坐标系下的栅格序号

从当前机器人位姿的栅格序号到该激光扫描点的栅格序号划线，找出所有空闲的栅格序号

遍历所有空闲的栅格更新空闲栅格状态

更新该激光扫描点击中的栅格状态

（4）结束

这里分享核心代码，具体算法功能包，可在公众号：小白学移动机器人，发送：栅格地图，即可获得。

本算法功能包，给出了详细的中文注释，是提升c++编程技能的阅读良品。