晶圆搬运机器人运动学分析与仿真

doi:10.12677/MOS.2021.103068

期刊菜单

晶圆搬运机器人运动学分析与仿真
Kinematics Analysis and Simulation of Wafer Handling Robot

DOI: 10.12677/MOS.2021.103068, PDF, HTML, XML,
作者: 朱良洪, 于大泳：上海理工大学机械工程学院，上海
关键词: 晶圆搬运机器人；运动学分析；MATLAB；仿真验算；Wafer Handling Robot； Kinematics Analysis； MATLAB； The Simulation Calculation

摘要: 本文选择实验室里的SCARA型搬运机器人作为研究对象，对其进行运动学分析与仿真。首先利用改进型MDH参数法建立各连杆的坐标系，确定D-H参数。接着推导出其运动学方程，利用MATLAB Robotics Toolbox进行运动仿真分析，验证模型的正确性，并使用蒙特卡洛法绘制出末端执行器的工作空间。最后，选择代数法进行逆运动学分析计算，得到运动学逆解，并在MATLAB中将求解过程封装成函数。通过此函数随机输入给定位姿都能得到给定位姿对应的最优逆解，证实了逆解算法的正确性。结果表明，对该晶圆搬运机器人的运动学分析正确，为后续的运动学标定与误差补偿奠定了坚实的理论依据。

Abstract: In this paper, the SCARA handling robot in the laboratory is selected as the research object, and its kinematics analysis and simulation are carried out. Firstly, the improved MDH parameter method is used to establish the coordinate system of each connecting rod and determine the D-H parameters. Then, the kinematics equation of the end-effector was derived, and the correctness of the model was verified by using the MATLAB Robotics Toolbox. The working space of the end-effector was plotted by using the Monte Carlo method. Finally, the algebraic method was used to analyze and calculate the inverse kinematics, the inverse kinematics solution was obtained, and the solution process was encapsulated as a function in MATLAB. The optimal inverse solution can be obtained by randomly inputting the orientation pose through this function, which verifies the correctness of the inverse solution algorithm. The results show that the kinematics analysis of the wafer handling robot is correct, which lays a solid theoretical basis for the subsequent kinematics calibration and error compensation.

文章引用：朱良洪, 于大泳. 晶圆搬运机器人运动学分析与仿真[J]. 建模与仿真, 2021, 10(3): 676-683. https://doi.org/10.12677/MOS.2021.103068

1. 引言

在集成电路产业中，最基本、最核心的环节就是晶圆的制造与加工。晶圆制造设备和技术的改进与创新已成为制约该行业进一步发展的关键问题，晶圆搬运机器人是晶圆加工设备的重要组成部分，它直接反映了整个系统的自动化程度和可靠性 [1]。由于对晶圆的需求量和工业生产规模的不断增加，要求其生产向高速、连续、高精度的生产模式发展。人工处理方式根本不能满足这一要求。这必须通过高速、高精度的硅片搬运机器人来实现。因此通过对晶圆搬运机器人的运动学分析以此提高其定位精度十分必要。

运动学分析分为正向与逆向。正向运动学即已知机器人的各关节变量求解出其末端执行器相对于基座标的齐次变换矩阵，从而了解末端执行器的位置和姿态，求解运动学正解有DH法 [2] 和旋量理论法 [3]。常用的分析方法是D-H法，这是建立机器人连杆坐标系最常用的方法 [4]。目前，在学术界有标准D-H法和改进D-H法两种体系构建方法，D-H参数法是我国最常用的分析方法，然而，标准的D-H法在实际应用中存在许多问题，其实际应用范围和效果与改进的MDH法相比不理想 [5]。因此，为了避免标准D-H方法对晶圆搬运机器人运动学研究带来的弊端，本文则采用改进的MDH方法对其进行分析建模，并使用Matlab验证运动学分析结果的正确性。

机器人运动学里根据机器人末端执行器的位置和位姿信息，计算出每个关节对应的转动关节角，称为机器人的逆运动学问题 [6]。为了将机器人末端执行器的运动控制在指定的空间位姿或沿一定的轨迹运动，需要求解其逆运动学。在复杂逆向运动学的研究中，求解逆运动学常用的方法是解析法，其主要包括代数法、几何法等解法。魏娟 [7] 等人通过矩阵的逆运算得到了机器人的逆解表达式。刘晓刚 [8] 等人将机器人的逆运动学分为四类，并通过几何法求解每一类对应的关节角机器人逆向问题，在实际生产和生活中具有重要意义。机器人反向运动学的解是不能确定能否得到封闭解的，所以不同结构的机器人通常是不同的，需要我们依据具体问题具体分析。

2. 正运动学分析与仿真

2.1. 建立运动学模型

图1所示为本文研究的晶圆搬运机器人。其采用的是SCARA型的结构，有垂直升降和三个水平旋转的关节，并且三个旋转关节的驱动轴相互独立，可以实现灵活运动，其有一个末端执行器，共包括四个个自由度，可以实现R、T、Z、W四个轴的运动 [9]。

Figure 1. Model of a wafer handling robot

图1. 晶圆搬运机器人模型

Figure 2. Structure diagram of wafer handling robot

图2. 晶圆搬运机器人结构简图

由图1的晶圆搬运机器人模型可得到其结构简图如图2所示，其中为i连杆序号； $a_{i}$ 为连杆长度，表示沿 $X_{i}$ 轴，从 $Z_{i - 1}$ 移动到 $Z_{i}$ 的距离； $α_{i}$ 为连杆转角，表示绕 $X_{i}$ 轴，从 $Z_{i - 1}$ 轴旋转到 $Z_{i}$ 轴的角度。 $d_{i}$ 为连杆偏距，表示沿 $Z_{i}$ 轴，从 $X_{i - 1}$ 移动到 $X_{i}$ 的距离； $θ_{i}$ 为关节角，表示绕 $Z_{i}$ 轴，从 $X_{i - 1}$ 旋转到 $X_{i}$ 的角度； $d_{i}$ 为连杆偏距，表示沿 $Z_{i}$ 轴，从 $X_{i - 1}$ 移动到 $X_{i}$ 的距离。关机的转动变换范围都在-Π~Π。由此并创建了机器人连杆的坐标系，从而可得到晶圆搬运机器人的D-H参数表，如表1所示。

Table 1. D-H parameter table

表1. D-H参数表

本文研究对象 $a_{2} = 165 mm$ ， $a_{3} = 165 mm$ ， $d_{2} = 40 mm$ ， $d_{3} = 36 mm$ ， $d_{4} = 23 mm$ 为了检验得出的D-H参数的正确性，利用MATLAB中的Robotics Toolbox建立了机器人模型。如图3所示。

Figure 3. Robot model in MATLAB

图3. Matlab中机器人模型

对在MATLAB所建立的模型进行连杆的简单运动，发现其与实际模型基本一致。因此，上述建立的晶圆搬运机器人连杆坐标系与D-H参数是正确的，下一步即可以进行求解运动学正解方面的研究。

2.2. 正运动学分析

正运动学求解即在已知各关节变量的条件下，通过计算，最终得到末端执行器的位姿与基座之间的函数关系的过程 [10]。根据上述所得出的改进D-H参数表中的数据和图2，图3中的坐标系与关节转角关系，可建立起各关节的位置关系，依次推导出末端执行器相对于基坐标系的齐次变换矩阵，从而建立运动学方程 [11]。为了简但表达，用 $C_{i}$ 和 $S_{i}$ 分别代表 $\sin θ_{i}$ 和 $\cos θ_{i}$ ， $S_{i}_{j}$ 表示 $\sin (θ_{i} + θ_{j})$ 。结合改进的D-H参数可得晶圆搬运机器人各坐标系之间的变换矩阵为：

$T_{1}^{0} = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_{1} \\ 0 & 0 & 0 & 1 \end{matrix}]$ , $T_{2}^{1} = [\begin{matrix} C_{2} & - S_{2} & 0 & 0 \\ S_{2} & C_{2} & 0 & 0 \\ 0 & 0 & 1 & d_{2} \\ 0 & 0 & 0 & 1 \end{matrix}]$ , $T_{3}^{2} = [\begin{matrix} C_{3} & - S_{3} & 0 & a_{2} \\ S_{3} & C_{3} & 0 & 0 \\ 0 & 0 & 1 & d_{3} \\ 0 & 0 & 0 & 1 \end{matrix}]$ , $T_{4}^{3} = [\begin{matrix} C_{4} & - S_{4} & 0 & a_{3} \\ S_{4} & C_{4} & 0 & 0 \\ 0 & 0 & 1 & d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}]$

再根据公式(1)即可求出最后的正运动学方程。

$T_{4}^{0} = T_{1}^{0} T_{2}^{1} T_{3}^{2} T_{4}^{3} = [\begin{matrix} C_{234} & - S_{234} & 0 & a_{3} C_{23} + a_{2} C_{2} \\ S_{234} & C_{234} & 0 & a_{3} S_{23} + a_{2} S_{2} \\ 0 & 0 & 1 & d_{1} + d_{2} + d_{3} + d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}]$ (1)

末端执行器的附着坐标系{E}相对于杆坐标系{4}的齐次变换矩阵为：

$T_{E}^{4} = [\begin{matrix} 1 & 0 & 0 & a_{4} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$ (2)

由式(2)可得末端执行器的坐标系{E}相对于基座标系齐次坐标变换矩阵为：

$T_{E}^{0} = [\begin{matrix} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} C_{234} & - S_{234} & 0 & a_{3} C_{23} + a_{2} C_{2} + a_{4} C_{234} \\ S_{234} & C_{234} & 0 & a_{3} S_{23} + a_{2} S_{2} + a_{4} S_{234} \\ 0 & 0 & 1 & d_{1} + d_{2} + d_{3} + d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}]$ (3)

2.3. 正运动学仿真与验证

为了验证推导出的正运动学方程的正确性，在Matlab里对上述正运动学方程的求解过程进行编程，并将其建立为一个函数T1独立保存。在关节角和移动关节允许变化的范围内给定一定的值 $q = [0, pi / 4, - pi / 4, pi / 3]$ ，将其代入到Matlab机器人工具箱的fkine函数中运算，所得结果为：

$T_{4}^{0} = [\begin{matrix} 0.5000 & - 0.8660 & 0 & 281.7 \\ 0.8660 & 0.5000 & 0 & 116.7 \\ 0 & 0 & 1 & 99 \\ 0 & 0 & 0 & 1 \end{matrix}]$

再将q值代入建立好的函数里进行运算。经过对比，发现得出的结果和上述结果相同，则说明建立的晶圆搬运机器人正运动学方程正确。

3. 工作空间仿真

晶圆搬运机器人的工作空间是其终端可以到达的空间所有位置的点的集合。工作空间代表了其末端执行器的运动范围，是其一个重要运动学指标。对晶圆搬运机器人工作空间的分析和轨迹规划等问题的研究具有实际意义 [12]。

已知晶圆搬运机器人的各关节参数，其中 $d_{1}$ 为[0, 300] mm之间的随机值，各转动角的范围在[−170˚, +170˚]之间，又由(3)式知各转动量与移动量之间的代数关系，在Matlab中依据各关节的运动范围和运动学方程，应用蒙特卡洛法 [13]。绘制出晶圆搬运机器人的工作空间，如图4所示。

由图4可得，机器人末端执行器沿x轴移动范围约为[−330, 330] mm，沿y轴移动范围约为[−327, 327] mm沿z轴的移动范围约为[101, 397] mm。工作空间之间的点分布致密，没有明显的空隙，说明晶圆搬运机器人能够满足正常的运行要求，也展示出其结构设计的合理性与可靠性，为之后研究晶圆搬运机器人的末端执行器的轨迹规划奠定了坚实的理论依据 [14]。

(a) 工作空间三维图 (b) xoy平面投影 (c) xoz平面投影 (d) yoz平面投影

Figure 4. Workspace of wafer handling robot

图4. 晶圆搬运机器人工作空间

4. 逆运动学分析与验证

4.1. 逆运动学分析

逆运动学实际上是正运动学的逆问题，即已知末端执行器的位置和姿态，以确定机器人各关节变量的值 [3]。对于SCARA型晶圆搬运机器人，只要确定四个参数 $p_{x}$ 、 $p_{y}$ 、 $p_{z}$ 和θ就能确定目标点位置，这四个参数分别是末端执行器坐标系相对于基坐标系和绕轴的方位角和三个坐标分量。

于是，从末端坐标系到基坐标系的变换矩阵为：

$T = [\begin{matrix} n_{x} & o_{x} & a_{x} & p_{x} \\ n_{y} & o_{y} & a_{y} & p_{y} \\ n_{z} & o_{z} & a_{z} & p_{z} \\ 0 & 0 & 0 & 1 \end{matrix}]$ (4)

T为空间中已知的一位姿矩阵，末端执行器所能到达的位置都在其所限定的空间里。

让式(1)和(4)相等，可以得到4个非线性方程，进一步可求得 $d_{1}$ 、 $θ_{3}$ 、 $θ_{2}$ 、 $θ_{4}$ ，即求出了运动学逆解。

$p_{x} = a_{2} \cos θ_{2} + a_{3} \cos (θ_{2} + θ_{3})$ (5)

$p_{y} = a_{2} \sin θ_{2} + a_{3} \sin (θ_{2} + θ_{3})$ (6)

$p_{z} = d_{1} + d_{2} + d_{3} + d_{4}$ (7)

$θ_{4} = atan (n_{y}, n_{x}) - θ_{2} - θ_{3}$ (8)

将式(5)和式(6)同时平方然后相加可得到

$p_{x}^{2} + p_{y}^{2} = a_{2}^{2} + a_{3}^{2} + 2 a_{2} a_{3} \cos θ_{3}$ (9)

由式(9)可推出 $\cos θ_{3}$ 、 $\sin θ_{3}$ 、 $θ_{3}$

$\cos θ_{3} = \frac{p_{x}^{2} + p_{y}^{2} - a_{2}^{2} - a_{3}^{2}}{2 a_{2} a_{3}}$ (10)

式(10)左边为一余弦函数，其值域在−1与+1之间，可以利用方程有无解的情况来判断末端执行器是否在可达范围内。假设其在可达范围内。利用三角函数关系可得

$\sin θ_{3} = \pm \sqrt{1 - \cos^{2} θ_{3}}$ (11)

利用2幅角反正切公式计算 $θ_{3}$

$θ_{3} = a t a n 2 (\sin θ_{3}, \cos θ_{3})$ (12)

式(12)的解有正负两种情况，即对应不同的象限。但当关节角的正弦和余弦值确定后，其象限也就确定了，在Matlab中编写此算法时，实际情况是必须应用2幅角反正切公式才能求解正确的关节角。

令 $k_{1} = a_{2} + a_{3} \cos θ_{3}$ ， $k_{2} = a_{3} \cos θ_{3}$ 则式(5) (6)可简化为

$p_{x} = k_{1} \cos θ_{2} - k_{2} \sin θ_{2}$ (13)

$p_{y} = k_{1} \sin θ_{2} + k_{2} \cos θ_{2}$ (14)

将式(13)乘 $k_{1}$ 加上式(14)乘 $k_{2}$ 可得

$\cos θ_{2} = \frac{k_{1} p_{x} + k_{2} p_{y}}{p_{x}^{2} + p_{y}^{2}} = \frac{(a_{2} + a_{3} \cos θ_{3}) p_{x} + a_{3} \sin θ_{3} p_{y}}{p_{x}^{2} + p_{y}^{2}}$

$\sin θ_{2} = \frac{k_{1} p_{y} - k_{2} p_{x}}{p_{x}^{2} + p_{y}^{2}} = \frac{(a_{2} + a_{3} \cos θ_{3}) p_{y} - a_{3} \sin θ_{3} p_{x}}{p_{x}^{2} + p_{y}^{2}}$

同上式(12)方法可求得

$θ_{2} = atan 2 (\sin θ_{2}, \cos θ_{2})$ (15)

至此，通过以上步骤，四个关节变量 $d_{1}$ 、 $θ_{3}$ 、 $θ_{2}$ 、 $θ_{4}$ 已完全解出，且有确定的代数形式，为后期晶圆搬运机器人的轨迹规划计算奠定了基础。

4.2. 运动学逆解验证

由于Matlab中机器人工具箱里求解逆解的ikine函数并不能求出逆解的最优解 [2]。于是将自己在4.1节求解逆解的过程在Matlab中编成一个求逆解的函数T2封装起来。为了验证此函数算法的正确性，先随机输入一组关节变量[0.2, pi/3, −pi/3, pi/3]利用T1函数可直接得到运动学正解矩阵

$nj = [\begin{matrix} 0 .5000 & - 0.8660 & 0 & 0 .2475 \\ 0 .8660 & 0 .5000 & 0 & 0 .1429 \\ 0 & 0 & 1 & 0 .299 \\ 0 & 0 & 0 & 1 \end{matrix}]$ ，将这个正解矩阵nj作为运动学逆解求解的期望值，利用T2函数可

求得矩阵nj对应的8组运动学逆解，结果如图5所示，有一组解[0.2, 1.0472, −1.0472, 1.0471]和期望值[0.2, pi/3, −pi/3, pi/3]相同，即其为最优逆解。当再次随机输入不同的关节角时得到的8组逆解中总存在一组逆解与之前期望值一样，即为最优解 [14]。因此，可以认为求出的运动学逆解正确，继而认为此求解运动学逆解的算法的合理性。

5. 结语

针对晶圆搬运机器人的运动学问题，本文以实验室里的SCARA型晶圆搬运机器人为研究对象，针对其结构特点，利用改进型MDH参数法得到其运动学方程，利用MATLAB进行运动仿真分析来验证模型的正确性；应用蒙特卡洛法绘制出末端执行器的工作空间。针对逆运动学存在多解的问题，选择代数

Figure 5. Results of inverse kinematics solution

图5. 运动学逆解求取结果

法进行逆运动学分析计算，准确地求解出运动学最优逆解，并对逆解结果进行了验证。结果表明，本文对该晶圆搬运机器人建立的运动学模型和运动学分析正确，正逆运动学算法正确，为之后研究晶圆搬运机器人末端执行器的轨迹规划、运动学标定与误差补偿奠定了坚实的理论依据。

参考文献

[1]	刘延杰, 郑小飞, 吴明月. 晶圆传输机器人标定技术研究[J]. 哈尔滨工业大学学报, 2010, 42(1): 74-78.
[2]	李小龙. SCARA机器人标定及轨迹规划[D]: [硕士学位论文]. 哈尔滨: 哈尔滨工业大学, 2017.
[3]	刘冠隆, 贺晓莹, 高兴宇, 等. 七自由度双臂机器人旋量理论正向运动学与工作空间分析[J]. 机械科学与技术, 2019, 38(5): 704-712.
[4]	吴静. 五自由度洁净晶圆传输机器人的动力学建模与分析[D]: [硕士学位论文]. 上海: 上海交通大学, 2013.
[5]	叶上高, 刘电霆. 机器人运动学逆解及奇异和多解的处理[J]. 机床与液压, 2014, 42(3): 27-29.
[6]	郭剑鹰, 吕恬生. 基于主动视觉的机器人末端姿态测量[J]. 上海交通大学学报, 2003, 37(5): 715-719.
[7]	魏娟, 郑欣, 向兵, 等. MOTOMAN-HP20D型机器人的运动学分析与仿真[J]. 中国科技论文, 2018, 13(22): 2580-2585.
[8]	刘晓刚, 陶凤荣. 几何的6R串联型焊接机器人运动学逆解算法[J]. 机械设计与制造, 2015, 288(2): 29-31, 35.
[9]	刘劲松, 朱杨冰, 邱进军. 晶圆传输机器人大臂的模态分析及其结构优化[J]. 制造业自动化, 2015, 37(14): 15-19.
[10]	卢钰庭. 基于视觉的机器人标定的研究[D]: [硕士学位论文]. 广州: 华南理工大学, 2013.
[11]	刘园园. 基于MATLAB的六足机器人运动学分析仿真[J]. 南方农机, 2021, 52(5): 48-49.
[12]	党浩明, 周亚丽, 张奇志. 六自由度串联机械臂建模与运动学分析[J]. 实验室研究与探索, 2018, 37(10): 9-14, 23.
[13]	何佳唯, 平雪良, 李朝阳, 等. 机器人工作空间求解方法研究及应用[J]. 机械传动, 2015, 39(10): 68-71.
[14]	朱庆浩, 臧强, 岳华, 等. 6R工业机器人的运动学分析与仿真[J]. 中国科技论文, 2020, 15(8): 953-958.

为你推荐

友情链接