4、动力学基础：牛顿-欧拉法、拉格朗日法，动力学方程推导与仿真

运动控制算法的落地质量，很大程度上取决于我们对被控对象动力学特性的刻画精度。无论是六轴工业机器人、SCARA 还是协作机械臂，其关节伺服系统所承受的负载力矩并非恒定值，而是随位形、速度、加速度以及外部接触力实时变化的强耦合非线性量。若控制算法仅基于运动学模型设计，忽略惯性力、科氏力、离心力和重力项的影响，在高动态轨迹跟踪或变负载工况下必然出现跟踪误差增大、抖动甚至失稳。本章聚焦动力学建模的两大经典体系——牛顿-欧拉法和拉格朗日法，从物理本质出发，推导刚体系统的动力学方程，并通过 MATLAB/Simulink 仿真验证模型正确性，为后续的力矩前馈、重力补偿、阻抗控制和自适应控制打下坚实的数学基础。

上图展示了本章的核心知识框架：牛顿-欧拉法与拉格朗日法从不同物理视角出发，最终收敛于统一的二阶非线性动力学方程形式。牛顿-欧拉法基于力/力矩的递推计算，适合实时控制中的在线求解；拉格朗日法基于能量标量函数，适合推导闭式方程和进行稳定性分析。两者在数学上等价，但工程应用场景各有侧重。下面我们分别深入剖析两种方法的推导流程，并通过一个二连杆机械臂的完整案例，演示从建模到仿真的全链路操作。

4.1 牛顿-欧拉法：递推形式的动力学建模

4.1.1 核心原理：从牛顿第二定律到多刚体递推

牛顿-欧拉法的基本思想是将每个连杆视为刚体，分别应用牛顿方程（平动）和欧拉方程（转动），然后通过连杆之间的运动约束（关节连接）建立递推关系。整个算法分为两个阶段：外推（从基座到末端执行器）计算每个连杆的角速度、角加速度、线加速度；内推（从末端执行器到基座）计算每个关节所需的驱动力/力矩。

对于第 i 个连杆，其牛顿方程为：

f_i = m_i * v̇_ci

其中 f_i 为作用在连杆质心上的合力，m_i 为连杆质量，v̇_ci 为质心线加速度。欧拉方程为：

n_i = I_i * ω̇_i + ω_i × (I_i * ω_i)

其中 n_i 为合力矩，I_i 为惯性张量，ω_i 为角速度。在实际推导中，需要将各物理量变换到统一的坐标系下（通常为连杆自身坐标系），并考虑相邻连杆之间的速度/加速度传递关系。

4.1.2 操作流程：标准递推算法步骤

1. 初始化：设定基座（连杆0）的角速度 ω₀=0、角加速度 ω̇₀=0、线加速度 v̇₀=[0,0,g]ᵀ（重力加速度以基座坐标系表示）。
2. 外推（正向递推）：对于 i=1 到 n，根据关节 i 的运动状态（角度、角速度、角加速度）计算连杆 i 的角速度、角加速度、质心线加速度。
3. 内推（反向递推）：对于 i=n 到 1，根据连杆 i 所受的外力和外力矩，计算关节 i 所需的驱动力/力矩。
4. 输出：得到关节力矩向量 τ = [τ₁, τ₂, ..., τₙ]ᵀ。

4.1.3 案例拆解：二连杆机械臂的牛顿-欧拉推导

考虑一个平面二连杆机械臂，连杆1和连杆2的质量分别为 m₁、m₂，长度分别为 l₁、l₂，质心位于连杆中点。关节变量为 θ₁、θ₂。我们通过牛顿-欧拉法推导其动力学方程。

外推阶段（以连杆1为例）：

ω₁ = θ̇₁ * z₀
ω̇₁ = θ̈₁ * z₀
v̇_c1 = ω̇₁ × r_c1 + ω₁ × (ω₁ × r_c1) + v̇₀

其中 r_c1 为连杆1质心在基座坐标系中的位置向量。同理可计算连杆2的运动量，但需注意连杆2的角速度包含 θ̇₁ 和 θ̇₂ 的叠加。

内推阶段：从末端执行器开始，假设末端受力 F_ext=0、力矩 N_ext=0。对连杆2应用牛顿-欧拉方程，得到关节2的驱动力矩 τ₂；再对连杆1应用方程，得到 τ₁。最终整理可得：

τ₁ = (m₁/3 + m₂)l₁²θ̈₁ + (m₂/2)l₁l₂cos(θ₁-θ₂)θ̈₂ + (m₂/2)l₁l₂sin(θ₁-θ₂)θ̇₂² + (m₁/2 + m₂)gl₁cosθ₁
τ₂ = (m₂/2)l₁l₂cos(θ₁-θ₂)θ̈₁ + (m₂/3)l₂²θ̈₂ - (m₂/2)l₁l₂sin(θ₁-θ₂)θ̇₁² + (m₂/2)gl₂cosθ₂

可以看到，方程中包含了惯性项（θ̈ 的系数）、科氏/离心项（θ̇² 的系数）以及重力项（g 的系数）。这些项在控制中分别对应不同的物理补偿需求。

4.1.4 常见误区与复盘

• 误区1：忽略科氏力项。在低速运动时科氏力影响较小，但在高速轨迹跟踪中，科氏力可能达到重力项的 30%-50%，忽略会导致明显的跟踪滞后。
• 误区2：坐标系变换错误。外推过程中，角速度、线加速度必须在同一坐标系下表示，常见的错误是混淆了连杆坐标系与基座坐标系。
• 误区3：惯性张量未转换。惯性张量是相对于连杆质心坐标系的，在计算欧拉方程时必须将其变换到当前计算坐标系。

4.2 拉格朗日法：基于能量的系统化建模

4.2.1 核心原理：从标量函数到运动方程

拉格朗日法从系统的能量角度出发，定义拉格朗日函数 L = K - P，其中 K 为系统总动能，P 为系统总势能。动力学方程由拉格朗日方程给出：

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

其中 qᵢ 为广义坐标（对于旋转关节即关节角度），τᵢ 为广义力（关节力矩）。该方法不需要考虑连杆之间的内部约束力，只需正确写出系统的动能和势能表达式，推导过程更加系统化，不易遗漏耦合项。

4.2.2 操作流程：拉格朗日法建模步骤

1. 定义广义坐标：选择独立的关节变量 q = [q₁, q₂, ..., qₙ]ᵀ。
2. 计算动能：对每个连杆，计算其平动动能 (1/2)mᵢv_ciᵀv_ci 和转动动能 (1/2)ωᵢᵀIᵢωᵢ，求和得到总动能 K。
3. 计算势能：通常为重力势能 P = Σmᵢgᵀhᵢ，其中 hᵢ 为连杆质心高度。
4. 代入拉格朗日方程：对每个广义坐标 qᵢ 求偏导，整理得到二阶微分方程组。
5. 整理为标准形式：将方程整理为 M(q)q̈ + C(q,q̇)q̇ + G(q) = τ 的形式，便于控制设计。

4.2.3 案例拆解：二连杆机械臂的拉格朗日推导

仍以平面二连杆机械臂为例。设连杆1质心位置为 (l₁/2 cosθ₁, l₁/2 sinθ₁)，连杆2质心位置为 (l₁cosθ₁ + l₂/2 cos(θ₁+θ₂), l₁sinθ₁ + l₂/2 sin(θ₁+θ₂))。计算动能：

K₁ = (1/2)m₁(l₁/2)²θ̇₁² + (1/2)I₁θ̇₁²
K₂ = (1/2)m₂[v_c2x² + v_c2y²] + (1/2)I₂(θ̇₁+θ̇₂)²

其中 v_c2x、v_c2y 为连杆2质心速度分量，需对位置求导得到。势能为：

P = m₁g(l₁/2)sinθ₁ + m₂g[l₁sinθ₁ + (l₂/2)sin(θ₁+θ₂)]

代入拉格朗日方程并整理，得到的方程与牛顿-欧拉法结果完全一致。这验证了两种方法的等价性。

4.2.4 常见误区与复盘

• 误区1：动能计算遗漏转动项。对于细长连杆，转动动能不可忽略，否则会导致惯性矩阵不准确。
• 误区2：势能基准选择不一致。势能零点可以任意选择，但必须保证所有连杆使用同一基准，否则重力项计算错误。
• 误区3：广义力方向混淆。对于旋转关节，广义力是力矩；对于移动关节，广义力是力。需根据关节类型正确对应。

4.3 动力学方程仿真验证与参数辨识

4.3.1 仿真验证的核心目标

动力学模型建立后，必须通过仿真验证其正确性。验证目标包括：（1）模型一致性——两种方法得到的力矩输出是否一致；（2）物理合理性——在给定轨迹下，力矩曲线是否平滑、是否符合直觉；（3）能量守恒——无外力时系统总能量是否恒定。

4.3.2 基于 Simulink 的仿真流程

1. 搭建仿真模型：使用 Simulink 的 S-Function 或 MATLAB Function 模块实现动力学方程。
2. 设计激励轨迹：采用五次多项式插值或正弦扫频信号，确保关节速度、加速度充分激励。
3. 运行仿真：记录关节位置、速度、加速度以及计算得到的力矩。
4. 对比验证：将牛顿-欧拉法与拉格朗日法的输出进行对比，误差应小于数值精度阈值（如 1e-6）。
5. 参数敏感性分析：改变质量、惯性参数，观察力矩变化是否符合物理规律。

% MATLAB 代码片段：二连杆动力学仿真初始化
% 参数定义
m1 = 3.0; m2 = 2.0;   % 质量 kg
l1 = 1.0; l2 = 0.8;   % 杆长 m
g = 9.81;              % 重力加速度

% 轨迹生成：五次多项式
t = 0:0.001:2;
q1 = 0.5*(1 - cos(pi*t));  % 从0到1 rad
q2 = 0.3*(1 - cos(pi*t));  % 从0到0.6 rad
dq1 = 0.5*pi*sin(pi*t);
dq2 = 0.3*pi*sin(pi*t);
ddq1 = 0.5*pi^2*cos(pi*t);
ddq2 = 0.3*pi^2*cos(pi*t);

4.3.3 案例闭环：从模型到前馈控制

我们以二连杆机械臂为对象，完成一个完整的仿真验证与力矩前馈控制案例。初始现象：在纯 PID 反馈控制下，跟踪正弦轨迹时出现约 0.15 rad 的相位滞后和 0.08 rad 的幅值衰减。分析判断：滞后主要来源于重力项和惯性项未补偿。实际操作：在 Simulink 中搭建动力学模型，计算前馈力矩 τ_ff = M(q)q̈_d + C(q,q̇)q̇ + G(q)，将其叠加到 PID 输出上。结果对比：加入前馈后，跟踪误差从 0.08 rad 降至 0.005 rad，相位滞后几乎消除。风险与代价：前馈控制依赖精确的模型参数，若参数误差超过 10%，前馈反而可能引入扰动。复盘经验：在参数不确定时，可采用自适应前馈或在线参数辨识。

4.3.4 常见误区与总结

• 误区1：仿真步长过大导致数值发散。动力学仿真建议使用变步长求解器（如 ode45），并设置相对误差 1e-6。
• 误区2：忽略关节摩擦。实际系统存在库仑摩擦和粘滞摩擦，仿真中可先忽略，但在控制设计中必须考虑。
• 误区3：参数辨识不充分。动力学参数（质量、质心、惯性张量）可通过 CAD 模型获取，但实际装配后需通过辨识实验修正。