4. 刚体动力学建模实战

本节把前面建立的拉格朗日方程真正落到 MATLAB/Simulink 里。目标不是"跑通一个仿真"，而是让读者能独立写出二连杆机械臂的动力学模型，并理解如何把同样的思路扩展到六自由度机械臂。核心工具是 MATLAB 的符号计算与 Simulink 的模块化建模，两者配合可以避免手工推导带来的符号错误。

一、二连杆机械臂的符号建模

先约定坐标系与参数。设二连杆在竖直平面内运动，关节1固定在基座上，关节2连接连杆1与连杆2。取逆时针为正方向，重力加速度 g 向下。连杆 i 的长度为 l_i，质量为 m_i，质心位于连杆中点，绕质心的转动惯量为 I_i。关节角 q₁、q₂ 从水平方向起算，关节力矩 τ₁、τ₂ 为输入。

用拉格朗日法得到标准形式：

M(q)q̈ + C(q,q̇)q̇ + G(q) = τ

其中 M 为 2×2 惯性矩阵，C 为科氏力与离心力项，G 为重力项。手工推导容易出错，因此用 MATLAB 符号工具箱自动生成。下面是一段可直接运行的脚本，参数取 l₁=l₂=0.5 m，m₁=m₂=1 kg，I₁=I₂=0.02 kg·m²，g=9.81 m/s²：

syms q1 q2 dq1 dq2 real
syms l1 l2 m1 m2 I1 I2 g real
q  = [q1; q2];
dq = [dq1; dq2];

% 质心位置
p1 = [l1/2*cos(q1); l1/2*sin(q1)];
p2 = [l1*cos(q1)+l2/2*cos(q1+q2);
      l1*sin(q1)+l2/2*sin(q1+q2)];

% 速度平方
v1sq = sum(diff(p1,q1)*dq1.^2);
v2sq = sum((diff(p2,q1)*dq1 + diff(p2,q2)*dq2).^2);

T = 1/2*m1*v1sq + 1/2*I1*dq1^2 ...
  + 1/2*m2*v2sq + 1/2*I2*(dq1+dq2)^2;
V = m1*g*p1(2) + m2*g*p2(2);
L = T - V;

% 拉格朗日方程
M = simplify(jacobian(jacobian(L,dq).',dq));
C = simplify(jacobian(jacobian(L,dq).',q)*dq - jacobian(L,q).');
G = simplify(jacobian(L,q).');

运行后 M、C、G 都是关于 q、dq 的符号表达式。为了在 Simulink 中使用，需要把它们转成可调用的函数。推荐用 matlabFunction 生成匿名函数文件：

matlabFunction(M,'File','M_fun','Vars',{q});
matlabFunction(C,'File','C_fun','Vars',{q,dq});
matlabFunction(G,'File','G_fun','Vars',{q});

这样在 Simulink 的 MATLAB Function 模块里就能直接调用 M_fun(q) 等函数，避免把长表达式硬编码进模块。

二、Simulink 中的动力学求解结构

动力学方程是二阶微分方程，Simulink 里最直接的做法是把它改写成状态空间形式。定义状态 x = [q; dq]，则：

q̈ = M(q) \ (τ - C(q,dq)*dq - G(q))

在 Simulink 中搭建时，用 Integrator 模块对 q̈ 积分一次得到 dq，再积分一次得到 q。q 和 dq 反馈回 MATLAB Function 模块计算 M、C、G，形成闭环。输入 τ 可以来自 Signal Builder 或 Step 模块，用于施加阶跃力矩或正弦激励。

下图给出二连杆动力学模型的 Simulink 信号流结构，重点标出状态反馈路径与矩阵求解环节：

搭建时注意两点：一是 Integrator 模块的初值要设为 q₀ 和 dq₀，例如 q₀=[0;0]、dq₀=[0;0]；二是求解器建议选 ode45 或 ode15s，固定步长取 1 ms，便于后续与控制器联合仿真时保持时序一致。

三、从二连杆扩展到六自由度

六自由度机械臂的建模思路与二连杆完全一致，区别在于符号推导的规模。以常见的 6R 串联臂为例，用 DH 参数描述各连杆，再用 Robotics Toolbox 的 SerialLink 对象自动生成动力学模型，可以省去手工推导：

L(1) = Link('d',0.4,'a',0,'alpha',pi/2,'m',5,'r',[0,0,0.2],'I',[0.1,0.1,0.05]);
L(2) = Link('d',0,'a',0.5,'alpha',0,'m',4,'r',[0.25,0,0],'I',[0.05,0.08,0.05]);
% ... 依次定义 6 个连杆
robot = SerialLink(L,'name','arm6');
robot.dyn;   % 生成动力学模型

若不用工具箱，也可以按同样的拉格朗日流程，用符号变量 q1…q6 推导。此时 M 是 6×6 矩阵，C 和 G 的表达式会非常长，建议用 matlabFunction 生成函数文件后在 Simulink 中调用，而不是把表达式直接贴进模块。六自由度模型的 Simulink 结构与二连杆相同，只是状态维数从 4 变为 12，MATLAB Function 模块内部改为矩阵运算。

四、结果观察与验证

仿真跑起来后，先做两项基本检查。第一，令 τ=0、q₀=[0;0]、dq₀=[0;0]，观察机械臂是否保持静止；若出现缓慢漂移，说明重力项符号或质心位置有误。第二，施加一个小的阶跃力矩，观察 q 是否平滑变化、dq 是否连续；若出现剧烈振荡，通常是积分步长过大或 M 矩阵奇异。

更严格的验证是能量守恒检查。无外力、无摩擦时，系统总能量 E=T+V 应保持常数。在 Simulink 中加一个 MATLAB Function 计算 E，用 Scope 观察其波动。若波动在 1% 以内，说明模型与求解器设置基本正确。下表给出二连杆模型的一组示例参数与预期现象，供搭建时对照：