一种机器人关节电机同步控制系统及其方法
【专利摘要】本发明涉及一种同步控制系统，尤其涉及一种机器人关节电机同步控制系统及其方法，属于工业自动化控制领域。包括嵌入式微处理器、伺服驱动器、EtherCAT总线和EtherCAT从站控制器。一种机器人关节电机同步控制系统及其方法大幅度提升了各从站之间的同步性能，同步精度达到ns级，使得机器人各关节电机能够实现高度同步的运动，极大地提高了机器人末端运动轨迹精度。
【专利说明】
一种机器人关节电机同步控制系统及其方法
技术领域
[0001]本发明涉及一种同步控制系统，尤其涉及一种机器人关节电机同步控制系统及其方法，属于工业自动化控制领域。
【背景技术】
[0002]随着“机器换人”战略的推进，越来越多的自动化生产线集成了工业机器人取代人力操作。工业机器人的应用领域越来越多，对控制精度的要求也越来越高。工业机器人的末端位姿是由多关机协同运动复合产生，其各关节电机的同步控制精度是影响末端运动轨迹精度的关键因素。
[0003]由于各个关节电机的驱动器都有独立运行的本地时钟，系统启动时，各个本地时钟之间存在差值;在运行过程中，由于各个驱动器的本地时钟使用独立的时钟源，它们的计时周期存在漂移，这又导致时钟运行不同步而产生时钟漂移;通信数据在各个关节电机的驱动器之间、控制器和驱动器之间传输时也会产生一定的延迟。以上因素综合影响，使各关节电机运动不同步，最终造成机器人末端运动轨迹产生较大误差。
【发明内容】
[0004]本发明主要是解决现有技术中存在的不足，提供一种结构紧凑，使机器人末端运动轨迹产生误差减少，进一步提升机器操作精度的一种机器人关节电机同步控制系统及其方法。
[0005]本发明的上述技术问题主要是通过下述技术方案得以解决的:
一种机器人关节电机同步控制系统，包括嵌入式微处理器、伺服驱动器、EtherCAT总线和EtherCAT从站控制器；
嵌入式微处理器中设有以太网控制器，通过EtherCAT总线与EtherCAT从站控制器相连接，EtherCAT从站控制器与伺服驱动器相连接。
[0006]根据权利要求1所述的机器人关节电机同步控制系统，其特征在于:
与嵌入式微处理器连接的第一个EtherCAT从站作为参考时钟从站；
所述EtherCAT从站控制器含4个数据收发端口，每个端口的接收时刻存储在4个不同的寄存器，端口 0-4寄存器中分别保存了数据帧到达端口 0-4时的本地时钟时刻；
所述EtherCAT从站控制器含本地系统时间寄存器，存放每个数据帧到达时的本地系统时间；
所述EtherCAT从站控制器含时间偏移寄存器，存放本地时间和系统时间的偏差值；所述EtherCAT从站控制器含传输延时寄存器，存放参考时钟从站与本从站之间的传输延时值；
所述EtherCAT从站控制器含系统时间差寄存器，存放本地系统时间副本与参考时钟系统时间的差值;本地系统时间副本指本地时间与初始偏移量之差；
嵌入式微处理器读取各从站各端口寄存器的时间值，计算传从站η的输延时并写入相应从站的传输延时寄存器，计算从站η时钟与参考时钟之间的初始偏移量并写入相应从站的时间偏移寄存器；
从站控制器使用自己的本地时间和初始偏移量计算它的本地时间副本，这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步；
嵌入式微处理器读取参考时钟的系统时间寄存器数值并将读取到结果写入到后续所有从站的本地系统时间寄存器;各个从站控制器锁存数据帧到达时的本地时钟时刻，并利用收到的参考时钟时间计算本地时钟漂移量，依据时钟漂移量的正负判断本地时钟相比参考时钟运行的快慢，进而调整本地时钟的运行速度来实时补偿动态时钟漂移。
[0007]应用于权利要求1或2所述的一种机器人关节电机同步控制系统的方法，其特征在于按以下步骤进行:
(1)、嵌入式微处理器读取参考时钟从站端口O寄存器记录的数据帧到达端口 O时刻的参考时钟时间tl ；
(2)、嵌入式微处理器读取从站η端口O寄存器记录的数据帧到达从站η端口 O时刻的本地时钟时间t2_n;
(3)、嵌入式微处理器读取从站η端口I寄存器记录的数据帧返回从站η端口 I时刻的本地时钟时间t3_n;
(4)、嵌入式微处理器读取参考时钟从站端口I寄存器记录的数据帧到达端口 O时刻的参考时钟时间t4;
(5)、嵌入式微处理器计算传输延时:
Tdelay_n=(Tref_n - Tn_ref)/2=[(t2_n - tl) - (t4 - t3_n)]/2；
(6)、嵌入式微处理器计算从站n时钟与参考时钟之间的初始偏移量:
Toffset_n=t2_n - tl - Tdelay；
(7)、除参考时钟从站之外的每个从站均实施以上步骤；
(8)、嵌入式微处理器将以上计算得到的传输延时Tdelay_n写入对应的从站η传输延时寄存器；
(9)、嵌入式微处理器将以上计算得到的初始偏移量1'#&的_11写入对应的从站η时间偏移寄存器；
从站控制器使用自己的本地时间t_l oca 1_11和初始偏移量计算它的本地时间副本:
t_sys_local_n=t_local_n - Toffset_n
这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步；
(10)、多次重复以上步骤，使各从站时钟同步；
(11)、嵌入式微处理器使用顺序读写命令读取参考时钟的系统时间寄存器数值〖^#，并将读取到的参考时钟时间写入到后续所有从站的本地系统时间寄存器；
(12)、各个从站控制器锁存数据帧到达时的本地时钟时刻t_loca l_n;
(13)、各个从站控制器计算本地时钟漂移量:
t_drift_n=t_local_n - Toffset_n - Tdelay_n - t_ref；
(14)、如果t_drift_n不为零，则从站控制器调整本地时钟的运行速度;如1:_(11^;1^_11为正，本地时间每10]18增加9个单位;如1:_(11^;1^_11为负，本地时间每10118增加11个单位；
(15),多次重复以上步骤(11)-(14)，以实时补偿动态时钟漂移；
(16)、嵌入式微处理器读取各从站系统时间差寄存器数值，判断时钟同步性是否满足需求。
[0008]因此，本发明的一种机器人关节电机同步控制系统及其方法，大幅度提升了各从站之间的同步性能，同步精度达到ns级，使得机器人各关节电机能够实现高度同步的运动，极大地提高了机器人末端运动轨迹精度。
【附图说明】
[0009]图1是本发明的组成结构示意图；
图2是本发明的传输延时和时钟初始偏移量测量原理图。
【具体实施方式】
[0010]下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
[0011]实施例1:如图1和图2所示，一种机器人关节电机同步控制系统，包括嵌入式微处理器、伺服驱动器、EtherCAT总线和EtherCAT从站控制器；
嵌入式微处理器中设有以太网控制器，通过EtherCAT总线与EtherCAT从站控制器相连接，EtherCAT从站控制器与伺服驱动器相连接。
[0012]与嵌入式微处理器连接的第一个EtherCAT从站作为参考时钟从站；
所述EtherCAT从站控制器含4个数据收发端口，每个端口的接收时刻存储在4个不同的寄存器，端口 0-4寄存器中分别保存了数据帧到达端口 0-4时的本地时钟时刻；
所述EtherCAT从站控制器含本地系统时间寄存器，存放每个数据帧到达时的本地系统时间；
所述EtherCAT从站控制器含时间偏移寄存器，存放本地时间和系统时间的偏差值；所述EtherCAT从站控制器含传输延时寄存器，存放参考时钟从站与本从站之间的传输延时值；
所述EtherCAT从站控制器含系统时间差寄存器，存放本地系统时间副本与参考时钟系统时间的差值;本地系统时间副本指本地时间与初始偏移量之差；
嵌入式微处理器读取各从站各端口寄存器的时间值，计算传从站η的输延时并写入相应从站的传输延时寄存器，计算从站η时钟与参考时钟之间的初始偏移量并写入相应从站的时间偏移寄存器；
从站控制器使用自己的本地时间和初始偏移量计算它的本地时间副本，这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步；
嵌入式微处理器读取参考时钟的系统时间寄存器数值并将读取到结果写入到后续所有从站的本地系统时间寄存器;各个从站控制器锁存数据帧到达时的本地时钟时刻，并利用收到的参考时钟时间计算本地时钟漂移量，依据时钟漂移量的正负判断本地时钟相比参考时钟运行的快慢，进而调整本地时钟的运行速度来实时补偿动态时钟漂移。
[0013]一种机器人关节电机同步控制系统的方法，按以下步骤进行: (1)、嵌入式微处理器读取参考时钟从站端口O寄存器记录的数据帧到达端口 O时刻的参考时钟时间tl ；
(2)、嵌入式微处理器读取从站η端口O寄存器记录的数据帧到达从站η端口 O时刻的本地时钟时间t2_n;
(3)、嵌入式微处理器读取从站η端口I寄存器记录的数据帧返回从站η端口 I时刻的本地时钟时间t3_n;
(4)、嵌入式微处理器读取参考时钟从站端口I寄存器记录的数据帧到达端口 O时刻的参考时钟时间t4;
(5)、嵌入式微处理器计算传输延时:
Tdelay_n=(Tref_n - Tn_ref)/2=[(t2_n - tl) - (t4 - t3_n)]/2；
(6)、嵌入式微处理器计算从站n时钟与参考时钟之间的初始偏移量:
Toffset_n=t2_n - tl - Tdelay；
(7)、除参考时钟从站之外的每个从站均实施以上步骤；
(8)、嵌入式微处理器将以上计算得到的传输延时Tdelay_n写入对应的从站η传输延时寄存器；
(9)、嵌入式微处理器将以上计算得到的初始偏移量1'#&的_11写入对应的从站η时间偏移寄存器；
从站控制器使用自己的本地时间t_l oca 1_11和初始偏移量计算它的本地时间副本:
t_sys_local_n=t_local_n - Toffset_n
这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步；
(10)、多次重复以上步骤，使各从站时钟同步；
(11)、嵌入式微处理器使用顺序读写命令读取参考时钟的系统时间寄存器数值〖^#，并将读取到的参考时钟时间写入到后续所有从站的本地系统时间寄存器；
(12)、各个从站控制器锁存数据帧到达时的本地时钟时刻t_loca l_n;
(13)、各个从站控制器计算本地时钟漂移量:
t_drift_n=t_local_n - Toffset_n - Tdelay_n - t_ref；
(14)、如果t_drift_n不为零，则从站控制器调整本地时钟的运行速度;如1:_(11^;1^_11为正，本地时间每10]18增加9个单位;如1:_(11^;1^_11为负，本地时间每10118增加11个单位；
(15),多次重复以上步骤(11)_(14)，以实时补偿动态时钟漂移；
(16)、嵌入式微处理器读取各从站系统时间差寄存器数值，判断时钟同步性是否满足需求。
【主权项】
1.一种机器人关节电机同步控制系统，其特征在于:包括嵌入式微处理器、伺服驱动器、EtherCAT总线和EtherCAT从站控制器； 嵌入式微处理器中设有以太网控制器，通过EtherCAT总线与EtherCAT从站控制器相连接，EtherCAT从站控制器与伺服驱动器相连接。2.根据权利要求1所述的机器人关节电机同步控制系统，其特征在于: 与嵌入式微处理器连接的第一个EtherCAT从站作为参考时钟从站； 所述EtherCAT从站控制器含4个数据收发端口，每个端口的接收时刻存储在4个不同的寄存器，端口 0-4寄存器中分别保存了数据帧到达端口 0-4时的本地时钟时刻； 所述EtherCAT从站控制器含本地系统时间寄存器，存放每个数据帧到达时的本地系统时间； 所述EtherCAT从站控制器含时间偏移寄存器，存放本地时间和系统时间的偏差值； 所述EtherCAT从站控制器含传输延时寄存器，存放参考时钟从站与本从站之间的传输延时值； 所述EtherCAT从站控制器含系统时间差寄存器，存放本地系统时间副本与参考时钟系统时间的差值;本地系统时间副本指本地时间与初始偏移量之差； 嵌入式微处理器读取各从站各端口寄存器的时间值，计算传从站η的输延时并写入相应从站的传输延时寄存器，计算从站η时钟与参考时钟之间的初始偏移量并写入相应从站的时间偏移寄存器； 从站控制器使用自己的本地时间和初始偏移量计算它的本地时间副本，这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步； 嵌入式微处理器读取参考时钟的系统时间寄存器数值并将读取到结果写入到后续所有从站的本地系统时间寄存器;各个从站控制器锁存数据帧到达时的本地时钟时刻，并利用收到的参考时钟时间计算本地时钟漂移量，依据时钟漂移量的正负判断本地时钟相比参考时钟运行的快慢，进而调整本地时钟的运行速度来实时补偿动态时钟漂移。3.应用于权利要求1或2所述的一种机器人关节电机同步控制系统的方法，其特征在于按以下步骤进行: (1)、嵌入式微处理器读取参考时钟从站端口O寄存器记录的数据帧到达端口 O时刻的参考时钟时间tl ； (2)、嵌入式微处理器读取从站η端口O寄存器记录的数据帧到达从站η端口 O时刻的本地时钟时间t2_n; (3)、嵌入式微处理器读取从站η端口I寄存器记录的数据帧返回从站η端口 I时刻的本地时钟时间t3_n; (4)、嵌入式微处理器读取参考时钟从站端口I寄存器记录的数据帧到达端口O时刻的参考时钟时间t4; (5 )、嵌入式微处理器计算传输延时: Tdelay_n=(Tref_n - Tn_ref)/2=[(t2_n - tl) - (t4 - t3_n)]/2； (6)、嵌入式微处理器计算从站n时钟与参考时钟之间的初始偏移量: Toffset_n=t2_n - tl - Tdelay； (7)、除参考时钟从站之外的每个从站均实施以上步骤； (8)、嵌入式微处理器将以上计算得到的传输延时1(^1&7_11写入对应的从站η传输延时寄存器； (9)、嵌入式微处理器将以上计算得到的初始偏移量1'#&的_11写入对应的从站η时间偏移寄存器； 从站控制器使用自己的本地时间t_l ocal_n和初始偏移量计算它的本地时间副本: t_sys_local_n=t_local_n - Toffset—η 这个时间用来产生同步信号和锁存信号时间标记，使从站可以在不改变自由运行的本地时钟的情况下实现时钟同步； (10)、多次重复以上步骤，使各从站时钟同步； (11)、嵌入式微处理器使用顺序读写命令读取参考时钟的系统时间寄存器数值〖^#，并将读取到的参考时钟时间写入到后续所有从站的本地系统时间寄存器； (12)、各个从站控制器锁存数据帧到达时的本地时钟时刻t_local_n; (13)、各个从站控制器计算本地时钟漂移量: t_drift_n=t_local_n - Toffset_n - Tdelay_n - t_ref； (14)、如果t_drift_n不为零，则从站控制器调整本地时钟的运行速度;如t_drift_nS正，本地时间每10]18增加9个单位;如1:_(11^;1^_11为负，本地时间每10118增加11个单位； (15)、多次重复以上步骤(11)-(14)，以实时补偿动态时钟漂移； (16)、嵌入式微处理器读取各从站系统时间差寄存器数值，判断时钟同步性是否满足需求。
【文档编号】G05B19/418GK105892430SQ201610282992
【公开日】2016年8月24日
【申请日】2016年5月3日
【发明人】李正刚, 徐进荣, 陈立, 金晶, 刘翠苹
【申请人】杭州新松机器人自动化有限公司