【LOAM系列】七：Fast-Lio论文笔记

Fast-Lio

2021.2 IEEE Robotics and Automation Letters Wei Xu 港大火星实验室

1、论文

1.1 系统框架

• 雷达输入到预处理模块,经过一段时间的累积后进行特征提取,提取出面点和角点
• IMU 输入进行前向传播 (积分得到粗略位姿估计),反向传播进行运动补偿
• 计算雷达里程计的残差,利用迭代卡尔曼滤波估计位姿变换,直到收敛。最后根据位姿建图,并更新特征地图。

缺陷：为了防止构建地图的k-d树的时间不断增加，该系统只能在较小的环境中工作，虽然使用卡尔曼增益新的计算公式可以进行scan-to-map的匹配，但是随着地图的增大map的维护无法保证实时性

运算符号定义

$$\begin{array}{r}\begin{array}{rl}& ⊞:\mathcal{M}\times {\mathbb{R}}^n\to \mathcal{M};\boxminus :\mathcal{M}\times \mathcal{M}\to {\mathbb{R}}^n\\ & \mathcal{M}=SO(3):\mathbf{R}⊞\mathbf{r}=\mathbf{R}\mathrm{Exp}(\mathbf{r});{\mathbf{R}}_1\boxminus {\mathbf{R}}_2=\mathrm{Log}({\mathbf{R}}_2^{\mathrm{T}}{\mathbf{R}}_1)\\ & \mathcal{M}={\mathbb{R}}^n:\mathbf{a}⊞\mathbf{b}=\mathbf{a}+\mathbf{b};\mathbf{a}\boxminus \mathbf{b}=\mathbf{a}-\mathbf{b}\end{array}\end{array}$$

关于流型的解释

流形学的观点是认为:我们所能观察到的数据实际上是由一个低维流形映射到高维空间上的,即这些数据所在的空间是“嵌入在高维空间的低维流形”。由于数据内部特征，用高维空间表示低维流型会产生维度上的冗余，只需要较低的维度就可以位移表示该维度下的流行

1.2 数据预处理

imu预积分

imu物理模型
$$\begin{array}{c}\begin{array}{rl}{}^G{\dot{\mathbf{p}}}_I& ={}^G{\mathbf{v}}_I,{}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{R}}_I\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}-{\mathbf{n}}_{\mathbf{a}}\right)+{}^G\mathbf{g},{}^G\dot{\mathbf{g}}=0\\ {}^G{\dot{\mathbf{R}}}_I& ={}^G{\mathbf{R}}_I\lfloor {\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}{\rfloor }_{\wedge },{\dot{\mathbf{b}}}_{\mathbf{\omega }}={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }},{\dot{\mathbf{b}}}_{\mathbf{a}}={\mathbf{n}}_{\mathbf{ba}}\end{array}\end{array}\text{\hspace{1em}(1)}$$
IMU的状态传播，其中$f{\delta }_t$对应着帧内的积分
$$\begin{array}{c}\begin{array}{rl}{\mathbf{x}}_{i+1}& ={\mathbf{x}}_i⊞\left(\Delta t\mathbf{f}({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i)\right)\end{array}\end{array}\text{\hspace{1em}(2)}$$

$$\begin{array}{rl}\mathcal{M}& =SO(3)\times {\mathbb{R}}^{15},\text{dim}(\mathcal{M})=18\\ \mathbf{x}& \doteq {\left[\begin{array}{cccccc}{}^G{\mathbf{R}}_I^T& {}^G{\mathbf{p}}_I^T& {}^G{\mathbf{v}}_I^T& {\mathbf{b}}_{\mathbf{\omega }}^T& {\mathbf{b}}_{\mathbf{a}}^T& {}^G{\mathbf{g}}^T\end{array}\right]}^T\in \mathcal{M}\\ \mathbf{u}& \doteq {\left[\begin{array}{cc}{\mathbf{\omega }}_m^T& {\mathbf{a}}_m^T\end{array}\right]}^T,\mathbf{w}\doteq {\left[\begin{array}{cccc}{\mathbf{n}}_{\mathbf{\omega }}^T& {\mathbf{n}}_{\mathbf{a}}^T& {\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}^T& {\mathbf{n}}_{\mathbf{ba}}^T\end{array}\right]}^T\\ \mathbf{f}& \left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)=\left[\begin{array}{c}{\mathbf{\omega }}_{m_i}-{\mathbf{b}}_{{\mathbf{\omega }}_i}-{\mathbf{n}}_{{\mathbf{\omega }}_i}\\ {}^G{\mathbf{v}}_{I_i}\\ {}^G{\mathbf{R}}_{I_i}\left({\mathbf{a}}_{m_i}-{\mathbf{b}}_{{\mathbf{a}}_i}-{\mathbf{n}}_{{\mathbf{a}}_i}\right)+{}^G{\mathbf{g}}_i\\ {\mathbf{n}}_{\mathbf{b}{\mathbf{\omega }}_i}\\ {\mathbf{n}}_{\mathbf{b}{\mathbf{a}}_i}\\ 0_{3\times 1}\end{array}\right]\end{array}\text{\hspace{1em}(3)}$$

优化的状态量是18维的，包含了世界坐标系下的重力向量

雷达特征提取

1. 使用Livox的固态雷达，20W个点/s，每20ms取一次扫描点集作为一次scan
2. 与LOAM一样提取平面点和边缘点，记录每个特征点的时间戳，最后一个特征点是一次 scan 的终点

1.3 误差卡尔曼滤波

使用ESKF完成运动方程的线性化，

卡尔曼滤波的时间复杂度$\mathcal{O}(m^2)$，m是测量的维度

$x$表示真值，$\tilde{\mathbf{x}}$表示误差，$\bar{\mathbf{x}}$表示最优估计值(后验)，$\hat{\mathbf{x}}$表示先验值

第k-1帧(已经完全优化完了)的误差：误差值 = 真值 “—” 优化完的估计值
$$\begin{array}{r}{\tilde{\mathbf{x}}}_{k-1}\doteq {\mathbf{x}}_{k-1}\boxminus {\bar{\mathbf{x}}}_{k-1}={\left[\begin{array}{cccccc}\delta {\mathbf{\theta }}^T& {}^G{\tilde{\mathbf{p}}}_I^T& {}^G{\tilde{\mathbf{v}}}_I^T& {\tilde{\mathbf{b}}}_{\mathbf{\omega }}^T& {\tilde{\mathbf{b}}}_{\mathbf{a}}^T& {}^G{\tilde{\mathbf{g}}}^T\end{array}\right]}^T\end{array}$$

误差状态的前向传播

通过每两个雷达帧之间的IMU预积分得到一个$x_k$的估计值，并使用估计值进行运动补偿
$$\begin{array}{c}\begin{array}{rl}{\hat{\mathbf{x}}}_{i+1}& ={\hat{\mathbf{x}}}_i⊞\left(\Delta t\mathbf{f}({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0)\right);{\hat{\mathbf{x}}}_0={\bar{\mathbf{x}}}_{k-1}.\end{array}\end{array}\text{\hspace{1em}(4)}$$
线性化的误差的传递：
$$\begin{array}{rl}{\tilde{\mathbf{x}}}_{i+1}& \simeq {\mathbf{F}}_{\tilde{\mathbf{x}}}{\tilde{\mathbf{x}}}_i+{\mathbf{F}}_{\mathbf{w}}{\mathbf{w}}_i.\end{array}\text{\hspace{1em}(5)}$$
推导F矩阵有两种方式，基于误差随时间变化的递推方程，和基于一阶泰勒展开的误差传递方法，最后推导出来的公式
$$\begin{array}{r}{\mathbf{F}}_{\tilde{\mathbf{x}}}=\left[\begin{array}{cccccc}\text{Exp}\left(-{\hat{\mathbf{\omega }}}_i\Delta t\right)& 0& 0& -\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0\\ 0& \mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0\\ {-}^G{\hat{\mathbf{R}}}_{I_i}\lfloor {\hat{\mathbf{a}}}_i{\rfloor }_{\wedge }\Delta t& 0& \mathbf{I}& 0& -{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& \mathbf{I}\Delta t\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right],{\mathbf{F}}_{\mathbf{w}}=\left[\begin{array}{cccc}-\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0& 0\\ 0& 0& 0& 0\\ 0& -{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& 0& 0\\ 0& 0& \mathbf{I}\Delta t& 0\\ 0& 0& 0& \mathbf{I}\Delta t\\ 0& 0& 0& 0\end{array}\right]\end{array}\text{\hspace{1em}(7)}$$
协方差的传播
$$\begin{array}{c}\begin{array}{r}{\hat{\mathbf{P}}}_{i+1}={\mathbf{F}}_{\tilde{\mathbf{x}}}{\hat{\mathbf{P}}}_i{\mathbf{F}}_{\tilde{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T;{\hat{\mathbf{P}}}_0={\bar{\mathbf{P}}}_{k-1}.\end{array}\end{array}\text{\hspace{1em}(8)}$$

反向传播，雷达运动补偿

目的是每个IMU时刻的积分相对于此帧点云结束时刻的位姿

先把 L 坐标系的特征点转到 I 坐标系，然后乘上反向传播的补偿矩阵,再转回 L 坐标系。所有的投影点都像这样转好后,就可以开始计算残差了。计算残差的时候把补偿后的特征点投影到世界坐标系
KaTeX parse error: Got group of unknown type: 'internal'
把点畸变矫正投影到这个雷达帧的结束时刻，然后在投影到世界坐标系，待估计位姿${}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }$
$$\begin{array}{c}\begin{array}{r}{}^{L_k}{\mathbf{p}}_{f_j}={}^I{\mathbf{T}}_L^{-1}{}^{I_k}{\check{\mathbf{T}}}_{I_j}{}^I{\mathbf{T}}_L{}^{L_j}{\mathbf{p}}_{f_j},\end{array}\end{array}\text{\hspace{1em}(10)}$$
$$\begin{array}{r}{}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }={}^G{\hat{\mathbf{T}}}_{I_k}^{\kappa }{}^I{\mathbf{T}}_L{}^{L_k}{\mathbf{p}}_{f_j};j=1,\dots ,m.\end{array}\text{\hspace{1em}(11)}$$

残差计算就是点到线的距离和点到面的距离，(fast-lio2 以及代码中,都省去了特征提取,只计算面点的残差)

1.4 IEKF迭代卡尔曼滤波

IEKF 可以证明和高斯牛顿 GN 法是等价的，IEKF迭代过程不会更新协方差矩阵 P，但是文章中更新了，由于每迭代一次后 $\tilde{x}$就会发生一次变化,理论上其协方差矩阵并不是初始的 $P_k$ 了,因此协方差矩阵可通过 $P=(J^k{)}^{-1}{\hat{P}}_k(J^k{)}^{-T}$ 在迭代过程中更新。
$$\begin{array}{c}\begin{array}{r}{\mathbf{J}}^{\kappa }=\left[\begin{array}{cc}\mathbf{A}{\left({}^G{\hat{\mathbf{R}}}_{I_k}^{\kappa }\boxminus {}^G{\hat{\mathbf{R}}}_{I_k}\right)}^{-T}& 0_{3\times 15}\\ 0_{15\times 3}& {\mathbf{I}}_{15\times 15}\end{array}\right]\end{array}\end{array}\text{\hspace{1em}(16)}$$
求解MAP问题(一个最小二乘问题)
$$\begin{array}{r}\underset{{\tilde{\mathbf{x}}}_k^{\kappa }}{\min }\left(\Vert {\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k{\Vert }_{{\hat{\mathbf{P}}}_k^{-1}}^2+\sum _{j=1}^m\Vert {\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }{\Vert }_{{\mathbf{R}}_j^{-1}}^2\right)\end{array}\text{\hspace{1em}(17)}$$
计算卡尔曼增益，H是观测残差关于状态量扰动的偏导，R是观测的协方差，P是ESKF推导出来的运动方程的协方差，$z_k$就是每个点匹配的残差
$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{H}}^T(\mathbf{H}\mathbf{P}{\mathbf{H}}^T+\mathbf{R}{)}^{-1},\\ {\hat{\mathbf{x}}}_k^{\kappa +1}& ={\hat{\mathbf{x}}}_k^{\kappa }⊞\left(-\mathbf{K}{\mathbf{z}}_k^{\kappa }-(\mathbf{I}-\mathbf{KH})({\mathbf{J}}^{\kappa }{)}^{-1}\left({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k\right)\right).\end{array}\text{\hspace{1em}(18)}$$
论文给出了一种求卡尔曼增益的等效方式，前面K计算过程中矩阵求逆的维度是观测的维度，后面K的计算过程矩阵求逆的维度就是状态量的维度
$$\begin{array}{c}\begin{array}{r}\mathbf{K}={\left({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}\right)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}.\end{array}\end{array}\text{\hspace{1em}(20)}$$
H矩阵：

经过k次迭代后得到最终的结果
$$\begin{array}{c}\begin{array}{rl}{\bar{\mathbf{x}}}_k& ={\hat{\mathbf{x}}}_k^{\kappa +1},{\bar{\mathbf{P}}}_k=\left(\mathbf{I}-\mathbf{KH}\right)\mathbf{P}\end{array}\end{array}\text{\hspace{1em}(19)}$$

地图更新：使用迭代之后的位姿将地图点投影到全局世界坐标系，初始化静止2s，初始化IMU零偏和重力向量

1.5 实验效果

32 米轨迹上的漂移为 0.08 米。漂移小于 0.05%(140 米轨迹上的漂移为 0.07 米),10HZ, 平均处理时间为 25ms，平均有效特征点为 1497 个