← 返回 2026-02-24

四足机器人的接触锚定本体感知里程计 Contact-Anchored Proprioceptive Odometry for Quadruped Robots

Minxing Sun, Yao Mao 📅 2026-02-19 👍 1 2026-07-13 08:35

卡尔曼滤波四足机器人接触锚定本体感知里程计状态估计

仅用IMU和电机测量，通过接触锚定实现四足机器人低漂移里程估计

前置知识

本体感知

指仅依靠机器人自身的内部传感器（如IMU、关节编码器、力矩传感器）来感知自身状态，不依赖外部环境传感器如摄像头或LiDAR。IMU提供线加速度和角速度，编码器提供关节角度和速度，力矩传感器提供关节扭矩信息。这些信息经过融合可以得到机器人的位姿、速度等状态估计。

本文完全基于本体感知，理解IMU漂移问题、编码器量化噪声、以及如何从关节扭矩推断接触状态是读懂本文的基础。

接触锚定

核心创新概念，将触地腿视为世界坐标系中的固定锚点。当腿触地时记录足尖的世界坐标位置c_i^W，在支撑阶段假设该点静止，通过当前身体帧的运动学反推身体位置和速度。这样每次触地事件都提供一个间歇性的世界帧约束，抑制IMU积分的长期漂移。关键是要正确识别哪些腿处于有效接触状态。

这是本文区别于传统方法的核心思想，所有后续的算法设计都围绕如何可靠地记录和使用这些接触锚点展开。

容积卡尔曼滤波

一种非线性贝叶斯滤波方法，通过在高斯分布的球面-径向交点处选取一组确定性采样点来近似积分，避免了扩展卡尔曼滤波（EKF）需要计算雅可比矩阵的问题。CKF选取2n个点（n为状态维度），权重相等，数值稳定且精度高于EKF，适用于强非线性系统。

本文使用CKF处理逆运动学观测模型中的非线性，避免解析或数值计算雅可比矩阵的复杂性，这是IKVel-CKF模块的技术基础。

研究动机

四足机器人在没有摄像头或LiDAR等外部感知设备的情况下进行可靠的状态估计仍然充满挑战。IMU的双积分对偏差、时序不匹配和离散采样噪声极度敏感，导致快速漂移。现代方法虽然引入腿部运动学提供速度约束，但关键难点在于确定哪些腿提供有效约束，尤其是在不完美的接触条件下。直接从关节速度计算足端速度时，编码器量化和数值微分会产生尖峰，影响估计稳定性。对于轮腿机器人，接触点会随着轮子滚动而移动，进一步增加了复杂度。现有方法在长时间运行、楼梯爬行、复杂地形等场景下仍然存在显著的累积误差。

本文的目标是开发一个统一的本体感知里程计框架，仅使用IMU和电机测量（关节角度、速度、估计扭矩），实现四足、双足和轮腿机器人的可靠位姿和速度估计。核心目标是通过接触锚定机制抑制长期漂移，在水平和垂直通道都保持高精度，同时处理编码器噪声、轮腿接触传播、偏航漂移等具体技术挑战，在多种平台上验证鲁棒性并公开实现和数据。

与已有工作不同的是，现有方法要么依赖外部传感器引入脆弱性，要么仅使用瞬时速度约束无法有效抑制长期位置漂移。本文的独特角度是将每个触地事件记录为世界坐标系中的离散锚点，在支撑阶段将其视为固定约束，从而提供间歇性的世界帧位置反馈。这种方法与形态无关，根据当前接触端执行器自动形成约束，统一处理点足和轮腿平台。相比仅依赖IMU积分或瞬时运动学约束的方法，接触锚定提供了长时程的几何一致性约束，特别适用于长时间、长距离的任务。

核心方法

CAPO的核心直觉是将触地腿视为世界坐标系中的固定锚点，通过记录触地时的世界坐标位置并在支撑阶段利用运动学反推身体状态，获得间歇性的世界帧约束来抑制漂移。技术路线包含四个关键组件：首先通过基于关节力矩的足部力估计选择可靠的接触腿；其次在触地时记录世界坐标足迹位置并用于支撑阶段的位置和速度观测；第三引入支撑面高度聚类和时间衰减校正防止高程漂移；第四对于轮腿平台通过有效轮转角度补偿处理接触点滚动；最后用IKVel-CKF滤波编码器引起的速度尖峰，并基于多接触几何一致性进行偏航估计。整个框架统一适用于双足、四足和轮腿机器人。

接触锚定的本质区别在于将传统的连续速度约束改为离散的位置锚点约束。当腿i触地时，通过前向运动学计算世界坐标位置p_ee,i,k^W = p_B,k^W + R_B,k^W p_ee,i,k^B并记录为c_i^W。在支撑阶段（i在C_k中），假设接触点静止（c_i^W的点速度为零），则身体位置可以通过p_~B,k^W(i) = c_i^W - R_B,k^W p_ee,i,k^B反推，身体速度通过v_~B,k^W(i) = -R_B,k^W omega_B,k叉乘p_ee,i,k^B + p_~dot_ee,i,k^B获得。多个接触腿的观测通过平均融合。这种机制在每次触地时提供世界帧位置重置，在支撑阶段提供连续约束，有效抑制长期漂移。

方法步骤详情

步骤1：接触选择。从关节扭矩和几何雅可比估计端执行器力f^B = (JJ^T)^(-1)J tau，当垂直分量f_z^W小于等于阈值f_th时认为腿i处于接触状态，触地事件通过i在C_k中且i不在C_k-1中检测。步骤2：足迹记录。在触地时初始化或重置世界坐标足迹记录c_i^W = p_ee,i,k^W。步骤3：接触锚定观测。支撑阶段从足迹记录反推身体位置p_~B,k^W(i) = c_i^W - R_B,k^W p_ee,i,k^B和速度v_~B,k^W(i) = -R_B,k^W omega_B,k叉乘p_ee,i,k^B + p_~dot_ee,i,k^B，多个接触的观测平均融合。步骤4：高度校正。维护支撑面高度记录集H = {(h_n, w_n, t_n)}，触地时丢弃过期记录，若原始高度与现有平面匹配则吸附并刷新权重，否则创建新记录。步骤5：轮腿补偿。计算有效轮转增量Delta psi_eff = Delta psi - Delta beta，沿地面传播接触点c_i,k^W = c_i,k-1^W + r_w Delta psi_eff t_hat_k。步骤6：IKVel-CKF滤波。维护6维笛卡尔状态x_i,k = [r_i,k, r_dot_i,k]^T，用CKF从关节测量更新，滤波后的速度替换前向运动学速度。步骤7：偏航估计。从多接触几何一致性计算成对偏航并圆平均，用增益alpha_k校正当前偏航。

技术新颖性

技术新颖性体现在四个方面：一是接触锚定机制本身，将离散触地事件转化为世界帧位置约束，与形态无关的统一框架；二是支撑面高度聚类和时间衰减校正，轻量级地防止高程漂移，通过可配置分辨率吸附新观测高度到先前观测平面；三是IKVel-CKF在逆运动学观测模型下直接从关节角度和速度估计笛卡尔足端速度，避免雅可比计算，有效抑制编码器量化引起的尖峰；四是基于多接触几何一致性的偏航估计，在长时间站立时通过连续重锚定航向到接触几何有效抑制IMU偏航漂移，当IMU偏航约束不可靠时提供运动学衍生的航向参考。相比线性卡尔曼滤波、EKF近似、不变EKF和UKF，CKF提供数值稳定、无雅可比的非线性滤波规则，避免了UKF的缩放参数。

**Fig. 1**: Footfall records provide continuous body position feedback

**Fig. 2**: Encoder-derived joint rates provide continuous body velocity feedback

**Fig. 3**: Footstep tracking correction strategy flowchart

**Fig. 4**: Correction of footstep position

**Fig. 5**: Sagittal-plane model of a rounded point foot

**Fig. 6**: Rolling-contact modeling bias for R = 0.03 m and touchdown pitch a1 = 80°, with a2 ∈ [60°, 140°]

**Fig. 7**: Estimated body linear velocity

**Fig. 8**: Representative hip–foot velocity feedback (leg 1)

**Fig. 9**: Illustration of the geometric cue exploited for yaw correction

实验结果

核心发现是接触锚定能够有效抑制长期漂移，在四个平台上的闭环轨迹验证了方法的鲁棒性。仿真实验（AlienGo模型）中，CAPO和CAPO-CKE在平坦地面回路MAE为X轴0.5671m、Y轴0.0818m、Z轴0.0072m，终端误差0.6157m，显著优于SLAM基线（Z轴MAE 0.6503m，终端误差2.078m）。楼梯爬行场景中，CAPO因单个速度尖峰导致错误接触判断引起持续Z估计退化，而CAPO-CKE通过IKVel-CKF抑制异常保持稳定高度跟踪。实机实验显示Astrall A（点足）约200m水平回路误差0.1638m、约15m垂直回路误差0.219m；Astrall B（轮腿）约200m水平回路误差0.2264m、约15m垂直回路误差0.199m；Astrall C（轮腿）约700m水平回路误差7.68m、约20m垂直回路误差0.540m；Unitree Go2 EDU约120m水平回路误差2.2138m、约8m垂直回路误差小于0.1m。垂直通道误差显著低于SLAM，证明接触锚定对高程漂移的抑制效果。水平回路误差与回路长度近似线性关系（Astrall C归一化约1.1%），表明航向误差和轮滑是主要误差来源。单步周转偏航误差约10度，长步约30度，说明残余对姿态耦合和接触柔性的敏感性。

**Fig. 10**: Locomotion trajectories in simulation

**Fig. 11**: Estimated x-position during flat-ground walking

**Fig. 12**: Estimated y-position during flat-ground walking

**Fig. 13**: Estimated z-position during flat-ground walking

**Fig. 14**: Estimated z-position during stair climbing

**Fig. 15**: Estimated z-velocity during stair climbing

**Fig. 16**: Unitree Go2 EDU planar closed-loop traversal on a basketball court

查看结构化数据

任务	指标	本文	基线	提升
平坦地面行走（仿真）	平均绝对误差（MAE）	X: 0.5671m, Y: 0.0818m, Z: 0.0072m	SLAM基线 X: 0.5329m, Y: 0.7149m, Z: 0.6503m	垂直通道MAE从0.6503m降至0.0072m，提升约99%
平坦地面行走终端误差（仿真）	欧氏终端位置误差	0.6157m	SLAM基线 2.078m	终端误差降低约70%
水平闭环轨迹（实机）	平面闭合误差	Astrall A: 0.1638m/200m, Astrall B: 0.2264m/200m, Go2: 2.2138m/120m	传统IMU积分方法（文中未给出具体数值，但强调快速漂移）	Astrall归一化误差约0.08-0.11%，Go2约1.8%，显著优于纯IMU积分
垂直闭环轨迹（实机）	高度闭合误差	Astrall A: 0.219m/15m, Astrall B: 0.199m/15m, Astrall C: 0.540m/20m, Go2: <0.1m/8m	未给出具体基线数值	垂直误差保持小范围，高程漂移得到有效控制
楼梯爬行（仿真）	高度估计稳定性	CAPO-CKE稳定跟踪，无持续退化	CAPO因速度尖峰产生错误接触判断，Z估计持续退化	IKVel-CKF有效抑制编码器异常，提升鲁棒性

局限与改进

作者承认的局限性包括：当前轮腿流水线没有显式检测或补偿轮地滑移，滑移违反滚动/接触假设可能是长程运行漂移的主要来源，特别是下台阶时可能涉及短暂弹道相违反静止接触假设；接触选择依赖固定垂直力阈值，对典型点足步态鲁棒但对轮腿模式下的长时间轻接触可能次优，降低阈值可能损害爬楼梯鲁棒性；轮接触传播假设局部平坦地面，沿水平面传播接触点，在斜坡上会引入偏差。作者还注意到当IMU偏航约束禁用时，运动学衍生的航向参考会优雅降级，但对单步周转误差约10度、长步约30度，表明对未建模姿态耦合和接触柔性的残余敏感性。实验方面主要使用闭环轨迹而非绝对真值对比，平台数量有限（四个），场景相对简单（平坦地面、楼梯），缺乏复杂野外地形的验证。

独立分析的弱点

独立分析的弱点包括：轮滑检测缺失是最关键的弱点，轮滑时接触点不再静止，违反核心假设会导致系统性偏差，建议通过运动学-惯性不一致性检测（如预测和观测速度差异）或力矩线索（异常力突变）进行滑移感知，对滑移接触降低观测权重或完全忽略。固定阈值接触选择在步态切换和轻接触时可能不稳定，建议采用自适应或概率接触模型，结合力矩大小、历史接触状态、运动一致性等多因素进行贝叶斯接触状态推断。平坦地面假设在斜坡上引入高度误差，建议从多接触几何估计局部支撑平面法向和高度，沿平面切向传播接触点。IKVel-CKF计算开销大（超过两倍），在资源受限平台上可能不可行，建议条件启用（仅在高动态场景）、低速率更新或更高效的sigma点实现。实验缺乏绝对真值对比和复杂地形验证，建议引入运动捕捉或激光跟踪真值，在碎石、草地、泥地等野外场景测试。偏航估计在IMU禁用时仍有漂移，说明未建模的姿态耦合和接触柔性的影响，需要更精确的姿态解耦和接触柔顺性建模。

未来方向

作者提出的未来方向包括：显式滑移检测和轮腿接触的下加权或门控，使用运动学-惯性不一致性和或力矩线索；超越固定阈值力门控的自适应接触分类，在不牺牲楼梯鲁棒性的情况下处理长时间轻接触；将轮接触传播从水平面假设推广到使用估计支撑平面法向的局部斜坡地形；减少IKVel-CKF运行时开销（条件启用、低速率更新、更高效的sigma点实现）。基于成果可延伸的方向包括：扩展到更多平台（ANYmal、MIT Mini Cheetah）和机器人形态（双足、六足），验证框架的泛化能力；集成学习组件用于接触分类和滑移检测，利用神经网络从多模态传感器数据学习鲁棒决策；结合有限的视觉或LiDAR约束作为补充，在适当场景下提供稀疏全局校正；开发实时自适应参数调整机制，根据地形和运动模式动态调整阈值、权重和增益；探索接触锚定与SLAM的融合，用接触约束初始化和约束SLAM优化；研究更复杂的轮滑模型，部分滑移条件下的接触传播；在野外长时间任务（数公里、数小时）上验证长期漂移性能。

复现评估

复现评估情况良好，作者公开了完整实现和代表性数据集。GitHub仓库（github.com/ShineMinxing/Ros2Go2Estimator.git）提供完整代码，包括MATLAB/MEX实现和ROS 2接口，覆盖所有关键组件（接触选择、足迹记录、高度校正、轮腿补偿、IKVel-CKF、偏航估计）。Google Drive提供Unitree Go2 EDU试验的ROS bag，包含同步视频，支持复现评估和离线分析。算力需求适中，主要计算在IKVel-CKF（增加超过两倍计算时间），但可通过禁用或条件启用减少开销。实验配置相对简单（平坦地面、楼梯），不需要特殊场地。开源程度高，代码加数据加视频完整，社区可直接复现和扩展。需要注意MATLAB/MEX环境配置，以及ROS 2依赖。缺乏预训练模型或复杂配置，复现难度较低。作者提供了详细的公式和伪代码，便于理解和实现。

论文图表

Fig. 17: Unitree Go2 EDU repeated ascent/descent over a single low step

Unitree Go2 EDU在单个低台阶上重复上升下降。左图（a）RViz可视化，显示五次上下台阶的轨迹。右图（b）实时估计位置，z轴显示重复的台阶高度变化。在五个周期内，估计器在垂直通道没有视觉可观察的累积漂移，返回初始区域时Delta z的绝对值小于0.1m。

这张图展示了垂直场景下的高程一致性，是Table II垂直回路误差小于0.1m的直观证据。读者可以看到重复台阶爬升的稳定跟踪，验证高度校正机制的有效性。

Fig. 18: Astrall closed-loop trials: real-time estimated (x, y, z) traces for Robot A (MP) and Robot B (MW)

Astrall闭环试验的实时估计轨迹。左图（a）Robot A（点足）：约200m水平回路和约15m垂直回路，误差0.1638m和0.219m。右图（b）Robot B（轮腿）：类似回路，误差0.2264m和0.199m。xyz用蓝红绿，轨迹返回起点附近。

这张图展示了Astrall平台的实验结果，与Table II数据对应。读者可以比较点足和轮腿机器人的性能差异，理解接触条件对精度的影响。

Fig. 19: Astrall Robot C closed-loop trials

Astrall Robot C闭环试验。左图（a）约700m水平闭环，误差7.68m。右图（b）约20m垂直闭环，误差0.540m。轨迹更复杂，覆盖更大区域。水平误差相比短程试验显著增加，但垂直误差仍保持相对小。

这张图展示了长程试验的性能，说明误差随距离增长的趋势。读者可以量化理解长期运行下的累积误差，支持关于航向误差和轮滑的讨论。

Table I: ESTIMATION ERROR DURING FLAT-GROUND WALKING IN SIMULATION

仿真平坦地面行走期间的估计误差对比。包含三种方法（CAPO-CKE、CAPO、SLAM）在X、Y、Z轴的平均绝对误差（MAE）和欧氏终端位置误差。数据：X MAE分别为0.5671m、0.5099m、0.5329m；Y MAE为0.0818m、0.0885m、0.7149m；Z MAE为0.0072m、0.0073m、0.6503m；终端误差为0.6157m、0.6232m、2.078m。

这个表格量化总结了仿真实验的性能，特别突出了垂直通道的优势（Z轴MAE从0.6503m降至0.0072m，提升99%）。是Fig. 11-13的数值总结，支持CAPO优于SLAM基线的结论。

Table II: CLOSED-LOOP PERFORMANCE

四个平台的闭环性能总结。Unitree Go2 EDU：约120m水平回路误差2.2138m，约8m垂直回路Delta z的绝对值小于0.1m。Astrall A（点足）：约200m水平0.1638m，约15m垂直0.219m。Astrall B（轮腿）：约200m水平0.2264m，约15m垂直0.199m。Astrall C（轮腿）：约700m水平7.68m，约20m垂直0.540m。

这个表格是实机实验的核心结果汇总，量化展示了四个平台上的性能。读者可以比较不同平台和不同回路长度下的误差，理解方法的鲁棒性和局限性。支持关于水平误差与长度近似线性关系、垂直误差保持小范围的主要发现。