1. 为什么偏偏是SCARA构型本质与应用价值1.1 从装配任务反推机器人构型我最早接触SCARA机器人是在一个3C电子元器件的自动装配项目里。那时候产线上需要把一颗颗小型芯片从料盘取放到PCB板的指定位置要求速度快、定位准而且末端姿态必须始终垂直于电路板表面。当时团队里有人提议用六轴关节臂说通用性更强但我算了一笔账六轴意味着六个电机、六套减速器和六组驱动成本直接翻倍控制难度也上了一个台阶。后来换成了四自由度SCARA问题一下子简化了很多。SCARA的全称是Selective Compliance Assembly Robot Arm翻译过来是“选择顺应性装配机器臂”。这个名字本身就点明了它的构型哲学在水平方向上有柔性、能够顺应装配过程中的微小偏差在垂直方向上却非常刚硬能够承受较大的下压力。这种特性从结构上就适配插拔、压装、螺丝锁付这类的工序。如果你只是需要在平面内快速移动同时末端还要绕垂直轴转动SCARA几乎是最优解。这张构型图为后面的建模定下了调子SCARA的连杆运动主要集中在水平面内垂直方向只有一个移动关节。所以它的运动学、动力学模型相比六轴机器人简化了许多但也正因如此它非常适合作为机器人建模与仿真的入门样板——模型不至于简单到没有代表性又不会复杂到让人陷入纯公式推导的泥潭。1.2 SCARA四自由度的分工逻辑SCARA的四个自由度并不是随意安排的。先看前两个关节J1和J2都是绕Z轴旋转的转动关节它们在水平面上共同决定末端在XY平面内的位置。第三个关节J3是沿Z轴的移动关节负责末端的升降。第四个关节J4是绕Z轴旋转的腕部关节它决定末端工具比如吸嘴、夹爪的朝向角。用一句话概括就是J1、J2管“到哪儿”J3管“多高”J4管“朝哪边”。这四个关节的动作在空间上是解耦的这给逆运动学求解带来了极大的便利。后面你会看到SCARA的逆运动学不需要迭代逼近可以直接用解析法解出来这在实时控制里意义重大——因为解析解的计算时间是确定的不会出现数值迭代不收敛的问题。如果你用MATLAB做过其他串联机器人的建模应该能体会到SCARA这种解耦结构的优势。RVC机器人可视化工具或者Robotics Toolbox里随便找一个六轴机械臂逆解常常要调初值、设权重稍不留神就跳到了另一组关节角。而SCARA的逆解最多就是出现“肘部向上”和“肘部向下”两种构型你用atan2函数一判断干干净净。所以这篇文章我会围绕四自由度SCARA把运动学、雅可比、动力学、Simulink仿真这一整条链路全部走一遍所有代码都用MATLAB实现保证你能直接跑起来。2. 运动学建模第一步D-H参数与正运动学推导2.1 坐标系建立与D-H参数表运动学建模绕不开D-H参数法。这个方法的核心思路是在每个关节处建立一个坐标系然后用四个参数连杆长度a、连杆扭转角α、关节距离d、关节角θ描述相邻两个坐标系之间的变换关系。四个参数里通常只有一个是变量——转动关节对应θ变化移动关节对应d变化。具体到四自由度SCARA连杆结构和坐标系分配如下J1基座旋转关节绕Z0轴旋转变量为θ1J2大臂末端旋转关节绕Z1轴旋转变量为θ2J3垂直移动关节沿Z2轴移动变量为d3J4腕部旋转关节绕Z3轴旋转变量为θ4以标准D-H法建立参数表假设大臂长度L1300mm小臂长度L2200mmJ3初始偏置为H0400mm关节 iθi (变量)di (mm)ai (mm)αi (rad)1θ10L130002θ20L2200030d3004θ4000注意J3那一行因为它是移动关节变量是d3θ3固定为0。同时因为SCARA结构里所有关节轴线都平行J1、J2、J4绕Z轴J3沿Z轴所以α这一列全部是0变换矩阵变成简单的旋转加平移组合计算量大幅下降。提示在写正运动学的齐次变换矩阵之前务必先把D-H参数表中零值和非零值区分清楚。我见过不少人在这一步把d3和θ4的位置搞混导致后面所有的变换矩阵全部出错排查起来极其痛苦。相邻坐标系之间的齐次变换矩阵公式为T_i Rot(z, θi) · Trans(z, di) · Trans(x, ai) · Rot(x, αi)展开之后是一个4×4矩阵。因为α0sin和cos相关的项直接化简。关节1的变换矩阵只包含θ1和L1关节2的变换矩阵包含θ2和L2关节3的变换矩阵是沿Z轴平移d3关节4的变换矩阵是绕Z轴旋转θ4。2.2 正运动学代码实现与验证正运动学的目标很简单给定四个关节变量θ1、θ2、d3、θ4求出末端在基坐标系下的位置和姿态。SCARA的正运动学可以直接写出闭合表达式但你最好还是用齐次变换矩阵一步步乘一遍这样结构清晰也方便后期扩展到带末端工具的情况。在MATLAB里我建议你用符号计算先验证推导再转成数值函数。符号验证代码如下syms theta1 theta2 d3 theta4 L1 L2 real T01 DH_transform(theta1, 0, L1, 0); T12 DH_transform(theta2, 0, L2, 0); T23 DH_transform(0, d3, 0, 0); T34 DH_transform(theta4, 0, 0, 0); T04 T01 * T12 * T23 * T34;其中DH_transform是你自己写的标准D-H变换函数把上面说到的四个参数依次传进去。这一步做完之后你会看到T04的表达式非常规整位置部分px L1·cos(θ1) L2·cos(θ1θ2)py L1·sin(θ1) L2·sin(θ1θ2)pz H0 - d3姿态部分末端绕Z轴的旋转角为 φ θ1 θ2 θ4这里有个细节H0是J3移动前的初始垂直高度。如果你的装配坐标系把Z轴正方向定义为朝上那么J3向下伸出时d3增大pz就减小。很多人在仿真里发现末端Z坐标“越走越高”往往就是符号约定不统一导致的。数值验证的时候取θ130°、θ245°、d3100mm、θ40°用上述表达式算出来的末端位置应该大约是px 300·cos(30°) 200·cos(75°) ≈ 259.81 51.76 311.57mmpy 300·sin(30°) 200·sin(75°) ≈ 150 193.19 343.19mmpz 400 - 100 300mm我建议你拿到一组参考构型之后先在Robotics Toolbox里用SerialLink和fkine函数交叉验证一遍两边结果一致再往下走。这一步花五分钟能帮你省掉后面排查一个小时的烦恼。3. 逆向运动学解析解把“末端点”变回“关节角”3.1 平面投影法求解J1、J2正运动学是从关节空间到操作空间逆运动学则是反过来。SCARA之所以讨喜是因为逆解几乎不需要费脑细胞你只需要把末端位置投影到XY平面就退化成了一个典型的平面两连杆问题。设末端期望位置为(px, py, pz)末端姿态角为φ。先解J1和J2在XY平面内末端到基座的水平距离为r sqrt(px² py²)根据余弦定理J2的转角θ2满足cos(θ2) (r² - L1² - L2²) / (2·L1·L2)这里就出现了SCARA逆解的两种构型当根号内取正时小臂向上弯折取负时小臂向下弯折。工程上一般默认取正解也就是让机械臂呈“肘部向上”的姿态这样更容易避开工作台面的干涉。θ1的求解稍微绕一点。定义辅助角β atan2(L2·sin(θ2), L1 L2·cos(θ2))则θ1 atan2(py, px) - β这个式子的几何含义是末端方向角减去连杆2在三角形中的贡献角。用atan2而不是atan是为了保证角度落在正确的象限里。注意如果你不想要镜像构型务必在逆解函数里固定θ2的符号。反复切换构型会导致后续轨迹跟踪时关节角出现“跳变”在真实控制里就是机械臂突然抡一个大弧度非常危险。3.2 J3与J4的解算及代码实现J3和J4的解算更简单。J3是垂直移动关节直接由末端Z坐标决定d3 H0 - pz这里H0是机械臂的参考安装高度。如果你的模型中J3向上移动为正那么表达式中的符号要相应调整。J4是腕部旋转关节由末端总姿态角φ减去前两个关节的贡献得到θ4 φ - θ1 - θ2由于θ1和θ2已经在前面的步骤解出θ4就是简单的代数差。整个逆解过程不需要求逆矩阵、不需要迭代、不需要设定初值所以计算耗时极短非常适合写进实时控制循环。MATLAB代码实现如下function q inverseKinematics(p, phi, L1, L2, H0) px p(1); py p(2); pz p(3); r sqrt(px^2 py^2); cos_theta2 (r^2 - L1^2 - L2^2) / (2*L1*L2); cos_theta2 max(-1, min(1, cos_theta2)); % 防止浮点误差越界 theta2 acos(cos_theta2); beta atan2(L2*sin(theta2), L1 L2*cos(theta2)); theta1 atan2(py, px) - beta; d3 H0 - pz; theta4 phi - theta1 - theta2; q [theta1, theta2, d3, theta4]; end这段代码里我特意加了一行max和min因为数值计算时r²-L1²-L2²/(2L1L2)可能因为浮点误差略超[-1,1]区间导致acos返回NaN。这个细节在真实项目中非常重要你拿着末端的目标点做轨迹插补时偶尔会遇到刚好落在工作空间边界上的点不做保护就会崩。4. 雅可比矩阵与奇异位形速度层面的建模要点4.1 几何法构造雅可比雅可比矩阵描述的是关节速度与末端速度之间的线性映射关系。SCARA末端速度包含三个平动分量和三个转动分量但实际只有四个关节所以雅可比是一个6×4矩阵。求雅可比有两条路一条是直接对正运动学的末端位置和姿态表达式求偏导另一条是几何法——对转动关节末端线速度等于角速度叉乘连杆向量对移动关节末端线速度直接沿关节移动方向。我推荐几何法因为物理意义直观而且对SCARA这种简单结构几乎可以口算出结果。SCARA的雅可比可以分块写前两列J1和J2旋转对末端线速度的贡献J1对应 [ -L1·s1 - L2·s12 , L1·c1 L2·c12 , 0 ]ᵀJ2对应 [ -L2·s12 , L2·c12 , 0 ]ᵀ第三列J3移动对XY平面无贡献只在Z方向有速度所以是[0, 0, -1]ᵀ符号取决于你定义的d3正方向第四列J4旋转因为腕部在末端位置理论上它不改变末端线速度所以线速度部分为[0,0,0]ᵀ角速度部分则简单得多J1、J2、J4都绕Z轴旋转所以角速度行是[1, 1, 0, 1]。拼起来就是J [ -L1*sin(q1) - L2*sin(q1q2), -L2*sin(q1q2), 0, 0; L1*cos(q1) L2*cos(q1q2), L2*cos(q1q2), 0, 0; 0, 0, -1, 0; 0, 0, 0, 0; 0, 0, 0, 0; 1, 1, 0, 1 ];4.2 奇异位形分析与MATLAB中的可观测性有了雅可比矩阵就可以分析SCARA的奇异位形。奇异位形是指雅可比矩阵降秩的关节构型此时末端在某些方向上会失去控制能力关节速度会趋于无穷大。对SCARA而言最典型的奇异位形是大臂和小臂完全展开或完全重叠即sin(θ2)0。这种情况下J1和J2两个关节在末端产生的线速度方向平行末端在平面内的移动能力退化到只剩一个方向。你可以想象一个人伸直手臂手腕只能在一条直线上推动东西而在垂直于手臂的方向上加再大的关节力矩末端也纹丝不动。在MATLAB里最简单的方法是直接算det(J(1:2, 1:2))的符号变化。对于SCARA这个行列式的值是L1·L2·sin(θ2)当θ20或θ2π时为零。因此在仿真中设置轨迹时尽量让θ2避开0和π附近区域。如果任务确实无法避免就得在逆解里加入奇异回避策略比如对关节速度做伪逆加阻尼处理J_inv_damped J / (J*J lambda^2 * eye(6));这里的lambda是阻尼系数取值很关键。lambda太大会让跟踪精度下降太小则起不到抑制速度爆炸的作用。工程上常用的做法是依据雅可比的最小奇异值动态调整lambda最小奇异值越小阻尼越大。5. 动力学建模拉格朗日方程与惯性项的完整推导5.1 从拉格朗日方程到标准动力学形式动力学建模用来回答一个问题给定关节的运动轨迹需要多大的关节力矩才能实现这对电机选型、关节减速比设计、轨迹规划中的加速度限制都有直接指导意义。我用拉格朗日法推导。拉格朗日函数L定义为系统动能T减去势能U然后对每个广义坐标求欧拉-拉格朗日方程。这里SCARA的结构优势又体现出来了它的前两个连杆和腕部都在水平面内运动重力势能只和J3相关另外三个关节的重力项直接为零这大幅简化了推导。先说动能部分。连杆1绕基座旋转其动能比较简单T1 0.5·I1·q̇1²。连杆2做平面运动既有绕基座的牵连运动又有相对大臂的相对运动所以动能会比直觉上复杂展开之后会多出一项与cos(θ2)相关的耦合项。连杆4腕部从动力学角度来看通常简化成集中在末端的一个质量块和绕自身轴线的转动惯量不考虑其质量分布对质心位置的影响。把所有连杆的动能和势能代入拉格朗日方程后可以整理成机器人动力学的标准形式M(q)·q̈ C(q, q̇)·q̇ G(q) τ其中M(q)是惯量矩阵C(q, q̇)是科氏力和离心力矩阵G(q)是重力项。5.2 惯量矩阵的具体表达式对SCARA这个四自由度结构M矩阵长得相当规整。用m2表示连杆2质量r2c表示连杆2质心到大臂末端的距离I2表示连杆2绕质心轴线的转动惯量m4、I4分别表示腕部组件的质量和转动惯量惯量矩阵可以写成M(q) M11M120I4M12M220I400m3 m40I4I40I4其中M11 I1 m2·rc2² I2 m4·L1² (m2·L1² 2·m2·L1·rc2·cos(θ2) m4·L2² 2·m4·L1·L2·cos(θ2)) I4M12 m2·rc2² I2 m2·L1·rc2·cos(θ2) m4·L2² m4·L1·L2·cos(θ2) I4M22 m2·rc2² I2 m4·L2² I4看着长但每一项都有物理意义比如M11里带cos(θ2)的项代表连杆2的位置变化对连杆1等效惯量的影响——当小臂伸直时整个系统的等效惯量最大这时候J1电机需要输出的力矩也最大。这对轨迹规划是有指导意义的如果想让机械臂跑得更快尽量让它在惯量小的姿态下加速。科氏力和离心力矩阵C可以用Christoffel符号从M矩阵求出来。对SCARA结构因为M矩阵里只有cos(θ2)项是变量最终只有一个关键的科氏力系数h -m2·L1·rc2·sin(θ2) - m4·L1·L2·sin(θ2)然后科氏力项写成C(q, q̇)q̇ [ h·q̇2·(2q̇1 q̇2), -h·q̇1², 0, 0 ]ᵀ注意第一行有两个子项h·q̇1·q̇2是科氏力项h·q̇2²是离心力项。第二行的-h·q̇1²是作用在J2上的反作用力矩——J1转动时会对J2产生一个“拖拽”力矩这在高速运动时不可忽略。重力项G(q)更简单前三行都是0水平面运动重力不做功第四行也是0J4轴线垂直只有第三行对应J3的垂直移动G3 (m3m4)·g。这些推导如果靠手算很容易错我在MATLAB里建议先用符号工具箱走一遍syms q1 q2 d3 q4 dq1 dq2 dd3 dq4 real syms L1 L2 rc2 m1 m2 m4 I1 I2 I4 g real % 构造M矩阵 M [M11, M12, 0, I4; M12, M22, 0, I4; 0, 0, m3m4, 0; I4, I4, 0, I4]; % 用Christoffel符号求C矩阵 C sym(zeros(4)); for i 1:4 for j 1:4 for k 1:4 C(i,j) C(i,j) 0.5*(diff(M(i,j), q(k)) diff(M(i,k), q(j)) - diff(M(j,k), q(i))) * dq(k); end end end这段代码不要想当然直接跑因为M11和M12里的θ2是符号变量diff函数对q2求导没问题但需要注意dq向量的构造必须正确。我自己用的时候会把q定义为[q1; q2; d3; q4]dq定义为[dq1; dq2; dd3; dq4]这样C矩阵的计算结果直接就是C·dq。5.3 为什么说SCARA的动力学特别适合做控制验证SCARA的动力学模型有个突出特点惯量矩阵中J3对应的行和列是解耦的也就是说垂直升降运动与水平面转动运动在惯性层面互不影响。这意味着如果你要把一个复杂的控制算法移植到SCARA上可以先在M矩阵里去掉第3行第3列单独验证平面内的控制效果再把J3的独立PD控制器加回来。这种分步调试策略在实际工程里非常管用。相比之下六轴机器人的惯量矩阵几乎每个元素都耦合在一起调试一个计算力矩控制器时根本分不清是哪个关节的参数没调对。所以很多搞控制算法的工程师喜欢拿SCARA做试验平台不是因为它便宜而是因为它的动力学结构“好拆”。6. 联合仿真验证让“纸面模型”真正动起来6.1 搭建Simulink机械臂仿真框架有了运动学和动力学模型下一步是让模型在MATLAB/Simulink里真正动起来。很多新手到这一步喜欢直接连PID控制器结果发现末端轨迹乱七八糟。我的建议是先把动力学仿真框架搭对再谈控制。最简单的动力学仿真结构是这样给一个期望轨迹控制器计算力矩τ力矩输入到动力学模型模型输出关节加速度经过两次积分得到关节位置和速度然后与期望轨迹比较形成闭环。在Simulink里核心模块是一个MATLAB Function内容就是动力学正问题计算function [qdd, qd, q] dynamics_update(tau, q, qd, param) M compute_M(q, param); C compute_C(q, qd, param); G compute_G(q, param); qdd M \ (tau - C*qd - G); end这里有个计算技巧不要在Simulink里用Symbolic工具箱那会让仿真慢到怀疑人生。预先用符号计算把M、C、G的解析表达式求出来再用matlabFunction转成普通的m函数文件仿真速度能提升几十倍。另外积分环节我习惯用离散积分器采样时间设为1ms。如果你用连续积分器配变步长求解器仿真过程中会出现步长自适应跳跃导致信号噪声比离散固定步长高很多反而更难判断控制器效果。6.2 轨迹跟踪与控制效果我做了一个典型的圆形轨迹跟踪实验期望末端在XY平面内走半径为50mm的圆圆心设置在(300, 200, 250)处完成时间为4秒然后在Simulink里分别用PD控制、计算力矩控制和滑模控制做对比。关键参数设置如下控制方法J1J2J3J4PDKp8006002000300PDKd403015020计算力矩Kp5004001500200计算力矩Kd302510015这里有个非常值得说的细节计算力矩控制的Kp、Kd不用调得很大因为模型补偿已经消除了大部分非线性项。如果PD控制下的Kp要调到800才能达到期望精度计算力矩控制往往Kp500就够了。这也是动力学的价值所在——它让控制器从“对抗物理系统”变成“修正残差”。仿真结果的轨迹误差PD控制在圆形轨迹切向会有明显滞后特别是在高速段计算力矩控制的误差则主要来自模型参数不准确我故意把模型里的m4增大10%然后观察计算力矩控制的跟踪精度结果发现在加速度较大的拐弯处出现了周期性偏差。这说明计算力矩控制对模型精度是有要求的模型参数误差会直接转化为跟踪误差。这个实验做完之后你应该能理解动力学模型不只是用来算几个公式好看的它直接决定了控制器能达到的上限。如果你只是想做个动起来的演示PD就够了但如果你想研究高性能运动控制动力学模型是绕不过去的基础。7. 建模与仿真过程中的四个常见坑7.1 D-H参数符号约定混乱这是我见过最多人踩的坑我自己也踩过。标准D-H与修正D-H的坐标系定义方式不同导致a和α的位置互换。如果你混合使用两种约定正运动学结果会差出一个旋转变换末端位置直接错位几百毫米。我的建议是一个项目里只用一种约定并且在代码开头用注释写明你选的是哪一种。如果你没有特殊需求就用标准D-H。然后把关节角的正方向也定义清楚。我的习惯是所有绕Z轴旋转的关节逆时针为正。MATLAB的三角函数默认弧度制所有角度输入输出都统一用弧度绝对不要在rad和deg之间来回切换。7.2 逆解中的浮点误差与边界角点逆运动学求解时acos函数对输入区间要求是[-1,1]但前面提到过浮点计算可能让cosθ2超出这个范围。这就是为什么我在逆解函数里加了那一行clip操作。另一个浮点相关的坑是当末端轨迹正好经过工作空间边界时r² - L1² - L2²刚好为零或负值此时θ2 π机械臂处于完全伸展的奇异位形。如果你在轨迹规划时没有对期望轨迹做工作空间检查这里就会出现NaN或者角度跳变。实际项目里我一般会在逆解函数内部加上一个返回值标志位标记当前构型是否接近奇异然后在轨迹规划层面对奇异区域做减速或重新规划。7.3 动力学模型符号推导的校验方法手推或代码生成动力学模型的过程中很容易出现符号错误。最典型的错误是M矩阵不对称或者科氏力矩阵与M矩阵的导数不满足关系式 Ṁ - 2C 的反对称性。这两个性质是动力学模型自洽的必要条件也是你在提交仿真代码之前必须验证的。在MATLAB里做符号校验很简单对M的每个元素求关于时间的全导数构造Ṁ - 2C矩阵然后检查它是否满足反对称性。只要有一项不满足说明推导或代码中有错误。这个校验方法是我做动力学建模多年下来最依赖的一道“安全网”。7.4 Simulink仿真实时性与代码生成问题最后一个坑是关于Simulink仿真的实时性。很多人在Simulink里搭建了漂亮的控制器模型想部署到实时机上跑结果发现模型跑一步要花远超1ms的时间根本无法实时运行。这时候要做两件事第一把所有的符号计算、循环和动态内存分配全部去掉。Simulink的代码生成器对固定大小数组、无动态内存的代码支持最好。第二把M矩阵的求逆运算改成预先计算好的解析逆矩阵或者使用M\b操作在代码生成时换成LAPACK的求解器比显式求逆快很多且数值更稳定。我见过太多人在这个环节卡住最后不得不重新写一遍C代码。其实只要你从一开始就把MATLAB Function的代码风格往嵌入式方向靠——参数全部作为结构体传入、中间变量预先分配、避免parse和eval——生成的代码几乎可以直接用。最后再分享一个小技巧当你在仿真中修改了动力学参数比如连杆质量不要只改Simulink里的常量模块记得把逆运动学模块和轨迹规划模块里的工作空间边界也一起更新。因为质量参数会影响关节力矩上限进而影响实际可达的末端加速度而轨迹规划模块如果还按原来的边界规划就会出现仿真中力矩饱和、实际轨迹偏离期望轨迹的情况。把参数集中放在一个MATLAB结构体里统一管理然后用assignin把它灌到模型工作空间这样改起来不容易漏。这个习惯帮我避免了很多次“仿真看着没问题、一上真机就翻车”的尴尬。