拿到“四自由度SCARA机器人的运动学和动力学MATLAB建模与仿真”这个题目，我第一反应就是点头。SCARA这种结构——工作台上一坐，水平面两个旋转关节负责“找位置”，垂直移动关节负责“上下抓取”，手腕再来一个绕竖轴的微调——几乎是把“装配机器人”四个字写脸上了。消费电子装配线上贴片、锁螺丝、分拣，到处都是它。
但真要自己从零搭一套运动学、动力学模型，再用MATLAB把仿真跑通，很多同学会被卡在“公式都会推，代码写不出来”这个节点上。这篇文章就是把我当时从DH参数表一路做到ODE45闭环仿真的完整过程摊开来讲，既给出可直接复用的MATLAB代码思路，也会解释每一步为什么要这么做，方便正在做课程设计、机器人毕设，或者刚入职想做前期运动学算法验证的工程师直接“抄作业”。
1. 项目概述与整体设计思路
1.1 为什么选SCARA结构做建模对象
先聊清楚一个大前提：建模对象那么多，为什么偏偏选SCARA？因为它的结构在工业机器人里属于“恰到好处”。
SCARA（Selective Compliance Assembly Robot Arm）结构上有四个自由度：基座处一个旋转关节，大臂末端一个旋转关节，这两个关节决定了末端在水平面上的XY位置；第三个关节是垂直方向上的移动关节，负责Z轴升降；第四个关节是手腕处的旋转关节，用来调整末端工具的朝向。正是这种“水平面内刚性大、垂直方向柔顺”的特点，让它非常适合插拔、装配这类动作，也是它名字里“选择性柔顺”的由来。
从数学建模角度来看，SCARA又确实“友好”：它的前两个旋转关节的轴线都沿竖直方向，所以水平面内的运动学可以当成经典的二连杆平面机构来处理，不会像六轴机械臂那样有大量球坐标耦合。同时Z轴移动关节与其他关节在动力学上近似解耦，这让质量的导入、重力的处理都变得直观很多。换句话说，SCARA是那种“结构有工程意义，建模又不至于难到劝退”的典型对象，特别适合用来建立完整的建模-仿真闭环。
1.2 从DH参数到闭环仿真的技术路线图
整个项目如果只有一个总纲，我的建议是把它拆成五步。
第一步，建立运动学模型。用Denavit-Hartenberg参数法把SCARA每个连杆的坐标系关系写出来，得到正运动学变换矩阵，再基于几何关系反推逆运动学解析解。第二步，求解速度雅可比矩阵，这步在方案里虽然不单独占章节，但确实是为动力学和轨迹分析做铺垫的关键。第三步，推导动力学模型，这步是全场最耗时的一个环节，最常用的方法是拉格朗日方程，把系统的动能和势能写出来，然后求偏导得到广义力。第四步，在MATLAB里面把前面推导得到的矩阵方程变成可执行的ode45微分方程，加上PD计算力矩控制器和五次多项式轨迹规划器。第五步，对仿真结果进行误差和运动状态分析，判断模型是否合理。
这套路线最大的好处是，每一步都有明确的数学工具支撑，不会出现“做完运动学不知道下一步干什么”的空档期。而且所有环节在MATLAB里都有对应的现成函数可以辅助验证，比如符号计算可以用Symbolic Math Toolbox快速求导，矩阵运算直接用原生语法就行。整条链路走通之后，后面换机构参数、换控制算法甚至换机器人型号，成本都很低。
2. 运动学建模：DH参数表与正逆解实现
2.1 SCARA的DH参数定义与坐标变换矩阵
运动学建模的第一步，是给这台SCARA建立挨个连杆之间的坐标系关系。这里我采用的是标准DENAVIT-Hartenberg约定，也就是每个变换矩阵由四个参数决定：连杆长度、连杆扭转角、关节距离和关节角。
对于我使用的这个四自由度SCARA，DH参数表如下：
| 关节 | 连杆长度 a_i (m) | 连杆扭转角 α_i (rad) | 关节距离 d_i (m) | 关节角 θ_i |
|---|---|---|---|---|
| 1 | l1 = 0.25 | 0 | d1 = 0.30 | q1（变量） |
| 2 | l2 = 0.25 | 0 | 0 | q2（变量） |
| 3 | 0 | 0 | q3（变量） | 0 |
| 4 | 0 | 0 | d4 = 0.08 | q4（变量） |
表中d1是基座到第一关节旋转中心的高度，l1是大臂长度，l2是小臂长度，d4是手腕末端到抓手的偏置。把这些参数代入标准DH变换公式，可以得到从基坐标系到每一个关节坐标系的齐次变换矩阵。
这里有一个特别容易踩坑的细节：SCARA中竖直移动关节d3的位置。工业中这个移动关节的坐标系有时会沿用前一个旋转关节的Z轴方向，也就是说正方向“向上”，而抓手的实际运动方向往往向下，等到做逆运动学时，就极容易出现符号错误。我自己的习惯是把Z轴正方向固定为“向上”，然后用z = d1 - q3 - d4来表示末端高度，这样一来，q3增大表示手臂上升、抓手升高，物理含义和坐标定义是自洽的。
2.2 正运动学推导：从连杆变换到末端位姿
有了DH表，正运动学就是一个“机械式”的过程：把四个齐次变换矩阵相乘，得到末端执行器相对于基坐标系的总变换矩阵。
具体来说，相邻连杆之间的变换矩阵满足：
T_{i-1,i} = Rz(θ_i) · Tz(d_i) · Tx(a_i) · Rx(α_i)
把四个矩阵按顺序乘起来之后，末端执行器的位置坐标可以直接提取为：
x_e = l1·cos(q1) + l2·cos(q1 + q2) y_e = l1·sin(q1) + l2·sin(q1 + q2) z_e = d1 - q3 - d4
末端朝向角则可以表示为phi_e = q1 + q2 + q4。
这段推导本身不难，但很多人会把l1·cos(q1) + l2·cos(q1+q2)这一步搞混，写成l1·cos(q1) + l2·cos(q1) + l2·cos(q2)。之所以出错，是因为忘了第二根连杆的角度是相对于第一根连杆末端测量的，所以它在全局坐标下的朝向是q1与q2之和。正运动学写对，后面逆运动学才有得谈。
在MATLAB里要实现正运动学，可以直接用符号工具或者手写矩阵函数。比如：
function [x, y, z, phi] = fkine_SCARA(q, param) % q = [q1; q2; q3; q4] % param: 含l1,l2,d1,d4的结构体 q1 = q(1); q2 = q(2); q3 = q(3); q4 = q(4); x = param.l1*cos(q1) + param.l2*cos(q1+q2); y = param.l1*sin(q1) + param.l2*sin(q1+q2); z = param.d1 - q3 - param.d4; phi = q1 + q2 + q4; end这段代码虽然只有几行，但在整个仿真里它是“基准线”，后面所有轨迹跟踪误差分析都靠它来对比。
2.3 逆运动学解析解：几何法推导与多解处理
逆运动学的目标是，给定末端执行器的期望位姿，求四个关节变量q1、q2、q3、q4。这个闭环系统最大的好处是，我们可以用几何法直接求出解析解，而不像六轴机器人那样要依赖数值迭代求逆。
先处理XY平面。已知期望位置(x_ref, y_ref)，二连杆机械臂平面逆解公式为：
令 c2 = (x_ref² + y_ref² - l1² - l2²) / (2 · l1 · l2)
则 q2 = atan2(±sqrt(1 - c2²), c2)，其中正负号对应“肘上”和“肘下”两种构型。
求出q2之后，再求q1：
令 beta = atan2(l2·sin(q2), l1 + l2·cos(q2))
则 q1 = atan2(y_ref, x_ref) - beta
这两步推导的关键是理解“期望点是否在可达空间内”。如果x_ref² + y_ref² > (l1 + l2)²，说明目标点超出了机械臂的最大伸展半径，无解；如果x_ref² + y_ref² < (l1 - l2)²，小于最小收缩半径，同样无解。这两个条件在实际代码里必须提前判断，否则sqrt括号里会出现负实数。
Z轴和手腕角度就简单多了：
q3 = d1 - z_ref - d4 q4 = phi_ref - q1 - q2
逆解的MATLAB函数大概是这个样式。实际写的时候，别忘了把返回值限制在[-pi, pi]范围内，可以省掉后面控制器里一大堆角度缠绕问题。
function q = ikine_SCARA(xref, yref, zref, phiref, param, EIbowUp) c2 = (xref^2 + yref^2 - param.l1^2 - param.l2^2) / (2*param.l1*param.l2); if abs(c2) > 1 error('目标点超出机器人可达范围'); end s2 = sqrt(1 - c2^2); if EIbowUp q2 = atan2(-s2, c2); % 肘上构型 else q2 = atan2(s2, c2); % 肘下构型 end beta = atan2(param.l2*sin(q2), param.l1 + param.l2*cos(q2)); q1 = atan2(yref, xref) - beta; q3 = param.d1 - zref - param.d4; q4 = phiref - q1 - q2; q = wrapToPi([q1; q2; q3; q4]); end逆运动学还有一个工程细节：当末端轨迹穿过工作空间中心区域时，肘上解和肘下解之间会发生构型突变。控制器如果直接切换构型，关节速度会瞬间拉到极大值，所以轨迹规划时最好先固定一种构型，连续走完再考虑切换。
3. 动力学建模：基于拉格朗日方程的推导与实现
3.1 为什么用拉格朗日方法，不推荐牛顿欧拉
到了动力学部分，思路要换一个频道。运动学只关心“几何位移关系”，动力学关心的是“关节力矩/力如何产生加速度”。
我在这部分的选型很明确：用拉格朗日方程，而不是牛顿-欧拉递推。理由不复杂：SCARA自由度少，动能和势能表达式写起来并不吓人，拉格朗日法从能量角度出发，可以自然得到质量矩阵、科里奥利-离心力向量和重力向量的解析表达式，而这些表达式在做控制仿真时是直接可编程的。牛顿-欧拉方法在六轴机器人里确实更高效，但对SCARA这种小型机构优势不大，反而推导过程更抽象，出了符号错误很难一眼看出来。
拉格朗日动力学方程的形式是：
τ = M(q)·q̈ + C(q, q̇)·q̇ + G(q)
其中M(q)为惯性矩阵，C(q, q̇)为科里奥利力与离心力矩阵，G(q)为重力向量。这三项全部来源于系统动能T和势能V的偏导。
3.2 质量矩阵、科里奥利项与重力项的推导
对于四自由度SCARA，前两个旋转关节所在的水平XY平面动力学，可以看成一个二连杆平面机器人的经典问题。假设第一根连杆（大臂）质量为m1，质心到关节1的距离为lc1，转动惯量为I1；第二根连杆（小臂）质量为m2，质心到关节2的距离为lc2，转动惯量为I2，那么二连杆系统的质量矩阵为：
M11 = m1·lc1² + I1 + m2·(l1² + lc2² + 2·l1·lc2·cos(q2)) + I2 M12 = M21 = m2·(lc2² + l1·lc2·cos(q2)) + I2 M22 = m2·lc2² + I2
注意M矩阵是q的函数，特别是M11和M12中带有cos(q2)项，说明机械臂在不同姿态下，各关节的等效惯量是变化的。这是机器人动力学最核心的特征，也是它与单轴伺服电机驱动系统最大的区别。
科里奥利力和离心项定义为一个向量，其中h = -m2·l1·lc2·sin(q2)，则：
C1 = h·(q̇1·q̇2 + q̇2²) + h·q̇1·q̇1 这是错的，正确形式应为： C向量 = [c11·q̇1 + c12·q̇2; c21·q̇1 + c22·q̇2]，其中 c11 = -m2·l1·lc2·sin(q2)·q̇2 c12 = -m2·l1·lc2·sin(q2)·(q̇1 + q̇2) c21 = m2·l1·lc2·sin(q2)·q̇1 c22 = 0
这里的c11来自离心力，c12和c21来自哥氏力。它们不产生能量储存，只代表旋转参考系之间的耦合效应。在实际仿真中，如果忽略了这一项，高速运动时轨迹会明显偏。
重力项G的推导是SCARA的一大红利：因为前两个旋转关节的运动平面是水平的，重力对它们不做功，所以G1 = 0, G2 = 0。但对于移动关节q3，系统要克服重物和手臂自身的重力，因此G3 = m_eff·g，其中m_eff是小臂末端及负载的等效质量。手腕旋转关节q4也水平旋转，G4 = 0。
把这几项合起来，得到完整的4x4质量矩阵是块对角结构：
M_total = [[M11, M12, 0, 0], [M21, M22, 0, 0], [0, 0, m_eff, 0], [0, 0, 0, I4_eff]]
这个块对角结构非常重要，它告诉我们：XY平面内的两个旋转关节和Z方向移动关节之间，在动力学上是解耦的。工程上这意味着我们可以把控制器拆成“水平面位置环”和“垂直方向力位环”两个独立子控制器，调试起来非常方便。
3.3 参数辨识与简化处理：哪些量不能随便省
动力学模型再漂亮，参数不准就等于白扯。这里有三个参数我是建议认真处理的。
第一是质量与质心位置。如果你手边有三维模型，SolidWorks或者Fusion 360可以直接算出每根连杆的质量、质心和转动惯量；如果没有CAD模型，那就只能用实验测量或者查参考数据。质心位置对M矩阵的cos(q2)项影响巨大，质心偏了5%，控制精度就可能差一个数量级。
第二是电机转子惯量。很多建模初学者只算连杆惯量，忘了电机转子和减速器本身也有转动惯量，而且经过减速比放大之后，折算到关节侧的等效惯量可能相当大。这一点最容易导致仿真里的系统比真实系统“轻”，从而控制器增益调试结果在真机上失效。一般在仿真里我会在M11、M22中额外加上折算后的转子惯量。
第三是摩擦项。工业SCARA关节里减速器的摩擦非常显著，线性段可以建模成粘性摩擦加库仑摩擦：τ_friction = B·q̇ + Fc·sign(q̇)。虽然基础拉格朗日模型里没有摩擦项，但真要和实际对照，摩擦必须补上，否则你会发现仿真里机器人“飘”得过分，轨迹误差小到不真实。
4. MATLAB仿真实现：从状态方程到轨迹跟踪
4.1 仿真主框架设计：ODE45下的状态微分化
动力学方程推导完成之后，下一步就是把二阶微分方程降阶成一阶状态空间形式，交给MATLAB的ODE45来积分。定义一个8维状态向量：
x = [q1; q2; q3; q4; q̇1; q̇2; q̇3; q̇4]
那么状态导数就是：
ẋ = [q̇; M⁻¹·(τ - C - G)]
在MATLAB里，这个动态函数大致是：
function dx = scara_dynamics(t, x, tau_func, param) q = x(1:4); dq = x(5:8); [M, C, G] = scara_mass_coriolis_gravity(q, dq, param); tau = tau_func(t, q, dq, param); dx = zeros(8,1); dx(1:4) = dq; dx(5:8) = M \ (tau - C - G); end这里M是4x4质量矩阵，C是科里奥利向量，G是重力向量。ODE45求解时，只需要在仿真时间区间上按0.001秒的步长调用动态函数即可。注意，MATLAB的ODE45是自适应步长求解器，它会自动判断误差调整步长，所以不要人为固定很小的步长，否则效率反而低。
4.2 计算力矩控制器设计：PD反馈加前馈补偿
我在仿真里采用的控制方式是计算力矩控制，原理一句话就能讲清楚：既然动力学模型已知，那我们可以在控制端对非线性项做精确补偿，让整个系统“线性化”成一组互相独立的二阶积分器。
控制律写为：
τ = M(q)·(q̈_des + Kp·(q_des - q) + Kd·(q̇_des - q̇)) + C(q, q̇) + G(q)
其中q_des、q̇_des、q̈_des来自规划的期望轨迹，Kp和Kd是PD系数矩阵。按这个式子，如果模型参数精确无误差，最终误差方程会退化为：
ë + Kd·ė + Kp·e = 0
也就是说，轨迹误差随时间指数收敛，收敛速度由Kp和Kd决定。这比单纯PD控制强在什么地方？强在它把非线性项提前“喂”给了控制器，让系统在高速、大角度运动时也能保持误差均匀，而不是只在平衡点附近效果好。
实现时，为了更贴近工程，我通常还会给Kp和Kd做成对角矩阵，每个关节单独配置。这里有一个经验值：Kp可以取频带宽度的平方，Kd取2倍阻尼比乘以带宽，比如带宽6 rad/s，阻尼比1，Kp=36，Kd=12。
4.3 轨迹规划：空间直线与五次多项式插值
控制算法需要一个明确的期望轨迹作为输入，轨迹规划部分我用的是“路径点加五次多项式插值”。
具体流程是：先在工作空间定义一条从起点到终点的直线路径，在这条路径上均匀插值若干路点；然后对每个路点调用逆运动学，得到对应的关节位置；再对关节位置序列做五次多项式插值，保证关节位置、速度和加速度在起始和终止时刻都为零，没有冲击。
五次多项式的表达式是：
q_des(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵
系数由六个边界条件唯一确定：
q(0)=q_start, q(T)=q_end, q̇(0)=0, q̇(T)=0, q̈(0)=0, q̈(T)=0
写成MATLAB代码时，可以用系数矩阵直接求解，也可以用polyfit思路。比较直观的写法是：
t_coef = [0, T]; q_edge = [q_start, q_end]; dq_edge = [0, 0]; ddq_edge = [0, 0]; A = [1, 0, 0, 0, 0, 0; 1, T, T^2, T^3, T^4, T^5; 0, 1, 0, 0, 0, 0; 0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4; 0, 0, 2, 0, 0, 0; 0, 0, 2, 6*T, 12*T^2, 20*T^3]; b = [q_start; q_end; 0; 0; 0; 0]; a = A \ b;再生成每个时刻的q_des、q̇_des、q̈_des即可。
4.4 仿真结果解读：从关节角到位姿误差
跑完仿真之后，主要看三张图。
第一张是四个关节的实际位置与期望位置对比曲线。正常情况下，实际位置会紧贴期望曲线，启动和结束段略有误差但很快收敛。如果发现曲线明显滞后或者振荡，优先检查Kp/Kd的比值，以及仿真步长。
第二张是末端执行器在XY平面上的实际轨迹与期望直线轨迹对比。这张图能直观看出轨迹是否沿直线“走偏”。计算力矩控制器在模型准确时，直线度通常可以控制在毫米级以内；如果偏差呈“香蕉状”，多半是C项或G项没有算对。
第三张是末端位置误差随时间变化曲线。启动段误差最大，中间段趋于零，结束段如果表现为单调收敛说明PD参数合理；如果出现等幅振荡，说明阻尼比偏低，要增大Kd。
5. 仿真中常见问题与排查技巧实录
5.1 DH参数符号错误导致的末端位置漂移
这个是我自己踩过最深的一个坑。DH表中α_i这一列非常容易被忽略，尤其是当大家觉得SCARA所有关节轴都平行时，就直接把α全部设为0。但实际上，标准DH约定中相邻Z轴如果不完全同向，α可能有π的变化。
更隐蔽的是q3的符号方向问题。很多参考程序里把Z轴正方向定义为“向上”，但实际工业SCARA工具会“向下”伸出。如果在逆运动学时没有做统一，末端在Z方向就会整体差一个正负号，表现就是仿真里抓手焊在台面上方几厘米处，看轨迹整体偏移。
排查建议：先单独做正运动学测试，给定一组q=[0; 0; 0; 0]，看末端位置是否等于[l1 + l2; 0; d1 - d4]。如果不是，先找正解的问题，再谈逆解。
5.2 ODE45发散：质量矩阵奇异与初始条件突变
仿真中积分发散是很常见的问题。原因一般有三种。
一是质量矩阵M奇异。当SCARA完全伸展，即q2 = 0时，二连杆的M11和M12会非常接近。从数学角度看，这时系统在腕点沿径向方向几乎没有惯性，理论上会退化。虽然四自由度的质量矩阵加了竖直和手腕维度，使得整体矩阵不一定奇异，但数值上接近奇异时，ODE45自适应性会崩溃。
二是初始条件突变。如果你的期望轨迹从t=0时就有非零速度和加速度，而实际状态还没跟上，控制器需要在第一个仿真步输出巨大的力矩，积分器很容易发散。解决办法是轨迹规划时保证初始状态与期望状态完全一致。
三是跳出物理边界。当手臂位置超出工作空间，逆运动学求出的q值会突变，进而带动控制器输出异常。仿真前一定要做可达性检测。
5.3 参数单位与数量级不一致
这个排查点最不起眼，但毁掉仿真的概率极高。我见过至少两三次，因为角度用了度，而PID增益按弧度标定，导致控制输出直接爆炸。建议所有角度量统一用弧度，长度量统一用米，质量用千克，惯量用kg·m²。仿真代码里可以用结构体param把所有参数装在一起，这样传参清晰，也不怕漏改。
另外还有一个容易被忽略的数量级问题：转动惯量包括电机折算惯量之后可能到10⁻³量级，而重力项和科里奥利项可能到几N·m。如果打印出来觉得数值诡异，不要急着怀疑代码，先检查单位换算。
6. 从仿真到实际：我的几点经验与扩展建议
6.1 模型验证不能只看仿真曲线，还要做一致性测试
仿真曲线再光滑，也只能说明“仿真自洽”，不能说明“模型正确”。我建议在跑完整仿真前，先做几个静态验证。比如让机械臂停在某个固定姿态，检查重力向量G是否与真值一致，看q3关节的控制输入是否约等于m_eff·g。再比如让关节2以恒定角速度运动、关节1锁死，看C项输出值是否与手算结果一致。这些测试虽然烦，但它们在仿真阶段就帮你拦下大部分建模错误，省得后面调试控制器的几天都在跟模型错误作斗争。
6.2 参数误差敏感性分析：做仿真该有的工程意识
做完基线仿真之后，我强烈建议顺手做一次参数误差敏感性分析。具体做法是，把质量、质心位置、惯量分别人为加入5%的偏差，再观察轨迹跟踪误差的变化。如果误差增幅还在可接受范围内，说明控制器抗扰能力强；如果误差直接炸开，说明模型参数对你的控制器影响极大，真机上这是一定要重点关注的。
这一步不用改代码主体，只需要在参数结构体里多写几个乘法系数，跑几组对照实验即可。
6.3 后续扩展：从仿真模型到关节柔性与摩擦辨识
最后聊聊这个项目的扩展方向。如果你仿真已经跑通，下一步可以往三个方向深入：一是把关节柔性（谐波减速器刚度）加进动力学模型，变成柔性关节模型，这个对工业SCARA的振动分析很重要；二是加入摩擦参数辨识环节，通过匀速轨迹实验反推粘性摩擦和库仑摩擦系数，这会让仿真更贴近真实；三是把MATLAB里的模型代码迁移到C++或ROS里，配合Rviz显示三维运动，可以用于验证整机控制系统的实时性能。
我个人的体会是，SCARA这套建模流程跑完一遍，最大的收获不是那几行公式，而是建立了一种“先建模、再仿真、后验证”的思路——以后你遇到六轴机器人、并联机器人，甚至移动机械臂，第一反应都会是：这个系统的广义坐标是什么、质量矩阵怎么算、控制目标怎么定义。思路一旦打通，换题目只是换参数的事。
