【特约综述】浙江大学刘勇教授团队：基于B样条的连续时间轨迹状态估计研究综述

文摘 2024-12-27 19:01 辽宁

本文由“机器人ROBOT”公众号原创，欢迎转载，请务必完整转载全文并在文章初始位置插入本公众号名片和链接。

欢迎引用：

吕佳俊, 郎晓磊, 李宝润, 刘勇. 基于B样条的连续时间轨迹状态估计研究综述[J]. 机器人, 2024, 46(6): 743-752.

机器人中的状态估计（state estimation）方法利用机器人携带的传感器的观测数据，对机器人的位置、姿态、速度以及加速度等状态作出最优估计，是控制与决策的基础。例如自动驾驶任务中，通过融合多传感器数据实现车辆自主定位和地图构建，从而提高自动驾驶车辆在不同环境下的智能导航和避障能力。状态估计一直以来是机器人领域中的核心问题，通常被建模为最大后验估计问题。在状态估计任务中，如何最大限度地利用传感器信息以及如何实现多传感器之间的紧耦合成为了重要的研究问题。

不同的应用场景对状态估计有着不同的要求。在四足机器人中，面对非结构的路面，以及足端落地时引起的冲击振动，高鲁棒性和实时精度是关键。微型无人机中，由于可携带的传感器数量有限，加上传感器初始化困难、感知能力有限，因此最大化信息利用率成了状态估计的关键。自动驾驶领域则更注重在各种天气和光照条件下的稳定性和可靠性，因此常使用多传感器融合方案，如摄像头、激光雷达和毫米波雷达，以提高决策能力。

多传感器融合技术[1-6]为状态估计提供丰富的多模态信息。首先，多传感器融合可以大幅提升感知能力，通过整合多种传感器数据，如相机、IMU（惯性测量单元）、激光雷达等，系统可以获得更全面的环境信息，提高自动驾驶汽车的安全性和对复杂环境的适应能力。这种融合还有助于改善传感器数据的准确性和信息冗余度，提供更多的输入数据供系统使用。此外，融合新型传感器如事件相机和4D毫米波雷达等，为状态估计技术在高动态场景和自动驾驶领域带来新的发展机会。另外，对于单传感器系统状态估计的研究，提高了该传感器应用的上下限，为多传感器耦合系统提供更多可能。

然而，多传感器融合系统给传感器信息处理带来了挑战，尤其是对状态估计器提出了更高的要求。状态估计常采用离散时间建模，但面对传感器高频特性和融合多个异频、异步传感器时，如图 1所示，时间同步问题和插值问题也需要妥善处理，以确保数据融合的准确性。总结而言，传统离散型建模方法在实际应用时存在两大类难题：

1) 异步、异频数据融合，是指不同传感器因测量时刻、测量频率不同而带来的融合困难。视觉－惯性系统中，在插值IMU的积分量时一般会引入匀速假设，从而获得与相机在相同测量时刻对应的IMU位姿；VINS-Mono算法[5]则将时间偏置转换为像素偏移。

2) 高频外感受型数据处理，是指对卷帘相机、事件相机、激光传感器的持续扫描式测量数据的妥善处理。若搭载这些传感器的平台有相对运动，则传感器测量数据会存在运动畸变，影响建图质量、状态估计精度。

图1 不同传感器的时间轴、采集频率、采集时刻示意图

总之，传感器的高频特性能为状态估计提供密集的测量序列，而多传感器融合更能提高感知能力的上限，但这也对状态估计方法提出了巨大挑战。

上述几个难点可总结为离散位姿难以满足系统对高频位姿的需求，而连续型方法将轨迹建模为连续时间函数，支持查询任意时刻的位姿、速度，自然而然地解决异步、异频、高频传感器数据处理及融合的问题。因此连续性建模方法近几年逐渐受到关注。连续时间状态估计的基本思路是使用连续函数表示估计状态，如均匀B样条函数。如图 2 所示，利用连续时间轨迹能够自然地关联不同传感器数据，构建有物理意义的误差项，使多传感器融合更直观。尽管实现更复杂，需要更多计算资源，且理论基础相对不完备，但连续时间轨迹状态估计在高频传感器数据处理及多传感器融合方向，如传感器外参标定、卷帘相机标定、事件相机里程计等领域，取得了显著的研究进展，这部分将在第3节详细介绍。

图2 基于连续时间轨迹的多源融合

本文围绕基于B样条的连续时间状态估计研究展开综述，从连续轨迹理论、离线标定应用、在线里程计应用3个方面进行分析，并展望了该方向未来可继续探索的研究点。

1. 连续时间轨迹理论

1.1 连续时间状态估计基础研究

状态估计问题通常估计离散时刻的状态量，且为保证计算量可控一般限制状态量的数量。但当使用高频率传感器（如惯性测量单元、激光雷达）时，离散时间方法存在扩展性问题：若每次测量数据都包含一个位姿变量，则将导致状态维度过大。其根本问题在于离散时间状态估计难以满足系统对高频位姿的需求。为了解决该问题，Furgale等[7]在连续时间轨迹上建模最大似然估计（MLE）问题，采用时间基函数保证状态量维度可控，并且支持查询任意时刻的位姿、速度等物理量。具体而言，采用了B样条曲线对连续时间轨迹进行建模，综合考虑了连续轨迹需要具备的局部性与可导性，并且采用均匀B样条曲线来表示惯性传感器的 6DoF 轨迹。

可导性：可导性体现为连续轨迹支持解析求导与积分运算，有利于在构建误差项时与传感器观测数据相结合，避免了高计算复杂度的数值积分运算。从图 3 中可直观地观察到，对于 k 阶样条曲线，每一条基函数曲线均可写为 k −1次多项式的形式，多项式求导十分便利。

图3 节点间距为1 s的均匀B样条基函数

局部性：局部性体现为修改样条曲线的部分参数仅仅对局部轨迹有影响，这在批量优化中有助于限制计算复杂度。从图 3 和图 4 中可发现，对于每一条基函数曲线，仅在 k 个时间区间的值非 0；并且由于任意时刻仅有 k 个基函数值非 0，因此改变某一条样条基函数曲线，仅会改变局部的轨迹，不会影响整段轨迹。

图4 轨迹定义为B样条基函数与控制点的乘积之和

文[7]在相机－IMU系统的外参标定问题上验证提出框架的有效性，但是此框架并不局限于标定问题。

Furgale等[8]进一步扩展该工作，详细介绍了B样条函数的基础理论，不再使用传统的递归定义形式，而是采用B样条曲线的矩阵形式来分析，细致介绍了B样条基函数、函数形式、函数求导、函数积分、多维函数积分、二次积分等话题，详细分析了旋转轨迹的参数化形式、角速度计算方法和对应的雅可比矩阵推导过程。Rehder等[9]提出了一种通用的标定框架，联合估计传感器之间的时空外参，利用连续时间批量估计方法求解最大似然估计问题，支持标定不同传感器组合包括但不限于相机－IMU系统、相机－陀螺仪系统、相机－加速度计系统、相机－IMU－单线激光雷达系统、相机－激光雷达系统。

Furgale等[8]讨论了基于样条函数的连续时间轨迹方法与离散时间方法的计算复杂性，其中图 5展示了连续时间状态估计问题的求解示意图[10]。使用高斯－牛顿法求解非线性优化问题主要分为4个步骤：

1) 线性化：在离散时间初始化的基础上，连续时间样条函数方法通常需要额外求解一个大型稀疏线性方程组实现样条拟合，因而产生额外的计算量。具体而言，B样条曲线拟合的计算量不会随样条阶数的增高而显著增大，且参数量小于完整非线性最小二乘法问题，额外增加的计算成本不高于高斯－牛顿法求解的1次迭代成本。

2) 误差计算：离散时间方法中状态量直接用于误差计算，样条函数方法则首先查询曲线以计算位姿、速度等参数，再进行误差计算。通常情况下，可以缓存曲线查询结果以减小重复查询的计算开销。然而，在某些在线时间标定问题中，每次迭代都需要重新查询曲线以计算误差。尽管单个误差计算并不会增加太多额外的计算负担，但当问题中涉及的误差数量庞大时，额外的计算开销可能会显著增大，尤其是时间偏置标定问题[11]。

3) 雅可比矩阵计算：雅可比矩阵的计算与误差计算类似，同样需要额外查询曲线。对于机器人领域中许多非线性估计问题，雅可比矩阵计算占据了大部分计算量[12]。这可能是基于样条函数的方法最主要的应用限制。

4) 线性系统求解：线性系统求解的计算复杂度有两个影响因素：待估参数的数量和矩阵稀疏度。连续时间样条函数方法有潜力通过减少需要估计的参数数量，减小问题的规模，实现计算加速。然而，与离散时间方法相比，样条函数方法生成的矩阵更为稠密，进行曲线查询会直接导致生成多维分块对角矩阵，且使用高阶B样条曲线会在矩阵中生成更宽的块对角线，相比于求解纯对角矩阵的系统耗时更久。

图5 连续时间状态估计方法

总体而言，连续时间状态估计方法因样条轨迹查询而导致的额外计算量，限制了其实时应用，因而在较长一段时间内仅在离线批量估计的标定问题上得到应用。Sommer等[13]发现现样条查询过程存在递归形式，可将轨迹查询过程加速2~4倍，连续时间状态估计的实时性应用逐渐有了可能性。此外分析预测轨迹的复杂程度后，采用非均匀B样条的轨迹表示形式可能是另一种加速方法。

Hug等[14]提出的HyperSLAM是一个通用的模块化的连续时间SLAM抽象框架，揭示了连续时间状态估计相对于离散时间状态估计的另一项优势，即一些传感器数据无法用来单独更新/约束6DoF位姿，如事件相机的单个事件、惯性传感器在剧烈运动场景下因超量程而仅能提供5个有效维度的观测数据等，连续时间状态估计的建模形式本身可轻松地覆盖此类数据的处理。

1.2 轨迹表示方法

在单位四元数B样条轨迹表示形式的基础上，Sommer等[15]将B样条轨迹表示法扩展至任意阶和所有李群，并推导了李群上批量连续时间状态估计所需的雅可比矩阵，探究了在李群上构建B样条轨迹时高效的初始化方法、在无约束批量优化时保持李群本身的参数性质，进一步深入探究连续时间轨迹的理论。

Ovrén等[16]按样条插值空间将连续轨迹表示法分为两类：旋转和平移分别表示的分解形式（split representation）、在 SE(3) 空间表示的联合形式。SE(3) 样条的插值隐含地将平移和旋转方向耦合在一起，线性加速度的计算更为复杂，实验中

S E

SE(3) 样条的收敛速度较慢，而且失败率更高。此外，SE(3) 样条的效率也较低，文[16]建议使用分解形式的样条表示。关于样条插值空间的更详细讨论可参考Haarbach等[17]的相关工作。

1.3 节点间距设置

在使用B样条曲线来近似复杂轨迹时，涉及到了一些超参数，如样条阶数（order）和节点间距（knot distance）。均匀B样条曲线的表示能力与样条阶数和节点间距相关。Oth等[18]在卷帘相机标定实验中发现，节点间距对标定结果有显著影响，较小的节点间距可以更好地捕捉相机畸变细节，但如果太小可能会导致过拟合。因此，节点间距参数的设置需要在精度和计算效率之间进行权衡。文中提出了一种自适应节点间距选择方法，根据残差与理论期望值的匹配来自适应地调整节点间距，以避免过拟合。Ovrén等[19]使用离散傅里叶变换将测量数据和样条基函数转换到频域中，不同节点间距的样条基函数在频率域中会产生不同的频率响应函数。文中定义了一个能量比例，用来衡量去除高频分量后的样条拟合结果与实际轨迹的接近程度，指定能量值来确定最佳的节点间距设置。除了分析最佳节点间距设置外，文中还研究了残差因子误差权重的设置问题。

2. 离线标定应用

连续轨迹被引入状态估计的契机，是处理IMU数据这样的高频数据，因而一开始主要应用于与IMU相关的离线标定问题中，后续不断扩展到其他系统的标定。

2.1 相机－IMU系统

Furgale等[7]开创性地提出了连续时间轨迹上的建模最大似然估计（MLE）问题，并通过相机－IMU系统的外参标定问题验证了所提出框架的有效性：将IMU轨迹建模为连续时间轨迹，通过查询连续时间轨迹模型得到加速度与角速度信息，并与IMU实际观测得到的信息进行比较，构建误差项，进而基于棋盘格完成图像特征数据关联，利用查询图像时刻的位姿与相机观测值基于轨迹和外参构建重投影误差项，最后采用非线性最小二乘法求解。Furgale等[11]在相机－IMU系统外参标定的基础上进一步标定了时间偏置，在查询图像位姿时考虑相机时间偏置，重投影误差项中额外包含了待估计的时间偏置。仿真和真实实验结果均证实所提出方法的时间偏置标定精度达到了亚毫秒级。Rehder等在文[9]中提出了一个支持多种传感器配置的外参标定系统，并在文[20]中进一步提出了多IMU外参标定系统，同时还支持标定IMU的内部参数，对应的开源标定方法Kalibr现在是GitHub平台上最受欢迎的相机－IMU系统标定工具包。

2.2 激光雷达－IMU系统

激光雷达－IMU系统的外参标定方法相比相机－IMU系统的难度更大。主要难点在于激光点云数据稀疏，点云数据关联困难，没有像图像中棋盘格这类的辅助关联目标物。另外，为了通过充分运动来激励IMU，采集数据时需要对装置进行剧烈晃动，但这会加剧激光点云的运动畸变。

Lv等[21]提出了一种基于连续时间轨迹的、无需目标物的激光雷达－IMU系统外参标定方法。连续轨迹的优势在于它支持查询任意激光点时间戳下的位姿，有助于去除畸变。另外运用环境中的小面片来建立点到面的关联，其中面片地图有2个来源：一是从基于纯激光里程计构建的点云地图中提取，其精度较低；二是采用经过优化的轨迹进一步构建更精确的点云地图，从而获得更高精度的面片地图。针对运动受限的平台，Lv等[22]提出了能观性觉察（observability-aware）的激光雷达－IMU系统时空外参标定方法，在充分考虑各个变量初始化的情况下，在退化运动（例如纯平移运动和匀速运动）情况下识别状态空间中的能观方向，仅更新可观测的参数，保持不能观方向的参数不变；并提出信息论度量方法选择信息量丰富的轨迹段进行标定，降低计算成本。

Zhi等[23]提出了一种支持激光雷达－相机－IMU系统的时空标定方法，观测得到的图像和激光点云之间不需要有共视区域，利用连续时间轨迹建立多种传感器间的数据联系。具体而言，作者在环境中布置多块AprilTag板，在板子的角落处贴上反光条便于激光平面点云的提取，利用AprilTag板的角点和平面点云构建数据关联。该方法同时支持传感器间的时间偏置标定。

2.3 其他系统的标定

除了外参标定，连续时间状态估计在传感器内部参数标定上也十分有效。Oth等[18]解决了卷帘相机行曝光时间的估计问题。传统的方法需要特殊的设备，而且对传感器数据的要求较高；该方法无需特殊的硬件支持，只需要用卷帘相机采集一段视频即可完成标定，且标定精度高。Huang等[24]将连续时间轨迹用于事件相机的内部参数标定，Rehder等[25]在Kalibr工具包中额外增加了单线激光雷达的外参标定。

Chen等[26]将基于连续时间轨迹估计的相机－IMU系统标定扩展到多相机－多IMU系统。提出了一种连续时间预积分方法，并利用虚拟出的中央IMU完成旋转外参初始化，采用连续时间批量优化法最小化测量残差。该方法可以标定由全局快门和卷帘快门组成的异构多相机－多IMU系统，并且得到了准确一致的标定结果。

2.4 标定应用总结

总结而言，连续轨迹在批量估计问题中（如标定）支持融合高频、异频、异步数据，有利于激光、卷帘相机、事件相机等持续测量型传感器时间域上的关联，并且对时间偏置的标定非常友好，因此连续轨迹在标定方面的应用较多。在精度方面，连续时间轨迹支持同时去除激光/卷帘相机的观测畸变与轨迹估计，因而在剧烈运动场景下具有优势，表 1 总结了连续时间模型在标定方面的应用。

表1 连续时间标定应用相关工作总结

展望未来工作，连续时间状态估计潜在的研究方向可能包括如下几个方面。现有的工作通常给予系统充分的运动激励，而在大型车等欠激励的场景下则难以对多传感器进行标定，对此可能的解决方案为Lv等提出的OA_LICalib[22]，标定过程中对退化方向进行识别，使用截断奇异值分解方法来更新状态量。另外，目前标定方法多为基于优化的方法，对初始值有一定的依赖性，大型车的传感器之间可能距离较远，对于这种大基线外参难以初始化的情况，优化可能无法收敛。这些均为未来值得研究的问题。

3. 在线里程计应用

传感器标定问题与多传感器融合SLAM问题的本质区别是，前者一次性处理采集的全部数据，采用批量优化法进行求解；而后者实时收集多传感器数据，要求尽可能及时地输出里程计估计结果并维护地图，这对系统的计算速度提出了更高的要求。下面将分别介绍连续时间轨迹与激光、视觉、惯性数据的融合方法。

3.1 激光里程计

作为基于激光的连续时间里程计的开创性工作，Alismail等[28]将迭代最近点（ICP）算法扩展至连续时间迭代最近点（CICP）算法。由于激光扫描帧中的每个激光点具有不同的时间戳，因此每个激光点对应不同的位姿，将点云视作刚性的并直接进行点云配准会引入较大的误差。传统方法大多通过对传感器运动做限制性假设来解决这一问题，而文中使用B样条曲线将传感器运动参数化为复杂度更高的连续时间轨迹，实现了一套CICP算法，达到了更高的位姿估计精度。

Quenzel等[29]提出了一种基于多自适应分辨率面元（MARS）的激光里程计，其将连续时间轨迹与高斯混合模型相结合以联合对齐局部多分辨率面元地图。稀疏体素网格确保了对地图面元的快速访问，并且自适应分辨率选择方案在不损失里程计精度的前提下有效地加快了配准速度。

Cong等[30]进一步提升了连续时间激光里程计的精度和建图质量。提出的系统主要由两大模块组成，即用于周围环境内快速自我运动估计的里程计模块和用于精确点云融合的建图模块。对于里程计，采用扫描到地图的点云配准方案，提取结构特征以提供有效的对应关系。对于重建，着眼于提高地图质量，将B样条运动模型与非刚性点云融合相结合，以方便对环境进行连续且非重复的测绘。

由于缺少惯性测量，激光系统常常会忽略掉运动学约束，这导致纯激光系统精度不高。Zheng等[31]通过在极短的时间窗口内作线性插值来拟合LiDAR连续时间轨迹，时间窗口被定义为超参数，即在运动剧烈时时间窗口更短，并且利用轨迹平滑性间接地为纯激光系统引入了运动学约束。该工作在多个数据集中的表现超越了激光惯性里程计，将纯激光系统重新带回到研究者的视野中。

3.2 视觉里程计

Spline Fusion[32]是将连续时间轨迹用于视觉里程计的开创性工作，其展示了如何使用与传感器的最小扭矩运动紧密匹配的B样条进行参数化。与传统的离散时间方法相比，连续时间方法对于解决高帧率传感器和多个不同步设备的问题特别有用，并且使用的B样条曲线公式并没有改变状态估计问题的整体复杂度。文中在卷帘合成数据集上展示了所提出的连续时间单目卷帘里程计的性能。连续时间框架下的卷帘BA（bundle adjustment）方法充分地考虑了卷帘不同行像素对应不同的相机位姿这一问题，使得连续时间卷帘里程计达到的精度显著超越传统方法。

使用RGB-D相机可以在里程计执行过程中构建更稠密、精细的地图。Kerl等[33]提出了一种用于卷帘RGB-D相机的稠密里程计，使用B样条曲线参数化相机轨迹，并分别使用深度图和RGB图构建深度误差和光度误差来优化相机轨迹。该方法还在优化过程中直接对深度图像和RGB图像中的卷帘快门进行建模，从而提高了低成本CMOS传感器的跟踪和重建质量。

基于事件的视觉传感器，例如动态视觉传感器（DVS），不像标准相机那样输出视频帧序列，而是输出异步事件流。进行像素检测时若发现场景中的亮度发生变化，就会触发事件。DVS的高动态范围和时间分辨率（微秒级）使其成为机器人和可穿戴设备等有高速计算需求的应用中非常有前途的传感器。然而，由于传感器输出的结构不同，需要利用传感器的高时间分辨率和异步特性的新算法。Mueggler等[34]使用连续时间框架来直接集成传感器传达的信息，解决基于事件的视觉传感器的自运动估计问题，提出的连续时间事件里程计在仿真数据集上以及在四旋翼飞行器执行翻转任务所获得的数据集上都获得了较好的性能。

现有的多相机SLAM系统假设所有相机的快门同步，但实际情况通常并非如此。Yang等[35]提出了一种考虑了相机异步问题的多相机SLAM系统。该框架集成了连续时间运动模型，以便在跟踪、局部建图和回环阶段关联异步信息，通过实验强调了对异步问题作合适建模的必要性。

3.3 激光－惯性里程计

在连续时间激光里程计的基础上，Lv 等[36]额外融合了IMU数据，为激光点云非刚性匹配提供了更好的初值。为了解决长时间里程计估计中累积误差导致连续时间轨迹估计全局不一致的问题，文中提出了一个二阶段的连续时间轨迹校正方法，如图 6 所示。在第一阶段，检测到回环后，首先进行离散时间位姿图优化，以优化关键帧的位姿。在第二阶段，通过保持轨迹的局部形状和将连续时间轨迹与优化后的关键帧位姿对齐，从而有效地解决了连续时间轨迹的闭环问题。作者在公开数据集和自采数据集上验证了所提出方法的准确性。实验结果表明，该方法在精度方面优于离散时间方法，尤其是在剧烈运动的情况下。

图6 连续时间轨迹闭环修正方法

Ramezani等[37]提出了Wildcat，一种新颖的连续时间激光惯性SLAM系统，可以估计机器人的6-DoF运动并有效地重建大规模环境地图。Wildcat通过不同类型的传感器（VLP-16和OS1-64）以及不同的（腿式、履带式、手动式）平台在各种环境中展示了其卓越的稳健性和多功能性。在DARPA地下挑战赛中，Wildcat在各种类型的退化和挑战环境中的鲁棒性优于其他SLAM系统。

相比文[36]方法，He等[38]在连续时间激光惯性里程计中额外融合了轮速计观测数据。横向加速度约束的加入使得轨迹估计遵循车辆的运动特性。此外，由于车辆模型参数随着不同的运动条件和轮胎压力而变化，作者在线估计了修正车辆模型参数变化的修正因子，并分析了修正因子的可观性。

连续时间轨迹在适应传感器高频特性的同时，引入了大量的控制点来精准拟合轨迹，这导致计算量极大。Nguyen等[39]推导了残差对状态量的解析雅可比阵并手写求解器，相比于低效的自动微分，整个计算过程相当于被定制化，极大提高了求解效率。实际应用中该方法可在保证准确性的前提下实时运行，对B样条曲线在在线里程计中的应用有推动意义。

3.4 视觉－惯性里程计

相比视觉里程计，视觉－惯性里程计因IMU观测数据的引入而得以估计真实尺度因子，并有更高的鲁棒性和精度。Mueggler等[40]利用基于B样条曲线的连续时间轨迹表示法来使用事件相机执行视觉－惯性里程计估计，该表示法允许直接集成微秒级的异步事件和高频惯性测量数据，显著减少了轨迹估计问题中的变量数量。

Hug等[41]研究了连续时间方法在常见的现实世界传感器设备中的适用性，将连续时间方法扩展至双目－IMU设备，提出了一个在线的连续时间双目惯性里程计框架。实验表明，该方法能很好地处理传感器异步的情况，并在公开数据集上取得了良好性能。

与文[41]同一时期的工作Ctrl-VIO[42]方法专注于卷帘相机，提出了第一个连续时间卷帘惯性里程计框架。全局相机整幅图像的像素是同时曝光的，而卷帘相机的像素是逐行曝光的，这便导致了不同行像素对应不同时刻的位姿，因此不可直接复用传统的适用于全局相机的方法。常用的解决思路是假设卷帘相机帧间的运动是匀速的，并同时建模帧间的线速度和角速度以考虑卷帘畸变问题。然而，匀速假设模型并不适用于加速度较大的情况，会造成对运动的降维建模。因此，Ctrl-VIO方法利用B样条曲线参数化相机轨迹，直接将卷帘误差项和惯性误差项建模在各自的时间戳上，从而优雅地处理卷帘畸变，如图 7所示。同时，Ctrl-VIO方法还采用了一种适用于连续时间框架的边缘化策略。实验表明，在卷帘数据集上，Ctrl-VIO方法优于先进的全局VIO方法和卷帘VIO方法。

图7 连续时间卷帘相机视觉－惯性里程计

3.5 激光－视觉－惯性里程计

激光－视觉－惯性里程计因充分利用了3种传感器的互补性而显示出强大的鲁棒性和精度，近些年来受到广泛的关注和研究。Lowe[43]提出了一种通用的适用于手持设备的建图框架，可用于几何、纹理缺失的环境。该框架利用连续时间轨迹融合来自激光雷达、相机、IMU的观测数据，且所有传感器均对优化产生约束。与最先进的视觉SLAM或激光SLAM方法相比，该框架对挑战性环境更鲁棒。

为了充分融合异步异频多模态传感器的观测数据，CLIC方法[6]通过因子图优化框架内的固定滞后平滑器来估计连续时间轨迹。为了限制连续时间固定滞后平滑器的计算复杂度，CLIC维持关键帧数恒定的滑动窗口，并概率性地边缘化B样条曲线的控制点，这允许为未来的滑动窗口优化保留先验信息。基于连续时间固定滞后平滑器，CLIC方法设计了具有多种传感器组合的紧耦合多模态SLAM算法，例如激光惯性SLAM系统和激光惯性相机SLAM系统，并自然地支持时间偏置的在线标定。更重要的是，得益于边缘化处理和推导的解析雅可比矩阵，所提出的连续时间SLAM系统可以实现实时性能。

相比于CLIC方法[6]，Coco-LIC方法[44]考虑了连续时间框架中B样条曲线控制点的密度设置问题并进一步优化了激光－视觉融合策略。主流的连续时间框架大多将控制点均匀放置，不可避免地会导致对实际轨迹建模的欠拟合或过拟合，尤其是在事先无法预知运动复杂度的情况下。Coco-LIC方法利用每0.1 s内的IMU观测数据自适应地分配该时间段内的控制点个数，在运动平缓的时间段分配更少的控制点，在运动剧烈的时间段分配更多的控制点。通过合理设置控制点，Coco-LIC方法有效地提升了连续时间里程计的精度和计算效率。此外，为了在优化阶段引入更少的控制点并在一个小滑窗内完成高效的激光－视觉－惯性融合，Coco-LIC方法直接将激光地图点与图像像素关联，避免使用相对耗时的大滑窗来优化图像像素深度。实验表明，Coco-LIC方法在退化数据集和大尺度数据集上都有出色的性能。

3.6 里程计应用总结

总结而言，连续时间轨迹状态估计问题在多传感器融合的里程计问题上已有许多研究，其基本思路是充分利用连续时间轨迹任意时间查询的特性。首先在前端数据处理部分，将每个像素/事件/激光点的测量数据视为独立观测数据以消除运动畸变从而改进前端数据关联；其次在后端优化中，为每个独立测量数据查询位姿、加速度/角速度，进而构建非线性优化问题并求解。此外在回环处理中，采用二阶段方法实现全局一致的连续时间轨迹。从实验结果来看，连续时间轨迹状态估计方法未对设备运动引入匀速运动等假设，因此在手持传感器、传感器运动不规律或剧烈运动的情况下，相比离散时间方法精度更优。然而，连续时间方法因复杂度高、求解速度慢限制了其实际部署应用。正如第1节中所提，有一些研究者发现B样条曲线轨迹查询计算存在递推规律进而在公式推导源头降低计算量；另外有研究者通过手写雅可比矩阵、手写求解器提升求解速度。整体而言，连续时间轨迹状态估计方法有望实现实时性能。

4. 总结与展望

除了离线标定法和在线里程计法外，连续时间轨迹的表达方式还在轨迹规划、SfM（运动恢复结构）等方面有一些应用。文[45-46]将B样条曲线应用于轨迹规划，规划出的轨迹利用B样条曲线进行建模。然而，与前文所述的应用略有区别，此处的B样条曲线表达的轨迹不含时间参数，仅包含位姿信息。Ovrén等[16]将连续时间轨迹应用于SfM，给出了连续时间框架下解决SfM问题的推导过程。表 2 总结了连续时间轨迹方法的相关工作。

表2 连‍续时间轨迹相关工作总结

本文针对连续时间状态估计进行了深入介绍，基于B样条曲线的连续时间SLAM方法为SLAM应用提供了一个新方向，现有方法仍然存在改进和提高空间。以下总结了未来可能的几个探索方向。

1) 非均匀样条的探索。基于均匀B样条曲线表示连续时间轨迹时，需要对轨迹的平滑性作出一些先验假设，但真实轨迹的复杂性各不相同。均匀样条分辨率设置过高会导致轨迹估计精度下降，设置过低可能会过度参数化。此外，均匀设计不方便动态调整轨迹精度。非均匀样条算法可以有效克服这些问题。非均匀样条算法的核心问题在于何时插入新的控制点。Dubé 等[49]通过比较误差数据级与噪声模型来判断轨迹是否满足精度要求，以决定控制点的插入时机。Yang等[35]则直接基于图像关键帧来插入控制点。非均匀样条算法的研究仍然是一个开放的研究课题，需要进一步探讨。

2) 连续化的状态量选择[47-48]是另一个可探究的方向。真实轨迹的复杂度高，相比而言估计值与实际值之间误差量轨迹的复杂度较低。若在求解连续轨迹时，选用线性化程度更高的误差量进行连续时间轨迹建模，则可减少控制点求解数量，从而减小优化过程中状态量的规模。

作者简介

刘勇，博士，浙江大学教授、控制学院智能驾驶与未来交通中心主任、先进智能系统研究中心副主任，浙江省机器换人专家，中组部万人计划青年拔尖人才、浙江省突出贡献青年科技人才。

主要研究方向为自主机器人与智能系统、机器人自主规划与导航控制、视觉识别与模式识别、SLAM技术及多传感器融合技术。以第一作者或通讯作者在 ICRA、CVPR、IJCAI、ICCV、IROS、ICLR、AAAI/IAAI 等知名期刊和机器人顶级会议发表论文百余篇。主持浙江省杰出青年科学基金项目，浙江省 151 人才项目等数项。获浙江省自然科学一等奖、浙江省科学技术一等奖、浙江省科学技术进步一等奖、浙江省自然科学学术二等奖。

识别二维码，访问全文PDF

参考文献（滑动查看更多）：

[1] SMITH R C, CHEESEMAN P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4): 56-68.

[2] ZUO X X, GENEVA P, LEE W, et al. LIC-fusion: LiDAR inertial-camera odometry[C]//IEEE/RSJ International Confer ence on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2019: 5848-5854.

[3] ZUO X X, YANG Y L, GENEVA P, et al. LIC-fusion 2.0: LiDAR-inertial-camera odometry with sliding-window plane feature tracking[C]//IEEE/RSJ International Conference on In telligent Robots and Systems. Piscataway, USA: IEEE, 2020: 5112-5119.

[4] SHAN T X, ENGLOT B, RATTI C, et al. LVI-SAM: Tightly coupled Lidar-visual-inertial odometry via smoothing and map ping[C]//IEEE International Conference on Robotics and Au tomation. Piscataway, USA: IEEE, 2021: 5692-5698.

[5] QINT, LI P L, SHEN S J. VINS-Mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.

[6] LV J J, LANG X L, XU J H, et al. Continuous-time fixed-lag smoothing for LiDAR-inertial-camera SLAM[J]. IEEE/ASME Transactions on Mechatronics, 2023, 28(4): 2259-2270.

[7] FURGALE P, BARFOOT T D, SIBLEY G. Continuous-time batch estimation using temporal basis functions[C]//IEEE Inter national Conference on Robotics and Automation. Piscataway, USA: IEEE, 2012: 2088-2095.

[8] FURGALE P, TONG C H, BARFOOT T D, et al. Continuous time batch trajectory estimation using temporal basis functions [J]. International Journal of Robotics Research, 2015, 34(14): 1688-1710.

[9] REHDERJ, SIEGWART R, FURGALE P. A general approach to spatiotemporal calibration in multisensor systems[J]. IEEE Transactions on Robotics, 2016, 32(2): 383-398.

[10] CIOFFI G, CIESLEWSKI T, SCARAMUZZA D. Continuous time vs. discrete-time vision-based SLAM: A comparative s tudy[J]. IEEE Robotics and Automation Letters, 2022, 7(2): 2399-2406.

[11] FURGALE P, REHDER J, SIEGWART R. Unified temporal and spatial calibration for multi-sensor systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2013: 1280-1286.

[12] SIBLEY D, MEI C, REID I, et al. Adaptive relative bundle adjustment[C]//Robotics: Science and Systems V. Cambridge, USA: MIT Press, 2009. DOI: https://doi.org/10.15607/RSS.2009 . V.023.

[13] SOMMER C, USENKO V, SCHUBERT D, et al. Efficient derivative computation for cumulative B-splines on Lie groups [C]//IEEE/CVF Conference on Computer Vision and Pattern Recognition. Piscataway, USA: IEEE, 2020: 11148-11156.

[14] HUG D, CHLI M. HyperSLAM: A generic and modular ap proach to sensor fusion and simultaneous localization and map ping in continuous-time[C]//International Conference on 3D Vi sion. Piscataway, USA: IEEE, 2020: 978-986.

[15] SOMMER H, FORBES J R, SIEGWART R, et al. Continuous time estimation of attitude using B-splines on Lie groups[J]. Journal of Guidance, Control, and Dynamics, 2016, 39(2): 242 261.

[16] OVR´ EN H, FORSS´ EN P E. Trajectory representation and land mark projection for continuous-time structure from motion[J]. International Journal of Robotics Research, 2019, 38(6): 686 701.

[17] HAARBACH A, BIRDAL T, ILIC S. Survey of higher order rigid body motion interpolation methods for keyframe anima tion and continuous-time trajectory estimation[C]//International Conference on 3D Vision. Piscataway, USA: IEEE, 2018: 381 389.

[18] OTH L, FURGALE P, KNEIP L, et al. Rolling shutter camera calibration[C]//IEEE Conference on Computer Vision and Pat tern Recognition. Piscataway, USA: IEEE, 2013: 1360-1367.

[19] OVR´ EN H, FORSS´ EN P E. Spline error weighting for robust visual-inertial fusion[C]//IEEE/CVF Conference on Computer Vision and Pattern Recognition. Piscataway, USA: IEEE, 2018: 321-329.

[20] REHDERJ,NIKOLICJ,SCHNEIDERT,etal.Extendingkali br: Calibrating the extrinsics of multiple IMUs and of individual axes[C]//IEEE International Conference on Robotics and Au tomation. Piscataway, USA: IEEE, 2016: 4304-4311.

[21] LV J J, XU J H, HU K W, et al. Targetless calibration of LiDAR-IMUsystembasedoncontinuous-time batch estimation [C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2020: 9968-9975.

[22] LV J J, ZUO X X, HU K W, et al. Observability-aware intrin sic and extrinsic calibration of LiDAR-IMU systems[J]. IEEE Transactions on Robotics, 2022, 38(6): 3734-3753.

[23] ZHI X Y, HOU J W, LU Y R, et al. Multical: Spatiotempo ral calibration for multiple IMUs, cameras and LiDARs[C]// IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2022: 2446-2453.

[24]HUANGK,WANGYF,KNEIPL.Dynamic event camera calibration[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2021: 7021 7028.

[25] REHDER J, BEARDSLEY P, SIEGWART R, et al. Spatio temporal laser to visual/inertial calibration with applications to hand-held, large scale scanning[C]//IEEE/RSJ Internation al Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2014: 459-465.

[26] CHEN S L, LI X X, LI S Y, et al. Targetless spatiotempo ral calibration for multiple heterogeneous cameras and IMUs based on continuous-time trajectory estimation[J]. IEEE Trans actions on Instrumentation and Measurement, 2023, 72. DOI: 10.1109/TIM.2023.3328688.

[27] MAYEJ,FURGALEP,SIEGWARTR.Self-supervisedcalibra tion for robotic systems[C]//IEEE Intelligent Vehicles Sympo sium. Piscataway, USA: IEEE, 2013: 473-480.

[28] ALISMAIL H, BAKER L D, BROWNING B. Continuous tra jectory estimation for 3D SLAM from actuated lidar[C]//IEEE International Conference on Robotics and Automation. Piscat away, USA: IEEE, 2014: 6096-6101.

[29] QUENZEL J, BEHNKE S. Real-time multi-adaptive resolution-surfel 6D LiDAR odometry using continuous-time trajectory optimization[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2021: 5499-5506.

[30] CONGYZ,CHENC,YANGBS,etal.3D-CSTM:A3Dcon tinuous spatio-temporal mapping method[J]. ISPRS Journal of Photogrammetry and Remote Sensing, 2022, 186: 232-245.

[31] ZHENG X, ZHU J K. Traj-LO: In defense of LiDAR-only odometry using an effective continuous-time trajectory[J]. IEEE Robotics and Automation Letters, 2024, 9(2): 1961-1968.

[32] LOVEGROVE S, PATRON-PEREZ A, SIBLEY G. Spline fu sion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras[C]//Proceedings of the British Machine Vision Conference. 2013. DOI: 10.5244/ C.27.93.

[33] KERL C, STUCKLER J, CREMERS D. Dense continuous time tracking and mapping with rolling shutter RGB-D cameras [C]//IEEE International Conference on Computer Vision. Pis cataway, USA: IEEE, 2015: 2264-2272.

[34] MUEGGLER E, GALLEGO G, SCARAMUZZA D. Continuous-time trajectory estimation for event-based vi sion sensors[C]//Robotics: Science and Systems XI. 2015. DOI: 10.15607/RSS.2015.XI.036.

[35] YANG A J, CUI C, Bˆ ARSAN I A, et al. Asynchronous multi view SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA: IEEE, 2021: 5669-5676.

[36] LV J J, HU K W, XU J H, et al. CLINS: Continuous-time tra jectory estimation for LiDAR-inertial system[C]//IEEE/RSJ In ternational Conference on Intelligent Robots and Systems. Pis cataway, USA: IEEE, 2021: 6657-6663.

[37] RAMEZANIM,KHOSOUSSIK,CATTG,etal.Wildcat: On line continuous-time 3D lidar-inertial SLAM[DB/OL].(2022 05-25) [2019-08-13]. https://arxiv.org/abs/2205.12595.

[38] HE B, DAI W C, WAN Z Y, et al. Continuous-time LiDAR inertial-vehicle odometry method with lateral acceleration con straint[C]//IEEE International Conference on Robotics and Au tomation. Piscataway, USA: IEEE, 2023: 3997-4003.

[39] NGUYENTM,XUX,JINT,etal. Eigen is all you need: Effi cient lidar-inertial continuous-time odometry with internal asso ciation[DB/OL]. (2024-02-04) [2024-06-05]. https://arxiv.org/ abs/2402.02337.

[40] MUEGGLERE,GALLEGOG,REBECQH,etal.Continuous time visual-inertial odometry for event cameras[J]. IEEE Trans actions on Robotics, 2018, 34(6): 1425-1440.

[41] HUG D, B¨ ANNINGER P, ALZUGARAY I, et al. Continuous time stereo-inertial odometry[J]. IEEE Robotics and Automa tion Letters, 2022, 7(3): 6455-6462.

[42] LANG X, L¨ U J J, HUANG J, et al. Ctrl-VIO: Continuous-time visual-inertial odometry for rolling shutter cameras[J]. IEEE Robotics and Automation Letters, 2022, 7(4): 11537-11544.

[43] LOWE T, KIM S, COX M. Complementary perception for handheld SLAM[J]. IEEE Robotics and Automation Letters, 2018, 3(2): 1104-1111.

[44] LANG X, CHEN C, TANG K, et al. Coco-LIC: Continuous time tightly-coupled LiDAR-inertial-camera odometry using non-uniform B-spline[J]. IEEE Robotics and Automation Let ters, 2023, 8(11): 7074-7081.

[45] DING W C, GAO W L, WANG K X, et al. An efficient B spline-based kinodynamic replanning framework for quadrotors [J]. IEEE Transactions on Robotics, 2019, 35(6): 1287-1306.

[46] NGUYENNT,SCHILLINGL, ANGERNMS,etal. B-spline path planner for safe navigation of mobile robots[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2021: 339-345.

[47] ZHENGX,LIMY,MOURIKISAI.Decoupledrepresentation of the error and trajectory estimates for efficient pose estimation [C]//Robotics: Science and Systems XI. 2015. DOI: 10.15607/ RSS.2015.XI.009.

[48] ANDERSON S, MACTAVISH K, BARFOOT T D. Relative continuous-time SLAM[J]. International Journal of Robotics Research, 2015, 34(12): 1453-1479.

[49] DUB´ E R, SOMMER H, GAWEL A, et al. Non-uniform sam pling strategies for continuous correction based trajectory esti mation[C]//IEEE International Conference on Robotics and Au tomation. Piscataway, USA: IEEE, 2016: 4792-4798.

联系我们：

电话：024-23970050

E-mail：jqr@sia.cn

网址：https://robot.sia.cn

机器人ROBOT

《机器人》是由中国科学院主管，中国科学院沈阳自动化研究所、中国自动化学会共同主办的科技类核心期刊，主要报道中国在机器人学及相关领域具有创新性的、高水平的、有重要意义的学术进展及研究成果。