无人驾驶-SLAM-栅格地图
激光雷达建栅格地图
-
一、前言
-
-
1.1 Gmapping算法
-
- 1.1.1 简介
- 1.1.2 话题订阅发布
- 1.1.3 TF变换详解
- 1.1.4 Gmapping伪代码
- 1.1.5 Gmapping源码
-
1.2 栅格地图创建
-
- 1.2.1 地图分类
- 1.2.2 栅格地图构建算法
-
1.3 激光雷达建栅格地图
-
- 1.3.1 算法核心
- 1.3.2 算法输入数据
- 1.3.3 算法流程
- 1.3.4 栅格地图构建代码
-
-
参考链接
一、前言
1.1 Gmapping算法
1.1.1 简介
本篇文章以激光SLAM算法gmapping为例，向大家介绍移动机器人构建环境地图的必备条件、算法流程、算法原理。其实，使用gmapping算法实现建图的步骤十分简单，一般分为了解算法、安装算法、更改参数、执行算法、保存地图。
Gmapping是一个基于2D激光雷达使用RBPF（Rao-Blackwellized Particle Filters）算法完成二维栅格地图构建的SLAM算法。
1.1.2 话题订阅发布
关于gmapping 算法功能包中的话题和服务如下图所示：
从上图可以看出，gmapping需要订阅坐标变换话题/tf和激光雷达扫描数据话题/scan ，将会发布栅格地图话题/map 。
1.1.3 TF变换详解
关于/tf又分为两个部分，gmapping功能包中的TF变换如下图所示：
所以，从上面的介绍我们就知道，gmapping算法的必备条件，/tf、/odom、/scan，就可以通过算法发布/map了。
1.1.4 Gmapping伪代码
1.1.5 Gmapping源码
关于gampping算法的源码，不是一篇文章就能介绍清楚的，这里仅仅给出一个算法函数的调用流程，以后有机会，会按照模块分析源码。
1.2 栅格地图创建
1.2.1 地图分类
移动机器人常见的地图有三种：尺度地图、拓扑地图、语义地图。
尺度地图：具有真实的物理尺寸，如栅格地图、特征地图、点云地图；常用于地图构建、定位、SLAM、小规模路径规划。
拓扑地图：不具备真实的物理尺寸，只表示不同地点的连通关系和距离，如铁路网，常用于超大规模的机器人路径规划。
语义地图：加标签的尺度地图，公认SLAM的未来–SLAM和深度学习的结合，常用于人机交互。
重点介绍尺度地图
1.2.2 栅格地图构建算法
（1）为什么使用占用栅格地图构建算法构建地图?
第一，构建栅格地图需要使用激光雷达传感器。
第二，激光雷达传感器是有噪声存在的，通俗的说，“不一定准”。
举个例子，机器人在同一位姿下的不同时刻，通过激光雷达对一个固定的障碍物的探测距离不一致，一帧为5m，一帧为5.1m，我们难道要把5m和5.1m的位置都标记为障碍物？这也就是使用占据栅格地图构建算法的原因。
（2）什么是栅格地图构建算法？
为了解决这一问题，我们引入占据栅格地图（Occupancy Grid Map）的概念。我们将地图栅格化，对于每一个栅格的状态要么占用，要么空闲，要么未知（即初始化状态）。
（3）栅格地图构建算法推导
（4）举例说明
首先，我们假设 looccu = 0.9，lofree = -0.7。那么，显而易见，一个栅格状态的数值越大，就越表示该栅格为占据状态，相反数值越小，就表示改栅格为空闲状态。（这也就解决了此前文中提出的激光雷达观测值"不一定准"的问题）
上图是用两次激光扫描数据更新地图的过程。在结果中，颜色越深越表示栅格是空闲的，颜色越浅越表示是占据的。这里要区分常用的激光SLAM算法中的地图，只是表述方式的不同，没有对错之分。
1.3 激光雷达建栅格地图
1.3.1 算法核心
显然，整篇文章得出的一个结论就是下图所示，这里假设lofree和looccu为确定的数值，一般情况下一正一负。
然后，我们通过激光雷达数据栅格进行判断，如果判定栅格是空闲，就执行上面公式；如果判定栅格是占据，就执行下面的公式。在经过许多帧激光雷达数据的洗礼之后，每一个栅格都存储了一个值，此时我们可以自己设定阈值与该值比较，来做栅格最终状态的判定。
1.3.2 算法输入数据
激光雷达数据包（每个扫描点包含角度（逆时针为正方向）和距离，每帧激光数据包含若干扫描点，激光雷达数据包包含若干帧激光雷达数据）
载体位姿数据包（每一个位姿包含世界坐标系下的载体位置和航向角，初始航向角与世界坐标系X轴正方向重合，逆时针为正方向）
地图参数（需要构建地图的高度和宽度，构建地图的起始点，lofree和looccu的设定值，地图的分辨率）
假设激光雷达坐标系和载体坐标系重合
1.3.3 算法流程
这里以处理第一帧激光雷达为例，从上向下依次介绍
（1）读取一帧激光雷达数据和该帧对应的机器人位姿
//获取每一帧的激光雷达、机器人位姿数据
GeneralLaserScan scan = scans[i];
Eigen::Vector3d robotPose = robot_poses[i];（2）计算该帧机器人位置的栅格序号
//获取该帧机器人位姿的栅格序号
GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));即从世界坐标系转入栅格坐标系，每个栅格序号有x，y两个数字。这里与地图分辨率有关，比如说：地图分辨率为0.05，也就是1m用20个栅格表示。
例如：世界坐标系下机器人位置（1,1）对应栅格坐标系的（20,20）。
注意：世界坐标系与像素坐标系区分开来，他们之间的y轴方向相反，其他都一致，地图的显示使用的像素坐标系（栅格坐标系）。
（3）遍历该帧激光雷达数据的所有扫描点执行以下操作
- 步骤一
计算每一个激光点击中栅格在像素坐标系下的栅格序号
//明确这里的世界坐标系world_x，不是真实的世界坐标系，而是像素坐标系，y轴与真实的世界坐标系相反，这样是laser_y加负号的原因
double laser_x = dist * cos(theta + angle);
double laser_y = -dist * sin(theta + angle);
//得到该激光扫描点，在世界坐标系下（像素坐标系下）的位置
double world_x = laser_x + robotPose(0);
double world_y = laser_y + robotPose(1);
//将该激光扫描点在世界坐标系下的位置，转化为栅格序号
GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);
- 步骤二
从当前机器人位姿的栅格序号到该激光扫描点的栅格序号划线，找出所有空闲的栅格序号
//从机器人的栅格序号到该激光扫描点的栅格序号划线
//目的：找到两点之间途径的空闲栅格，将栅格序号存入std::vector<GridIndex>中
std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);
- 步骤三
遍历所有空闲的栅格更新空闲栅格状态
data += mapParams.log_free;//log_free=-1，data将变小- 步骤四
更新该激光扫描点击中的栅格状态
data += mapParams.log_occ;//log_occ=2，data将变大1.3.4 栅格地图构建代码
//占据栅格地图构建算法
//输入激光雷达数据和机器人位姿数据
//目的：通过遍历所有帧数据，为pMap[]中的每个穿过的空闲栅格或者击中栅格赋新值，中间有个计算方法，也就是占用栅格地图构建的理论实现
void OccupanyMapping(std::vector<GeneralLaserScan>& scans,std::vector<Eigen::Vector3d>& robot_poses)
{
std::cout <<"Scans Size:"<<scans.size()<<std::endl;
std::cout <<"Poses Size:"<<robot_poses.size()<<std::endl;
//遍历所有帧激光雷达数据
for(int i = 0; i < scans.size();i++)
{
//获取每一帧的激光雷达、机器人位姿数据
GeneralLaserScan scan = scans[i];
Eigen::Vector3d robotPose = robot_poses[i];
//获取该帧机器人位姿的栅格序号
GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));
//判断该帧机器人位姿的栅格序号，是否在自己设定的栅格地图范围内
if(isValidGridIndex(robotIndex) == false) continue;
//遍历该帧激光雷达数据所有扫描点
for(int id = 0; id < scan.range_readings.size();id++)
{
//取出该激光雷达扫描点的距离和角度
double dist = scan.range_readings[id];
double angle = scan.angle_readings[id];
//剔除异常数据，跳过该次循环，不作处理
if(std::isinf(dist) || std::isnan(dist)) continue;
//机器人航向角，机器人x轴与世界坐标系x轴夹角
double theta = robotPose(2);
//在旋转过后（与世界坐标系（像素坐标系下）平行）的激光雷达坐标系下的坐标x,y
//该开始一直不理解这个为啥laser_y要加一个负号
//明确激光雷达数据的角度逆时针变化
//明确机器人航向角与世界坐标系x轴呈逆时针变化
//明确这里的世界坐标系world_x，不是真实的世界坐标系，而是像素坐标系，y轴与真实的世界坐标系相反，这样是laser_y加负号的原因
double laser_x = dist * cos(theta + angle);
double laser_y = -dist * sin(theta + angle);
//得到该激光扫描点，在世界坐标系下（像素坐标系下）的位置
double world_x = laser_x + robotPose(0);
double world_y = laser_y + robotPose(1);
//将该激光扫描点在世界坐标系下的位置，转化为栅格序号
GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);
//判断该激光扫描点的栅格序号，是否在自己设定的栅格地图900x900范围内，如果不在则跳过
if(isValidGridIndex(mapIndex) == false)continue;
//从机器人的栅格序号到该激光扫描点的栅格序号划线
//目的：找到两点之间途径的空闲栅格，将栅格序号存入std::vector<GridIndex>中
std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);
//遍历该扫描激光点通过的所有空闲栅格
for(int k = 0; k < freeIndex.size();k++)
{
GridIndex tmpIndex = freeIndex[k];
//将空闲栅格的栅格序号，转化到数组序号,该数组用于存储每一个栅格的数据
int linearIndex = GridIndexToLinearIndex(tmpIndex);
//取出该栅格代表的数据
int data = pMap[linearIndex];
//根据栅格空闲规则，执行data += mapParams.log_free;
if(data > 0)//默认data=50
data += mapParams.log_free;//log_free=-1，data将变小
else
data = 0;
//给该空闲栅格赋新值，最小为0
pMap[linearIndex] = data;
}
//更新该激光扫描点集中的栅格，
int tmpIndex = GridIndexToLinearIndex(mapIndex);
int data = pMap[tmpIndex];
//根据栅格击中规则，执行data += mapParams.log_occ;
if(data < 100)//默认data=50
data += mapParams.log_occ;//log_occ=2，data将变大
else
data = 100;
//给击中的栅格赋新值，最大100
pMap[tmpIndex] = data;
//到这里，对一个位姿下的一个激光扫描数据经过的空闲栅格和击中栅格的pMap进行了重新赋值
}
//到这里，对一个位姿下的一帧激光扫描数据经过的空闲栅格和击中栅格进行了重新赋值
}
//到这里，对所有帧激光扫描数据经过的空闲栅格和击中栅格进行了重新赋值
}