关于2d激光slam的一些思考

目的

学习slam的时间已经有几年了, 希望基此巩固所学. 其实之前已经有写过一些框架但总不满意, 删了又改, 不过就像贝叶斯公式所表达的, 这大概就是学习的过程, 可能过段时间又重构代码或者框架了, 嘿嘿嘿!!!

本文设计思路

理论结合代码, 尽量不涉及公式, 思考工程模式下的思路, 想法. 认知是片面的, 希望以我浅薄的认知, 给大家抛砖引玉.

坐标变换

这里构造了一个pose2d的类集成了二维坐标变换的所有方式

Pose2D Pose2D::TransformFrom(const Pose2D &new_pose)
{
    Point2D d_pos = new_pose.m_pos - m_pos;
    return Pose2D(d_pos.x * std::cos(m_theta.value()) + d_pos.y * std::sin(m_theta.value()), 
                    -d_pos.x * std::sin(m_theta.value()) + d_pos.y * std::cos(m_theta.value()), 
                    new_pose.m_theta.value() - m_theta.value());
}

Pose2D Pose2D::TransformAdd(const Pose2D &d_pose)
{
    float d_x = d_pose.m_pos.x * std::cos(m_theta.value()) - d_pose.m_pos.y * std::sin(m_theta.value());
    float d_y = d_pose.m_pos.x * std::sin(m_theta.value()) + d_pose.m_pos.y * std::cos(m_theta.value());

    return Pose2D(m_pos.x + d_x, m_pos.y + d_y, m_theta.value() + d_pose.m_theta.value());
}

Point2D Pose2D::TransformFrom(const Point2D &point)
{
    Point2D d_pos = point - m_pos;
    return Point2D(d_pos.x * std::cos(m_theta.value()) + d_pos.y * std::sin(m_theta.value()), 
                    -d_pos.x * std::sin(m_theta.value()) + d_pos.y * std::cos(m_theta.value()));
}

Point2D Pose2D::TransformAdd(const Point2D &d_point)
{
    float d_x = d_point.x * std::cos(m_theta.value()) - d_point.y * std::sin(m_theta.value());
    float d_y = d_point.x * std::sin(m_theta.value()) + d_point.y * std::cos(m_theta.value());

    return Point2D(m_pos.x + d_x, m_pos.y + d_y);
}

前端

我们从前端开始看, 这里我对于前端的定义做了一个泛概念化, 即一切单独提供里程计或里程计变化的模块. (因为回环也是提供相对坐标变化, 所以这里我也放在了前端里程里边)

根据2dslam来看前端大体上可以分为: 数据预处理, 激光里程方法, 概率地图等

数据预处理

激光去畸变

这方面学习应用比较少, 主要还是结合里程计的激光去畸变, 希望大佬可以补充.

Q: 为什么需要去畸变?

A: 机器人在运动时, 直接输出扫描的原始值, 而没有加上机器人运动导致的point偏差, 导致了scan信息不准

去畸变思路: 找到机器人的运动偏差, 并补偿上去, 如下:

calibrated_scan_point = odom_offset + origin_scan_point.

注意这里有一个假设, 假设车体运动是匀速的. 这会导致所得到的激光会有一些误差, 这是在做观测模型时需要注意考虑的.

以下是实现思路:

void Calibrate()
{
    TimedPose2D start_odom = m_odom_que->Speculate(m_scan->time);
    for(uint i = 0; i < m_scan->points.size(); i++)
    {
	float scan_point_time = m_scan->scan_increment_diff * i;
        TimedPose2D cur_point_odom = m_odom_que->Speculate(m_scan->time + scan_point_time);

	TimedPose2D odom_offset = start_odom.TransformFrom(cur_point_odom);
	m_calibrated_scan.push_back(odom_offset.pose.TransformAdd(m_scan->points[i]));
    }
}

注意这里的Speculate可能会比较耗时, 所以可以做一些近似操作, 比如5ms或者5次做一次等, 例如:

void Calibrate()
{
    uint cnt = 0;
    TimedPose2D start_odom = m_odom_que->Speculate(m_scan->time);
    for(uint i = 0; i < m_scan->points.size(); i++)
    {
        if(cnt++ % 5 != 0) continue;
   
	float scan_point_time = m_scan->scan_increment_diff * i;
        TimedPose2D cur_point_odom = m_odom_que->Speculate(m_scan->time + scan_point_time);

	TimedPose2D odom_offset = start_odom.TransformFrom(cur_point_odom);
	m_calibrated_scan.push_back(odom_offset.pose.TransformAdd(m_scan->points[i]));
    }
}

我这里的思路是针对点云实现的, 实际上对于原始range数据, 道理相似, 可以参考2D激光雷达运动畸变去除

数据特殊需要做的滤波处理

这部分包括简单采样滤波, 以及去除外点操作. 这一步我以为对于原始数据而言非常重要, 一个好的数据往往成功了一半, 所以我们更喜欢用贵的, 好的传感器, 哈哈哈哈哈

十分建议大家去看ethz-asl/libpointmatcher, 代码框架与滤波方法等都非常值得学习

以下是几种常见的滤波:

• BoundingBoxFilter: 根据所画的空间, 将内部数据保存下来
• DistanceLimitFilter: 高于或低于一定距离的数据删除掉
• Voxelfilter*: 体素滤波用的非常多, 比如carto中的自适应体素滤波等, 原理是将点云划分方格, 然后去内部所有point均值, 这也是ndt匹配的基础
• MaximumDensityFilter: 这里与李素滤波类似, 不过处理方式是在方格内若超过一定数量, 则随机删除掉几个数据, 数据量保持在max_num内
• RandomSamplingFilter: 随机保存一定数量的point
• ShadowPointFilter: 判断遮挡点然后剔除
• FootPrintFilter: 判断足迹点然后剔除

这里因为代码比较多, 我就不放了, 放在github上大家自己看point_cloud_filter

里程计标定

我们在做2d激光slam的时候很多时候需要用到编码器, 而时间一长, 会导致轮径磨损, 标定不准, 所以我们可能需要定期标定一下, 编码器参数. 一般思想是根据slam坐标与odom偏差做最小二乘(我们这里默认为slam_pose是真值, 然后让编码器的值尽量趋近slam_pose). 思路很简单, 所以直接看代码好了, 这里我提供了两种, 一种是差分轮标定, 一种是纯里程计标定(之后需要根据偏差转换到我们编码器参数上)

概率地图

我们需要思考这样几个问题:

Q0. 为什么使用概率地图? 
A0: 地图实际上是对现实场景信息的重现, 这个不确定性我们需要在地图上表示出来, 另一方面, 概率地图对于定位也拥有激励作用(csm, scan2map等等). 
    从贝叶斯公式角度来讲, 对于定位我们需要保存下来一些先验信息, 这里的先验信息可以以地图的方式存在.
    另一方面, grid_map实际上是对现实场景的重现, 而概率信息就是场景的体现

Q1. 怎样更新概率信息? 
A1: 根据我们一般经验, 反复被hit到的点, 其为占用点的概率更大. 根据此假设, 我们可以设计更新策略, 
    整体思路实际上是建一个更趋近于现实的, 传感器的概率更新模型
    比如最常见的对数几率更新等, 在设计时需要注意的是, 根据传感器的不同, 你希望多快区分占用与非占用.
	1. cnt++ == 5; 认为占用
	2. 根据对数几率决定占用概率: 目的: 1. 对数几率在0-1之间符合概率信息 2. 对数几率可以迅速分辨occ与free 3. 数值稳定, 不会越界; 4. 易于融合和更新, 通过加减操作就可实现
	3. 可以自己设计一种, 但要想清楚为何如此设计: cnt计数hit++, free--; 最后prob = cnt >= 0 ? 1 - 0.5^(cnt+1) : 0.5^cnt;
		1. 基本符合对数几率优点, 并且计算简单, 可以通过查表很快区分;
		2. 节省内存, 我甚至可以通过储存3bit来实现;
		3. 储存方便, cnt++, 或者cnt--即可;
		4. 惰性计算, 只在最后输出prob时计算;
	4. 通过添加高斯核更新概率, 原因是scan我们可以近似认为其符合高斯分布, 加入高斯核符合设计初衷, 不过是否会导致地图越来越糊需要具体设计, 测试, 
		这部分思路实际上是参考这位大佬的(https://github.com/scomup/lslam/blob/master/costmap.py)
	5. 可以结合匹配信息更新地图, 比如匹配置信度在0.9, 这个置信度结合更新地图

Q2. 如何更新整张地图? 
A2: 地图实际上是对现实场景信息的重现, 我们拥有的信息是pose与scan, 我需要思考的是如何通过这两者更新地图, 最简单的方式是hit上就更新栅格等
	1. 仅更新hit图, 这样会导致一些问题, 比如未来的图会越来越糊, 需要加一些free策略来解决这个问题, 
		这里实际上就相当于只储存点云信息了, 与3d点云地图类似, 所以我们要思考去除过滤动点.或者定时free地图, 这里根据设计来
	2. 通过pose与point之间连线更新free, 而point点更新hit, 使用BrasenHam更新, 这种是最方便的, 不过log_odds_p_occ与log_odds_p_free的更新需要好好设置, 测试, 非常重要

Q3. 地图储存形式应该是怎样的? 
A3: 地图储存一直困扰着我们, 它直接影响了我们的检索速度与内存消耗, 所以它的设计同样非常重要
	1. 假如算法在嵌入式或者类似于gmapping这种对于内存要求高的, 我们需要设计储存方式
		a. 通过每个栅格地图进行设计, 比如上面的概率更新, 一个栅格只需要3bit, 相较于float小了将近11倍
		b. 可以通过四叉树储存, 这有个好处是一方面未占用的区域占用会非常少; 另一方面, 四叉树自带降采样地图, 这太完美了
		c. 可以参考百度, 高德地图的方式, 比如2m*2m为一个指针, 只在有数据的时候构造地图
		d. 类似于carto的submap方式, 使用时调用submap图, 这里回环检测后修正图的过程有一个假设(在子图范围内, 累计误差不大)
		实际我理解的submap的目的在于为了更方便的后端重建图, 我们一般做法是将所有关键帧都保存下来, 然后后端优化完成之后更新所有关键pose, 做一次重建图. 
		这种方式记录信息就比较多, 显得并不优雅, 使用submap的话, 只需要优化submap的pose, 甚至不需要重建图, 也不必储存scan信息
		e. 类似于csm, 上山法匹配等, 这种仅需要占用与非占用信息的匹配方式而言, 我们可以生成一张匹配图, 也就是说一个栅格只需要1bit就可以储存信息 
		f. 设计自适应扩展地图等
	2. 时间上:
		a. 越简单的储存方式调用越快(我之前尝试使用map(index, prob)的方式储存, 时间贼慢, 建议不要尝试)
		b. 在做icp匹配时, 可以构建kdtree查找, 加快速度

Q4. 地图如何优化? 
A4: 1. 建图时歪着建图, 这样会导致图歪着建; 
    2. 对于服务型机器人而言, 地图需要好看规整, 可能需要后期p图; 
    3. 由于地图是场景的重现, 但重现时会存在误差, 我们需要重新整理地图, 比如在长期建图时容易出现黑刺, 这种黑刺可以通过一些批图手段过滤掉等, 以便于更好的定位 

Q5. grid_map的一些工程化应用讨论?
A5. 一些关于地图的工程化应用, 使用增量地图, 自适应地图扩展等 

Q6: 关于地图的其他讨论?
A6: 多传感器融合建图, 多机融合建图, 根据示意图重建图(例如消防示意图), 这些方向上没有做过, 仅给一点思路与设想
	多传感器融合: 根据<概率机器人>, 我们可以知道实际上最终更新地图是根据不同传感器定位置信度融合更新
	多机融合: 重点在回环定位上, 重点是找到回环点
	示意图重建图: 大体思路应该在归一化后的maptomap匹配上, 或者转换成图片做相似度匹配等

这里我实现了一个grid_map, 大家可以简单参考一下

激光里程方法

激光里程方法大概可以分为两种方向: 滤波与优化(因此实际上我们一改深刻理解贝叶斯公式与最小二乘)

常见的滤波方法有很多, 我了解的基于采样的: carto的csm, gmapping的上山法等

csm:

csm在carto里基本上所有的匹配都是用这种方式, 这种匹配的总体思路是穷举, 所以会比较慢, 势必需要一些优化:

• 多层地图加速搜索
• 查表避免反复计算三角函数
• 设置合理的采样范围
• 对scan进行体素滤波

这里快速csm匹配, 实际上是采样小范围的map做匹配(这里的采样是可以低于栅格大小的, 大家可以思考一下), 便于让我们的匹配达到预期

具体可以看这篇文章讲的非常清楚也很简单, 代码可以看csm与static_relocate

山上法:

显得更加优雅一些, 使用迭代的方式找到最优匹配, 避免了生成粒子时的麻烦

maptomap:

提取有序三角特征之后做匹配, 配准完做坐标变换, 可以尝试一下

实际上原来也有设想过通过csm实现的maptomap方案, 不过没真实做过

常见的优化方法也有很多, 比如: icp, ndt, hector的scantomap等

icp匹配: 我认为icp匹配是所有优化匹配的基础, 基本上所有优化匹配方式都可以在icp匹配中看到它的影子, 所以非常重要

我们可以思考一下如何做一个icp匹配:

1. 找到点对: 方法不是主要的, 最终思路才是. 最简单的思路, 在点云附近搜索, 搜索最近的, 比如类似于csm的采样查找, 或者kdtree最近邻查找, 或者ndt的index查找
2. 匹配: svd分解与最小二乘迭代
3. 当然icp匹配还有很多变种, 比如点对线的pl-icp等

ndt匹配:

其实ndt的本质还是最小二乘, 只不过他得残差是马氏距离的最小二乘

步骤: 1. 构建栅格, 并求解mean_pose与协方差 2. 做icp匹配, 只不过在求残差的时候加入残差信息

scantomap: 论文

同上, 思路是一样的, 只不过这里是点云与地图做匹配; 由于grid_map先天带有概率信息, 这个信息是其他scantomap手段所没有使用的(这也是我非常喜欢这种方式的原因)

占用栅格地图的离散性限制了可以实现的精度，也不允许直接计算插值值或导数. 所以我们需要使用二次线性插补来解决这个问题, 一般在使用这个方法的时候会结合降采样地图来实现,

一方面避免陷入局部最优, 另一方面收敛速度会快很多

思考一些问题

Q1: 为什么会叠图?
A1: 哈哈, 挺傻的一个问题. 匹配错了当然会地图. 我们需要思考一下更加根本的原因. 具体是因为什么, 是选旋转的原因呢, 还是特征消失呢, 还是累计误差导致的呢, 还是约束不足呢

1. 先从最常见的叠图原因说起, 由于先验位姿与实际位姿偏差过大导致的匹配失败. 
	a. 能否提供更好的先验位姿, 比如通过odom差分出一个近似位姿. 
	b. 我们在设计匹配算法的时候需要考虑机器人的实际运行偏差, 比如机器人速度是1m/s, 5hz的scan, 机器人的最大偏移应该是0.2m, 
	那我们设计的匹配方式是否能够解决在0.2m内偏移可以很好的收敛,当然角度也是一样的道理

2. 对于特征消失的问题:
	实际上我们需要考虑, 为什么特征消失, 比如长廊环境下纵向特征会消失, 从这个角度思考可以帮助我们认清问题, 从而解决问题. 
	根据不同算法有不同的解决思路, 比如基于采样算法, 我们可以只在长廊横向采样, 然后通过odom来补充定位;
	比如scantomap方法,可以在横向上的权重加大; 
	比如从后端优化角度, 我们也可以将scan_odom的信息矩阵在纵向上的权重降低, 表示我们不太相信scan_odom在纵向上的特征.
	对于特征丢失问题是从根本上是匹配无法解决的问题, 只能通过其他方式做补充, 比如odom, 比如gps, 二维码等

3. 对于累计误差的叠图问题: 
	我们从grid_map构建角度来看这个问题: 我们需要pose与scan, 也就是说如果pose完全正确, scan完全没有误差我们理论上可以完美复现现场, 
	但实际上, 我们在定位时pose会有微小误差, scan也会有微小误差, 很容易想到的思路是:
	a. 让这个pose尽可能正确, 这就有了通过图优化
	b. 另一种思路是, 能否打断这种累计的过程, 就有了回环检测, 通过回环检测加图优化使误差控制在定位误差之内, 
	或者加入绝对约束, 比如反光板, gps, 二维码都是这种思路; 
	打断的思路还可以, 比如每次在充电座上更换为最原始的图, 然后重新建图, 这样累计的过程就被打断了
	c. 还有一种思路是, 能否让map概率, 尽可能趋近于我得传感器误差或者定位误差, 也就是说建一个更完美的概率更新模型, 
	例如我们在grid_map写的加入高斯核更新map, 当然我们也可以结合匹配得分更新grid_map概率信息
	d. 另外还可以尝试将这种误差延迟, 比如我们维护两张地图, 老地图匹配定位, 新地图更新mapping, 一天保存一次, 这样我们的误差就被延迟了

辅助定位方式

反光板定位: 总体思路是预测步通过轮速计更新odom, 更新步通过反光板更新, 由于需要建图, 所以使用扩展状态空间的方式.

二维码定位: 思路与反光板定位基本一致, 具体大佬知乎上写的很清楚, 不多赘述

重定位/回环检测

因为重定位与回环检测有很多相似性, 所以常常方法在两者之间都是通用的

!!!注意, 重定位与回环检测都非常看重稳定性, 所以避免误成功十分重要

通过特征保存的方式

1. scan_context: 通过此特征保存然后匹配, 实际上可以保存关键帧pose在一定范围的scan_context用于保存, 由于scan_context缺乏角度不变性, 所以需要通过快速傅里叶变换或者双指针的方式找到角度, 提供初步角度后使用匹配方法得到更精确位姿
2. icp/ndt: 保存所有scan信息, 然后根据先验位姿匹配一定方位内的scan, 配上了则认为成功

仅通过地图方式

• csm: 通过分支定界找到最适合的匹配
• maptomap: 这种方式看起来适合做重验证
• pf(仅用于重定位): particle_filter_location, amcl, 对于重采样之后缺乏多样性问题, 可以通过在重采样的时候按照高斯分布加一些粒子, 补充多样性