Point-LIO： 一个能够应对剧烈运动的激光雷达惯性里程计

注： 如果在没有IMU的情况下使用Point-LIO，将imu_en设置为false，并在yaml文件中尽可能提供一个预定义的重力值，将gravity_init设置为true，并保持use_imu_as_input为0。

1.主要贡献

Point-LIO：一种具有高带宽和鲁棒性的 Lidar-Imu 里程计，能够估计极为激烈的机器人运动。Point-LIO有两个关键创新。

• 提出了一种逐点 LIO框架，该框架在实际采样时间融合激光雷达点，而不会累积到帧中。去除点累积消除了帧内运动失真，并允许以接近点采样率的高里程计输出和建图更新，这进一步使系统能够跟踪非常快的运动；
• 为了进一步提高系统带宽到超出IMU测量范围，用随机过程模型对IMU测量进行建模。将该模型扩展到系统运动学中，并将IMU测量值视为系统输出。即使IMU饱和，随机过程增强的运动学模型也可以平滑估计系统状态（包括角速度和线加速度）；
• 将这两个关键技术集成到一个完全紧耦合的LIO系统中，称为Point-LIO。系统使用流形扩展卡尔曼滤波器，通过在其各自的采样时间融合每个LiDAR点或IMU数据来更新系统状态。通过利用系统的稀疏性和线性，所开发的系统甚至在低功耗的基于高级精简指令集（ARM）计算机的微型无人飞行器（UAV）上也能实现实时状态估计；
• 开发的系统在多种挑战性的实际数据集上进行了测试，这些数据集来自一种具有非常小视场（FoV）的新兴固态激光雷达。结果表明，Point-LIO能够有效补偿运动畸变，提供高频率的里程计输出（4–8 kHz）和高带宽（>150Hz）。该系统还能够在极其激烈的运动下（角速度超过75rad/s）进行状态估计，即使在初期后IMU测量已经饱和。此外，在来自多个开源激光雷达数据集的12个序列上的详尽基准对比表明，Point-LIO在准确性和效率方面始终与其他同行系统相当，同时消耗的计算资源较少。最后，展示了该系统在实际无人机（UAV）上的应用。

2.系统框架

其设计理念主要基于:

1. LiDAR点是在相应的时间顺序采样的，而不是同时采样的帧;
2. IMU数据是系统的测量(输出)，而不是输入。

接收到每个测量值（即LiDAR点或IMU数据），就将其在流形扩展卡尔曼滤波器框架中融合。顺序采样的LiDAR点和IMU数据都用于在各自的时间戳更新状态，从而得到极高频率(4-8 kHz)的里程计输出。

具体来说，对于接收到的每个 LiDAR 点，都会从地图中搜索相应的平面。如果该点与由地图中的点拟合的平面匹配，则计算残差以使用流形卡尔曼滤波器更新系统状态。优化后的位姿最终将LiDAR点注册到全局框架中并合并到地图中，之后继续处理下一个测量值（激光雷达点或IMU数据）。否则，如果该点没有匹配的平面，则通过卡尔曼滤波器预测的位姿将其直接添加到地图中。

为了在接纳新注册点的同时快速进行平面对应搜索，我们使用了增量k-d树结构（ikd-Tree），该结构最初是在FAST-LIO2中开发的。对于每个IMU测量，我们分别进行每个通道的饱和检查，已饱和的通道将不用于状态更新。

3.状态估计

3.1符号说明

为了简化解释，我们采用以下符号表示法。

符号	含义
$x_k$	第 $k$ 次测量采样时刻的状态 $x$
$x$	状态 $x$ 的真实值
$\hat{x},\bar{x}$	传播和更新后的状态 $x$ 的值
$\delta x$	真实状态 $x$ 与其估计 $\hat{x}$ 之间的误差

然后是定义的广义加法和广义减法（fast-lio中已经多次接触过）
$$\left[\begin{array}{c}R\\ a\end{array}\right]\oplus \left[\begin{array}{c}r\\ b\end{array}\right]=\left[\begin{array}{c}R\oplus r\\ a+b\end{array}\right]=\left[\begin{array}{c}R\cdot \text{Exp}(r)\\ a+b\end{array}\right]$$$$\left[\begin{array}{c}{R}_1\\ a\end{array}\right]\ominus \left[\begin{array}{c}{R}_2\\ b\end{array}\right]=\left[\begin{array}{c}{R}_1\ominus R_2\\ a-b\end{array}\right]=\left[\begin{array}{c}\text{Log}(R_2^T\cdot R_1)\\ a-b\end{array}\right]\text{\hspace{1em}(1)}$$

3.2运动学模型

首先推导出系统模型，该模型由状态转移模型和测量模型组成。

状态转移模型

以 IMU 帧为 body 帧，第一个 IMU 坐标系为全局坐标系，连续运动学模型为
$$\begin{array}{l}{}^G{\dot{\mathbf{R}}}_I={}^G{\mathbf{R}}_I\lfloor {}^I\mathbf{\omega }\rfloor ,{}^G{\dot{\mathbf{p}}}_I={}^G{\mathbf{v}}_I,{}^G{\dot{\mathbf{v}}}_I={}^G{\mathbf{R}}_I{}^I\mathbf{a}+{}^G\mathbf{g},{}^G\dot{\mathbf{g}}=0\\ {\dot{\mathbf{b}}}_g={\mathbf{n}}_{{\mathrm{b}}_g},{\dot{\mathbf{b}}}_a={\mathbf{n}}_{{\mathrm{b}}_a},{}^I\dot{\mathbf{\omega }}={\mathbf{w}}_g,{}^I\dot{\mathbf{a}}={\mathbf{w}}_a\end{array}\text{\hspace{1em}(2)}$$
其中，

• ${}^G{\mathbf{R}}_I,{}^G{\mathbf{p}}_I,{}^G{\mathbf{v}}_I$ 分别表示全局坐标系中IMU的姿态、位置和速度；

• ${}^G\mathbf{g}$ 是全局坐标系中的重力矢量；

• $b_g$ 和 $b_a$ 是由高斯噪声驱动的随机游走IMU偏置，分别记为 $n_b\sim \mathcal{N}(0,Q_b)$ 和 $n_a\sim \mathcal{N}(0,Q_b)$;

• 符号 $\lfloor {}^{}\mathbf{a}\rfloor$ 表示 $a\in {\mathbb{R}}^3$ 中的反对称矩阵；

• ${}^I\omega$ 和 ${}^I\mathbf{a}$ 分别表示IMU在自身坐标系的角速度和加速度。（也就是测量值）

连续模型（2） 随后在每个测量步骤 $k$ 处进行离散化。设 $\Delta t_k$ 为当前的测量间隔，它是上一个测量（IMU数据或激光雷达点）与当前测量（IMU数据或激光雷达点）之间的时间差。通过假设输入在间隔 $\Delta t_k$ 内保持恒定,连续模型被离散化，得到：
$$x_{k+1}=x_k⊞\left(\Delta t_{k}f(x_k,w_k)\right)\text{\hspace{1em}(3)}$$
状态量 $x$，函数$f$，以及噪声$w$ 定义为：
$$\mathcal{M}\triangleq SO(3)\times {\mathbb{R}}^{21}\dim (\mathcal{M})=24$$$$\mathbf{x}\triangleq \left[\begin{array}{llllllll}{}^G{\mathbf{R}}_I& {}^G{\mathbf{p}}_I& {}^G{\mathbf{v}}_I& {\mathbf{b}}_g& {\mathbf{b}}_a& {}^G\mathbf{g}& {}^I\omega & {}^I\mathbf{a}\end{array}\right]$$$$\mathbf{w}\triangleq \left[\begin{array}{llll}{\mathbf{n}}_{b_g}& {\mathbf{n}}_{b_a}& {\mathbf{w}}_g& {\mathbf{w}}_a\end{array}\right]\approx \mathcal{N}(0,\mathcal{Q})$$$$\mathbf{f}(\mathbf{x},\mathbf{w})\triangleq \left[\begin{array}{llllllll}{}^I\mathbf{\omega }& {}^G{\mathbf{v}}_I& {}^G{\mathbf{R}}_I{}^I\mathbf{a}+{}^G\mathbf{g}& {\mathbf{n}}_{b_g}& {\mathbf{n}}_{b_a}& 0_{3\times 1}& {\mathbf{w}}_g& {\mathbf{w}}_a\end{array}\right]\text{\hspace{1em}(4)}$$

注：Point-LIO代码中的get_f_output 对应此处的 f(x,w)。get_f_input 对应Fast-LIO的 f(x,u,w)

观测模型

该系统有两个测量值，一个 LiDAR 点或一个 IMU 数据（包括角速度和加速度测量值）。这两个测量值通常在不同时间被系统采样和接收，因此我们分别对它们进行建模。

假设 LiDAR 坐标系与 IMU 坐标系重合或已预先校准了外部参数 (外参标定)，则LIDAR点 ${}^I{\mathbf{p}}_{{\mathrm{m}}_k}$ 等于在IMU 坐标系 ${}^I{\mathbf{p}}_k^{\mathrm{gt}}$的真实位置，受到高斯噪声的影响
$${}^I{\mathbf{p}}_{{\mathrm{m}}_k}={}^I{\mathbf{p}}_k^{\mathrm{gt}}+{\mathbf{n}}_{{\mathrm{L}}_k}\text{\hspace{1em}(5)}$$
在使用真实（未知）IMU 位姿 ${}^G{\mathbf{T}}_{{\mathrm{I}}_k}=({}^G{\mathbf{R}}_{{\mathrm{I}}_k},{}^G{\mathbf{p}}_{{\mathrm{I}}_k})$ 投影到全局坐标系，该真实点应恰好位于地图中的局部小平面块上(图2)，即：
$$0=\underset{{\mathbf{h}}_{\mathrm{L}}\left({\mathbf{x}}_k,{}^I{\mathbf{p}}_{{\mathrm{m}}_k},{\mathbf{n}}_{{\mathrm{L}}_k}\right)}{\underbrace{{}^G{\mathbf{u}}_k^T\left({}^G{\mathbf{T}}_{I_k}\left({}^I{\mathbf{p}}_{{\mathrm{m}}_k}-{\mathbf{n}}_{{\mathrm{L}}_k}\right){-}^G{\mathbf{q}}_k\right)}}\text{\hspace{1em}(6)}$$
其中，${}^G{\mathbf{u}}_{{}_k}$ 是相应平面的法向量，${}^G{\mathbf{q}}_{{}_k}$ 是平面上的任意点。

IMU测量由角速度测量${}^I{\omega }_{{}_m}$ 和 加速度测量${}^I{\mathbf{a}}_{{}_m}$ 组成：
$$\left[\begin{array}{c}{}^I{\mathbf{\omega }}_{{\mathrm{m}}_k}\\ {}^I{\mathbf{a}}_{{\mathrm{m}}_k}\end{array}\right]=\underset{{\mathbf{h}}_I\left({\mathbf{x}}_k,{\mathbf{n}}_{I_k}\right)}{\underbrace{\left[\begin{array}{c}{}^I{\mathbf{\omega }}_k+{\mathbf{b}}_{{\mathrm{g}}_k}+{\mathbf{n}}_{{\mathrm{g}}_k}\\ {}^I{\mathbf{a}}_k+{\mathbf{b}}_{{\mathrm{a}}_k}+{\mathbf{n}}_{{\mathrm{a}}_k}\end{array}\right]}}$$

综上所述， 系统的观测模型可以用以下紧凑的形式表示
$$0={\mathbf{h}}_L({\mathbf{x}}_k,{}^I{\mathbf{p}}_{{\mathrm{m}}_k},{\mathbf{n}}_{L_k})$$$$\left[\begin{array}{c}{}^I{\mathbf{\omega }}_{{\mathrm{m}}_k}\\ {}^I{\mathbf{a}}_{{\mathrm{m}}_k}\end{array}\right]={\mathbf{h}}_I({\mathbf{x}}_k,{\mathbf{n}}_{I_k})\text{\hspace{1em}(8)}$$

3.3扩展卡尔曼滤波器

紧耦合的 EKF 用于 Point-LIO 的状态估计

状态传播

假设我们已经接收到直到步骤 k 的测量，并且在该时间步的更新状态为 $\overline{x_k}$ ,同时更新了协方差矩阵$P_k$ .从步骤 k 到下一个测量步骤 k+1 的状态传播直接遵循状态转移模型（方程 (3)），并设置 $w_k$=0
$${\hat{\mathbf{x}}}_{k+1}={\overline{\mathbf{x}}}_k⊞\left(\Delta t_k\mathbf{f}\left({\overline{\mathbf{x}}}_k,0\right)\right)\text{\hspace{1em}(9)}$$
协方差传播如下：
$${\hat{\mathbf{P}}}_{k+1}={\mathbf{F}}_{{\mathbf{x}}_k}{\overline{\mathbf{P}}}_k{\mathbf{F}}_{{\mathbf{x}}_k}^T+{\mathbf{F}}_{{\mathbf{w}}_k}{\mathcal{Q}}_k{\mathbf{F}}_{{\mathbf{w}}_k}^T\text{\hspace{1em}(10)}$$

残差计算

LiDAR 测量： 使用从卡尔曼传播（方程(9)）得到的预测位姿 ${}^G{\hat{\mathbf{T}}}_{I_{k+1}}=\left({}^G{\hat{\mathbf{R}}}_{I_{k+1}},{}^G{\hat{\mathbf{p}}}_{I_{k+1}}\right)$ ,将测量得到的LIDAR点投影到全局坐标系${}^G{\hat{\mathbf{p}}}_{k+1}={}^G{\hat{\mathbf{R}}}_{I_{k+1}}{}^I{\mathbf{p}}_{m_{k+1}}+{}^G{\hat{\mathbf{p}}}_{I_{k+1}}$ ,并在由 kd-Tree 组成的地图中搜索其最近的五个点。

然后使用找到的最近邻点来拟合具有法向量和质心的局部小平面（如测量模型中所示，参见方程(6)和图2）。如果这五个最近点不在平面路径上（即，任何点距离平面大于 0.1 米），当前的 LiDAR 测量会直接合并到地图中，而无需残差计算或状态更新。否则，如果局部平面成功拟合，则根据方程(8)计算残差
$$\begin{array}{rl}{\mathbf{r}}_{{\mathrm{L}}_{k+1}}& =0-{\mathbf{h}}_{\mathrm{L}}\left({\hat{\mathbf{x}}}_{k+1},{}^I{\mathbf{p}}_{{\mathrm{m}}_{k+1}},0\right)\\ & ={\mathbf{h}}_{\mathrm{L}}\left({\mathbf{x}}_{k+1},{}^I{\mathbf{p}}_{{\mathrm{m}}_{k+1}},{\mathbf{n}}_{{\mathrm{L}}_{k+1}}\right)-{\mathbf{h}}_{\mathrm{L}}\left({\hat{\mathbf{x}}}_{k+1},{}^I{\mathbf{p}}_{{\mathrm{m}}_{k+1}},0\right)\\ & \approx {\mathbf{H}}_{{\mathrm{L}}_{k+1}}\delta {\mathbf{x}}_{k+1}+{\mathbf{D}}_{{\mathrm{L}}_{k+1}}{\mathbf{n}}_{{\mathrm{L}}_{k+1}}\end{array}\text{\hspace{1em}(12)}$$
IMU 测量：对于 IMU 测量，我们首先通过检查当前测量值与额定测量范围之间的差距来评估 IMU 的任何通道是否饱和。如果间隙太小，则丢弃该通道的IMU测量而不更新状态。然后，收集来自不饱和 IMU 通道的加速度和角速度测量值，以根据等式（7）计算 IMU 残差
$$\begin{array}{rl}{\mathbf{r}}_{I_{k+1}}& ={\left[\begin{array}{c}{}^I{\mathbf{\omega }}_{{\mathrm{m}}_{k+1}}\\ {}^I{\mathbf{a}}_{{\mathrm{m}}_{k+1}}\end{array}\right]}^T-{\mathbf{h}}_I\left({\hat{\mathbf{x}}}_{k+1},0\right)\\ & ={\mathbf{h}}_I\left({\mathbf{x}}_{k+1},{\mathbf{n}}_{I_{k+1}}\right)-{\mathbf{h}}_I\left({\hat{\mathbf{x}}}_{k+1},0\right)\\ & ={\mathbf{H}}_{I_{k+1}}\delta {\mathbf{x}}_{k+1}+{\mathbf{D}}_{I_{k+1}}{\mathbf{n}}_{I_{k+1}}\end{array}\text{\hspace{1em}(14)}$$

状态更新

更新后的状态与协方差矩阵一起用于下一个测量的传播，状态估计整体过程在算法1中进行了总结