接触锚定式四足机器人本体感知里程计

摘要

在不依赖相机或激光雷达的情况下，为腿式机器人实现可靠的里程计仍面临IMU漂移和关节速度传感噪声的挑战。本文提出一种纯本体感知的状态估计器，仅利用IMU与电机测量值协同估计机体位姿与速度，其统一架构适用于双足、四足及轮腿式机器人。核心思想是将每条触地腿视作运动学锚点：基于关节扭矩的足端力矩估计筛选可靠接触，对应的落足点位置通过间歇性的世界坐标系约束抑制长期漂移。为消除长距离行进中的高度漂移，我们引入轻量级高度聚类与时间衰减校正机制，将新记录的落足高度对齐至已观测的支撑平面。针对编码器量化导致的足端速度观测误差，采用逆运动学容积卡尔曼滤波器直接从关节角度与速度中滤出足端速度。该实现还通过多接触几何一致性缓解偏航角漂移，并在IMU偏航约束不可靠时优雅降级为运动学推导的航向参考。我们在四台四足平台（三台Astrall机器人及一台Unitree Go2 EDU）上通过闭环轨迹进行评估：Astrall尖足机器人A在模拟200米水平环路和15米垂直环路中的误差分别为0.1638米和0.219米；轮腿式机器人B的对应误差为0.2264米和0.199米。轮腿式机器人C在模拟700米水平环路中误差为7.68米，模拟20米垂直环路误差为0.540米。Unitree Go2 EDU在模拟120米水平环路中误差为2.2138米，模拟8米垂直环路的垂直误差小于0.1米。代码见：github.com/ShineMinxing/Ros2Go2Estimator.git

English

Reliable odometry for legged robots without cameras or LiDAR remains challenging due to IMU drift and noisy joint velocity sensing. This paper presents a purely proprioceptive state estimator that uses only IMU and motor measurements to jointly estimate body pose and velocity, with a unified formulation applicable to biped, quadruped, and wheel-legged robots. The key idea is to treat each contacting leg as a kinematic anchor: joint-torque--based foot wrench estimation selects reliable contacts, and the corresponding footfall positions provide intermittent world-frame constraints that suppress long-term drift. To prevent elevation drift during extended traversal, we introduce a lightweight height clustering and time-decay correction that snaps newly recorded footfall heights to previously observed support planes. To improve foot velocity observations under encoder quantization, we apply an inverse-kinematics cubature Kalman filter that directly filters foot-end velocities from joint angles and velocities. The implementation further mitigates yaw drift through multi-contact geometric consistency and degrades gracefully to a kinematics-derived heading reference when IMU yaw constraints are unavailable or unreliable. We evaluate the method on four quadruped platforms (three Astrall robots and a Unitree Go2 EDU) using closed-loop trajectories. On Astrall point-foot robot~A, a sim200\,m horizontal loop and a sim15\,m vertical loop return with 0.1638\,m and 0.219\,m error, respectively; on wheel-legged robot~B, the corresponding errors are 0.2264\,m and 0.199\,m. On wheel-legged robot~C, a sim700\,m horizontal loop yields 7.68\,m error and a sim20\,m vertical loop yields 0.540\,m error. Unitree Go2 EDU closes a sim120\,m horizontal loop with 2.2138\,m error and a sim8\,m vertical loop with less than 0.1\,m vertical error. github.com/ShineMinxing/Ros2Go2Estimator.git

接触锚定式四足机器人本体感知里程计

Contact-Anchored Proprioceptive Odometry for Quadruped Robots

摘要

Support