[发明专利]一种机器人关节电机同步控制系统及其方法

摘要

本发明涉及一种同步控制系统，尤其涉及一种机器人关节电机同步控制系统及其方法，属于工业自动化控制领域。包括嵌入式微处理器、伺服驱动器、EtherCAT总线和EtherCAT从站控制器。一种机器人关节电机同步控制系统及其方法大幅度提升了各从站之间的同步性能，同步精度达到ns级，使得机器人各关节电机能够实现高度同步的运动，极大地提高了机器人末端运动轨迹精度。

主权项

1.一种应用于机器人关节电机同步控制系统的方法，其特征在于机器人关节电机同步控制系统包括嵌入式微处理器、伺服驱动器、EtherCAT总线和EtherCAT从站控制器；嵌入式微处理器中设有以太网控制器，通过EtherCAT总线与EtherCAT从站控制器相连接，EtherCAT从站控制器与伺服驱动器相连接；与嵌入式微处理器连接的第一个EtherCAT从站作为参考时钟从站；所述EtherCAT从站控制器含4个数据收发端口，每个端口的接收时刻存储在4个不同的寄存器，端口0‑4寄存器中分别保存了数据帧到达端口0‑4时的本地时钟时刻；所述EtherCAT从站控制器含本地系统时间寄存器，存放每个数据帧到达时的本地系统时间；所述EtherCAT从站控制器含时间偏移寄存器，存放本地时间和系统时间的偏差值；所述EtherCAT从站控制器含传输延时寄存器，存放参考时钟从站与本从站之间的传输延时值；所述EtherCAT从站控制器含系统时间差寄存器，存放本地系统时间副本与参考时钟系统时间的差值；本地系统时间副本指本地时间与初始偏移量之差；应用于机器人关节电机同步控制系统的方法，按以下步骤进行：(1)、嵌入式微处理器读取参考时钟从站端口0寄存器记录的数据帧到达端口0时刻的参考时钟时间t1；(2)、嵌入式微处理器读取从站n端口0寄存器记录的数据帧到达从站n端口0时刻的本地时钟时间t2_n；(3)、嵌入式微处理器读取从站n端口1寄存器记录的数据帧返回从站n端口1时刻的本地时钟时间t3_n；(4)、嵌入式微处理器读取参考时钟从站端口1寄存器记录的数据帧到达端口0时刻的参考时钟时间t4；(5)、嵌入式微处理器计算传输延时：Tdelay_n＝(Tref_n‑Tn_ref)/2＝[(t2_n‑t1)‑(t4‑t3_n)]/2；(6)、嵌入式微处理器计算从站n时钟与参考时钟之间的初始偏移量：Toffset_n＝t2_n‑t1‑Tdelay_n；(7)、除参考时钟从站之外的每个从站均实施以上步骤；(8)、嵌入式微处理器将以上计算得到的传输延时Tdelay_n写入对应的从站n传输延时寄存器；(9)、嵌入式微处理器将以上计算得到的初始偏移量Toffset_n写入对应的从站n时间偏移寄存器；从站控制器使用自己的本地时间t_local_n和初始偏移量计算它的本地时间副本：t_sys_local_n＝t_local_n‑Toffset_n这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步；(10)、多次重复以上步骤，使各从站时钟同步；(11)、嵌入式微处理器使用顺序读写命令读取参考时钟的系统时间寄存器数值t_ref，并将读取到的参考时钟时间写入到后续所有从站的本地系统时间寄存器；(12)、各个从站控制器锁存数据帧到达时的本地时钟时刻t_local_n；(13)、各个从站控制器计算本地时钟漂移量：t_drift_n＝t_local_n‑Toffset_n‑Tdelay_n‑t_ref；(14)、如果t_drift_n不为零，则从站控制器调整本地时钟的运行速度；如t_drift_n为正，本地时间每10ns增加9个单位；如t_drift_n为负，本地时间每10ns增加11个单位；(15)、多次重复以上步骤(11)‑(14)，以实时补偿动态时钟漂移；(16)、嵌入式微处理器读取各从站系统时间差寄存器数值，判断时钟同步性是否满足需求。