CN110815229A - 一种码垛机器人驱动方法及驱动系统 - Google Patents

Abstract

本发明公开了一种码垛机器人驱动方法及驱动系统，包括ARM模块和FPGA模块，ARM模块负责与上位机进行指令或者数据交互，FPGA模块进行电机控制信号发送以及光栅信号的处理，通过电机信号控制、光栅闭环反馈信号处理以及限位开关等模块实现对烟箱进行解包码垛的功能。在基于该架构下的码垛机器人中，ARM将串行口下载的程序编译，通过上位机、ARM及配置芯片、FPGA的可重构设计，上位机发出重构命令和配置数据，通过ARM控制重写配置芯片来实现控制2片FPGA，最终驱动卡的接口板分别控制X、Y、Z、A、B、C六轴伺服电机，具有很强的可修改和可移植性，方便底层驱动修改，使系统更具拓展和维护性，有良好、开放的软硬件接口，其控制柔性特性较好。

Description

一种码垛机器人驱动方法及驱动系统

技术领域

本发明属于机器人驱动技术领域，具体涉及一种码垛机器人驱动方法及驱动系统。

背景技术：

码垛是把物料根据一定规则和次序码放在托盘上，以便实现物料的运输、装卸、存储等物流活动。卷烟制丝生产线烟箱解包码垛机器人主要是将切割后的烟包外部包装分离，以一定秩序传输并分类叠放纸箱、塑料带和其他扎带，完成烟箱自动解包码垛的功能的机器人。目前，应用较广的码垛机器人驱动卡主要分两类。第一类是基于PC控制系统，其硬件部分是计算机插上运动控制卡来负责机器人的运动学解析、轨迹规划算法以及控制电动机运动，软件部分主要基于PC机的高级编程语言实现。但其缺点为无法适应底层驱动修改需求，可移植、可修改性差、价格高昂，德国KUKA码垛机器人就是这种架构的产品；第二类是基于PLC控制系统，其硬件组成主要是PLC控制器，依靠强大I/O控制功能来控制电机运动，利用组态软件设计友好的HMI，该结构工作稳定性和抗干扰能力强，但缺点为系统架构落伍、控制精度不高、柔性特性差、灵活度不高以及通用性较差。

发明内容

本发明的目的在于提供一种码垛机器人驱动方法及驱动系统，以解决现有技术中存在的码垛机器人的控制精度不高的缺点。

本发明的技术方案由如下步骤实现：

第一方面，提供了一种码垛机器人驱动方法，所述方法包括如下步骤：

微处理器接收上位机的控制指令，进行分析后得到控制数据；

微处理器将控制数据发送至可编程器件；

电机驱动器读取可编程器件中的控制数据控制电机运行；

电机运动控制器监测电机运行状态，并将运行状态信息发送至微处理器；

微处理器将运行状态信息上传至上位机，上位机根据电机运行状态发出控制指令。

结合第一方面，进一步的，微处理器将控制数据发送至电机寄存器的方法包括如下步骤：

微处理器将控制数据写给可编程器件；

可编程器件将控制数据存入电机寄存器中。

结合第一方面，进一步的，电机的运行控制方法包括如下步骤：

可编程器件接收微处理器发出的控制数据，并按照控制数据的内容配置相关电机寄存器；

电机驱动器读取电机寄存器中产生的电机脉冲及方向信号控制电机运行；

电机运动控制器实时监控电机运行的状态，将电机运行状态信息反馈给微处理器；

微处理器判断电机的运行状态是否符合规定，若符合则待定；

若不符合，则根据状态种类自行发出控制数据或上传至上位机进行处理。

结合第一方面，进一步的，所述控制指令包括加减速运动指令、直线插补指令、快速停止指令以及Z相定位回零指令中的一种或多种。

结合第一方面，进一步的，所述运行状态信息包括各轴电机所处位置、实时运行速度以及应答信息中的一种或多种。

第二方面，提供了一种码垛机器人驱动系统，包括：

上位机；

与上位机双向连接，接收及分析上位机发出的控制指令，并将控制数据上传至上位机的微处理器模块；

与所述微处理器模块连接，存储所述微处理器发出的控制数据的可编程器件模块；

与所述可编程器件模块连接，读取控制数据并控制与其连接的电机运行的电机驱动模块；

以及与所述微处理器模块和所述电机连接，监控所述电机运行并将运行信息发送至所述微处理器模块的电机运动控制器模块。

结合第二方面，进一步的，所述微处理器模块与所述可编程器件模块之间通过通讯模块连接，所述通讯模块包括数据总线、地址总线以及控制总线中的一种或多种。

结合第二方面，进一步的，所述系统还包括电源转换模块，所述电源转换模块包括24V转5V电路模块、5V转3.3V电路模块、5V转1.8V电路模块以及5V转1.2V电路模块中的一种或多种。

结合第二方面，进一步的，所述微处理器模块包括ARM芯片，所述可编程器件模块包括分别控制若干组电机的FPGA模块1和FPGA模块2。

结合第二方面，进一步的，所述电机包括至少六个电机组成的控制X、Y、Z、A、B、C六个方向的伺服电机。

本发明的优点在于：本发明型由于选用ARM+FPGA为核心并利用FPGA的可重构设计技术进行构建，具有很强的可修改性和可移植性；由于能植入LinuxCNC实时操作系统保证装置运行的实时性，并且能够根据特定的需要编写自定义IP核，自由添加所需要的特定功能，同时方便底层驱动修改，使系统更具可拓展和维护性；又由于FPGA、ARM芯片具有具有出众的产品性能及可靠性，其性价比较高；由于选用良好的、规范的、开放的软硬件接口，其控制柔性特性较好。

附图说明

图1为本发明型涉及的码垛机器人控制构成。

图2为本发明型ARM+FPGA驱动控制方案示意图。

图3为本发明的结构示意图。

图4为本发明型24V转5V电路模块原理图。

图5为本发明型5V转3.3V电路模块原理图。

图6为本发明型5V转1.8V电路模块原理图。

图7为本发明型5V转1.2V电路模块原理图。

图8为本发明型ARM芯片JTAG接口电路原理图。

图9为本发明型ARM芯片振荡电路原理图。

图10为本发明型ARM芯片复位电路原理图。

图11为本发明型ARM芯片ISP接口和上电启动电路原理图。

图12为本发明型FPGA芯片配置电路原理图。

图13为本发明型FPGA芯片振荡电路原理图。

图14为本发明型ARM和FPGA通讯模块原理图。

图15为本发明型外部存储器电路原理图。

图16为本发明型与上位机通讯电路原理图。

图17为本发明型电机驱动接口电路原理图。

图18为本发明型光栅反馈信号电路原理图。

图19为本发明型软件系统组成。

图20为本发明型ARM主程序流程图。

图21为本发明型直线插补流程图。

图22为本发明型加减速控制流程图。

图23为本发明型点位运动程序流程图。

图24为本发明型快速停止指令实现流程图。

图25为本发明型Z相定位回零指令的程序流程图。

图26为本发明型ARM与FPGA通讯流程图。

图27为本发明型重构软件控制流程图。

具体实施方式

为使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实施方式，进一步阐述本发明。

如图1所示，本发明型适用的码垛机器人结构：基于Altera EP2C8系列FPGA芯片和基于ARM7TDMI-S系列LPC2294芯片的X、Y、Z、A、B、C六轴码垛机器人驱动卡，应用于卷烟制丝生产线烟箱解包码垛机器人。ARM负责与上位机进行指令或者数据交互，FPGA进行电机控制信号发送以及光栅信号的处理，通过电机信号控制、光栅闭环反馈信号处理以及限位开关等模块实现对烟箱进行解包码垛的功能。在基于该架构下的码垛机器人中，ARM将串行口下载的程序编译，通过上位机、ARM及配置芯片、FPGA的可重构设计，上位机发出重构命令和配置数据，通过ARM控制重写配置芯片来实现控制2片FPGA，最终驱动卡的接口板分别控制X、Y、Z、A、B、C六轴伺服电机。

如图2所示，ARM负责与上位机进行指令或者数据交互，FPGA进行电机控制信号发送以及光栅信号的处理。当接收到指令和数据后，ARM进行指令解析，发给FPGA，并且按照一定频率读写FPGA内部的数据，并上传给上位机。在本系统中，FPGA作为ARM外扩的数据存储器，以三总线(数据总线、地址总线、控制总线)的方式将控制数据写给FPGA。FPGA主要是实现6个电机的运动控制，并处理光栅、编码器、限位开关等的反馈信号，在控制每根轴的运行过程中，实现S曲线加减速，运行到指定位置后，电机停止运行，同时给ARM发送一个中断信号。本系统中采用两片FPGA，FPGA1模块负责1-3号电机控制，FPGA2模块负责4-6号电机控制。

ARM处理器的设计功能：(1)通过RS232串口接收上位机控制指令；(2)解析控制指令，生成FPGA的控制数据；(3)通过控制总线、地址总线、数据总线配置FPGA中的电机寄存器；(4)读写FPGA的电机运行数据信息；(5)运行信息反馈。在系统运行的过程中，运动控制器将实时的运行信息(各轴电机所处位置、实时速度、应答信息)上传给上位机；(6)运行状态提示。开机提示、运行故障提示、运行完毕提示等。

FPGA功能及其实现逻辑(1)实现与ARM微处理器通讯功能；(2)接收ARM处理器发出电机控制信号，按照控制指令配置相关电机寄存器，产生电机的脉冲、方向信号发送给步进电机驱动器，实现驱动电机运行；(3)电机在运行的过程中，实现加减速S曲线控制，使电机能够平稳运行；(4)电机精确定位。接收光栅产生的闭环信号，对信号进行四细分和辨向，计算得出电机位移信息，从而实现对位移的闭环控制，进行精确定位；(5)监控电机运行的状态，将电机运行状态等信息快速反馈给ARM处理器，ARM处理器根据不同的电机状态采取不同的措施；(6)限位保护。

如图3所示，基于ARM和FPGA的六轴烟箱解包码垛机器人驱动卡，该系统包括电源转换模块、ARM模块、FPGA模块、ARM和FPGA通讯模块、外部存储器模块、与上位机通讯模块、电机驱动接口电路模块和光栅反馈信号电路模块；所述的电源转换模块包括24V转5V电路、5V转3.3V电路、5V转1.8V电路、5V转1.2V电路；所述的ARM模块由JTAG接口电路、振荡电路、复位电路以及ISP接口和上电启动电路组成；所述的FPGA模块包括配置电路和振荡电路。

如图4所示，24V转5V电路模块，采用LM2596芯片进行电压转换，4007二极管可以防止电压插反、可恢复保险起过流保护的作用，增强整个电路的安全性和可靠性。外围电路的电感电容用于滤除干扰。

如图5、6、7所示，5V转3.3V电路模块、5V转1.8V电路模块、5V转1.2V电路模块，采用SPX1117芯片进行电压转换,0.1uF陶瓷电容和一个10uF的钽电容降低电源扰动和高频噪声对整个路的影响。

如图8所示，ARM芯片JTAG接口电路模块采用ARM公司的标准20脚JTAG仿真调试接口。

如图9所示，ARM芯片振荡电路模块采用11.059M外部晶振，ARM芯片时钟信号由内部晶体振荡器和外部连接的晶振产生。

如图10所示，ARM芯片复位电路采用CAT809R作为电源监测芯片，在系统上电时，上升到大于阈值后的至少140ms内复位信号的状态仍保持有效，当电源电压低于+2.63V时产生一个低电平信号。

如图11所示，ARM芯片ISP接口和上电启动电路模块中通过P0.14引脚控制ISP选择信号，BOOT0\BOOT1作为上电启动选择。

如图12所示，FPGA芯片配置电路采用EPCS4SI8专用配置芯片进行主动方式配置的电路中，主要有nCE、nCS、CONFIG-DONE、nSTATUS、nCONFIG，DATAO、CLK七种信号。其中nCE需阻值为1K的下拉电阻，nSTATUS、CONFIG-DONE和nCONFIG三个信号线采用阻值10K电阻上拉到3.3V的电源部分，这样保证了在进行JTAG配置或者正常使用时，这些配置引脚处在一个正确的电平状态。在JTAG配置电路中，TCK也是需要上拉到3.3V，TMS和TDI接入阻值为10K的电阻下拉到地，以保证芯片的正常配置以及工作。JTAG接口进行配置可以使用ALTERA的下载线，通过Quartus II工具下载。

如图13所示，FPGA芯片振荡电路采用40MHZ有源晶振作为芯片工的时钟输入，磁珠(FB305)和两个贴片电容的作用都是为了减小源扰动对晶振的影响，33欧姆电阻为抗匹配，使晶振的输出尽量最优。

如图14所示，ARM和FPGA通讯模块中，FPGA作为ARM外扩的数据存储器，以三总线(数据总线、地址总线、控制总线)的方式将数据写给FPGA。LPC2294使用外部存储器接口的BANK1和BANK2地址空间分配给FPGA1和FPGA2。片选信号CS0和CS1分配连接FPGA1和FPGA2的片选端；LPC2294使用16为数据总线D0-D15与FPGA1和FPGA2的对应16位数据线引脚相连；LPC2294的OE和WE外部控制总线和写信号，分别与两块FPGA的读写信号线连接；LPC2294使用16位的数据总线，其他地址去掉A0，使用A1-A10与FPGA1和FPGA2的A0-A9地址总线连接。

如图15所示，外部存储器模块选用AT24C64芯片主要用来存储一些参数信息，包括六轴伺服电机的脉冲当量、运动控制器出厂时配置信等，采用I2C总线方式进行访问。

如图16所示，与上位机通讯模块选用MAX3232芯片实现RS-232串口通讯方式。

如图17所示，电机驱动接口电路模块采用AM26LS31转换芯片得到差分信号以提高系统的抗干扰能力，采用升压至5V电路供电。

如图18所示，光栅反馈信号电路模块采用MC3486芯片对光栅反馈信号进行差分转单端信号的处理，其外部I/O引脚3.3V电压由74LVT-H245芯片转换实现。

如图19所示，基于ARM和FPGA的六轴烟箱解包码垛机器人驱动卡，其软件系统由LinuxCNC核心模块、自行开发底层驱动、ARM与FPGA通讯程序、可重构软件设计模块以及FPGA总线通讯模块组成。

所述的LinuxCNC是一套用于机器控制的计算机软件控制系统，可用来切换数控部件运动模式、加载编辑G代码或者实时显示数控部件状态等；运动控制模块(EMCMOT)是LinuxCNC的核心，它负责G代码翻译、插补运算、运动控制等数控系统核心功能，该模块实时性要求较高，需要运行在ARM实时内核中。

如图20所示，ARM作为一个虚拟PLC是运动控制器的直接管理者。其一方面能完成PLC的逻辑控制功能，另一方面能实现运动控制输出功能。前者主要由PLC来实现，后者则通过PLC将运动控制指令通过接口发送到FPGA，由FPGA来完成。ARM植入LinuxCNC系统后，实现主程序控制。

如图21所示，在需要执行加减速运动指令时，已知当前坐标和终点的坐标，在LinuxCNC系统调用相关函数执行时间分割发直线插补运动的程序。

如图22所示，在需要执行直线插补指令时，为了让电机能够运行平稳，特别是在启动和停止阶段速度不至于有太大的突变，往往会调用加减速控制程序。

如图23所示，LinuxCNC以点动运动控制代码形式，输出至自行开发底层驱动，再次进行程序编译，点动运动控制流程图如图所示。

所述的自行开发底层驱动主要包括快速停止指令、Z相定位回零指令等程序。如图24所示，当快速停止指令命令被触发后，CPU向FPGA发送快速停止命令，然后等待FPGA回应，当收到电机已经停止的信号后清当前正在运动标志位。FPGA接收到该命令后，无论当前处于何种状态，都直接停止PWM的发送，并将其状态标识标记为停止态，并将停止信号反馈给CPU。

如图25所示，Z相定位回零指令用于在系统刚上电时并不能确定电机当前的实际位置，在控制系统每一次上电时都需要对电机进行复位操作。Z相定位回零的执行过程如下：步骤1：在CPU收到Z相定位回零命令后，PU控制执行点位运动指令，运动的总步数为电机转一圈所需要的总步数，方向为正向，其具体的执行过程见点位运动指令；步骤2：若等到正向转完半圈后，没有收到Z相信号，则CPU再执行一次点位运动指令，运动点数为电机转半圈所需要的总步数，方向为反向，其具体的执行过程见点位运动指令；步骤3：若电机在正向运动的时候，CPU收到Z相定位信号，则CPU立即对FPGA发送减速命令，直到FPGA停止发脉冲；步骤4：在执行完步骤3后，若CPU判断到FPGA已经停止发脉冲，则执行点位运动指令，其运动的总步数为电机转1/10圈所需要的总步数，方向为反向，其具体执行过程见点位运动指令；步骤5：在执行步骤2或者步骤4的过程中，若CPU收到Z相定位信号，则CPU立即对FPGA发送减速命令，直到FPGA停止发脉冲；若CPU没收到Z相定位信号，则重复步骤2或者4；步骤6：在执行完步骤5后，若CPU判断到FPGA停止发脉冲后，执行点位运动命令，其运动的总步数为电机转1/10圈所需要的总步数，方向为正向。若在执行过程中CPU接收到Z相定位信号，CPU立即给FPGA发送快速停止指令，Z相定位完成；若没收到Z相定位信号，则重复步骤6。

如图26所示，ARM与FPGA的通信模块主要为ARM将运动控制命令和相关参数发到负责运动控制的FPGA内部的双口RAM中，读取各轴电机当前的相关状态。

如图27所示，可重构软件设计模块中，PC是重构命令发起者。当系统需要对FPGA进行硬件升级(重构)时，可以由PC机将新的配置数据.rpd文件(Quartus II工程的输出文件)和重构命令通过串口发送至ARM。ARM是重构的执行者。一旦ARM接收到上位机发送来的重构命令和配置数据，ARM就抢夺FPGA对配置芯片的控制权，开始控制配置芯片，将新数据写入配置芯片内。在对EPCS配置芯片写数据完成后，ARM将放开对配置芯片的控制，使FPGA重新获得对配置芯片的控制权下载最近一次写入的配置数据。

由技术常识可知，本发明可以通过其它的不脱离其精神实质或必要特征的实施方案来实现。因此，上述公开的实施方案，就各方面而言，都只是举例说明，并不是仅有的。所有在本发明范围内或在等同于本发明的范围内的改变均被本发明包含。

Claims (10)

1.一种码垛机器人驱动方法，其特征在于，所述方法包括如下步骤：

微处理器接收上位机的控制指令，进行分析后得到控制数据；

微处理器将控制数据发送至可编程器件；

电机驱动器读取可编程器件中的控制数据控制电机运行；

电机运动控制器监测电机运行状态，并将运行状态信息发送至微处理器；

微处理器将运行状态信息上传至上位机，上位机根据电机运行状态发出控制指令。

2.根据权利要求1所述的一种码垛机器人驱动方法，其特征在于：微处理器将控制数据发送至电机寄存器的方法包括如下步骤：

微处理器将控制数据写给可编程器件；

可编程器件将控制数据存入电机寄存器中。

3.根据权利要求1所述的一种码垛机器人驱动方法，其特征在于：电机的运行控制方法包括如下步骤：

可编程器件接收微处理器发出的控制数据，并按照控制数据的内容配置相关电机寄存器；

电机驱动器读取电机寄存器中产生的电机脉冲及方向信号控制电机运行；

电机运动控制器实时监控电机运行的状态，将电机运行状态信息反馈给微处理器；

微处理器判断电机的运行状态是否符合规定，若符合则待定，

若不符合，则根据状态种类自行发出控制数据或上传至上位机进行处理。

4.根据权利要求1所述的一种码垛机器人驱动方法，其特征在于：所述控制指令包括加减速运动指令、直线插补指令、快速停止指令以及Z相定位回零指令中的一种或多种。

5.根据权利要求1所述的一种码垛机器人驱动方法，其特征在于：所述运行状态信息包括各轴电机所处位置、实时运行速度以及应答信息中的一种或多种。

6.一种码垛机器人驱动系统，其特征在于，包括：

上位机；

与上位机双向连接，接收及分析上位机发出的控制指令，并将控制数据上传至上位机的微处理器模块；

与所述微处理器模块连接，存储所述微处理器发出的控制数据的可编程器件模块；

与所述可编程器件模块连接，读取控制数据并控制与其连接的电机运行的电机驱动模块；

以及与所述微处理器模块和所述电机连接，监控所述电机运行并将运行信息发送至所述微处理器模块的电机运动控制器模块。

7.根据权利要求5所述的一种码垛机器人驱动系统，其特征在于：所述微处理器模块与所述可编程器件模块之间通过通讯模块连接，所述通讯模块包括数据总线、地址总线以及控制总线中的一种或多种。

8.根据权利要求5所述的一种码垛机器人驱动系统，其特征在于：所述系统还包括电源转换模块，所述电源转换模块包括24V转5V电路模块、5V转3.3V电路模块、5V转1.8V电路模块以及5V转1.2V电路模块中的一种或多种。

9.根据权利要求4所述的一种码垛机器人驱动系统，其特征在于：所述微处理器模块包括ARM芯片，所述可编程器件模块包括分别控制若干组电机的FPGA模块1和FPGA模块2。

10.根据权利要求4所述的一种码垛机器人驱动系统，其特征在于：所述电机包括至少六个电机组成的控制X、Y、Z、A、B、C六个方向的伺服电机。

Images