text: 论文解读 Robust Real-time LiDAR-inertial Initialization (IROS2022)
  LI-Init是一个鲁棒实时的激光雷达IMU初始化工具包，可快速标定出三维激光雷达和IMU的时差以及外参。该工具包具有较强的鲁棒性，无需任何初值即可优化出相对准确的结果，满足绝大多数雷达惯性融合类算法的需求。该文章发布在了2022年的机器人顶会IROS上，项目代码开源在github: lidar_imu_init.
  激光雷达通过过主动发射光线照射物体，利用三角测距或飞行时间法计算物体与自身的距离，凭借其高精度、长探测范围、对光线变化鲁棒等优点，被广泛应用于地理测绘、机器人、自动驾驶等领域。惯性测量单元（IMU）可以在没有任何外部参考的情况下提供高频的自我运动估计，常常和激光雷达或相机一起用于机器人、自动驾驶汽车等的运动估计。随着激光雷达技术的发展，使用了激光雷达和IMU的状态估计算法逐渐开始大量涌现。
  基于激光雷达的三维重建面临一个基本挑战，即在持续运动中准确地确定激光雷达的姿态。解决方案之一是使用高精度RTK/GPS来获取激光雷达姿态，这被广泛应用于地理测绘和航空激光雷达绘图等行业。虽然这种解决方案非常准确，但高精度IMU的成本仍然很高，大大增加了设备的总成本。另一个主要缺点是RTK服务要求建图设备、GPS卫星和地面站之间保持持续、良好和直接的连接。然而，在绝大多数实际环境中，包括所有室内环境和一些有掩蔽的室外环境（如桥梁、隧道、树木、高楼），这样的连接显然是不能保证的。另一种方法是通过直接利用激光雷达的原始测量（即点云）来确定激光雷达的姿态。连续的激光雷达扫描测量同一场景，因此它们之间存在重叠区域。通过在重叠区域配准点云，可以确定不同激光雷达扫描之间的相对姿态，从而建立一个全局一致的地图。这种方法通常被称为同时定位与建图（SLAM）技术，由于避免了任何外部依赖传感器（如GPS/RTK），因此更具前景。但是仅仅依靠激光雷达，会遇到一些额外的挑战和困难，比如：1) 由于一帧激光点云不是在一瞬间完成采集的，所以点云会受到运动畸变的影响，尤其是在高速运动情况下，点云畸变会非常严重；2) 激光雷达可能会在退化场景失效，如长直走廊、隧道或者光滑平面，这可能会导致错误的点云匹配，使得系统崩溃。
  惯性测量单元（Inertial Measurement Unit, IMU）是与相机或激光雷达融合的极好的补充传感器，因为它可以在没有任何外部参考的情况下提供短期自我运动估计。IMU是缓解由于光线暗淡对相机和缺乏结构环境对激光雷达造成的短期里程计失败的理想选择。此外，IMU提供的高频运动测量有助于补偿激光雷达扫描中的运动畸变，尤其是当机器人处于高速运动状态时。
  因此，激光雷达和IMU的传感器组合，成为了状态估计的最佳选择之一。
  传感器融合系统的性能通常严重依赖于由初始化模块提供的准确的初始状态，例如传感器之间的外参和时差。对于大多数自组装设备，激光雷达和IMU通常没有进行严格的时间同步，并且激光雷达和IMU的相对坐标变换（外参）是未知的，因此，一个高效、鲁棒、实时的激光雷达IMU初始化算法是非常必要的，它可以为激光雷达IMU融合类算法提供其必需的参数：激光雷达和IMU之间的外参和时差，从而保证融合算法的性能。
  目前学界中的激光雷达和IMU标定算法都或多或少有一些缺陷，例如依赖于额外的传感器，或是忽略了另外一些状态只标定部分状态，或是计算复杂度高，无法实时进行标定。因此，本文目标是提出一个鲁棒、快速且实时的激光雷达IMU初始化方法，能够有效地标定出激光雷达和IMU之间的外参和时差以及陀螺仪和加速度计偏移量等状态，其标定精度需要满足绝大多数激光雷达IMU融合算法（例如FAST-LIO2[1]）的要求。
  本文提出的标定方法需要采集一组（或多组）激励充分的激光雷达点云和IMU数据，通过纯激光里程计计算得到的角速度及加速度信息，与IMU测量进行匹配，从而来标定激光雷达和IMU的外参和时差。由于IMU仅在运动时激活才有激励，所以本文的初始化过程是基于运动的方法，这意味着需要足够的激励。该方法的工作流程概述如图2所示。
  本文提出的激光雷达里程计是从FAST-LIO2中修改而来，通过采用匀速（Constant velocity）模型来预测激光雷达的运动并补偿点云畸变。为了缓解匀速模型与实际传感器运动之间的误差，通过将输入帧切分成几个子帧来提高激光雷达里程计的频率，使得匀速模型更加合理。如果激光雷达里程计不漂移（例如由于退化）且估计的激光雷达角速度和线性速度满足我们提出的激励评估准则，则认为激励足够，激光雷达里程计输出和相应的IMU数据都被输入到初始化模块中。在初始化过程中，首先通过将IMU测量数据进行滑动以与激光雷达里程计对齐来标定时差，然后进行优化实现进一步的时差和外参估计，并同时估计陀螺仪和加速度计的偏置以及重力向量。
  激光里程计部分（仅仅使用LiDAR）建立在匀速模型上，该模型假设角速度和线速度在连续的两个雷达帧之间，即时间间隔 内为常量，即：
  其中， 是两个扫描之间的时间间隔，状态向量 ，噪声 和离散状态转移函数 定义如下：
  其中， 是激光雷达在全局坐标系中的姿态和位置（这里是第一个激光雷达的体坐标系 ）， 是激光雷达在全局坐标系中描述的线速度， 是激光雷达在激光雷达体坐标系中的角速度，它们分别被建模为由高斯噪声 和 驱动的随机游走过程。
  我们使用基于流形的误差状态迭代卡尔曼滤波器（Error State Iterated Kalman Filter，ESIKF）来估计激光雷达里程计的状态。ESIKF的预测步骤包括状态预测和协方差传播，具体如下所示：
  其中， 分别是状态估计和过程噪声 的协方差矩阵。 和 定义可以参考文章FAST-LIO2。
  在本文考虑的问题中，IMU和LiDAR之间的时差未知，即传感器并未完成时间同步，因此采用IMU辅助的运动补偿方法不可行。在接收到时间戳为 的新LiDAR扫描后，为了补偿运动畸变，我们将每个时间戳 采样的点 投影到扫描结束时的LiDAR坐标系 中，具体如下所示。基于匀速模型，我们有 , ，这可以推导出从 的相对位姿 ， 具体形式如下： 然后，局部测量 可以投影到扫描结束时的 LiDAR 坐标系中，如下所示：
  经过畸变补偿的点云 提供了未知状态 的隐式测量，该测量以点到平面距离残差的形式表示，基于该残差，通过迭代卡尔曼滤波器的框架逐步估计完整的状态 ，直到收敛。收敛的状态估计值 将用于传播后续的 IMU 测量。有关此迭代估计的详细信息可以参考 FAST-LIO2。我们使用带有运动补偿和不带运动补偿的扫描进行的激光雷达里程计的建图结果比较如下图所示。
  IMU原始测量受到噪声 和 的影响。IMU测量模型为：
  其中， 是IMU角速度和线性加速度的真实值。同样，从LiDAR里程计中得到的估计值 也包含噪声。为了抑制这些通常具有高频的噪声，我们使用了一个非因果的零相位低通滤波器[2]来滤除噪声，而不引入任何滤波延迟。零相位滤波器通过前向和反向运行Butterworth低通滤波器来实现，从而生成经过去噪处理的IMU测量值 。为了简化表示，经过去噪处理的LiDAR估计仍然表示为 .
  根据LiDAR里程计估计出的状态 ，我们通过非因果的中心差分得到LiDAR的角加速度 和线加速度 。我们将得到的LiDAR里程计数据集合表示为： 同样地，我们使用经过去噪处理后的陀螺仪测量值 来获取IMU的角加速度 ，得到如下的IMU测量：
  由于IMU的频率通常高于LiDAR里程计的频率，因此两个序列 和 的长度不相同。为了解决这个问题，我们提取在相同时间段内接收到的LiDAR和IMU数据，并通过在每个LiDAR里程计时间戳 处进行线性插值来对 进行下采样（参见图4）。
  下采样后的IMU数据表示为 ：
  其中， 和 具有相同的时间戳 （但数据实际上被未知的时间常量 延迟）。
  在大多数情况下，由于数据在被LiDAR惯性里程计模块接收之前，存在不可避免的传输和处理延迟，并且不同传感器的延迟通常并不相同，因此LiDAR数据 和IMU数据 之间存在一个未知但恒定的时间偏移 ，使得如果将IMU测量 的时间戳减去 ，则它将与LiDAR里程计 对齐。由于LiDAR数据和IMU数据是在离散时间 上的，因此对IMU数据的时间戳修改只能是以离散步长 进行，其中 是两个LiDAR扫描之间的时间间隔。具体而言，对于角速度，我们有： 忽略陀螺仪偏置 （通常很小），我们发现 和 的大小应该相同，而不考虑外参 ，我们使用零中心的最大互相关方法来量化雷达和IMU角速度模长之间的相似性。然后，偏移量 可以从以下优化问题中解决：
  互相关方法对噪声和微小的陀螺仪偏差具有鲁棒性。但是其具有的一个明显缺陷是，仅依靠此方法做时差标定的精度受限于LiDAR里程计的采样间隔 ，任何小于 的剩余时差 都无法被标定出来。设 为LiDAR里程计 和IMU数据 之间的总时差，则 。类似的，如果将IMU测量 提前 的时间，则它将与LiDAR里程计 对齐：
  由于实际的LiDAR里程计 仅在时间戳 处可获得，将 和 代入(13)，注意到 ，我们有：
  注意到 是在时间 附近的IMU角速度，其中角速度和角加速度分别为 和 ，其中 。我们可以通过假设角加速度在极小的 时间内保持不变来插值计算 的值：
  这可以被代入到(14) 中，得到以下表达式：
  最终，基于(16)中的约束，统一的时空优化问题可以表述为：
  首先，我们根据之前校准得到的偏移量 和残余偏移量 ，将 IMU 数据 与 LiDAR 数据 对齐。对齐后的IMU数据记为 ，假设它与 完全对齐，没有任何时间偏移。具体而言，时间 时刻的IMU角速度 与对应的LiDAR角速度 之间的关系由方程(14)给出。 可做泰勒展开，可以被近似表示为
  类似地，IMU 加速度 对应于时间戳为 的 LiDAR 加速度，可表示为：
  然后，类似于公式 (13)，我们可以找到 IMU 和 LiDAR 之间的加速度约束关系。正如 [3] 中所标注的那样，具有固定外参的两个坐标系 $A$ 和 $B$ 的加速度满足以下关系：
  我们将 LiDAR 视为 ，IMU 视为 。由于 LiDAR 的加速度 是以全局坐标系为参考的，我们需要计算 LiDAR 在车体坐标系中的瞬时加速度，表示为 ： 最后，我们可以通过以下优化问题联合估计外部平移、加速度计偏差和重力向量：
  该软件工具基于Ubuntu系统开发和测试，需保证Ubuntu系统的版本大于等于18.04。所需的基础依赖包括Robot Operating System (ROS), 对应激光雷达的驱动程序 (ROS driver)，飞控所需的PX4固件和对应的mavros驱动，以及Ceres优化库等。具体的安装指南可以参考github仓库 https://github.com/hku-mars/LiDAR_IMU_Init。
  首先启动激光雷达和IMU各自的ROS驱动程序，确保点云和IMU数据稳定且频率正确（激光雷达点云话题10Hz，IMU数据为200Hz左右即可）；接着启动初始化程序，确保重要参数正确。表1中为一些重要参数含义以及推荐取值：
  |参数名称||意义||推荐取值|
  |lid_topic||点云话题名字|
  |imu_topic||IMU话题名字|
  |cut_frame_num||分帧数目（提高odom频率）||3 ~ 5|
  |orig_odom_freq||原始输入点云频率(Hz)||10|
  |mean_acc_norm||静止时加速度计读数的模(m/s^2)||9.805|
  |data_accum_length||初始化所需数据的激励程度，越大所需数据越多||400 ~ 800|
  |filter_size_surf||输入点云降采样分辨率(m)||室内0.1，室外0.3 ~ 0.5|
  |filter_size_map||ikd-tree地图降采样分辨率(m)||室内0.2，室外0.5|
  |gyr_cov||纯雷达里程计阶段，角速度的过程噪声||10 ~ 40|
  |acc_cov||纯雷达里程计阶段，线速度的过程噪声||2 ~ 10|
  因为IMU仅在运动情况下才有效，所以需要对传感器进行各个维度上充分的激励，以确保能获得更准确的标定结果。参考的激励方式如图5所示。使用者需移动数据采集平台，沿着激光雷达的X,Y,Z三个轴画弧线，以保证同时给予了角速度和加速度的激励。与此同时，软件包里的数据评估模块会启动，并在终端里实时给出当前激励程度的评估，并引导使用者如何继续给予激励，如图5和图6所示。
  时差标定：
  外参标定：
  [1] Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, and Fu Zhang. Fast-lio2: Fast direct lidar-inertial odometry. IEEE Transactions on Robotics, 2022.
  [2] Fredrik Gustafsson. Determining the initial states in forward-backward filtering. IEEE Transactions on signal processing, 44(4):988–992, 1996.
  [3] Wei Xu, Dongjiao He, Yixi Cai, and Fu Zhang. Robots’ state estimation and observability analysis based on statistical motion models. IEEE Transactions on Control Systems Technology, 2022
  [4] Jiajun Lv, Jinhong Xu, Kewei Hu, Yong Liu, and Xingxing Zuo. Targetless calibration of lidar-imu system based on continuous-time batch estimation. In 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 9968–9975. IEEE, 2020.
  [5] Subodh Mishra, Gaurav Pandey, and Srikanth Saripalli. Target-free extrinsic calibration of a 3d-lidar and an imu. In 2021 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), pages 1–7. IEEE, 2021.