专利名称：硅片传输机械手双电机同步控制装置及其方法
技术领域：
本发明涉及硅片传输机械手控制技术，具体地说为一种硅片传输机械手双电机同步控制装置及其方法。
背景技术：
硅片传输机械手是一种在集成电路制造业中，用于硅片在生产线上不同的工艺加工模块之间进行高效的传输和定位的专用机器人。集成电路制造业的特点是超精密化、超洁净环境和细微化，硅片传输过程中的轻微震动都可能对硅片造成损坏，这就对硅片传输机械手在运行过程中的平稳性提出了较高要求。当前应用较广的frog-leg型真空机械手，其由两个电机配合驱动两个手臂的运行。由于机械摩擦、机构设计、使用材料等因素的影响，两个手臂的机械特性不可能完全相同。当同时对两个电机施加相同的控制作用时，两个手臂的运行将会出现不同步性，这种不同步性将导致手臂末端托盘出现抖动现象，甚至会滑落损坏硅片。如何减小手臂运行过程中的不同步性、减小机械手抖动成为硅片传输机械手研制的关键问题。
发明内容
针对现有技术中硅片传输机械手存在的不同步性等不足之处，本发明要解决的技术问题是提供一种能够根据两手臂的运行情况对两电机的控制进行补偿，实现机械手臂的同步控制的硅片传输机械手双电机同步控制装置及其方法。为解决上述技术问题，本发明采用的技术方案是本发明硅片传输机械手双电机同步控制装置包括主计算机、DSP电机同步控制卡、电机驱动器以及电机，其中主计算机作为主机，DSP电机同步控制卡作为从机，二者进行通信连接，DSP电机同步控制卡的两输出端分别与两个电机驱动器相连，每个电机驱动器的控制输出端接至电机，电机转速信息通过电机驱动器送至DSP电机同步控制卡。所述DSP电机同步控制卡具有DSP数字信号处理器，通过CAN接口电路与主计算机进行通信，通过存储器保存程序运行过程中的数据，通过模拟量输出电路与电机驱动器相连，通过编码器接口电路接收电机驱动器返回的码盘数据。本发明硅片传输机械手双电机同步控制方法包括以下步骤主计算机通过轨迹规划计算，得到两个电机输出轴的期望位置；通过编码器接口电路读取两个电机输出轴的实际位置；通过期望位置与实际位置求差值进行PID计算得到单轴位置闭环控制量输出给电机驱动器，实现单轴位置闭环控制；通过两个电机输出轴的实际位置计算得到两个电机输出轴的实际速度；根据两个电机输出轴的实际速度分别求两个电机输出轴的速度差和位置差；将上述速度差和位置差输入交叉耦合控制器进行处理，得到交叉耦合控制量；将交叉耦合控制量与单轴位置闭环控制量相加或减，然后输出给电机驱动器，实现两轴的交叉耦合同步控制。所述交叉耦合控制器对速度差和位置差相加，再通过比例计算得到交叉耦合控制量。本发明具有以下有益效果及优点1.本发明提供一种硅片传输机械手同步控制方法和装置，能够在机械手运行过程中提高手臂同步性，避免硅片损坏。2.本发明提供一种速度差和位置差求和后比例计算的交叉耦合控制算法，能够对机械手不同步性进行补偿，提高同步性。
图1为硅片传输机械手双电机同步控制装置总体结构图；图2为DSP电机同步控制卡示意框图；图3为本发明方法流程图；图4为交叉耦合控制算法结构图。
具体实施例方式如图1所示，本发明包括主计算机、DSP电机同步控制卡、电机驱动器以及电机，其中主计算机作为主机，DSP电机同步控制卡作为从机，二者进行通信连接，DSP电机同步控制卡的两输出端分别与两个电机驱动器相连，每个电机驱动器的控制输出端接至电机，电机转速信息通过电机驱动器送至DSP电机同步控制卡。本实施例中，主计算机采用PC104，主计算机和DSP电机同步控制卡通过CAN总线实现通信，DSP电机同步控制卡作为从机。主计算机每个控制周期将轨迹规划的位置点通过CAN总线发送给DSP电机同步控制卡。DSP电机同步控制卡通过模拟量输出线与电机驱动器连接，电机驱动器又通过码盘线将码盘值返回给DSP电机同步控制卡。DSP电机同步控制卡根据主计算机发送来的位置值和电机驱动器返回的码盘值计算控制量，然后将控制量通过模拟量输出线输出给电机驱动器。电机驱动器与电机直接通过驱动器连线连接，实现对电机的伺服控制。如图2所示，DSP电机同步控制卡具有DSP数字信号处理器，通过CAN接口电路与主计算机进行通信，通过存储器保存程序运行过程中的数据，通过模拟量输出电路与电机驱动器相连，通过编码器接口电路接收电机驱动器返回的码盘数据。DSP数字信号处理器运行电机控制程序，CAN接口电路实现DSP数字信号处理器与主计算机的通信，存储器用于保存程序运行过程中的数据，模拟量输出电路将DSP数字信号处理器的计算结果输出给电机驱动器，编码器接口电路用于接收电机驱动器返回的码盘 fn息ο如图3所示，本发明硅片传输机械手双电机同步控制方法包括以下步骤主计算机通过轨迹规划计算，得到两个电机输出轴的期望位置；通过编码器接口电路读取两个电机输出轴的实际位置；通过期望位置与实际位置求差值进行PID计算得到单轴位置闭环控制量输出给电机驱动器，实现单轴位置闭环控制；
通过两个电机输出轴的实际位置计算得到两个电机输出轴的实际速度；根据两个电机输出轴的实际速度分别求两个电机输出轴的速度差和位置差；将上述速度差和位置差输入交叉耦合控制器进行处理，得到交叉耦合控制量；将交叉耦合控制量与单轴位置闭环控制量相加或减，然后输出给电机驱动器，实现两轴的交叉耦合同步控制。如图4所示，所述交叉耦合控制器对速度差和位置差相加，再通过比例计算得到交叉耦合控制量。一轴、二轴位置控制器在DSP电机同步控制卡中通过软件实现，用于位置环控制。 一轴、二轴驱动单元对应两个电机驱动器，实现速度环和电流环的控制。交叉耦合控制器为 DSP电机同步控制卡中软件的一部分，实现交叉耦合控制算法。位置反馈由电机驱动器、码盘线和DSP电机同步控制卡实现。电机驱动器通过码盘线将码盘值返回给DSP电机同步控制卡，DSP电机同步控制卡通过编码器接口电路实现对码盘值的接收。交叉耦合控制算法结构图中，一轴、二轴分别对位置P通过位置环进行闭环跟踪，一轴实际位置为P1，二轴实际位置为P2。对两轴的位置P1、P2求微分得到两轴速度Vl和V2。分别求两轴的速度差和位置差作为交叉耦合控制器的输入，在交叉耦合控制器内部，通过交叉耦合控制算法得到控制量的补偿值，该补偿值加入到速度环的输入，实现两轴的交叉耦合同步控制。
权利要求
1.一种硅片传输机械手双电机同步控制装置，其特征在于包括主计算机、DSP电机同步控制卡、电机驱动器以及电机，其中主计算机作为主机，DSP电机同步控制卡作为从机，二者进行通信连接，DSP电机同步控制卡的两输出端分别与两个电机驱动器相连，每个电机驱动器的控制输出端接至电机，电机转速信息通过电机驱动器送至DSP电机同步控制卡。
2.按权利要求1所述的硅片传输机械手双电机同步控制装置，其特征在于所述DSP 电机同步控制卡具有DSP数字信号处理器，通过CAN接口电路与主计算机进行通信，通过存储器保存程序运行过程中的数据，通过模拟量输出电路与电机驱动器相连，通过编码器接口电路接收电机驱动器返回的码盘数据。
3.一种硅片传输机械手双电机同步控制方法，其特征在于包括以下步骤主计算机通过轨迹规划计算，得到两个电机输出轴的期望位置；通过编码器接口电路读取两个电机输出轴的实际位置；通过期望位置与实际位置求差值进行PID计算得到单轴位置闭环控制量输出给电机驱动器，实现单轴位置闭环控制；通过两个电机输出轴的实际位置计算得到两个电机输出轴的实际速度；根据两个电机输出轴的实际速度分别求两个电机输出轴的速度差和位置差；将上述速度差和位置差输入交叉耦合控制器进行处理，得到交叉耦合控制量；将交叉耦合控制量与单轴位置闭环控制量相加或减，然后输出给电机驱动器，实现两轴的交叉耦合同步控制。
4.按权利要求3所述的片传输机械手双电机同步控制方法，其特征在于所述交叉耦合控制器对速度差和位置差相加，再通过比例计算得到交叉耦合控制量。
全文摘要
本发明涉及一种硅片传输机械手双电机同步控制装置及其方法，步骤为主计算机通过轨迹规划计算，得到两个电机输出轴的期望位置；通过编码器接口电路读取两个电机输出轴的实际位置；进行PID计算得到单轴位置闭环控制量输出给电机驱动器；通过两个电机输出轴的实际位置计算得到两个电机输出轴的实际速度；根据两个电机输出轴的实际速度分别求两个电机输出轴的速度差和位置差；将上述速度差和位置差输入交叉耦合控制器进行处理，得到交叉耦合控制量；将交叉耦合控制量与单轴位置闭环控制量相加或减，然后输出给电机驱动器，实现两轴的交叉耦合同步控制。本发明能够在机械手运行过程中提高手臂同步性，避免硅片损坏，能够对机械手不同步性进行补偿，提高同步性。
文档编号H02P5/52GK102545733SQ20101061422
公开日2012年7月4日 申请日期2010年12月30日 优先权日2010年12月30日
发明者宋吉来, 曲道奎, 王金涛, 邹风山 申请人:沈阳新松机器人自动化股份有限公司