[论文阅读][SLAM]Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation

Targetless Calibration of LiDAR-IMU System Based on Continuous-time Batch Estimation

原文链接: https://arxiv.org/pdf/2007.14759v1.pdf

代码地址https://github.com/APRIL-ZJU/lidar_IMU_calib

Abstract

这篇文章提出了一种LiDAR-IMU标定方法(称为LI-Calib). LI-Calib 采用了基于B-Spline的连续时间轨迹。相比于基于离散时间的方法，这种方法对于IMU和LiDAR这种高采样率的传感器更合适。此外， LI-Calib decomposes the space into cells and identifies the planar segments for data association，这使得标定问题在没有任何人工target的场景下仍然是well-constrained的（？？？？？这说了个啥）。在仿真和真实数据上做了验证，结果显示该方法精度高，可重复性好。

Introduction

LiDAR这种传感器由于其在精度和鲁棒性方面的优良性质，被广泛应用于定位、建图、跟踪、检测等任务。但其缺点在于，LiDAR对连续的3D点在不同时间点进行采样，因此传感器的运动会引入畸变，比如rolling-shutter effect. IMU可以作为辅助传感器解决这个问题。

对于LiDAR，每个点都是在不同时刻采集的，每个点的精确位置同采集时刻LiDAR的位姿是有关系的; 因此做Pose Estimation对于移除运动畸变是有益的; 基于离散时间的方法[6]使用的是关键帧的位姿插值，牺牲了高动态场景下的精度。Gentil等[7]引入了高斯过程来做IMU的插值，部分的解决了使用low-frequency IMU readings为higher-frequency LiDAR points做插值的矛盾。Rehder等[8]提出了基于continuous-time batch estimation，用于标定camera-IMU-LiDAR system的两阶段方法。先使用棋盘格标定camera-IMU，然后单线激光雷达再同camera-IMU 系统做标定。

受[8]的启发，本文提出了基于continuous-time framework的方法，该法可以看作[9]的工作的延伸。通过使用temporal basis fuctions对轨迹参数化，基于continuous-time的方法可以得到轨迹上任何时间点的精确姿态，这对于高帧率测量系统的标定问题是很有帮助的。同时由于角速度和线加速度是temporal basis fuctions的导数，continuous-time formulation 将IMU的测量以一种自然的方式整合了进来（？？？？？）

总结下本文的贡献有三个：

1. 提出了一种精确的、可重复性高的LiDAR-IMU标定系统，无需其他sensor或人工设计的target的参与
2. 基于continuous-time trajectory， 提出了LiDAR-IMU标定问题的一种新的formulation，在continuous-time batch optimization的过程中，IMU原始数据的误差和LiDAR的point-to-surfel distances（雷达的某种误差？？？？？）被联合优化，这使得标定问题在没有任何人工target的场景下仍然是well-constrained的
3. 在仿真和真实数据上做了验证，结果显示该方法精度高，可重复性好。代码开源（作者声称这是全世界首个开源LiDAR-IMU标定工具包）

Related Work

Motion compensation（运动补偿）和 data association（指点云配准吗？？？？？） 是LiDAR-IMU标定的两个主要难题。

针对运动补偿问题，大多数方法[6,10] 对每个LiDAR 点采用线性插值，其前提假设是在短时间内，角速度和线速度或线加速度是一个常量。这个假设对于低速和平滑运动是成立的。但是对于任意运动（比如手持sensor或将sensor置于四旋翼无人机）则不成立。为解决该问题，Neuhaus[11]使用IMU数据来预测LiDAR的运动。本文和最近的一些工作使用continuous-time representations 来参数化轨迹，好处是无需任何关于运动的先验，即可解决高帧率的运动畸变问题。

LiDAR Points的Data association 是另一个重要问题，[7] [14]分别使用了point-to-point和point-to-plane的方法。[15]则使用了一种point-to-model的策略，考虑了局部邻域的形状信息。本文采用了point-to-surfel的方法，点云被离散化，并用surfels（面元，关于面元与面元地图的代表性工作见SUMA )表示;points和对应的surfels对齐。

基于temporal basis functions的continuous-time batch optimization在标定问题上的应用已被广泛研究。Furgale[18]基于B样条基函数，给出了SLAM问题的证明和实现；并将所提出的框架用于解决Camera-IMU标定问题，该框架对temporal和spatial calibration（？？？？？啥区别）都支持。Rehder在[8]中使用了一个类似的框架来标定camera-IMU和LiDAR的外参，在[20]中，拓展了[18]中的工作以支持多IMU标定。本文提出了一种基于continuous-time batch optimization的LiDAR-IMU标定方法，和[8]的工作在精神上相通，但不需要棋盘格和辅助传感器。

Trajectory Representation AND Notation

符号定义：

• IMU Frame： { I } \{I\} {I}
• LiDAR Frame: { L } \{L\} {L}
• Map Frame: { M } \{M\} {M}​ located at the first LiDAR scan { L 0 } \{L_0\} {L0​} at the beginning of calibration
• ${}_L^I\mathbf{q},{}^I{\mathbf{p}}_L$ 定义了从LiDAR到IMU的外参，${}_L^I\mathbf{R}\in SO(3)$ 是与${}_L^I\mathbf{q}$对应的 旋转矩阵

使用B-Spline 来做轨迹的参数化，优点是：

1. 可计算其封闭形式的导数，这可以简化高采样信息融合（？？？？？怎么简化）
2. B-Spline修改一个控制点，仅会影响其附近的曲线段（ 详见 Bezier和B样条曲线)， 这意味着我们所需要的控制点数目是有限的

使用B-Spline 来做轨迹参数化的方式有多种。可以直接在$SE(3)$中做，也可以分别在 $R_3$和$SO(3)$中做。直接在$SE(3)$中做，优点是 torque-minimal(???), 但是translation curve的形状很难控制，因为translation和rotation耦合在一起了。而且该形式对于本文的标定方法来说很难求解，因此最终选择了分别参数化的方法。

接下来介绍了B-Spline的矩阵表示形式。本文采用节点均匀分布的三次B-Spline ，并且是cumulative representation of B-spline，用来参数化translation和rotation。（可参考Spline fusion）

translation：
$$\mathbf{p}(t)=\sum _{j=0}^d{\mathbf{u}}^{\top }{\mathbf{M}}_{(j)}^{(d+1)}{\mathbf{p}}_{i+j}(1)$$

rotation(详见A General Construction Scheme for Unit Quaternion Curves with Simple High Order Derivatives)：
$$\mathbf{p}(t)=\sum _{j=0}^d{\mathbf{u}}^{\top }{\mathbf{M}}_{(j)}^{(d+1)}{\mathbf{p}}_{i+j}\mathbf{q}(t)={\mathbf{q}}_i\otimes \prod _{j=1}^d\exp \left({\mathbf{u}}^{\top }{\tilde{\mathbf{M}}}_{(j)}^{(4)}\log \left({\mathbf{q}}_{i+j-1}^{-1}\otimes {\mathbf{q}}_{i+j}\right)\right)$$
轨迹对时间$t$求导，可得线加速度和角加速度。（？？？？？咋求的要看下）

Methodology

Step1:外参Roation的初始化

感觉就是一个手眼标定的过程，最后求解一个$AX=XB$形式的问题。

1. IMU的姿态不是通过积分得到的（缺点是不准确，而且受到IMU的噪声和漂移影响），而是通过上文提到的B-Spline Fitting得到的，具体是求解下面这个最小二乘问题(要细看???):
   $${\mathbf{q}}_0,\cdots ,{\mathbf{q}}_N=\arg \min \sum _{k=0}^M\Vert {}^{I_k}{\mathbf{\omega }}_m-{}_I^{I_0}{\mathbf{R}}^{\top }{\left(t_k\right)}_I^{I_0}\dot{\mathbf{R}}\left(t_k\right)\Vert$$

2. $[t_k,t_{k+1}]$时间段内的IMU rotation可通过 上步的B-Spline得到，LiDAD的rotation可以通过NDT得到。任意$k$时刻两个sensor之间的rotation需满足以下关系：
   $$\mathbf{q}{\otimes }_L^I\mathbf{q}={}_L^I\mathbf{q}\otimes {}_{L_{k+1}}^{L_k}\mathbf{q}$$
   将不同时刻的方程Stack起来求解即可。（这里方程加了个权重要细看？？？？？）

3. 作者专门解释了下为什么没有做translation的初始化：

   • 加速度与重力耦合，并与方向相关。因此，rotation B-Spline 会影响translation B-Spline的精度
   • B-Spline的二阶导数在式$(1)$的$u$向量中引入了两个零元素，使得控制点无法求解 (???以上两点没看懂)
   • 使用IMU的raw data 来计算B-Spline是不准确的

   虽然没有做translation的初始化，从实验结果来看这对Calibration的结果影响很小。

Step2：Surfels Map and Data Association

使用Step1估计出的Rotation，可以为Lidar帧间配准提供Rotation的初值，还可以移除Rotation引起的畸变，因此可以提升lidar odometry和lidar point cloud map的质量。将lidar point cloud map的点云离散化在3D cells中，每个cells内的点计算一阶和二阶矩（？？？？？就是重心和协方差矩阵吧）。然后使用了一个指标来找Surfels[32]：
$$P=2\frac{{\lambda }_1-{\lambda }_0}{{\lambda }_0+{\lambda }_1+{\lambda }_2},$$
${\lambda }_0,{\lambda }_1,{\lambda }_2$是协方差矩阵的特征值，从小到大排列。当cell中的点接近一个平面的时候，$P$接近1（${\lambda }_0$接近0, 另两个接近相等）。如果一个cell中的$P$值大于某个阈值，则使用RANSAC拟合平面，并用该平面到原点的距离和法向表示该平面 $\pi ={\left[{\mathbf{n}}^{\top },d\right]}^{\top }$。

所有的平面和Lidar point cloud map都是在$L_0$ 系的.

使用tiny planes， 而不是环境中的大平面，好处是可以使用环境中所有的平面状区域提供的信息，为batch optimization 提供了更可靠的约束（？？？？？）。为了鲁棒性，距离超出阈值的point-to-plane correspondences 被拒绝（？？？？？有提到这些correspondences的作用吗）。

点云使用了random sample作降采样。

Step3：Continuous-time Batch Optimization

总结下，需要估计的系统状态为：

$\mathbf{x}={\left[{}_L^I{\mathbf{q}}^{\top },{}^I{\mathbf{p}}_L^{\top },{\mathbf{x}}_q^{\top },{\mathbf{x}}_p^{\top },{\mathbf{b}}_a^{\top },{\mathbf{b}}_a^{\top }\right]}^{\top }$

其中${\mathbf{x}}_q={\left[{\mathbf{q}}_0^{\top },\cdots ,{\mathbf{q}}_N^{\top }\right]}^{\top }$ and ${\mathbf{x}}_p={\left[{\mathbf{p}}_0^{\top },\cdots ,{\mathbf{p}}_N^{\top }\right]}^{\top }$为Trajectory Representation 一节中提到的continuous-time trajectory的rotation和translation的控制点。

问题可以定义为：

有以下测量：

sequence of

1. associated LiDAR points $\mathcal{L}$
2. linear acceleration measurements $\mathcal{A}$
3. angular velocity measurements $\mathcal{W}$

以上测量的噪声项都是独立的高斯白噪声。

求极大似然估计$p(\mathbf{x}\mid \mathcal{L},\mathcal{A},\mathcal{W})$

可被以下最小二乘问题近似：
$$\hat{\mathbf{x}}=\arg \min \left\{\sum _{k\in \mathcal{A}}{\Vert {\mathbf{r}}_a^k\Vert }_{{\Sigma }_a}^2+\sum _{k\in \mathcal{W}}{\Vert {\mathbf{r}}_{\omega }^k\Vert }_{{\Sigma }_{\omega }}^2+\sum _{j\in \mathcal{L}}{\Vert {\mathbf{r}}_{\mathcal{L}}^j\Vert }_{{\Sigma }_{\mathcal{L}}}^2\right\}$$
其中，IMU残差定义：
$${\mathbf{r}}_a^k={}^{I_k}{\mathbf{a}}_m-{}^I\mathbf{a}\left(t_k\right)-{\mathbf{b}}_a{\mathbf{r}}_{\omega }^k={}^{I_k}{\mathbf{\omega }}_m-{}^I\mathbf{\omega }\left(t_k\right)-{\mathbf{b}}_g$$
激光雷达的残差计算方式，大概就是把每个点转到$L_0$ 系，然后计算和Step2中的面元的距离。类似一个point2plane error。

Step4: Refinement

Batch optimization之后，使用$\mathbf{x}$的最佳估计再做一遍去畸变，rebuild lidar sufels map和point-to-surfel data associations。注意：NDT只在第一次迭代中做。实验中大概4次迭代可以得到一个精度较高的标定结果。

Experienments

simulated和real world data 都被拿来做实验。Continuous-time batch optimization 是使用自己写的Kontiki 库来做的。

Simulation

模拟生成10组数据，（均为约10s的数据），加入高斯噪声（特性与真实传感器噪声特性相同）。结果是：translational errors： 0.0043 ± 0.0006m， orientational errors ： 0.0224 ± 0.0026度。

另外，在vicon room 中测量了trajectory的精确度。

Real-world

室内和室外场景，每个场景5组数据（25-30s，角速度47.39度 /s，加速度0.628$m/s^2$）。外参真值由CAD中的测量得到。

对数据的要求：所有的场景要保证足够的线加速度和角加速度， knot spacing
of the spline 是0.02s。

作者使用图表展示了以下内容：

• 针对不同数据，结果的一致性
• BSpline trajectory error
• 多次迭代结果的收敛性

一些结论：

• 在室内场景，平面区域更多，结果收敛性的更好；一致性也更好（误差在mm，零点几度这个量级）。
• 方法当前的局限在于初次迭代时使用了NDT，NDT结果如果很差，会无法得到enough reliable point-to-surfel correspondences,标定结果也会很差。希望未来可以有更鲁棒的lidar 前端。