菜单 学习猿地 - LMONKEY

VIP

开通学习猿地VIP

尊享10项VIP特权 持续新增

知识通关挑战

打卡带练!告别无效练习

接私单赚外块

VIP优先接,累计金额超百万

学习猿地私房课免费学

大厂实战课仅对VIP开放

你的一对一导师

每月可免费咨询大牛30次

领取更多软件工程师实用特权

入驻
339
0

LR FAST-LIO

原创
05/13 14:22
阅读数 67568

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. 我们测试了很多.

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}\)\(\bar{x}_{k-1}\), 协方差是 \(\bar{P}_{k-1}\).

生成一个MAP:

\[\min _{\widetilde{\mathbf{x}}_{k}^{\kappa}}\left(\left\|\widetilde{\mathbf{x}}_{k}^{\kappa}\right\|_{\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}\right\|_{\mathbf{R}_{j}^{-1}}^{2}\right)\tag{17} \]

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

\[\begin{aligned} \mathbf{K} &=\mathbf{P} \mathbf{H}^{T}\left(\mathbf{H} \mathbf{P} \mathbf{H}^{T}+\mathbf{R}\right)^{-1} \\ \widetilde{\mathbf{x}}_{k}^{\kappa} &=-\mathbf{K} \mathbf{z}_{k}^{\kappa} \Longrightarrow \widehat{\mathbf{x}}_{k}^{\kappa+1}=\widehat{\mathbf{x}}_{k}^{\kappa} \oplus\left(-\mathbf{K} \mathbf{z}_{k}^{\kappa}\right) \end{aligned} \tag{18} \]

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

所以 \(\bar{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}\right)^{-1} \mathbf{H}^{T} \mathbf{R}^{-1} \tag{23} \]

D. Initialization

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

4. Experiment Result

A. Computational Complexity Experiments

发表评论

0/200
339 点赞
0 评论
收藏