企业官网建设流程全解析

简介：面向机器人控制与仿真学习者的三连杆机械臂动力学仿真工程，基于Simscape Multibody多体工具箱构建3R机器臂模型，采用PD反馈控制策略，实现无余差跟踪控制，适合正在学习机械臂动力学建模、MATLAB/Simulink仿真或机器人控制算法的学生与工程师参考。工程使用MATLAB 2018b建立，需该版本或更高版本打开。资源包大小约20.26MB，共5个文件，含txt使用说明、PDF工具箱安装教程、slx仿真主模型、m脚本以及Robot Toolbox工具箱安装包，覆盖从仿真模型到运行环境的完整闭环。目前已有2551人学习下载。获取后可直接运行的机器臂动力学仿真模型、配套控制脚本与工具箱安装指引，配合包内使用说明可快速理解平面三自由度机器人动力学建模与仿真流程，便于在此基础上扩展轨迹规划或更复杂控制算法。

1. 3R机械臂动力学仿真用Simulink，到底在仿什么？

3R机械臂动力学仿真，听上去是控制课的标准作业，但真要在 Simulink（标题里写成 Sinulink，就是笔误）里把三连杆模型跑稳，你会发现能发散的姿势比能收敛的多得多。有人把力矩值直接拉到 1000 N·m，仿真瞬间 NaN；有人用 ode45 硬扛高阶刚性问题，半小时跑不完一秒的运动。下面沿着最常用的路径走一遍——从拉格朗日方程整理出 M、C、G，到在 Simulink 里落地正动力学与逆动力学，再到求解器参数和几个典型踩坑。适合正在做机械臂控制的在校生，以及准备做硬件在环的工程师。

2. 先建物理模型：3R机械臂的动力学方程与参数准备

2.1 从DH参数到质量参数：建模前必须准备好的输入

3R机械臂的“3R”指三个旋转关节串联，每根杆的姿态用 DH 模型描述。在 Simulink 里仿真前，先把 DH 参数表准备好，因为后面无论是 Simscape 里设置关节还是解析代码里的几何关系，都以它为准。一个平面 3R 的 DH 参数表可以这样写：

这里 a 是连杆长度，alpha 是连杆扭角，d 是沿 z 轴的偏距，theta 是关节角。平面 3R 的 alpha 都是 0，所以所有关节轴线平行，重力只作用在垂直方向。如果做空间 3R，alpha 不为 0，后面 M、C、G 的表达式会复杂一个量级，但 Simulink 实现思路不变。如果你只是验证运动轨迹，用运动学就够了；但只要涉及力矩、控制器或硬件在环，动力学参数就避不开。这一步别省。

光有几何参数不够。动力学仿真必须有质量、质心和惯性张量。质量直接从 CAD 模型读取；质心和惯性张量要看坐标基准：质心位置是相对于连杆坐标系的，惯性张量通常是相对于质心坐标系的。下面用一段 MATLAB 代码把参数集中到一个结构体里，方便后面脚本调用：

param.m = [3, 2.5, 1.5]; % 三根连杆的质量，kg param.r = [0.2, 0, 0; % 质心在连杆坐标系中的位置，m 0.25, 0, 0; 0.15, 0, 0]; param.I = zeros(3,3,3); % 惯性张量，kg·m^2 param.I(:,:,1) = diag([0.02, 0.01, 0.02]); param.I(:,:,2) = diag([0.05, 0.02, 0.05]); param.I(:,:,3) = diag([0.01, 0.005, 0.01]);

这段代码把连杆近似成细长杆，轴向转动惯量比横向小，数值上比较符合实际。注意 param.I 第三维索引是连杆编号，第一维和第二维是 3×3 张量。在 Simscape 里填惯性张量时，填的就是这个矩阵，单位是 kg·m²。这里多解释一句：惯性张量不是随便写几个正数就完事，它必须和质心位置配套。连杆的质心在 0.2m 处，但转动惯量却设成围绕连杆末端的大数值，动力学方程看起来还走得通，验证能量时一定翻车。

这里有一个常见的“看着没问题但结果翻车”的隐患：有些 CAD 工具导出的惯性张量是相对于关节坐标系，而不是质心坐标系。如果你直接把那个矩阵填进 Simscape 的刚体模块，仿真出来的运动轨迹可能还挺顺，但用能量守恒一验证就露馅。所以建模前花费十分钟把坐标基准统一，后面省两小时。我的习惯是做一个参数检查脚本，专门打印每个连杆的质心坐标和惯性张量矩阵，确认质心坐标系下惯性张量的对角线占优，非对角线接近零，才算合规。

2.2 拉格朗日方程与标准形式：让代码有据可依

Simulink 里做动力学仿真，不需要在模型里画拉格朗日推导过程，但需要知道最后要计算什么。3R 机械臂的动力学方程标准形式是：

M(q) qdd + C(q, qd) qd + G(q) = tau

其中 q 是 3×1 关节角向量，M 是 3×3 惯性矩阵，C 是科氏力与离心力矩阵，G 是重力项，tau 是关节力矩向量。正动力学仿真就是给定 tau、q、qd 求 qdd；逆动力学就是给定 q、qd、qdd 求 tau。

很多人在这一步直接去翻教科书抄公式，结果 M 矩阵和 C 矩阵对不上。更实际的做法是：先用 MATLAB 符号推导一次，生成可调用的函数，后面 S 函数和 MATLAB Function 里直接调用。以平面 3R 为例，推导骨架是：

syms q1 q2 q3 dq1 dq2 dq3 real q = [q1; q2; q3]; dq = [dq1; dq2; dq3]; % 需要先写出每个连杆质心的速度向量vc_i和角速度w_i % 动能 T = sum(0.5*m_i*vc_i'*vc_i + 0.5*w_i'*I_i*w_i) % 重力势能 V = sum(m_i * g * h_i(q)) % 惯性矩阵 M = simplify(jacobian(jacobian(T, dq), dq)); % 重力项 G = simplify(jacobian(V, q)); % 科氏力矩阵 C 用Christoffel符号从M计算

这段代码不能直接运行，因为需要补全位置和速度表达式。但它的作用是给你一条路径：先写运动学，再写能量，最后求 M 和 G。C 矩阵有一个工程技巧——不用单独展开，可以用数值方法由 M 和 dq 构造，这样严格满足反对称性，后面做控制律验证更稳。符号推导生成的 M 函数可能非常长，我建议在matlabFunction里打开优化选项，把公共子表达式提取出来，计算速度能快接近一倍。

如果不想自己推导，也可以从 URDF 模型直接生成 M、C、G。常见的做法是用机器人工具箱读入 URDF，再用它的动力学接口：

robot = importrobot('threeR.urdf'); % 读取URDF文件 q = [0.1; 0.2; 0.3]; dq = zeros(3,1); M = massMatrix(robot, q); C = velocityProduct(robot, q, dq); G = gravityTorque(robot, q);

注意这个工具生成的 C 矩阵不一定是唯一形式，但数值上满足动力学方程。把这三个输出保存成函数或参数，就算把动力学“立住”了。实际项目里，我一般先用机器人工具箱算一次 M 矩阵，再和手工符号推导的结果做数值对比，两者差在 1e-6 以内才敢往下走，否则就说明运动学或惯性参数里有错。

2.3 正动力学与逆动力学：明确你要仿真哪个方向

在 Simulink 里，正动力学和逆动力学的模型结构差别很大。正动力学输入力矩、输出运动，需要把 qdd 积分两次得到 q 和 qd，这意味着模型内部有反馈回路，容易出代数环。逆动力学输入运动、输出力矩，是一个纯粹的函数计算，很少出现数值稳定问题。

用途也不一样。我在做控制器验证时，会把逆动力学放到前馈通道，把正动力学作为被控对象模型。初学建议先搭逆动力学，因为它简单、不容易翻车，跑通之后再改成正动力学。

正动力学与逆动力学的对比：

下一步就是决定在 Simulink 里用哪种方式落地。常见方案有两个：基于 Simscape Multibody 的实体建模，以及基于 S 函数或 MATLAB Function 的解析建模。前者不需要写动力学方程，后者控制更精细，下一章把两条路都拆开讲。

3. 用Simulink搭建动力学仿真：两种路径与最小实现

3.1 路径一：基于Simscape Multibody的实体建模

Simscape Multibody 是 Simulink 里做多体动力学仿真的官方环境。它的好处是：你不需要手动推导 M、C、G，模型直接从 CAD 或 URDF 导入，关节、刚体、传感器都是现成模块，而且可视化的 3D 动画对排查运动干涉很有用。坏处是：模型里模块数量多、参数隐藏在 UI 里，仿真步长稍大就容易有接触和摩擦的数值抖动。

我一般建议按这样几步走：

1. 准备一个 URDF 文件，描述 3R 机械臂的几何、质量和惯性参数。URDF 可以从 CAD 工具导出，也可以手写，格式本身不复杂。
2. 在 MATLAB 命令行执行一次导入，生成对应的 Simulink 模型。
3. 打开生成模型，检查每个关节模块的类型是否都是旋转关节；设置关节阻尼和摩擦系数（初学建议先设 0）。
4. 在每个刚体模块里核对质量和惯性张量，特别是惯性张量的坐标基准。
5. 添加力矩信号源连接到关节驱动端口，添加角度测量模块输出三个关节角。
6. 配置求解器并仿真。

导入 URDF 的命令是：

addpath('urdf_folder'); robot = importrobot('threeR.urdf'); % 先读进来，失败时能及时看到错误 smimport('threeR.urdf'); % 生成 Simulink 模型文件 open_system('threeR_simscape'); % 打开生成的模型，名称以URDF文件名为基础

smimport会生成一个名为threeR_simscape的模型文件，里面包含刚体、关节和坐标系变换的所有模块。导入成功后，重点检查以下几点（这是 Simscape 踩坑的高发区）：

• 单位是不是米/千克/秒。有些 CAD 导出 URDF 时长度单位是毫米，导入后模型尺寸放大 1000 倍，重力下的动力学全乱。
• 关节轴方向对不对。URDF 里<joint>的axis如果写错，三个关节就会“互锁”。
• 惯性张量是不是相对质心。URDF 标准规定惯性张量相对质心，但很多导出工具不遵守，必须核对。

如果你没有 URDF，也可以直接在 Simscape 模块库里手动拖：一个 Revolute Joint 加一个 Solid 组成一根杆，三个串起来。但手动搭容易把坐标系搞错，不如 URDF 导入省心。

3.2 路径二：用S函数封装解析模型

对于想完全掌控动力学方程、减少依赖的人来说，路径二更合适。思路是把上一章的 M、C、G 表达式写进一个 S 函数或 MATLAB Function 模块，在模块内部解出 qdd，然后积分。

Simulink 里最灵活的是 Level-2 S 函数，但模板代码长，新手容易抄错。我常用一个更省事的中间方案：在模型里放一个 MATLAB Function 模块，把动力学计算写在这个模块里，外部只需要接积分器。

MATLAB Function 内的核心计算如下：

function qdd = threeR_dyn(tau, q, dq) %#codegen % 输入tau为3x1力矩向量，q为角度，dq为角速度 % 输出qdd为关节加速度 param = coder.load('threeR_param.mat'); % 物理参数单独放文件，免得函数体太长 M = compute_M(q, param); C = compute_C(q, dq, param); G = compute_G(q, param); qdd = M \ (tau - C - G); % 求解线性方程组，比inv(M)*rhs稳定 end

代码里的compute_M、compute_C、compute_G就是上一章里准备的函数。注意用反斜杠运算符而不是inv(M)*rhs，在 M 矩阵条件数大时，反斜杠的数值稳定性更好。

Simulink 模型结构很简单：一个 Signal Builder 提供三个关节的力矩信号，连接 MATLAB Function 模块的 tau 输入；MATLAB Function 输出 qdd；qdd 接一个 Integrator 得到 dq；dq 接第二个 Integrator 得到 q；q 和 dq 再接回 MATLAB Function 的输入端口。这里的关键问题是，两个积分器构成了反馈环，MATLAB Function 在同一个步长内需要看到当前时刻的 q，但 q 是从 qdd 积分来的，这就会形成 Simulink 的代数环。

解决代数环的办法有三个，我按优先级给你：

1. 用 S 函数或 MATLAB Function 把整个动力学连同积分器一起封装，不让 Simulink 拆解内部回路；
2. 在反馈通道加一个 Memory 模块，把上一步的 q 延迟给当前步；
3. 用单位延迟替换积分器，改用离散状态空间。

其中第 2 种最简单，代价是引入一个步长的延迟，对于仿真精度要求不高的场景够用。如果做控制器验证，这点延迟会略微改变相位，最好用第 3 种。

给一个离散化积分的示意：

%% 在Simulink中配置离散积分器 % 设置两个Unit Delay模块，采样时间设为0.001 s % qdd -> 单位延迟1 -> dq -> 单位延迟2 -> q % 在MATLAB Function里把M\(tau-C-G)算出的qdd作为当前步的输入

这样反馈环变成离散的，Simulink 求解器只处理代数的力矩输入，不再和连续积分器纠缠。

3.3 最小可运行框架：输入力矩、输出关节角度

不管选哪条路径，都要先跑通一个最小框架。我定义一个“最小”的标准：三个关节都给一个恒定的阶跃力矩，输入信号是 0 时刻从 0 跳到设定值，输出能在一段有限时间内看到角度连续变化，并且不发散。

对于 Simscape 路径，运行方式：

mdl = 'threeR_simscape'; set_param(mdl, 'StopTime', '5'); % 仿真5秒 set_param(mdl, 'Solver', 'ode15s'); % 对刚性问题更稳 out = sim(mdl); % 开始仿真 t = out.tout; q = out.logsout.get('q').Values.Data; % 输出三个关节角 plot(t, q); legend('q1','q2','q3');

set_param里的求解器设置后面会展开讲。这里先给一个能出图的配置。注意logsout的变量名取决于你在模型里是否添加了信号日志记录，如果没有，可以直接用 Scope 看。

对于解析路径，模型结构对应关系如下：

力矩源（3路） -> MATLAB Function -> 积分器1 -> 积分器2 -> 输出Scope ^ | +--------------------------------+ (q, dq反馈)

这里没有代码，用文字描述。为了让你能跑通，给出初始化并运行的脚本：

mdl = 'threeR_analytic'; % 创建模型（手动搭好后保存） open_system(mdl); set_param(mdl, 'StopTime', '10'); set_param(mdl, 'MaxStep', '0.001'); out = sim(mdl);

如果仿真一开始就 NaN，或者长时间卡住不动，大概率是代数环或者模型里参数没加载。先检查 MATLAB Function 模块里是否能看到参数param，再检查积分器初值是否设成了非零的离谱值。积分器初值建议都设 0，除非你要模拟平衡位置附近的小扰动。

4. 参数设置与仿真调试：步长、求解器与初始条件

4.1 求解器选择和步长设置：刚度过大别硬扛

很多新手直接用默认的 ode45，但 3R 机械臂的动力学方程里往往存在非常大的惯性矩阵数值差异，比如第一关节转动惯量大、第三关节很小，这种刚性问题会让 ode45 的步长不断缩小，仿真速度慢到怀疑人生。我的经验是：先试 ode15s，如果变化太慢再试 ode23t，两者对刚性系统的处理都远好于 ode45。下面的表格是常用选择：

在命令行改求解器参数：

set_param('threeR_sim', 'SolverType', 'Variable-step'); set_param('threeR_sim', 'Solver', 'ode15s'); set_param('threeR_sim', 'RelTol', '1e-4'); set_param('threeR_sim', 'AbsTol', '1e-6'); set_param('threeR_sim', 'MaxStep', '0.001');

注意 MaxStep 不要设得太小，不然仿真时间会爆炸。0.001 秒对大多数机械臂足够，如果力矩变化很剧烈，再配合信号平滑去处理，而不是无限压缩步长。

4.2 重力与初始条件：让仿真启动不“飞”

机械臂动力学仿真里，重力方向是一个特别容易错的地方。Simscape 的 World Frame 默认 z 轴向上，重力加速度向量是 [0 0 -9.81]。如果你从 CAD 导入的模型 z 轴方向不是向上，重力就会变成侧面拉力，仿真第一帧就把机械臂甩出去。

检查方法很简单：在没有输入力矩的情况下运行仿真，如果机械臂静止不动，说明重力和关节平衡条件正确；如果疯狂摆动甚至飞走，先暂停，量一下重力方向。还可以在模型里加一个“重力显示”模块，看它作用于刚体上的方向箭头。许多导入的 URDF 中，base_link 的原点方向不规范，导致整个模型的 z 轴并不朝上，必须先在固连坐标系里旋转到和 World Frame 对齐。

初始条件同样重要。正动力学模型里的积分器初值对应 q 和 dq 的初值。两个建议：

• q 初值不要选在奇异位形附近，比如 q2=0 或 q3=0 可能导致 M 矩阵条件数极大，仿真步长骤降。
• dq 初值设 0，除非你要研究离心力启动的场景。

设置初值用积分器模块的对话框，或者在命令行用set_param设置包含积分器的状态初值比较麻烦，一般直接在模块里双击把 Initial Condition 写好。

4.3 结果验证：用能量守恒和逆动力学交叉检查

仿真跑通了，不代表正确。我每次都会做两个验证，缺一个都心里没底。

验证一：能量守恒。假设模型没有摩擦和阻尼，系统总能量（动能+势能）应该保持恒定。Simscape 导入的模型通常默认没有摩擦，如果曲线漂移得很厉害，说明惯性参数或重力设置有问题。验证脚本：

% 从仿真输出中提取角度、角速度 q = out.logsout.get('q').Values.Data; dq = out.logsout.get('dq').Values.Data; T = 0.5*dq' * M * dq; % M需要按每个时刻重新计算，这里示意 V = potential_energy(q, param); E = T + V; plot(t, E);

严谨写法是在每个时间步调用compute_M，我习惯把结果写成列向量再画。如果 E 的波动超过初始值的 5%，就要回头查质量分布和惯性张量。

验证二：逆动力学校验。这个方法更直接：用正动力学仿真得到的 q、dq、qdd，代入逆动力学方程计算力矩，然后与输入的力矩信号比较，误差应该在 1e-6 量级。如果有隔着几个数量级的偏差，说明 C 或 G 函数里有符号错误。

% 计算逆动力学力矩 tau_calc = threeR_inverse_dynamics(q, dq, qdd, param); % 和实际输入力矩对比 err = tau_input - tau_calc; rms_err = sqrt(mean(err.^2, 1)); disp(rms_err);

如果 rms_err 大于 1e-3，我建议先检查 C 矩阵的 Christoffel 构造是否满足反对称性，再检查重力项求导是否正确。很多时候符号推导的 G 是负号反的，因为势能定义错了方向，这种错误在曲线图形上根本看不出来。

5. 动力学仿真路上的常见坑与排查

5.1 “仿真发散”：先查单位和力矩突变

现象：仿真跑到某个时间点，角度突然变成 NaN 或 Inf，Scope 里曲线直接飞上天。原因：最常见的两类，一是单位不统一（比如长度用了毫米、质量用了克），二是输入力矩在 0 时刻阶跃过大，导致加速度瞬间极大，积分器步长控制失败。解决：先检查所有物理量的单位是否全部为国际单位；再把力矩信号的阶跃改成带斜坡或低通滤波的平滑输入，斜坡时间设个 0.1 秒就能解决大多数发散。

5.2 代数环报错：从模型结构上拆

现象：Simulink 报“存在代数环”，仿真卡在初始化阶段，或者即使能跑也来回震荡。原因：正动力学模型中，qdd 由 q 和 dq 决定，而 q 又是 qdd 积分得到的，同一个仿真步内两者互相依赖，形成代数环。解决：优先用 S 函数把积分器和动力学计算封装成一个模块，Simulink 对纯 S 函数的内部循环不会做代数环检查。或者用 Memory/Unit Delay 断开直接反馈，在路径二我给了离散化方案。注意 Memory 模块会引入一个步长的相位延迟，做控制验证时要能接受。

5.3 惯性张量与质心对不上：结果看起来合理但一验证就翻车

现象：仿真曲线画出来很漂亮，角度没有发散，但用能量守恒或逆动力学校验时误差明显。原因：惯性张量的坐标基准不对。URDF 标准规定惯性张量必须在质心坐标系，而很多 CAD 插件导出时把转动惯量放在了关节坐标系；也有人把质量分布矩阵填成了相对于质心的值，但质心位置却填成了相对于关节的值，导致两者错位。解决：把质心位置和惯性张量统一到同一个坐标系，通常以质心坐标系为准。如果两套数据来自不同导出工具，必须用平行轴定理换算后再填。换算公式是 I_new = I_cm + m*(d^2E - dd')，其中 d 是质心到新原点的向量。

5.4 仿真步长与关节柔性的玄学

现象：同一个模型，只用 ode45 和 Simscape 默认步长时结果完全变样，有时机械臂会自己振起来。原因：步长太大时，积分器无法捕捉关节角速度的高频变化，尤其是连杆接头处存在默认的柔性和阻尼。Simscape 的 Joint 模块即使没有显式设置接触，也会有一定的数值阻尼和弹性。解决：先把关节模块里的阻尼设为 0，看能量是否守恒；再把 MaxStep 设为 0.001 对比结果。如果变化仍然很大，就去检查模型中是否存在隐含的弹簧阻尼器模块，比如 URDF 导入时自动添加的模拟关节弹性。

5.5 模拟结果与期望方向反了

现象：给定正力矩，关节却朝负向运动，或者 3D 动画中机械臂是镜像的。原因：关节的正方向定义不一致。URDF 里的 joint axis 可能与 Simulink 中关节模块 Motion 的正方向相反，也可能基座坐标系旋转了 180 度导致所有角度符号翻转。解决：在模型里加一个很小的测试力矩，观察第一个采样周期的角度变化方向，如果和力矩符号不符，就把力矩信号取反，或者修改 Joint 模块的 Motion 方向属性。注意同时验证所有三个关节，不要只调第一个。

6. 把动力学仿真用起来：控制器验证与代码生成的一点经验

跑通正动力学模型之后，最高价值的用法是拿它验证控制器。我常做的是计算力矩控制：逆动力学算出前馈力矩，再加 PD 反馈。

function tau = computed_torque(q, dq, q_des, dq_des, ddq_des, param) % 计算力矩控制器：前馈+PD [M, C, G] = threeR_dynamics(q, dq, param); Kp = 50 * eye(3); % 位置增益 Kd = 10 * eye(3); % 速度增益 tau = M * (ddq_des + Kd*(dq_des - dq) + Kp*(q_des - q)) + C + G; end

把这段代码的输出作为正动力学模型的力矩输入，在 Simulink 里闭环跑一遍，就能直观看到跟踪效果。调 Kp 和 Kd 时，先从小增益开始，不要上一步还只有 10，下一步直接拉到 500，力矩饱和加一个饱和模块再进模型。我在这上面栽过跟头：增益调大后仿真直接失去稳定，后来发现是输入力矩没有加饱和，模型计算出几十 N·m 的瞬时力矩，物理上根本不可能。

另一个实用点是代码生成。如果仿真验证通过，可以把 MATLAB Function 模块配置成定点或浮点 C 代码生成，用于硬件快速原型。注意代码生成前把函数里的coder.load替换成常量定义，因为嵌入式环境没有文件系统。以前带过的某位同学就因为这个没加载参数，仿真没问题，生成后编译报一堆未定义符号。之后我的习惯是：所有参数全部用常量定义或者全局结构体，避免运行时候 IO 调用。希望这个从建模到排查的流程，能帮你在 Simulink 里把 3R 机械臂动力学仿真做得又快又稳。希望帮到你。

本文还有配套的精品资源，点击获取