一种基于CANopen的多机械臂同步跟随控制方法

1.一种基于CANopen的多机械臂同步跟随控制方法，该多机械臂至少包括一个主机械臂和一个从机械臂，其特征在于：包括以下步骤：
S1.结合对象字典，基于PDO协议对多机械臂进行PDO配置；
S2.分别设置主、从机械臂的运动模式以及各关节电机的运动模式；包括：
对主机械臂运动模式、关节电机运动模式均无要求；
对从机械臂运动模式设置为绝对跟随或相对跟随，关节电机运动模式设置为力矩控制模式，其中
绝对跟随：从机械臂的各关节运动参考零位B与主机械臂各关节参考零位BM相同，B＝BM；
相对跟随：从机械臂的各关节运动参考零位B为从机械臂各关节当前位置；
S3.获取主、从机械臂运动状态，以及各关节反馈数据，设定多机械臂各关节参考位置；
S4.根据主、从机械臂反馈量计算得到从机械臂各关节跟随力矩控制量；
S5.对从机械臂各关节电机发送控制指令，一个同步跟随控制周期结束，重复步骤S1-S5，完成对主机械臂的同步跟随控制。
2.根据权利要求1所述的一种基于CANopen的多机械臂同步跟随控制方法，其特征是所述步骤S1中进行PDO配置过程包括：
对主机械臂各关节电机伺服TPDO设置至少包含状态字、实际位置、实际速度对象映射，对主机械臂各关节电机伺服RPDO无要求；
对从机械臂各关节电机伺服TPDO设置至少包含状态字、实际位置、实际速度对象映射，对从机械臂各关节电机伺服RPDO设置至少包括控制字、力矩、速度上限、速度下限对象映射。
3.根据权利要求1所述的一种基于CANopen的多机械臂同步跟随控制方法，其特征是所述步骤S3中设置多机械臂各关节参考位置的具体内容包括：
若从机械臂运动模式为绝对跟随模式，设定从机械臂的各关节运动参考零位B为主机械臂各关节参考零位BM，
若从机械臂运动模式为相对跟随模式，设定从从机械臂的各关节运动参考零位B为从机械臂各关节的当前位置。
4.根据权利要求1或2所述的一种基于CANopen的多机械臂同步跟随控制方法，其特征是所述步骤S4中从机械臂各关节跟随力矩控制量计算包括以下过程：
主机械臂开始运动，通过主、从机械臂TPDO中各关节实际位置反馈数据xM、x，将主机械臂的相对运动量作为从机械臂的期望相对轨迹，计算得到从机械臂在相对跟随模式下的位置同步误差e＝x-B-(xM-BM)，在绝对跟随模式下的位置同步误差e＝x-xM；
结合主、从机械臂TPDO中各个关节速度反馈数据vM、v，将主机械臂的实际速度作为从机械臂的期望速度轨迹，计算相应各关节速度同步误差eV＝v-vM；
计算得到从机械臂各个关节的力矩控制量τ＝KDeV+KPe+KJ(vM+sgn(e))，其中KD、KP、KJ均为正常数，sgn为符号函数。
5.根据权利要求1或2所述的一种基于CANopen的多机械臂同步跟随控制方法，其特征是所述步骤S5中对从机械臂各关节电机发送控制指令的过程包括：
根据获得的从机械臂各个关节的同步跟随力矩控制量，转换后通过从机械臂RPDO力矩对象下发给各关节伺服。

一种基于CANopen的多机械臂同步跟随控制方法\n技术领域\n[0001] 本发明涉及一种机械臂控制技术领域，尤其是涉及一种基于CANopen的多机械臂同步跟随控制方法。\n背景技术\n[0002] CANopen是一种架构在控制局域网路(Controller Area Network,CAN)上的高层通讯协定，包括通讯子协定及设备子协定常在嵌入式系统中使用，也是工业控制常用到的一种现场总线。CANopen设备都需要具备对象字典，用来设定设备组态及进行非即时的通讯。通过CANopen协议的支持，能够对不同电机伺服驱动、不同厂商、不同型号的机械臂通过总线进行配置。\n[0003] 多机械臂同步跟随运动能够实现远距离、大数量机械臂的同步跟踪控制，对于机械臂的统一化控制与高精度协同场合应用有较大意义。\n[0004] 现有技术中实现多机械臂同步跟随运动控制相关方法基本没有，一般机械臂关节采用采用的CANopen位置控制模式下的PTP方法难以实现实时的同步跟随，难以建立多机械臂之间的同步关系，因此如何基于CANopen实现多机械臂同步跟随运动是有待解决的问题。\n发明内容\n[0005] 本发明主要是解决现有技术中一般控制方法难以建立多机械臂之间同步关系的问题，提供了一种基于CANopen的多机械臂同步跟随控制方法。\n[0006] 本发明的上述技术问题主要是通过下述技术方案得以解决的：一种基于CANopen的多机械臂同步跟随控制方法，该多机械臂至少包括一个主机械臂和一个从机械臂，包括以下步骤：\n[0007] S1.结合对象字典，基于PDO协议对多机械臂进行PDO配置；\n[0008] CANopen设备均具备对象字典，用于进行通讯，通讯可以基于多种协议。\n[0009] S2.分别设置主、从机械臂的运动模式以及各关节电机的运动模式；\n[0010] S3.获取主、从机械臂运动状态，以及各关节反馈数据，设定多机械臂各关节参考位置；\n[0011] S4.根据主、从机械臂反馈量计算得到从机械臂各关节跟随力矩控制量；\n[0012] S5.对从机械臂各关节电机发送控制指令，一个同步跟随控制周期结束，重复步骤S1-S5，完成对主机械臂的同步跟随控制。本方法基于CANopen技术来实现对多机械臂同步跟随控制，适用于多种总线形式下支持CANopen协议的电机伺服多机械臂系统，包括所有的CAN与EtherCAT等总线形式，并且不局限总线形式，也适用于多品牌多种机械臂。基于本方法的系统结构简单、易于实现，且对于主机械臂关节电机控制模式没有限制要求，主机械臂可以是位置控制、速度控制或者力矩控制中的一种，本方法均能实现从机械臂的同步跟随力矩控制。\n[0013] 作为一种优选方案，所述步骤S1中进行PDO配置过程包括：\n[0014] 对主机械臂各关节电机伺服TPDO设置至少包含状态字、实际位置、实际速度对象映射，对主机械臂各关节电机伺服RPDO无要求；\n[0015] 对从机械臂各关节电机伺服TPDO设置至少包含状态字、实际位置、实际速度对象映射，对从机械臂各关节电机伺服RPDO设置至少包括控制字、力矩、速度上限、速度下限对象映射。PDO协议中包括传送用的TPDO以及接收用的RPDO。RPDO中需要设置各关节的速度上下限，防止加速度过大保证系统安全。\n[0016] 作为一种优选方案，所述步骤S2中设置主、从机械臂的运动模式以及各关节电机的运动模式包括以下内容：\n[0017] 对主机械臂运动模式、关节电机运动模式均无要求；\n[0018] 对从机械臂运动模式设置为绝对跟随或相对跟随，关节电机运动模式设置为力矩控制模式，其中\n[0019] 绝对跟随：从机械臂的各关节运动参考零位B与主机械臂各关节参考零位BM相同，B＝BM；\n[0020] 相对跟随：从机械臂的各关节运动参考零位B为从机械臂各关节当前位置。\n[0021] 作为一种优选方案，所述步骤S3中设置多机械臂各关节参考位置的具体内容包括：\n[0022] 若从机械臂运动模式为绝对跟随模式，设定从机械臂的各关节运动参考零位B为主机械臂各关节参考零位BM，\n[0023] 若从机械臂运动模式为相对跟随模式，设定从从机械臂的各关节运动参考零位B为从机械臂各关节的当前位置。\n[0024] 作为一种优选方案，所述步骤S4中从机械臂各关节跟随力矩控制量计算包括以下过程：\n[0025] 主机械臂开始运动，通过主、从机械臂TPDO中各关节实际位置反馈数据xM、x，将主机械臂的相对运动量作为从机械臂的期望相对轨迹，计算得到从机械臂在相对跟随模式下的位置同步误差e＝x-B-(xM-BM)，在绝对跟随模式下的位置同步误差e＝x-xM；\n[0026] xM为主机械臂实际位置数据，x为从机械臂实际位置数据，BM为主机械臂各关节参考零位，B为从机械臂的各关节运动参考零位。\n[0027] 结合主、从机械臂TPDO中各个关节速度反馈数据vM、v，将主机械臂的实际速度作为从机械臂的期望速度轨迹，计算相应各关节速度同步误差eV＝v-vM；\n[0028] 计算得到从机械臂各个关节的力矩控制量τ＝KDeV+KPe+KJ(vM+sgn(e))，其中KD、KP、KJ均为正常数，sgn为符号函数。\n[0029] 作为一种优选方案，所述步骤S5中对从机械臂各关节电机发送控制指令的过程包括：\n[0030] 根据获得的从机械臂各个关节的同步跟随力矩控制量，转换后通过从机械臂RPDO力矩对象下发给各关节伺服。\n[0031] 因此，本发明的优点是：1.基于CANopen技术来实现对多机械臂同步跟随控制，适用于多种总线形式下支持CANopen协议的电机伺服多机械臂系统，包括所有的CAN与EtherCAT等总线形式，并且不局限总线形式，也适用于多品牌多种机械臂。2.基于本方法的系统结构简单、易于实现。3.对于主机械臂关节电机控制模式没有限制要求，主机械臂可以是位置控制、速度控制或者力矩控制中的一种，本方法均能实现从机械臂的同步跟随力矩控制。\n附图说明\n[0032] 附图1是本发明中主电路的一种流程示意图；\n[0033] 附图2是本发明中主、从机械臂测试关节的位置实际反馈图。\n具体实施方式\n[0034] 下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。\n[0035] 实施例：\n[0036] 本实施例一种基于CANopen的多机械臂同步跟随控制方法，该多机械臂至少包括一个主机械臂和一个从机械臂，本实施例中以2个同型号的6个自由度机械臂的第6关节为测试对象，任选其一为主机械臂，另一为从机械臂。如图1所示，方法包括以下步骤：\n[0037] S1.结合对象字典，基于PDO协议对多机械臂进行PDO配置；\n[0038] 对主机械臂各关节电机伺服TPDO设置至少包含状态字(0x6041)、实际位置(0x6064)、实际速度(0x606C)对象映射，对主机械臂各关节电机伺服RPDO无要求；\n[0039] 对从机械臂各关节电机伺服TPDO设置至少包含状态字、实际位置、实际速度对象映射，对从机械臂各关节电机伺服RPDO设置至少包括控制字(0x6040)、力矩(0x6071)、速度上下限(0x2946)对象映射。在此对主、从机械臂关节进行相同PDO配置如下表，[0040]\n[0041]\n[0042] S2.分别设置主、从机械臂的运动模式以及各关节电机的运动模式；\n[0043] 考虑常用运行模式为位置控制模式，实现容易，为突出对主机械臂各关节的运动模式没有特殊要求，结合上述PDO设置，选用较少使用的力矩控制模式进行测试，设置主机械臂测试关节电机运动模式均为力矩控制模式；\n[0044] 对从机械臂运动模式设置为相对跟随，关节电机运动模式设置为力矩控制模式；\n[0045] 相对跟随：从机械臂的各关节运动参考零位B为从机械臂各关节当前位置。\n[0046] S3.获取主、从机械臂运动状态，以及各关节反馈数据，设定多机械臂各关节参考位置；\n[0047] 设置主机械臂测试关节电机的目标力矩为0，由于设定为相对跟随模式，多机械臂初始化过程中，设定从机械臂的各关节运动参考零位B为从机械臂各关节的当前位置。\n[0048] S4.根据主、从机械臂反馈量计算得到从机械臂各关节跟随力矩控制量；\n[0049] 主机械臂开始运动，通过主、从机械臂TPDO中各关节实际位置(0x6064)反馈数据xM、x，将主机械臂的相对运动量作为从机械臂的期望相对轨迹，计算得到从机械臂在相对跟随模式下的位置同步误差e＝x-B-(xM-BM)，在绝对跟随模式下的位置同步误差e＝x-xM；\nxM为主机械臂实际位置数据，x为从机械臂实际位置数据，BM为主机械臂各关节参考零位，B为从机械臂的各关节运动参考零位。\n[0050] 结合主、从机械臂TPDO中各个关节速度反馈数据vM、v，将主机械臂的实际熟读作为从机械臂的期望速度轨迹，计算相应各关节速度同步误差eV＝v-vM；\n[0051] 计算得到从机械臂各个关节的力矩控制量τ＝KDeV+KPe+KJ(vM+sgn(e))，其中KD、KP、KJ分别设置为0.00001、0.001、0.00001，sgn为符号函数。\n[0052] S5.对从机械臂各关节电机发送控制指令，一个同步跟随控制周期结束，重复步骤S1-S5，完成对主机械臂的同步跟随控制。\n[0053] 根据获得的从机械臂各个关节的同步跟随力矩控制量，转换后通过从机械臂RPDO力矩对象下发给各关节伺服，RPDO中设置各关节的速度上下限，防止速度过大保证系统安全。步骤S1-S5一个同步跟随控制周期结束，循环进行直至完成对主机械臂的同步跟随控制。\n[0054] 如图2所示，为主、从机械臂测试关节的位置实际反馈，其中横轴、竖轴分别对应周期数和关机实际反馈角度，下方曲线对应主机械臂，上方曲线对应从机械臂，可见本方法的有效性。\n[0055] 本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。\n[0056] 尽管本文较多地使用了主机械臂、从机械臂、关节电机等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。