一种基于Zynq的协作机器人伺服控制器及控制方法

技术领域

本发明涉及一种协作机器人伺服控制器，尤其涉及一种基于Zynq的协作机器人伺服控制器及控制方法。

背景技术

在协作机器人伺服控制领域，常用的伺服控制器大多搭载传统三闭环PID控制算法，传统三闭环PID控制算法在被控对象所处环境不变时表现性能良好，但当被控对象所处环境发生变化时表现性能则较差。传统伺服控制器若搭载先进智能算法，则因计算时间过长，而直接影响机器人关节的响应速度。

目前，在协作机器人伺服控制器方面，控制器是直接集成于机器人的关节内部，因此控制器的尺寸将直接影响机器人关节的横向尺寸；同时控制器所需PCB的数量也会影响机器人关节的纵向尺寸。

综上所述，现有的协作机器人伺服控制器存在的问题主要是：①对于复杂算法运行速度不够快，导致协作机器人关节的响应速度不够快；②控制器集成度过低，占用关节内部空间过大。

发明内容

本发明的目的在于提供一种基于Zynq的协作机器人伺服控制器及控制方法，用于解决现有的伺服控制器对于复杂算法的运行速度较慢以及伺服控制器集成度较低的技术问题。

为实现上述目的，本发明的技术解决方案如下：

一种基于Zynq的协作机器人伺服控制器，用于设置在协作机器人的模块化单关节内，包括Zynq芯片、通信模块、数据采集模块、内存模块及电源模块，所述电源模块用于为Zynq芯片、通信模块、数据采集模块及内存模块提供稳压电源；其特殊之处在于：

所述Zynq芯片包括PL端和与PL端通过AXI4交互的PS端，其中，所述PS端用于对协作机器人永磁同步电机的运动轨迹进行优化计算；所述PL端烧录有三闭环控制算法，用于实现对永磁同步电机的位置、速度、电流的闭环控制；

所述通信模块包括JTAG电路和以太网，所述JTAG电路作为代码下载通道，与Zynq芯片的PS端连接；所述以太网与Zynq芯片的PL端连接，用于传递位置指令；

所述数据采集模块用于采集协作机器人的绝对式光电编码盘和增量式光电编码盘的数据，并将其传递至Zynq芯片的PL端；

所述内存模块与PS端连接，内存模块包括DDR3和Flash；所述DDR3作为PS端的运行内存，为其提供代码运行环境，所述Flash用于程序固化与数据存储。

进一步地，所述PS端包括BANK0、BANK500及BANK502；

所述DDR3与BANK502连接；所述Flash与BANK500连接；所述JTAG电路与BANK0连接。

进一步地，所述Zynq芯片、通信模块、数据采集模块、内存模块及电源模块集成在一个PCB板上，所述PCB板的层数为8层，其中第1、3、5、6、8层为Signer层，第2、7层为GND层，第4层为3.3V电源层。

进一步地，所述PL端包括BANK34和BANK35；所述BANK34烧录有三闭环控制算法；所述数据采集模块包括BISS-C通信电路和ABZ通信电路；

所述BISS-C通信电路用于采集绝对式光电编码盘的数据并将其传递至BANK34；所述ABZ通信电路用于采集增量式光电编码盘的数据并将其传递至BANK34；所述以太网与BANK35连接，用于将上位机的指令传递至Zynq芯片的PL端。

此外，本发明还提供一种基于Zynq的协作机器人伺服控制方法，所述协作机器人包括上位机，永磁同步电机，绝对式光电编码盘，增量式光电编码盘，逆变器，模块化单关节及设置在单关节内的伺服控制器，所述伺服控制器为上述的基于Zynq的协作机器人伺服控制器；所述控制方法包括以下步骤：

1】协作机器人上位机将输入位置和输入速度编译成位置指令，位置指令通过伺服控制器的通信模块发送至伺服控制器的Zynq芯片；

2】Zynq芯片的PL端对位置指令进行解析，得到位置轨迹；所述PL端烧录有三闭环控制算法，三闭环控制算法包括位置环控制算法、速度环控制算法及电流环控制算法；所述电流环控制算法包括交轴电流环控制算法和直轴电流环控制算法；三闭环即永磁同步电机运动的位置环、速度环和电流环，电流环包括交轴电流环和直轴电流环；

3】Zynq芯片的PS端通过差分进化算法对步骤2】所述的位置轨迹进行优化，并输入位置环；

4】Zynq芯片的PL端通过三闭环控制算法完成对永磁同步电机的位置、速度及电流的闭环控制，进而完成对协作机器人的闭环控制。

进一步地，步骤4】具体为：

4.1】Zynq芯片的PL端接收到数据采集模块采集到的绝对式光电编码盘反馈的永磁同步电机的实际位置，以及增量式光电编码盘反馈的永磁同步电机的实际速度；

4.2】将接收到的永磁同步电机的实际位置与步骤1】所述输入位置进行运算，并通过位置环控制算法，将优化后的位置轨迹与永磁同步电机的实际位置进行运算，将本步骤的所有运算结果输入位置环，由位置环输出速度环所需的速度信号；

4.3】将接收到的永磁同步电机的实际速度分别与步骤1】所述输入速度进行运算，并通过速度环控制算法，将位置环的速度信号与永磁同步电机的实际速度进行运算，将本步骤的所有运算结果输入速度环，由速度环输出电流环所需的交轴电流信号；

4.4】在Zynq芯片的PL端设计CORDIC算法、Park变换、Clark变换及Park逆变换；所述Clark变换与Park变换用于将永磁同步电机的三相交流电转换为两相直流电，从而为电流环反馈永磁同步电机的实际直轴电流与实际交轴电流；所述CORDIC算法用于为Park变换与Park逆变换计算永磁同步电机旋转角度的三角函数值；

4.5】通过交轴电流环控制算法，将速度环输出的交轴电流信号与步骤4.4】反馈的实际交轴电流的运算结果输入交轴电流环，并输出Park逆变换所需的交轴电压；

4.6】通过直轴电流环控制算法，将直轴输入电流与步骤4.4】反馈的实际直轴电流的运算结果输入直轴电流环，并输出Park逆变换所需的直轴电压；

4.7】Zynq芯片的PL端将步骤4.5】输出的交轴电压与步骤4.6】输出的直轴电压通过Park逆变换与SVPWM转换为逆变器所需要的六路PWM信号，再由逆变器输出永磁同步电机所需的三相电流信号，以此控制永磁同步电机运动；

4.8】返回步骤4.1】，Zynq芯片的PL端实时接收永磁同步电机的实际位置和实际速度，实现对永磁同步电机的位置、速度及电流的闭环控制，进而完成对协作机器人的闭环控制。

进一步地，步骤4.1】具体为：

在Zynq芯片的PL端使用Verilog HDL编写位置UART与速度UART，位置UART与速度UART分别用于接收绝对式光电编码盘反馈的实际位置，以及增量式光电编码盘反馈的实际速度。

进一步地，步骤4.4】中，使用Verilog HDL在Zynq的PL端设计CORDIC算法、Park变换、Clark变换及Park逆变换。

进一步地，步骤2】中，在Zynq芯片的PL端使用Verilog HDL编写以太网通信单元，用于解析上位机通过通信模块中的以太网UDP协议发送的位置指令。

进一步地，步骤3】中，使用存储模块中的DDR3为PS端提供运行环境，并使用C语言进行编程。

与现有技术相比，本发明的有益效果如下：

1、本发明的伺服控制器的Zynq芯片包括PL端和与PL端通过AXI4交互的PS端，其中，PS端用于对协作机器人永磁同步电机的运动轨迹进行优化计算；PL端烧录有三闭环控制算法，用于实现对永磁同步电机的位置、速度、电流的闭环控制，以此提高伺服控制器对于复杂算法的运行速度，进而提高协作机器人关节的响应速度。

2、本发明的伺服控制器中，Zynq芯片、通信模块、数据采集模块、内存模块及电源模块集成在一个PCB板上，以此提高了伺服控制器的集成度，进而减小了其在关节内的占用空间。

3、本发明的伺服控制方法，通过各模块的合理布局以及算法的匹配设计，不仅提升了伺服机器人对于复杂算法的运行速度，同时也提升了单关节的响应速度与控制精度，并降低了关节超调，以此增加了协作机器人的工作效率。

附图说明

图1为本发明一种基于Zynq的协作机器人伺服控制器实施例的结构示意图；

图2为本发明一种基于Zynq的协作机器人伺服控制器实施例中通信模块的结构示意图；

图3为本发明一种基于Zynq的协作机器人伺服控制器实施例中数据采集模块的结构示意图；

图4为本发明一种基于Zynq的协作机器人伺服控制器实施例中电源模块的结构示意图；

图5为本发明一种基于Zynq的协作机器人伺服控制器实施例中内存模块的结构示意图；

图6为本发明一种基于Zynq的协作机器人伺服控制方法实施例的算法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和具体实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明的协作机器人包括上位机，永磁同步电机，绝对式光电编码盘，增量式光电编码盘，逆变器，模块化单关节，以及基于Zynq的协作机器人伺服控制器。

如图1所示，本发明的一种基于Zynq的协作机器人伺服控制器，设置在模块化单关节内，包括Zynq芯片、通信模块、数据采集模块、内存模块及电源模块。Zynq芯片、通信模块、数据采集模块、内存模块及电源模块集成在一个PCB板上，PCB板的层数为8层，其中第1、3、5、6、8层为Signer层，第2、7层为GND层，第4层为3.3V电源层，以此提高了伺服控制器的集成度，减小了伺服控制器在关节内的占用空间。

为了克服现有技术中伺服控制器横向体积大，以及对于先进人工智能算法的不适用性，本发明的Zynq芯片包括PL端和与PL端通过AXI4交互的PS端，其中，PS端包括BANK0、BANK500及BANK502，用于对协作机器人永磁同步电机的运动轨迹进行优化计算。PL端包括BANK34和BANK35，其中PL端的BANK34烧录有三闭环控制算法，用于实现对永磁同步电机的位置、速度、电流的闭环控制。

结合图1和图2所示，通信模块包括JTAG电路和以太网，JTAG电路作为代码下载通道，与Zynq芯片的PS端的BANK0连接，数据流经过JTAG并进行上拉后输入进ZYNQ芯片的BANK0。以太网作为指令控制通道，与Zynq芯片PL端的BANK35连接，用于将上位机的指令传递至Zynq芯片的PL端，控制指令经过RJ45插座进入PHY芯片电路后进行电平转换，并将四位差分数据转换为四位单端数据输入进Zynq芯片的BANK35。本实施例中，RJ45插座的产品型号为DGKYD111Q066，以太网PHY芯片的产品型号为RTL8211FDI。

结合图1和图3所示，数据采集模块包括BISS-C通信电路和ABZ通信电路，BISS-C通信电路和ABZ通信电路均采用基于RS485通信的芯片实现。其中，数据采集模块用于采集协作机器人的绝对式光电编码盘和增量式光电编码盘的数据信息，并将其传递至Zynq芯片的PL端，即绝对式光电编码盘发出的一对差分数据经过BISS-C通信电路、差分转单端及滤波电路转换为稳定的单端数据，并由BANK34接收并进行运算处理。增量式光电编码盘发出的3比特ABZ数据经过ABZ通信电路、5V上拉电路及滤波电路、3.3V电平转换电路，最终由BANK34接收并进行运算处理。本实施例中，BISS-C通信电路的芯片产品型号为ISO3088，ABZ通信电路的芯片产品型号为SN74LV4T125-Q1。

结合图1和图5所示，内存模块与PS端连接，内存模块包括DDR3和Flash；其中，DDR3与BANK502使用16bit数据线、时钟线、地址线进行交互，其中，地址线进行1.5V上拉处理。DDR3作为PS端的运行内存，为其提供代码运行环境。Flash与BANK500采用数据线和地址线进行交互，Flash用于程序固化与数据存储。本实施例中，DDR3芯片产品型号为MT41J128M16HA，Flash存储芯片型号为S25FL256SAGNFI00。

结合图1和图4所示，电源模块用于为Zynq芯片、通信模块、数据采集模块及内存模块提供稳压电源。电源模块包括LDO(低压差线性稳压器)稳压电路和DC-DC稳压电路，分别将5V电压转换为3V、1.8V、1.5V、1.2V、1.0V电压，分别对板上不同的模块进行供电。本实施例中，LDO稳压电路的芯片产品型号为TPS71733DCK，DC-DC稳压电路芯片的产品型号为TPSM82903。电源模块供电的过程为：+5V电压先经过+5V电源滤波电路，之后分别进入3.3VLDO稳压电路、3.3VDC-DC稳压电路、1.8V稳压电路、1.5V稳压电路、1.2V稳压电路及1.0V稳压电路，再分别经过3.3VLDO滤波电路、3.3VDC-DC滤波电路、1.8V滤波电路、1.5V滤波电路、1.2V滤波电路及1.0V滤波电路，最后输出稳定的3.3VLDO电压、3.3VDC-DC电压、1.8V电压、1.5V电压、1.2V电压及1.0V电压。

结合图1至图6所示，本发明还提供一种基于Zynq的协作机器人伺服控制方法，该方法采用的伺服控制器即为本发明前述的一种基于Zynq的协作机器人伺服控制器，本发明的控制方法包括以下步骤：

1】协作机器人上位机将输入位置和输入速度编译成位置指令，位置指令通过伺服控制器的通信模块发送至伺服控制器的Zynq芯片。

2】Zynq芯片的PL端对位置指令进行解析，得到位置轨迹；所述PL端烧录有三闭环控制算法，三闭环控制算法包括位置环控制算法、速度环控制算法及电流环控制算法；所述电流环控制算法包括交轴电流环控制算法和直轴电流环控制算法；三闭环即永磁同步电机运动的位置环、速度环和电流环，电流环包括交轴电流环和直轴电流环。

本实施例中，在Zynq芯片的PL端使用Verilog HDL编写以太网通信单元，用于解析上位机通过通信模块中的以太网UDP协议发送的位置指令。

3】Zynq芯片的PS端通过差分进化算法对步骤2】所述的位置轨迹进行优化，并输入位置环。

本实施例中，使用差分进化算法对位置轨迹进行优化，考虑到差分进化算法具有大量迭代运算，将其布局于Zynq的PS端，并使用DDR3为PS端提供运行环境，使用的编程语言为C语言。

4】Zynq芯片的PL端通过三闭环控制算法完成对永磁同步电机的位置、速度及电流的闭环控制，进而完成对协作机器人的闭环控制。

步骤4】具体为：

4.1】Zynq芯片的PL端接收到数据采集模块采集到的绝对式光电编码盘反馈的永磁同步电机的实际位置，以及增量式光电编码盘反馈的永磁同步电机的实际速度。

本实施例中，在Zynq芯片的PL端使用Verilog HDL编写位置UART与速度UART，位置UART与速度UART分别用于接收绝对式光电编码盘反馈的实际位置，以及增量式光电编码盘反馈的实际速度。

4.2】通过位置环控制算法，将优化后的位置轨迹与永磁同步电机的实际位置进行运算，将运算结果输入位置环，由位置环输出速度环所需的速度信号。

4.3】通过速度环控制算法，将位置环的速度信号与永磁同步电机的实际速度进行运算，将运算结果输入速度环，由速度环输出电流环所需的交轴电流信号。

4.4】在Zynq芯片的PL端设计CORDIC算法、Park变换、Clark变换及Park逆变换；所述Clark变换与Park变换用于将永磁同步电机的三相交流电转换为两相直流电，从而为电流环反馈永磁同步电机的实际直轴电流与实际交轴电流；所述CORDIC算法用于为Park变换与Park逆变换计算永磁同步电机旋转角度的三角函数值。

本实施例中，使用Verilog HDL在Zynq的PL端设计CORDIC算法、Park变换、Clark变换及Park逆变换。

4.5】通过交轴电流环控制算法，将速度环输出的交轴电流信号与步骤4.4】反馈的实际交轴电流的运算结果输入进交轴电流环，输出Park逆变换所需的交轴电压。

4.6】通过直轴电流环控制算法，将直轴输入电流与步骤4.4】反馈的实际直轴电流的运算结果输入进直轴电流环，输出Park逆变换所需的直轴电压。

4.7】Zynq芯片的PL端将步骤4.5】的交轴电压与步骤4.6】的直轴电压通过Park逆变换与SVPWM转换为逆变器所需要的六路PWM信号，再由逆变器输出永磁同步电机所需的三相电流信号，以此控制永磁同步电机运动。

4.8】Zynq芯片的PL端实时对接收到的永磁同步电机的实际位置和实际速度分别与步骤1】所述输入位置与输入速度进行运算，最终将运算结果分别输入永磁同步电机的位置环与速度环，实现对永磁同步电机的位置、速度及电流的闭环控制，进而完成对协作机器人的闭环控制。

本发明的控制方法在传统PID三闭环矢量控制的基础上使用模糊控制优化位置环，使用差分进化算法从能量的角度对输入轨迹进行优化，将10°、50°、70°、100°位置信号下的输出转矩分别降低了76.7％、71.4％、68.7％、61.3％，并且均无超调。本发明的控制器可以满足协作机器人伺服系统的小型化、高效率、高可靠性要求。

以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。