4、动力学建模：牛顿-欧拉法、拉格朗日法、关节力矩与惯性矩阵
人形机器人的运动控制，本质上是对关节力矩的精确调控。无论是双足行走时的步态切换，还是上肢操作时的力位混合控制，其底层都依赖于对机器人本体动力学的准确描述。动力学建模解决的核心问题是：给定关节位置、速度和加速度，求解所需的关节驱动力矩（正动力学），或给定关节力矩，求解系统的运动响应（逆动力学）。本章围绕两种主流建模方法——牛顿-欧拉法和拉格朗日法展开，并深入剖析关节力矩与惯性矩阵的物理意义与工程应用。
4.1 动力学建模的核心原理与工程意义
动力学模型描述了机器人运动状态（位置、速度、加速度）与关节驱动力矩之间的映射关系。对于人形机器人，这种映射关系远比工业机械臂复杂：一方面，人形机器人具有多自由度、开链与闭链混合的拓扑结构（如双腿支撑相时形成闭链）；另一方面，其运动速度高、动态性强，科氏力和离心力对系统稳定性的影响不可忽略。
从控制工程的角度看，动力学模型的价值体现在三个层面：前馈补偿（将期望加速度映射为前馈力矩，提高跟踪精度）、控制器设计（如计算力矩法、阻抗控制、力位混合控制均依赖模型）、仿真验证（在部署到真实硬件前，通过动力学仿真评估算法性能）。
两种主流建模方法各有侧重：牛顿-欧拉法从力的平衡出发，通过连杆间的速度、加速度递推和力、力矩的逆向递推，直接得到关节力矩，计算效率高，适合实时控制；拉格朗日法从系统能量出发，通过拉格朗日方程推导出闭式动力学方程，物理意义清晰，便于分析系统结构特性（如惯性矩阵的对称正定性），但计算量较大。
4.2 牛顿-欧拉法：递推求解关节力矩
牛顿-欧拉法基于牛顿第二定律和欧拉方程，将动力学问题分解为两个递推过程：外推（从基座到末端，计算每个连杆的角速度、角加速度、线加速度）和内推（从末端到基座，计算每个关节的约束力和驱动力矩）。
对于人形机器人，通常将每个肢体视为刚性连杆，关节视为理想转动副。外推过程从基座（如髋关节）开始，利用关节角度、角速度和角加速度，依次计算每个连杆质心的线加速度和角加速度；内推过程则从末端执行器（如脚掌或手掌）开始，考虑外部接触力（如地面反作用力），逆向递推得到每个关节所需的驱动力矩。
以单腿支撑相为例，支撑腿的脚掌与地面接触，地面反作用力通过内推过程传递到髋关节，进而影响躯干和摆动腿的动力学计算。这一过程中，科氏力和离心力项自然包含在速度递推公式中，无需单独建模。
4.2.1 递推公式与计算流程
设连杆 i 的角速度为 ωi，角加速度为 αi，质心线加速度为 aci，则外推过程的核心公式为：
// 外推：从基座到末端
ω_{i+1} = ω_i + q̇_{i+1} · z_{i+1}
α_{i+1} = α_i + q̈_{i+1} · z_{i+1} + ω_i × (q̇_{i+1} · z_{i+1})
a_{ci+1} = α_{i+1} × r_{i+1,ci} + ω_{i+1} × (ω_{i+1} × r_{i+1,ci}) + a_{i+1}
// 内推：从末端到基座
F_i = m_i · a_ci
N_i = I_i · α_i + ω_i × (I_i · ω_i)
f_i = f_{i+1} + F_i
n_i = n_{i+1} + r_{i,ci} × F_i + r_{i+1,ci} × f_{i+1} + N_i
τ_i = n_i^T · z_i其中，zi 为关节 i 的旋转轴方向向量，ri,ci 为连杆 i 质心相对关节 i 的位置向量，Ii 为连杆 i 的惯性张量。实际工程中，这些向量和矩阵均需在统一的基坐标系下表示，通常使用旋转矩阵进行坐标变换。
4.2.2 工程实现要点
- 坐标系约定：推荐使用修正的 DH 参数或 PoE（Product of Exponentials）公式建立运动学，确保递推过程中坐标变换的一致性。
- 惯性参数辨识：连杆的质量、质心位置、惯性张量需通过 CAD 模型或系统辨识获得，参数误差会直接影响力矩计算的准确性。
- 计算效率优化：利用连杆间的稀疏耦合关系，可进一步简化计算。例如，人形机器人上肢与下肢在行走时的动力学耦合较弱，可分段建模。
4.3 拉格朗日法：基于能量的闭式建模
拉格朗日法从系统的动能 T 和势能 U 出发，定义拉格朗日函数 L = T - U，然后通过拉格朗日方程推导动力学方程：
d/dt(∂L/∂q̇) - ∂L/∂q = τ对于 n 自由度的机器人系统，展开后得到标准的二阶动力学方程：
M(q)q̈ + C(q,q̇)q̇ + G(q) = τ其中，M(q) 为 n×n 惯性矩阵，C(q,q̇) 为科氏力和离心力矩阵，G(q) 为重力项。拉格朗日法的优势在于：惯性矩阵 M(q) 天然对称正定，这一数学性质对控制器设计至关重要（如计算力矩法需要求 M(q) 的逆）。
4.3.1 惯性矩阵的物理意义
惯性矩阵 M(q) 的对角元素 Mii 表示关节 i 的等效转动惯量，非对角元素 Mij（i≠j）表示关节 i 和 j 之间的惯性耦合。对于人形机器人，当腿部关节快速运动时，惯性耦合会导致躯干产生扰动，这正是行走控制中需要补偿的关键因素。
| 矩阵元素 | 物理含义 | 工程影响 |
|---|---|---|
| Mii（对角） | 关节 i 的等效转动惯量 | 决定关节加速度对力矩的响应速度 |
| Mij（非对角） | 关节 i、j 的惯性耦合 | 导致多关节协调运动时的力矩耦合 |
| M(q) 的逆 | 系统惯性传递特性 | 用于计算力矩控制器的前馈解耦 |
4.3.2 科氏力与离心力矩阵
C(q,q̇) 矩阵描述了速度对系统动力学的影响。当机器人高速运动时，科氏力（由不同关节速度耦合产生）和离心力（由同一关节速度产生）会显著影响系统行为。例如，人形机器人快速摆臂时，科氏力会对躯干产生侧向力矩，影响行走稳定性。
工程中，C 矩阵的选取并非唯一，但需满足 Ṁ - 2C 为反对称矩阵的性质，这一性质在 Lyapunov 稳定性分析中具有重要作用。实际建模时，可通过 Christoffel 符号从 M(q) 推导出 C(q,q̇)，保证该性质的成立。
4.4 关节力矩的构成与计算案例
关节力矩 τ 由四部分构成：惯性力矩 M(q)q̈、科氏/离心力矩 C(q,q̇)q̇、重力矩 G(q) 以及摩擦力矩 τfriction。在工程实践中，前三项通常由动力学模型计算，摩擦力矩则通过实验辨识获得。
4.4.1 案例：双足机器人膝关节力矩计算
背景与约束：某双足机器人（总质量 60kg）在平坦地面以 1.2m/s 速度行走，步频 2Hz，支撑相占周期 60%。膝关节在支撑相中期需承受最大力矩。已知大腿质量 8kg、小腿+脚掌质量 5kg，大腿长度 0.45m，小腿长度 0.42m。
初始现象与目标：初步采用静力学模型（仅考虑重力）计算膝关节力矩，发现行走时实际电机电流比理论值高 35%，且高速行走时跟踪误差增大。目标是通过完整动力学建模，准确计算膝关节力矩，为电机选型和前馈控制提供依据。
关键输入数据：
- 支撑相中期：膝关节角度 ≈ 15°（屈曲），角速度 ≈ 0.8 rad/s，角加速度 ≈ 2.5 rad/s²
- 躯干质心加速度：水平方向 0.3 m/s²，垂直方向 0.2 m/s²
- 地面反作用力：垂直方向 720N，水平方向 85N（摩擦系数 0.12）
分析判断过程：
- 首先计算重力项：Gknee = (mthigh·dthigh + mshank·Lthigh)·g·sin(θknee) ≈ 8×0.2×9.8×sin15° + 5×0.45×9.8×sin15° ≈ 4.1 + 5.7 = 9.8 N·m
- 计算惯性项：Mknee·q̈ ≈ (Ithigh + mshank·Lthigh²)·q̈ ≈ (0.15 + 5×0.45²)×2.5 ≈ 3.2 N·m
- 计算科氏项：C·q̇² ≈ mshank·Lthigh·dshank·q̇² ≈ 5×0.45×0.2×0.64 ≈ 0.29 N·m
- 考虑地面反作用力传递：Fgrf 对膝关节的力矩 ≈ 720×0.42×sin15° ≈ 78.3 N·m
实际操作步骤：将上述各项叠加，得到支撑相中期膝关节总力矩 ≈ 9.8 + 3.2 + 0.29 + 78.3 ≈ 91.6 N·m。对比静力学模型（仅重力+地面反力）≈ 88.1 N·m，差异主要来自惯性项和科氏项。
结果对比与复盘：采用完整动力学模型后，前馈力矩精度提升约 25%，电机电流跟踪误差从 12% 降至 4%。同时发现，高速行走时科氏项占比从 0.3% 升至 2.1%，验证了高速场景下动力学补偿的必要性。风险点在于惯性参数辨识误差（约 5%）会直接映射到力矩计算中，需通过实验辨识校准。
4.5 常见误区与总结复盘
误区一：忽略科氏力和离心力。在低速或小范围运动场景下，科氏项确实可以忽略；但人形机器人行走时，摆动腿的角速度可达 3-5 rad/s，科氏项对髋关节力矩的贡献可达 10% 以上。建议根据任务速度范围评估是否需要完整模型。
误区二：惯性矩阵视为常数。M(q) 随关节位形变化显著。例如，当膝关节从伸直（0°）到屈曲（90°）时，小腿对髋关节的等效转动惯量可变化 3 倍以上。控制器设计时必须考虑 M(q) 的时变性。
误区三：忽视摩擦力矩。谐波减速器的摩擦力矩可达峰值力矩的 5%-10%，且具有非线性（Stribeck 效应）。建议在动力学模型中增加摩擦补偿项，并通过实验辨识库仑摩擦和粘性摩擦系数。
误区四：两种方法混用导致不一致。虽然牛顿-欧拉法和拉格朗日法数学等价，但若在仿真中使用拉格朗日模型、在实时控制中使用牛顿-欧拉模型，且两者参数不一致，会导致前馈力矩偏差。建议统一建模工具链，并定期交叉验证。
总结：本章系统介绍了人形机器人动力学建模的两种核心方法。牛顿-欧拉法以递推计算见长，适合实时力矩前馈；拉格朗日法以闭式方程见长，适合控制器设计与系统分析。关节力矩的准确计算需要综合考虑惯性、科氏、重力和摩擦力四部分，其中地面反作用力对下肢关节影响最大。工程实践中，应根据任务需求选择合适的建模方法，并通过实验辨识校准模型参数，确保动力学模型的可靠性和实时性。