1 引言（Introduction）

随着自动驾驶技术的迅速发展，多传感器融合方法成为了一大趋势。激光雷达可以实时捕捉环境中的3D信息，具有较高的精度和较广的视野范围，是目前自动驾驶技术的主流传感器之一。然而由于通常无法避免激光雷达在扫描时产生的运动，因此其获取的点云数据往往会存在一定的畸变。尤其是在高速运动的场景下，严重的运动畸变会在很大程度上影响基于点云的定位或感知算法的结果。针对这一问题，IMU（惯性测量单元）可以起到很好的补充作用。较为常见的6轴IMU，可以高频地获取自身的3轴加速度信息和3轴角速度信息，从而对激光雷达扫描期间的运动进行估计，进而实现对点云的去畸变处理。另外，在实际的复杂场景下，动态物体的出现以及结构特征的缺失，都会影响激光雷达SLAM（同步定位与地图构建）的定位精度；而IMU在不同场景中具有较为稳定的定位结果。目前的定位方法正朝着多传感器融合的方向发展，已有一些优秀的LiDAR-IMU紧耦合的SLAM方法^[1-2]，取得了较为鲁棒的定位结果。

作为多传感器融合技术最基础的模块，标定方法往往会在很大程度上影响最终的性能，因此如何提高标定方法的精度和鲁棒性，成为多传感器融合的研究热点。目前，激光雷达与相机间的标定方法^[3-6]、以及相机与IMU间的标定算法^[7-9]已较为成熟，而关于激光雷达与IMU之间标定算法的研究工作^[10-14]相对较少。其中，文[10-11]中的方法需要基于高精度GPS信息对IMU姿态进行估计，才能实现对激光雷达和IMU之间的外部参数标定。而文[12]中的方法则是需要将相机作为辅助传感器来实现对激光雷达和IMU的间接标定，即先基于棋盘对相机和IMU进行外部参数标定，再基于相机－IMU系统对单光束激光雷达进行校准。因此，文[10-12]中的方法都存在一定局限性。

目前直接对激光雷达和IMU进行联合标定的算法，是通过优化模型对激光雷达和IMU数据进行紧耦合融合实现的。2018年，Le Gentil等^[13]提出了一种可行的LiDAR-IMU联合标定方法，该方法参考了基于优化的紧耦合VIO（视觉惯性里程计）的思路，构建了激光雷达约束因子和IMU约束因子，并基于因子图进行联合优化。其中，由于激光雷达约束因子是通过计算点到平面结构的距离来获得的，因此该标定算法仅适用于室内墙角这类具有3个平面的区域。针对高速运动下激光雷达点云失真的问题，该算法基于高斯过程回归（GPR）方法对IMU数据进行上采样，再通过预积分获得高频的位姿估计结果，基于该结果对单帧点云中每个点进行去畸变处理。2020年，Lü 等^[14]开源了第一款LiDAR-IMU联合标定算法LI-Calib。针对IMU离散数据，该方法基于B样条（B-spline）方法，实现了连续的位姿估计。并构建了激光雷达点面距离残差和IMU残差，基于Ceres优化库实现了联合优化。不过，该方案对于标定数据的采集环境有较高要求，需要在具有大块完整的平面结构的区域中才能达到较为理想的标定结果。在日常的室内复杂环境，或是室外道路环境中，通常存在较多的遮挡，且结构特征较不明显，往往不具有大块完整的平面结构，因此标定精度受到一定影响。

针对该问题，本文提出了一种鲁棒的LiDAR- IMU联合标定方法。主要创新点如下：

(1) 将线特征^[15]的思想引入到标定任务中，通过同时构建线特征匹配对和面片匹配对，提高了算法对复杂环境的自适应特征提取能力，有效提升了标定精度和标定算法对复杂环境的适应能力。

(2) 考虑到在迭代优化前期参数估计精度不高，对标定算法的优化过程提出了双阶段的迭代优化方式，选择在优化达到一定精度后再加入线特征匹配对，使得迭代优化能进一步收敛。同时，考虑到随着迭代优化的逐步收敛，匹配对的几何残差会大幅度地减小，进而在优化问题中该类残差的参与度（贡献）占比会迅速减小，较容易陷入局部最优解，进一步根据每轮迭代优化的几何残差设计了自适应损失权重，使得迭代优化流程能更好地收敛，从而提高标定算法的精度。

本文中坐标系定义如下：IMU坐标系为$ \{I \} $，激光雷达坐标系为$ \{L \} $，将$ t_{0} $时刻的激光雷达坐标系作为地图坐标系$ \{M \} $。另外，将激光雷达与IMU之间的外部参数矩阵命名为$ {}_{L}^{I}\mathit{\boldsymbol{T}} $，其中旋转矩阵为$ {}_{L}^{I}\mathit{\boldsymbol{R}} $，对应的四元数为$ {}_{L}^{I}\mathit{\boldsymbol{q}} $，平移向量为$ {}_{L}^{I}\mathit{\boldsymbol{t}} $。

2 算法流程（Process of the algorithm） 2.1 算法概述

本文提出的LiDAR-IMU联合标定方法主要包含3个阶段，初始化阶段、匹配构建阶段和迭代优化阶段，具体如图 1所示。

首先，初始化阶段是将IMU的旋转姿态与激光雷达的旋转姿态进行对齐，从而初步估计激光雷达与IMU之间的旋转矩阵。而匹配构建阶段则是提取点云特征，并融合激光雷达和IMU数据构建线特征地图和面片地图，进而构建对应的匹配关系。最后，迭代优化阶段则是利用构建的匹配对来优化雷达与IMU之间的外部参数，以及IMU内加速度计和陀螺仪的内部参数，并通过更新地图和匹配对，来进一步迭代优化，最终得到收敛的标定结果。

2.2 初始化阶段

初始化阶段主要参考了文[14]中的工作，该方案将IMU的位姿估计分解成了旋转估计$ \mathit{\boldsymbol{p}}(t) \in $ $ SO(3) $和平移估计$ \mathit{\boldsymbol{q}}(t) \in\mathbb{R}^{3} $两个部分，再分别用2个样条曲线对旋转和平移估计进行拟合：

$ \begin{align} \mathit{\boldsymbol{p}}(t) & = \mathit{\boldsymbol{p}}_{i} + \sum\limits_{j = 1}^{3} \mathit{\boldsymbol{u}}^{\rm T}\mathit{\boldsymbol{M}}_{(j)}^{(4)}(\mathit{\boldsymbol{p}}_{i + j} - \mathit{\boldsymbol{p}}_{i + j - 1}) \end{align} $

(1)

$ \begin{align} \mathit{\boldsymbol{q}}(t) & = \mathit{\boldsymbol{q}}_{i} \otimes \prod\limits_{j = 1}^{3} \exp \left(\mathit{\boldsymbol{u}}^{\rm T}\mathit{\boldsymbol{M}}_{(j)}^{(4)} \ln(\mathit{\boldsymbol{q}}_{i + j - 1}^{- 1} \otimes \mathit{\boldsymbol{q}}_{i + j})\right) \end{align} $

(2)

式中，$ \mathit{\boldsymbol{p}}(t) $和$ \mathit{\boldsymbol{q}}(t) $分别为平移曲线和旋转曲线，其在时间$ t \in [\, t_{i}, \; t_{i + 1}) $的值分别由时刻$ t_{i}, t_{i + 1}, \cdots, t_{i + 3} $处的平移控制点$ {p}_{i}, {p}_{i + 1}, \cdots, {p}_{i + 3} $和旋转控制点$ {q}_{i}, $ $ {q}_{i + 1}, \cdots, {q}_{i + 3} $控制，$ \otimes $为四元数的乘法，$ {(\cdot)}^{- 1} $为四元数的逆，$ \exp(\cdot) $为将李代数映射到$ S^{3} $空间的运算，$ \ln(\cdot) $为$ \exp(\cdot) $的逆运算。节点向量$ \mathit{\boldsymbol{u}}^{\rm T} = [1 \; \; u\; \; \cdots $ $ u^{3} ] $，其中$ u = (t - t_{i})/(t_{i + 1} - t_{i}) $，$ \mathit{\boldsymbol{M}}_{(j)}^{(4)} $为样条矩阵$ \mathit{\boldsymbol{M}}^{(4)} $的第$ j $列，其中样条矩阵$ \mathit{\boldsymbol{M}}^{(4)} $具体为

$ \begin{align} \mathit{\boldsymbol{M}}^{(4)} = \frac{1}{6} \begin{bmatrix} 6 & 5 & 1 & 0 \\ 0 & 3 & 3 & 0 \\ 0 & -3 & 3 & 0 \\ 0 & 1 & -2 & 1 \end{bmatrix} \end{align} $

(3)

通过对平移曲线和旋转曲线求导，可以很方便地得到加速度曲线和角速度曲线，从而和IMU数据中的3轴加速度和3轴角速度构建联系，具体公式如下：

$ \begin{align} {}^{I}\mathit{\boldsymbol{a}}(t) & ={}_{I}^{I_{0}} \mathit{\boldsymbol{R}}^{\rm T} (t) \left({}_{I}^{I_{0}} \ddot{\mathit{\boldsymbol{p}}} (t) -{}^{I_{0}}\mathit{\boldsymbol{g}}\right) \end{align} $

(4)

$ \begin{align} {}^{I}\mathit{\boldsymbol{\omega}}(t) & ={}_{I}^{I_{0}}\mathit{\boldsymbol{R}}^{\rm T}(t)_{I}^{I_{0}}\dot{\mathit{\boldsymbol{R}}}(t) \end{align} $

(5)

式中，$ {}^{I}\mathit{\boldsymbol{a}}(t) $和$ {}^{I}\mathit{\boldsymbol{\omega}}(t) $分别为$ t $时刻传感器平台在IMU当前坐标系下的加速度和角速度，$ I_{0} $表示$ t_{0} $时刻的IMU坐标系$ \{I_0\} $，平移曲线和旋转曲线都是以此作为参考系。可基于四元数转旋转矩阵，由旋转曲线$ {}_{I}^{I_{0}}\mathit{\boldsymbol{q}}(t) $转换获得$ {}_{I}^{I_{0}}\mathit{\boldsymbol{R}}(t) $，$ {}_{I}^{I_{0}}\ddot{\mathit{\boldsymbol{p}}}(t) $表示对平移曲线求2阶导数，$ {}_{I}^{I_{0}}\dot{\mathit{\boldsymbol{R}}}(t) $表示对旋转曲线求1阶导数，$ {}^{I_{0}}\mathit{\boldsymbol{g}} $表示重力加速度在$ t_{0} $时刻IMU坐标系下的各轴分量。利用IMU的原始数据，可以对平移曲线和旋转曲线进行拟合。

对激光雷达的轨迹估计可以基于激光SLAM实现，将原始点云与局部地图配准，估计每一帧点云相对于$ t_{0} $时刻的位姿增量。其中，点云匹配算法选择的是NDT（normal distribution transform）算法^[16-17]。

将基于B样条曲线获得的IMU帧间旋转增量和基于激光雷达SLAM获得的激光雷达帧间旋转增量进行逐帧对齐，从而求解出激光雷达与IMU之间的旋转外部参数：

$ \begin{align} {}_{I_{k + 1}}^{\kern 8pt I_{k}}\mathit{\boldsymbol{q}} \otimes{}_{L}^{I}\mathit{\boldsymbol{q}} ={}_{L}^{I}\mathit{\boldsymbol{q}} \otimes{}_{L_{k + 1}}^{\kern 8pt L_{k}}\mathit{\boldsymbol{q}} \end{align} $

(6)

式中，$ {}_{I_{k + 1}}^{\kern 8pt I_{k}}\mathit{\boldsymbol{q}} $为IMU的帧间旋转增量，可以根据激光SLAM位姿估计结果求得，$ {}_{L_{k + 1}}^{\kern 8pt L_{k}}\mathit{\boldsymbol{q}} $为激光雷达的帧间旋转增量，可以根据旋转曲线求得。可以将所有帧的帧间旋转增量等式整合在一起，估计激光雷达和IMU之间的旋转外部参数$ {}_{L}^{I}\mathit{\boldsymbol{q}} $。

2.3 匹配构建阶段

匹配构建阶段大致可以分为以下3个环节：点云去畸变与线特征提取、地图构建、匹配构建。相比文[14]，本文主要的改进在于引入了线特征，从复杂环境中提取出直线结构，特征定位更加精细，而且不易受遮挡等情况的影响。同时，构建了点线匹配对，来强化标定算法的几何约束，以达到更高的精度和更好的鲁棒性。下面展开描述匹配构建阶段的3个环节。

2.3.1 点云去畸变与线特征提取

基于初始化阶段估计的IMU旋转曲线和激光雷达与IMU之间的旋转外部参数，可以预测激光雷达实时的旋转，估计单帧点云中每个点相对于帧头时刻的旋转增量：

$ \begin{align} \Delta\mathit{\boldsymbol{q}}_{k}^{i} ={}_{L}^{I}\mathit{\boldsymbol{q}}^{- 1} \otimes {}_{I}^{I_{0}}\mathit{\boldsymbol{q}}^{- 1} (t_{k}^{0}) \otimes {}_{I}^{I_{0}}\mathit{\boldsymbol{q}} (t_{k}^{i}) \otimes {}_{L}^{I}\mathit{\boldsymbol{q}} \end{align} $

(7)

式中，$ \Delta\mathit{\boldsymbol{q}}_{k}^{i} $表示第$ k $帧点云中第$ i $个点相对于帧头时刻的旋转增量，$ t_{k}^{0} $表示第$ k $帧的帧头时刻，$ t_{k}^{i} $表示第$ k $帧中扫描到第$ i $个点时对应的时刻。通过对上述每个点的旋转增量进行补偿，实现了对单帧原始点云逐点进行旋转去畸变的处理。

而线特征提取模块的主要功能，在于从去畸变后的单帧点云中提取出平面交线，即线特征点。首先，对于去畸变后的点云，计算每个点的曲率值：

$ \begin{align} c_{i} = \frac{1}{2n}\sum\limits_{j \in S} | d_{i} - d_{j} | \end{align} $

(8)

式中，$ c_{i} $表示某一扫描线中第$ i $个点的曲率，$ S $为第$ i $个点左右各$ n $个点组成的集合，$ d_{i} $为第$ i $个点与雷达中心的距离。通常，分布在平面交线结构上的点云具有较大的曲率，因此基于曲率值，可以对逐行扫描线中的点进行排序，将其中曲率值较大的点作为线特征点进行存储。

2.3.2 地图构建

地图构建的主要功能在于基于激光雷达SLAM构建线特征地图和面片地图。该模块首先需要基于NDT匹配算法对去畸变后的点云进行逐帧匹配，从而得到激光雷达的位姿$ {}_{L_{k}}^{L_{0}}\mathit{\boldsymbol{T}} $和基于所有帧构建的点云地图$ M_{\text{full}} $。接着，基于激光雷达的位姿估计结果，可以将每帧线特征点投影到地图坐标系（$ t_{0} $时刻激光雷达的坐标系）下，得到线特征地图$ M_{\text{line}} $。最后，对点云地图$ M_{\text{full}} $进行栅格化处理，计算每个栅格内的点云的协方差矩阵，并进行特征值分解。通过特征值，可以计算平面概率$ \mathcal{P} $，来判断该栅格内点云是否属于同一个平面。

$ \begin{align} \mathcal{P} = 2\frac{\lambda_{1} - \lambda_{0}}{\lambda_{0} + \lambda_{1} + \lambda_{2}} \end{align} $

(9)

式中，$ \lambda_{0} \leqslant \lambda_{1} \leqslant \lambda_{2} $，求得的平面概率$ \mathcal{P} $越接近1，代表该栅格内的点云越接近平面的结构，可以基于RANSAC（随机抽样一致性）方法实现平面拟合，从而构建出面片地图$ M_{\text{surfel}} $。

图 2展示了办公室场景地图的可视化效果，为了更加直观，展示的是标定算法最终收敛后构建的地图。其中，从上往下分别是点云地图$ M_{\text{full}} $、线特征地图$ M_{\text{line}} $和面片地图$ M_{\text{surfel}} $，点云地图和线特征地图中的颜色代表雷达反射强度值（蓝色代表强度值较低的区域，绿色代表强度值较高的区域），面片地图中的颜色用于区分不同的面片。

2.3.3 匹配构建

匹配构建模块的主要功能在于，基于最新的标定参数，将每一帧点云投影到地图坐标系下，并基于最近邻方法查找，将投影后的点云与线特征地图和面片地图分别进行匹配，构建点线匹配对和点面匹配对。

其中，将单帧点云投影到地图坐标系的步骤如下：

(1) 基于激光雷达与IMU之间的外部参数矩阵，将当前帧激光雷达坐标系下的点云投影到IMU坐标系下。

(2) 基于拟合的IMU旋转曲线和平移曲线，将当前帧IMU坐标系下的点云投影到$ t_{0} $时刻IMU坐标系下。

(3) 基于激光雷达与IMU之间的外部参数矩阵，将$ t_{0} $时刻IMU坐标系下的点云投影到$ t_{0} $时刻激光雷达坐标系下，此时的点云和地图处在同一坐标系中。

上述点云投影的过程可以表示为

$ \begin{align} {}^{L_{0}}\mathit{\boldsymbol{p}}_{i} ={}_{L}^{I}\mathit{\boldsymbol{R}}^{\rm T} \left({}_{I_{j}}^{I_{0}}\mathit{\boldsymbol{R}} \left({}_{L}^{I}\mathit{\boldsymbol{R}}{}^{L_{j}}\mathit{\boldsymbol{p}}_{i} +{}_{L}^{I}\mathit{\boldsymbol{t}}\right) +{}_{I_{j}}^{I_{0}}\mathit{\boldsymbol{t}}\right) -_{L}^{I}\mathit{\boldsymbol{t}} \end{align} $

(10)

式中，$ {}^{L_{j}}\mathit{\boldsymbol{p}}_{i} $表示第$ j $帧激光雷达坐标系下点云中第$ i $个点的位置，而$ {}^{L_{0}}\mathit{\boldsymbol{p}}_{i} $则表示该点投影到了$ t_{0} $时刻的激光雷达坐标系下的位置。

构建点线匹配对的方法用到了最近邻算法的思想：遍历所有的线特征点，在基于KD（$ K $维）树的在线特征地图中找到与之最近的5个点，若该5个点满足共线条件，且线特征点与该直线之间的距离小于预设阈值，则将之作为一对点线匹配对。

构建点面匹配对的方法同样用到了最近邻算法的思想：基于与面片地图相同的栅格对投影后的单帧点云进行分块处理，遍历所有具有平面结构的栅格，将栅格内的点与面片进行距离判定，若小于预设阈值，则将之作为一对点面匹配对。

2.4 迭代优化阶段

迭代优化阶段是整个标定算法的最重要的一个环节，主要思想是通过逐轮的匹配构建和优化求解，经过迭代收敛，得到最终的精确解。本文在该阶段的主要创新在于：通过引入线特征约束来完善优化函数，并以双阶段的方式来实现迭代优化，同时增加了与每轮几何残差收敛程度有关的动态权重，以达到更好的收敛效果。

2.4.1 优化函数

迭代优化阶段，待估计的参数包括了激光雷达与IMU之间的旋转矩阵和平移向量、IMU的旋转曲线和平移曲线、IMU内部加速度计和陀螺仪的偏移量、以及激光雷达与IMU之间的时延：

$ \begin{align} \mathit{\boldsymbol{x}} = [{}_{L}^{I}\mathit{\boldsymbol{q}}^{\rm T}, \; {}_{L}^{I}\mathit{\boldsymbol{t}}^{\rm T}, \; \mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{q}}}}^{\rm T}, \; \mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{t}}}}^{\rm T}, \; \mathit{\boldsymbol{b}}_{a}^{\rm T}, \; \mathit{\boldsymbol{b}}_{\omega}^{\rm T}, \; \delta t]^{\rm T} \end{align} $

(11)

式中，$ \mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{q}}}} = [\mathit{\boldsymbol{q}}_{0}^{\rm T}, \cdots, \mathit{\boldsymbol{q}}_{N}^{\rm T}]^{\rm T} $，$ \mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{t}}}} = [\mathit{\boldsymbol{t}}_{0}^{\rm T}, \cdots, \mathit{\boldsymbol{t}}_{N}^{\rm T}]^{\rm T} $分别为IMU旋转曲线和平移曲线的控制点，$ \mathit{\boldsymbol{b}}_{a}^{\rm T} $和$ \mathit{\boldsymbol{b}}_{\omega}^{\rm T} $则分别为IMU内部加速度计和陀螺仪的偏移量，$ \delta t $表示激光雷达与IMU之间的时延。

迭代优化阶段的优化函数包括激光雷达约束和IMU约束2个部分。其中，激光雷达约束包括了点线匹配约束$ \mathcal{L}_{L} $和点面匹配约束$ \mathcal{L}_{\rm s} $两个部分，而IMU约束也包括了角速度曲线约束$ \mathcal{W} $和加速度曲线约束$ \mathcal{A} $两个部分。本阶段的优化问题可以用如下最小二乘问题来表示：

$ \begin{align} \hat{\mathit{\boldsymbol{x}}} =\, \mathop{\arg\min}\limits _{{\mathit{\boldsymbol{x}}}} \Big\{ & \sum\limits_{k \in \mathcal{W}} \| \mathit{\boldsymbol{r}}_{\omega}^{k} \| ^{2} + \sum\limits_{k \in \mathcal{A}} \| \mathit{\boldsymbol{r}}_{a}^{k} \| ^{2} + \\ & \sum\limits_{j_{L} \in \mathcal{L}_{L}} \| \mathit{\boldsymbol{r}}_{\mathcal{L}_{L}}^{j_{L}} \| ^{2} + \sum\limits_{j_{\rm s} \in \mathcal{L}_{\rm s}} \| \mathit{\boldsymbol{r}}_{\mathcal{L}_{\rm s}}^{j_{\rm s}} \| ^{2}\Big\} \end{align} $

(12)

式中，$ \mathit{\boldsymbol{r}}_{\omega}^{k} $和$ \mathit{\boldsymbol{r}}_{a}^{k} $分别是与IMU角速度曲线和加速度曲线相关的残差，而$ \mathit{\boldsymbol{r}}_{\mathcal{L}_{L}}^{j_{L}} $和$ \mathit{\boldsymbol{r}}_{\mathcal{L}_{\rm s}}^{j_{\rm s}} $则分别是点线匹配对和点面匹配对的距离残差。其中，与IMU角速度曲线和加速度曲线相关的残差可通过式(13)(14) 计算：

$ \begin{align} \mathit{\boldsymbol{r}}_{a}^{k} & ={}^{I_{k}}\mathit{\boldsymbol{a}}_{\rm m} -{}^{I}\mathit{\boldsymbol{a}}(t_{k} + \delta t) - \mathit{\boldsymbol{b}}_{a} \end{align} $

(13)

$ \begin{align} \mathit{\boldsymbol{r}}_{\omega}^{k} & ={}^{I_{k}}\mathit{\boldsymbol{\omega}}_{\rm m} -{}^{I}{\mathit{\pmb{\omega}}}(t_{k} + \delta t) - \mathit{\boldsymbol{b}}_{\omega} \end{align} $

(14)

式中，$ {}^{I_{k}}\mathit{\boldsymbol{a}}_{\rm m} $为IMU内部加速度计的原始数据，$ {}^{I_{k}}\mathit{\boldsymbol{\omega}}_{\rm m} $为IMU内部陀螺仪的原始数据，$ {}^{I}\mathit{\boldsymbol{a}}(t_{k} + \delta t) $和$ {}^{I}{\mathit{\pmb{\omega}}}(t_{k} + \delta t) $分别表示加速度曲线和角速度曲线在$ t_{k} + \delta t $时刻的值，分别由式(4)(5) 获得。

2.4.2 双阶段优化

迭代优化的流程具体分以下4个步骤：

(1) 联合IMU约束和激光雷达约束来构造优化函数，基于最小二乘法求解激光雷达与IMU之间的外部参数和IMU的内部参数、以及IMU的旋转曲线和平移曲线。

(2) 基于最新的IMU旋转曲线、平移曲线和激光雷达与IMU之间的外部参数矩阵，对原始点云同时进行旋转和平移去畸变处理，并基于最新的位姿估计结果，更新线特征地图和面片地图。

(3) 将去畸变后的点云重新投影到地图坐标系，并基于最新的线特征地图和面片地图，重新构建点线匹配对和点面匹配对。

(4) 重复步骤(1)~(3)，迭代优化直至收敛。

其中，激光雷达与IMU之间外部参数和IMU的内部参数都以0作为初始值。在迭代优化的初期，由于构建的地图精度不高，在构建点与直线的匹配对时容易出现错误的匹配关系，导致迭代优化收敛缓慢。因此，本文提出了双阶段的迭代优化方式，2个阶段的区别具体体现在上述步骤(1) 构造的优化函数中。

阶段1：在迭代初期，仅将IMU约束和点面匹配约束加入优化方程，如式(15) 所示。基于该优化方程进行逐轮迭代优化，直至收敛。

$ \begin{align} \hat{\mathit{\boldsymbol{x}}} = \mathop{\arg\min}_{{\mathit{\boldsymbol{x}}}} \Big\{\sum\limits_{k \in \mathcal{W}} \| \mathit{\boldsymbol{r}}_{\omega}^{k} \| ^{2} + \sum\limits_{k \in \mathcal{A}} \| \mathit{\boldsymbol{r}}_{a}^{k} \| ^{2} + \sum\limits_{j_{\rm s} \in \mathcal{L}_{\rm s}} \| \mathit{\boldsymbol{r}}_{\mathcal{L}_{\rm s}}^{j_{\rm s}} \| ^{2} \Big\} \end{align} $

(15)

阶段2：当阶段1收敛后，再将点线匹配约束加入优化方程，即基于式(12) 进行迭代优化，直至收敛得到最终解。

2.4.3 动态权重

通常，在对不同的约束进行联合优化时，需要基于优化权重来调整不同约束的残差值大小，尽可能使得这些残差在同一量级，往往采取的方式是对固定的权重进行手动调整。这种固定的方式在整个迭代优化的过程中会存在一定缺陷。例如，随着迭代过程的收敛，激光雷达几何匹配的距离残差会逐渐变小，若全程基于固定的权重，则会导致激光雷达几何约束在优化问题中相比IMU曲线拟合残差的贡献占比逐渐变小，导致容易陷入局部最优解。因此，本文针对几何约束，设计一种与激光雷达几何残差收敛程度有关的动态权重，来对每一轮优化中IMU和激光雷达残差的比重进行平衡。增加了动态权重后的第$ i $轮的最小二乘问题如下所示（以阶段2的优化方程为例）：

$ \begin{align} \hat{\mathit{\boldsymbol{x}}}^{i} = & \mathop{\arg\min}_{{\mathit{\boldsymbol{x}}}} \Big\{ \alpha_{\omega}^{0}\sum\limits_{k \in \mathcal{W}} \| \mathit{\boldsymbol{r}}_{\omega}^{k} \| ^{2} + \alpha_{a}^{0}\sum\limits_{k \in \mathcal{A}} \| \mathit{\boldsymbol{r}}_{a}^{k} \| ^{2} + \\ & \alpha_{\mathcal{L}_{L}}^{i}\sum\limits_{j_{L} \in \mathcal{L}_{L}} \| \mathit{\boldsymbol{r}}_{\mathcal{L}_{L}}^{j_{L}} \| ^{2} + \alpha_{\mathcal{L}_{\rm s}}^{i}\sum\limits_{j_{\rm s} \in \mathcal{L}_{\rm s}} \| \mathit{\boldsymbol{r}}_{\mathcal{L}_{\rm s}}^{j_{\rm s}} \| ^{2}\Big\} \end{align} $

(16)

式中，$ \alpha_{\omega}^{0} $和$ \alpha_{a}^{0} $为手动设置的保持固定的权重，分别作用于IMU的角速度曲线残差和加速度曲线残差，而$ \alpha_{\mathcal{L}_{L}}^{i} $和$ \alpha_{\mathcal{L}_{\rm s}}^{i} $则为第$ i $轮的动态权重，分别作用于点线匹配残差和点面匹配残差，具体如下：

$ \begin{align} \alpha_{\mathcal{L}_{L}}^{i} & = \alpha_{\mathcal{L}_{L}}^{0} ( {r_{\mathcal{L}_{L}}^{0}}/ {r_{\mathcal{L}_{L}}^{i - 1}}) \end{align} $

(17)

$ \begin{align} \alpha_{\mathcal{L}_{\rm s}}^{i} & = \alpha_{\mathcal{L}_{\rm s}}^{0} ( {r_{\mathcal{L}_{\rm s}}^{0}}/ {r_{\mathcal{L}_{\rm s}}^{i - 1}}) \end{align} $

(18)

式中，$ \alpha_{\mathcal{L}_{L}}^{0} $和$ \alpha_{\mathcal{L}_{\rm s}}^{0} $分别为手动设置的点线匹配和点面匹配的初始权重，$ r_{\mathcal{L}_{L}}^{0} $和$ r_{\mathcal{L}_{\rm s}}^{0} $分别为手动设置的点线匹配和点面匹配残差阈值，$ r_{\mathcal{L}_{L}}^{i - 1} $和$ r_{\mathcal{L}_{\rm s}}^{i - 1} $分别为第$ i -1 $轮优化结束时的点线匹配和点面匹配的平均几何残差。

3 实验分析（Experimental analysis） 3.1 数据集

本文实验所用的数据集主要有以下2个：

(1) 自建数据集

在室内复杂环境下，使用自主搭建的LiDAR-IMU平台采集的数据集。自主搭建的LiDAR-IMU平台如图 3(a)所示，包括了1个Velodyne VLP-16系列激光雷达和1个MEMSICc VG800系列IMU，两个传感器以刚性连接的方式固定在同一平台上。其中，激光雷达的采集频率为10 Hz，IMU的采集频率为100 Hz，为8轴的数据输出，包括3轴加速度计数据、3轴陀螺仪数据，以及2轴重力方向数据，两个传感器之间未作严格的时间同步，同步时间误差也是标定的内容。

数据的采集方式是，通过手持该平台进行2个轴以上的旋转，采集约1 min的数据，以rosbag的形式存储下来，用于离线的标定，总共在同一室内复杂环境中采集了10个序列。数据采集环境如图 3(b)所示，为复杂的办公室场景。该场景内物体之间遮挡较为严重，平面结构较为碎片化，对于LiDAR-IMU联合标定任务来说具有一定难度。

(2) 开源数据集

该数据集为LI-Calib^[14]项目开源的室外场景数据。采集平台包括了1个Velodyne VLP-16系列激光雷达、3个Xsens系列IMU和一个相机（未使用）。其中，激光雷达的采集频率为10 Hz，IMU的采集频率为400 Hz，为8轴的数据输出，两个传感器之间作了时间同步处理。

该数据集的采集场景为室外的羽毛球场区域，总共采集了5个序列。该场景较为空旷，周围建筑上的平面结构较不明显，且由于距离较远，点云分布也较为稀疏，因此该场景也存在一定的标定难度。

3.2 评价指标

在评价指标的选择上，目前主要有2种方式：

(1) 标定误差。该方式需要事先估计激光雷达与IMU之间的外部参数真值。文[13]中将相机作为中间传感器，先对IMU和相机进行外部参数标定，并对相机和激光雷达进行外部参数标定，再基于以上结果推导出IMU和激光雷达的外部参数，并将之作为真值。这种获取真值的方式会引入一定误差，较难用于评价标定精度较高的算法。文[14]则是将CAD（计算机辅助设计）装配图中的各个传感器间的相对位姿作为外部参数真值，但由于传感器中心不够明确，因此该方法也存在精度上的缺陷。

(2) 标准差。文[14]中对同一场景多组序列的数据进行标定，统计标定各参数的均值和标准差，并基于标准差来评价算法的可重复性，同时可以在一定程度上反映标定算法的精度。因此，本文选择标准差作为定量实验的评价指标。

3.3 定量实验

实验目的是证明引入线特征思想以及设计与几何残差收敛程度有关的动态权重确实能够提升标定算法的性能。分别在2个数据集上进行定量的分析，评价指标则采用了对多组序列的最终标定结果统计得到的标准差。本实验中选择的基准方法为LI-Calib^[14]，并将添加线特征后的算法命名为$ + $L，将添加线特征和动态权重后的算法命名为$ + $LW。

(1) 自建数据集

表 1统计了上述3种算法在自建数据集上的标定结果的均值和标准差（数据的形式表示成平均值$ \pm $标准差），具体包括了3轴平移外部参数$ x $、$ y $、$ z $和3轴旋转外部参数（翻转角、俯仰角、航向角），加粗字表示某一参数在各版本中最小的标准差。

从表 1中可以发现：在自建的数据集上，LI-Calib标定算法对平移参数的标定标准差约为2 cm，对旋转参数的标定标准差约为0.7$ ^{\circ} $，在部分序列上标定结果的偏差较大。而增加了线特征后，对外部参数的标定性能均有一定提升，其中对旋转参数的标定性能的提升较为明显。具体的原因是，自建数据集上，有几个较难的序列（如之前定性实验所列举的），仅基于LI-Calib算法中的点面匹配对优化，难以正常收敛，而在增加了线特征之后，可以进一步优化收敛，从而使得这几个序列的标定精度得到大幅的提升。在同时增加了线特征和动态权重后，对外部参数的标定结果达到了最优。其中，对平移参数的标定标准差提升了一个数量级，均值达到了约2 mm，对旋转参数的标定标准差达到了约0.04$ ^{\circ} $。

(2) 开源数据集

表 2统计了3种算法在开源数据集上对3个不同IMU的标定结果的均值和标准差，加粗的部分表示同一IMU的某一参数在各版本中最小的标准差。从中可以发现：对于平移参数的标定，LI-Calib标定算法的标准差约为3 mm，在增加了线特征之后，对平移参数的标定性能都能有一定程度的提升，其中，对$ z $轴的标定性能提升得较为明显。这是因为在室外环境中，若仅依靠点面匹配关系，那么通常只有地面这一个平面结构可以对垂直方向上的距离提供较为直观的约束，因此LI-Calib标定方法对$ z $轴平移参数的估计精度较低。而增加线特征则可以从室外环境中提取出一些水平分布的直线（例如建筑的上下沿），从而提高对垂直方向上距离的感知，进一步提高标定算法对$ z $轴平移参数的估计精度。另外，增加线特征和动态权重的版本对平移参数的标定性能达到了最优，标准差约为1 mm。

对于旋转参数的标定，LI-Calib标定算法的标准差约为0.02$ ^{\circ} $，而增加线特征或是增加动态权重对于旋转参数的标定性能的影响都较不明显。原因可能是，该场景中基于点面匹配的标定方法对于旋转参数的标定已经能达到较高的标定精度，而由于室外场景较为空旷，从一些距离较远的直线结构中提取出的线特征较为稀疏，因此基于5个线特征拟合出的直线方向精度较低，进而导致构建的点线匹配约束在旋转上提供的信息较为模糊，对于旋转参数标定精度的影响存在波动。

3.4 定性实验

本实验是对定量实验的补充，实验数据为自建数据集，该数据集的采集环境为办公室场景的室内环境。从图 3中可以看出，该环境内的几何结构较为复杂，且物品较多，雷达数据中平面碎片化较严重。因此针对该环境，主流的仅基于平面残差的LiDAR-IMU联合标定算法难以取得鲁棒的标定结果。而通过引入线特征的思想，对于这类复杂环境，可以提取出更丰富的结构特征，并增加几何匹配约束的数量和质量，从而提高LiDAR-IMU联合标定算法对这类复杂环境的标定精度和鲁棒性。

下面选择了自建数据集上一个最难的序列进行定性实验，该序列的难点主要体现在2个方面：一是在采集数据时，传感器平台处于较低的高度，因此环境遮挡更为严重；二是在采集数据时，传感器的旋转运动更为剧烈，因此单帧点云的运动畸变更为严重。

对标定过程中每一轮的全局点云地图结果进行可视化展示。该全局点云地图，作为标定算法每一轮优化后的中间产出，由于与标定结果中的IMU旋转曲线、平移曲线和激光雷达与IMU之间的标定外部参数直接相关，因此可以定性地反映每一轮标定算法的收敛精度。针对这个最难的序列，总共进行了15轮的迭代优化，每一轮优化后得到的全局地图如图 4所示。其中，第1~5轮为阶段1的迭代优化过程，即此阶段的激光雷达约束只包括了点面匹配约束，第5轮时的全局点云地图为阶段1迭代优化的收敛结果。而第6~15轮为阶段2的迭代优化过程，即此阶段的激光雷达约束在点面匹配约束的基础上，增加了点线匹配约束。可以发现添加了点线约束后，标定算法可以进一步收敛，最终在第15轮时，阶段2的迭代优化达到了收敛。比较第5轮和第15轮的全局点云地图结果，可以很明显地发现，引入线特征的思想可以很有效地提高LiDAR-IMU联合标定算法的标定精度和鲁棒性。

4 结论与展望（Conclusion and prospect）

针对目前主流的LiDAR-IMU联合标定方法在遮挡较为严重的复杂环境下精度较低的问题，提出了一种鲁棒的LiDAR-IMU联合标定方法，将线特征引入到主流的标定算法中，通过同时构建线特征匹配对和面片匹配对，来完善激光雷达约束。针对优化阶段，提出双阶段迭代优化流程，并根据每轮迭代设计自适应的损失权重，使得优化能更好地收敛。利用自建数据集和开源数据集对该算法进行的测试与评估结果表明，本文算法能有效降低雷达与IMU标定时的条件要求，在雷达数据中没有多个大块平面的场景也能实现较高精度的标定。

目前的LiDAR-IMU联合标定方法只能支持对手持平台采集的离线数据进行标定，而在自动驾驶等应用中，需要实现对车载传感器的实时在线标定。为了达到这一需求，仍有许多方面的难点需要克服，比如：(1) 目前采用逐轮迭代优化的思路还难以满足实时性需求，因此需要考虑如何能快速收敛获得较为准确的标定结果；(2) 在车载平台的标定中，通常只能进行1个轴上的旋转操作（航向上的旋转），姿态变化不够丰富，这会对标定算法的收敛性造成较大影响。因此在对车载传感器进行标定时，还需要设计相应的方案来应对这些问题。

致谢: 感谢上汽集团创新研究开发总院高阶智驾团队王俊博士在本论文研究中给予的支持和帮助。