4、动力学基础：牛顿-欧拉方程、拉格朗日方法，四足机器人动力学建模，地面反作用力与质心动力学
四足机器人的运动控制，无论是基于模型预测控制（MPC）的全身控制（WBC），还是基于强化学习的策略网络，其底层物理逻辑都离不开动力学。动力学研究的是力与运动之间的关系，它回答了“施加怎样的关节力矩，机器人会产生怎样的加速度”这一核心问题。本章将系统梳理动力学建模的两大经典体系——牛顿-欧拉方程与拉格朗日方法，并深入探讨四足机器人特有的地面反作用力（GRF）与质心（CoM）动力学，为后续的力控、步态规划与状态估计打下坚实的理论基础。
上图展示了本章的知识架构。牛顿-欧拉法与拉格朗日法从不同视角推导出等价的动力学方程，而质心动力学则是对系统的高层抽象，将复杂的多连杆动力学简化为质心运动与地面反作用力之间的关系。这种分层理解是设计高效控制器的关键。
4.1 核心原理：从牛顿-欧拉到拉格朗日
牛顿-欧拉法基于牛顿第二定律和欧拉方程，对每个连杆进行受力分析，通过“外推”（从基座到足端计算速度与加速度）和“内推”（从足端到基座计算力与力矩）两个递归过程，得到关节力矩与运动状态的关系。其最终形式可写为：
M(q) * q̈ + C(q, q̇) * q̇ + G(q) = τ + J^T * F_ext其中，M(q) 为惯性矩阵，C(q, q̇) 为科氏力和离心力项，G(q) 为重力项，J 为接触雅可比矩阵，F_ext 为外部接触力（如地面反作用力）。
拉格朗日法则从能量角度出发，定义拉格朗日函数 L = T - U（动能减势能），通过欧拉-拉格朗日方程：
d/dt(∂L/∂q̇) - ∂L/∂q = τ推导出与牛顿-欧拉法完全等价的动力学方程。拉格朗日法的优势在于其系统性和对约束的自动处理，但计算效率较低，通常用于离线仿真和理论分析。
4.2 四足机器人动力学建模流程
建立四足机器人完整动力学模型通常遵循以下步骤：
- 运动学准备：定义各连杆坐标系，使用 DH 参数或李代数描述关节变换，计算各连杆的质心位置、惯性张量。
- 速度与加速度传递：从基座（机身）开始，依次计算每个连杆的角速度、角加速度、线加速度。
- 力的反向递推：从足端接触力开始，依次计算每个关节所需的驱动力矩，同时考虑连杆重力、惯性力和科氏力。
- 接触约束处理：根据足端是否与地面接触，施加相应的运动约束（如足端速度为零）或力约束（摩擦锥）。
- 模型简化与验证：对于控制设计，通常将完整模型简化为质心动力学模型或线性倒立摆模型，并通过仿真或实验数据验证模型精度。
| 建模方法 | 计算复杂度 | 物理直观性 | 适用场景 |
|---|---|---|---|
| 牛顿-欧拉法 | O(n)，n为连杆数 | 高（力/力矩直接可见） | 实时控制、力控、WBC |
| 拉格朗日法 | O(n³) | 中（能量标量运算） | 离线仿真、模型分析、教学 |
| 质心动力学 | O(1)（简化后） | 高（宏观运动） | 步态规划、MPC、状态估计 |
4.3 地面反作用力与质心动力学：控制的核心
四足机器人运动控制的本质，是通过调节足端与地面的接触力（GRF）来驱动质心（CoM）运动。质心动力学将复杂的多连杆系统抽象为单刚体模型：
m * r̈_c = Σ F_i + m * g
L̇_c = Σ (p_i - r_c) × F_i其中，m 为总质量，r_c 为质心位置，F_i 为第 i 条腿的 GRF，p_i 为对应足端位置，L_c 为质心角动量。第一个方程描述质心平动，第二个方程描述绕质心的转动。
地面反作用力的约束条件是动力学可行性的关键：
- 单边约束：法向力 F_z ≥ 0，地面只能推不能拉。
- 摩擦锥约束：水平力 |F_x| ≤ μ·F_z，|F_y| ≤ μ·F_z，防止打滑。
- ZMP 约束：零力矩点（ZMP）必须位于支撑多边形内，保证运动稳定。
在实际控制中，这些约束通常被线性化为摩擦锥的多边形近似，并作为二次规划（QP）问题的约束条件，求解最优的 GRF 分配。
4.4 案例拆解：基于质心动力学的 MPC 步态控制器
以 MIT Cheetah 3 和 Mini Cheetah 的 MPC 控制器为例，其核心思想是：在每个控制周期（如 50Hz）内，基于当前质心状态和期望步态，求解未来 0.5~1.0 秒内的最优 GRF 序列。
具体实现步骤：
- 状态估计：通过 IMU 和足端力传感器，估计当前质心位置、速度、姿态角及角速度。
- 步态规划：根据用户指令（前进速度、转向角速度）生成足端落点轨迹和支撑相/摆动相时序。
- MPC 求解：将质心动力学离散化，构建预测模型，以 GRF 为决策变量，最小化质心轨迹跟踪误差和 GRF 变化率，同时满足摩擦锥约束。
- 关节力矩计算：将 MPC 解出的 GRF 通过雅可比矩阵映射为关节力矩，并叠加摆动腿的 PD 控制力矩。
- 执行与反馈：将力矩指令发送至关节电机，同时更新状态估计，进入下一控制周期。
关键参数与调试经验：
| 参数 | 典型值 | 影响 |
|---|---|---|
| MPC 预测时域 | 10~20 步（0.2~0.4s） | 过长导致计算量大且模型失准，过短则前瞻性不足 |
| 质心高度 | 0.25~0.35m（A1） | 影响 ZMP 稳定裕度和足端力分配 |
| 摩擦系数 μ | 0.6~0.8 | 过小导致约束过紧，运动保守；过大则可能打滑 |
| QP 权重矩阵 | Q_pos=10, Q_vel=1 | 平衡位置跟踪与速度跟踪的优先级 |
结果对比：在仿真环境（如 MuJoCo）中，采用 MPC 控制器后，机器人可以在 0.5m/s 的行走速度下保持姿态角误差小于 2°，而传统的基于 ZMP 的静态步行控制器在相同速度下姿态误差超过 5°。在实物测试中，MPC 控制器能够应对 10° 以内的斜坡和轻度外力推搡。
4.5 常见误区与总结复盘
误区一：混淆质心（CoM）与机身中心（Body Center）。质心是机器人质量分布的平均位置，在运动过程中会随关节姿态变化而移动。如果直接用机身中心代替质心进行动力学计算，会导致 GRF 分配偏差，尤其在高速奔跑或携带负载时。
误区二：忽略科氏力和离心力项。在低速行走时，C(q, q̇) 项可以忽略，但在跑步或快速转弯时，科氏力对关节力矩的贡献可达 20% 以上。建议在高速运动场景下使用完整的牛顿-欧拉递推，而非简化模型。
误区三：摩擦锥约束线性化过度。将圆形摩擦锥近似为四边形或六边形时，会引入保守性。如果约束过紧，MPC 可能无法找到可行解，导致控制器输出饱和。建议在 QP 求解器中加入松弛变量，允许在极端情况下轻微违反约束。
误区四：忽视足端力传感器的动态响应。大多数力传感器（如 ATI Mini45）的带宽为 1kHz 左右，但在高频振动下会产生谐振。如果控制频率为 1kHz，建议对力信号进行 50~100Hz 的低通滤波，避免将传感器噪声引入动力学计算。