4、动力学基础：牛顿-欧拉法、拉格朗日法建立双足机器人动力学模型，质心动力学、角动量守恒与地面反作用力

双足机器人的步态规划与行走控制，本质上是对其动力学行为的塑造与约束。与运动学仅描述“位置、速度、加速度”的几何关系不同，动力学揭示了“力与力矩如何产生运动”的因果链条。对于双足系统而言，动力学模型不仅是仿真验证的基础，更是设计步态规划器、构造状态估计器以及实现全身力控的核心依据。本章将系统梳理建立双足机器人动力学模型的两大经典方法——牛顿-欧拉法与拉格朗日法，并深入剖析在行走控制中极具工程价值的质心动力学、角动量守恒约束以及地面反作用力的计算与作用机制。

4.1 牛顿-欧拉法：从关节力到末端加速度的递推逻辑

牛顿-欧拉法基于牛顿第二定律和欧拉方程，从机器人的基座（或浮动基座）开始，逐连杆向外递推速度与加速度，再向内递推力与力矩。该方法物理意义明确，计算效率高（复杂度为O(N)，N为关节数），非常适合实时控制中的逆动力学求解。其核心思想是将每个连杆视为刚体，分别建立平动和转动的运动方程，并通过关节约束力将相邻连杆耦合起来。

4.1.1 核心原理：外推与内推的双向递推

牛顿-欧拉法的标准流程分为两步：外推（从基座到末端执行器）计算每个连杆的角速度、角加速度、线加速度；内推（从末端执行器到基座）计算每个连杆所受的力与力矩，并最终得到关节驱动力矩。对于浮动基座的双足机器人，基座（通常是骨盆）的加速度由重力、地面反作用力和关节力共同决定，因此需要先求解基座加速度，再执行递推。

平动：m_i · a_i = F_i - F_{i+1} + m_i · g
转动：I_i · α_i + ω_i × (I_i · ω_i) = τ_i - τ_{i+1} - r_i × F_i + r_{i+1} × F_{i+1}

4.1.2 操作流程：从运动学参数到关节力矩

1. 初始化：设定基座（骨盆）的初始加速度（通常由重力加速度和期望的质心加速度合成）。
2. 外推：对于每个连杆 i（从基座到脚踝或末端），利用关节角速度、角加速度和上一连杆的速度，递推计算当前连杆的角速度 ω_i、角加速度 α_i 和线加速度 a_i。
3. 内推：从末端（脚掌或摆动腿）开始，已知外力（如地面反作用力或零力），向内递推计算每个关节的净力 F_i 和净力矩 τ_i。
4. 关节力矩提取：关节驱动力矩等于内推得到的关节力矩在关节轴向上的投影（对于旋转关节）或力在平移轴上的投影（对于直线关节）。

4.1.3 案例拆解：单腿支撑期的逆动力学计算

假设一个简化的5连杆双足机器人（骨盆+两条大腿+两条小腿）处于单腿支撑期。已知支撑脚与地面接触且无滑动，摆动腿在空中运动。我们期望计算各关节力矩以实现给定的关节加速度轨迹。

该案例中，若地面反作用力测量噪声较大，会导致内推力矩出现高频抖动。工程上通常采用低通滤波或基于模型的卡尔曼滤波来平滑GRF信号。

4.2 拉格朗日法：从能量函数到闭式动力学方程

拉格朗日法从系统总动能和势能出发，通过拉格朗日方程推导出动力学方程。该方法不涉及关节内力，直接得到关节力矩与广义坐标、速度、加速度之间的闭式关系，形式为：

M(q)q̈ + C(q, q̇)q̇ + G(q) = τ + J^T F_ext

其中，M(q)为惯性矩阵，C(q,q̇)为科里奥利力和离心力项，G(q)为重力项，J为雅可比矩阵，F_ext为外力（如地面反作用力）。

4.2.1 核心原理：能量视角下的系统建模

拉格朗日法将系统视为一个整体，通过计算动能 T(q,q̇) 和势能 U(q) 构造拉格朗日函数 L = T - U，然后代入拉格朗日方程：

d/dt(∂L/∂q̇) - ∂L/∂q = τ

该方法特别适合处理多自由度系统的耦合效应，且方程形式规整，便于进行控制律设计（如计算力矩控制、自适应控制）。但计算复杂度较高（O(N³)），且需要大量符号运算，因此更适合离线分析与规划。

4.2.2 操作流程：从几何参数到矩阵方程

1. 定义广义坐标：选择一组独立的关节变量 q（例如髋关节、膝关节角度）。
2. 计算动能与势能：对每个连杆，计算其质心速度（通过雅可比矩阵）和高度，累加得到总动能 T 和总势能 U。
3. 求偏导：计算 ∂L/∂q̇ 和 ∂L/∂q，并代入拉格朗日方程。
4. 整理成标准形式：将结果整理为 M(q)q̈ + C(q,q̇)q̇ + G(q) 的形式，提取惯性矩阵、科里奥利矩阵和重力向量。

4.2.3 实战场景：双足机器人全身动力学仿真

在步态规划验证阶段，我们常使用拉格朗日法建立全身动力学模型，用于仿真测试。例如，在MATLAB/Simulink中，通过符号工具箱推导出 M、C、G 矩阵，然后封装为S-function进行实时仿真。相比牛顿-欧拉法，拉格朗日法更容易处理约束（如闭链约束），但计算量较大。实际工程中，常将两者结合：用拉格朗日法进行离线规划，用牛顿-欧拉法进行在线控制。

4.3 质心动力学与角动量守恒：行走稳定性的基石

双足行走的稳定性分析通常不直接使用全状态动力学，而是将系统简化为质心（CoM）模型。质心动力学描述了质心位置与地面反作用力之间的关系，而角动量守恒则约束了身体的旋转运动。这两个概念是理解ZMP（零力矩点）和步态规划的关键。

4.3.1 质心动力学：线性倒立摆模型（LIPM）

在行走过程中，若假设质心高度恒定，且忽略角动量变化，则质心动力学可简化为线性倒立摆模型：

m · ẍ_com = F_x
m · (z_com + g) = F_z

其中，F_x 和 F_z 为地面反作用力的水平与垂直分量。进一步可推导出ZMP位置：

p_zmp = x_com - (z_com / g) · ẍ_com

ZMP是地面反作用力合力在支撑多边形内的作用点。当ZMP位于支撑多边形内部时，行走稳定；当ZMP接近边界时，机器人有倾倒风险。

4.3.2 角动量守恒：上身姿态与摆动腿协调

对于双足机器人，角动量守恒并非严格成立（因为地面反作用力会产生外力矩），但我们可以通过控制策略使角动量变化最小化。在行走中，若上身（躯干）保持直立且不旋转，则角动量主要由摆动腿和手臂产生。通过规划摆动腿的轨迹，可以控制角动量的大小，从而影响ZMP位置。

4.3.3 案例拆解：基于质心动力学的步态规划

假设我们要规划一个步长为0.4m、周期为1.2s的平地行走步态。采用LIPM模型，设定质心高度为0.8m。根据ZMP约束，我们可以生成质心轨迹：

1. 设定ZMP轨迹：在单脚支撑期，ZMP固定在支撑脚中心；在双脚支撑期，ZMP从后脚向前脚线性过渡。
2. 求解质心轨迹：利用ZMP方程反解质心加速度，并通过积分得到质心位置和速度。通常使用模型预测控制（MPC）在有限时域内优化求解。
3. 验证约束：检查质心轨迹是否满足摩擦锥约束（|F_x| ≤ μF_z）和ZMP边界约束。

质心高度 z_com = 0.8 m
重力加速度 g = 9.81 m/s²
步长 = 0.4 m，周期 = 1.2 s
最大ZMP偏移 = 0.1 m（脚掌前后边界）
摩擦系数 μ = 0.6

通过MPC求解，得到质心轨迹后，再通过逆运动学映射到关节角度，即可生成步态。该方法的优势在于计算效率高，且能显式处理ZMP约束。

4.4 地面反作用力：测量、估计与利用

地面反作用力（GRF）是双足机器人唯一的主动外力来源，它决定了机器人的线性加速度和角加速度。精确获取GRF对于力控、阻抗控制和步态稳定性评估至关重要。

4.4.1 GRF的测量与估计方法

• 直接测量：在脚底安装六维力/力矩传感器，直接测量GRF的三个力分量和三个力矩分量。优点是精度高、带宽高；缺点是成本高、增加脚部重量和复杂度。
• 间接估计：通过关节力矩传感器和动力学模型，利用牛顿-欧拉法反推GRF。该方法无需额外硬件，但依赖模型精度，且对关节力矩噪声敏感。
• 融合估计：将力传感器测量值与基于模型的估计值通过卡尔曼滤波融合，兼顾精度与鲁棒性。

4.4.2 操作流程：基于动力学模型的GRF估计

1. 获取状态：从编码器读取关节角度、角速度，通过状态估计器（如扩展卡尔曼滤波）得到质心位置、速度和加速度。
2. 计算惯性力：根据质心加速度和机器人总质量，计算惯性力 F_inertia = m · a_com。
3. 求解GRF：根据牛顿第二定律，F_grf = m(g + a_com)（忽略其他外力）。对于旋转部分，根据角动量变化率求解GRF的力矩分量。
4. 验证约束：检查GRF是否落在摩擦锥内，以及ZMP是否在支撑多边形内。

4.4.3 实战案例：外力扰动下的GRF变化与恢复

假设机器人在行走过程中受到一个向后的水平推力（例如被人在背部推了一下）。初始现象：质心速度突然减小，ZMP向后移动，接近支撑脚后跟边缘。关键数据：推力大小为50N，持续0.2s，机器人质量30kg，质心高度0.8m。

分析过程：推力导致质心加速度变为负值，GRF水平分量反向增大以抵抗推力。ZMP后移，若不干预将超出支撑边界。控制策略：通过踝关节主动发力，将ZMP向前推回安全区域，同时调整下一步的落脚点以吸收扰动能量。复盘经验：仅依赖踝关节力矩可能不足以应对大扰动，需要结合步态调整（如跨步）和上身姿态控制。

4.5 总结与工程实践要点

本章从牛顿-欧拉法和拉格朗日法出发，建立了双足机器人动力学模型的理论框架，并深入探讨了质心动力学、角动量守恒和地面反作用力在行走控制中的核心作用。在实际工程中，建议遵循以下原则：

• 模型选择：实时控制优先采用牛顿-欧拉法（O(N)计算），离线规划与分析采用拉格朗日法（闭式方程便于优化）。
• 简化与降维：行走控制中，将全身动力学简化为质心模型（LIPM）可大幅降低计算复杂度，但需注意角动量变化的影响。
• 传感器融合：GRF测量与估计应结合力传感器和模型，通过滤波提高信噪比。
• 稳定性判据：ZMP位于支撑多边形内是行走稳定的必要条件，但不是充分条件。还需考虑角动量变化率和摩擦约束。