面向半导体晶圆传送机械臂的自适应轨迹优化方法及系统与流程

1.本发明实施例涉及半导体晶圆传送技术领域，尤其是一种面向半导体晶圆传送机械臂的自适应轨迹优化方法及系统。


背景技术：

2.目前机械臂在沿轨迹运行时，会对起停速度进行规划，一般都是加速过程和减速过程，常用的速度规划方法有梯形规划、s型规划、多项式规划等。梯形规划是机械臂速度规划方法中最简单的方式，在梯形规划方法中，机械臂整个轨迹的运动分为加速、匀速、减速的过程，速度曲线为一个梯形，速度一阶导不连续，二阶导不连续，加速度较大时会出现抖动；s型速度规划相比梯形规划多了加速度变化的阶段，包括加加速、减加速、加减速、减减速，整个过程更为平滑，同时速度一阶导连续，二阶导不连续，速度曲线为s形；而多项式规划则是设计一个多项式去拟合速度曲线，如三阶多项式、五阶多项式等，速度变化更平滑，但运行时间更长。
3.现有技术中可以通过设置机械臂的最大速度、最大加速度、加加速度来让机械臂较为平滑的实现加速过程和减速过程，然而这些技术只考虑了机械臂起和停阶段，在中间运行过程中一直保持相同速率运动。但在机械臂实际运行过程中，如果轨迹出现较大拐角或者曲率发生较大变化时，机械臂末端为了保持原有线速度会产生较大的加速度以改变速度方向，从而导致机械臂会出现抖动、异响甚至过速的情况。


技术实现要素：

4.本发明实施例提供一种面向半导体晶圆传送机械臂的自适应轨迹优化方法及系统。
5.为解决上述技术问题，本发明创造的实施例采用的一个技术方案是：提供一种面向半导体晶圆传送机械臂的自适应轨迹优化方法，包括：
6.基于建立的机械臂笛卡尔空间轨迹的描述函数构建曲率半径变化模型；
7.确定拟合曲率半径与速度的变化关系的运动函数以根据所述曲率半径变化模型构建机械臂在任意位置的速度变化模型；
8.根据笛卡尔空间与关节空间的映射关系获得关节空间内的速度函数；
9.将根据所述速度变化模型计算得到的速度值依据所述速度函数获得关节空间内的速度约束；
10.在所述速度约束中加入奇异点位置以限制机械臂在奇异点附近的最大速度。
11.作为本发明的一种优选方案，所述基于建立的机械臂笛卡尔空间轨迹的描述函数构建曲率半径变化模型具体包括：
12.建立机械臂笛卡尔空间轨迹的描述函数，所述描述函数为r＝r(t)＝xi yj zk；
13.求解所述描述函数的一阶导数以得到空间轨迹的切向量；
14.求解所述描述函数的二阶导数以得到空间轨迹的点s处的曲率k(s)，其中，k(s)＝
|r``(s)|；
15.根据所述切向量和所述曲率构建曲率半径变化模型，所述曲率半径变化模型具体为：
[0016][0017]
作为本发明的一种优选方案，所述确定拟合曲率半径与速度的变化关系的运动函数以根据所述曲率半径变化模型构建机械臂在任意位置的速度变化模型具体包括：
[0018]
采用对数函数拟合曲率半径与速度的变化关系以得到运动函数，所述运动函数具体为：
[0019][0020]
根据机械臂的运行轨迹设定一曲率半径控制区间[τ0，τ]，并设置在大于τ的曲率半径时的运行速度v
max
和在小于τ0的曲率半径下时的运行速度v
min
；
[0021]
建立速度和曲率半径的对应函数v(r)，其中，
[0022][0023]
将所述曲率半径变化模型代入函数v(r)中以得到速度变化模型，所述速度变化模型具体为：
[0024][0025]
作为本发明的一种优选方案，所述根据笛卡尔空间与关节空间的映射关系获得关节空间内的速度函数具体包括：
[0026]
根据机械臂运行学公式得到笛卡尔空间与关节空间的映射关系其中，为笛卡尔空间坐标向量，为关节空间坐标向量；
[0027]
根据笛卡尔空间坐标向量对关节空间坐标向量的求导运算得到描述关节空间速度对笛卡尔空间速度的映射的雅可比矩阵j，
[0028][0029]
根据对雅可比矩阵的求逆运算得到关节空间内的速度函数vq，vq＝j-1vx
。
[0030]
作为本发明的一种优选方案，所述根据笛卡尔空间坐标向量对关节空间坐标向量的求导运算得到描述关节空间速度对笛卡尔空间速度的映射的雅可比矩阵j具体包括：
[0031]
将笛卡尔空间与关节空间的映射关系两边求解对时间导数
[0032][0033]
将关节空间对时间的导数写为笛卡尔空间对关节空间的倒数乘以关节空间对时间的导数即得雅可比矩阵j，即：
[0034][0035]
作为本发明的一种优选方案，所述将根据所述速度变化模型计算得到的速度值依据所述速度函数获得关节空间内的速度约束具体为：
[0036]
将所述速度变化模型的计算值代入速度函数采用向量的形式获得关节空间内的速度约束
[0037]
作为本发明的一种优选方案，所述在所述速度约束中加入奇异点位置以限制机械臂在奇异点附近的最大速度具体包括：
[0038]
在所述速度约束中加入奇异点位置以得到关节空间约束，记为
[0039]
根据机械臂的型号设定在奇异点处的最大速度得到关节空间约束的计算公式
[0040][0041]
将关节空间约束映射为笛卡尔空间约束得出机械手在笛卡尔空间相应的运行速度
[0042]
为解决上述技术问题，本发明实施例还提供一种面向半导体晶圆传送机械臂的自适应轨迹优化系统，包括：
[0043]
第一构建单元，用于基于建立的机械臂笛卡尔空间轨迹的描述函数构建曲率半径变化模型；
[0044]
第二构建单元，用于确定拟合曲率半径与速度的变化关系的运动函数以根据所述曲率半径变化模型构建机械臂在任意位置的速度变化模型；
[0045]
映射计算单元，用于根据笛卡尔空间与关节空间的映射关系获得关节空间内的速度函数；
[0046]
约束设定单元，用于将根据所述速度变化模型计算得到的速度值依据所述速度函数获得关节空间内的速度约束；
[0047]
速度限制单元，用于在所述速度约束中加入奇异点位置以限制机械臂在奇异点附近的最大速度。
[0048]
为解决上述技术问题，本发明实施例还提供一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机可读指令，所述计算机可读指令被所述处理器执行时，使得所述处理器执行上述所述轨迹优化方法的步骤。
[0049]
为解决上述技术问题，本发明实施例还提供一种存储有计算机可读指令的存储介
质，所述计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行上述所述轨迹优化方法的步骤。
[0050]
综上所述，本发明具有如下有益效果：
[0051]
本发明实施例通过提供一种面向半导体晶圆传送机械臂的自适应轨迹优化方法及系统，通过建立曲率半径变化模型和速度变化模型，实现了在笛卡尔空间中轨迹速度的自适应加速和降速，提高了在轨迹拐点的运行稳定性和精度；同时对关节空间中的速度突变情况进行分析，增加了关节空间的速度约束，并考虑机械臂在奇异点附近时的速度规划问题进而对笛卡尔空间速度施加约束，实现了关节空间和笛卡尔空间的双向反馈，提高机械臂各关节运动的稳定性，使得机械臂各关节的跃度曲线连续，提高了机械臂轨迹跟踪的精度。
附图说明
[0052]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0053]
图1为本发明实施例面向半导体晶圆传送机械臂的自适应轨迹优化方法的基本流程示意图。
[0054]
图2为本发明实施例面向半导体晶圆传送机械臂的自适应轨迹优化系统基本结构示意图。
[0055]
图3为本发明实施例计算机设备基本结构框图。
[0056]
图中数字和字母所表示的相应部件名称：
[0057]
601、第一构建单元；602、第二构建单元；603、映射计算单元；604、约束设定单元；605、速度限制单元。
具体实施方式
[0058]
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
[0059]
在本发明的说明书和权利要求书及上述附图中的描述的一些流程中，包含了按照特定顺序出现的多个操作，但是应该清楚了解，这些操作可以不按照其在本文中出现的顺序来执行或并行执行，操作的序号如101、102等，仅仅是用于区分开各个不同的操作，序号本身不代表任何的执行顺序。另外，这些流程可以包括更多或更少的操作，并且这些操作可以按顺序执行或并行执行。需要说明的是，本文中的“第一”、“第二”等描述，是用于区分不同的消息、设备、模块等，不代表先后顺序，也不限定“第一”和“第二”是不同的类型。
[0060]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0061]
具体请参阅图1，图1为本实施例面向半导体晶圆传送机械臂的自适应轨迹优化方
法的基本流程示意图。
[0062]
如图1所示，一种面向半导体晶圆传送机械臂的自适应轨迹优化方法，包括：
[0063]
s100、基于建立的机械臂笛卡尔空间轨迹的描述函数构建曲率半径变化模型。
[0064]
s100具体包括：
[0065]
s101、建立机械臂笛卡尔空间轨迹的描述函数，描述函数为r＝r(t)＝xi yj zk，其中r代表空间曲线的位置向量，t为笛卡尔空间轨迹的时间，计算空间轨迹的曲率与计算空间曲线的斜率类似；
[0066]
s102、求解描述函数的一阶导数以得到空间轨迹的切向量，即
[0067]
s103、求解描述函数的二阶导数以得到空间轨迹的点s处的曲率k(s)，其中，k(s)＝|r``(s)|，在计算时定义空间曲线r＝r(s)；
[0068]
s104、根据切向量和曲率构建曲率半径变化模型，曲率半径变化模型具体为：
[0069][0070]
当k(s)≠0时，其倒数即为曲线在点s处的曲率半径，曲率半径越大，说明轨迹越平滑，曲率半径越小，说明轨迹越陡峭。在计算曲率过程中，如果某一点出现不可导的情况，则采用分段函数的形式，在这一阶段求dx、dy、dz的偏导作为参考曲率，在获取轨迹曲率函数后，再根据分段函数计算轨迹曲率的变化情况。如果在某个阶段轨迹的曲率较大，说明这一段轨迹较为平滑，没有出现较大角度的转弯，那么机械臂在这段轨迹上可以保持较大速度运行，如果曲率变小，则说明轨迹进入一个较大角度的转弯，那么在这个阶段就要对机械臂降速以保证运动的稳定性。通过对曲率函数分段进行整理即可最终得到上述曲率半径变化模型，根据曲率半径变化模型可计算速度的变化。
[0071]
s200、确定拟合曲率半径与速度的变化关系的运动函数以根据曲率半径变化模型构建机械臂在任意位置的速度变化模型。
[0072]
步骤s200具体包括：
[0073]
步骤s201、采用对数函数拟合曲率半径与速度的变化关系以得到运动函数，运动函数具体为：
[0074][0075]
步骤s202、根据机械臂的运行轨迹设定一曲率半径控制区间[τ0，τ]，并设置在大于τ的曲率半径时的运行速度v
max
和在小于τ0的曲率半径下时的运行速度v
min
；在机械臂运行的过程中一般会设置最大的运行速度，在曲率半径较大的轨迹段，机械臂可以以最大速度运行，而在曲率半径较小的轨迹段，机械臂则应降速运行以维持其平稳性和精度，因此相应的设置曲率半径控制区间[τ0，τ]，记曲率半径为r，当r》τ时，机械臂以最大速度v
max
运行，当r＜τ0时，机械臂以最小速度v
min
运行；
[0076]
步骤s203、建立速度和曲率半径的对应函数v(r)，以上述曲率半径控制区间进行分段划分，即可将速度和曲率半径的对应函数v(r)整合为：
[0077][0078]
即，
[0079][0080]
步骤s204、将曲率半径变化模型代入函数v(r)中以得到速度变化模型，经过运算得出速度变化模型具体为：
[0081][0082]
通过对速度进行微分即可求得机械臂加速度acc＝v`(r)，当曲率半径在曲率半径控制区间之外时，机械臂以最大速度或者最小速度进行匀速运动，当曲率半径变小时，机械臂沿轨迹减速运动，当曲率半径变大时，机械臂沿轨迹加速运动，从而保证机械臂能够平稳的经过轨迹拐点，根据该速度变化模型可以对轨迹上的任意一点计算其对应的速度及加速度，从而实现机械臂运动速率的自适应变化。
[0083]
s300、根据笛卡尔空间与关节空间的映射关系获得关节空间内的速度函数。
[0084]
步骤s300具体包括：
[0085]
s301、根据机械臂运行学公式得到笛卡尔空间与关节空间的映射关系其中，为笛卡尔空间坐标向量，为关节空间坐标向量；
[0086]
s302、根据笛卡尔空间坐标向量对关节空间坐标向量的求导运算得到描述关节空间速度对笛卡尔空间速度的映射的雅可比矩阵j，雅可比矩阵是由向量求导获得，矩阵中的每一项即为关节空间的速度对笛卡尔空间速度的映射，
[0087][0088]
s302具体包括：
[0089]
s3021、将笛卡尔空间与关节空间的映射关系两边求解对时间导数
[0090][0091]
s3022、将关节空间对时间的导数写为笛卡尔空间对关节空间的倒数乘以关节空间对时间的导数即得雅可比矩阵j，即：
[0092][0093]
s303、根据对雅可比矩阵的求逆运算得到关节空间内的速度函数vq，具体计算步骤为
[0094][0095][0096]
vq＝j-1vx
。
[0097]
笛卡尔空间的速度向量一般为六维，即在计算过程中六个维度的速度向量都会对关节空间产生影响。
[0098]
s400、将根据速度变化模型计算得到的速度值依据速度函数获得关节空间内的速度约束，其具体为将速度变化模型的计算值代入速度函数采用向量的形式获得关节空间内的速度约束其中，与均为向量形式，关节空间每个维度的速度都会受笛卡尔空间的全部维度的影响。
[0099]
s500、在速度约束中加入奇异点位置以限制机械臂在奇异点附近的最大速度。
[0100]
在机械臂关节的奇异点附近，会发生较高的关节速率变化来满足笛卡尔空间的期望速度，但往往会出现关节速度超限，从而对机械臂的运行平稳性造成影响，当机械臂运动到奇异点附近时，需要限制关节空间的最大速度来保证关节的运行平稳，再通过正向雅可比矩阵反馈到笛卡尔空间速度，实现低速通过奇异区域。
[0101]
步骤s500具体包括：
[0102]
s501、在速度约束中加入奇异点位置以得到关节空间约束，记为
[0103]
s502、根据机械臂的型号设定在奇异点处的最大速度得到关节空间约束的计算公式
[0104][0105]
s503、将关节空间约束映射为笛卡尔空间约束得出机械手在笛卡尔空间相应的运行速度
[0106]
由于机械臂型号不同，关节空间可以达到的最大速度也不同，根据机械臂使用手册设置最大的同时，不同构型的机械臂奇异点位置也不相同，对于六轴工业机械臂来说，常见的奇异点为五轴角度为0、三轴角度为0的情况，当前五轴到达奇异点位置后，机械臂无法使用笛卡尔坐标进行运动，在机械臂运动中需要人为避开这些位置。
[0107]
本发明通过建立曲率半径变化模型和速度变化模型，实现了在笛卡尔空间中轨迹速度的自适应加速和降速，提高了在轨迹拐点的运行稳定性和精度；同时对关节空间中的速度突变情况进行分析，增加了关节空间的速度约束，并考虑机械臂在奇异点附近时的速度规划问题进而对笛卡尔空间速度施加约束，实现了关节空间和笛卡尔空间的双向反馈，提高机械臂各关节运动的稳定性，使得机械臂各关节的跃度曲线连续，提高了机械臂轨迹跟踪的精度。
[0108]
为解决上述技术问题，本发明实施例还提供一种面向半导体晶圆传送机械臂的自
适应轨迹优化系统。
[0109]
具体请参阅图2，图2为本实施例面向半导体晶圆传送机械臂的自适应轨迹优化系统基本结构示意图。
[0110]
如图2所示，一种面向半导体晶圆传送机械臂的自适应轨迹优化系统包括：第一构建单元601，用于基于建立的机械臂笛卡尔空间轨迹的描述函数构建曲率半径变化模型；第二构建单元602，用于确定拟合曲率半径与速度的变化关系的运动函数以根据曲率半径变化模型构建机械臂在任意位置的速度变化模型；映射计算单元603，用于根据笛卡尔空间与关节空间的映射关系获得关节空间内的速度函数；约束设定单元604，用于将根据速度变化模型计算得到的速度值依据速度函数获得关节空间内的速度约束；速度限制单元605，用于在速度约束中加入奇异点位置以限制机械臂在奇异点附近的最大速度。
[0111]
面向半导体晶圆传送机械臂的自适应轨迹优化系统在进行运动控制时，建立曲率半径变化模型和速度变化模型，实现了在笛卡尔空间中轨迹速度的自适应加速和降速，提高了在轨迹拐点的运行稳定性和精度；同时对关节空间中的速度突变情况进行分析，增加了关节空间的速度约束，并考虑机械臂在奇异点附近时的速度规划问题进而对笛卡尔空间速度施加约束，实现了关节空间和笛卡尔空间的双向反馈，提高机械臂各关节运动的稳定性，使得机械臂各关节的跃度曲线连续，提高了机械臂轨迹跟踪的精度。
[0112]
为解决上述技术问题，本发明实施例还提供计算机设备。具体请参阅图3，图3为本实施例计算机设备基本结构框图。
[0113]
如图3所示，计算机设备的内部结构示意图。该计算机设备包括通过系统总线连接的处理器、非易失性存储介质、存储器和网络接口。其中，该计算机设备的非易失性存储介质存储有操作系统、数据库和计算机可读指令，数据库中可存储有控件信息序列，该计算机可读指令被处理器执行时，可使得处理器实现一种面向半导体晶圆传送机械臂的自适应轨迹优化方法。该计算机设备的处理器用于提供计算和控制能力，支撑整个计算机设备的运行。该计算机设备的存储器中可存储有计算机可读指令，该计算机可读指令被处理器执行时，可使得处理器执行一种面向半导体晶圆传送机械臂的自适应轨迹优化方法。该计算机设备的网络接口用于与终端连接通信。本领域技术人员可以理解，图3中示出的结构，仅仅是与本技术方案相关的部分结构的框图，并不构成对本技术方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
[0114]
本实施方式中处理器用于执行图2中第一构建单元601、第二构建单元602、映射计算单元603、约束设定单元604和速度限制单元605的具体功能，存储器存储有执行上述模块所需的程序代码和各类数据。网络接口用于向用户终端或服务器之间的数据传输，服务器能够调用服务器的程序代码及数据执行所有子模块的功能。
[0115]
计算机设备在进行运动轨迹规划时，建立曲率半径变化模型和速度变化模型，实现了在笛卡尔空间中轨迹速度的自适应加速和降速，提高了在轨迹拐点的运行稳定性和精度；同时对关节空间中的速度突变情况进行分析，增加了关节空间的速度约束，并考虑机械臂在奇异点附近时的速度规划问题进而对笛卡尔空间速度施加约束，实现了关节空间和笛卡尔空间的双向反馈，提高机械臂各关节运动的稳定性，使得机械臂各关节的跃度曲线连续，提高了机械臂轨迹跟踪的精度
[0116]
本发明还提供一种存储有计算机可读指令的存储介质，计算机可读指令被一个或多个处理器执行时，使得一个或多个处理器执行上述任一实施例轨迹优化方法的步骤。
[0117]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，前述的存储介质可为磁碟、光盘、只读存储记忆体(read-only memory，rom)等非易失性存储介质，或随机存储记忆体(random access memory，ram)等。
[0118]
应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。