CN104290096A - 一种基于CANopen的机械臂关节电机控制方法及系统 - Google Patents

Abstract

本发明公开了一种基于CANopen的机械臂关节电机控制方法及系统，该方法包括以下几个步骤：步骤1：将各机械臂关节电机的驱动器连接于CAN总线上，CAN总线进入预操作状态；步骤2：基于PDO协议对电机进行PDO配置；步骤3：选择电机运行模式；步骤4：对关节运动轨迹采用高阶多项式插值得到控制量；步骤5：对电机发送控制指令，完成对机械臂关节电机的控制；该方法适用于所有CAN总线下支持CANopen协议的电机伺服控制系统，由于现场总线的引入，省去了大量的数据线和接口，使得系统调试和维护更加的简便。基于该方法的控制系统结构简单、易于实现，通过CAN网络对电机控制，实现了关节电机位置环和速度环控制。

Description

一种基于CANopen的机械臂关节电机控制方法及系统

技术领域

本发明涉及伺服电机控制技术，特别涉及一种基于CANopen的机械臂关节电机控制方法及系统。

背景技术

CAN是Controller Area Network的缩写，是ISO国际标准化的串行通信协议，广泛应用于汽车产业。从ISO网络模型的角度来看同，CAN只定义物理层和数据链路层，没有规定应用层，本身并不完整，需要一个高层协议来定义CAN报文中的11/29位标识符、8字节数据的使用。CANopen协议是CAN-in-Automation(CiA)定义的标准之一，并且在发布后不久就获得了广泛的承认，尤其是在欧洲，CANopen协议被认为是在基于CAN的工业系统中占领导地位的标准。大多数重要的设备对象，例如数字和模拟的输入输出模块、驱动设备、操作设备、控制器、可编程控制器或编码器，都在被称为“设备描述”的协议中进行描述。“设备描述”定义了不同类型的标准设备及其相应的功能，依靠CANopen协议的支持，可以对不同厂商的设备通过总线进行配置。

CiA Draft Standard 301和CiA Draft Standard Proposal 402分别为CANopen协议中的通讯子协议和面向运动控制的设备子协议。CiA Draft Standard 301中主要是对通讯对象的描述，CAN总线的管理和PDO的配置都要使用到其中的对象字典；CiA Draft Standard Proposal 402描述了运动控制设备对象，比如电机运动的转速、编码器位置值和电机转矩等对象，这些对象以对象字典的形式存放，只要修改对象字典的值就可以实现对电机的控制。

然而，现有技术中机械臂的PTP运动控制复杂，因此如何基于CANopen协议实现对机器人关节电机的控制，并且使得关节运动平滑、稳定，始终保持在关节运动容许的范围内是有待解决的问题。

发明内容

本发明提出了一种基于CANopen的机械臂关节电机控制方法及系统，通过采用通用于CANopen下的电机伺服控制方法，同时利用高阶多项式插值算法对关节轨迹插值得到每个时间点关节角度、速度、加速度等控制量，确保机械臂关节连续、平稳、无抖动的运动。

本发明采用如下技术方案：

一种基于CANopen的机械臂关节电机控制方法，包括以下几个步骤：

步骤1：将各机械臂关节电机的驱动器连接于CAN总线上，并对所有驱动器上电，CAN总线进入预操作状态；

步骤2：控制端通过CAN卡将电机控制量对应的对象字典映射到CAN总线上的每个驱动器的PDO单元，基于PDO协议进行PDO配置，并将CAN总线设为可操作状态；

所述电机控制量包括电机转速和电流，电机所在关节的目标位置、加速度、减速度、位置反馈量及速度反馈量；

步骤3：按照控制需求，利用控制端从位置模式或速度模式中选择电机运行模式；

步骤4：依照控制目标，对关节运动轨迹采用高阶多项式插值得到每个时间点关节位置、速度及加速度的控制量；

步骤5：依据步骤4得到的控制量，通过PDO通讯方式对工作在位置模式或速度模式下的电机发送控制指令，完成对机械臂关节电机的控制；

所述电机控制指令包括位置指令、速度指令以及加减速度指令。

PDO(process data object)通讯方式适用于高速数据传输，一个报文最多可以传输8个字节数据，电机运动控制中驱动器状态、电机运行速度、电机位置值、加减速度等控制量对实时性要求很高，且数据量大，将这些控制量对应的对象字典衍射到R/TPDO上可以满足实时性要求，标识符采用默认方式定义。PDO的配置是由服务数据对象SDO(service data object)通讯方式实现的，SDO通讯方式通过索引和子索引来访问设备的对象字典，数据携带量小，适合于参数设置、功能配置等工作。

基于PDO协议将电机控制的对象字典映射为下表中所示的标识符及传输方式：

表1

其中，R/TPDOx为接收或发送对象，节点id为驱动器在CAN总线上的节点标号。

在CAN总线中报文的COB-ID标识符越小，优先级越高。将电机控制字0x6040和电机状态字0x6041分别配置到第一个RPDO和第一个TPDO可以不受总线繁忙的影响无延时的控制电机运行状态和监控电机是否正常运行，防止意外事故发生，RPDO1的传输方式为同步信号传输为多轴电机同步运行提供了基础；RPDO2、RPDO4以及TPDO2充分利用了CAN报文8个字节的数据长度，提高了通讯效率；将电机实际位置、速度和电流反馈的传输方式设置为200ms的时间传输，既能满足电机控制的实时性要求，同时不会过多的占用总线资源。

PDO配置生效后，控制端可以通过标识符为0x200+节点id的CAN报文控制对应节点电机的启动和停止，通过标识符为0x300+节点id的报文来控制对应节点电机运动到指定目标位置和电机速度，通过标识符0x400+节点id的报文来控制对应节点电机在速度模式下的运行速度。同时控制端接收到来自驱动器节点以0x280+节点id为标识符的报文，解析该报文可以检测对应节点电机是否正常运行，解析以0x380+节点id为标识符的报文可以检测对应节点电机当前的电机位置和当前运行速度，解析以0x480+节点id为标志符的CAN报文监测对应节点电机当前的电流值。

利用同步信号传输驱动器实时获取机械臂关节电机的工作状态。

对各机械臂关节的轨迹P(t)采用高阶多项式插值得到每个时间点关节位置、速度及加速度：

P(t)＝α₀+α₁t+α₂t²+α₃t³+α₄t⁴+α₅t⁵

且需要满足以下约束条件：

P(0)＝P₀，P′(0)＝P′₀，P″(0)＝P″₀

P(t_d)＝P_d，P′(tx)＝P′_d，P″(t_d)＝P″_d

其中，α₀＝P₀，α₁＝P′₀， ${\mathrm{\α}}_2=\frac{P_0^{\′\′}}{2},$ ${\mathrm{\α}}_3=\frac{{20P}_d-{20P}_0-({8P}_d^{\′}+{12P}_0^{\′})t_d-({3P}_0^{\′\′}-P_d^{\′\′})t_d^2}{{2t}_d},$

${\mathrm{\α}}_4=\frac{{30P}_0-{30P}_d-({14P}_d^{\′}+{16P}_0^{\′})t_d+({3P}_0^{\′\′}-{2P}_d^{\′\′})t_d^2}{{2t}_d^4},$

${\mathrm{\α}}_5=\frac{{12P}_d-{12P}_0-({6P}_d^{\′}+{6P}_0^{\′})t_d-(P_0^{\′\′}-P_d^{\′\′})t_d^2}{{2t}_d^5};$

其中，t表示机械臂关节运动时间，t_d表示机械臂关节目标运动时间，P₀和P_d分别为机械臂关节初始位置和目标位置，P′₀和P′_d分别为机械臂关节初始位置速度和目标位置速度，P″₀和P″_d分别为机械臂关节初始位置加速度和目标位置加速度。

高阶5次多项式插值算法同样适用于多路径点的关节轨迹规划，将整个轨迹看作由多个轨迹段组成，对相邻的路径点五次多项式插值，实现整个轨迹的光滑、连续、无抖动的运动。

CANopen协议只能实现对机械臂关节电机的有效控制，为了使机械臂关节电机顺滑平稳的运动必须应用到运动轨迹规划，轨迹规划既可在关节空间又可以在笛卡尔空间进行。关节空间轨迹规划是以关节角度函数来描述机器人轨迹，并进行轨迹规划，无需在笛卡尔坐标系描述两个路径点之间的路径形状，计算简单、容易，更加满足实时性的要求。在关节空间中进行轨迹规划，是将关节变量映射成时间的函数，并规划它的一阶(关节速度)和二阶(关节加速度)时间导数，轨迹规划的主要任务是选择合理的多项式函数和其他线性函数进行插补运算，使关节运动平滑、稳定，始终保持在关节运动容许的范围内。

一种基于CANopen的机械臂关节电机控制系统，包括CAN卡、上位机及CAN总线，上位机、CAN卡及CAN总线依次相连，各机械臂关节电机的驱动器连接于CAN总线上；上位机采用所述的基于CANopen的机械臂关节电机控制的实现方法通过CAN卡对连接在CAN总线上的各机械臂关节电机的驱动器进行PDO配置，基于PDO协议和配制的控制量，对机械臂关节电机进行控制。

有益效果

本发明提供了一种基于CANopen的机械臂关节电机控制方法及系统，该方法包括以下几个步骤：步骤1：将各机械臂关节电机的驱动器连接于CAN总线上，并对所有驱动器上电，CAN总线进入预操作状态；步骤2：控制端通过CAN卡将电机控制量对应的对象字典映射到CAN总线上的每个驱动器的PDO单元，基于PDO协议进行PDO配置，并将CAN总线设为可操作状态；步骤3：按照控制需求，利用控制端从位置模式或速度模式中选择电机运行模式；步骤4：依照控制目标，对关节运动轨迹采用高阶多项式插值得到每个时间点关节位置、速度及加速度的控制量；步骤5：依据步骤4得到的控制量，通过PDO通讯方式对工作在位置模式或速度模式下的电机发送控制指令，完成对机械臂关节电机的控制；

该方法适用于所有CAN总线下支持CANopen协议的电机伺服控制系统，由于现场总线的引入，省去了大量的数据线和接口，使得系统调试和维护更加的简便。基于这种控制方法的电机伺服控制系统具有很好的可扩展性，只要在CAN节点容许范围内都可以进行扩充，不影响系统的控制性能。

在实现对电机的有效控制基础上，增加了机械臂关节空间的轨迹规划，实现了关节电机位置环和速度环控制，多电机同步控制，关节电机实时位置、实时速度、实时电流反馈。

基于该方法的控制系统结构简单、易于实现，通过CAN网络对电机控制，实现了关节电机位置环和速度环控制。

附图说明

图1为本发明所述的机械臂关节电机控制系统结构图；

图2为本发明所述的机械臂关节电机控制流程图；

图3为关节状态实时反馈图；

图4为位置模式控制图；

图5为速度模式控制图；

图6为采用5阶多项式插值分别得到关节电机位置仿真曲线、速度仿真曲线和加速度仿真曲线，其中，(a)为位置仿真曲线，(b)为速度仿真曲线，(c)为加速度仿真曲线。

具体实施方式

下面将结合附图和实施例对本发明做进一步的说明。

如图1所示，一种基于CANopen的机械臂关节电机控制系统，包括CAN卡、上位机及CAN总线，上位机、CAN卡及CAN总线依次相连，各机械臂关节电机的驱动器连接于CAN总线上；上位机作为控制端通过CAN卡对连接在CAN总线上的各机械臂关节电机的驱动器进行PDO配置，基于PDO协议和配制的控制量，对机械臂关节电机进行控制。

在电机上安装霍尔传感器和增量式光电编码器。

如图2所示，一种基于CANopen的机械臂关节电机控制方法，包括以下几个步骤：

步骤1：将各机械臂关节电机的驱动器连接于CAN总线上，并对所有驱动器上电，CAN总线进入预操作状态；

步骤2：控制端通过CAN卡将电机控制量对应的对象字典映射到CAN总线上的每个驱动器的PDO单元，基于PDO协议进行PDO配置，并将CAN总线设为可操作状态；

所述电机控制量包括电机转速和电流，电机所在关节的目标位置、加速度、减速度、位置反馈量及速度反馈量；

电机驱动器上电后都会从启动状态进入预操作状态，在预操作状态完成节点心跳协议的配置和PDO协议配置。

心跳协议的配置是总线管理的一部分，为了实时监控每一个CAN节点的健康状态，每个驱动器节点每隔200ms向主节点发送一次当前状态，对象字典为0x1017，同时为每一个心跳节点设定一个超时值10ms，当超时发生时采取关闭电机动作。

步骤3：按照控制需求，利用控制端从位置模式或速度模式中选择电机运行模式；

步骤4：依照控制目标，对关节运动轨迹采用高阶多项式插值得到每个时间点关节位置、速度及加速度的控制量；

步骤5：依据步骤4得到的控制量，通过PDO通讯方式对工作在位置模式或速度模式下的电机发送控制指令，完成对机械臂关节电机的控制；

所述电机控制指令包括位置指令、速度指令以及加减速度指令。

基于PDO协议将电机控制的对象字典映射为下表中所示的标识符及传输方式：

表1

其中，R/TPDOx为接收或发送对象，节点id为驱动器在CAN总线上的节点标号。

以RPDO2为例，其在对象字典中由0x1401h和0x1601h来描述，0x1401h描述了这个PDO采用的COB-ID标示符和传输类型，0x1601h描述了映射到该PDO中的对象列表，具体赋值参见下表。完成RPDO2衍射后，以0x300+id为标识符的报文数据段中前4个字节表示电机的目标位置，后4字节表示电机运行速度。

表2

采用PDO通讯是为了满足电机实时性控制要求，将控制字(0x6040)衍射到RPDO1，占用2个字节，同步传输方式，当驱动器接受到一个同步信号时传输控制字命令，标识符为0x200+CAN节点号。电机驱动器状态之间的转换由控制字实现，主要有ready to switch on、switch on、operation enable、quick stop active和fault等状态，电机只有在驱动器operation enable状态下才能运行；为了实时显示驱动器状态，将状态字(0x6041)衍射到TPDO1，事件传输方式，当驱动器状态字改变时传输发送PDO1，标识符为0x280+节点id。

位置模式下的目标位置(0x607a)和运行速度(0x6081)衍射到RPDO2，各占用4个字节，事件传输立即执行，标识符为0x300+节点id；速度模式下的目标速度(0x60ff)衍射到RPDO3，占用4个字节，传输方式为事件传输立即执行，标识符为0x400+节点id；电机运行加速度值(0x6083)和减速度值(0x6084)衍射到RPDO4，各占用4个字节，传输方式同样为事件传输，标识符为0x500+节点id。

实时监测每个节点的电机运行状态，每隔200ms反馈一次电机当前的运行速度、相对位置和电流值，这也需要用到PDO配置。将电机位置反馈(0x6064)和速度反馈(0x606c)衍射到TPDO2，各占4个字节，传输方式为时间传输，时间间隔设为200ms，标识符为0x380+节点id；电机电流值反馈(0x6078)衍射到TPDO3，占用4个字节，同样采用200ms为周期的时间传输，标识符是0x480+节点id。以上是整个PDO配置内容。

图3显示了在完成每个CAN节点驱动器的PDO配置后，让总线进入可操作状态，心跳协议和PDO通讯协议开始生效，每个CAN节点每隔200ms发送一次当前节点的健康状态，每个电机驱动器每隔200ms报告一次电机当前的位置、速度和电流状态。

在CAN总线中报文的COB-ID标识符越小，优先级越高。将电机控制字0x6040和电机状态字0x6041分别配置到第一个RPDO和第一个TPDO可以不受总线繁忙的影响无延时的控制电机运行状态和监控电机是否正常运行，防止意外事故发生，RPDO1的传输方式为同步信号传输为多轴电机同步运行提供了基础；RPDO2、RPDO4以及TPDO2充分利用了CAN报文8个字节的数据长度，提高了通讯效率；将电机实际位置、速度和电流反馈的传输方式设置为200ms的时间传输，既能满足电机控制的实时性要求，同时不会过多的占用总线资源。

PDO配置生效后，控制端可以通过标识符为0x200+节点id的CAN报文控制对应节点电机的启动和停止，通过标识符为0x300+节点id的报文来控制对应节点电机运动到指定目标位置和电机速度，通过标识符0x400+节点id的报文来控制对应节点电机在速度模式下的运行速度。同时控制端接收到来自驱动器节点以0x280+节点id为标识符的报文，解析该报文可以检测对应节点电机是否正常运行，解析以0x380+节点id为标识符的报文可以检测对应节点电机当前的电机位置和当前运行速度，解析以0x480+节点id为标志符的CAN报文监测对应节点电机当前的电流值。

利用同步信号传输驱动器实时获取机械臂关节电机的工作状态。

图4显示了电机在位置模式下所需的控制量及其对应的对象字典。在选择电机进入位置运行模式后，让电机驱动器进入operation enable状态，电机运行中使用最频繁的控制量目标位置(0x607a)、运行速度(0x6081)、加速度值(0x6083)和减速度值(0x6084)在PDO配置中已配置好，其他的控制量主要是限制量和电机保护量，在电机开始运行前通过SDO通讯设定好。可以直接发送标识符为0x300+id的报文来控制电机的目标位置和运行速度，标识符为0x500+id的报文来控制电机运行过程中的加速度和减速度。

图5显示了电机在速度模式下所需的控制量及其对应的对象字典。选择速度模式后让电机进入使能状态，速度模式主要的控制量有目标速度(0x60ff)、加速度值(0x6083)和减速度值(0x6084)，其他的控制量并不占用数据流，可以在电机运行前通过SDO来设定好。在电机进入速度模式使能后，直接发送标识符为0x400+id的报文来控制电机运行速度，标识符为0x500+id的报文来控制电机运行的加速度和减速度。

多电机同步运动应用于时序要求严格的多轴控制系统，给出每个需要运动的电机的控制指令后电机不会立即运行。同步信号(0x80)是以广播的信息发送的，每个CAN节点都能接收得到，接收到同步信号后多电机才开始统一运行，因为电机开始运动和停止运动是由控制字(0x6040)控制的，而控制字在PDO配置中的传输方式为同步信号传输。

CANopen协议只能实现对机械臂关节电机的有效控制，为了使机械臂关节电机顺滑平稳的运动必须应用到运动轨迹规划，轨迹规划既可在关节空间又可以在笛卡尔空间进行。关节空间轨迹规划是以关节角度函数来描述机器人轨迹，并进行轨迹规划，无需在笛卡尔坐标系描述两个路径点之间的路径形状，计算简单、容易，更加满足实时性的要求。在关节空间中进行轨迹规划，是将关节变量映射成时间的函数，并规划它的一阶(关节速度)和二阶(关节加速度)时间导数，轨迹规划的主要任务是选择合理的多项式函数和其他线性函数进行插补运算，使关节运动平滑、稳定，始终保持在关节运动容许的范围内。

对各机械臂关节的轨迹P(t)采用高阶多项式插值得到每个时间点关节位置、速度及加速度：

P(t)＝α₀+α₁t+α₂t²+α₃t³+α₄t⁴+α₅t⁵

且需要满足以下约束条件：

P(0)＝P₀，P′(0)＝P′₀，P″(0)＝P″₀

P(t_d)＝P_d，P′(t_d)＝P′_d，P″(t_d)＝P″x

其中，α0＝P0，α1＝P0′， ${\mathrm{\α}}_2=\frac{P_0^{\′\′}}{2},$ ${\mathrm{\α}}_3=\frac{{20P}_d-{20P}_0-({8P}_d^{\′}+{12P}_0^{\′})t_d-({3P}_0^{\′\′}-P_d^{\′\′})t_d^2}{{2t}_d},$

${\mathrm{\α}}_4=\frac{{30P}_0-{30P}_d-({14P}_d^{\′}+{16P}_0^{\′})t_d+({3P}_0^{\′\′}-{2P}_d^{\′\′})t_d^2}{{2t}_d^4},$

${\mathrm{\α}}_5=\frac{{12P}_d-{12P}_0-({6P}_d^{\′}+{6P}_0^{\′})t_d-(P_0^{\′\′}-P_d^{\′\′})t_d^2}{{2t}_d^5};$

其中，t表示机械臂关节运动时间，t_d表示机械臂关节目标运动时间，P₀和P_d分别为机械臂关节初始位置和目标位置，P′₀和P′_d分别为机械臂关节初始位置速度和目标位置速度，P″₀和P″_d分别为机械臂关节初始位置加速度和目标位置加速度。

高阶5次多项式插值算法同样适用于多路径点的关节轨迹规划，将整个轨迹看作由多个轨迹段组成，对相邻的路径点五次多项式插值，实现整个轨迹的光滑、连续、无抖动的运动。

如图6所示，以关节起始位置为0弧度，目标位置-π/2弧度，同时经过π/2和3π/4中间点为例，采用5阶多项式插值分别得到关节电机位置曲线、速度曲线和加速度曲线。从仿真曲线中可以看到相邻位置点之间曲线平稳、连续，同时速度曲线和加速度曲线没有出现跳变，保证了运动轨迹的光滑、连续、无抖动的运动。

Claims (5)

1.一种基于CANopen的机械臂关节电机控制方法，其特征在于，包括以下几个步骤：

步骤1：将各机械臂关节电机的驱动器连接于CAN总线上，并对所有驱动器上电，CAN总线进入预操作状态；

步骤2：控制端通过CAN卡将电机控制量对应的对象字典映射到CAN总线上的每个驱动器的PDO单元，基于PDO协议进行PDO配置，并将CAN总线设为可操作状态；

所述电机控制量包括电机转速和电流，电机所在关节的目标位置、加速度、减速度、位置反馈量及速度反馈量；

步骤3：按照控制需求，利用控制端从位置模式或速度模式中选择电机运行模式；

步骤4：依照控制目标，对关节运动轨迹采用高阶多项式插值得到每个时间点关节位置、速度及加速度的控制量；

步骤5：依据步骤4得到的控制量，通过PDO通讯方式对工作在位置模式或速度模式下的电机发送控制指令，完成对机械臂关节电机的控制；

所述电机控制指令包括位置指令、速度指令以及加减速度指令。

2.根据权利要求1所述的基于CANopen的机械臂关节电机控制方法，其特征在于，基于PDO协议将电机控制的对象字典映射为下表中所示的标识符及传输方式：

其中，R/TPDOx为接收或发送对象，节点id为驱动器在CAN总线上的节点标号。

3.根据权利要求2所述的基于CANopen的机械臂关节电机控制方法，其特征在于，利用同步信号传输驱动器实时获取机械臂关节电机的工作状态。

4.根据权利要求2所述的基于CANopen的机械臂关节电机控制方法，其特征在于，对各机械臂关节的轨迹P(t)采用高阶多项式插值得到每个时间点关节位置、速度及加速度：

P(t)＝α₀+α₁t+α₂t²+α₃t³+α₄t⁴+α₅t⁵

且需要满足以下约束条件：

P(0)＝P₀，P′(0)＝P′₀，P″(0)＝P″₀

P(t_d)＝P_d，P′(t_d)＝P′_d，P″(t_d)＝P″_d

其中，α₀＝P₀，α₁＝P′₀， ${\mathrm{\α}}_2=\frac{P_0^{\′\′}}{2},$ ${\mathrm{\α}}_3=\frac{{20P}_d-{20P}_0-({8P}_d^{\′}+{12P}_0^{\′})t_d-({3P}_0^{\′\′}-P_d^{\′\′})t_d^2}{{2t}_d},$

${\mathrm{\α}}_4=\frac{{30P}_0-{30P}_d-({14P}_d^{\′}+{16P}_0^{\′})t_d+({3P}_0^{\′\′}-{2P}_d^{\′\′})t_d^2}{{2t}_d^4},$

${\mathrm{\α}}_5=\frac{{12P}_d-{12P}_0-({6P}_d^{\′}+{6P}_0^{\′})t_d-(P_0^{\′\′}-P_d^{\′\′})t_d^2}{{2t}_d^5};$

其中，t表示机械臂关节运动时间，t_d表示机械臂关节目标运动时间，P₀和P_d分别为机械臂关节初始位置和目标位置，P′₀和P′_d分别为机械臂关节初始位置速度和目标位置速度，P″₀和P″_d分别为机械臂关节初始位置加速度和目标位置加速度。

5.一种基于CANopen的机械臂关节电机控制系统，其特征在于，包括CAN卡、上位机及CAN总线，上位机、CAN卡及CAN总线依次相连，各机械臂关节电机的驱动器连接于CAN总线上；上位机采用权利要求1-4任一项所述的基于CANopen的机械臂关节电机控制方法，通过CAN卡对连接在CAN总线上的各机械臂关节电机的驱动器进行PDO配置，基于PDO协议和配制的控制量，对机械臂关节电机进行控制。