基于STM32+FPGA的六自由度机器人运动控制器设计

2020-04-28史钊亮高锦宏王殿君

机械设计与制造 2020年4期

陈亚，史钊亮，高锦宏，王殿君

（北京石油化工学院机械工程学院，北京 102617）

1 引言

传统的机器人控制系统采用封闭式结构，柔性差、不利于系统维护升级和二次开发[1]。PC机+运动控制器的开放式控制系统，具有很强的开放性和灵活性，很好的解决上述不足，同时还具有较高的实时性和控制精度，成为国内外研究热点[2，3]。国内外各大公司生产的运动控制器大多为通用型产品普遍存在以下不足：首先功能强大、价格昂贵，但是在实际使用中往往只用到其少数功能，造成资源浪费，性价比较低；其次封闭式的结构在使用时往往很难进行二次开发，灵活性较差；另外复杂的软硬件结构，参数众多，使用不便[4-7]。设计一种六自由度机器人运动控制器。采用STM32+FPGA结构设计运动控制器硬件电路，进行相应的软件设计实现各项功能。搭建机器人控制系统，进行六自由度机器人运动控制实验。

2 六自由度机器人控制系统方案设计

六自由度机器人的运动控制系统整体方案，如图1所示。包括上位PC机、运动控制器、伺服系统和限位报警传感器等四部分组成[8-9]。

图1 六自由度机器人控制系统结构Fig.1 Six Degree of Freedom Robot Control System Structure

PC机提供人机交互界面，对机器人进行监视和控制，根据用户操作进行机器人运动学计算，得出各关节的运动参数，经以太网传输到运动控制器，同时获取各关节的位置信息和运行状态，实时显示机器人的末端位姿和运行状态。

运动控制器以STM32和FPGA为核心，STM32型号为STM32F103ZET6，FPGA型号为EP4CE10E22C8N。由STM32通过以太网与PC机通信，将接收到的位置增量、运行速度等运动参数转换为脉冲频率和脉冲数量等写入FPGA寄存器。FPGA根据各寄存器的参数输出脉冲方向信号并接受编码器反馈信号完成伺服电机的闭环控制，同时控制I/O信号完成机器人末端手爪的控制和限位回零等标志信号的接收。

机器人各关节均采用松下A5系列伺服电机和绿地谐波减速器驱动。伺服电机工作模式为位置控制模式，通过接收运动控制器的脉冲方向信号实现精确的位置控制。

松下A5系列伺服电机配有增量式光电编码器，增量式编码器在断电后位置信息丢失，因此在机器人各个关节安装光电开关，以实现机器人断电复位等操作。

3 运动控制器硬件设计

基于STM32+FPGA的结构，采用模块化设计思想进行运动控制器硬件电路设计，主要包括以太网通信模块，STM32与FPGA通信模块，编码器信号接收模块等。

3.1 以太网通信模块

基于STM32的高速SPI接口扩展W5500以太网控制器，实现运动控制器和PC机的以太网通信。W5500采用全硬件TCP/IP协议栈，支持 TCP、UDP、IPv4、ICMP、IGMP 和 PPPOE 等协议。使用此芯片实现以太网通信只需简单的套接字编程，极大的降低了以太网通信开发难度[10]。并使用型号为HR911105A的隔离变压器和RJ45接口实现以太网物理连接。

3.2 STM32与FPGA通信模块

STM32与FPGA的通信质量和速度对运动控制的性能有很大影响。将FPGA作为STM32的外设连接到FSMC存储器扩展接口上，通过在FPGA内部设置数据缓存区，实现STM32与FPGA通信。16位分时复用的地址/数据总线，用于传输地址和数据信息；1位片选信号，低电平有效，表示开始进行通信；1位地址锁存信号，低电平表示地址/数据总线为地址信息，高电平表示地址/数据总线为数据信息；2位读/写信号，低电平有效，分别表示读出数据和写入数据；1位中断信号，由FPAG发起的中断。

这种方式相对于SPI、I2C、USART等串行通信方式，有较快的通信速率；而相对于基于双口RAM芯片的通信方式可以节约成本。

3.3 编码器器信号接收模块

增量式编码器的输出信号一般为三组差分信号，分别为A+/A-，B+/B-和Z+/Z-。三组差分信号分别经过AM26LS32等效芯片转换为单端信号A、B和Z。

选用了AM26LV32低压高速四路差动线路接收芯片，它将光电编码器输出的5V差分信号转换为3.3V单端信号供FPGA使用，还可以对信号进行滤波，减少干扰影响。

使用Cadence软件绘制详细的电路原理图，并绘制印刷电路板，完成六自由度机器人运动控制器硬件制作，运动控制器硬件实物，如图2所示。

图2 运动控制器硬件实物图Fig.2 Hardware Object Diagram of Motion Controller

4 系统软件设计

基于PC机+运动控制器结构组成的机器人控制系统软件设计，一般分为PC机应用程序软件设计和运动控制器软件设计两部分。运动控制器软件又分为STM32的程序设计和FPGA的程序设计。

4.1 STM32程序设计

STM32程序主要完成与PC机通信，并根据PC机指令控制FPGA实现运动控制，使用Keil uVision5采用模块化设计思想完成STM32程序设计。

（1）通信模块程序设计。通信模块的程序通过硬件电路完成与上位机的通信，通信功能是运动控制器能否正常运行的前提条件。通信模块程序流程图，如图3所示。

图3 通信模块程序流程图Fig.3 Communication Module Program Flow Chart

（2）数据处理模块程序设计。数据处理模块程序将接收并处理来自上位机的数据，以获取电机运行速度、位置、方向等运动控制或I/O操作等命令信息。数据处理模块程序流程图，如图4所示。

（3）初始化参数设置模块程序设计。初始化参数设置程序将上位机设置的脉冲当量、最大速度、复位速度、复位方向等相关参数保存，并在初始化过程中读取相关参数。

（4）电机控制模块程序设计。电机控制模块程序将接收的运动控制命令中运行速度、位置增量等信息转换为脉冲分频值、脉冲数等信息，并将此信息写入FPGA中的电机控制寄存器中。

（5）通用I/O控制模块程序设计。通用I/O控制模块程序根据上位机命令，控制机器人手爪等末端执行器或指示灯、蜂鸣器等设备。

图4 数据处理模块程序流程图Fig.4 Data Processing Module Program Flow Chart

图6 单轴控制逻辑框图Fig.6 Single Axis Control Logic Block Diagram

4.2 FPGA程序设计

FPGA程序设计主要负责完成运动控制器各关节伺服控制任务。FPGA程序主要由FPGA与STM32通信程序和电机控制程序组成：通过FPGA与STM32通信程序接受STM32控制指令，并配置FPGA相关寄存器，电机控制程序根据各寄存器的值完成运动控制功能。使用Quartus II软件完成FPGA程序设计。

4.2.1 FPGA与STM32通信程序

根据FSMC的读写时序，使用状态机完成FPGA与STM32的通信的程序设计，状态分为空闲状态IDLE、写地址状态WRITEADDR、读写数据判断状态WRRDDATA、写数据状态WRITEDATA、读数据状态READDATA五种，其状态转移图，如图5所示。

图5 通信过程状态转移图Fig.5 State Transition Diagram of Communication Process

4.2.2 电机控制程序

电机控制主要包括脉冲产生、加减速控制、增量式编码器信号接收、限位开关以及定位控制等，电机控制程序，如图6所示。

加减速模块根据加减速寄存器和预置分频寄存器的将分频数交由脉冲产生模块；脉冲产生模块根据分频数对100MHz的基准时钟分频产生脉冲信号输出用于控制伺服电机；编码器反馈模块接收增量式编码器反馈信号并进行四倍频处理；位置控制模块使用计数器根据控制信息对脉冲产生模块或编码器反馈模块输出的脉冲和方向信号进行计数完成位置控制功能。

4.3 PC机应用程序软件设计

本设计中使用Visual Studio 2012软件开发PC机应用程序软件。上位机软件主要分为以下模块：

（1）初始化模块。建立PC机与运动控制器以太网通信连接，并对脉冲当量、最大速度、复位方向等运动参数进行初始化；

（2）关节运动模块。手动进行机器人各个关节的位置控制、速度控制等不同模式的运动控制，并且能够选择加速方式、加速度等参数；

（3）状态显示模块。获取机器人各个关节的实时位置通过机器人正运动学计算得到机器人末端位姿，并实时显示机器人各个关节的运动状态；

（4）示教模块。通过示教得到机器人运动的关键点，记录各个关键点位置机器人各关节位置，机器人自动运动再现机器人的运动，即示教再现；

5 实验

搭建机器人控制系统，进行机器人示教再现的运动控制实验，如图7所示。实验流程为：首先通过手动操作控制机器人抓取工件1，将工件1装配到与之配套的工件2中，并在此过程中记录机器人关键位置；然后通过示教再现命令重复上述过程。

图7 机器人示教再现试验Fig.7 Robot Teaching Playback Test

使用FARO激光跟踪仪，如图8所示。测定机器人的重复定位精度，其试验过程：通过机器人示教得到三个点A、B和C，反复执行从A到B，B到C，C到A的运动过程，记录机器人的末端位置。点A重复定位精度试验结果，如表1所示。点B重复定位精度试验结果，如表2所示。点C重复定位精度试验结果，如表3所示。实验结果，如表1～表3所示。