Point-LIO：鲁棒高带宽激光惯性里程计

标题：Point-LIO: Robust High-Bandwidth LiDAR-Inertial Odometry

作者：He et al., HKU MARS Lab

Paper: https://github.com/hku-mars/Point-LIO/files/10989136/Point-LIO_preprint.pdf

Code: https://github.com/hku-mars/Point-LIO

1 动机

现有系统都是基于帧的，类似于VSLAM系统，频率固定（例如10Hz), 但是实际上LiDAR是在不同时刻进行顺序采样，然后积累到一帧上，这不可避免地会引入运动畸变，从而影响建图和里程计精度。此外，这种低帧率会增加延时，限制系统带宽（里程计带宽的定义类似动态系统的带宽，即系统增益降至0.707以下的频率，表示里程计在能够满意地估计时可以运动多快）。

2 主要贡献

1) 提出了一种逐点(point-wise) LIO框架，该框架在实际采样时间融合激光雷达点，而不会累积到帧中。去除点累积消除了帧内运动失真，并允许以接近点采样率的高里程计输出和建图更新，这进一步使系统能够跟踪非常快的运动。

2) 为了进一步提高系统带宽到超出IMU测量范围，用随机过程模型对IMU测量进行建模。将该模型扩展到系统运动学中，并将IMU测量值视为系统输出。即使IMU饱和，随机过程增强的运动学模型也可以平滑估计系统状态，包括角速度和线加速度。

3) 将这两个关键技术集成到一个完全紧耦合的LIO系统中，称为Point-LIO。系统使用流形扩展卡尔曼滤波器通过在其各自的采样时间融合每个LiDAR点或IMU数据来更新系统状态。通过利用系统的稀疏性和线性，开发的系统即使在微型飞行器上基于低功耗ARM的计算机上也能实现实时状态估计。

4) 开发的系统在由具有非常小FoV的新兴固态LiDAR收集的各种挑战性的现实世界数据中进行了测试。结果表明，Point-LIO具有运动畸变补偿能力，具有高里程计输出速率 (4-8 kHz) 和高带宽 (>150Hz) 的能力。该系统还能够在初始阶段后通过饱和IMU测量来估计极端激进运动 (角速度大于75 rad/s) 下的状态。此外，对来自各种公开LiDAR数据集的12个序列的详尽基准比较表明，Point-LIO实现了与其他方法一致可比的准确性和效率，同时花费更少的计算资源。最后演示了实际无人机上的实际应用。

3 方法概览

其设计理念主要基于:

1) LiDAR点是在相应的时间顺序采样的，而不是同时采样的帧;

2) IMU数据是系统的测量，而不是输入。

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

• 对于接收到的每个LiDAR点，搜索地图的对应平面 (为了在允许新注册点的同时实现快速的平面对应搜索，使用了在FAST-LIO2 中开发的增量k-d树 (ikd-Tree))。
• 如果点与从地图中的点拟合的平面匹配，则计算残差以使用流形卡尔曼滤波器更新系统状态。优化的位姿最终将LiDAR点注册到全局帧中并合并到地图，然后进行下一个测量 (LiDAR点或IMU数据)。
• 否则，如果点没有匹配的平面，则通过卡尔曼滤波器预测的位姿将其直接添加到地图中。
• 对于每个IMU测量，分别对IMU的每个通道进行饱和度检查，具有饱和值的通道将不用于状态更新。

4 实验

主要评估三方面：

• 去除运动畸变
• 高频率、高带宽的里程计
• 饱和IMU情况下的状态估计 4.1 实现细节

使用C++和ROS实现：

• EKF是基于作者在先前工作中开发的IKFoM工具箱(https://github.com/hku-mars/IKFoM)实现的。
• 使用FAST-LIO2中开发的增量k-d树 (ikd-Tree) 作为地图结构，其默认参数为:
• 局部地图大小 = 2000 m，
• 空间下采样分辨率 = 0.25 m，
• ikd-Tree的重平衡阈值为 = 0.6， = 0.5，
• 用于并行重建(在第二线程中)的子树大小阈值为 = 1500。
• 尽管系统被设计为在每次LiDAR点接收后执行状态估计，但实际上，受LiDAR制造商提供的可用驱动程序的限制，LiDAR点在累积完整扫描后被打包，然后发送到LIO系统。为了满足这一实际限制，Point-LIO根据它们各自的时间戳对接收到的包中包含的所有LiDAR点和IMU数据进行排序。然后，将排序后的数据逐个逐点处理-LIO。

在所有评估中，将Point-LIO与最先进的基于帧的里程计FAST-LIO2进行了比较：

• FAST-LIO2的所有结果都是使用开源的FAST-LIO2及其默认参数值 (基本上也是上面的参数值) 得到的
• 由于FAST-LIO2为每次接收到的LiDAR帧执行分辨率为0.3的空间降采样，为了公平比较，也在Point-LIO中执行这样的空间降采样，然后再进行数据排序和逐点更新

消融实验：

• 逐点更新：仅用逐点更新，而没有应用有色随机模型。系统与FAST-LIO2相同，将IMU测量值建模为系统运动学模型的输入，但不同之处在于扫描中的每个单独的LiDAR点都用于像本文提出的系统那样顺序更新系统，该系统命名为Point-LIO-input。

4.2 实验平台

• 固态3D激光雷达Livox Avia 1）70.4° (水平) × 77.2° (垂直) 圆形FoV和非常规的非重复扫描模式，可产生230,000Hz的点测量结果 2）内置的IMU (BMI088型) 可产生200Hz的IMU数据。 3）点和IMU数据以从10 hz到100Hz可调的频率收集。
• 第一人称视角 (FPV) 相机
• 五个用于真值测量的Vicon标记。
• 为了产生不同类型的运动，构建了三个不同的平台来承载传感器套件，包括 1）机器人车 (图3 (b))，深圳大疆开发的RoboMaster 2019 AI; 2）由步进电机驱动的旋转平台 (图3 (c))，Nimotion STM4260A； 3）一个钟摆（图3(d)） 4.3 运动畸变处理

使用图3 (b) 所示的机器人车收集了一些序列，测试了三种不同的场景，即

• 公园：Belcher Bay Park (非结构化场景)
• 广场：HKU的百年小广场 (半结构化场景)
• 走廊：HKU的Haking Wong大楼的走廊 (结构化场景)

在所有序列中，机器人车返回起点，能够计算漂移。

LiDAR数据的频率为10Hz。

一个挑战是机器人车在地面上移动时的强烈振动。由于传感器套件安装在底盘上，没有任何减震器，振动将直接传递给传感器，导致剧烈的抖动运动，如图4所示的内置IMU数据所示。

当机器人汽车在开始时静止时，IMU测量值稳定地很小。随着汽车开始移动，IMU的测量值迅速变化。

4.3.1 建图结果

每个图里，

• 首先展示最终建图结果的全局视图 (即子图 (a))，然后关注包含大平面 (例如墙) 的某些局部区域 (子图 (b))。
• 进一步通过墙上点的一致性 (子图 (c))，比较建图精度 1）FAST-LIO2 (即 (b1)，(c1)) 2）Point-LIO-input (即 (b2)，(c2)) 3）Point-LIO (即 (b3)，(c3))。
• 最后，为了显示帧内运动失真，展示了上面选择的局部区域中的一次扫描 (在一个扫描周期0.1 s内的累积) 的点 (子图 (d)) 以及进一步放大(子图 (e)), 其中红点指的是当前扫描中已注册的LiDAR点，白点是累积到当前扫描的地图结果 1）FAST-LIO2 (即(d1) 、 (e1)) 2）Point-LIO-input (即 (d2)，(e2)) 3）Point-LIO (即 (d3)，(e3))

从图5，图6和图7的子图 (c) 可以看出，FAST-LIO2 (c1) 的整体图明显比Point-LIO-input (c2) 厚，Point-LIO (c3) 产生的墙比Point-LIO-input更薄。这种现象的原因在于如图5、图6和图7的子图 (e) 所示的每个单独扫描中的帧内运动补偿。可以看出，所选壁周围的所有红点都应该属于同一平面，但是由于帧内运动失真，它们实际上会从壁上散射FAST-LIO2 (e1)。Point-LIO-input和Point-LIO的这种帧内失真现象得到了很大缓解 ((e2) 和 (e3))。

FAST-LIO2使用基于IMU测量的向后传播来将扫描的所有点投影到扫描结束的位姿。此过程很容易受到IMU测量噪声，偏差估计误差和有限的IMU采样率的干扰。具体来说，由传感器振动引起的加速度和角速度即使在IMU的一个采样间隔内也以高速率变化，导致大的IMU传播误差，因为假设角速度和加速度在一个采样间隔期间是恒定的。此外，低帧率 (即10Hz) 还需要长时间 (即100 ms) 的IMU传播，这累积了位姿误差并导致大的帧内失真，如子图 (b1) 和 (c1) 所示。

相反，Point-LIO在其真实采样时间处融合LiDAR点，而没有任何点累积，这从根本上消除了运动失真，如Point-LIO-input的子图 (c2) 和Point-LIO的子图 (c3) 所示。

此外，当将两个逐点更新方案Point-LIO-input (c2) 与Point-LIO (c3) 进行比较时，Point-LIO的性能稍好。这是因为Point-LIO-input仍然使用IMU测量来传播状态 (尽管仅用于一个LiDAR点间隔)，因此仍然受IMU测量噪声和偏差估计误差影响。相反，Point-LIO使用IMU测量的滤波但不使用原始数据来传播状态，这稍微减少了 (e3) 中观察到的运动失真。

4.3.2 漂移结果

由于机器人车的不完善操作，开始和结束位置之间的距离不完全为零，但小于10cm。

可以看出：

• 对于Square和Corridor序列，FAST-LIO2，Point-LIO-input和Point-LIO具有可比的漂移。
• 对于Park序列，FAST-LIO2无法返回起点，这是因为公园是一个非结构化的环境，这使得运动失真对测距精度的影响更加明显。

4.4 高频率、高带宽里程计输出

通过向步进电机提供快速变化的速度命令，在使用旋转平台 (图3 (c)) 收集的室内数据集 (表示为 “Odo”) 上测试FAST-LIO2、Point-LIO-input和Point-LIO的里程计输出频率。在本实验中，LiDAR包的速率为100Hz。

即使以100Hz的LiDAR速率收集数据集，通过将一个帧分成多个帧 (但低于200Hz的IMU速率)，FAST-LIO2框架也自然可扩展到更高的状态更新频率。因此，将一个LiDAR帧分为两个运行FAST-LIO2。

图8显示了里程计输出频率的分布。FAST-LIO2的输出频率是200Hz，和帧率一样。Point-LIO-input和Point-LIO的频率在4kHz和8kHz之间。

为了能够进行带宽分析，以上实验以最高频率300Hz记录来自Vicon跟踪器的真值测量重新进行。将系统输出 (根据里程计估计的偏航角) 除以系统输入 (由Vicon系统测量的真值偏航角)，我们得到在不同的输入频率下FAST-LIO2、Point-LIO-input和Point-LIO的幅度响应 (dB)，如图9所示。

可以看出，当输入频率接近100Hz时，FAST-LIO2的幅度响应开始下降，这表明带宽为100Hz (见表2)。根据Nyquist-Shannon采样定理，当输出频率为200Hz时，100Hz也是可达到的最高带宽。相反，Point-LIO-input和Point-LIO都具有大于150Hz的带宽，这超出了Vicon系统的测量能力。

4.5 饱和IMU测量下的极激进运动

尽管到目前为止，Point-LIO-input和Point-LIO具有可比的性能，但本节展示了Point-LIO即使超出IMU测量范围，也能够跟踪极其激进的运动。

在实验中产生两种类型的运动：

• 旋转运动 (表示为 “Satu-1”)
• 在空间中盘旋 (表示为 “Satu-2”)。

由于较高的旋转速率或较大的离心力，两个实验在初始阶段后都会遭受IMU饱和。据我们所知，以前的SLAM系统无法应对这种激进的运动或饱和的IMU测量。

4.5.1 旋转运动

使用放置在杂乱的实验室环境中的旋转平台进行 (见图10 (a1)-(a4))。

在实验过程中，传感器套件由步进电机在步进角速度命令下旋转，该步进角速度命令从零逐步增加到峰值，然后在结束时减小到零。所得的峰值角速度为75 rad/s (yaw)，远远超过IMU测量范围，即35 rad/s。高角速度还会导致大约80m/s2 的峰值加速度，也远远超出IMU测量范围，即大约30m/s2 。

图10 (c1) 和 (c2) 中所示的板载FPV图像给出了旋转过程的图示。Point-LIO的建图结果如图10 (b1) 所示，它显示了建图结果与环境基本一致，估计的结束位置与开始位置非常吻合，如图10 (b2) 所示。

将估计的运动学状态 (包括欧拉角旋转和位置) 与图11 (a) 中的GT进行比较，其中x轴被分成三个部分以放大时间段84-85 s。yaw的连续快速变化是由于步进电机驱动的连续旋转，位置上的正弦形波动是由Vicon标记和步进电机轴之间的偏移引起的。可以看出：

• 估计的yaw角可以在整个过程中紧密跟踪实际yaw角
• 总体旋转和平移误差 (以RMSE计) 分别为4.60°和0.233m 1）平移的RMSE略大主要是由y方向引起的，从一开始，该方向的约束就不足。考虑到实验中的极端运动，这种平移误差是可以接受的。

Point-LIO的另一个好处是能够估计IMU饱和时的角速度和加速度 (即系统的状态，因此可以通过卡尔曼滤波器进行估计)。图11 (b) 展示了估计值与IMU测量值的关系。可以看出，在50-106 s的时间段内，IMU饱和 (陀螺仪的z轴和加速度计的y轴)，而Point-LIO仍然可以给出合理的估计。在该区域之外，尽管对一些高频分量进行了滤波，但Point-LIO的估计与IMU测量结果非常吻合。

通过以不同的电动机初始角速度启动Point-LIO来进一步挑战它。如图12中的地图结果和表3中的旋转RMSE所示，当初始角速度低于IMU饱和值，即35 rad/s，Point-LIO能够通过构建合理的地图和状态估计来生存。与上述传感器从静止位姿开始的情况相比，状态估计的质量略有下降。这种性能下降是合理的，因为快速的初始角速度导致Point-LIO在一开始就构建有偏差的地图，这进一步误导了随后的状态估计。这也导致旋转RMSE随初始角速度的增加而增加，如表3所示。当初始角速度超出IMU测量范围时，Point-LIO由于太大的初始状态估计而失败 (例如，初始角速度估计设置为零，而实际大于35 rad/s)。

最后，作为对比，在同一数据集上运行FAST-LIO2和Point-LIO-input，旋转和位置的误差比较如图13所示。可以看出:

• FAST-LIO2，Point-LIO-input和Point-LIO的估计在IMU正常工作的前50 s期间具有可比的旋转误差。
• 从IMU开始饱和的50s开始，FAST-LIO2和Point-LIO-input的旋转估计开始立即发散，并且位置的估计也开始漂移然后发散。

总之，FAST-LIO2以及Point-LIO-input在饱和IMU测量下无法工作，而如果没有从一开始就饱和，Point-LIO可以很好地生存。

4.5.2 盘旋运动

该实验是在相同的实验室环境中使用摆锤进行的 (参见图14 (d))。在此实验中，传感器套件被绑在绳索的一端，该绳索在垂直平面中摆动成盘旋轨迹 (请参见图14 (b))。

此运动导致圆底部的加速度高达40m/s2，超过了IMU测量范围30m/s2。图14 (e) 中所示的FPV图像给出了运动的视觉图示。

定性结果：

图14 (c1) 和 (c2) 显示了Point-LIO的建图结果。由于LiDAR面向前方，具有70.4° × 77.2° 圆形FoV，因此仅绘制了实验室的一侧。估计的轨迹如图14 (a) 所示，与图14 (b) 所示的实际传感器路径高度吻合。

定量结果：

将估计的欧拉角旋转和位置与图15 (a) 中由Vicon测量的真值进行比较，其中欧拉角和位置的估计在12个连续圆都成功了。平均旋转误差和平移误差的RMSE分别为4.42°和0.0990 m。

图15 (b) 显示了通过Point-LIO与IMU测量值的估计角速度和加速度。可以看出：

• 当这些动态状态在IMU测量范围内时，估计值与IMU测量非常吻合
• 即使IMU饱和，Point-LIO也可以给出合理的加速度估计

图16进一步示出了Point-LIO、Point-LIO-input和基于帧的里程计FAST-LIO2之间的误差比较。与上一个实验类似，由于能够应对IMU饱和，Point-LIO的估计误差始终低于其他两个。

4.6 实时性能

在图17中示出了用于处理LiDAR点的一次扫描的Point-LIO-output的每个步骤的平均时间成本，其在基于intel i7的微型UAV机载计算机、具有1.8 GHz四核Intel i7-8550U CPU和8 GB RAM的DJI Manifold2-C7上进行测试。

建图包括搜索最近的点并将点添加到地图中，这占用了最大的时间消耗。即使在每个LiDAR点处更新系统状态，EKF滤波的时间包括状态传播和更新，对于10Hz序列小于10ms，对于100Hz序列小于1 ms。在表4中的FAST-LIO2、Point-LIO-input和Point-LIO-output之间比较一次扫描的平均总时间消耗。表示LIO在这些序列中失败。

可以看出，Point-LIO (Point-LIO-input或Point-LIO)，具有与FAST-LIO2相当的时间消耗，且都实现了实时性能，即，对于10Hz序列在100ms内，对于100Hz序列在10ms内。最后，所有序列上每秒处理的平均点数 (包括具有和不具有平面对应关系的点数)是33,710，并且每个点的平均处理时间为9 us。

5 基准结果

在各种公开数据集序列上对Point-LIO进行测试，这些序列具有更平缓的运动，没有IMU饱和，并将其与其他最先进的LiDAR惯性里程计方法进行了比较，包括FAST-LIO2，LILI-OM，LIO-SAM 和LINS。

用于基准比较的计算平台是与FAST-LIO2中使用的相同的轻型无人机机载计算机，它是具有1.8 GHz四核英特尔i7-8550U CPU和8GB RAM的DJI Manifold2-C7，因此FAST-LIO2，LILI-OM，LIO-SAM，LINS可以直接从FAST-LIO2论文中获得。

Point-LIO使用与FAST-LIO2相同的建图结构，为了公平比较，将Point-LIO的建图参数设置为与FAST-LIO2的默认值相同，即：

• 局部地图大小L = 1000 m
• 在1:4 时间下采样(每四个LiDAR点取一个) 之后，LiDAR原始点直接输入状态估计
• 空间下采样分辨率l= 0.5m
• ikd-Tree的重新平衡阈值为

,Nmax=1500

• 对于Point-LIO的EKF部分，将卡尔曼滤波器的LiDAR测量噪声设置为

(见 (18))

这些参数值对于所有序列都保持相同。

在FAST-LIO2中使用的相同12个序列上评估本文的方法，这些序列来自4个不同的公共数据集，即 “lili”（来自LILI-OM），“utbm”，“ulhk”， “liosam”（LIO-SAM中）。其中，“lili” 使用固态3D激光雷达Livox Horizon，而其他三个数据集使用旋转的激光雷达，即，用于 “utbm” 和 “ulhk” 的Velodyne HDL-32E激光雷达，以及用于 “liosam” 的VLP-16激光雷达。这些激光雷达有不同的扫描模式。有关数据集和所选序列的更多详细信息，参见FAST-LIO2论文。

5.1 准确率评估

和FAST-LIO2一样，采用ATE（平均平移距离）的RMSE（对于有好GT轨迹的序列）和端到端误差（对于起点终点在同一位置的序列）进行准确率评估。

5.1.1 RMSE基准

Point-LIO在5个序列中的4个达到了最好的RMSE，在utbm_9序列上最显著，在liosam_1上略比FAST-LIO2和LIO-SAM差。总体来说，Point-LIO和其他方法准确率相当（大部分情况要更好）。

5.1.2 漂移基准

总体趋势与RMSE基准结果相似，即Point-LIO在7个序列中的5个序列中实现了最低的漂移。

• 序列lili_8的结果比LILI-OM和FAST-LIO2差，这是因为LILI-OM对他们自己的 “lili” 序列调整了参数，而Point-LIO的参数在所有序列中保持相同。
• 由于lili_8具有比其他两个 “lili” 序列长得多的轨迹，由不适当的参数引起的漂移将沿着后续过程累积，并导致比FAST-LIO2差10米以上的漂移。
• Point-LIO在序列ulhk_6上显示出比FAST-LIO2稍大的RMSE，尽管差距非常小。

从以上基准结果可以看出，Point-LIO在大多数序列中实现了更高的精度，而对于其余序列，和最佳方法的差距并不显著。考虑到所有数据集和序列的各种类型的LiDAR、环境和移动平台，这有效地显示了Point-LIO在实际数据上的准确性和鲁棒性。

5.2 处理时间评估

Point-LIO和FAST-LIO2都将里程计和建图集成在一起，其中在里程计更新的每个步骤中立即更新地图。因此，总时间 (表7中的 “总计”) 计算了里程计中发生的所有可能过程，包括点到地图匹配，状态估计和建图。

另外，LILI-OM，LIO-SAM和LINS都基于里程计 (包括特征提取和粗位姿估计) 和建图 (例如LILI-OM中的后端融合，LIO-SAM中的增量平滑和建图，和在LINS中进行的地图细化)，其每个LiDAR扫描的平均处理时间在对计算时间进行排序时由这两个部分 (“Odo.” 和 “Map.”) 求和。

可以看出，与其他方法相比，Point-LIO和FAST-LIO2实现了最少的计算时间。与FAST-LIO2相比，Point-LIO在12个序列中的7个上花费的时间更少。这两种方法的平均计算时间非常接近。

需要注意，基于帧的FAST-LIO2使用4个线程来并行化最近邻搜索，而Point-LIO是逐点的，必须串行执行这样的操作。尽管如此，Point-LIO平均计算时间仍然相当，这表明计算资源的使用较少，可以保留给其他模块 (例如规划、控制)。计算效率归功于系统的稀疏性以及不需要迭代卡尔曼滤波。

总之，Point-LIO具有与FAST-LIO2相当的准确性和计算效率，且花费更少的计算资源。同时，Point-LIO比当前最先进的LIO算法 (即LILI-OM，LIO-SAM，LINS) 明显更快，实现了相当或更好的准确率。

6 应用

将Point-LIO应用于两个无人机 (UAV) 的状态估计:

• 图18 (a) 中所示的竞赛四旋翼无人机上，其推力重量比高达5.4。高推力重量比使其能进行极其敏捷的运动。
• 另一个UAV位于称为自旋转UAV的敏捷、单驱动飞机上，如图18 (b) 所示。螺旋桨叶片通过两个被动铰链连接到电机轴，通过调节螺旋桨旋转速度的瞬时加速度和减速度，能够产生稳定UAV姿态所需的roll和pitch力矩。由于电机产生的未补偿力矩，无人机将产生高速率的连续yaw旋转。

6.1 竞赛无人机

如图18 (a) 所示，竞赛无人机安装有Livox Avia LiDAR和FPV相机。LiDAR FoV与FPV相机的FoV对齐，基于该相机，专业人员手动控制无人机以执行极其敏捷的飞行操作。飞行是在植被、池塘和建筑物的农田上方进行的 (见图19 (b)，(c))。在飞行过程中执行了几种激进的动作，包括极快的滚动翻转 (见图19 (e1)-(e3))，俯冲和横向加速。在翻转期间，角速度达到59.37 rad/s，超过IMU测量范围35 rad/s。

进行了两次飞行都成功了。由于空间限制，仅展示一次飞行的结果。建图结果如图19 (d1)-(d3) 所示，可以看出所构造的地图与地面上易于区分的精细结构 (例如树木和建筑物) 一致。

欧拉角旋转、位置和速度的估计如图20 (a) 所示，角速度与IMU测量值的估计如图20 (b) 所示。可以看到，即使在极端激进的运动下，也能够估计无人机的状态。一个轴上的最大速度达到14.63 m/s，最大加速度达到30 m/s，角速度达到59 rad/s。此外，LiDAR偶尔会面对天空，导致没有LiDAR测量，但是状态仍然能稳定地被估计。

6.2 自转无人机

Livox Avia LiDAR位于UAV前方，当UAV经历连续yaw旋转时，FoV会快速变化。IMU是LiDAR内置的，测量范围为17.5 rad/s，而无人机的平均yaw角速度约为25 rad/s。无人机还配备了基于ARM的低功耗计算机Khadas VIM3 Pro，该计算机具有2.2 GHz四核Cortex-A73 CPU和4 GB RAM。机载计算机运行Point-LIO实时估计无人机状态，估计的状态输入给飞行控制器Pixhawk 4 Mini，以执行实时控制任务。

使用这种无人机进行了两个实验，一个是香港大学Haking Wong楼外的室外实验，如图21 (a1) 和 (a2) 所示，另一个是在图21(b1)和(b2)所示的杂乱无章的实验室进行的室内实验。

两个实验的实时建图结果如图21所示， (c) 为室外，(d) 为室内。两个环境都能成功建图，且没有明显的重影。

图22 (a) 和 (b) 分别进一步示出了运动学状态 (即旋转，位置和速度)，角速度和加速度的估计。在53-55s的时间间隔内放大了这些图，以更好地显示估计结果。

可以看出：

• 尽管IMU在中间饱和以及快速旋转引起的快速FoV变化，仍可以产生与IMU测量一致的稳定状态估计。
• 每个LiDAR包的平均处理时间为14.63 ms，而LiDAR包的速率为50Hz，确保了实时性能。实验也证实了这一点，其中控制器能够通过实时状态反馈执行稳定的受控飞行。

7 总结

Point-LIO是一个鲁棒的高带宽LIO框架：

• 基于一种新颖的逐点更新方案，在每个点的真实采样时间更新系统状态，而无需将点累积到帧中。消除点累积消除了长期存在的帧内运动失真，并允许以接近点采样率 (4-8 kHz) 的高里程计输出，这进一步使系统能够跟踪非常快的运动
• 为了进一步将系统带宽提高到IMU测量范围之外，将有色随机过程扩展到运动学模型中，将IMU测量作为系统输出

所开发系统的带宽、鲁棒性、精度和计算效率已在具有极其剧烈运动的现实世界的实验，以及具有多种LiDAR类型、环境和运动模式的公共数据集上进行了详尽的测试。在所有测试中，Point-LIO都实现了与其他最先进的LIO算法相当的计算效率和里程计精度，同时显著提高了系统带宽。

作为里程计，Point-LIO可以用于各种自主任务，例如轨迹规划、控制和感知，尤其是在涉及非常快的自运动的情况下 (例如，在存在剧烈振动和高角速度或线速度的情况下) 或需要高速里程计输出和建图 (例如，用于高速反馈控制和感知)。

将来可以在没有惯性测量的LiDAR系统，以及动态物体检测系统中探索逐点策略。

本文仅做学术分享，如有侵权，请联系删文。