FAST-LIO论文翻译

2023-05-16

摘要

本文提出了一个计算效率较高和鲁棒的激光-惯性里程计框架。我们使用基于紧耦合的迭代扩展卡尔曼滤波器将LiDAR特征点与IMU数据融合在一起，以便在发生退化的快速运动，嘈杂或杂乱环境中实现稳健的导航。为了在存在大量测量值的情况下降低计算负载，我们提出了一个计算卡尔曼增益的新公式。新公式的计算负载取决于状态维度而不是测量维度。提出方法及其实现在各种室内和室外环境下进行了测试。在所有测试中，我们的方法都能实时产生可靠的导航结果：在机载四旋翼飞行器上运行时，它在一个scan中融合了多达1200个有效特征点，并在25毫秒内完成iEKF步骤的所有迭代。我们的程序在Github上已经开源了。

1 介绍

2 相关工作

3 方法

A 框架概览

本文将使用表I中所示的符号。我们的工作流程概述如图 2 所示。将激光雷达的原始点云输入特征提取模块以获得平面和边缘特征。然后将提取的特征和IMU测量值输入到我们的状态估计模块中用于 10Hz−50Hz 时的状态估计。然后，估计的姿势将特征点对齐到全局坐标系下，并将它们与之前构建的特征点地图合并。更新后的地图最终用于在下一步中对齐更多的点。

B 系统描述

1） $\oplus and \ominus$ 操作符

设M是维数为n的流形（例如，M=SO（3））。由于流形局部homeomorphic to $R_{n}$ ，我们可以通过两个封装算子 $\oplus and \ominus$ 【23】建立从流形M的局部邻域到其切空间Rn的双射映射。

其中 $Exp(r)=I + \frac{r}{\left \| r \right \|}sin(\left \| r \right \|)+\frac{r^{2}}{\left \| r \right \|}^{2}(1-cos(\left \| r \right \|))$ 是指数映射【23】，对数（·）是其逆映射。对于一个compound manifold $M=SO^{3}*R^{n}$ ，有以下：

对于上面的定义，可以容易验证如下：

2）连续模型

假设IMU与激光雷达之间是刚性连接，另外它们之间的外参是已知的 ${_{}^{I}\textrm{T}}_{L}=({_{}^{I}\textrm{R}}_{L}, {_{}^{I}\textrm{P}}_{L})$ 。令IMU坐标系为机体坐标系（以符号I表示），可以推导出下面的运动学模型：

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

3) 离散模型

基于上述定义的 $\oplus$ ，我们基于采样时间 $\Delta t$ 使用零阶保持器对方程（1）进行离散。离散结果模型如下：

其中i是IMU测量值的索引，函数f，状态x，输入u和噪声w等的定义如下：

4）激光雷达测量值的预处理

激光雷达测量值是lidar局部坐标系中的点。由于激光雷达以非常高的速率采样（例如200kHz），因此通常不可能在接收到每个新点后就进行处理。一种更实用的方法是将这些点积累一段时间，然后一次性处理。在FAST-LIO中，最小累积间隔设置为20 ms，导致高达50 Hz的全状态估计（即里程计输出）和地图更新，如图2（a）所示。这种累积的点集称为scan，处理它的时间表示为tk（见图2（b））。我们从原始点中提取局部平滑度高的平面点【8】和局部平滑度低的边缘点，如【10】所示。假设特征点的数量为m，特征点的采样时间 $p_{j}{\epsilon [t_{k-1}, t_{k-1}]}$ ，特征点表示为 $_{}^{L_{j}}\textrm{P}_{f_{j}}$ ，其中 $L_{j}$ 是时间 $p_{j}$ 处的激光雷达局部坐标系。在此期间，有很多的IMU测量值，其采样时间 $\tau _{j}{\epsilon [t_{k-1}, t_{k-1}]}$ ，对应的系统状态为 $x_{k}$ 。需要注意的是，最后一个激光雷达特征点是scan的终点，即 $p_{m}=t_{k}$ ，而IMU测量值可能不一定与扫描的起点或终点对齐。

C 状态估计

为估计状态方程（2）中的状态，我们使用了一个迭代扩展卡尔曼滤波。此外，同时，采用【23，24】中的方法刻画了状态估计的切线空间中的估计协方差。假设上一激光帧在 $t_{k-1}$ 的最优状态是 $\bar{x_{k-1}}$ ，另外协方差矩阵是 $\bar{p_{k-1}}$ 。 $\bar{p_{k-1}}$ 表示如下定义的随机变量的协方差：

其中 $\delta \theta =log(^{G}\bar{R}_{I}^{T}*_{}^{G}\textrm{}R_{I}^{T})$ 代表了姿态误差，剩余部分是附加误差（the error in the estimate of a $\bar{x}$ quantity x is $\tilde{x}=x-\bar{x}$ ）。直观上，姿态误差 $\delta \theta$ 直观地描述了真实姿态与估计姿态之间的(小)偏差。这种误差定义的主要优点是，它允许我们用3×3协方差矩阵 $E({\delta \theta\delta \theta^{T}})$ 表示姿态的不确定性。由于姿态有3个自由度(DOF)，这是一个最小表示。

C1 前向传播

一接收到IMU数据，就执行前向传播（如图2所示）。更具体而言，状态按照方程（2）前向传播（并设置过程噪声 $w_{i}$ 为零）。

其中 $\Delta t=t_{i+1}-t_{i}$ 。为了传递协方差，我们使用如下的误差状态动态模型：

其中方程（5）中的矩阵 $F_{\tilde{w}}$ 和 $F_{w}$ 在接下来的附录中计算。结果如（7）所示，其中 $\widehat{w_{i}}=w_{m_{i}}-\widehat{b_{w_{i}}}$ ， $\widehat{a_{i}}=a_{m_{i}}-\widehat{b_{a_{i}}}$ ， $A_{u}^{-1}$ 则沿袭【25】中的如下定义：

表示高斯白噪声w的协方差为Q，然后前向传播后的协方差 $\widehat{P_{i}}$ 可以根据下面的等式进行迭代更新。

前向传播一直继续直到到达激光帧的帧尾时间 $t_{k}$ ，其中前向传播后的状态和协方差以符号 $\widehat{P_{k}}$ 和 $\widehat{x_{k}}$ 表示。其中的协方差代表了真实状态 $x_{k}$ 和传播后的状态 $\widehat{x_{k}}$ 之间的误差（ $x_{k}\ominus \widehat{x_{k}}$ ）。

C2 反向传播和运动补偿

当点云累积时间间隔到达时时间 $t_{k}$ ，the new scan的特征点应该与传递的状态 $\widehat{x_{k}}$ 和协方差 $\widehat{P_{k}}$ 进行融合，从而产生一个最优的状态。然而，尽管the new scan的时间戳是 $t_{k}$ ，但特征点的实际采样时间 $p_{i}<t_{k}$ （参考部分III-B.4 和图.2(b)所示），这会导致不匹配（基于body坐标系下）。

为了补偿时间 $t_{i}$ 和时间 $t_{j}$ 相对运动（也即是运动畸变），我们按照方程（2）进行反向传播（ $\check{x}_{j-1}=\check{x}_{j}\oplus (-\Delta _{t}f(\check{x}_{j}, u_{j}, 0))$ ），starting from zero pose and rests states (速度和零偏) from $\widehat{x_{k}}$ 。反向传播按照特征点的频率进行，因而其比IMU的频率高很多。对于在两个IMU测量值之间的所有特征点，我们使用左侧的IMU测量值作为反向传播的输入。进一步，需要注意的是 $f(\check{x}_{j}, u_{j}, 0)$ 的the last three block elements(对应于陀螺仪零偏，加速度零偏和外参)是零，反向传播可以被简化为下面方程：

其中 $p_{j-1}\in [t_{i-1}, t_{i}]$ ， $\Delta t=p_{i}-p_{j-1}$ 以及s.f.代表了"starting form"。

反向传播会产生时间 $p_{j}$ 和帧尾时间 $t_{k}$ 之间的一个相对位姿， $_{}^{I_{k}}\textrm{}\check{T}_{I_{j}}=(_{}^{I_{k}}\textrm{}\check{R}_{I_{j}}, _{}^{I_{k}}\textrm{}\check{P}_{I_{j}})$ 。这个相对位姿可以保证我们把局部测量值 $_{}^{I_{k}}\textrm{}\check{p}_{f_{j}}$ 映射到帧尾测量值 $_{}^{L_{k}}\textrm{}\check{p}_{f_{j}}$ （如图2所示）。

其中 $_{}^{I}T_{l}$ 是已知的外参（参考部分III-B.2）。变换后的点云 $_{}^{L_{k}}\textrm{}\check{p}_{f_{j}}$ 在接下来的章节用于构建残差。

C3 残差计算

使用方程（10）进行运动补偿后，我们可以把一帧所有的特征点 $_{}^{L_{k}}p_{f_{j}}$ 认为是同一个时刻 $t_{k}$ 采样的，并用它构建残差。假设迭代卡尔曼滤波的当前迭代次数是k，并且相应的估计状态是 $\widehat{x}_{k}^{k}$ 。当k=0的时候， $\widehat{x}_{k}^{k}=\widehat{x}_{k}$ ，the predicted state from the propagation in (4)。然后，特征点 $_{}^{L_{k}}p_{f_{j}}$ 通过如下的方程变换到全局坐标系下：

对于每个激光雷达特征点，最近的平面和边（where the point truly belongs to）是由地图中它的最近特征点定义的。这就是说，残差是全局坐标系下特征点 $_{}^{G}\widehat{p}_{fj}^{K}$ 和地图中它最近的平面(边)之间的距离。以 $u_{j}$ 代表对应平面或者对应边的法向量，对于其上的点 $_{}^{G}q_{j}$ ，残差的计算公式 $z_{j}^{k}$ 如下：

其中 $G_{j}=u_{j}^{T}$ (对于平面特征来说)， $G_{j}=\left | |u_{j}^{T} \right | _{\wedge }$ （对于边特征来说）。The computation of the $u_{j}$ and the search of nearby points in the map，其定义了相应的平面和边，is achieved by building a KD-tree of the points in the most recent map【10】。甚至，我们只考虑模低于0.5m的残差。超过这个阀值的残差要么是外点，要不就是新观测的点。

C4 迭代状态更新

为了融合方程（12）中计算的残差 $z_{j}^{k}$ （其中基于IMU的数据前向传播，预测的状态是 $\widehat{x_{k}}$ ，协方差是 $\widehat{P_{k}}$ ），我们需要线性化测量模型 $z_{j}^{k}$ ，并将the ground-truth的状态 $x_{k}$ 、测量噪声与残差关联起来。

测量噪声来源于激光雷达的距离和beam-directing噪声 $_{}^{n_{j}}L_{j}$ (测量点是 $_{}^{L_{j}}\textrm{}p_{f_{j}}$ )。从测量值 $_{}^{L_{j}}\textrm{}p_{f_{j}}$ 移除这个噪声，则该点的真正位置是:

This true point，通过（10）映射到 $L_{k}$ 坐标系下，然后通过the ground-truth state $x_{k}$ （位姿）映射到全局坐标系下，应该准确的位于地图中的平面或者边。也就是，把方程（13）代入到（10），然后代入到（11），并且进一步代入到（12），其应该为零。

对于上述方程在进行一阶线性展开 $\widehat{x}_{k}^{k}$ ，也就是：

其中 $\widetilde{{x}_{k}^{k}}=x_{k}\ominus \widehat{x}_{k}^{k}$ （或者等价与 $x_{k}=\widetilde{{x}_{k}^{k}}\oplus \widehat{x}_{k}^{k}$ ）， $H_{j}^{k}$ 是 $h_{j}$ 的雅可比矩阵（ $\widetilde{{x}_{k}^{k}}\oplus \widehat{x}_{k}^{k}$ ， $_{}^{n_{j}}L_{j}$ ）with respect to $\widetilde{{x}_{k}^{k}}$ ，等于零，并且 $v_{j}\in N(0, R_{j})$ 来自于测量噪声 $_{}^{L_{j}}\textrm{}p_{f_{j}}$ 。

注意到 $x_{k}$ 的先验来自于前向传播（章节III-C.1中），也就是如下：

其中 $J_{k}$ 是 $\widetilde{{x}_{k}^{k}}\oplus \widehat{x}_{k}^{k}\ominus \widehat{x_{k}}$ （基本等于零）在 $\widetilde{{x}_{k}^{k}}$ 的偏微分。

其中 $A(\cdot )^{-1}$ 在方程（6）中定义。对于第一次迭代（以扩展卡尔曼滤波为例）， $\widetilde{{x}_{k}^{k}}= \widehat{x_{k}}$ ，其中 $J^{k}=I$ 。

联合（15）中的先验分布和（14）中的后验分布产生了一个最大后验估计问题（MAP）：

其中 $\left \| x \right \|_{M}^{2}=x^{T}Mx$ 。将（15）中线性化的先验代入到（17）中，然后优化得到的二次代价函数，等于进行标准迭代卡尔曼滤波【21】；计算如下（为了简化符号，我们令 $H=[{H_{1}^{k}}^{T}, ...,{H_{m}^{k}}^{T}]$ ， $R=diag({R_{1},...,R_{m}})$ ， $P=((J_{k}^{-1}\widehat{P_{k}}J_{k}))^{-T}$ ， $Z_{k}^{k}=[(Z_{1}^{k})^{T},... (Z_{m}^{k})^{T}]$ ）。

其中更新后的状态 $\widetilde{{x}_{k}^{k}}$ 接下来用于计算章节III-C.3的残差，并重复上述过程直到收敛（ $\left \| \widehat{x}_{k}^{k+1}\ominus \widehat{x}_{k}^{k} \right \|<s$ ）。收敛后，最优的状态和协方差是：

使用（18）中通用的卡尔曼增益公式存在一个问题就是它需要变换如下矩阵 $HP^{T}H+R$ (其在测量的维度)。在实际中，激光雷达的特征点在数量上是非常大的，变换这样大数量的一个矩阵是不太可能的。As such ，当前的工作如【21、26】仅仅使用了测量值的一小部分。在本文中，我们展示了这种限制是可以被避免的。直觉源自（17），其中的代价函数is over the state，因此应根据状态维度计算复杂度。实际上，如果直接求解（17），我们可以获得与（18）的相同解，不过我们需要一个新的卡尔曼增益公式如下所示：

我们证明附录B中两种卡尔曼增益的形式事实上是相同的（基于matrix inverse lemma【27】）。由于激光雷达的测量值是独立的，矩阵R的协方差（黑色的）是对角矩阵，因子新的卡尔曼增益公式只需要在状态的维度而不是测量的维度上变换两个矩阵。新的卡尔曼增益公式极大的节约了计算量（由于状态的维度在LIO通常比观测的维度低很多），对于10hz的激光雷达来说，在一帧中有超过1000个有效的特征点，而同时状态的维度只有18）。

C5 算法

我们的状态估计总结在算法1中。

D 地图更新

当状态更新为 $\bar{x_{k}}$ 后（也就是 $^{G}\bar{T_{I_{k}}}=(^{G}\bar{R_{I_{k}}},^{G}\bar{p_{I_{k}}})$ ），body坐标系下的每个特征点 $^{L_{k}}{p_{f_{j}}}$ 通过下面的方程变换到全局坐标系下：

这些特征点最终被添加到的现有地图中（包含前面所有步骤中的特征点）。

E 初始化

系统需要进行初始化，以获得系统状态的良好初始估计(例如重力加速度 $^{G}\textrm{g}$ 、零偏和噪声协方差)，以便加速状态估计。在FAST-LIO中，初始化很简单，只需要保持LiDAR静态数秒(本文所有实验均为2秒)，然后利用收集到的数据初始化IMU零偏和重力加速度。如果激光雷达支持非重复扫描(如Livox AVIA)，保持静态也允许激光雷达捕获初始的高分辨率地图，这对后续导航非常有帮助。

4 实验

5 结论

6 附录

A 计算 $F_{\tilde{x}}$ 和 $F_{w}$

回忆一下： $x_{i}=\hat{x_{i}}\oplus \tilde{x_{i}}$ ，表示 $g(\tilde{x_{i}, w_{i})=f(x_{i}, u_{i}, w_{i})\bigtriangleup _{t}=f(*, u_{i}, w_{i})\bigtriangleup _{t}$ ，然后误差状态方程（5）可以重写为如下：

根据偏微分的链式法则，方程（5）中的 $F_{\tilde{x}}$ 和 $F_{w}$ 计算如下：

B 卡尔曼增益计算公式

基于矩阵求逆的lemma【27】，我们可以得到：

把上式代入到（20），我们可以得到：

我们注意到存在这个方程：

把上述方程带入上面，我们可以得到标准的卡尔曼增益公式（18），如下所示：

参考文献

[1] K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 965–972, 2018.
[2] 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.
[3] 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.
[4] D. Wang, C. Watkins, and H. Xie, “Mems mirrors for lidar: A review,” Micromachines, vol. 11, no. 5, p. 456, 2020.
[5] 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.

[6] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” in Sensor fusion IV: control paradigms and data structures, vol. 1611. International Society for Optics and Photonics, 1992, pp. 586–606.
[7] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics: science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435.
[8] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in realtime.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
[9] T. Shan and B. Englot, “Lego-loam: Lightweight and groundoptimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758–4765.
[10] 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.
[11] W. Zhen, S. Zeng, and S. Soberer, “Robust localization and localizability estimation with a rotating laser scanner,” in 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp. 6240–6245.
[12] Y. Balazadegan Sarvrood, S. Hosseinyalamdary, and Y. Gao, “Visuallidar odometry aided by reduced imu,” ISPRS international journal of geo-information, vol. 5, no. 1, p. 3, 2016.
[13] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “Lic-fusion: Lidarinertial-camera odometry,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019, pp. 5848–5854.
[14] P. Geneva, K. Eckenhoff, Y. Yang, and G. Huang, “Lips: Lidarinertial 3d plane slam,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 123–130.
[15] 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.
[16] M. Hsiao, E. Westman, and M. Kaess, “Dense planar-inertial slam with structural constraints,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 6521–6528.
[17] 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.
[18] A. Bry, A. Bachrach, and N. Roy, “State estimation for aggressive flight in gps-denied environments using onboard sensing,” in 2012 IEEE International Conference on Robotics and Automation. IEEE, 2012, pp. 1–8.
[19] J. A. Hesch, F. M. Mirzaei, G. L. Mariottini, and S. I. Roumeliotis, “A laser-aided inertial navigation system (l-ins) for human localization in unknown indoor environments,” in 2010 IEEE International Conference on Robotics and Automation. IEEE, 2010, pp. 5376–5382.
[20] Z. Cheng, D. Liu, Y. Yang, T. Ling, X. Chen, L. Zhang, J. Bai, Y. Shen, L. Miao, and W. Huang, “Practical phase unwrapping of interferometric fringes based on unscented kalman filter technique,” Optics express, vol. 23, no. 25, pp. 32 337–32 349, 2015.
[21] 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.
[22] M. Raitoharju and R. Piche, “On computational complexity reduction ´methods for kalman filter extensions,” IEEE Aerospace and Electronic Systems Magazine, vol. 34, no. 10, pp. 2–19, 2019.
[23] C. Hertzberg, R. Wagner, U. Frese, and L. Schroder, “Integrating ¨ generic sensor fusion algorithms with sound state representations through encapsulation of manifolds,” Information Fusion, vol. 14, no. 1, pp. 57–77, 2013.
[24] W. Xu, D. He, Y. Cai, and F. Zhang, “Robots state estimation and observability analysis based on statistical motion models,” 2020.
[25] F. Bullo and R. M. Murray, “Proportional derivative (pd) control on the euclidean group,” in European control conference, vol. 2, 1995, pp. 1091–1097.
[26] M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended kalman filter based visual-inertial odometry using direct photometric feedback,” The International Journal of Robotics Research, vol. 36, no. 10, pp. 1053–1072, 2017.
[27] N. J. Higham, Accuracy and stability of numerical algorithms. SIAM, 2002.

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