点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心【全栈算法】技术交流群
后台回复【相机标定】获取超详细的单目双目相机模型介绍、内外参标定算法原理视频！
摘要：这项工作提出了一种基于扩展卡尔曼滤波器(EKF)的3D激光雷达和IMU的无目标外参标定算法。该算法首先在最小二乘优化框架中使用基于运动的校准约束确定传感器间旋转外参的初始估计，接着在扩展卡尔曼滤波器框架中使用基于运动的校准约束进行量测更新进一步估计传感器间的旋转和平移。作者通过实际实验对该算法进行了验证。
3D 激光雷达和 IMU 在自主机器人中无处不在。3D 激光雷达能够生成环境的 3D 点云并且不受光照影响。这证明了激光雷达在一些机器人的应用中是有益的。然而，由于旋转式激光雷达的旋转特性及其产生测量结果的顺序方式，当机器人进行动态机动时，激光雷达会出现明显的运动失真。运动失真会恶化激光雷达里程计或SLAM的结果。惯性测量单元 (IMU) 可用于减轻运动对旋转式激光雷达的影响。IMU以高于激光雷达旋转速率的频率测量线加速度和角速度。最先进的激光雷达里程计或SLAM算法使用IMU来校正激光雷达扫描中的运动失真（也称为去畸变），以更好地估计机器人的位置和建图。为了使用 IMU 的测量值，这些算法要求 IMU 和激光雷达之间的空间/外部参数是先验已知的，这样来自这两个传感器的数据可以在一个公共的参考系中表示。研究人员通常使用从不同来源采购的产品来组装传感器套件，激光雷达和 IMU 之间的外部校准参数通常是未知的。因此，必须进行外部校准才能使用激光雷达惯性里程计或 SLAM 算法。
有几篇已发表的不依赖于第三个辅助传感器的关于 3D 激光雷达和相机（例如[1] – [4]）、相机和IMU（例如[5] – [7]）的外部校准工作，以及许多3D激光雷达和IMU校准的工作（例如[8] – [13]），然而，其中工作 [8] - [10]是依赖GNSS信息对惯性传感器进行精确姿态估计的方法。因此，当GNSS不可用时，则无法使用这些方法。工作[11]先做相机和IMU 标定，然后用激光雷达交叉标定视觉惯性系统。工作[12] ,[13]不依赖辅助传感器进行 3D 激光雷达和IMU 外部校准，因此在辅助传感器不可用的情况下更易于使用。
工作[12]使用高斯过程回归。工作[14]对IMU测量值进行上采样，以便可以为每个激光雷达发射时间推断出相应的IMU读数。IMU测量的上采样有助于消除在激光雷达-IMU校准数据的收集阶段发生的运动失真。他们使用 3 个正交平面作为校准参考地图/目标，并利用每个测量的激光雷达点在相应平面上的投影作为几何约束，这需要了解未知的外部校准参数。他们对上采样的IMU测量执行IMU预积分[15] ，并解决非线性批量估计问题以确定未知的外部校准参数。
另一方面，工作[13]将IMU状态（姿态、速度、偏差）建模为连续时间样条，而不是 IMU 测量（如[12]），该样条可与IMU测量值进行微分和等效。将 IMU 轨迹建模为连续时间样条有助于推断激光雷达发射时间的 IMU 姿态。与之前的方法（如[12])不同的是该方法利用了校准环境中的所有平面，类似的是该方法也使用每个测量激光雷达点在相应平面上的投影作为几何约束，这需要了解未知的外部校准参数。几何约束被平方并相加以形成一个损失函数，该损失函数使用非线性最小二乘法最小化。尽管[13]不使用任何校准目标，但它仍然需要一个需要具有多个明确定义的平面结构的环境。
本文方法利用扩展卡尔曼滤波器进行3D激光雷达IMU外部校准，从[5]中介绍的相机 IMU 外部校准算法中汲取灵感。与[5]相比，本文使用适合激光雷达的测量模型，与分别使用高斯过程回归和样条的[12]和[13]相比，在EKF框架下使用离散时间IMU状态传播模型来消除激光雷达扫描中的运动失真。
需要外部校准来消除运动失真，并且在收集数据进行外部校准时会发生运动失真。EKF框架允许我们在外部校准过程中使用外部校准参数的最佳估计值来对激光雷达扫描进行运动补偿。该方法遵循预测→去畸变→修正循环，使用扫描匹配技术，该技术使用去畸变扫描来产生状态/协方差更新的测量值，以便对3D激光雷达IMU系统进行外部校准。该工作使用Open-VINS框架来开发校准代码。
该工作所提出的方法不需要任何校准目标或特定的环境特征（如平面）来进行3D-Lidar和IMU的外部校准，与[12]和[13]不同。该方法不依赖于辅助姿态传感器，如GNSS或相机惯性传感器套件，而[8]、[9]、[10]、[11]则需要。该方法不需要对校准参数进行任何测量值初始化，与[5]不同。该方法利用[16]中提出的用于视觉惯性导航的通用EKF，并将其重新设计为结合[17]中使用的基于运动的校准约束，以制定本工作的3D激光雷达-IMU外部校准算法。最后，这是第二个开源的3D激光雷达-IMU校准算法，它不依赖于任何辅助传感器，第一个是[13]。
基于运动的约束类似于校准中常用的标准手眼校准[18]约束，并在[17]中扩展到多模态移动机器人传感器的校准。在本工作的场景中，是两次激光雷达扫描之间的运动，可通过使用任何扫描匹配技术[19]获得，是IMU在两个扫描之间的运动。由于传感器在空间上被固定的刚性位姿（外部校准参数）彼此分离，则获得以下方程中给出的约束
是旋转矩阵，平移向量。使用公式(1)给出的基于运动的约束来估计传感器间的旋转和平移。具体来说，该工作利用公式(1)的旋转分量来初始化传感器间的旋转，并基于该初始化使用扩展卡尔曼滤波器算法来估计传感器间平移。
该工作目标是确定空间6自由度分离，即 3D 激光雷达和 IMU 之间的外在校准参数（图 1）。将校准过程分为三个步骤，即数据收集、传感器间旋转初始化和完全外部校准。
数据采集是任何外部校准算法的重要步骤。由于传感器套件包含一个IMU，一个只能感知本体运动的传感器，因此通过充分激发所有旋转和平移，外部校准参数都是完全可观的。然而，如[5]中所述，沿着至少两个旋转自由度的运动激励对于可观性至关重要。尽管[5]涉及相机IMU校准，但关于可观测性的结论也将扩展到3D激光雷达IMU校准问题，因为在这两种情况下，外部传感器（[5]中的相机和本工作的激光雷达）都用作姿态传感器。数据采集轨迹如图2所示。
通过使用基于运动的校准约束来估计IMU和3D激光雷达之间的外参。旋转分量如公式2所示。
在该工作的实现中，使用传感器的旋转矢量表示。公式(2)可以重写为
其中和分别和的旋转矢量。如图3a所示，使用NDT扫描匹配[19]来估计连续激光雷达扫描之间的激光雷达旋转。在两个扫描间隔之间积分陀螺仪角速度，以估计IMU旋转。对于每个和对应的，结合公式(3)中给出的约束形成一个未知的目标函数公式(4)。
为了获得估计，需要将公式(4)相对于最小化，如公式(5)所示。使用Ceres[20]非线性最小二乘解算器来解决公式(5)中的优化问题。
在此步骤中，用于估计的传感器旋转和存在一定的缺点。首先，是通过在不考虑陀螺仪偏差的情况下积分陀螺仪测量值来计算的，其次，通过激光雷达扫描的NDT获得，由于传感器套件在数据收集步骤中进行的运动，激光雷达扫描可能会产生明显的运动失真。此步骤仅提供的初始估计值，用于初始化基于EKF的算法。
传感器间平移的估计取决于IMU平移，其涉及IMU加速计测量的双重积分，但在不知道偏差的情况下执行双重积分将引入显著误差。使用扩展卡尔曼滤波器，该滤波器除了估计和之外，还估计加速度计和陀螺仪偏差、激光雷达扫描瞬间IMU的姿态和速度。EKF方法的框图如图3b所示。我们将估计的状态是：
这里M是扫描次数。是扫描时间戳k处的IMU状态。EKF状态向量有演化分量和静态分量。不断发展的组件是扫描时间戳k处的IMU状态：
1）状态传播
我们使用[5]，[16]中给出的离散时间实现将EKF状态从IMU时间戳i传播到i+1。陀螺仪和加速计测量值在IMU采样周期Δt期间假定为恒定。在以下等式中，是由于重力在全局框架G中产生的加速度。
这里，是高斯白噪声。离散化并取期望值，式15可以写成：
除了状态变量的传播，还需要使用式17 将EKF 状态协方差𝐏从IMU时间戳i传播到i+1。
其中
2）去畸变
3D激光雷达使用旋转机制顺序产生点测量。当激光雷达移动时，它产生的原始扫描会受到运动失真的影响。校准数据收集过程要求传感器套件呈现运动激励，这会将原始扫描中的点从其真实位置移开。在激光雷达扫描中，每个3D点都是从时间上唯一的帧中测量的，并带有时间戳(位于两个相邻扫描时间戳之间)。为了解决运动失真的问题，我们需要预测IMU在时间点的姿态。IMU传播模型用于IMU在时间点的姿态预测。一旦有了IMU在点云时间戳的姿态的估计，就可以使用外部校准参数的最佳估计来推断相应的Lidar姿态，这可以通过式1中给出的运动约束来完成。例如，考虑第k次扫描中带有时间戳的点。为了纠偏该点，利用式1，并使用它来估计扫描时间戳k和点时间戳k_i之间的激光雷达运动(式18)
用于变换点，以获得校正的激光雷达扫描点。在这里，是当时的外部校准参数估计值，是扫描时间戳k处IMU姿态的估计值，最后是使用IMU状态传播模型获得的点时间戳处的IMU姿态。
3）NDT
在去畸变后，使用NDT扫描匹配[19]在连续畸变校正激光雷达扫描k-1和k之间生成激光雷达运动估计值。使用这些激光雷达运动估计值作为EKF状态更新的测量值。
4）状态更新
状态更新模块需要了解测量模型、测量残差和与状态变量相关的测量雅可比。使用式1中的运动约束来推导测量模型：
其中
分离式19的旋转和平移分量，我们得到
在状态估计值下计算的测量模型提供了预测的旋转和平移测量值，即分别为和 。真实测量值和预测测量值之间的差异为我们提供了状态更新所需的测量残差：
系统（图1）由Ouster 128线激光雷达和Vectornav VN-300 IMU组成。激光雷达以 10Hz 输出扫描，IMU 以 400 Hz 输出陀螺仪和加速度计测量值。
Ouster 128通道激光雷达，除了在128通道模式下使用它外，我们还可以将其重新配置为16、32、64通道模式。因此，该工作还展示了当激光雷达在较少线数时，该算法的性能，目的是确定无论激光雷达中的通道数量如何，该算法都会提供比较性能。通过将IMU分别放置在两个不同的位置，即位置1和2（见图1），收集了两个不同的数据集1和2。在以下部分中，所有结果都是使用数据集2生成的，3D 激光雷达在128通道模式下运行。对于所有通道模式和两个数据集，我们获得了类似的结果，但为了版面考虑，作者省略了这些结果。
图4和图5显示了估计的外在校准的收敛性。就考虑平移变量而言，我们使用初始化滤波器，滤波器收敛到具有紧密±1σ边界的固定值。就旋转变量而言，我们使用从传感器间旋转初始化技术中获得的估计值初始化滤波器。
在本节中，介绍了在基于EKF的校准过程中消除激光雷达扫描畸变的重要性。在图6中，展示了在校准过程中获得的扫描匹配的结果。图6中的左图显示了模糊和错位的边缘/角。边缘/拐角是模糊的，因为扫描没有去畸变，并且它们是未对准的，因为来自原始扫描匹配的激光雷达里程计是不正确的。运动失真的激光雷达扫描导致激光雷达里程计的结果较差，进而导致校准估计的整体恶化(见图7)。然而，在图 6 的右图中，可以看到边缘/角落很清晰且不模糊，这要归功于激光雷达扫描的去畸变。校准结果的对比如图7所示。
表I和II显示了卡尔曼滤波过程结束时针对激光雷达不同线数获得的最终校准估计值，数据集1和2分别在位置1和2收集。在表I和表II中，可以发现该校准算法收敛于校准参数的估计值与激光雷达线数无关，从而证明了其在具有不同线数的激光雷达上的可用性。
验证校准算法的通常方法是使用模拟器、辅助传感器（如相机或GPS），或者在原始设备制造商的情况下，使用CAD图作为地面实况。当通过从不同的供应商采购传感器来组装传感器套件时，则无法了解实际情况。此外，使用任何辅助传感器也将带来额外开销。在这项工作中，作者使用了一种相对验证技术，该技术可以根据标尺测量获得的参考来验证该校准结果。如前所述，作者收集了两个数据集1和2，IMU分别保存在两个不同的位置1和2（见图1）。根据标尺测量，两个IMU位置之间的相对偏移沿y轴为5cm，以此作为参考，将校准结果进行比较。在表III中显示了两个位置的校准差异（即表I和表II中显示的结果之间的差异）。在128、64、32和16通道模式下沿y轴分别获得3.2%、0.6%、5.4%和9.4%的百分比误差。
如[5]所述，建议在校准过程开始时激发至少两个旋转自由度足够长的时间，以确保收敛到可靠的校准值。图8和图9显示了传感器套件在数据收集过程中进行的旋转和平移轨迹。在图4中，平移校准参数在25-30秒内收敛，对应于围绕两个轴（即X和Z）的旋转运动（见图8）停止执行的时间。就旋转校准参数而言（图5），它们在基于EKF的校准过程中略有变化，因为它们已经在初始化过程中接近最终估计值。
该工作提出了一种基于EKF的3D激光雷达IMU标定算法，该算法不依赖于任何标定目标或辅助传感器来估计外参标定。作者介绍了该算法在不同激光雷达线数下的可用性；对于从两个不同的IMU位置收集的两个数据集，给出了所有激光雷达线数的对比结果。另外作者还提出了一种在无法获得地面真值或没有辅助传感器时验证标定结果的方法。
[1] Mishra S , Pandey G , Saripalli S . Target-free Extrinsic Calibration of a 3D-Lidar and an IMU[C]. International Conference on Multisensor Fusion and Integration for Intelligent Systems. IEEE, 2021.
往期回顾
最新综述 | 复杂环境中的计算机视觉问题介绍及解决！
【知识星球】日常干货分享
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；
添加汽车人助理微信邀请入群
备注：学校/公司+方向+昵称
自动驾驶之心【知识星球】
想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！
