MIT Cheetah机器人刚体模型详解：从DH参数到足端位置计算的保姆级推导

MIT Cheetah机器人刚体建模实战从坐标系定义到足端轨迹计算的完整推导在四足机器人研究领域MIT Cheetah系列机器人以其卓越的运动性能和创新设计闻名。对于想要深入理解其运动控制原理的工程师和学生来说掌握其刚体建模方法是必经之路。本文将抛开复杂的控制理论聚焦于最基础的刚体模型构建过程手把手带你完成从坐标系定义到足端位置计算的全套数学推导。1. 坐标系定义与机器人结构解析MIT Cheetah采用典型的四足哺乳动物形态设计每条腿由三个主动关节组成分别对应髋关节的侧摆abduction/adduction、髋关节的前后摆动flexion/extension和膝关节的屈伸。这种三自由度设计为机器人提供了足够的灵活性来实现各种步态。**机身坐标系Body Frame**通常固定在机器人几何中心Z轴垂直向上X轴指向机器人前进方向Y轴由右手定则确定。每条腿的基坐标系则位于髋关节旋转中心前右腿FR基坐标系X轴指向机器人右侧Y轴向后Z轴向上前左腿FL基坐标系X轴指向机器人左侧Y轴向后Z轴向上后右腿RR基坐标系X轴指向机器人右侧Y轴向前Z轴向上后左腿RL基坐标系X轴指向机器人左侧Y轴向前Z轴向上注意不同研究团队可能采用不同的坐标系约定实际应用中需与具体代码实现保持一致2. DH参数建模详解Denavit-HartenbergDH参数法是机器人运动学建模的标准工具。对于MIT Cheetah的每条腿我们可以建立如下DH参数表关节θ (rad)d (m)a (m)α (rad)描述1q100-π/2髋关节侧摆2q20L10髋关节前后摆动3q30L20膝关节屈伸其中q1, q2, q3为关节角度变量L1为大腿长度髋关节到膝关节L2为小腿长度膝关节到足端对应的齐次变换矩阵可通过标准DH公式计算def dh_matrix(theta, d, a, alpha): return np.array([ [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta)], [sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta)], [0, sin(alpha), cos(alpha), d], [0, 0, 0, 1] ])3. 前向运动学完整推导基于DH参数我们可以逐级计算从基坐标系到足端的变换矩阵。以右前腿为例从髋关节基坐标系到第一个关节的变换T_1^0 \begin{bmatrix} \cos q1 0 -\sin q1 0 \\ \sin q1 0 \cos q1 0 \\ 0 -1 0 0 \\ 0 0 0 1 \end{bmatrix}第一个关节到第二个关节的变换T_2^1 \begin{bmatrix} \cos q2 -\sin q2 0 L1 \cos q2 \\ \sin q2 \cos q2 0 L1 \sin q2 \\ 0 0 1 0 \\ 0 0 0 1 \end{bmatrix}第二个关节到足端的变换T_3^2 \begin{bmatrix} \cos q3 -\sin q3 0 L2 \cos q3 \\ \sin q3 \cos q3 0 L2 \sin q3 \\ 0 0 1 0 \\ 0 0 0 1 \end{bmatrix}最终足端在基坐标系中的位置可通过矩阵连乘得到T_3^0 T_1^0 \cdot T_2^1 \cdot T_3^2提取位置向量矩阵的第四列前三行即可得到足端坐标def forward_kinematics(q1, q2, q3, L1, L2): T01 dh_matrix(q1, 0, 0, -pi/2) T12 dh_matrix(q2, 0, L1, 0) T23 dh_matrix(q3, 0, L2, 0) T03 np.dot(T01, np.dot(T12, T23)) return T03[:3, 3]4. 实际应用中的坐标系转换在实际控制系统中我们通常需要将足端位置从腿基坐标系转换到机身坐标系再到世界坐标系。完整的转换流程包括腿基坐标系到机身坐标系的静态变换由机械设计决定# 前右腿基坐标系相对于机身坐标系的位置 FR_base_offset np.array([0.18, -0.1, 0]) # 示例值需根据实际设计调整机身坐标系到世界坐标系的动态变换取决于机器人位姿def body_to_world(position, robot_pos, robot_ori): # robot_pos: 机器人在世界坐标系中的位置 # robot_ori: 机器人的姿态四元数或旋转矩阵 R quaternion_to_matrix(robot_ori) return R.dot(position) robot_pos完整的足端世界坐标计算def get_foot_position_world(leg_joint_angles, leg_type, robot_pos, robot_ori): # 计算腿基坐标系中的足端位置 foot_pos_leg forward_kinematics(*leg_joint_angles) # 加上腿基坐标系的偏移 foot_pos_body foot_pos_leg get_leg_base_offset(leg_type) # 转换到世界坐标系 return body_to_world(foot_pos_body, robot_pos, robot_ori)5. 模型验证与调试技巧建立数学模型后验证其正确性至关重要。以下是几种实用的验证方法极限位置测试将关节置于0°或其它特殊角度验证足端位置是否符合机械设计# 所有关节为0时足端应位于(L1L2, 0, 0)附近考虑DH参数定义 assert np.allclose(forward_kinematics(0, 0, 0, 0.2, 0.2), [0.4, 0, 0])可视化检查使用机器人工具箱或3D绘图工具绘制机器人姿态import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D def plot_leg(q1, q2, q3): fig plt.figure() ax fig.add_subplot(111, projection3d) # 绘制各关节位置 # ... plt.show()逆向验证对于已知的足端位置使用逆运动学求解关节角度再代入正运动学验证常见问题排查足端位置镜像错误检查左右腿的坐标系定义是否对称Z轴方向相反确认DH参数中的α符号是否正确长度比例异常核实L1和L2的取值与实际机械尺寸一致6. 从数学模型到代码实现将数学模型转化为高效代码时有几个优化技巧值得注意矩阵运算优化避免不必要的矩阵乘法直接计算最终位置def optimized_fk(q1, q2, q3, L1, L2): # 直接计算足端位置跳过完整矩阵乘法 x L2*cos(q2 q3) L1*cos(q2) y sin(q1)*(L2*sin(q2 q3) L1*sin(q2)) z -cos(q1)*(L2*sin(q2 q3) L1*sin(q2)) return np.array([x, y, z])批量计算使用向量化操作同时处理多个时间步长的数据def batch_forward_kinematics(joint_angles, L1, L2): # joint_angles: N x 3数组 q1, q2, q3 joint_angles[:,0], joint_angles[:,1], joint_angles[:,2] x L2*np.cos(q2 q3) L1*np.cos(q2) y np.sin(q1)*(L2*np.sin(q2 q3) L1*np.sin(q2)) z -np.cos(q1)*(L2*np.sin(q2 q3) L1*np.sin(q2)) return np.column_stack([x, y, z])C实现示例适用于实时控制系统Eigen::Vector3d computeFootPosition(double q1, double q2, double q3, double L1, double L2) { Eigen::Vector3d position; position.x() L2*cos(q2 q3) L1*cos(q2); position.y() sin(q1)*(L2*sin(q2 q3) L1*sin(q2)); position.z() -cos(q1)*(L2*sin(q2 q3) L1*sin(q2)); return position; }7. 运动学模型在控制中的应用前向运动学模型在四足机器人控制中扮演着基础角色主要应用于足端轨迹规划在机身坐标系中规划足端运动路径通过逆运动学转换为关节指令姿态稳定性控制计算机身重心投影与支撑多边形的相对位置步态生成协调四条腿的运动时序和足端落点地形适应根据足端接触反馈调整机身姿态一个简单的足端轨迹生成示例def generate_swing_trajectory(start_pos, end_pos, height, duration, t): # 三次样条插值 if t 0: return start_pos if t duration: return end_pos ratio t / duration x start_pos[0] (end_pos[0] - start_pos[0]) * ratio y start_pos[1] (end_pos[1] - start_pos[1]) * ratio z start_pos[2] 4 * height * ratio * (1 - ratio) return np.array([x, y, z])在实际项目中我发现将运动学模块封装成独立类最为方便提供以下接口更新机器人位姿获取指定腿的足端位置批量计算所有足端位置坐标系转换工具方法这种封装方式使得上层控制算法可以专注于步态策略而不必关心底层的数学细节。