紧耦合LiDAR-IMU-腿部里程计系统

文章：Tightly-Coupled LiDAR-IMU-Leg Odometry with Online Learned Leg Kinematics Incorporating Foot Tactile Information

作者：Taku Okawara , Kenji Koide , Aoki Takanose, Shuji Oishi, Masashi Yokozuka, Kentaro Uno, and Kazuya Yoshida

编辑：点云PCL

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

从技术角度观察行业发展，努力跟上时代的步伐。公众号致力于点云处理，SLAM，三维视觉，具身智能，自动驾驶等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。文章未申请原创，未经过本人允许请勿转载，有意转载联系微信920177957。

摘要

本文提出了一种紧耦合的LiDAR-IMU-腿部里程计系统，能够有效应对无特征环境与可变形地形等挑战性场景。作者开发了一种基于在线学习的神经腿部运动学模型，通过融合足部触觉信息（地面反作用力）隐式表达机器人足部与地面间的非线性动力学特性。该模型的在线训练机制使其能自适应机器人负重变化（如执行运输任务时）及复杂地形条件。

基于神经自适应腿部里程计因子与腿部运动学模型预测的在线不确定性估计，我们在统一因子图上联合优化运动学模型在线训练与里程计估计，确保两者的一致性。通过四足机器人在两类极端场景下的实地实验验证了所提方法：1）代表无特征可变形地形的沙滩环境；2）包含沥青、砾石（可变形地形）及草地等多种地形的校园场景。实验结果表明，融合神经腿部运动学模型的里程计系统性能优于现有先进方法。详见项目主页：

https://takuokawara.github.io/RAL2025_project_page/

主要贡献

基础腿部里程计算法:传统腿部里程计算法基于关节运动（腿部正向运动学）在理想条件下（地面平坦、无变形、无足部打滑）估计机器人速度。该模型在理想条件下合理，但在地面变形或足部打滑时表现不佳。Michael 等人提出基于 EKF 的方法，结合腿部正向运动学和 IMU，通过零均值高斯噪声的随机游走模型约束接触足位置，以应对挑战性场景。

基于本体与外部传感器融合的足式机器人里程计估计:融合外部传感器（如相机、LiDAR）和本体传感器（如 IMU、关节编码器）可增强对无特征环境的鲁棒性并减少漂移。已有研究提出基于因子图优化的松耦合和紧耦合视觉-IMU-腿部里程计，以应对动态物体和无特征环境。该方法未充分考虑地面依赖现象（如足部打滑、地面或腿部变形）导致的误差。David 等人提出基于简单线性模型估计速度偏差，Yang 等人则基于卡尔曼滤波器在线标定运动学参数。这些方法在无特征区域表现良好，但未考虑极端无特征环境与地形变化同时存在的挑战性场景。

融合触觉信息的腿部里程计模型:触觉信息包括足部反作用力和接触状态（抬起或触地），可通过力传感器、关节扭矩或触觉传感器测量。多数方法仅用接触检测来约束运动预测，而 Fourmy 等人提出利用足部接触力估计机器人位移，即通过力除以质量计算加速度，再积分得到速度和位移。Kang 等人在统一因子图上联合估计里程计和外部力。现有研究假设机器人质量恒定，但实际任务（如运输或加装设备）可能导致负载变化。神经腿部运动学模型，通过在线学习自适应机器人负载和地形变化。

基于学习的足式机器人运动学模型:与前述基于模型的腿部正向运动学方法不同，基于学习的腿部运动学模型能够更好地刻画依赖地形的非线性动力学特性（如足部打滑、腿部或地面变形）。这些研究通过结合学习模型与外感受传感器约束，实现了鲁棒且精确的里程计估计。文献提出了一种基于不变EKF的里程计估计方法，融合IMU测量与结合触觉信息（足部反作用力）的学习型腿部正向运动学。其学习模型仅通过足部力传感器数据输出各接触足的权重（即可靠性），并利用权重计算腿部运动学预测的加权平均值，从而提升估计精度。

针对无特征环境与可变形地形等挑战性场景，本文提出一种紧耦合LiDAR-IMU与腿部运动学约束的里程计估计算法。当传统腿部里程计算法的假设条件失效时，通过将足部触觉信息（反作用力、接触状态）融入腿部运动学模型，直接表征足部与地面的动态交互特性。

足部反作用力会随土壤参数、摩擦系数及下陷深度等地形特征动态变化。但由于足地交互模型具有强非线性特征，精确辨识这些参数极为困难。为此采用神经网络隐式表征这类难以严格建模的动力学特性，并通过在线训练机制使模型能自适应机器人负重变化与地形类型。基于提出的神经自适应腿部里程计因子，在统一因子图上同步执行里程计估计与网络在线训练，确保二者的一致性。本工作是对前期轮式运动学模型研究的拓展（该模型未考虑轮轴扭矩等触觉信息），主要贡献包括：

1. 神经腿部运动学模型：融合足部反作用力的在线训练网络模型，可自适应机器人负载与地形条件变化；

2. 神经自适应里程计因子：在统一因子图上实现里程计估计与运动学模型在线训练的联合优化；

3. 运动约束不确定性在线估计：实时计算基于腿部运动学的运动约束协方差矩阵，确保可变形地形等挑战条件下的约束合理性。

图片

图1. (a) 校园场景下的里程计估计结果，该场景包含地形条件变化和无特征区域(b-1、b-2和b-3)。为验证我们基于在线学习的运动学模型(神经腿部运动学模型)对机器人负载变化的适应能力，实验过程中途移除了3kg外部配重。

主要内容

A.系统概述

图片

图2. 系统框架概览。为实现可靠的足式机器人状态估计，我们在统一因子图上同步执行LiDAR-IMU-腿部里程计与神经腿部运动学模型的在线训练。本方法提出的网络在推断机器人运动旋量ξt时，不仅融合本体感知数据，还引入了足部触觉信息。

如图2所示，我们在统一因子图上同步执行里程计估计与神经腿部运动学模型的在线训练，以保持所有约束的一致性。该模型的在线训练增强了四足机器人对负重变化和地形条件的适应能力——由于足部反作用力会随机器人负载和地形类型动态变化，该机制能有效利用足部触觉信息（反作用力）进行运动预测。

为平衡网络训练的精度与计算成本，我们将模型分为在线训练和离线训练两部分：

• 离线模型：在里程计估计过程中保持固定
• 在线模型：动态更新以适应实时变化

关键机制：

1. 初始化假设：算法默认机器人在估计初期处于特征丰富的环境，此时在线网络开始动态调整。

2. 自适应收敛：当网络收敛至适应当前地形与负载的状态时，基于神经网络的运动约束将具备高可靠性，即使在极端条件下也能保证估计精度。

3. 持续优化：通过因子图优化，模型始终与里程计估计同步在线更新，以应对负载与地形的动态变化。

B 腿部神经网络运动学模型概述

1. 网络输入与输出

我们开发了一种神经网络（神经腿部运动学模型），能够基于本体传感器（关节编码器、IMU）和触觉信息（足部反作用力）输出6自由度运动旋量（机器人本体的平移与旋转速度）。网络输入数据为时间序列传感器数据，包括：

• IMU数据（线加速度）
• 关节角度
• 关节扭矩
• 足部力传感器值

2. 触觉信息的融合意义

解决地形依赖问题：足部反作用力包含地形变形与打滑信息（受摩擦力和地面特性影响），可增强模型对可变形地形的适应性。

替代方案：由于实验平台（Unitree Go2）未直接提供3D反作用力测量，采用关节扭矩间接表达（基于腿部运动方程），并辅以1D足部力传感器（Unitree Foot Pad）补偿精度。

学习优势：相比传统基于运动方程的显式建模，神经网络能隐式表征复杂动力学（如未知干扰、质量/惯量变化），避免精确参数辨识的困难。

3. 网络结构设计

如图3所示，网络分为两部分以平衡计算成本与精度：

• 离线模型：学习与负载/地形无关的静态特征（如基础腿部运动学、惯性传播）。
• 在线模型：动态适应地形与负载变化，规模较小（因静态特征主导运动学约束）。

C. 自适应非极大值抑制（ANMS）的实现

图片

图3，腿部. 神经网络运动学模型结构。

离线学习模型的训练

训练流程

离线学习模型旨在提取与机器人负载和地形无关的恒定特征，以实现精确运动预测。其训练需满足以下条件：在线学习模型（自适应运动预测层）随负载和地形变化时，离线模型需保持通用性。通过将多样化环境（图4）和负载配置纳入训练数据集，确保离线模型提供的恒定特征能有效补充在线模型的自适应响应，从而提升模型在不同地形与负载场景下的性能与适应性。

图片

图4. 离线批量学习各地形数据集示意图

离线训练细节

1. 数据集构建：

• 环境类型数Ne=7，负载配置数Nm=4（1kg/2kg/3kg外挂配重），共Ns=28组序列
• 单序列时长约10分钟，涵盖可变形/硬质地面及斜坡等场景（图4）

2. 训练参数：

• 优化器：Adam（学习率5×10⁻⁴，2500轮次，批量大小50）
• 接触状态标定：基于足部力传感器阈值法离线分析
• 运动旋量真值：通过全向LiDAR（Livox MID-360）与IMU紧耦合里程计获取

3. 数据采集：

• 人工操控Unitree Go2完成斜坡、台阶等复杂运动
• 局部运动策略确保LiDAR-IMU里程计无累积误差

网络性能评估

为验证触觉信息融合与大旋转权重（w₃=200）在损失函数中的重要性，本研究对比了以下三种网络架构：

• 基准网络（完整提案网络）
• 无触觉信息网络
• 小旋转权重网络（w₃=1）

测试方法：

• 使用10%离线训练数据集（图4）作为验证集
• 计算每行进1米的平均相对轨迹误差（RTE）

定量结果（见表I）

图片

可以得出结论：

1. 触觉信息必要性：在复杂地形中，触觉信息使平移预测精度提升67%

2. 旋转权重优化价值：大旋转权重（w₃=200）使方位角误差降低至原1/3

在线学习模型训练

1. 紧耦合优化框架：首次在因子图中统一优化在线模型训练与里程计估计，通过神经自适应因子实现二者一致性。

2. 动态不确定性管理：基于滑动窗口的实时协方差估计，自适应调整运动约束权重，提升可变形地形下的鲁棒性。

3. 工程实现优化：结合LibTorch（自动微分）与GTSAM（因子图优化），高效求解高维非线性问题。

实现细节

该优化的目标函数（即图2所示的因子图）由多个部分组成，包括基于激光雷达的匹配代价因子 、基于IMU的预积分因子、基于腿部运动学模型的约束，以及其他因子（如多层感知机（MLP）时间转换因子、MLP参数固定因子 和先验因子）。关于除神经自适应腿部里程计因子之外的其他因子。使用 GTSAM 实现了该因子图，并采用 iSAM2 对其进行增量优化。为了保证在一个时间窗口内实现实时优化（例如6秒），我们使用了固定延迟平滑器。在该时间窗口内，仅优化当前激活的状态，而将边缘化的状态固定不变。

实验条件

使用配备了窄视场激光雷达（Livox AVIA）和额外配重的 Unitree Go2（如图5所示）作为实验平台，来验证所提出的里程计算算法。我们分别以10 Hz、60 Hz、500 Hz、500 Hz 和 500 Hz 的频率记录了点云、IMU数据、关节角度、关节力矩和足部受力数据，以估计机器人状态 xtx 和 MLP 参数。使用全站仪（Leica TS16）获取了地面真实轨迹作为对比基准。

图片

图5. 实验平台Unitree Go2示意图。需特别说明：全向LiDAR仅用于离线训练阶段参考轨迹记录，而实际实验（图1、图6）采用窄视场LiDAR以模拟点云严重退化场景（如隧道、月表等极端环境）。

将所提出的方法（Ours）与以下基线方法进行了比较：

• Ours w/o online learning：构建了一个消融版本，使用固定神经网络生成的运动约束替代神经自适应腿部里程计因子，该网络通过所有数据集进行简单的离线批量训练（见图4），因此不包含图3中的在线训练的自适应运动预测层。
• Ours w/o tactile information：作为第二个消融版本，构建了一个不包含触觉信息 和足部力传感器数据的网络，用于替代神经自适应腿部里程计因子以生成运动约束。
• FAST-LIO2：当前最先进的紧耦合 LiDAR-IMU 里程计方法。
• Unitree odometry：由 Unitree 自研算法生成的基于本体传感器的里程计结果。
• Unitree odometry w/ LIO：将匹配代价因子、IMU 预积分因子与 Unitree 运动约束集成入一个因子图中，用于公平对比。

实验场景与性能评估

测试场景：

• 校园：地形变化+无特征区域；
•  沙滩：严重无特征区域；

关键性能指标： ATE（绝对轨迹误差）与 RTE（相对轨迹误差），主要结果：

• 所提方法表现最佳（校园 ATE 0.29 m，沙滩 ATE 0.08 m）；
• 在线学习+触觉信息显著提升对地形和负载变化的适应能力；
• FAST-LIO2 在无特征区域失效；
• 无在线学习/无触觉的模型在不同场景下均存在明显误差。

图片

图6，（a）沙滩场景下的里程计估计结果，其中包括（b）严重缺乏特征的区域。

表II列出了所有方法的绝对轨迹误差（ATE）和相对轨迹误差（RTE）。

图片

模型性能与收敛性验证

• 图7分析结果： 在线训练模型在负重移除后快速降低误差，表现优于离线模型；
• 图8参数可视化： 使用 t-SNE 显示在线模型参数在不同序列中的演化趋势，表明模型在不同任务中具备良好的一致性与适应性。

图片

图7. 在沙滩场景（图6）中，我们的网络在不使用激光雷达和IMU因子的情况下，其运动误差的时间变化曲线。

图片

图8. 基于 t-SNE 的可视化结果

总结

本文提出了一种里程计估计算法，将激光雷达-IMU约束与融合触觉信息的在线可训练腿部运动学约束（即神经腿部运动学模型）以紧耦合的方式进行融合。为了有效利用触觉信息，神经腿部运动学模型通过在线训练，能够动态适应当前机器人的负载变化和地形条件。

实验结果表明，即使在多种环境条件下（包括不同地形类型，如沥青、碎石、草地和沙滩，以及机器人负重变化达3 kg），所提出的里程计算法仍优于当前最先进的方法（如结合 LiDAR-IMU 的 Unitree 自研算法和 FAST-LIO2），以及消融版本（去除触觉信息或在线学习的模型）。这一优越性得益于神经自适应腿部里程计因子及其在线不确定性估计能力。

未来的工作中，我们将扩展该网络功能，使其不仅能够推理机器人身体和足部所受的外力，还能进行地形分类判断，并结合姿态与足部接触状态的估计。这将使该网络能够全面应用于腿式机器人的控制任务。我们还计划将网络拓展为支持多种连杆参数的通用模型，并开源其代码。

相关阅读：2024年度历史文章大汇总