Point-LIO阅读笔记 | freesix blog

关于Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry的阅读随笔

介绍

这篇文章介绍了一种名为Point-LIO的鲁棒高带宽激光雷达惯性测距（LiDAR）系统。该系统通过使用一种点云式的LiDAR惯性测距框架，在实际采样时间上融合LiDAR点云，而不是将它们累积到帧中。这消除了运动畸变，实现了高速率的测距输出和地图更新。此外，该系统使用了一种随机过程增强的运动模型来建模IMU测量，即使IMU饱和也能平滑估计系统状态。文章还讨论了Point-LIO的状态估计和地图结果。简单点来说处理点云过程中不以帧为单位，而是以雷达点顺序为单位，因为在一帧过程中雷达本体是在运动的，会产生运动畸变，并且限制里程计的带宽，降低系统增益。

贡献

点对点的LIO框架：Point-LIO在每个LiDAR点测量时更新状态，而不是将点累积到帧中。这种方法消除了帧内运动畸变的影响，使得输出的里程计具有高频率，增加了里程计的带宽，并能够跟踪快速运动。
随机过程增强的运动模型：Point-LIO使用随机过程模型来建模IMU测量，并将其融合到系统的运动模型中。这种增强的运动模型允许平滑估计系统状态，包括角速度和线性加速度，即使IMU饱和也能保持较好的估计精度。
高精度的地图生成：Point-LIO通过对墙面上的点进行一致性分析，比较了FAST-LIO2、Point-LIO-input和Point-LIO的地图生成精度。结果显示，Point-LIO能够产生比FAST-LIO2更精细的墙面地图，消除了帧内运动畸变的影响。
抗振动能力：Point-LIO在实验中面临强烈的振动挑战，但通过点对点的更新方式和随机过程增强的运动模型，能够有效减轻振动对传感器的影响，提高系统的稳定性和鲁棒性。

总之，Point-LIO通过创新的点对点更新方式和随机过程增强的运动模型，实现了高精度、高带宽的LiDAR惯性里程计，具有较好的抗振动能力和地图生成精度。

框架

系统设计的两个策略：

激光雷达点是在各自的时间顺序采样，不是在帧内等间隔采样。
IMU数据是系统的测量值，不是输入值，一旦接受到测量值(雷达点或者IMU数据)，就在流形EKF上融合这两个测量值。

对于运动失真的补偿一般是假设帧内是匀速运动来补偿，但在剧烈运动或变化剧烈的运动场景下会带来非常大的漂移甚至是里程计的失效。另外一种比较流行的运动补偿方式是基于连续的时间轨迹优化，例如B样条和高斯过程模型，然后这些方法非常耗时，很难在线运行，除此之外，这类方法还需要为轨迹估计积累足够的点，这样增加了里程计的等待时间。

一种有效运动失真补偿方法就是利用IMU的高频率测量来补偿激光点的运动失真。(采用预积分的方式)。本文采用对单个激光点序列进行运动估计的方法，这样避免了运动失真补偿这一过程。

松耦合是将IMU数据预积分后用这个估计作为初始位姿作为后续序列的配准。紧耦合是将IMU数据和激光雷达数据作为联合优化输入。两种耦合方式都有基于EKF和优化的方法。但是在紧耦合框架中，IMU数据都被用作运动学模型的输入，在机器人运动超出IMU测量范围的情况下，这种EKF传播或者预积分过程会遇到饱和问题。将IMU数据作为模型输出的测量值可以很自然的解决这种由剧烈运动产生的饱和问题。

如下框架图中顺序采样的LiDAR点和IMU数据都用于更新各自时间戳的状态，从而产生极高的里程计输出。对于每个接收到的点从地图中搜索相应平面，一个残差用流形KF计算用来更新系统状态，最终优化的位姿用于将雷达点匹配到地图中。如果这个点没有匹配的平面，直接通过KF后加入地图。为了在匹配新的点过程中快速实现平面的搜索采用了增量kd树。

状态估计

Point-LIO是一个在流形卡尔曼滤波上的紧耦合框架。流形卡尔曼滤波就是在一个特殊流形上的卡尔曼滤波，本文指的是在由欧式空间和李群这两个子流形构成的流形上进行的卡尔曼滤波，至于这个流形卡尔曼滤波可不可以用于其它流形就要看原文了。

首先引入了两个流形上的封装运算和其逆运算，它们的在和上的运算法则如下：

状态转移模型

其中，，表示IMU的位姿、位置和速度在全局坐标中。和是IMU的随机偏差，分别服从和的高斯分布。符号表示反对称叉积矩阵。和表示在IMU坐标系(即机体坐标系)下的角速度和加速度。

将上面的运动学模型离散化处理，将表示测量间隔，并且假设在输入的测量间隔保持不变。于是将上面连续运动学模型离散化为：其中，流形、函数、状态和过程噪声的定义如下：这里的表示过程噪声的协方差矩阵。

测量模型

该系统有两个测量，一个是LiDAR点和IMU数据。这两个测量值通常在不同的时间被系统采样和接收，因此将它们分别建模，假设LiDAR坐标系和机体(IMU坐标系)重合或者具有预校准的外参，于是LiDAR点等于在本地IMU坐标系这样红的真实位置，该位置是未知的，收到加性高斯噪声的影响，于是有：真实点使用未知的IMU位姿变换来判断此点是否属于地图中平面的一个点：其中表示这个平面的法向量，表示平面中的任意一点，其中被包含在状态向量中。

下面给出IMU的测量模型那么整个的测量模型就可以写为如下的紧凑模式

扩展卡尔曼滤波

一个紧耦合框架的扩展卡尔曼滤波

状态传播

状态从传播到下一个状态并设置然后协方差传播方程为：这里是过程噪声的协方差矩阵，然后矩阵和计算如下：这里 $，$

残差计算

LiDAR测量的残差计算

根据卡尔曼滤波状态传播步骤预测得到的位姿，将测量雷达点映射到全局坐标并且在地图中寻找前后最邻近的五个点通过ikd-Tree。然后使用找到的最邻近点来拟合局部最小平面，其法向量为，重心为，如果最近的五个点不在拟合的平面路径上(即任何点到平面的距离大于0.1m)，则当前点直接合并到地图中，无需进行残差计算或者状态更新。否则，则根据测量模型方程计算残差：这里的，表示真实状态值在步。并且：

IMU测量残差计算

通过检查当前测量值与额定测量值范围之间的差距来判断IMU测量通道是否被丢弃，如果差距太小，则该IMU测量会被丢弃不会用于更新状态。然后收集未饱和IMU的加速度和角速度测量值，根据IMU的状态方程计算残差。这里的，表示步骤的状态真实值，并且：总之来自LiDAR或者IMU测量的残差 $与状态$ _{k+1}$的关系有：这里的在后面指代LiDAR或者IMU。

状态更新

在传播方程中的状态和协方差对未知状态为高斯分布：测量模型对给出了另外一个高斯分布：然后将前面的先验分布和测量模型相结合得到状态的后验分布：这里的，上面式子优化问题是一个标准的二次分配问题，这个优化解很容易获得，本质上是卡尔曼滤波更新过程：然后跟新：然后更新的状态用于下一步骤的传播。同样我们也需要估计误差的协方差，估计状态和真实状态的误差表示为这里的是投影矩阵的协方差是优化问题方程的协方差的逆，也是矩阵，因此根据前面的式子的协方差为：整个状态估计的总体过程为：

介绍

贡献

框架