MIT Cheetah 四足机器人的运动学与动力学建模 (II) —— 从刚体模型到足端轨迹的算法实现

1. 从刚体模型到足端轨迹的挑战四足机器人想要像真正的猎豹一样奔跑首先得搞清楚自己的腿该怎么动。这就像你要控制一个木偶必须精确知道每根绳子拉动时木偶手脚会怎么移动。MIT Cheetah的刚体模型就是这套牵线木偶的骨架系统而前向运动学算法则是计算每根绳子关节转动时脚掌最终会落在哪里的数学工具。我刚接触这个问题时以为只要把教科书上的DH参数套进去就能搞定。结果实测发现实际机器人的腿部结构比课本例题复杂得多——髋关节有内外旋两个自由度膝关节虽然简单但和髋关节形成空间联动再加上足端是个空间点而非平面机构。第一次用标准DH法计算时足端位置误差竟然达到5cm这足以让机器人摔个跟头。2. 刚体模型的数学表达2.1 连杆坐标系建立MIT Cheetah的每条腿可以看作由3个刚体大腿、小腿、足端组成的链式结构。我的经验是先在大腿旋转中心建立基坐标系Base Frame然后按照以下规则延伸Z轴沿关节旋转轴方向X轴沿相邻Z轴的公垂线方向Y轴按右手定则确定# 示例建立髋关节坐标系 import numpy as np def create_hip_frame(abduction_angle): # 外展/内收关节的坐标系变换 R_z np.array([[np.cos(abduction_angle), -np.sin(abduction_angle), 0], [np.sin(abduction_angle), np.cos(abduction_angle), 0], [0, 0, 1]]) return R_z2.2 运动链的参数化实际调试时发现MIT Cheetah的腿部参数需要特别处理大腿长度0.21m小腿长度0.22m髋关节偏移量0.083m这个参数容易被忽略这些参数必须精确测量我当初用游标卡尺反复测量了三次才确定。特别是髋关节偏移量如果忽略这个参数足端在Y方向会有明显漂移。3. 前向运动学的实现细节3.1 变换矩阵的级联从基座到足端的完整变换需要级联四个变换矩阵髋关节外展/内收髋关节屈曲/伸展膝关节屈曲/伸展足端固定变换def forward_kinematics(q_abduction, q_hip, q_knee): # 各关节角度输入弧度 T_hip create_hip_frame(q_abduction) T_thigh thigh_transform(q_hip) T_shank shank_transform(q_knee) T_foot foot_transform() # 级联变换 T_total T_hip T_thigh T_shank T_foot return T_total[0:3, 3] # 返回足端位置3.2 姿态计算的陷阱很多开源实现只计算位置而忽略姿态但在实际步态控制中足端姿态即变换矩阵的旋转部分同样重要。我踩过的坑是没有考虑足端坐标系朝向忽略了Z轴朝下的工业约定旋转矩阵顺序错误导致姿态抖动正确的做法是同时维护位置和姿态信息可以用4x4齐次变换矩阵统一表示。4. 算法验证与调试技巧4.1 仿真验证三板斧零位验证所有关节角为零时足端应在理论位置极限位置测试将各关节转到机械限位检查是否干涉轨迹连续性检查缓慢改变关节角观察足端轨迹是否平滑我在Gazebo中建了个简易模型通过RViz可视化足端轨迹。发现当膝关节超过90度时原始算法会出现突变后来发现是反余弦函数的多值问题导致的。4.2 实际调试中的经验在机器人静止时用手动模式逐个关节微调用激光测距仪验证足端位置记录算法输出与实际测量的误差建立误差补偿表特别注意奇异点位置比如膝关节完全伸直时会出现雅可比矩阵秩亏有次半夜调试时发现足端Z坐标总是差2mm最后发现是膝关节的编码器零点漂移。这个教训让我明白前向运动学的精度不仅取决于算法更依赖传感器校准。5. 从算法到实际应用5.1 步态规划中的使用前向运动学的结果会直接影响落脚点选择摆动腿轨迹生成机身高度控制在trot步态中我通常以100Hz的频率更新足端目标位置。实测发现如果计算延迟超过5ms就会导致机身明显晃动。5.2 与力控制的配合足端位置误差会通过雅可比矩阵传递到关节力矩τ JᵀF其中F是足端期望力。这意味着前向运动学的误差会被放大特别是在单腿支撑期。我的改进方案是增加编码器采样频率使用二阶低通滤波平滑关节角在线更新DH参数针对装配误差这套算法最终在mini Cheetah上实现了0.5mm的位置控制精度足够完成小跑和跳跃动作。看着机器人第一次稳稳地走过不平整地面时那些熬夜调参的日子都值了。