硅片传输机械手双电机同步控制装置及其方法

摘要

本发明涉及一种硅片传输机械手双电机同步控制装置及其方法，步骤为：主计算机通过轨迹规划计算，得到两个电机输出轴的期望位置；通过编码器接口电路读取两个电机输出轴的实际位置；进行PID计算得到单轴位置闭环控制量输出给电机驱动器；通过两个电机输出轴的实际位置计算得到两个电机输出轴的实际速度；根据两个电机输出轴的实际速度分别求两个电机输出轴的速度差和位置差；将上述速度差和位置差输入交叉耦合控制器进行处理，得到交叉耦合控制量；将交叉耦合控制量与单轴位置闭环控制量相加或减，然后输出给电机驱动器，实现两轴的交叉耦合同步控制。本发明能够在机械手运行过程中提高手臂同步性，避免硅片损坏，能够对机械手不同步性进行补偿，提高同步性。

主权项

一种硅片传输机械手双电机同步控制方法，其特征在于包括主计算机、DSP电机同步控制卡、电机驱动器以及电机，其中主计算机作为主机，DSP电机同步控制卡作为从机，二者进行通信连接，DSP电机同步控制卡的两输出端分别与两个电机驱动器相连，每个电机驱动器的控制输出端分别接有一个电机，电机转速信息通过电机驱动器送至DSP电机同步控制卡，交叉耦合控制器为所述DSP电机同步控制卡的一部分，用以实现交叉耦合控制算法；所述DSP电机同步控制卡具有DSP数字信号处理器，通过CAN接口电路与主计算机进行通信，通过存储器保存程序运行过程中的数据，通过模拟量输出电路与电机驱动器相连，通过编码器接口电路接收电机驱动器返回的码盘数据；主计算机通过轨迹规划计算，得到两个电机输出轴的期望位置；通过编码器接口电路读取两个电机输出轴的实际位置；通过期望位置与实际位置求差值进行PID计算得到单轴位置闭环控制量输出给电机驱动器，实现单轴位置闭环控制；通过两个电机输出轴的实际位置计算得到两个电机输出轴的实际速度；根据两个电机输出轴的实际速度分别求两个电机输出轴的速度差和位置差；将上述速度差和位置差输入交叉耦合控制器进行处理，得到交叉耦合控制量；将交叉耦合控制量与单轴位置闭环控制量相加或减，然后输出给电机驱动器，实现两轴的交叉耦合同步控制。