MIT Cheetah 3状态估计算法复现：用Python手把手实现EKF融合IMU与编码器
MIT Cheetah 3状态估计算法实战：Python实现EKF融合IMU与编码器
在动态四足机器人领域，状态估计是确保稳定运动控制的基础环节。MIT Cheetah系列机器人凭借其卓越的动态性能，其核心算法一直备受关注。本文将带您从零开始，用Python完整实现Cheetah 3所采用的状态估计算法，重点解决IMU与编码器数据融合的实际工程问题。
1. 环境准备与基础概念
实现一个可靠的状态估计系统，首先需要明确几个关键组件的作用。IMU提供身体的角速度和线性加速度测量，但存在漂移问题；电机编码器通过正运动学计算足端位置，能有效修正IMU的累积误差。这两类传感器的互补特性，正是EKF能够发挥优势的基础。
核心工具链配置：
# 创建Python虚拟环境 python -m venv cheetah_ekf source cheetah_ekf/bin/activate # Linux/Mac cheetah_ekf\Scripts\activate # Windows # 安装核心依赖 pip install numpy scipy matplotlib pyquaternion四元数处理是状态估计中的关键难点，我们使用pyquaternion库来简化相关运算。以下是四元数基础操作的代码示例：
from pyquaternion import Quaternion # 初始化四元数 q = Quaternion(axis=[1, 0, 0], degrees=30) # 绕x轴旋转30度 # 四元数乘法（表示旋转组合） q2 = Quaternion(axis=[0, 1, 0], degrees=45) q_combined = q * q2 # 先x轴旋转30度，再y轴旋转45度 # 旋转向量应用 v = np.array([0, 0, 1]) v_rotated = q.rotate(v)2. EKF算法框架搭建
扩展卡尔曼滤波的核心在于预测-更新两个阶段的交替进行。我们首先构建算法的主框架：
class CheetahEKF: def __init__(self, init_state, init_covariance): self.state = init_state # [position, velocity, quaternion, foot_positions...] self.covariance = init_covariance self.last_imu_time = None def predict(self, imu_data): """IMU驱动的预测步骤""" dt = imu_data['timestamp'] - self.last_imu_time if self.last_imu_time else 0.01 self._predict_state(imu_data, dt) self._predict_covariance(imu_data, dt) self.last_imu_time = imu_data['timestamp'] def update(self, encoder_data): """编码器驱动的更新步骤""" y, H, R = self._prepare_update(encoder_data) self._apply_kalman_gain(y, H, R) def run_filter(self, imu_stream, encoder_stream): """主循环处理传感器数据流""" while True: # 实际实现中需处理传感器同步问题 self.predict(next(imu_stream)) if encoder_data_available(): self.update(next(encoder_stream))状态向量设计： 我们采用与原始论文一致的状态定义：
| 状态分量 | 维度 | 说明 |
|---|---|---|
| r | 3 | 机体中心在世界坐标系下的位置 |
| v | 3 | 机体在世界坐标系下的速度 |
| q | 4 | 世界系到机体系的旋转四元数 |
| p_i | 3×N | N个足端在世界系中的位置 |
| b_f | 3 | 加速度计偏置 |
| b_ω | 3 | 陀螺仪偏置 |
3. IMU预测模型实现
IMU预测是EKF中最为复杂的部分，涉及四元数积分和噪声传播。以下是关键实现细节：
def _predict_state(self, imu_data, dt): # 解包IMU数据 acc = imu_data['acceleration'] gyro = imu_data['angular_velocity'] # 去除偏置 acc_unbiased = acc - self.state['b_f'] gyro_unbiased = gyro - self.state['b_ω'] # 四元数积分 q = Quaternion(self.state['q']) omega = np.append(gyro_unbiased, 0) # 转为四元数格式 q_dot = 0.5 * q * Quaternion(omega) q_new = q + q_dot * dt q_new = q_new.normalised # 位置和速度预测 rot_mat = q.rotation_matrix gravity = np.array([0, 0, -9.81]) acc_world = rot_mat.T @ acc_unbiased + gravity self.state['v'] += acc_world * dt self.state['r'] += self.state['v'] * dt self.state['q'] = q_new.elements # 足端位置预测（假设接触点不变） # 偏置建模为随机游走 self.state['b_f'] += np.random.normal(0, self.process_noise['b_f'], 3) self.state['b_ω'] += np.random.normal(0, self.process_noise['b_ω'], 3)协方差预测需要计算状态转移雅可比矩阵F：
def _predict_covariance(self, imu_data, dt): F = self._compute_jacobian_F(imu_data, dt) Q = self._compute_process_noise_Q(dt) self.covariance = F @ self.covariance @ F.T + Q def _compute_jacobian_F(self, imu_data, dt): # 构建雅可比矩阵各区块 F = np.eye(self.state_dim) # 位置相关 F[0:3, 3:6] = np.eye(3) * dt # ∂r/∂v # 速度相关 q = Quaternion(self.state['q']) acc = imu_data['acceleration'] - self.state['b_f'] F[3:6, 6:10] = self._compute_dv_dq(q, acc) * dt # 四元数相关 gyro = imu_data['angular_velocity'] - self.state['b_ω'] F[6:10, 6:10] = self._compute_dqdot_dq(q, gyro) * dt F[6:10, 16:19] = self._compute_dqdot_dbw(q) * dt return F4. 编码器更新模型实现
编码器更新是校正IMU漂移的关键。我们需要将编码器读数转换为足端位置观测：
def _prepare_update(self, encoder_data): # 正运动学计算足端位置 foot_positions_body = [self._leg_forward_kinematics(leg_angles) for leg_angles in encoder_data['leg_angles']] # 转换到世界坐标系 q = Quaternion(self.state['q']) foot_positions_world = [self.state['r'] + q.rotate(foot_pos) for foot_pos in foot_positions_body] # 构建观测残差 y = [] for i, foot_pos in enumerate(foot_positions_world): observed_diff = encoder_data['foot_obs'][i] # 编码器观测的足端位置 expected_diff = q.rotate(foot_pos - self.state['r']) y.append(observed_diff - expected_diff) y = np.concatenate(y) # 构建观测矩阵H H = np.zeros((len(y), self.state_dim)) for i in range(len(encoder_data['foot_obs'])): start_row = i * 3 H[start_row:start_row+3, 0:3] = -q.rotation_matrix # ∂y/∂r H[start_row:start_row+3, 6:10] = self._compute_dy_dq(q, foot_positions_world[i]) H[start_row:start_row+3, 10+i*3:13+i*3] = q.rotation_matrix # ∂y/∂p_i # 观测噪声R R = self._compute_observation_noise_R(encoder_data) return y, H, R正运动学实现示例：
def _leg_forward_kinematics(self, joint_angles): """简化版的正运动学计算""" l1, l2, l3 = 0.2, 0.2, 0.1 # 大腿、小腿、足端长度 theta1, theta2, theta3 = joint_angles # 简化模型，实际需根据机器人具体结构实现 x = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2) + l3 * np.sin(theta1 + theta2 + theta3) y = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2) + l3 * np.cos(theta1 + theta2 + theta3) z = 0 # 平面模型 return np.array([x, y, z])5. 系统集成与性能优化
将各模块整合后，我们需要关注几个关键性能指标：
状态估计精度评估：
- 位置误差：与地面真实轨迹的偏差
- 姿态误差：四元数夹角
- 速度误差：与参考速度的差异
实时性优化技巧：
# 使用Numba加速关键计算 from numba import jit @jit(nopython=True) def fast_matrix_update(F, P, Q): return F @ P @ F.T + Q # 稀疏矩阵优化 from scipy.sparse import lil_matrix def sparse_jacobian(): H = lil_matrix((obs_dim, state_dim)) # 只设置非零元素 return H.tocsr()调试建议：
- 先验证各模块单独功能：
- IMU积分能否正确预测短时运动
- 正运动学计算是否符合预期
- 分阶段测试：
- 静态测试（机器人静止）
- 动态测试（简单平移运动）
- 完整运动测试（包含旋转）
- 可视化工具：
def plot_state_evolution(ekf_states): fig, axs = plt.subplots(3, 1) axs[0].plot([s['r'][0] for s in ekf_states], label='X') axs[1].plot([Quaternion(s['q']).yaw_pitch_roll[0] for s in ekf_states], label='Yaw') axs[2].plot([np.linalg.norm(s['v']) for s in ekf_states], label='Velocity')在实际项目中，我们发现最大的挑战来自传感器同步和足端接触检测。一个实用的技巧是使用接触力阈值来判断足端是否可靠：
def is_foot_contact(foot_force, threshold=5.0): """根据足端力判断是否接触地面""" return np.linalg.norm(foot_force) > threshold对于更复杂的运动场景，可以考虑引入运动学接触约束作为额外的观测源，这能显著提升在滑动情况下的估计鲁棒性。