openCalib中Lidar和IMU(INS)标定--论文解读
文章目录
- An Extrinsic Calibration Method of a 3D-LiDAR and a Pose Sensor for Autonomous Driving 自动驾驶三维激光雷达与姿态传感器的外参标定方法
-
- 摘要
- 1.引言
- 2.相关工作
- 3.方法理论
-
- A.问题抽象化
- B.粗标定
- C. 精标定
- D. *Z*轴校正
- 4. 试验部分
-
- A. 试验设置
- B. 定性评价
- C. 定量评价
- D. 对比试验
- 5. 总结
- 写在后面
An Extrinsic Calibration Method of a 3D-LiDAR and a Pose Sensor for Autonomous Driving 自动驾驶三维激光雷达与姿态传感器的外参标定方法
OpenCalib是商汤科技&上海AI实验室开发的一款开源的自动驾驶传感器标定工具箱，其中包含了这一领域的一整套自动驾驶相关标定方法。本篇论文对应工具箱中的Lidar和IMU(INS)标定方法。
论文：
- https://arxiv.org/pdf/2209.07694.pdf（原文）
- https://arxiv.org/pdf/2205.14087.pdf（opencalib论文）
代码：
- https://github.com/OpenCalib/LiDAR2INS（单独把lidar2imu从opencalib中择出来，代码一样）
- https://github.com/PJLab-ADG/SensorsCalibration（opencalib）
摘要
精确可靠的传感器校准对于自动驾驶中融合lidar和惯性测量至关重要。该论文提出了一种新的自动驾驶三维激光雷达和姿态传感器的three-stage外参标定方法。第一阶段通过点云表面特征快速校准传感器之间的外部参数，使外部参数在短时间内从较大的初始误差范围缩小到较小的误差范围；第二阶段在去除运动畸变的同时，根据lidar制图空间占用率进一步标定外部参数。在最后阶段，对自动驾驶车辆平面运动引起的z轴误差进行校正，最终得到精确的外部参数（论文所说的最后一步讲述的并不是很详细，后续我测试看下效果）。具体来说，该方法利用了道路场景的自然特性，使其具有独立性，易于在大尺度条件下应用。在实际数据集上的实验结果证明了该方法的可靠性和准确性。这些代码在Github网站上是开源的。据我们所知，这是第一个专门为自动驾驶设计的开源代码，用于校准激光雷达和姿态传感器的外部参数（其他比较出名的lidar2imu开源标定工作有浙大的lidar_imu_calib、ETH的lidar_align、百度aollo的在线标定服务等，详见我另一篇博客）。
图1：(a)实验平台左视图。(b)实验平台俯视图。
1.引言
随着科学技术的不断发展，自动驾驶技术越来越受到人们的重视。要实现自动驾驶汽车的各种复杂功能，精确可靠的位置信息是必不可少的。因此，大多数自动驾驶汽车将配备高精度姿态传感器，如NovAtel、Trimble等高精度定位设备。3D-LiDAR是自动驾驶技术中的另一个关键传感器。SLAM和目标检测是激光雷达在自动驾驶领域的两个最重要的应用，而SLAM的主要作用是建图和定位。需要注意的是，在自动驾驶汽车的运动过程中，LiDAR的每个激光点都是在不同的参考位姿下产生的，这也是LiDAR运动失真的来源。在自动驾驶中，由运动引起的激光坐标系运动误差是不可忽视的。通常，激光雷达的运动畸变是通过位姿传感器来消除的。高精度地图的构建和定位在很大程度上依赖于激光雷达和位姿传感器融合。精确的三维激光雷达和位姿传感器外部标定对于确定它们的坐标关系和进行传感器融合至关重要。
由于近年来LiDAR-IMU系统[1]-[4]的快速发展，也出现了许多校准LiDAR-IMU[5]-[10]的方法。这些方法通常对小型机器人平台LiDAR-IMU的内部参数和外部参数进行标定。然而，这些校准方法在直接用于校准激光雷达在自动驾驶汽车上的传感器时并不一定能很好地工作。自动驾驶汽车通常配备高精度的位姿传感器（例如常见的组合惯导设备，rtk+imu方案，华测、导远等品牌），只需要标定外部参数。目前，还没有一种从激光雷达到自动驾驶专用姿态传感器的标定方法。因此，本文提出了一种基于激光雷达的自动驾驶传感器标定方法。受到[11]（香港大学的BALM）的启发，我们提出了一种基于多帧间对应表面特征点的约束的粗糙标定方法。然后，利用基于lidar建图空间占用率的优化方程进一步优化了粗糙标定结果。同时，通过多个基准点对车辆平面运动引起的z轴上无法评估的误差进行修正。通过这种三阶段方法，逐步得到克服平面运动维数的精确外部参数。这种三阶段框架是无目标的，可以使用环境中的一些表面特征进行校准。虽然第一阶段称为粗标定，但标定精度也很高，特别是在平面特征较多的环境中。第二阶段在第一阶段的基础上进行小优化，进一步提高校准精度。同时，我们利用世界坐标的一些基点来修正平面运动造成的对Z轴缺乏约束的问题，使其在旋转和平移的六自由度参数上非常准确。该方法对环境设置要求较低，是一个全自动校准过程，校准所需时间相对较小，因此该方法有助于更高效和实用的大规模自动驾驶汽车校准和部署。
本文工作的贡献如下：
- 该方法是为自动驾驶汽车设计的一种由粗到细的自动标定方法，用于标定激光雷达和位姿传感器的外部参数；
- 介绍了一种通过直接最小化特征点到平面的距离来求解激光雷达和姿态传感器外部标定初始化的算法；
- 定义了一种基于八叉树的空间占用细化方法，进一步细化外部参数，通过姿态传感器提高激光雷达的建图质量。通过基准点匹配，提高了z轴的标定精度；
- 在真实世界的数据集上，我们定量和定性地证明了我们的方法的鲁棒性和准确性；同时，相关代码已在GitHub上开源。
2.相关工作
部分研究提出了许多方法来解决多传感器标定问题。传感器标定可分为内部参数标定和外部参数标定两部分，内部参数决定了传感器的内部映射关系。例如，IMU的固有参数包括陀螺仪和加速度计的零偏、比例因子和安装误差，可通过[12]-[16]方法进行校准。外部参数决定了传感器与外部坐标系之间的变换关系，包括旋转和平移的6个自由度参数。LiDAR-IMU的外在校准通常与IMU的内在校准相一致，例如[5]-[10]方法。LV等人[7]提出了第一个基于连续时间批处理估计的开源LIDAR-IMU校准工具箱。该方法可以对不同位置的坐标系偏移量进行校正，并证明了该方法的可靠性。随后，他们对该方法进行了扩展，提出了OALICalib[10]，该方法旨在自动选择信息量最大的数据段进行校准，避免了一些没有足够运动或场景约束的数据，从而提高校准精度，降低计算成本。Gentil等[9]采用高斯方程回归消除IMU运动失真，采用基于因子图的优化方法对LIDAR和IMU进行标定。Jiao等[17]通过手眼标定[18]得到标定初值，然后，采用基于外观的方法，通过最小化特征点对平面的残差函数来优化得到的参数。Mishra等人[8]提出了一种基于EKF的优化方案来标定LIDAR和IMU。[19]，[20]提出了一种基于连续时间批估计框架的camera-IMU时空定标方法。Forster等人[21]解决了确定未知外部标定参数的非线性批估计问题。以上校准方法需要以良好的初值为基础。Park等人[22]采用由粗到细的校正方法，首先估计闭合形式解，然后批量优化连续时间轨迹，以获得更精确的结果。
现有的大多数方法都是同时对lidar-IMU的外部和内部进行标定。由于缺乏z向运动，现有的校准方法通常难以校正z轴的偏移量。在自动驾驶系统中，姿态传感器可以在全局坐标系下输出精确的姿态，只需要标定外部参数而不需要考虑内部参数。我们的工作代表了激光雷达和姿态传感器从粗到精的标定。在得到满意的结果后，我们的方法还通过基准点匹配来校正z轴的偏差。
3.方法理论
本节介绍了我们的方法的细节，包括粗略校准，校准细化，和z轴校正。图2显示了所提方法的概述。
图2：提出的激光雷达和姿态传感器外部标定方法的流程包括粗糙标定、标定精化和z轴校正三个部分。
A.问题抽象化
如图3所示，车辆通过行走三个8字形轨迹在路口收集激光雷达和姿态传感器序列数据。然后，通过数据处理模块获得LiDAR时间戳对应的位姿传感器位姿数据。在获得准确的外部参数的情况下，利用激光雷达和姿态传感器数据可以对周围环境进行重构。因此，我们的目标是找到一个从LiDAR到姿态传感器的刚性变换 T = { R , t } \boldsymbol{T}=\{\boldsymbol{R}, \boldsymbol{t}\} T={R,t}，使多帧点云通过姿态传感器提供的姿态拼接得到的三维重建结果更加准确。R是一个三维旋转， R ∈ S O ( 3 ) \boldsymbol{R} \in S O(3) R∈SO(3)。t是一个三维平移， t ∈ R 3 \boldsymbol{t} \in R^3 t∈R3。首先，我们在世界坐标系中对姿态传感器输出位姿 T pose-sensor T{\text {pose-sensor }} Tpose-sensor 进行处理，并通过外部参数T转换得到点云的位姿 T lidar T_{\text {lidar }} Tlidar 。
T lidar i = T ⋅ T pose-sensor i = { R lidar i , t lidar i } T_{\text {lidar }}^i=\boldsymbol{T} \cdot T_{\text {pose-sensor }}^i=\left\{R_{\text {lidar }}^i, t_{\text {lidar }}^i\right\} Tlidar i=T⋅Tpose-sensor i={Rlidar i,tlidar i}
图3：标定数据采集方法为自动驾驶汽车绕十字路口转3个8字，图中的轨迹为车辆采集数据时的行驶轨迹。
然后，通过LiDAR位姿 T lidar T_{\text {lidar }} Tlidar 和LiDAR点云序列 P lidar P_{\text {lidar }} Plidar 恢复三维重建地图M。
M = ∑ i = 1 N ( R lidar i ⋅ P lidar i + t lidar i ) \boldsymbol{M}=\sum_{i=1}^N\left(R_{\text {lidar }}^i \cdot \boldsymbol{P}_{\text {lidar }}^i+t_{\text {lidar }}^i\right) M=i=1∑N(Rlidar i⋅Plidar i+tlidar i)
其中M为世界坐标系下的全局三维点云图，我们的目标是找到一个外部参数T，使重构的地图M具有最好的质量。
B.粗标定
第一步是粗标定。粗标定的目的是快速地将外部参数从任意初始值降低到一个小的误差范围。然而，通过传感器的坐标轴对准和位移测量，初始外部参数不会特别离谱，因此在我们的试验中没有选择特别离谱的初始外部参数（该论文的标定方法需要给模型提供一个较好的初值，并不是完全的自动标定，可以采用卷尺手工测量的方法）。在我们的实验中，我们的粗略校准可以在不到20s的时间内将角度和平移的外部误差从相对于地面真实值的20多度和0.5m降低到0.2度和0.03m以内。为了减少时间，我们在点云上进行特征提取。与[11]BALM方法相似，我们通过自适应体素化提取点云特征中的平面特征。下一步是结合姿态传感器的姿态信息将点云投影到相同的坐标系中，然后在滑动窗口中进行优化。具体的优化方法是假设有一个n帧的滑动窗口，在考虑曲率较小的点在平面内，然后通过平面拟合记录当前平面的中心点X和法向量n。然后通过点云坐标变换将点云投射到第一帧上。将第n帧LIDAR坐标系中的点云投影到第1帧位姿传感器坐标系的公式如下：
x I 1 = T I 1 − 1 T I n T x L n x_{I 1}=T_{I 1}^{-1} T_{I n} \boldsymbol{T} x_{L n} xI1=TI1−1TInTxLn
其中 x L n x_{L n} xLn表示LiDAR坐标系中的第n帧点云。
在转换到相同的坐标系之后，下一步是数据关联。同样，通过点云特征提取找到平面上的点，然后联合求解帧n中与帧1对应的平面的点云，以及帧1和帧n中所有平面点的最近邻，求解优化问题:
T L I = argmin { ∑ k = 1 N ∥ γ plane k ∥ 2 } ∥ γ plane k ∥ 2 = ∑ p = 1 M ( x p − X ) N \begin{array}{c} T_L^I=\operatorname{argmin}\left\{\sum_{k=1}^N\left\|\gamma_{\text {plane }}^k\right\|^2\right\} \\ \left\|\gamma_{\text {plane }}^k\right\|^2=\sum_{p=1}^M\left(x_p-\mathbf{X}\right) \mathbf{N} \end{array} TLI=argmin{∑k=1N∥∥∥γplane k∥∥∥2}∥∥∥γplane k∥∥∥2=∑p=1M(xp−X)N
其中 ∥ γ plane k ∥ 2 \left\|\gamma_{\text {plane }}^k\right\|^2 ∥∥∥γplane k∥∥∥2为点到平面的残差。
通过求解上述最小二乘问题，可以得到标定的初始解。需要注意的是，在粗略校准时不要进行点云畸变校正处理，因为收集校准数据的车辆速度非常慢。在我们的测试中，去畸变和不去畸变的结果相差不大。因此，为了提高粗校正的速度，不进行去畸变过程。
粗标定提取点云特征，然后根据特征优化外部参数。图4显示了通过粗糙标定过程提取的表面特征，以及粗标定使用的特征点云，从初始状态开始进行快速、完整的标定。该过程的校准速度非常快。我们在实验中收集了三个不同场景的多个校准集数据，每个数据包含1000+帧点云，粗略校准的平均时间如表1所示，校准精度如表2所示。就耗时的校准而言，通过特征提取和优化，可以非常快速地完成标定，从而节省了大量标定耗时，有利于大规模的标定。在校准质量方面，如果要求不是特别苛刻，可以将粗标定结果作为最终的校准结果。
C. 精标定
通过上一步，我们得到了一个很好的外部参数初值。为了进一步增强映射效果，我们使用类似于[24]的基于八叉树的优化，将三维空间划分为体素网格。与[24]相比，我们使用了多帧（5帧）点云结构，点云密度相对较密，所以效果更好。这里我们优化的目标是空间占用率。空间占用的网格数越少，建图效果越好，对应的外部参数越准确。首先，利用粗标定结果，通过匀速模型去除点云运动畸变；然后，将初始标定结果通过位姿传感器位姿公式(3)： x I 1 = T I 1 − 1 T I n T x L n x_{I 1}=T_{I 1}^{-1} T_{I n} \boldsymbol{T} x_{L n} xI1=TI1−1TInTxLn转换为第一帧的位姿传感器系统。将点云转换到同一坐标系后，将空间划分为体素网格。在校正结果准确的情况下，同一坐标系下所有点云所占空间体素最小，如图5所示。
图5：基于空间占用优化的原理图。对于不准确的外部参数引起的点云分叉，经过细化后，空间中所占网格数减少。
T = arg min T { occupancy ( T , x p ) } T=\arg \min _T\left\{\text { occupancy }\left(T, x_p\right)\right\} T=argTmin{ occupancy (T,xp)}
其中， x p x_p xp表示T拼接后的点云。
在校正的最后，我们得到了更精确的校正结果和更好的建图效果。基于八叉树的标定改进需要块空间和遍历空间，空间和时间成本非常高。由于粗糙标定的优异性能，我们在细化标定步骤中只在较小的外部参数范围内进行优化，因此整体消耗非常低，标定速度非常快。精细标定的结果在视觉上与图6中的粗标定相似，这一阶段的性能通过表2中的定量指标进行评价。
图6：通过精细标定，进一步提高了校正结果。(a)是粗校正的结果，(b)是细化的结果，可以看出细化后的墙体和车道线变细了。
D. Z轴校正
由于大多数标定场景下地面足够平坦，而此时z轴方向的激励不够充分，因此激光雷达和位姿传感器在z轴方向的外部参数标定效果会很糟糕（上面的标定流程对Z轴几乎没有校正效果）。为了解决这一问题，我们提出了利用基准点来优化z轴的标定。由于姿态传感器是由GPS和IMU融合输出的，因此输出全局姿态。基准点是由定位装置测得的精确世界坐标的三维点。在我们的校准场地，我们可以随机选择地面上的三个或更多的点作为基准点进行精确测量（可以使用组合惯导设备直接测量基准点）。通过基准点优化Z轴的误差，进一步降低建图的高度误差到更小的值。
在获得精度校准结果后，最后一步是校正校准结果的z向平移。如前所述，由于车辆在z轴方向上缺乏运动，所得到的结果在z轴方向上的平移很可能存在误差，只能得到具有5维精度的校准结果。最后，我们纠正z轴上的误差。我们取整个地图的K个参考点，将地图投影到全局坐标系中，生成局部地图。然后在局部地图上找到每个基准点的最近邻居，进行最小二乘优化，得到Z轴上的最终校正偏移量。优化方程如下：
Z f i x = arg min Z ∑ i = 1 K { ∥ X m i − X b i ∥ 2 } Z_{f i x}=\arg \min _Z \sum_{i=1}^K\left\{\left\|X_{m i}-X_{b i}\right\|^2\right\} Zfix=argZmini=1∑K{∥Xmi−Xbi∥2}
其中 X b i X_{b i} Xbi表示第i个基准点（全局地图-世界坐标系）， X m i X_{m i} Xmi表示第i个基准点在局部地图上的最近邻居点。
图7显示了由于z轴误差，局部地图上的接地点与实际全局坐标系中参考点之间的高度误差。
图7：z轴的标定误差通过预先在地面设置的参考点进一步修正。通过三维坐标测量装置得到世界坐标中的参考点。
图8显示了经过Z轴校正和未经过Z轴校正的地图对图像的投影结果。如果校正z轴，就可以完美匹配。否则，z轴上的一些误差会导致高度不对。
图8：(a)为未经过z轴校正的地图在图像上的投影，地图的车道线投影与图像中的车道线不对齐。(b)是经过z轴校正后的映射在图像上的投影。
4. 试验部分
为了评估我们的方法的性能，我们在三组数据上进行了实验。如图9所示，我们在道路上选择了3个标定场景，每个场景记录了10组标定数据。数据采集方法如图3所示。通过定性和定量评价，结果表明该方法具有较高的准确性和鲁棒性。
图9：图中左列为初始不准确外部参数下（手工测量参数）的点云拼接结果，右列为校正后的点云拼接结果。
A. 试验设置
我们在真实的无人驾驶平台上进行了实验，图1显示了我们的真实实验设置。上面是禾赛Pandar64激光雷达。姿态传感器是Novatel PwrPak7，放置在后轴上，车顶上有两个天线。为了评估我们的方法在参考校准方面的性能，我们分别测量了平移和旋转的误差。我们还计算了平移的三个分量，即 Δ t x \Delta t_x Δtx、 Δ t y \Delta t_y Δty和 Δ t z \Delta t_z Δtz的平均绝对误差(MAE)，以及遵循ZYX表示的 Δ y a w \Delta yaw Δyaw、 Δ p i t c h \Delta pitch Δpitch和 Δ r o l l \Delta roll Δroll三个欧拉角的平均绝对误差(MAE)。我们的方法是在一台台式机上用c++实现的，该台式机采用Intel酷睿i7-8700 CPU和Nvidia 1660 GPU。在我们的试验中，整个过程只需要30 s左右的时间，如果只需要粗略的校准过程，收敛时间不到10 s，如图10和图11所示。此外，由于我们的方法需要更少的时间消耗和全自动化（并非完全的自动化，需要先手工测量），我们认为我们的方法是高效和用户友好的，可广泛部署在自动驾驶汽车上。
图10：在粗标定过程中，三个角度随时间收敛到一个固定值，纵轴为当前优化结果与地面真相之间的误差。
图11：在粗标定过程中，两个平移随时间收敛到一个固定值，纵轴为当前优化结果与地面真值之间的误差。
B. 定性评价
为了更好地可视化我们的方法的性能，我们根据校准结果拼接点云。图9为标定前后的结果。因为粗标定结果已经很好了，所以最终的标定结果和粗略的标定结果非常相似，在可视化上很难看出差异。此外，为了评价粗标定的收敛性，我们给出了差异较大的外部参数的初值。从图10和图11可以看出，粗标定的收敛性和稳定性都比较好，从图12可以看出，不同标定场景下的一致性也比较好。图8显示了带Z轴校正和不带Z轴校正的可视化效果的对比，Z轴校正可以减少高精度地图的高度误差。这些实验结果充分证明了算法的鲁棒性和适应性。
图12：(a)和(b)为粗标定旋转和平移的一致性评价。©和(d)为精标定旋转和平移的一致性评价。
C. 定量评价
为了定量评估我们的校准方法的误差，我们通过[25]中的手动调谐工具获得了一个相对准确的校准地面真值。首先通过标定算法得到一个外部参数，然后根据该外部参数进行人工调优。经过人工调优和下游算法的验证，结果可作为地面真值。MAE如表2所示。我们对30个校准数据的校准结果进行了误差分布统计，得到了残差向量，其统计信息(如均值、方差)定量地揭示了粗优化和精优化结果。结果如图12所示。从图中可以看出，粗标定和精标定的标定外部变量波动较小，标定一致性也较好。
同时，精细化标定进一步提高了标定精度，特别是旋转精度。在对z轴的定量评价上，z轴校正后的z轴平均误差保持在0.01 m以内。
D. 对比试验
我们主要将其与一种三维激光雷达与六自由度位姿传感器之间的开源外部标定方法[23]（ETH的lidar_align）进行比较。表1和表2为校准时间和校准精度的比较。当外部参数计的初值误差较大时（手工测量误差），[23]的标定精度较低。当初始值较好时，精度也较好，且该方法不校正z轴校准精度。相比之下，我们的方法在初始值较差的情况下仍然保持较高的精度。对比实验表明，该方法在耗时和校准精度方面均有优势。
5. 总结
提出了一种three-stage激光雷达对姿态传感器的外部标定方法，该方法在时间消耗和精度方面都具有较高的性能。我们的方法是专门为自动驾驶设计的，可以应用于大型自动驾驶汽车的快速校准。该方法利用多平面特征进行优化（参考BALM），在平面特征场景相对较少的情况下，可能会降低性能。未来，我们将改进我们的方法，以适应特征较少的极端环境。
写在后面
非常感谢商汤科技和上海AI实验室开源了这套自动驾驶标定工具opencalib，其中包含了camera、lidar、imu等传感器的内参标定和外参标定，标定方法也包含了手动标定和自动标定方式，整套标定方法相当丰富。目前刚刚接触opencalib，还没有细细地看代码，后续会更新一些和其相关的博客，感兴趣的朋友可以点下点赞、关注、收藏三连一波。
说回lidar和姿态传感器的标定，目前比较出名的开源项目有浙大的LI-Calib和ETH的lidar_align等,详见我另一篇博客:Lidar和IMU(INS)外参标定----常用开源项目总结。这篇论文在对比试验中就将自己的方法和lidar_align进行了性能对比，精度和速度都有改进。其实浙大和ETH这两个开源项目主要针对的是Lidar和IMU的标定，在标定过程中需要平台在横滚、俯仰、偏航方向上有一定的变化，例如lidar_align更多的是针对小型移动机器人或无人机的设备标定；而浙大的LI-calib在试验过程中使用了手持的平台进行试验，这两种使用情形都比较容易采集到在3个欧拉角或z轴方向上的激励数据。
然而无人驾驶车辆通常行驶在较为平坦的地面上，所以上述两种方案在自动驾驶领域不是很适用。无人驾驶车辆通常以INS（组合导航设备）作为自己的姿态传感器，其输出的数据是GNSS和IMU数据和结合，与纯IMU不同。本文所解读的论文正是借助这个传感器特征推出了一种无人驾驶车辆lidar和ins设备的标定方法，并且提出了在z轴方向上的校正手段。
如果觉得我的文章对你起到了些许的帮助，做下手指操点击下面的大拇指感谢~
我是专注自动驾驶、LiDAR感知、三维点云处理、激光SLAM领域的阿川，欢迎各位志同道合的朋友在下面积极留言。欲将心事付瑶琴，希望能在交流中收获知识、经验和挚友