LR FAST-LIO

Abstract

一个高效的LiDAR-inertial 里程计框架. 我们融合了LiDAR特征点和IMU数据, 用紧耦合的迭代EKF.

为了降低大量观测数量导致的计算负载, 我们用了一种新的方法来计算 Kalman gain. 新的方法的计算量是基于状态量维度而不是测量维度.

我们提出的方法在很多地方测过了, 在一次扫描中用了多余1200个特征点, 一次iEKF的耗时是25ms.

1. Introduction

1. LiDAR观测的特征点一般是几何结构(e.g edge 和 plane). 当UAV在cluttered环境里运行的时候, LiDAR的方案会退化, 特别是小FOV的LiDAR.
2. ...
3. ...

我们提出了FAST-LIO. 贡献:

1. 为了解决快速移动, 噪声或者是cluttered环境(退化会发生的时候), 我们采用了紧耦合的 iEKF来融合LiDAR特征点和IMU观测.
2. 降级计算力, 提出了新方法. 跟传统的卡尔曼增益是等价的
3. 没啥子.
4. 我们测试了很多.

2. Related works

A. LiDAR Odometry and Mapping

[7] 提出了ICP方法. 但是对于稀疏的LiDAR观测来说, ICP需要的准确的点的匹配很少.

为了解决这个问题, [8] 提出了一个 generalized-ICP, 是基于 点-平面距离的.

然后[9] 又把ICP方法和点-edge进行组合, 开发了一个LiDAR odometry and mapping 框架 (LOAM).

后面就有很多LOAM的变种了, 比如 LeGO-LOAM [10] 和 LOAM-Livox[11].

虽然这些方法对于几何环境, 还有大FoV工作的很好, 但是对于没有特征的环境很差.

B. Loosely-coupled LiDAR-Inertial Odometry

IMU-aided LOAM [9].

[12] 将IMU观测和 LiDAR测量的粒子滤波融合, 用error-state EKF.

[13] 加入了IMU-重力 模型来估计6DOF ego-motion 来辅助LiDAR扫描.

[14] 用 MSCKF 来融合扫描结果和IMU以及视觉测量.

还是有点问题, 它忽略了系统其他状态 (比如 速度) 和新扫描的pose.

C. Tightly-coupled LiDAR-Inertial Odometry

和松耦合不同, 紧耦合 LiDAR-inertial 里程计方法融合了 raw-feature points (而不是 scan registration results) with IMU.

有两种紧耦合的方法, 优化和滤波.

[15] 用了图优化和IMU预积分约束[16] 和 LiDAR 特征的平面约束.

[18] 最近日出了LIOM, 用类似的graph 优化, 但是是基于edge 和平面特征的.

[19] 用 Gaussian Partical Filter (GPF) 融合IMU的数据和平面2D LiDAR. 波士顿动力 Atlas 任性机器人就用的这个方法.

因为粒子滤波随着LiDAR点的算力提升的太快了, EKF [20] , UKF [21] 和 IKF [22]更受偏好.

3. Methodolody

A. Framework Overview

B. System Description

1. Continuous model

假设IMU跟LiDAR是刚体互联的, kinematic model如下:

1. Discrete model

1. Measurement model

因为lidar点的采样率很高 (e.g. 200kHz), 一般不会每次收到就处理. 一般采用积累到一定阶段然后处理的方法. 这种叫做一个scan.

C. States Estimator

我们用iterated extended Kalman filter.

假设上一次最优状态估计在时间 (t_{k-1})是 (ar{x}_{k-1}), 协方差是 (ar{P}_{k-1}).

生成一个MAP:

[min _{widetilde{mathbf{x}}_{k}^{kappa}}left(left|widetilde{mathbf{x}}_{k}^{kappa} ight|_{widehat{mathbf{P}}_{k}^{-frac{1}{2}} mathbf{J}^{T}}+sum_{j=1}^{m}left|mathbf{z}_{j}^{kappa}+mathbf{H}_{j}^{kappa} widetilde{mathbf{x}}_{k}^{kappa} ight|_{mathbf{R}_{j}^{-1}}^{2} ight) ag{17} ]

(17)的结果是标准的迭代Kalman滤波[22, 26].

[egin{aligned} mathbf{K} &=mathbf{P} mathbf{H}^{T}left(mathbf{H} mathbf{P} mathbf{H}^{T}+mathbf{R} ight)^{-1} \ widetilde{mathbf{x}}_{k}^{kappa} &=-mathbf{K} mathbf{z}_{k}^{kappa} Longrightarrow widehat{mathbf{x}}_{k}^{kappa+1}=widehat{mathbf{x}}_{k}^{kappa} oplusleft(-mathbf{K} mathbf{z}_{k}^{kappa} ight) end{aligned} ag{18} ]

(hat{x}^{k+1}_k) 可以用 ((I_KH)P) 来计算, 这个是真值状态 (x_k) 的误差的协方差???.

所以 (ar{P}_k = J^{-1}(I-KH)PJ^{-T}).

如果IKF完全收敛了, (J=I). 状态更新 (19) 和协方差更新 (21) 可以用于下一次扫描.

---

一个经常的问题是(18)里, 需要对于 (HPH^T+R) 求逆, 是以测量为维度的. 实际上, LiDAR的个数是很大的, 对于这个矩阵大小来求逆是难以接受的. 如[22, 26], 是只用了少量的测量.

---

我们的灵感来源于(17), 这里面cost function是 over the state, 所以可以以状态维度来作为计算复杂度. 事实上, 如果能直接解决(17), 就可以获得(18)一样的解.

[mathbf{K}=left(mathbf{H}^{T} mathbf{R}^{-1} mathbf{H}+mathbf{P}^{-1} ight)^{-1} mathbf{H}^{T} mathbf{R}^{-1} ag{23} ]

D. Initialization

静止初始化, 就可以得到IMU偏置和重力方向.

4. Experiment Result

A. Computational Complexity Experiments