请问如何实现动态场景的slam和避障功能？

1、引言

在单次激光SLAM或者定位的过程中，动态障碍物（如：移动的车辆、行人等）会对SLAM或者定位造成两个方面的负面影响：点云配准 和 地图构建

针对点云配准，动态障碍物能造成最极端的情况是“当一帧激光数据中的大部分（甚至100%）点云都是从动态障碍物上返回的数据，那么该帧点云则没有能力在reference scan（或者reference map）中找到correspondent points，点云的配准就会失效，但在实际情况下，点云配准时的query scan数据中受到动态障碍物影响的点云个数是“有限的”，由此query scan中大多数的点云还是可以找到它们的correspondences，少数的异常量不会产生太大的干扰。另一方面，若是采用特征法进行点云配准，动态障碍物在预处理阶段也会被剔除，由此基于特征的配准方法受到动态障碍物的影响会更小。针对地图构建，动态障碍物对点云地图的构建的影响在于会在最终生成的地图中产生“鬼影”，而若简单使用在第一种情况中提到的特征点云构建地图的话，又会错误地滤除掉很多实际需要的物体信息，导致地图特征稀疏，难以进一步处理，具体的影响可见以下图示：

最后，一言以蔽之：（1）在点云配准层面动态障碍物过多会影响点云配准的精度和鲁棒性（导致配准出现失效），（2）在地图构建层面时动态障碍物会在地图中形成“鬼影”，影响后期地图的进一步处理（路径规划、特征提取、定位，etc）。这里也推荐「3D视觉工坊」新课程《面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)》。

作者：Sky Shaw | 来源：计算机视觉工坊
原论文：综述 | 自动驾驶和SLAM中动态障碍物滤除方案综述
添加微信：dddvisiona，备注：激光SLAM，拉你入群。文末附行业细分群。

2、典型场景下动态障碍物滤除的需求差异

2.1、平面移动机器人中对动态障碍物滤除的需求

首先，以移动机器人（AGV类）的使用场景为例，其基础使用场景是二维室内环境，所以使用的SLAM算法和机器人位姿估计器模型都是基于2D平面操作。那基于2D平面场景，SLAM中最常见、最经典的算法是gmapping、cartographer或者karto，定位中常用的算法是EKF、AMCL和ICP的组合，导航中的路径规划和运动控制就都是基于概率栅格地图（pgm）的平面规划了。由此可见，基于移动机器人（AGV）的SLAM、定位和导航都离不开概率栅格地图（pgm），而在概率栅格地图上最传统和最有效的去除动态障碍物的方法就是利用“ray casting算法”的栅格地图概率的“更新”，而“更新”的最终目的是为了在栅格地图上不留下动态障碍物的hit点（此处的更新之所以打上引号，是因为更新概率栅格的模型有很多，用引号表示泛指）。

2.2、自动驾驶中对动态障碍物滤除的基本标准

另一个会使用到SLAM和Localization的工程场景就是低速无人驾驶（或自动驾驶）领域了，不管是使用高精地图的“传统”自动驾驶方案，还是“重感知，轻地图”的自动驾驶策略，都还是或多或少地需要使用到“地图”（此处的引号代表一系列的泛指，pgm地图、点云地图、语义地图等），高精地图的各个图层中（语义图层也好，矢量图层也罢）都要求提取出精细的车道线、箭头、路标等特征并且肯定不会再包含动态障碍物的信息，定位所使用的点云图层也是要求尽可能地保证“干净”无动态障碍物点云干扰。在自动驾驶领域中的动态障碍物滤除思量上，一方面区别于AGV只针对2D的场景的操作，其需要滤除的障碍物是3D物体，另一方面除了地图上不能留有动态障碍物信息外，定位过程中的实时点云也需要尽可能地要求不受其动态障碍物的影响且其定位过程中的动态障碍物的滤除最好是在线的、实时的、高效地，以保证自动驾驶的精度和鲁棒性。

至此，不难得出一个结论：（1）AGV导航中动态障碍物去除可以使用基于栅格地图概率更新方式，本质上是ray-casting，同时滤除过程可以容忍一定程度的“延迟”（概率更新需要多次、反复进进行以达到滤除的效果），（2）自动驾驶过程中点云数据去除动态障碍物是高精度地图构建的基础任务，同时保证在线地、实时地动态障碍物滤除能在一定程度上提升定位过程中精度和保证定位的鲁棒性。

3、动态障碍物滤除算法综述

3.1、为什么需要滤除“动态障碍物”信息？

基于第一节中的阐述，相信大家都能体会到在SLAM和定位的工程应用中动态障碍物的过滤是一件对SLAM & Localization位姿输出精度的提升效果有限，但是能极大地提升“用户体验”的一个模块，更重要的是动态障碍物滤除模块是一个完备导航系统的重要组成成分。

3.2、如何去选择动态障碍物滤除的具体方法？

基于第二节中的描述，也可以发现在不同的工程应用场景下，动态障碍物滤除的具体要求也不相同，其一，工程师需要根据具体使用的场景和后续导航模块需要的“地图”来确认动态障碍物的滤除算法，比如2D场景下可以基于2D的概率栅格地图，利用ray-casting的算法实现滤除，3D场景下需要基于3D点云地图，利用特征提取或者聚类的方式去除点云地图中的动态障碍物点云；其二，根据使用场景的不同，动态障碍物滤除的时机和时效也存在差异，例如2D场景下基于2D的概率栅格地图的“更新”方式在滤除动态障碍物信息的时候可以容忍一定程度的“延迟”，且2D定位时query scan中也不一定需要进行动态障碍物点云的过滤处理，但在3D场景下，更具体些，在自动驾驶的使用场景下，定位过程就已经需要尽可能地保证能在线实时地滤除动态障碍物信息，在3D地图的构建过程中会有更多精细的处理以保证“地图”不被动态障碍物的信息污染。

3.2.1、 实时 or 离线的动态障碍物滤除方案

至此，考虑“动态障碍物滤除”算法应用的层面，动态障碍物去除算法模块的使用分为两个时机：（1）在线实时处理和（2）离线后处理。针对第一个“在线实时处理”，顾名思义就是在接收到每一帧（或者每一个batch）点云数据时，通过相邻“单位点云数据”帧间的对比以分离动态障碍物点云，然后再直接将“单位点云数据”中的动态障碍物点云滤除，针对第二个时机：离线处理，此处的“离线”指的是区分“地图构建”与“位姿估计”这两个阶段，因为在离线处理的前提是需要点云数据及其对应的全局位姿信息，故而“离线地图构建”的阶段需要在“位姿估计”阶段后进行，通常是在SLAM过程记录关键帧的位姿及其对应的点云数据，输出给到“离线地图构建”的阶段并在此阶段内对每一帧点云进行动态障碍物点云的去除。

由此，虽然在线实时的滤除方案可以保证时效性，但往往在线滤除的方法都存在动态障碍物信息滤除不够干净的问题，而离线后处理的方法虽然牺牲了实时性，但却可以获取SLAM周期内的完整的轨迹信息作为参考信息并用以来识别动态障碍物点云，以追求“极致的”动态点云滤除的准确性和充分性。

3.2.2、动态障碍物滤除方案原理分类

动态障碍物滤除的算法既可以根据上文中基于时机来完成分类，也可以基于原理来进行细分，大体上主要分为三类：

• （1）基于点云分割的滤除算法（segmentation-based method）：基于分割的方法中当前比较主流的是基于深度学习的语义分割方法，通过前期的点云特征训练，在使用过程中直接标注出点云中的动态障碍物信息部分，在定位和建图算法中根据点云中的标注直接过滤即可，虽然滤除的内容仅限于前期训练得到的模型且容易有假阳性和假阴性的问题，但其方法胜在简单高效。另一类传统的点云分割的方法则基于聚类的模式，如快速点特征直方图 ( fast point feature histograms， FPFH )、基于确定视角下的特征分布直方图VFH(viewpoint feature histogram)，用以在静态聚类中区分出动态聚类，此类方法能够改善计算成本提高实时性从而得到广泛研究，再者就是基于区域（逆）生长以分割混杂着动态和静态点云的目标，识别出动态点云目标以便于后期滤除，等。
• （2）基于可见性的滤除算法（visibility-based method）：该类算法中是基于一个物理前提：“光沿直线传播，如果在同一条光路上存在远、近两个点云，则近侧的点云肯定就是动态点云”，基于可见性的方法在当前的动态障碍物识别和滤除的研究中十分火热，且研究的重点除了提升算法效率外，就是研究怎么解决该方案中的局限性：“误杀问题”和“静态点不可见”的问题。其一，误杀问题包括但不限于点云自干扰、平行点差异导致的假阳性、遮挡点误判、接触点假阳性等一系列由于雷达点云的物理特性引发的误杀问题。其二，静态点不可见的问题相较于误杀问题会少见一些，但更加难以解决，例如当存在动态障碍物一直遮挡了部分或者全部的激光雷达的视线，则雷达没有机会看到这些动态障碍物后方的静态物体信息以至于动态障碍物点云永远不会被识别和滤除掉。

• （3）基于栅格的滤除算法（voxel-based or ray-casting based method）：该方案是最为经典和传统的，其方案背后的物理前提与基于可见性的滤除方案一致，不同点在于其具体实现的时候需要结合栅格地图（例如概率占据栅格、八叉树栅格，etc），识别与区分静态和动态点的基本原理：激光点云所落在的栅格处hits计数+1或者占据概率上升，激光光束穿过的所有栅格misses计数+1或者空闲概率上升，通过统计同一栅格中hits和misses数量差值或者占据概率，以判断栅格中包含的点云是否需要滤除。

4、动态障碍物滤除算法推荐

以上，介绍了动态障碍物滤除算法应用层面和基本原理，这一章节中将具体推荐几个本人认为很有参考价值、值得学习并值得花时间去复现的几个动态障碍物滤除算法和理论。

以下论文pdf和代码链接，在公众号「计算机视觉工坊」后台，回复「原论文」即可获取。

4.1、基于点云分割的滤除算法（segmentation-based method）推荐

【1】Zhou, H. et al. (2020) Cylinder3D: An effective 3D framework for driving-scene lidar semantic segmentation, arXiv.org. Available at: https://arxiv.org/abs/2008.01550 (Accessed: 22 August 2023).

在线实时的过滤算法，长期霸榜SemanticKITTI:Moving Object Segmanteion，

【2】Pfreundschuh, Patrick, et al. "Dynamic Object Aware LiDAR SLAM based on Automatic Generation of Training Data."2021 IEEE International Conference on Robotics and Automation (ICRA), 2021.

在线实时的过滤算法，苏黎世理工出品，必属精品

【3】PRBonn (no date) PRBonn/LIDAR-MOS: (LMNet) moving object segmentation in 3D LIDAR DATA: A learning-based approach exploiting sequential data (RAL/IROS 2021), GitHub. Available at: https://github.com/PRBonn/LiDAR-MOS (Accessed: 22 August 2023).

在线实时的过滤算法，新晋动态障碍物滤除算法，近期热度很高

4.2、基于可见性的滤除算法（visibility-based method）推荐

【4】G. Kim and A. Kim, “Remove, then revert: Static point cloud map construction using multiresolution range images,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.

离线后处理的过滤算法，应该是2020年热度最高的动态障碍物滤除算法了，理论好理解，代码移植容易，且还可以深度开发用于life-long slam。详细论文的解析可见：https://zhuanlan.zhihu.com/p/491271775

【5】F. Pomerleau, P. Krusi, F. Colas, P. Furgale, and R. Siegwart, “Long-term 3d map maintenance in dynamic environments,” in 2014 IEEE International Conference on Robotics and Automation (ICRA), 2014.

离线后处理的过滤算法，与REMOVERT一致的地方在于也是可以深度开发用于life-long slam。这里也推荐「3D视觉工坊」新课程《面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)》。

4.3、基于栅格的滤除算法（voxel-based method）推荐

【6】J. Schauer and A. Nuchter, “The peopleremover—removing dynamic objects from 3-d point cloud data by traversing a voxel occupancy grid,” IEEE robotics and automation letters (RAL), 2018.

离线后处理的过滤算法，经典中的经典，考虑的细节很多，开源代码的工程实现也很值得学习。

【7】cartographer中2D、3D概率占栅格地图中的更新和维护方式，传统且稳定，简单且优质，强推，值得学习。可以在线处理也可以离线后处理的过滤算法。

系统学习3D视觉
基础课程：
[1]面向三维视觉算法的C++重要模块精讲：从零基础入门到进阶
[2]面向三维视觉的Linux嵌入式系统教程[理论+代码+实战]
[3]如何学习相机模型与标定？（代码+实战）
[4]ROS2从入门到精通：理论与实战
[5]彻底理解dToF雷达系统设计[理论+代码+实战]
工业3D视觉方向课程：
[1]（第二期）从零搭建一套结构光3D重建系统[理论+源码+实践]
[2]保姆级线结构光（单目&双目）三维重建系统教程
[3]机械臂抓取从入门到实战课程（理论+源码）
[4]三维点云处理：算法与实战汇总
[5]彻底搞懂基于Open3D的点云处理教程！
[6]3D视觉缺陷检测教程：理论与实战！
SLAM方向课程:
[1]深度剖析面向机器人领域的3D激光SLAM技术原理、代码与实战
[1]彻底剖析激光-视觉-IMU-GPS融合SLAM算法：理论推导、代码讲解和实战
[2]（第二期）彻底搞懂基于LOAM框架的3D激光SLAM：源码剖析到算法优化
[3]彻底搞懂视觉-惯性SLAM：VINS-Fusion原理精讲与源码剖析
[4]彻底剖析室内、室外激光SLAM关键算法和实战(cartographer+LOAM+LIO-SAM)
[5]（第二期）ORB-SLAM3理论讲解与代码精析
视觉三维重建
[1]彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进)
自动驾驶方向课程：
[1]深度剖析面向自动驾驶领域的车载传感器空间同步（标定）
[2]国内首个面向自动驾驶目标检测领域的Transformer原理与实战课程
[3]单目深度估计方法：算法梳理与代码实现
[4]面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
[5]如何将深度学习模型部署到实际工程中？（分类+检测+分割）
最后
1、3D视觉文章投稿作者招募：文章投稿通道
2、3D视觉课程（自动驾驶、SLAM和工业3D视觉）主讲申请：主讲老师招募
3、顶会论文直播邀请与3D视觉传感器测评直播邀请：顶会论文分享与3D视觉传感器行业直播邀请

5、小结

不论实时地、在线的动态障碍物过滤还是离线的动态障碍物过滤，背后的研究因素是因为权衡了实时性和算法质量，两者之间存在一个trade off，每一位研究学者期待的都是高效且准确地的动态障碍物滤除算法。

一方面，基于可见性的动态障碍物滤除算法，可以发现相关论文里使用的数据集和处理方式必须保证稀疏性，否则在实际操作中对大地图（百万级别的点云地图）处理的计算消耗很大，运行效率很低。其主要原因是在于基于可见性的算法，需要在每一次遍历过程中将query point map和global point map进行视角的坐标转换，当global point map的点云数量超过百万级别时，其坐标转换的消耗就会十分明显，导致算法的使用体验变差。再者，基于可见性的方案中，往往需要进行点云到range image的数据格式变换，但range image又存在特别依赖分辨率和位姿精度，位姿估计不准了，点云分辨率低了，其识别动态点云的效果退化就严重了。

另一方面，基于栅格的动态障碍物滤除算法，栅格结合ray casting算法在理论不存在大的问题，但是在一些空旷区域激光光束没有return的信息，导致无法进行ray-casting来更新栅格里的概率值，但如果加大miss range的casting，其计算量又会大幅上升，导致最终的效果变差。

除此之外，基于可见性和基于栅格的动态障碍物滤除算法的过程中，遗留的问题在于：在建图时过程中，若操作人员（动态障碍物）跟着机器一直往前走的话，那基于ray casting或者visibility based模式去删除动态障碍物的话效果很差，因为相当于进入了上文中提到的“静态点不可见”的问题，同时基于可见性和基于栅格的局限性是由于算法设计时的物理前提所决定的，所以我认为根本解决不了。目前的思路是，基于学习的动态障碍物去除方法的局限性跟基于可见性和基于栅格的局限性不一样，结合基于deep-learning based聚类方法来处理基于可见性和基于栅格的动态障碍物滤除算法“误杀问题”和“静态点不可见”的问题，同时利用基于可见性或栅格的算法补充基于deep-learning based聚类方法的不足。

3D视觉细分群成立啦！
目前工坊已经建立了3D视觉方向多个社群，包括SLAM、工业3D视觉、自动驾驶方向。
细分群包括：
[工业方向]三维点云、结构光、机械臂、缺陷检测、三维测量、TOF、相机标定、综合群；
[SLAM方向]多传感器融合、ORB-SLAM、激光SLAM、机器人导航、RTK|GPS|UWB等传感器交流群、SLAM综合讨论群；
[自动驾驶方向]深度估计、Transformer、毫米波|激光雷达|视觉摄像头传感器讨论群、多传感器标定、自动驾驶综合群等。
[三维重建方向]NeRF、colmap、OpenMVS等。除了这些，还有求职、硬件选型、视觉产品落地等交流群。
大家可以添加小助理微信: dddvisiona，备注：加群+方向+学校|公司, 小助理会拉你入群。