FAST-LIO2论文学习笔记

上官天耀

已于 2024-05-07 16:25:59 修改

阅读量888

点赞数 25

文章标签：自动驾驶 c++

于 2024-05-03 21:52:24 首次发布

本文链接：https://blog.csdn.net/Luiiiiiiiiiis/article/details/136338656

版权

GitHub - hku-mars/FAST_LIO: A computationally efficient and robust LiDAR-inertial odometry (LIO) packageA computationally efficient and robust LiDAR-inertial odometry (LIO) package - hku-mars/FAST_LIOhttps://github.com/hku-mars/FAST_LIO

摘要

1，直接将原始点注册到地图中（然后更新地图），不用提取人工设计的特征。这种方式适用于各种不同类型的激光雷达。

2、ikd-Tree

INTRODUCTION

LiDAR SLAM面临的问题：

1）当前LiDAR每秒会产生大量的点，如2M/s。实时处理这些点需要比较高的算力。

2）一般使用特征点去节省算力，比如平面特征点和边缘特征点。这些特征点是由局部特征进行提取的。特征点的提取受环境的影响大，在无特征结构的环境中无法有效地提取到足够地特征点，尤其是在小FOV的LiDAR上特别明显。另外对于不同的LiDAR（不同的扫描方式，不同的FOV等），需要设计不同的特征提取器。这需要进行额外大量的工作。

3）同时LiDAR点云中包含了运动畸变，尤其是LiDAR运动快时，点云的运动畸变会很严重。IMU可以减轻运动畸变的影响，但会引入额外的状态变量，比如偏置和标定外参。

4）LiDAR拥有较长的测量范围，但是每条扫面线之间的分辨率较低。结果点云稀疏地分布在三维空间中，这需要一个范围广、稠密地地图来管理这些稀疏点云。同时点云地图还需要能够同时支持快速查询和加入新测量点。相比于视觉测量，维护如此地点云地图是一个很大的挑战。

本文通过两个关键创新点来解决以上为题：

1）增量kd-tree（incremental k-d tree, ikd-Tree）

ikd-Tree能够有效管理大规模稠密点云。除了高效率的最邻近查找，ikd-Tree还支持增量地图更新（点的插入、删除等）以及用较小的计算量进行动态重平衡。对于LiDAR里程计和建图，ikd-Tree支持100Hz的更新频率

2）直接进行点注册（direct points registration)

该方法能够保证在快速运动和退化环境下仍然保持较高精度。同时不使用人工设计的特征点，使得该方法适用于不同雷达。

3）使用IMU数据，通过反向传播步骤来进行每个点的运动补偿。使用在流形上的迭代卡尔曼滤波来估计系统的全部状态。使用了新的卡尔曼增益来减少计算量。

RELEATED WORKS

基于特征的LiDAR SLAM一般包含三个主要模块：特征提取，里程计，建图。

集成IMU能够为ICP提供一个合适的初始值，因此提高了LiDAR Odometry的精度和稳定性。同时高频输出的IMU能够有效地补偿雷达运动畸变。

k临近（kNN）查找通过创建数据的空间索引进行。一般分为两种：一种是对数据进行分割，另一种是对空间进行分割。具体方法有：R-Tree 以及 R*-Tree；Octree以及kd Tree。

kd-Tree是二叉树，其每个节点都是一个超平面，将空间划分为两部分。研究表明，在低维度下，kd-Tree拥有优秀的kNN性能[42,43]。

在kd-Tree中，插入和删除操作会破坏二叉树的平衡，因此需要进行重平衡操作。

ikd-Tree基于scapegoat kd-tree[50]，能够实现实时建图。ikd-tree支持逐点进行插入，并同时进行降采样（体素降采样）。

当有大量点需要进行重平衡时，会使用额外的线程继续，这样会保证主线程的实时性和精度。

SYSTEM OVERVIEW

如果当前激光雷达的视场范围越过地图边界，距离激光雷达姿态最远的地图区域中的历史点将从ikd-Tree中删除。因此，ikd-Tree在特定长度的大体素中存储地图点，并来计算状态估计中的残差。最终，使用优化后的位姿将点保存在全局帧中，并将这些点插入到ikd-Tree中。

FAST-LIO2中的状态估计使用紧耦合迭代卡尔曼滤波，并同时进行LiDAR-IMU的在线标定外参加入进去。

从该图中可以看出，前向传播针对的是IMU数据，一般情况下，IMU的数据要高于雷达，将IMU数据进行积分，得到雷达点所在时刻的位置姿态。后向传播针对的是点云数据，当积累够一定量的点时，使用此时前向传播得到的IMU位姿，对这一批次的点进行运动畸变的去除，并注册到到全局坐标系中。然后通过ikd-tree搜索，计算残差，进行状态更新，输出里程计信息。

STATE ESTAMATION

状态转移模型

使用第一个IMU数据作为全局（初始）位姿。

运动学模型如下：

$^G\dot{R}_I = {^G}R_I\left \lfloor \omega_m - b_{\omega} - n_{\omega} \right \rfloor _\wedge$

${^G}\dot{p}_I = {^G}v_I$

${^G}\dot{v}_I = {^G}R_I(a_m - b_a - n_a) + {^G}g$

$\dot{b}_{\omega} = n_{b\omega}, \dot{b}_a = n_{ba}$

${^G}\dot{g} = 0, {^I}\dot{R}_L = 0, ^{I}\dot{p}_L = 0$

上面第一个式子是姿态微分方程，第二个是位置微分方程，第三个是速度微分方程。第四个表示陀螺仪和加速度计的偏置是随机游走模型。第五行表示这三个量是常量，不随时间而变化。

对于MEMS IMU的陀螺仪输出是角速度，加速度计输出是比力（比力方程：f = a - g，在上面公式中a_m即是比力f）。经过惯性导航算法，会得到在导航系下的姿态（attitude）、速度（velocity）和位置（position）。导航系多采用当地水平坐标系。因此在导航系下，重力只有沿着z轴的分量，在x轴和y轴上无分量。

一般在SLAM中，使用的都是经过惯性导航算法解算后的姿态速度位置，在FAST-LIO2中，采用了IMU第一个输出作为全局坐标系（G系），因此全局坐标系就是当地水平坐标系（n系）。

状态转移模型如下：

M表示一个流行，上图中M定义为一个24维的向量，即状态x。以上公式是所有状态写在一起的。

具体的状态转移公式：

对于姿态，状态转移方程如下

${^G}R_{I(t)} = {^G}R_{I(t-1)} * R^{t-1}_{t}$

上式中最后一个R式通过陀螺仪输出计算得到，表示的式上一时刻的IMU坐标系到当前时刻IMU坐标系的变换（注意：此处是坐标系的变换）。

姿态使用的是流形空间进行更新的，速度和位置在向量空间更新。

${^G}v_{I(t)} = {^G}v_{I(t-1)} + {^G}R_{I(t)}(a_m - b_a) \times {\Delta}t + {^G}g \times {\Delta}t$

${^G}p_{I(t)} = {^G}p_{I(t-1)} + {^G}v_{I(t-1)} \times{\Delta}t + \frac{1}{2} ({^G}R_{I(t)}(a_m - b_a) + {^G}g) {\Delta}t^2$

陀螺仪和加速度计的偏置为随机游走模型（高斯白噪声的积分），可以认为相邻两个时刻的值相等，最后在误差状态空间得到增量值后进行改正即可。

最后三个零向量表示重力以及LiDAR和IMU的外参不随时间进行变化。

测量模型

雷达一帧数据是逐点扫描得到的，因此每个点的扫描时间是不相同的。因此一帧数据中包含着运动畸变。本文使用后向传播（back propagation）进行运动畸变的去除。按每个点的时间戳，将所有点投影到该帧的最后一个点的坐标系下。可以看作该帧中的所有点都是在该帧结束时刻进行采样的。

每个点都包含由测距和方向导致的测量误差

${^L}p^{gt}_j = {^L}p_j + {^L}n_j$

使用点到平面的残差构建测量模型

$0={^G}u^T_j({^G}T_{I_k} {^I}T_L({^L}p_j + {^L}n_j) - {^G}q_j)$

上式是点到平面距离的经典公式。使用平面的法向量，以及平面上一点，计算平面外一点到平面的距离。

迭代卡尔曼滤波

包含两个重要的步骤：（1）IMU前向传播：通过每一个IMU测量值进行状态传播，（2）对每一雷达扫帧进行迭代更新。因为IMU的频率要比雷达高，因此会在更新中间经历多个状态传播（IMU前向传播）。

状态传播

在两个雷达扫面帧之间使用以上公式进行状态的计算。当遇到新的雷达扫描帧的时候，进行状态的更新。

残差计算

当遇到新的雷达扫描帧的时候，使用状态传播的最新状态，将雷达点注册到全局坐标系（G系）中。搜索该点的最邻近5个点，使用这5个点拟合一个平面，然后调用点到平面的观测方程计算残差。

迭代更新

下式（13）中前一部分式状态的误差，后一部分式观测的误差，将这两部分最小化，即可得到最优的状态。

迭代卡尔曼公式如下：

最终得到最优结果，然后将该帧点变换到全局坐标系，并插入到地图中。

MAPPING

建图模块主要的任务是维护地图，包括地图点的插入、删除和查找。

地图管理

维护一个长度为L的地图。当雷达位置位于地图边界时，地图的范围会移动，删除（b）图中橘黄色区域的点。

地图的移动距离是一个固定值。所有超出当前地图边界的点，都会通过逐体素删除操作进行删除。

树的结构和构造

ikd-Tree是一个二叉查找树(Binary search tree, BST)。对于二叉查找树，任何一个节点的左子树上的节点都小于该节点，任何一个节点右子树上的节点都大于该节点。任何一个子树都满足以上两个条件。

ikd-Tree在叶子节点和中间节点都进行了点的存储。这样能够更好的支持动态插入点和树的重平衡。这种存储方式在KNN搜索中同样是高效的。

每个节点都是一个点，其中有：该点的所有数据（x, y, z, intensity...)，指向左右子节点的指针，划分空间的坐标轴（axis），以该节点为根节点的子树或树的点的数量，是否要删除该节点的标志位（deleted），是否要删除以该节点为根节点的子树或树的标志（treedeleted）和要删除节点的数量（invalidnum），以及该节点以及其子节点的范围（三个坐标轴的最大最小值）。

构造ikd-Tree和构造静态kd-tree过程一样。根据最长范围所在的轴的中点进行划分，直到子空间中只有一个点为止。

增量更新

增量更新支持两种类型的操作：逐点操作和逐体素操作。对于点插入操作，逐点和逐体素都包含这降采样步骤。

1）包含降采样的点插入操作：

对于状态估计后的点集，将其所在空间划分成体素，选择最靠近体素中心的点。从Algorithm 2中可以看到，首先找到一点所在的体素，然后找到在该体素内的所有点，然后搜索最靠近体素中心的点。将最靠近中心的点插入到树中，并在点集中删除该体素中的点。

ikd-tree中点的插入是递归实现的。从树根节点开始向下搜索，直到遇到空节点。初始化该节点。然后向上递归更新节点的范围以及大小等属性。

2）使用延迟标签进行体素删除操作：

在删除操作中，使用延迟删除策略，当需要删除一个点时，并不会立刻进行删除操作，而是将该节点的deleted属性标记为true。如果一个节点的所有子节点的deleted属性都被标记为true，则该节点的treedeleted属性标记为true。deleted和treedeleted被称为lazy labels。需要删除的节点会在树的重构时进行删除。

逐体素删除过程中使用了节点的range属性。通过比较该体素的空间范围和节点的range属性，确定体素和节点的关系：包含，相交或相离。如果时相离，则直接范围，不进行操作。如果要删除的体素包含节点的范围，则该节点标记为直接删除。如果两者相交，则进一步查找确定需要删除的节点。

树的重平衡（re_balancing）

ikd-tree在每次增量更新后主动进行树平衡属性的检查，然后通过只重构相关节点进行树的动态重平衡。

平衡标准：alpha-balanced，alpha-deleted。

alpha-balanced：

alpha-deleted：

如果一个子树同时满足以上两个条件，则该树是平衡的。如果树的所有子树都同时满足以上两个条件，则整个树是平衡的。否则会对不满足条件的子树进行重新构建，来达到平衡条件。

alpha-balanced标准维护了树的最大高度。

alpha-deleted标准维护了树中需要删除的节点。

树的重新构建分为单线程模式和多线程模式。当树的节点数较少时，直接在主线程中进行单线程重构；否则将树复制展平成数据，然后单独的线程中进行处理，并记录树重构过程中的操作，在重构结束后补齐这些操作，以达到同步的效果。

K临近搜索

k临近搜索使用了节点的range属性进行快速查找。使用优先级队列来存储查找到的点以及到目标点的距离。在递归查找的过程中，首先计算目标节点到树节点的距离，如果该距离大于等于优先级队列中的最大距离，则该节点可以略过。

时间复杂度分析

体素删除和查找的时间复杂度：

点插入及降采样的时间复杂度：

O(log(n))

多线程重构时间复杂度：

O(n)

单线程重构时间复杂度：

O(nlog(n))

k临近查找时间复杂度：

O(log(n))

BENCHMARK RESULTS

实现

c++ ros系统下实现。

迭代卡尔曼滤波在IKFOM工具箱中已经实现。

local map的尺寸L=1000m。

雷达原始点经过1：4时间降采样后将原始数据输入到状态估计模块中。同时降采样还包括0.5m体素的空间降采样。

idk-tree的参数：alpha-bal=0.6, alpha-del=0.5, N_max = 1500。

计算平台选中机载轻量级计算机：DJI Manifold 2-C， Interi7-8550U 1.8GHz 以及8GB RAM。

同时对于FAST-LIO2，也在ARM上进行了测试，Khadas VIM3， 2.2GHz quad-core Cortex-A73 CPU 以及 4GB RAM。在文中标记为ARM的为在该平台上的测试结果。

评估

对于ikd-tree，使用boost geometry library中的R* -tree，以及pcl库中的octree和使用nanoflann实现的动态dk-tree。

ikd-tree支持树上的降采样，对比的数据结构不支持，因此使用距离或半径搜索代替。

octree对于点插入（point insertion）性能最优，但是与其他数据结构优势不大，但是octree的查询时间是最高的（octree是非平衡树）

对于nanoflann kd-tree，其插入时间略小于ikd-tree和R*-tree，但是会偶尔出现巨大的峰值，该峰值会严重影响了算法的实时性，尤其是在维护一个大的地图。对于查询，nanoflann kdtree时间要略高于ikd-tree，尤其是树的尺寸大时（10e5-10e6）。

R*-tree的插入时间略小于ikd-tree，但是数据量大时查询时间过长。

上表是各种数据结构的运行时间，ikd-tree整体上的性能是最好的。

上表是RMSE精度评估，可以看到，随着地图尺寸的增加，FAST-LIO2的精度也在增加，因为当雷达重新回到相同的地方时，新的点可以通过历史点进行配准。当地图尺寸大于2000时，精度不在增加，因为里程计漂移可能导致点的错误匹配。

对比其他LIO算法，FAST-LIO2在19个数据序列中有18个表现最好，并且FAST-LIO2是最鲁棒的算法。

上表对漂移误差进行了测量。整体趋势和RMSE结果相同。FAST-LIO2和其变种在5个数据集中都达到了最好的表现。

上表对运动时间进行了统计。

FAST-LIO2将里程计和建图整合到一起，地图的更新频率和里程计的输出频率一致，因此，上表中Total表示所有时间。其他算法里程计和建图是两个模块，里程计用于输出一个初始位置，建图线程去优化这个位置。

从上表中可以看到，FAST-LIO2的运行时间较其他算法少很多。

对于不同尺寸地图的FAST-LIO2，运行时间差距很小，这说明ikd-tree对地图尺寸不敏感。

REAL-WORLD EXPERIMENTS

运行结果如下：

该场景包含室内室外环境，大约650m后回到原点，整体漂移只有0.14m。

运行时间评估如下：

剧烈运动实验

如上图，四旋翼无人机悬停，然后快速反转（图9）。FAST-LIO2能够很好的计算出快速猛烈的运动。建图结果为图10。下图是该运动过程中的角度，位置，角速度，速度量。

手持快速移动实验

手持设备，在桥上进行快速移动和快速旋转。如下图

运动角度，位置，角速度和速度如下图。

为了测试FAST-LIO2的性能，该实验结束时回到了开始的位置。轨迹长度81m，开始-结束位置误差小于0.06m。

室外无人机实验

FAST-LIO2在制备环境中也表现很好。

CONCLUSION

本片文章提出了FAST-LIO2。FAST-LIO2使用了直接法，具有很好的鲁棒性，并达到了雷达-惯性里程计的先进水平。本文在多种数据集上进行了实验，都说明了FAST-LIO2有很高的竞争力，并且精度都很高。由于没有特征提取模块，FAST-LIO2在速度上得到了加强。同时本文对新的增量kd-tree数据结构进行了验证和实验，ikd-tree支持动态点插入删除和多线程重建。大量的实验表明，ikd-tree达到了总体领先的水平。从实验结果来看，建图的精度效率、快速移动的鲁棒性，以及稀疏场景的建图都通过里程计中使用更多点得到了提升。另外，由于没有特征提取模块，FAST-LIO2天然使用于各种不同类型的雷达。