点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
作者：robot L | 来源：知乎
https://zhuanlan.zhihu.com/p/137954753
本文仅做学术分享，如有侵权，请联系删除。
本文中，我们提出一种新的激光惯性里程计和建图方法来达到实时，低漂移和鲁棒的位姿估计在大尺度高速环境中。提出的方法主要由四部分组成，帧预处理模块，动态物体检测模块，激光-惯性里程计模块和激光建图模块。帧预处理模块使用惯性测量来补偿每个激光帧的运动扰动。然后，动态物体检测模块被用于检测和移除每帧的动态物体通过应用CNN分割网络。在获得没有运动物体的未扰动点云后，激光惯性里程计模块使用误差状态卡尔曼滤波器来融合激光和IMU的数据并以高频率输出粗糙的位姿估计。最终，激光建图模块执行精准的处理步骤同时“帧-模型”扫描匹配策略被用于创造一个静止全局地图。我们比较我们方法的表现和两个SOTA方法，LOAM和SuMa，使用KITTI数据集和真实高速场景数据集。实验结果展示了我们的方法效果优于SOTA方法在真实高速场景并达到了具有竞争力的表现在KITTI数据集中。
S. Zhao, Z. Fang, H. Li, S. Scherer. A Robust Laser-Inertial Odometry and Mapping Method for Large-Scale Highway Environments. IEEE/RSJ International Conference on Intelligent Robots and Systems, 1285-1292, 2019.
对于智能移动机器人，SLAM是一个基本技术以达到自运动估计和建图在未知环境中。自运动估计对于汽车控制是重要的，其中地图对于障碍物感知和路径规划非常重要。最近几年，大量基于视觉[1]-[3]和激光[4]的SLAM方法被提出，其提供了许多令人满意的解决方案。与基于视觉的SLAM方法相比，基于激光的方法更可靠在视觉退化的环境中，并提供更鲁棒和准确的位姿估计。然而，基于激光的SLAM算法仍然不能在所有场景中达到鲁棒的位姿估计。其中一个具有挑战性的场景是高速环境。
高速环境带来了许多挑战。例如
速度快：速度一般会达到70km/h，其导致点云在每帧中的扰动并影响建图的准确率。
缺乏几何特征：大多数高速环境缺少垂直方向的几何特征。
高运动物体：高速上有很多运动的车辆。
激光回环区域：高速上很少有回环场景，回环检测策略很难修正轨迹误差。
不幸的是，大多数激光SLAM方法假设环境是静态的。同时，因为这些方法依赖激光提供的测量，它们很难处理剧烈运动在几何退化的环境中。因为这两种限制，这些方法很难提供鲁棒和准确的运动估计在高速场景中。
受上述讨论的启发，在本工作中我们展示了一种新颖的激光-惯性里程计和建图方法（LIOM），其不仅达到了鲁棒的位姿估计而且建立了一个静止的高速地图，如图1所示。主要贡献如下：
我们提供了一个实时的激光里程计和建图流程，同时利用CNN分割网络和激光-惯性框架。CNN分割网络移除了动态物体在每帧激光测量中的的影响，激光-惯性框架克服了剧烈运动并达到了高速环境的运动估计。
为了验证我们提出的解决方案，大量实验被执行在各种挑战环境中，例如具有运动物体，剧烈运动和大尺度的的环境。实验结果展示了我们的方法能够同时在静止和动态高速环境中工作。
剩余部分如下：第2节展示了大量与3D激光扫描仪相关的位姿估计算法。第3节描述了提出方法的细节。定量和定性评估在第5节展示。第6节总结全文。
为了允许移动机器人达到长期自主在挑战性环境中，许多研究人员致力于如何提高激光位姿估计方法的鲁棒性。以高速环境中的运动估计为例，两种相关的问题需要被解决：如何解决剧烈运动，如何排除动态物体以达到鲁棒的运动估计。
由于激光扫描仪的低扫描帧率，一帧不能被看作是一个刚体当传感器的外部运动非常剧烈时。然而，大多数激光SLAM方法假设一帧中的激光点共享一个坐标系，其等价于认为在扫描过程中没有外部运动。这种假设是不合理的，并且会导致运动扰动[6]。
为了解决该问题，Zlot[7]使用IMU技术作为运动模型并匹配连续的激光帧来达到运动估计。Le[6]提出一种外参标定框架，使用IMU测量并建模点云的运动扰动通过预积分技术。Ji[8]4]展示了一种鲁棒的多传感器融合流程以估计自运动并建立一个一致的地图在多种挑战性环境中。该方法使用一个IMU用于运动预测和一个视觉-惯性耦合方法来解决剧烈运动问题。然而，因为上述方法主要考虑静止环境，它很难达到好的结果在高速场景中。
有大量SLAM的解决方案在动态环境中。但是，大多数方法基于视觉传感器，例如RGB-D相机[2][3]或双目相机[1]。相反地，动态环境中基于激光的方法相对较少。大多数基于激光的SLAM方法假设周围环境是静态的，导致动态环境中的一系列问题，例如位姿估计和回环检测的失效。
为了解决该问题，Walcott[9]考虑地图构建过程中的时间因素，并维护一个准确的地图在动态环境中。但是，这种方法主要被设计为低动态环境，其不适合高动态环境例如高速场景。Fehr[10]使用贝叶斯模型来获取点是动态还是静态的，其能够构建一个一致的地图在动态环境中。Rendong[11]和Masoud[112]使用提高的RANSAC算法来跟踪运动物体并达到运动估计在动态环境中。但是，因为上述方法不能整合IMU测量，这些方法很难处理剧烈运动并达到准确的运动估计在高速场景中。Jiang[13]提供了一种激光-相机SLAM系统，使用基于稀疏子空间聚类的运动分割方法来构建动态环境中的静止地图。尽管如此，提出的系统不能实时运行并需要足够的环境光照。因此，应用场景非常有限。Johannes[14]提供一种激光-相机SLAM方法，拒绝运动物体通过语义标号。但是，缺点是它仅使用了lidar来提供视觉特征的深度测量，并且没有利用lidar来达到独立的运动估计。因此，该方法不能达到鲁棒的位姿估计在视觉退化的环境中。
在最近几年，一些其他方法被提出用于3D激光。Jens[15]提供了一种稠密的基于面元的方法用于运动估计和建图。Young-Sik[16]提供了一种激光-相机SLAM系统，基于直接法并达到了好的实时表现。Jean-Emmanuel[17]提供了一种新的激光SLAM方法，使用特定的采样策略和新的帧图匹配方法。但是，上述方法假设环境静止，很难达到鲁棒的运动估计在高速环境中。
从上述的讨论中，我们可以发现基于LiDAR的位姿估计和建图方法在高速环境中没有被解决。本文致力于达到实时，低漂移和鲁棒的位姿估计在大尺度高速环境中。
在本节，我们介绍我们Laser-Inertial Odometry and Mapping（LIOM）方法，如图2所示，由四部分组成。上述模块允许鲁棒的运动估计和建图在高速环境中。
图2. LIOM综述
由于车辆在高速环境中的剧烈运动，点云扰动存在于每一帧，影响位姿估计的准确度。因此，移除点云扰动非常必要。这里，我们主要考虑由于车辆的非均匀运动导致的点云运动扰动。实时步骤如下：
第1步：用时间戳标记激光帧中的激光点，如图3所示。根据等式1，每一帧的所有激光点都被标记时间戳：
(1)
其中 是激光帧的扫描周期。
第2步：找到从时间戳 到 的连续IMU测量，它是最接近当前激光点时间戳 的IMU测量。
第3步：通过整个过程，我们可以获得IMU在时刻 和 在世界坐标系中的位姿， 和 。
第4步：使用线性插值方法以确定IMU在时刻 在世界坐标系中的位姿，其等价于找到当前位姿 。特定公式如下：
(2)
第5步：我们要解决的是当前激光点相对开始点由于非均匀运动导致的运动扰动。由于当前激光点和开始点的位置 和速度 可以获得通过 和 ，我们可以得到运动扰动 在开始的激光坐标系中，如等式3所示
(3)
第6步：变换开始点坐标系中的所有激光点，并且对每个激光点，提取它们的点云运动扰动 。
在该步骤，一个全卷积神经网络（FCNN）[18]被应用来准确检测和分割运动物体，例如车辆，行人和自行车。该过程可以被分为四个连续步骤：1. 通道特征提取。2. 基于CNN的障碍物预测。3. 障碍物聚类。4. 后处理。
通道特征提取：在该步骤，输入点云被投影到2D X-Y平面。根据点的X和Y坐标，每个点被分到一个2D栅格。然后每个栅格内的点集的八个统计量被计算并喂入FCNN。我们建议读者参考[18]进一步了解八个统计量的细节。
基于CNN的障碍物预测：FCNN被用于预测逐栅格的障碍物性质，包括中心偏置，物体类别，置信度，物体高度和类别概率。
物体聚类：上述五个栅格物体属性被用于生成一个障碍物物体。然后一个压缩的并查集被使用来寻找物体类别的候选。
后处理：在障碍物聚类后，我们获得一组候选物体簇。后处理进一步精修潜在的候选簇并输出最终的簇通过预先设计的参数。
这四个步骤将会移除动态物体，并保持静态背景点云用于位姿估计和建图。
为了鲁棒的位姿估计，我们使用误差状态卡尔曼滤波器（ESKF）来实现激光扫描仪和IMU的传感器融合。与传统的卡尔曼滤波器方法相比，该方法主要有两个优点。首先，误差状态很小，意味着我们将忽略二阶乘积并减少计算量。第二，朝向误差状态很小，避免了过参数化和万向锁问题。
在本节，我们将给出简单的运动估计和测量更新的描述。运动预测被用于提供状态预测通过IMU测量，同时测量更新被用于修正预测的状态。更多细节被发现在[19][2]。
1）运动预测：运动预测由两部分组成：误差状态动力学和传播过程。
a）误差状态动力学：根据之前的工作[20]，真实状态 ，预测状态 和误差状态 可以被描述在等式4中
(4)
这里 表示一般的复合。
误差动力学系统如下：
(5)
其中 和 分别表示全局坐标系中的速度和位置。 表示旋转矩阵。 和 是加速度计和陀螺仪偏置。 和 表示加速度计和陀螺仪噪声。 和 是偏置的高斯随机游走噪声。 被表示为 向量。
b）传播过程：传播过程包含预测状态传播和误差协方差传播。我们直接使用欧拉积分来传播预测状态。误差协方差矩阵被推导从线性化误差状态运动学开始。传播规则的表达式在等式6和7中：
(6)
(7)
其中 和 是等式6相对误差状态和噪声向量的的雅克比矩阵； 是噪声向量的协方差矩阵。更多细节见[20]。
2）测量更新：测量更新由四部分组成：观测模型，恢复测量，修正和重置名义状态。
a）观测模型：在误差状态形式中，观测模型可以被写为：
(8)
b）恢复测量：在运动预测过程中，我们获得了位姿先验 并使用它们作为多线程正态分布变换（NDT）的初始猜测。然后，位姿后验 可以被计算通过扫描匹配。最终，测量 和噪声 被恢复通过对KF测量更新求逆：
(9)
(10)
其中 是卡尔曼增益。
c）修正：一旦测量 被计算后，全误差状态后验 和协方差 可以被更新通过等式(11)-(13)。 表示标准KF更新的卡尔曼增益。
(11)
(12)
(13)
d) 重置名义状态：最终，名义状态被更新通过等式(14)
(14)
在完成上述过程后，鲁棒的激光-惯性里程计以高频率（100Hz）被获得。
为了进一步提高位姿估计的准确率，我们采用近似LOAM算法[8]的建图策略。主要不同是我们应用了多线程NDT算法[21]（代码：koide3/ndt_omp）以完成“帧-模型”扫描匹配而不是基于特征的方法。原因是NDT方法更鲁棒相对于特征方法来说。因此，它能够提供更准确的运动估计在几何退化的环境中，特别是高速公路场景。
运行时间展示
推荐阅读：
专辑|相机标定
专辑|3D点云
专辑|SLAM
专辑|深度学习与自动驾驶
专辑|结构光
专辑|事件相机
专辑|OpenCV学习
专辑|学习资源汇总
专辑|招聘与项目对接
专辑|读书笔记
重磅！3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
3Ｄ视觉工坊 认证博客专家 算法 3D视觉 个人公众号：３D视觉工坊。公众号特邀嘉宾及合伙人，先后就职于国内知名研究机构、自动驾驶公司、海康研究院，主要研究方向为深度学习、目标检测、语义分割、图像处理、自动驾驶感知算法等，博客专家。博主先后任职于国内知名研究院、知名大厂，致力于3D视觉算法、VLAM算法开发，涉及相机标定、手眼标定、结构光、点云后处理、三维重建等相关领域的研究，同时也是博客专家。3D视觉工坊坚持原创，近一年来输出了非常多的高质量文章，获得了粉丝的一致好评，我们将始终坚持走原创路线，打造一个铁杆粉丝的聚集区。