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

本文主要是介绍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. 状态估计

4.1. 符号

本文使用了误差状态卡尔曼滤波器，相关变量的定义如下：

另外，定义了两个符号

4.2. 运动学模型

4.2.1. 状态转移模型

连续运动模型

离散运动模型

4.2.2. 测量模型

系统中存在两种测量，分别是激光雷达测量和Imu测量。二者的频率一般是不同的，所以也不是同步触发的。

本文忽略标定，则

利用位姿把点转换到全局坐标系后，根据局部平面假设有

其原理图如图2所示

Imu的测量包含角速度测量和加速度测量。

可以把激光雷达和Imu和测量联立为如下形式

4.3. 扩展卡尔曼滤波

Point-LIO的状态估计使用了紧耦合的扩展卡尔曼滤波。

4.3.1. 状态传播

设在第k步时状态为 $\bar{x}_k$ ，协方差为 $\bar{P}_k$ ，此时收到了传感器的最新测量值，则需要把状态由k步更新到k+1步，其状态转移方差为

协方差传播为

其中

4.3.2. 残差计算

激光雷达测量

利用位姿把每个激光雷达点投影到全局坐标系中，寻找最近邻点拟合为局部平面并计算残差。如果最近邻的5个点不能拟合为平面，则当前点直接进入地图，而不参与残差计算与状态更新。

其中

Imu测量

其中

把源自激光雷达测量和Imu测量组合在一起：

4.3.3. 状态更新

状态和协方差的传播给了未知状态一个先验的高斯分布

观测模型也给了一个高斯分布

把先验分布和测量模型组合起来，生成状态 $x_{k+1}$ 的后验分布。

其中 $\left \| x \right \|_A^2 = x^T A^{-1} x$ ，该优化问题是一个标准的二次规划问题，最优解 $\delta x_{k+1}^o$ 可以由卡尔曼更新得到。

继而得到

更新的状态可以用于下一步的传播。

接下来，要估计协方差：

4.4. 分析

本文提出的LIO框架，在接受每个激光雷达点时消费之并更新状态，形成了点级别的里程计。其输出频率等于激光点的频率。

点级别LIO框架的另一个优点是运动补偿。

5. 评测

主要评估三方面：

去除运动畸变；
高频率、高带宽的里程计；
饱和IMU情况下的状态估计；

5.1. 实现细节

使用C++和ROS实现：

EKF是基于作者在先前工作中开发的IKFoM工具箱(https://github.com/hku-mars/IKFoM)实现的。
使用FAST-LIO2中开发的增量k-d树 (ikd-Tree) 作为地图结构，其默认参数为：局部地图大小 L = 2000 m，空间下采样分辨率 l = 0.25 m，ikd-Tree的重平衡阈值为 $\alpha_{bal}$ = 0.6， $\alpha_{del}$ = 0.5，用于并行重建(在第二线程中)的子树大小阈值为 $N_{max}$ = 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。

5.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)）

5.3. 运动畸变处理

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

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

在所有序列中，机器人车返回起点，能够计算漂移。LiDAR数据的频率为10Hz。一个挑战是机器人车在地面上移动时的强烈振动。由于传感器套件安装在底盘上，没有任何减震器，振动将直接传递给传感器，导致剧烈的抖动运动，如图4所示的内置IMU数据所示。

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

5.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) 中观察到的运动失真。

5.3.2. 漂移结果

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

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

5.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系统的测量能力。

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

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

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

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

5.5.1. 旋转运动

使用放置在杂乱的实验室环境中的旋转平台进行 (见图10 (a1)-(a4))。在实验过程中，传感器套件由步进电机在步进角速度命令下旋转，该步进角速度命令从零逐步增加到峰值，然后在结束时减小到零。所得的峰值角速度为75rad/s (yaw)，远远超过IMU测量范围，即35rad/s。高角速度还会导致大约 $80m/s^2$ 的峰值加速度，也远远超出IMU测量范围，即大约 $30m/s^2$ 。

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

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

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

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

通过以不同的电动机初始角速度启动Point-LIO来进一步挑战它。如图12中的地图结果和表3中的旋转RMSE所示，当初始角速度低于IMU饱和值，即35rad/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正常工作的前50s期间具有可比的旋转误差。
从IMU开始饱和的50s开始，FAST-LIO2和Point-LIO-input的旋转估计开始立即发散，并且位置的估计也开始漂移然后发散。

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

5.5.2. 盘旋运动

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

此运动导致圆底部的加速度高达 $40m/s^2$ ，超过了IMU测量范围 $30m/s^2$ 。图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的估计误差始终低于其他两个。

5.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。

6. 基准结果

在各种公开数据集序列上对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的重新平衡阈值为 $\alpha_{bal} = 0.6$ ， $\alpha_{del} = 0.5$ ， $N_{max} = 0.5$
对于Point-LIO的EKF部分，将卡尔曼滤波器的LiDAR测量噪声设置为

这些参数值对于所有序列都保持相同。在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论文。

6.1. 准确率评估

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

6.1.1. RMSE基准

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

6.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在实际数据上的准确性和鲁棒性。

6.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) 明显更快，实现了相当或更好的准确率。

7. 应用

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

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

7.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测量，但是状态仍然能稳定地被估计。

7.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，确保了实时性能。实验也证实了这一点，其中控制器能够通过实时状态反馈执行稳定的受控飞行。

8. 总结

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

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

所开发系统的带宽、鲁棒性、精度和计算效率已在具有极其剧烈运动的现实世界的实验，以及具有多种LiDAR类型、环境和运动模式的公共数据集上进行了详尽的测试。在所有测试中，Point-LIO都实现了与其他最先进的LIO算法相当的计算效率和里程计精度，同时显著提高了系统带宽。作为里程计，Point-LIO可以用于各种自主任务，例如轨迹规划、控制和感知，尤其是在涉及非常快的自运动的情况下 (例如，在存在剧烈振动和高角速度或线速度的情况下) 或需要高速里程计输出和建图 (例如，用于高速反馈控制和感知)。将来可以在没有惯性测量的LiDAR系统，以及动态物体检测系统中探索逐点策略。