1. 项目缘起为什么动力学仿真值得你花时间如果你正在接触机器人无论是做学术研究、参加机器人竞赛还是进行工业机械臂的离线编程迟早会碰到一个绕不开的坎动力学。静态的运动学规划比如让机械臂末端画个圆看起来挺酷但一上真机或者想模拟真实负载下的运动问题就来了——关节电机扭矩够不够运动过程中会不会因为惯性太大而抖动高速运动时末端精度还能不能保证这些问题都得靠动力学仿真来回答。动力学仿真的门槛说高不高说低不低。自己从头推导拉格朗日方程或者牛顿-欧拉递推公式再写成代码对大多数人来说既枯燥又容易出错。而MATLAB的机器人工具箱Robotics System Toolbox / Robotics Toolbox恰恰是解决这个痛点的利器。它把机器人建模、正/逆运动学、动力学计算这些底层复杂的数学和算法都封装好了你只需要用清晰的、近乎“白话”的代码来描述你的机器人就能快速得到仿真结果。这就像你不需要从造轮子开始学开车直接上手就能体验驾驶的乐趣和解决实际问题。网上很多教程要么太理论要么代码片段零散跑不通。这篇内容的目的就是让你能“抄作业”。我会以一个经典的SCARA机器人为例手把手带你走通从建模、设置轨迹、进行动力学仿真到分析结果的完整流程。你不需要完全理解背后的微分方程但你能立刻得到可运行的代码看到关节力矩如何变化并理解这些数据背后的工程意义。这对于做机械设计选型、控制器参数整定或者仅仅是验证你的轨迹规划是否合理都至关重要。2. 环境准备与工具箱“食用”指南工欲善其事必先利其器。在开始“复制粘贴”代码之前有几个关键步骤必须确保无误这能避免你掉进版本兼容和路径错误的坑里。2.1 MATLAB版本与工具箱确认首先你需要一个安装了Robotics System Toolbox的MATLAB。从R2015b左右开始MathWorks逐渐用这个新的工具箱替代了旧的Robotics Toolbox。我们这里用的函数主要基于这个新工具箱。如何检查在MATLAB命令窗口输入ver在输出的列表里找“Robotics System Toolbox”。或者直接在命令行输入which rigidBodyTree如果能返回路径说明工具箱已就位。如果你的版本比较老比如2015a以前可能只有旧的Robotics Toolbox函数名和用法差异很大建议升级到较新版本如R2019b及以上兼容性和功能都更好。2.2 理解核心对象rigidBodyTree与rigidBodyRobotics System Toolbox的核心是面向对象的。它用rigidBodyTree对象来表示整个机器人就像一棵树树干是基座树枝是连杆连接处就是关节。rigidBody(刚体)代表机器人的一个连杆。你需要为它起个名字如link1并指定它的质量、质心位置、惯性张量。这些动力学参数至关重要如果全设为0或默认值仿真结果将毫无意义。rigidBodyJoint(关节)连接两个刚体。你需要指定关节类型revolute旋转关节prismatic移动关节以及关节轴的方向如绕Z轴旋转[0 0 1]。rigidBodyTree(刚体树)把所有rigidBody通过rigidBodyJoint添加进来就构成了机器人模型。这个对象提供了计算正运动学、逆运动学、动力学的方法。一个常见的误区是只关心关节角度忽略了质量属性。请记住没有质量的动力学仿真是“静力学”真实的力矩计算离不开精确的质量、质心和惯性矩阵。对于SCARA这类结构我们可以进行合理的简化估算。2.3 SCARA机器人动力学参数估算实操关键假设我们要为一个四自由度SCARA建模两个旋转关节一个移动关节末端一个旋转关节。我们没有精确的CAD模型如何估算参数这里分享一个工程上的实用方法几何简化将每个连杆包括电机视为密度均匀的圆柱体或长方体。质量估算根据材料如铝型材和大致尺寸估算体积乘以密度得到质量。电机质量可以查数据手册。质心位置对于均匀规则形状质心就在几何中心。在连杆的坐标系下描述从关节到该连杆质心的向量。惯性张量这是最复杂的部分但对于绕主轴旋转的规则形状有公式可循。例如对于绕其中心轴Z轴旋转的细长圆柱体其转动惯量 Izz 很小而 Ixx 和 Iyy 较大。我们可以使用简化公式或者一个更实用的技巧利用MATLAB的inertia函数来自物理建模工具箱或在线计算器输入形状、尺寸和质量自动计算惯性矩阵。对于快速上手我们可以先使用一个对角矩阵diag([Ixx, Iyy, Izz])作为近似其中Ixx, Iyy, Izz是根据简化形状估算的值。注意惯性张量的值相对于质心坐标系给出。如果估算不准仿真的力矩曲线可能会抖动或不合理但整体趋势和量级仍然具有重要参考价值。我们的首要目标是跑通流程理解数据流向。3. 手把手构建SCARA动力学模型理论说再多不如一行代码。下面我们一步步构建模型。你可以直接复制这段代码块到MATLAB的脚本文件中运行。%% 1. 创建机器人树 robot rigidBodyTree(‘DataFormat’, ‘row’); % ‘row’ 表示后续输入的行向量格式 % ‘DataFormat’设置为’row’可以让后续的关节角向量以行向量形式输入更符合我们的习惯。 %% 2. 创建基座base和第一个连杆link1 % 基座是一个固定的“虚拟”刚体通常不需要质量属性。 base rigidBody(‘base’); addBody(robot, base, ‘base’); % 将基座添加到树上其父节点是机器人根节点 % 第一个连杆 (L1) link1 rigidBody(‘link1’); % 估算参数假设为铝制连杆长度0.3m质量约1.5kg质心在连杆中心。 link1.Mass 1.5; % 质心位置在link1的坐标系下从关节1link1的起点指向其质心的向量。 % 假设连杆沿X轴方向质心在(0.15, 0, 0)处。 link1.CenterOfMass [0.15 0 0]; % 惯性张量相对于质心坐标系。这里用一个估算的对角矩阵。 % 对于绕Z轴旋转的细长杆Izz较小Ixx和Iyy较大。 Ixx 0.011; Iyy 0.011; Izz 0.001; % 单位: kg*m^2 link1.Inertia [Ixx, Iyy, Izz, 0, 0, 0]; % 后三个是惯性积简化设为0 % 关节1绕Z轴旋转 jnt1 rigidBodyJoint(‘jnt1’, ‘revolute’); jnt1.JointAxis [0 0 1]; % 绕世界坐标系的Z轴旋转 % 设置关节的“家”位置Home Position即零位角度。 jnt1.HomePosition 0; % 将关节与连杆关联并将该连杆添加到树上其父节点是’base’ link1.Joint jnt1; addBody(robot, link1, ‘base’); %% 3. 创建第二个连杆link2 link2 rigidBody(‘link2’); link2.Mass 1.2; % 质量略小于link1 % 注意link2的坐标系原点在关节2处。其质心位置是相对于这个新原点的。 % 假设link2也沿X轴方向长度0.25m质心在(0.125, 0, 0) link2.CenterOfMass [0.125 0 0]; Ixx2 0.006; Iyy2 0.006; Izz2 0.0008; link2.Inertia [Ixx2, Iyy2, Izz2, 0, 0, 0]; jnt2 rigidBodyJoint(‘jnt2’, ‘revolute’); jnt2.JointAxis [0 0 1]; jnt2.HomePosition 0; link2.Joint jnt2; % 将link2添加到树上其父节点是’link1’。这意味着link2连接在link1的末端。 addBody(robot, link2, ‘link1’); %% 4. 创建第三个连杆link3移动关节 link3 rigidBody(‘link3’); link3.Mass 0.8; % 移动部分的质量 % 质心假设在Z轴方向上的中心 link3.CenterOfMass [0 0 0.05]; % 对于移动关节惯性张量主要考虑绕X,Y轴的旋转Z轴平移惯性很小。 Ixx3 0.002; Iyy3 0.002; Izz3 0.0005; link3.Inertia [Ixx3, Iyy3, Izz3, 0, 0, 0]; jnt3 rigidBodyJoint(‘jnt3’, ‘prismatic’); % 移动关节 jnt3.JointAxis [0 0 1]; % 沿Z轴移动 jnt3.HomePosition 0; % 零位长度 link3.Joint jnt3; addBody(robot, link3, ‘link2’); %% 5. 创建末端执行器工具tool tool rigidBody(‘tool’); tool.Mass 0.3; % 夹爪或工具的质量 tool.CenterOfMass [0 0 0.02]; tool.Inertia [1e-4, 1e-4, 1e-4, 0, 0, 0]; % 末端惯性很小 % 末端通常是一个固定的关节‘fixed’表示工具固连在最后一个连杆上。 jnt_tool rigidBodyJoint(‘fix_jnt’, ‘fixed’); tool.Joint jnt_tool; addBody(robot, tool, ‘link3’); %% 6. 显示机器人模型检查结构 figure(‘Name’, ‘SCARA Robot Model’) show(robot); % 在零位HomePosition显示机器人 xlabel(‘X’); ylabel(‘Y’); zlabel(‘Z’); title(‘SCARA Robot at Home Configuration’); grid on; view(60, 30); % 调整视角运行这段代码你应该能看到一个三维图形显示SCARA机器人在零位时的形态。这确认了我们的模型结构是正确的。你可以用robot.showdetails命令在命令窗口查看详细的关节和刚体信息。4. 规划一条让机器人“动起来”的轨迹动力学仿真需要输入随时间变化的关节位置、速度和加速度。我们通常先规划一条光滑的关节空间轨迹。这里使用工具箱自带的trapveltraj函数来生成一条梯形速度轮廓的轨迹它计算简单且能保证加速度有界。假设我们想让机器人在2秒内从初始位姿q0运动到目标位姿qf。%% 7. 定义轨迹参数 t_total 2; % 总时间 2秒 fs 100; % 采样频率 100Hz t 0:(1/fs):t_total; % 时间向量共201个点 num_points length(t); % 定义起始点和终点关节角度/位置 % [关节1角度, 关节2角度, 关节3位置, 关节4角度] 单位弧度米 q0 [0, 0, 0.1, 0]; % 初始状态关节1和2在0度关节3在0.1m高度关节4在0度。 qf [pi/4, -pi/6, 0.2, pi/2]; % 目标状态 %% 8. 生成梯形速度轨迹 % trapveltraj 需要起点和终点以列向量形式输入且可以一次为多个维度生成轨迹。 waypoints [q0‘, qf’]; % 两列分别是起点和终点的关节值 [t_samples, q, qd, qdd] trapveltraj(waypoints, num_points); % 输出说明 % t_samples: 采样时间点与我们的t基本一致 % q: 关节位置4行 x num_points列需要转置成行向量格式以适应我们的机器人 % qd: 关节速度 % qdd: 关节加速度 % 将q, qd, qdd转置使其每一行是一个时间点的状态方便后续循环 q q’; qd qd’; qdd qdd’; %% 9. 可视化轨迹可选 figure(‘Name’, ‘Planned Joint Trajectories’) subplot(3,1,1) plot(t, q) ylabel(‘Position (rad/m)’) legend(‘q1’, ‘q2’, ‘q3’, ‘q4’) title(‘Joint Positions’) grid on subplot(3,1,2) plot(t, qd) ylabel(‘Velocity (rad/s or m/s)’) legend(‘qd1’, ‘qd2’, ‘qd3’, ‘qd4’) title(‘Joint Velocities’) grid on subplot(3,1,3) plot(t, qdd) ylabel(‘Acceleration (rad/s^2 or m/s^2)’) xlabel(‘Time (s)’) legend(‘qdd1’, ‘qdd2’, ‘qdd3’, ‘qdd4’) title(‘Joint Accelerations’) grid on运行后你会看到三条曲线。位置曲线是平滑的S形速度曲线是梯形两端加速、中间匀速、末端减速加速度曲线是方波在加速和减速阶段为常数。这种轨迹对电机冲击较小是工业中常用的规划方式。5. 核心环节逆动力学计算与结果分析现在我们有了模型和轨迹可以调用工具箱的逆动力学函数inverseDynamics来计算每个时刻为了跟踪这条轨迹各关节需要提供的力矩/力。%% 10. 逆动力学计算 % 初始化一个数组来存储计算出的关节力矩/力 % 力矩单位牛顿米 (Nm) 对于旋转关节牛顿 (N) 对于移动关节 tau zeros(num_points, robot.NumBodies); % 列数与关节数相同注意base不算 % 循环遍历每一个时间点 for i 1:num_points % 获取当前时刻的关节位置、速度、加速度 q_i q(i, :); qd_i qd(i, :); qdd_i qdd(i, :); % 计算逆动力学 % 语法tau_i inverseDynamics(robot, q_i, qd_i, qdd_i) % 该函数会考虑重力默认重力加速度为[0 0 -9.81] tau_i inverseDynamics(robot, q_i, qd_i, qdd_i); % 存储结果 tau(i, :) tau_i’; end %% 11. 可视化关节力矩/力 figure(‘Name’, ‘Joint Torques/Forces from Inverse Dynamics’) plot(t, tau) xlabel(‘Time (s)’) ylabel(‘Torque (Nm) or Force (N)’) legend(‘Tau1 (Nm)’, ‘Tau2 (Nm)’, ‘Tau3 (N)’, ‘Tau4 (Nm)’) title(‘Required Joint Actuation’) grid on这张图是动力学仿真的核心输出。它告诉你关节1和关节2旋转关节力矩曲线。你会看到力矩在加速和减速阶段出现峰值匀速阶段力矩主要用来克服重力对于SCARA重力影响主要在Z轴对旋转关节力矩贡献较小和科氏力/离心力。峰值力矩的大小直接决定了你需要选择多大扭矩的伺服电机。关节3移动关节力曲线。它需要克服重力mass * g和加速度力mass * acceleration。你会看到在加速和减速阶段力有一个明显的阶跃。关节4末端旋转由于我们假设末端惯性很小且轨迹简单其力矩通常接近0。如何解读这些曲线假设关节1的峰值力矩计算出来是12 Nm。你需要查阅电机手册确保电机的额定扭矩和峰值扭矩能满足这个要求并留有一定的安全余量比如1.5-2倍。如果仿真出的力矩远超电机能力你就需要重新规划轨迹降低加速度、延长时间或者优化机械结构减轻重量、调整质心。6. 进阶验证正动力学与闭环仿真逆动力学告诉我们“需要多少力才能实现预定运动”。那如果我们“施加这些力”机器人会不会按预定轨迹运动呢这需要用正动力学来验证它模拟机器人在给定力矩和初始状态下的运动过程。这更接近真实的控制器仿真。%% 12. 使用正动力学进行验证仿真 % 我们需要一个微分方程求解器。这里使用ode45。 % 首先定义机器人的状态向量状态 [关节位置; 关节速度] initialState [q0, zeros(1, 4)]’; % 初始位置为q0初始速度为0 % 定义时间跨度 tspan [0 t_total]; % 定义动力学微分方程函数句柄 % ode45要求函数格式为 dstate dynamicsFunc(t, state) % 其中 state [q; qd] dynamicsFunc (t, state) dynamicsWrapper(t, state, robot, (t)interpControl(t, t_samples, tau)); % 使用ode45求解 options odeset(‘RelTol’, 1e-6, ‘AbsTol’, 1e-9); % 设置求解精度 [t_ode, state_ode] ode45(dynamicsFunc, tspan, initialState, options); % 提取仿真得到的位置和速度 q_sim state_ode(:, 1:4); qd_sim state_ode(:, 5:8); %% 13. 对比规划轨迹与仿真轨迹 figure(‘Name’, ‘Trajectory Tracking Verification’) for j 1:4 subplot(4,1,j) plot(t, q(:, j), ‘b-‘, ‘LineWidth’, 1.5, ‘DisplayName’, ‘Planned’) hold on plot(t_ode, q_sim(:, j), ‘r--‘, ‘LineWidth’, 1, ‘DisplayName’, ‘Simulated (Forward)’) ylabel([‘q’ num2str(j)]) grid on if j 1 title(‘Comparison: Planned vs. Forward Dynamics’) legend(‘Location’, ‘best’) end if j 4 xlabel(‘Time (s)’) end end这段代码需要两个辅助函数% 辅助函数1控制输入插值函数 function tau_t interpControl(t, t_samples, tau_matrix) % 根据当前时间t从预先计算好的逆动力学力矩矩阵中线性插值得到当前力矩 % tau_matrix 是 (num_points x num_joints) tau_t zeros(size(tau_matrix, 2), 1); % 列向量 for j 1:size(tau_matrix, 2) tau_t(j) interp1(t_samples, tau_matrix(:, j), t, ‘linear’, ‘extrap’); end end % 辅助函数2正动力学包装函数 function dstate dynamicsWrapper(t, state, robot, controlFunc) % state: [q; qd] numJoints robot.NumBodies; q state(1:numJoints)‘; qd state(numJoints1:end)’; % 从控制函数获取当前时刻的关节力矩 tau_input controlFunc(t); % 列向量 % 计算正动力学加速度 % 语法qdd forwardDynamics(robot, q, qd, tau) qdd forwardDynamics(robot, q, qd, tau_input); % 返回行向量 % 状态导数速度就是qd加速度是qdd dstate [qd’; qdd’]; % 返回列向量 end运行后你会看到四条对比曲线。如果我们的模型准确且逆动力学计算正确那么正动力学仿真出来的轨迹红色虚线应该与规划轨迹蓝色实线几乎完全重合。如果出现偏差尤其是漂移或发散可能的原因有动力学参数不准确质量、质心、惯性张量误差太大。数值积分误差可以尝试减小ode45的误差容限RelTol,AbsTol。控制输入插值误差在轨迹变化剧烈处线性插值可能不够精确。这个“规划-逆动力学-正动力学”的闭环验证是检验你整个模型和仿真流程是否自洽的黄金标准。它能极大增强你对仿真结果的信心。7. 从仿真到现实参数敏感性与下一步拿到仿真曲线只是第一步。一个有经验的工程师会进一步问如果我的参数估错了20%结果会差多少这就是参数敏感性分析。你可以写一个简单的循环将link1.Mass在 ±20% 范围内变动重新计算逆动力学观察峰值力矩的变化。通常移动部件的质量关节3和工具对力矩影响最显著。这个分析能告诉你在机械加工和选型时哪些参数的精度需要严格控制。下一步可以做什么导入CAD模型如果你有URDF文件或SolidWorks模型可以使用importrobot函数直接导入获得精确的质量属性这是最理想的情况。添加摩擦力模型真实的关节有摩擦。你可以在inverseDynamics计算后根据速度符号简单加上库伦摩擦和粘性摩擦项让模型更真实。设计控制器用计算出的力矩作为前馈再配合一个简单的PD反馈控制器在正动力学仿真中模拟闭环控制观察抗干扰性能。轨迹优化基于力矩曲线优化轨迹如时间最优轨迹、能耗最优轨迹使得峰值力矩最小化从而可以选用更小、更便宜的电机。动力学仿真不是一个“一次性”的作业而是一个强大的设计循环工具。通过不断调整模型参数和运动轨迹你可以在制造物理样机之前就预见到潜在问题并找到优化方案这能节省大量的时间和成本。希望这份可以直接运行的代码和详细的步骤解读能成为你探索机器人动力学世界的一块坚实跳板。
