FAST-LIO2论文翻译

2023-05-16

摘要

本文提出了FAST-LIO2：一个快速，鲁棒和多功能的雷达惯性里程计框架。基于高效的紧耦合的迭代卡尔曼滤波，FAST-LIO2有两个关键的创新点，可实现快速、鲁棒和准确的激光导航（和建图）。第一个就是直接将原始数据与地图对齐（随后更新地图，也就是建图）而不用提取特征。这使得我们可以利用环境中微小的特征因此进而提高了精度。特征提取模块的消除使得我们的算法可以很自然的适配于具有不同扫描模式的激光雷达。第二个创新点是通过一个增量k-d树数据结构：ikd-Tree，维护地图，保证了增量更新（点的插入和删除）和动态的re-balancing。与现在的动态数据结构（octree， R*-Tree， nanlflann k-d树），ikd-tree在性能上领先；另外其也支持对树进行下采样。我们对来自于开放的19个激光雷达数据集进行了细致的基准测试。与其他state-of-art的激光惯性导航系统比较，FAST-LIO2在较低算力消耗下获得了更高的精度。并基于固态小视野激光雷达，我们在各种真实环境中也进行了测试。总之，FAST-LIO2计算效率高（在室外环境下里程计和建图最高可以达到100hz），鲁棒性好（在cluttered室内环境中旋转最高得到1000deg/s依旧可以获得可靠的位姿估计），通用性强(适合多线激光雷达，也适合固态激光雷达；平台可以是无人机或者手持的设备，处理器可以是Intel的也可以是arm的)，并且比其他现在的方法精度高。我们所提出的FAST-LIO2及其相关的数据结构ikd-tree的实现均在Github上开源。

1 介绍

前面省略了价值不大的部分。

在本文中，我们通过两个关键的新颖的技术来解决这些问题：增量式的k-d树和直接的点云对齐。更具体的，我们的贡献如下：1）我们开发了一个增量式k-d树的数据结构，ikd-tree，用来更有效的表示一个稠密的大规模的点云地图。除了有效的最近邻搜索外，这个新的数据结构可以用最小的算力支持增量地图更新（points insertion，on-tree downsampling，points delete）和动态的re-balancing。以上的这些特征使得ikd-Tree非常适合激光雷达里程计和建图应用，特别是在算力受限的平台如基于Inter i7的micro-UAV onboard computer甚至是基于ARM的处理器。ikd-Tree数据结构在Github上已经开源了。2）由于ikd-Tree对计算效率的提高，使得我们能够将原始数据与直接地图进行对齐，这就保证了即使是在复杂环境中进行剧烈运动scan registration也非常准确的和可靠。如视觉SLAM一样，我们把这个raw points-based registraion称为直接法【21】。由于没有了hand-engineered 特征提取模块，我们的系统自然地适合不同的激光雷达传感器。3）我们把以上的两个关键技术集成到一个完全紧耦合的激光雷达-惯性里程计系统FAST-LIO【22】中。4）我们进行了各种各样的实验评估ikd-Tree、直接点云对齐和整个系统的有效性。在18个不同大小的数据集上测试结果表明，ikd-Tree与其他数据结构（octree，R*-tree，nanoflann k-d tree）相比在激光雷达里程计和建图上性能优越。在各种不同开放的19个激光雷达数据集上的详细测试表明，FAST-LIO2比其他state-of-art激光雷达惯性导航系统在较低算力下获得了更高的精度。We finally show the effectiveness of FAST-LIO2 on challenging realworld data collected by emerging solid-state LiDARs with very small FoV，including aggressive motion (e.g. rotation speed up to 1000 deg/s) and structure-less environments。

本文的剩余部分组织结构如下：在第2部分，我们讨论了相关工作。在第3、4和5部分，我们给出了整个系统pipline的架构和每个关键模块的细节。开放数据集的对比验证在第6部分。真实环境中的测试在第7部分。接下来是第8部分结论。

2 相关工作

A 激光惯性里程计

目前3D激光SLAM的工作基本上继承于【23】提出的LOAM。它包含了三个主要模块：特征提取、里程计和建图。为了降低计算负荷，新的激光雷达scan首先根据曲率进行特征提取。然后里程计模块（scan-to-scan）通过对连续两帧的特征匹配获取一个比较粗糙的实时的激光雷达位姿里程计（10hz）。基于里程计数据，多个scans被合并sweep，然后接下来将其与全局地图对齐和合并。在这个过程中，我们使用地图点构建了一个kd树，用来确保高效的k最近邻搜索（KNN搜索）。Then, the point cloud registration is achieved by the Iterative Closest Point (ICP) 【24】-【26】 method wherein each iteration, several closest points in the map form a plane or edge where a target point belongs to。为了降低kd树的构建时间，地图点以一定的分辨率进行下采样。优化建图过程以一个较低的频率（1-2Hz）执行。

随后的激光里程计工作与LOAM的框架基本保持了一致。例如，Lego-LOAM【27】引入了地面点分割降低计算量，和回环检测模块消除长期漂移。之后，LOAM-Livox【16】把LOAM扩展到了大疆固态激光雷达上。为了处理small FoV和非重复扫描（两个连续关键帧的相关性小），通过将new scan直接与the global map对齐获取里程计。直接与地图进行对齐提高了里程计的精度，但随之而来的代价增加了计算量（每步构建k-d树更新地图点）。

集成IMU数据后，由于其可以给ICP提供一个好的位姿初值，因此可以显著的提供激光雷达里程计的精度和鲁棒性。此外，IMU的测量频率比较高，因此可以有效的补偿激光雷达scan的运动畸变。LION【28】是一个松耦合激光惯性SLAM框架，继续保持了LOAM的scan-to-scan的对齐，并且由于其通过引入了an observability awareness check into the odometry降低点云数量，因此节约了计算量。不过更多的基于紧耦合的激光惯性融合工作【17，29】-【31】是在一个很小数量的局部地图中（包含了固定最近scans（或者关键帧））计算里程计。与scan-to-scan对齐相比较，scan-local map由于使用了很多的信息通常更精确。LILIO【17】为非重复扫描激光雷达开发了一个新的特征提取方法，并在一个包含了最近20帧的局部地图中进行帧对齐产生里程计数据。LIO-SAM【30】需要一个9轴的IMU的姿态测量值作为scan与局部地图对齐的初值。LINS【31】引入了紧耦合的迭代卡尔曼滤波并且robocentric formula into the LiDAR pose optimization in the odometry。上述工作为了获得实时性能局部地图通常比较小，里程计会很快的漂移，因此需要一个低速率的建图过程，如map refining（LINS【31】）、滑窗优化（LILI-OM【17】和LIOM【29】）和因子图优化【32】（LIO-SAM【30】）。与上述的方法像比较，FAST-LIO【22】引入了一个formal back-propagation，精确的考虑了scan中每个点的采样时间，并通过a rigorous kinematic model driven by IMU measurements补偿运动畸变。此外，一个新的卡尔曼增益计算公式被用来降低计算复杂度（from the dimension of the measurements to the dimension of the state）。从数学意义上，新公式被证明与传统的是等效的，不过其计算量相比之前降低了几个数量级。计算效率的提高使得我们可以在每步直接和实时的进行scan-to-map计算里程计和更新地图。The timely mapping of all recent scan points leads to increased odometry accuracy。然而，为了避免构建k-d树地图时间的增长，这个系统只能在小环境中工作（几百米）。

FAST-LIO2基于FAST-LIO【22】，因此也继承了紧耦合的融合框架，特别是反向传播修正运动畸变和提高效率的快速卡尔曼增益计算。同时，为了解决计算量快速增加的问题，我们提出了一个新的数据结构ikd-Tree（其可以支持在每步的增量地图更新和快速的KNN搜索）。受益于其对计算效率极大地减小，里程计计算时可以直接使用激光雷达原始点云直接与地图进行对齐，这样极大的提高了里程计的精度和鲁棒性，特别是在当新scan没有包含任何特征的时候（由于视野比较小或者在结构缺乏的环境中）。与上述提到的其他紧耦合激光惯性方法（使用了所有的特征点）相比较，我们的方法更加轻量级，并且建图速度更快，里程计精度更高，and eliminates the need for parameter tuning for feature extraction。

我们工作中将原始点云直接与地图对齐这个想法首先在LION【28】中应用，不过如上所述其是一个松耦合的框架。这个想法与【26】提出的generaalized-ICP（G-ICP）非常相似，点与地图中的一个小的局部平面对齐。这利用环境中是平滑的被视为由一个小的局部平面构成的这个假设。不过，generalized-ICP的计算量通常比较大【33】。其他工作如基于NDT【34】-【36】也对齐原始点云，不过NDT与ICP相比较稳定性不太好，而且容易发散【36】。

B Dynamic Data Structure in Mapping

为了实现实时建图，我们需要一个动态数据结构既支持增量更新又支持kNN高效搜索。通常来讲，kNN搜索问题可以通过为data points构建spatial indices来解决，一般分为两种策略：partitionning the data and splitting the space。一个典型的数据分区的例子是R-tree【37】， which clusters the data into potential overlappend axis-aligned cuboids based on data proximity in space。Various R-trees根据线性、二次和指数复杂性划分节点，它们均支持最近邻搜索和逐点更新（插入、删除和重新插入）。除此之外，R-trees也支持在指定区域中搜索目标点或者满足一定条件的目标点。R-trees的另外一个版本是R*-tree，其可能比原始版本更加的优秀【38】。The R*-tree handles insertion by minimum overlap criteria and applies a forced re-insertion principle for the node splitting algorithm。

八叉树【39】和k-d树【40】是另外两种有名的切分空间以进行kNN搜索的数据结构。八叉树通过把空间迭代的等分为8个立方体的方式组织3-D点云。当子立方体为空的时候或者满足停止规则（最小分辨率或者最小的点数量）的时候划分停止。新的点被插入到八叉树的叶节点，如果需要当进一步切割的时候，新的3D点背插入到八叉树的子节点。八叉树支持kNN搜索和box-wise搜索，which returns data points in a given axis-aligned cuboid。

k-d树是一个二叉树，其节点代表了一个把空间划分为两部分的axis-aligned超平面。在标准的重建规则中，切割点通常选择为最长维度的中点，以实现比较紧凑的空间划分【41】。When considering the data characteristics of low dimensionality and storage on main memeory in mapping，comparative studies show that k-d trees achieve the best performance in kNN problem【42，43】。然而，插入新点或者删除旧点会恶化k-d树的平衡性能；因此，为了re-balance 树我们需要re-building。建图的时候会使用k-d树的一些库，比如ANN【44】、linnabo【43】和FLANN【45】；完全re-build k-d树更新地图，会导致很大的计算量。Though hardware-based methods to re-build k-d trees have been thoroughly investigated in 3D graphic applications 【46】-【49】, the proposed methods rely heavily on the computational sources which are usually limited on onboard computers for robotic applications. Instead of re-building the tree in full scale, Galperin等提出了一个 scapegoat k-d tree where re-building is applied partially on the unbalanced sub-trees to maintain a loose balance property of the entire tree 【50】。实现增量操作的另一种方法是，以类似于【51，52】的对数方法维护一组kd树，并重建精心选择的子集。Bkd树在主存储器中保持最大大小为M的k-d树 $\tau _{0}$ ，在外部存储器上保持一组k-d树 $\tau _{i}$ ，其中第i棵树的大小为 $2^{i-1}M$ 【53】。当树 $\tau _{0}$ 满时，从 $\tau _{0}$ 到提取点 $\tau _{k-1}$ 并插入到第一个空树 $\tau _{k}$ 中。最先进的实现nanoflann k-d树利用对数结构进行增量更新，而lazy标签仅标记删除的点，而不将其从树中删除（因此是内存）【54】。

我们基于scapegoat k-d tree【50】提出了一个动态数据结构，也就是增量式的k-d tree（ikd-Tree），可以实现实时建图。我们的ikd-Tree支持逐点插入with on-tree downsampling which is a common requirement in mapping， whereas downsampling must be done outside before inserting new points into other dynamic data structures【38、39、54】。当需要移除给定区域（规则形状，比如长方体）中不需要的点时，当前的实现方法如R-trees和八叉树在给定区域中搜索这些点，并且一个接一个的删除它们，而常见的k-d tree使用半径搜索获取点的索引。与这些间接和低效的方法像比较，ikd-Tree通过维护距离信息和lazy labels直接删除位于axis-aligned cuboies中的点。标签为“deleted”的点在re-building过程中被移除。此外，尽管当应用partial re-balancing methods 如scapegoat k-d tree【50】和nanoflann k-d tree【54】后，增量更新是可以使用的，但建图方法使用k-d tree很大数量的点的时候存在间歇性的延时问题。为了克服这个问题，通过并行重建避免了ikd树中的显著延迟，同时保证了主线程中的实时性和准确性。

3 系统概述

FAST-LIO2的pipline如图1所示。顺序采样的激光雷达原始点云首先在10ms（100Hz更新）和100ms（10Hz更新）之间的时间段内累积。累计的点云被称为scan。为了进行状态估计，new scan中点首先与维护在一个通过紧耦合迭代卡尔曼滤波框架（红色虚线部分）维护的一个很大局部地图进行对齐。在一个很大局部地图中的全局地图点以增量式k-d树结构ikd-Tree（蓝色虚线部分）组织。如果当前激光雷达的视场范围穿过地图边界，则距离激光雷达位姿最远的地图区域中的历史点将从ikd树中删除。因此，ikd-Tree会跟踪一个一定长度方形区域的所有地图点（论文中提高的“map size”），并且这些点在状态估计模块中会用来计算残差。优化之后的位姿最后将new scan中的点registers到全局帧并且通过插入到ikd-Tree的方式（以里程计或者建图的速度）将它们与地图合并。

4 状态估计

FAST-LIO2的状态估计是继承于FAST-LIO【22】的一个紧耦合迭代卡尔曼滤波框架，不过另外集成了在线校正激光雷达-IMU外参的部分。这里我们简要介绍一下必须的公式和滤波器的流程，不过推荐读者阅读【22】了解更多的细节。

A 运动学模型 $b_{a}$

我们首先推导系统模型，其中包含了状态转移模型和测量模型。

1）状态转移模型

以第一帧IMU帧（以I表示）作为全局坐标系（以G表示），并且以 $_{}^{I}\textrm{T}_{L}=(_{}^{I}\textrm{R}_{L}, _{}^{I}\textrm{P}_{L})$ 代表了激光雷达和IMU之间的外参，运动学方程如下：

其中 $_{}^{G}\textrm{P}_{I}$ $_{}^{G}\textrm{R}_{I}$ 代表了IMU在全局坐标系下的位置和姿态， $_{}^{G}\textrm{g}$ 代表了全局坐标系的重力加速度； $a_{w}$ 和 $w_{m}$ 代表了IMU的测量值； $n_{a}$ 和 $n_{w}$ 代表了IMU测量值中的噪声； $n_{ba}$ 和 $n_{bw}$ 代表了IMU测量值中的零偏； $a_{\wedge }$ 代表了向量a的反对称矩阵。

以i代表IMU测量值中的索引。基于【22】中定义的 $\oplus$ ，连续的运动学模型（1）能以IMU的采样间隔 $\Delta t$ 离散为方程（2）【55】：

其中的函数 $f$ ，状态 $x$ ，输入 $u$ ，噪声 $w$ 定义如下：

2）测量模型

激光雷达通常一个接一个的对点进行采样。因此当激光雷达进行连续运动的时候，最后的点云实际上是在激光雷达不同的位姿时采样的。为了修正这个in-scan的运动，我们采用【22】提到的反向传播，它根据 IMU 测量值估计scan中每个点相对于扫描结束时间的位姿的LiDAR位姿。估计的相对姿势使我们能够根据scan中每个点的确切采样时间将所有点投影到scan结束时间。结果，scan中的每个点可以被视为在scan的结束时间同时进行采样的。

我们以k表示激光雷达scan中的索引。 $_{}^{L}\textrm{P}_{j}$ 表示了第k个scan中在激光雷达局部坐标系以及scan结束时间采样的点。由于激光雷达的测量噪声，每个测量点 $_{}^{L}\textrm{P}_{j}$ 都包含了噪声 $_{}^{L}\textrm{n}_{j}$ （距离和方向）。剔除噪声后即是激光雷达局部坐标系中真正的点 $_{}^{L}\textrm{P}_{j}^{gt}$ 。

真值点，使用相应的激光雷达位姿 $_{}^{G}\textrm{T}_{Ik}=(_{}^{G}\textrm{R}_{Ik}, _{}^{G}\textrm{P}_{Ik})$ 和相应的外参映射 $_{}^{I}\textrm{T}_{L}$ 到全局坐标系下，其应该精确的位于地图中一个局部的小平面上。

其中 $_{}^{G}\textrm{U}_{j}$ 代表了相应平面的法向量， $_{}^{G}\textrm{q}_{j}$ 代表了位于平面上的点（如图2所示）。我们应该注意到 $_{}^{G}\textrm{T}_{Ik}$ 和 $_{}^{I}\textrm{T}_{L}$ 都包含在相应的状态变量 $x_{k}$ 中。总结于方程（4）中基于第j个点 $_{}^{L}\textrm{P}_{j}$ 的测量模型的一个更紧凑的形式如下：

其为状态 $x_{k}$ 定义了一个隐式的观测模型。

B 迭代卡尔曼滤波

基于状态转移模型（2）和观测模型（3），我们在流形M上参考【55】和【22】的操作使用迭代卡尔曼滤波。它包含了两个关键的步骤：基于IMU测量值的前向传播和基于激光雷达scan的迭代更新，这两个步骤都自然地估计流形 M 上的状态，从而避免了任何重新归一化。由于IMU的测量频率比激光雷达高，因此在更新之前通常已经进行了多次传播。

1）传播

假设融合第k-1次激光雷达scan后的最优状态是 $\bar{x_{k-1}}$ ，且其协方差是 $\bar{p_{k-1}}$ 。接下来，当有IMU数据到来时则进行前向传播。更具体的，令过程噪声 $w_{i}=0$ ，基于方程（2）前向传播状态和协方差。

其中 $Q_{i}$ 是噪声 $w_{i}$ 的协方差。矩阵 $F_{\tilde{x_{i}}}$ 和 $F_{\tilde{w_{i}}}$ 的计算如下，具体推导需要参考【55】和【22】。

状态和协方差一直前向传播，直到新的scan到来。预测状态和预测协方差以 $\hat{x_{k}}$ 和 $\hat{p_{k}}$ 表示。

2）残差计算

假设在本次迭代更新的状态变量 $x_{k}$ 为 $\hat{x_{k}^{k}}$ ，其中k=0（在第一次更新之前）， $\hat{x_{k}^{k}}=x_{k}$ ，预测状态根据方程（6）进行前向传播。然后，我们把激光雷达扫描点 $_{}^{L}\textrm{p}_{i}$ 映射到全局坐标系中（ $_{}^{G}\textrm{}\hat{P}_{j}=_{}^{G}\textrm{}\hat{T}_{I_{k}}^{k}*_{}^{I}\textrm{}\hat{T}_{L_{k}}^{k}*_{}^{L}\textrm{p}_{j}$ ）。然后在以ikd-tree表示的地图中搜索到其最近的5个点。使用发现的最近点拟合出一个法向量为 $_{}^{G}\textrm{u}_{i}$ 中心 $_{}^{G}\textrm{q}_{j}$ 为局部的平面（接下来会在观测模型中使用）。将观测模型（5）在 $\hat{x_{k}^{k}}$ 处进行一阶泰勒展开进行近似：

其中 $\tilde{x_{k}^{k}}=x_{k}\ominus \hat{x_{k}^{k}}$ （或者等于 $x_{k}=\hat{x_{k}^{k}}\oplus \tilde{x_{k}^{k}}$ ）， $H_{j}^{k}$ 是 $H_{j}(\hat{x_{k}^{k}}\oplus \tilde{x_{k}^{k}}, _{}^{L}\textrm{n}_{j})$ 在 $\tilde{x_{k}^{k}}$ 的雅可比矩阵，近似等于0； $v_{j}\epsilon (0, R_{j})$ 是由于原始测量噪声导致的 $_{}^{L}\textrm{n}_{j}$ ， $z_{j}^{k}$ 是残差因子。

3）迭代更新

前向传播的状态 $\hat{x}_{k}$ 和协方差 $\hat{p}_{k}$ ，为未知的状态 $x_{k}$ 提供了一个高斯先验分布。更具体而言， $\hat{p}_{k}$ 代表了下面误差状态的协方差。

其中 $J^{k}$ 是 $\hat{x_{k}^{k}}\oplus \tilde{x_{k}^{k}}\ominus\hat{x_{k}^{k}}$ 偏微分（对应于 $\tilde{x_{k}^{k}}$ 近似为零）。

其中 $A(\cdot )^{-1}$ 在【22、55】中定义。 $\delta_{}^{G}\textrm{}\hat{\theta }_{I_{k}}^{k}=_{}^{G}\textrm{}\hat{R}_{I_{k}}^{k}\ominus_{}^{G}\textrm{}\hat{R}_{I_{k}}$ 和 $\delta_{}^{G}\textrm{}\hat{\theta }_{I_{k}}^{k}=_{}^{I}\textrm{}\hat{R}_{I_{k}}^{k}\ominus_{}^{I}\textrm{}\hat{R}_{I_{k}}$ is the error states of IMU’s attitude and rotational extrinsic, respectively。对于第一次迭代， $\hat{x_{k}^{k}}=\hat{x_{k}}$ ，其中的 $J^{k}=I$ 。

除了先验分布之外，由于观测模型（8）我们还有一个状态的分布：

将（10）中的先验分布与（12）中的测量模型相结合，可得到由 $\hat{x_{k}^{k}}$ 等价表示的状态 $x_{k}$ 后验分布及其最大似然估计（MAP）：

其中 $\left \| x \right \|_{m}^{2}=x^{T}Mx$ 。最大似然估计可以通过如下的迭代卡尔曼滤波求解（为了简化符号表示，令 $H=[{H_{1}^{k}}^{T}, ...,{H_{m}^{k}}^{T}]^{T}$ $R=diag(R_{1}, ..., R_{m})$ $P=(J_{k})^{-1}\widehat{P_{k}}J_{k}^{-1})^{-T}$ ） $z_{k}^{k}=[{z_{1}^{k}}^{T}, ...,{z_{m}^{k}}^{T}]^{T}$

需要注意的是，卡尔曼增益 K 计算需要求一个状态维度矩阵的逆，而不是之前工作中使用的测量维度矩阵。

上述的处理过程不断重复知道收敛。收敛之后，最优状态和协方差估计如下：

基于最优状态 $\overline{x_{k}}$ ，在第k个scan中的每个激光雷达点 $_{}^{L}\textrm{P}_{i}$ 然后被变换到全局坐标系下：

变换后的激光雷达点 $_{}^{G}\textrm{}\overline{\textrm{P}_{i}}$ 然后插入到以ikd-tree表示的全局地图中。我们的状态估计过程总结如下：

5 建图

这部分将参考faster-lio，这里就不再学习和翻译。

6 开放数据集结果

7 真实环境的实验

8 结论

参考文献

[1] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, “Svo: Semidirect visual odometry for monocular and multicamera systems,” IEEE Transactions on Robotics, vol. 33, no. 2, pp. 249–265, 2016.
[2] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
[3] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[4] C. Campos, R. Elvira, J. J. G. Rodr´ıguez, J. M. Montiel, and J. D. Tardos, ´“Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam,” IEEE Transactions on Robotics, 2021.
[5] R. Newcombe, “Dense visual slam,” Ph.D. dissertation, Imperial College London, 2012.
[6] M. Meilland, C. Barat, and A. Comport, “3d high dynamic range dense visual slam and its application to real-time object re-lighting,” in 2013 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE, 2013, pp. 143–152.
[7] M. Bloesch, J. Czarnowski, R. Clark, S. Leutenegger, and A. J. Davison, “Codeslam—learning a compact, optimisable representation for dense visual slam,” in Proceedings of the IEEEconference on computer vision and pattern recognition, 2018, pp. 2560–2568.
[8] C. Kerl, J. Sturm, and D. Cremers, “Dense visual slam for rgb-d cameras,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013, pp. 2100–2106.
[9] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron, J. Diebel, P. Fong, J. Gale, M. Halpenny, G. Hoffmann, et al., “Stanley: The robot that won the darpa grand challenge,” Journal of field Robotics, vol. 23, no. 9, pp. 661–692, 2006.
[10] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer, et al., “Autonomous driving in urban environments: Boss and the urban challenge,” Journal of Field Robotics, vol. 25, no. 8, pp. 425–466, 2008.
[11] J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel, J. Z. Kolter, D. Langer, O. Pink, V. Pratt, et al., “Towards fully autonomous driving: Systems and algorithms,” in 2011 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2011, pp. 163–168.
[12] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” IEEE Robotics and Automation Letters, vol. 2, no. 3, pp. 1688–1695, 2017.
[13] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, vol. 36, no. 4, pp. 710–733, 2019.
[14] D. Wang, C. Watkins, and H. Xie, “Mems mirrors for lidar: A review,” Micromachines, vol. 11, no. 5, p. 456, 2020.
[15] Z. Liu, F. Zhang, and X. Hong, “Low-cost retina-like robotic lidars based on incommensurable scanning,” IEEE/ASME Transactions on Mechatronics, pp. 1–1, 2021.
[16] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3126–3131.
[17] K. Li, M. Li, and U. D. Hanebeck, “Towards high-performance solidstate-lidar-inertial odometry and mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 5167–5174, 2021.
[18] J. Lin and F. Zhang, “A fast, complete, point cloud based loop closure for lidar odometry and mapping,” arXiv preprint arXiv:1909.11811, 2019.
[19] H. Wang, C. Wang, and L. Xie, “Lightweight 3-d localization and mapping for solid-state lidar,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1801–1807, 2021.
[20] Z. Liu and F. Zhang, “Balm: Bundle adjustment for lidar mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184–3191, 2021.
[21] C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 15–22.
[22] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, pp. 1–1, 2021.
[23] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in realtime.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
[24] G. C. Sharp, S. W. Lee, and D. K. Wehe, “Icp registration using invariant features,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 24, no. 1, pp. 90–102, 2002.
[25] K.-L. Low, “Linear least-squares optimization for point-to-plane icp surface registration,” Chapel Hill, University of North Carolina, vol. 4, no. 10, pp. 1–3, 2004.
[26] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics: science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435.
[27] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758–4765.
[28] A. Tagliabue, J. Tordesillas, X. Cai, A. Santamaria-Navarro, J. P. How, L. Carlone, and A.-a. Agha-mohammadi, “Lion: Lidar-inertial observability-aware navigator for vision-denied environments,” arXiv preprint arXiv:2102.03443, 2021.
[29] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometry and mapping,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 3144–3150.
[30] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020, pp. 5135–5142.
[31] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “Lins: A lidar-inertial state estimator for robust and efficient navigation,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 8899–8906.
[32] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012.
[33] K. Koide, M. Yokozuka, S. Oishi, and A. Banno, “Voxelized gicp for fast and accurate 3d point cloud registration,” EasyChair Preprint, no. 2703, 2020.
[34] P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” in Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453), vol. 3. IEEE, 2003, pp. 2743–2748.
[35] M. Magnusson, “The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection,” Ph.D. dissertation, Orebro universitet, 2009. ¨
[36] M. Magnusson, A. Nuchter, C. Lorken, A. J. Lilienthal, and J. Hertzberg, “Evaluation of 3d registration reliability and speed-a comparison of icp and ndt,” in 2009 IEEE International Conference on Robotics and Automation. IEEE, 2009, pp. 3907–3912.
[37] A. Guttman, “R-trees: A dynamic index structure for spatial searching,” in Proceedings of the 1984 ACM SIGMOD international conference on Management of data, 1984, pp. 47–57.
[38] N. Beckmann, H.-P. Kriegel, R. Schneider, and B. Seeger, “The r*-tree: An efficient and robust access method for points and rectangles,” in Proceedings of the 1990 ACM SIGMOD international conference on Management of data, 1990, pp. 322–331.
[39] D. Meagher, “Geometric modeling using octree encoding,” Computer graphics and image processing, vol. 19, no. 2, pp. 129–147, 1982.
[40] J. L. Bentley, “Multidimensional binary search trees used for associative searching,” Communications of the ACM, vol. 18, no. 9, pp. 509–517, 1975.
[41] J. H. Friedman, J. L. Bentley, and R. A. Finkel, “an algorithm for finding best matches in logarithmic expected time,” ACM Transactions on Mathematical Software (TOMS), vol. 3, no. 3, pp. 209–226, 1977.
[42] J. L. Vermeulen, A. Hillebrand, and R. Geraerts, “A comparative study of k-nearest neighbour techniques in crowd simulation,” Computer Animation and Virtual Worlds, vol. 28, no. 3-4, p. e1775, 2017.
[43] J. Elseberg, S. Magnenat, R. Siegwart, and A. Nuchter, “Comparison ¨ of nearest-neighbor-search strategies and implementations for efficient shape registration,” Journal of Software Engineering for Robotics, vol. 3, no. 1, pp. 2–12, 2012.
[44] S. Arya and D. Mount, “Ann: library for approximate nearest neighbor searching,” in Proceedings of IEEE CGC Workshop on Computational Geometry, Providence, RI, 1998.
[45] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration.” VISAPP (1), vol. 2, no. 331-340, p. 2, 2009.
[46] W. Hunt, W. R. Mark, and G. Stoll, “Fast kd-tree construction with an adaptive error-bounded heuristic,” in 2006 IEEE Symposium on Interactive Ray Tracing. IEEE, 2006, pp. 81–88.
[47] S. Popov, J. Gunther, H.-P. Seidel, and P. Slusallek, “Experiences with streaming construction of sah kd-trees,” in 2006 IEEE Symposium on Interactive Ray Tracing. IEEE, 2006, pp. 89–94.
[48] M. Shevtsov, A. Soupikov, and A. Kapustin, “Highly parallel fast kd-tree construction for interactive ray tracing of dynamic scenes,” Computer Graphics Forum, vol. 26, no. 3, pp. 395–404, 2007.
[49] K. Zhou, Q. Hou, R. Wang, and B. Guo, “Real-time kd-tree construction on graphics hardware,” ACM Transactions on Graphics, vol. 27, no. 5, pp. 1–11, 2008.
[50] I. Galperin and R. L. Rivest, “Scapegoat trees.” in SODA, vol. 93, 1993, pp. 165–174.
[51] J. L. Bentley and J. B. Saxe, “Decomposable searching problems i: Static-to-dynamic transformation,” J. algorithms, vol. 1, no. 4, pp. 301–358, 1980.

[52] M. H. Overmars, The design of dynamic data structures. Spinger Science & Business Media, 1987, vol. 156.

[53] O. Procopiuc, P. K. Agarwal, L. Arge, and J. S. Vitter, “Bkd-tree: A dynamic scalable kd-tree,” in International Symposium on Spatial and Temporal Databases. Springer, 2003, pp. 46–65.
[54] J. L. Blanco and P. K. Rai, “nanoflann: a C++ header-only fork of FLANN, a library for nearest neighbor (NN) with kd-trees,” https://github.com/jlblancoc/nanoflann(v1.3.2), 2014.
[55] D. He, W. Xu, and F. Zhang, “Embedding manifold structures into kalman filters,” arXiv preprint arXiv:2102.03804, 2021.
[56] P. Chanzy, L. Devroye, and C. Zamora-Cura, “Analysis of range search for random kd trees,” Acta informatica, vol. 37, no. 4-5, pp. 355–383, 2001.
[57] Z. Yan, L. Sun, T. Krajnik, and Y. Ruichek, “EU long-term dataset with multiple sensors for autonomous driving,” in Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
[58] W. Wen, Y. Zhou, G. Zhang, S. Fahandezh-Saadi, X. Bai, W. Zhan, M. Tomizuka, and L.-T. Hsu, “Urbanloco: a full sensor suite dataset for mapping and localization in urban scenes,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 2310–2316.
[59] N. Carlevaris-Bianco, A. K. Ushani, and R. M. Eustice, “University of michigan north campus long-term vision and lidar dataset,” The International Journal of Robotics Research, vol. 35, no. 9, pp. 1023–1035, 2016.
[60] G. Barend, L. Bruno, L. Mateusz, W. Adam, K. Menelaos, and F. Vissarion, “Boost geometry library,” https://www.boost.org/doc/libs/1 651/libs/geometry/, September 2017.
[61] R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),” in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 1–4.

[62] G. Lu, W. Xu, and F. Zhang, “Model predictive control for trajectory tracking on differentiable manifolds,” arXiv preprint arXiv:2106.15233, 2021.
[63] F. Kong, W. Xu, and F. Zhang, “Avoiding dynamic small obstacles with onboard sensing and computating on aerial robots,” arXiv preprint arXiv:2103.00406, 2021.

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)