Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry
摘要
一种鲁棒、高带宽的光探测和测距(LiDAR)惯性里程计，具有估计极激进的机器人运动的能力。Point-LIO有两个关键的新颖之处，第一个是逐点LIO框架，更新与每个LiDAR点测量对应的状态，该框架允许极高频率的里程表输出，显著增加了里程表带宽，并从根本上消除了人为的帧内运动畸变；第二个是随机过程增强的运动学模型，将IMU测量作为输出建模。这种新的建模方法即使在惯性测量单元(IMU)测量在运动中间饱和的情况下，也能对猛烈运动进行准确的定位和可靠的建图。
各种现实世界的实验进行了性能评估。总体而言，Point-LIO能够在超出IMU测量范围的剧烈振动和高角速度(75 rad/s)的剧烈运动下提供准确的高频里程计(4-8 kHz)和可靠的测绘。此外，还进行了详尽的基准比较。Point-LIO始终如一地达到相当的精度和时间消耗。最后，给出了Point-LIO的两种应用实例，一种是竞速无人机，另一种是自旋无人机，两者都具有激进的运动。
介绍
现有畸变矫正方法以及局限性
系统概览
我们的设计理念是真实地认识到:1)LiDAR点是在各自时间按顺序采样的，而不是同时采样的帧;2)IMU数据是系统的测量数据，而不是系统的输入。一旦接收到各自的测量值(每个LiDAR点或IMU数据)，我们将这两个测量值融合在一个流形EKF框架[55]中。
我们设计的系统的概述如图1所示;顺序采样的LiDAR点和IMU数据都用于在各自的时间戳更新状态，从而导致极高速率的里程计输出，即在实践中为4-8 kHz。特别是，对于接收到的每个LiDAR点，从地图中搜索相应的平面。
如果点与地图中的平面匹配，将计算残差使用流形卡尔曼滤波器来更新系统状态。优化后的姿态最终将LiDAR点注册到全局坐标系中，并合并到地图上，然后进行下一个测量(LiDAR点或IMU数据)。否则，如果点没有匹配的平面，则通过卡尔曼滤波器预测的姿态直接添加到地图中。为了在允许新配准点的同时实现快速平面对应搜索，我们使用了最初在fast - lio2中开发的增量k-d树结构ikd-Tree。[29]对于每次IMU测量，IMU的每个通道的饱和度检查是单独进行的，具有饱和值的通道不会被用于状态更新。
状态估计
Point-LIO的状态估计是一个紧耦合的流形卡尔曼滤波器。在这里，我们简要地解释滤波器的基本公式和工作流程，并参考[55]对流形上卡尔曼滤波器进行更详细的理论解释。
符号约定
为了便于解释，我们采用如下符号
动态模型
状态转移模型
以IMU框架(记为I)为机体系，第一帧IMU坐标系为全局坐标系(记为G)，连续运动模型为：
其中，
- \({ }^G \mathbf{R}_I,{ }^G \mathbf{p}_I\), and \({ }^G \mathbf{v}_I\)分别表示imu的姿态、位置和速度在全局坐标系的表示
- \({ }^G \mathbf{g}\)表示重力向量在全局坐标系的表示（第一帧IMU坐标系）
- \(b_g\) and \(b_a\)建模为受高斯噪声\(\mathbf{n}_{\mathrm{b}_{\mathrm{g}}} \approx \mathcal{N}\left(\mathbf{0}, \mathcal{Q}_{\mathrm{b}_{\mathrm{g}}}\right)\) and \(\mathbf{n}_{\mathrm{b}_{\mathrm{a}}} \approx \mathcal{N}\left(\mathbf{0}, \mathcal{Q}_{\mathrm{b}_{\mathrm{a}}}\right)\)影响的imu bias，具有随机游走的性质
- \(\lfloor\mathbf{a}\rfloor\)记号表示关于向量\(a \in \mathbb{R}^3\)的反对称矩阵
- \({ }^I \boldsymbol{\omega}\) and \({ }^I \mathbf{a}\)分别表示imu在body系的测量值。
如文献[14]所提出的，机器人的某个运动(角速度\({ }^I \boldsymbol{\omega}\)和线加速度\({ }^I \mathbf{a}\))总是可以被视为信号集合或集合的一个样本，这使我们能够通过随机过程统计地描述机器人的运动。
此外，如文献[14]所述，由于机器人系统的运动通常具有一定的平滑性(例如，由于执行器的延迟)，角速度和加速度的快速变化相对不太可能，并且n阶积分器随机过程通常足以满足实际使用。
特别地，我们选择了由高斯噪声\(\mathbf{w}_{\mathrm{g}} \approx \mathcal{N}\left(\mathbf{0}, \mathcal{Q}_{\mathrm{g}}\right)\) 和 \(\mathbf{w}_{\mathrm{a}} \approx \mathcal{N}\left(\mathbf{0}, \mathcal{Q}_{\mathrm{a}}\right)\)驱动的一阶积分器模型来分别模拟角速度\({ }^I \boldsymbol{\omega}\)和线性加速度\({ }^I \mathbf{a}\)。
然后将连续时间模型(2)在每个测量步骤k处离散化。表示\(\Delta t_k\)为当前测量间隔，即之前测量(IMU数据或LiDAR点)与当前测量(IMU数据或LiDAR点)之间的时间差。对连续模型(2)进行离散化，假设输入在区间\(\Delta t_k\)内保持恒定（加速度、角速度），得到：
其中，流形\(\mathcal{M}\)，函数\(\mathbf{f}\)，状态\(\mathbf{x}\)以及过程噪声\(\mathbf{w}\)定义为：
其中，\(\mathcal{Q}=\operatorname{diag}\left(\mathcal{Q}_{\mathrm{b}_{\mathrm{g}}}, \mathcal{Q}_{\mathrm{b}_{\mathrm{a}}}, \mathcal{Q}_{\mathrm{g}}, \mathcal{Q}_{\mathrm{a}}\right)\)是过程噪声\(\mathbf{w}\)的协方差矩阵。
测量模型
该系统有两个测量，一个激光雷达点或IMU数据(由角速度和加速度组成)。这两个测量通常是系统在不同时间采样和接收的，因此我们分别对它们进行建模。
假设 LiDAR 系与Body（即 IMU）系重合，或者具有预先标定的外参，一个雷达点\({ }^I \mathbf{p}_{\mathrm{m}_k}\)等于局部IMU坐标系的真实位置\({ }^I \mathbf{p}_k^{\mathrm{gt}}\)，但是，实际上我们并不知道对应局部IMU坐标系的点的真实位置，我们假设该值受高斯噪声\(\mathbf{n}_{\mathrm{L}_k} \approx \mathcal{N}\left(\mathbf{0}, \mathscr{R}_{\mathrm{L}_k}\right)\)的影响，因此有：
这个真正的点，在使用真实(但未知)IMU位姿\({ }^G \mathbf{T}_{I_k}=\left({ }^G \mathbf{R}_{I_k},{ }^G \mathbf{p}_{I_k}\right)\)投影到全局坐标系后，应该恰好位于地图中的局部小平面补丁上(见图2)，即有：
其中，
- \({ }^G \mathbf{u}_k\)是关联平面的法向量
- \({ }^G \mathbf{q}_k\)是平面上任意点
- 注意，\({ }^G \mathbf{T}_{I_k}\)被包含在状态向量\(\mathbf{X}_k\)中，式（6）对状态向量\(\mathbf{X}_k\)施加隐式测量模型
IMU测量由角速度测量(\({ }^I \mathbf{\omega}_{\mathrm{m}}\))和加速度测量(\({ }^I \mathbf{a}_{\mathrm{m}}\))组成：
其中，
- \(\mathbf{n}_{\mathrm{g}} \approx \mathcal{N}\left(\mathbf{0}, \mathscr{R}_{\mathrm{g}}\right) \quad\) 和 \(\quad \mathbf{n}_{\mathrm{a}} \approx \mathcal{N}\left(\mathbf{0}, \mathscr{R}_{\mathrm{a}}\right)\)是高斯噪声
- 那么，记\(\mathbf{n}_I=\left[\begin{array}{ll}\mathbf{n}_{\mathrm{g}}^T & \mathbf{n}_{\mathrm{a}}^T\end{array}\right]^T \approx \mathcal{N}\left(\mathbf{0}, \mathscr{R}_I\right) = \mathcal{N}\left(\mathbf{0}, \operatorname{diag}\left(\mathscr{R}_{\mathrm{g}}, \mathscr{R}_{\mathrm{a}}\right)\right)\)表示为IMU的测量噪声
因此，状态方程 (2) 中分开的两个状态 \(\omega\)、\(b_g\)（类似地 \(a\)、\(b_a\)）现在在角速度测量\(\boldsymbol{\omega}_{\mathrm{m}}\)（或加速度测量 \(\mathbf{a}\)）中相关，综上所述，系统的测量模型可以用以下紧凑形式呈现：
扩展卡尔曼滤波器
紧密耦合EKF用于Point-LIO的状态估计。本节将介绍EKF的工作流程
状态传播
假设我们已经收到了最多步骤 k 的测量，并且该时间步的更新状态是\(\overline{\mathbf{X}}_k\)以及更新的协方差矩阵 \(\overline{\mathbf{P}}_k\)。通过设置\(\mathbf{w}_k=\mathbf{0}\)，根据公式(3)中的状态转换模型，可以得到从时间步k到下一次观测时间步k+1的状态传播，即：
协方差传播如下：
其中\(\mathcal{Q}_k\)为过程噪声\(\mathbf{W}_k\)的协方差，矩阵\(\mathbf{F}_{\mathbf{x}_k}, \mathbf{F}_{\mathbf{w}_k}\)计算如下：
其中，
- \(\mathbf{x}_{k+1}\)是时间步k+1的真实状态向量
- \(\mathbf{F}_{11}=\operatorname{Exp}\left(-{ }^I \overline{\boldsymbol{\omega}}_k \Delta t_k\right)\)
- \(\mathbf{F}_{38}={}^G \overline{\mathbf{R}}_{I_k} \Delta t_k\)
残差计算
激光测量
利用卡尔曼滤波(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点\({ }^I \mathbf{p}_{\mathrm{m}_{k+1}}\)投影到全局坐标系，得到\({ }^G \hat{\mathbf{p}}_{k+1}={ }^G \hat{\mathbf{R}}_{I_{k+1}}{ }^I \mathbf{p}_{\mathrm{m}_{k+1}}+{ }^G \hat{\mathbf{p}}_{I_{k+1}}\)，并在由ikd-tree构造的地图结构中搜索其最近的5个点(距离\({ }^G \hat{\mathbf{p}}_{k+1}\) 5米以内)
然后使用找到的最近邻点拟合具有法向量\({ }^G \mathbf{u}_{k+1}\)和质心\({ }^G \mathbf{q}_{k+1}\)的局部小平面块，如测量模型所示（参见公式（6）和图 2）。
如果最近的五个点不位于拟合平面路径上(即任何点到平面的距离大于0.1 m)，则当前LiDAR点\({ }^G \hat{\mathbf{p}}_{k+1}\)的测量直接合并到地图中，而不需要残差计算或状态更新。 【？？？？】
否则，如果局部平面成功拟合，则根据式(8)计算残差\(\left(\mathbf{r}_{\mathrm{L}_{k+1}}\right)\)如下：
其中，
- \(\delta \mathbf{x}_{k+1}=\mathbf{x}_{k+1} \boxminus \hat{\mathbf{x}}_{k+1}\)
- \(\mathbf{x}_{k+1}\)是时间步K+1的真值状态向量
并且，
IMU测量
对于IMU测量，我们首先通过检查当前测量与额定测量范围之间的差距来评估IMU的任何通道是否饱和，如果差值太小，则IMU测量通道的测量值被丢弃而不用于更新状态。然后，收集不饱和IMU通道的加速度和角速度测量，根据式(7)计算IMU残差\(\left(\mathbf{r}_{I_{k+1}}\right)\)，为了简化符号，我们在这里使用所有6个通道测量)：
其中，
- \(\delta \mathbf{x}_{k+1}=\mathbf{x}_{k+1} \boxminus \hat{\mathbf{x}}_{k+1}\)
- \(\mathbf{x}_{k+1}\)是时间步K+1的真值状态向量
并且，
综上所述，从激光雷达点测量(12)或IMU测量(14)的残差与状态\(\mathbf{x}_{k+1}\)相关，通过以下关系，各自的测量噪声为：
其中，
对于激光点测量，我们有：
- \(\mathbf{r}_{k+1}=\mathbf{r}_{L_{k+1}}\)
- \(\mathbf{H}_{k+1}=\mathbf{H}_{\mathrm{L}_{k+1}}\)
- \(\mathbf{D}_{k+1}=\mathbf{D}_{\mathrm{L}_{k+1}}\)
- \(\mathscr{R}_{k+1}=\mathscr{R}_{L_{k+1}}\)
对于IMU测量，我们有：
- \(\mathbf{r}_{k+1}=\mathbf{r}_{I_{k+1}}\)
- \(\mathbf{H}_{k+1}=\mathbf{H}_{\mathrm{I}_{k+1}}\)
- \(\mathbf{D}_{k+1}=\mathbf{D}_{\mathrm{I}_{k+1}}\)
- \(\mathscr{R}_{k+1}=\mathscr{R}_{I_{k+1}}\)
状态更新
式(9) 中的传播状态\(\hat{\mathbf{x}}_{k+1}\) 和 (10) 中的协方差\(\hat{\mathbf{P}}_{k+1}\) 对未知状态\(\mathbf{x}_{k+1}\) 施加先验高斯分布，如下所示：
这里的意思是：误差状态向量服从高斯分布
另外的，观测模型(16)给出了\(\mathbf{x}_{k+1}\)的另一个高斯分布形式：
然后将(17)中的先验分布与来自(18)的测量模型相结合，得到状态\(\mathbf{x}_{k+1}\)的后验分布(等价地用\(\delta \mathbf{x}_{k+1}\)表示)：
其中，
- \(\|\mathbf{x}\|_{\mathbf{A}}^2=\mathbf{x}^T \mathbf{A}^{-1} \mathbf{x}\)
式（19）的优化问题是一个标准的二次规划，可以很容易地得到最优解\(\delta \mathbf{x}_{k+1}^0\)，这本质上是卡尔曼更新[56]：
然后，更新状态\(\mathbf{x}_{k+1}\)如下：
更新后的状态将用于下一步传播，为此，我们还需要估计状态的协方差（上面的是误差状态的协方差），用\(\overline{\mathbf{P}}_{k+1}\)表示，最优状态与状态真值之间的误差表示如下：
