五自由度工业机器人控制系统设计

朱庆爽 ，王平 ，董登峰 ，周维虎

(1. 南京航空航天大学 自动化学院，江苏 南京 210016； 2. 中国科学院 光电研究院，北京 100094)

0 引言

传统的工业机器人控制系统通常采用专用微处理器、专用计算机和专用机器人语言的封闭式控制系统结构，这种结构限制了机器人系统的开放性和通用性。随着中国制造业的升级转型，工业机器人的使用越来越多，然而工业机器人的市场份额主要被外国企业占据，而且价格比较昂贵，并不能满足国内的一些中小企的需求。因此国内的中小企业更需要价格便宜，开放性较高，操作简便的自动化设备[1]。本文主要目的是采用“PC+DSP+FPGA”架构设计实现一个五自由度工业机器人控制系统。并且使开发的五自由度工业机器人控制系统可以完成注塑机的下料作业任务，同时具有较高的开放性、较低的成本、良好的性能和可操作性。

1 控制系统硬件总体设计

硬件部分是工业机器人控制系统的基础，其中硬件的性能对工业机器人控制系统控制的精度和实时性有决定性影响。本文设计的控制系统主要包括人机接口设备和运动控制器2个部分。人机接口设备提供可视化监控界面，并且可以通过人机接口对机器人完成示教操作；运动控制器则负责运动学解算、插补运算、轨迹规划以及控制机器人各个关节的位置和加减速等[2]。

为了满足工业机器人的作业任务，同时使控制系统的硬件具有较好扩展性、较高的性能以及低成本的特点。由于DSP具有强大的计算能力，非常适合工业机器人的运动学解算和轨迹规划等大量复杂的实时计算，以及FPGA强大的并行计算能力可以实现对五路增量式光电编码器的快速采集，所以采用DSP+FPGA结构作为控制系统的运动控制器。同时由于通用的PC平台拥有良好软件开发环境，并且具有丰富的通信接口，例如以太网、串口、USB接口等，因此控制系统的人机接口设备选择了PC，也就是控制系统的上位机。综上所述，下料工业机器人控制系统采用了基于“PC+DSP+FPGA”的架构[3-5]。图1是五自由度工业机器人控制系统总体硬件框图。系统主要由上位机(PC)、DSP控制模块、FPGA采集模块、驱动器、伺服电机以及减速器等构成。

图1 工业机器人控制系统总体硬件框图

用户可以在上位机示教和检测机器人的运动状态，在示教过程中上位机的作用相当于示教器。上位机通过以太网将机器人的作业顺序、位置和速度信息发送给DSP控制模块，DSP控制模块将这些示教信息保存起来。示教结束后用户在上位机上选择轨迹的生成方式命令发送给DSP控制模块后由其生成相应的运动轨迹；在执行工作任务时，机器人可以按照DSP生成的轨迹进行运动。

2 控制系统软件设计

控制系统的软件由DSP控制程序、FPGA采集程序和数据通信程序以及上位机的人机界面程序4个部分组成。控制系统软件结构如图2所示。DSP控制程序主要完成单关节运动控制、运动学解算、轨迹规划、轨迹生成和参数设置等功能，以上功能的实现是用C语言在CCS3.3集成开发环境上编写调试的。FPGA采集程序是完成对五路伺服电机的增量式光电编码器信号的采集功能，其功能是用Verilog硬件描述语言在ISE 13.4集成开发环境上编写实现。数据通信程序主要完成DSP与上位机的通信、DSP与FPGA的数据通信和DSP与交流伺服电机驱动器的通信。人机界面程序主要是完成对机器人示教再现以及显示机器人运行状态的功能。上位机为一台安装Windows 7操作系统的计算机，在Visual Studio 2010集成开发环境下进行开发和调试，用C#语言实现了上位机的人机界面程序。

图2 控制系统软件结构图

3 DSP控制模块软硬件设计

DSP控制模块的硬件电路设计包括DSP最小系统和通信电路的设计。DSP应用系统的最小系统一般包括：电源电路、时钟电路、复位电路、DSP芯片和JTAG接口等其他电路。通信电路主要包括DSP与FPGA的双端RAM通信、DSP与驱动的CAN通信和DSP与上位机的网络通信电路。

DSP程序是整个控制系统软件的最重要部分，接收来自上位机人机界面发送的指令后，通过DSP计算出每个时刻各个关节角的给定值发送给交流伺服电机驱动器驱动机器人完成上位机的指令。DSP的程序主要有单关节运动控制、运动学解算、关节空间轨迹规划、笛卡儿空间轨迹规划以及轨迹生成等子程序。为了使DSP程序逻辑更加清晰同时方便代码的维护，因此在编写以上子程序时分别在不同的C文件中实现上述功能，同时为每个C文件编写与之对应的头文件，这只需在主程序中包含以上头文件就可以了，使程序结构简单清晰，方便修改和维护。DSP主程序流程框图如图3所示。

图3 DSP主程序流程框图

DSP上电系统复位后，进行系统初始化、GPIO初始化、重新映射外部中断3和外部中断4、初始化外设主要是W5300网口管理模块和eCAN模块初始化。然后进入循环检查W5300网口模块是否连接，若socket已经成功连接，则允许外部中断3和外部中断4，然后循环等待外部中断；若socket没有连接成功，则禁止外部中断3和外部中断4，然后循环等待socket连接。本文中的外部中断3选用GPIO62引脚作为触发源，外部中断4选用GPIO63引脚作为触发源；在设计硬件电路时将DSP的GPIO62引脚和GPIO63引脚通过PC104接口与FPGA的引脚相连，由FPGA的引脚作为触发源；同时需要在软件中初始化配置GPIO62引脚和GPIO63引脚[6]。代码如下：

GpioCtrlRegs.GPBMUX2.bit.GPIO62 = 0; // GPIO62作为通用输入输出

GpioCtrlRegs.GPBDIR.bit.GPIO62 = 0; //GPIO62作为输入

GpioCtrlRegs.GPBQSEL2.bit.GPIO62 = 1; // 输入量化限定3个采样周期

GpioIntRegs.GPIOXINT3SEL.bit.GPIOSEL = 62; // GPIO62为外部中断XINT3的中断源

XIntruptRegs.XINT3CR.bit.POLARITY = 1; //上升沿触发中断

XIntruptRegs.XINT3CR.bit.ENABLE = 1; // XINT3 中断允许

GpioCtrlRegs.GPBMUX2.bit.GPIO63 = 0; // GPIO63作为通用输入输出

GpioCtrlRegs.GPBDIR.bit.GPIO63 = 0; //GPIO63作为输入

GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 1; //输入量化限定3个采样周期

GpioIntRegs.GPIOXINT4SEL.bit.GPIOSEL = 63; // GPIO63为外部中断XINT3的中断源

XIntruptRegs.XINT4CR.bit.POLARITY = 0; //下降沿触发中断

XIntruptRegs.XINT4CR.bit.ENABLE = 1; //XINT4 中断允许

GpioCtrlRegs.GPBCTRL.bit.QUALPRD3 = 0x03; // 中断周期为：2* QUALPRD3*Tsys

// QUALPRD3控制GPIO63～56引脚

按照以上硬件和程序的设计，外部中断3和外部中断4的触发源是以FPGA产生的时钟作为时基，在程序中可以设置中断时间间隔来定时触发外部中断3和外部中断4。这里设计编码器的采样周期为外部中断时间。当该中断时钟的上升沿到来时，触发外部中断3进入中断服务子程序中，在外部中断3服务子程序中接收来自上位机的指令，读取FPGA采集的数据并且根据相应的指令调用不同的子程序来计算处理，然后将处理的结果封装成驱动器的CAN协议指令格式，存储在指定的位置后返回外部中断3服务子程序。当该中断时钟的下降沿到来时，触发外部中断4进入中断服务子程序中，在外部中断4服务子程序中将外部中断3服务子程序中生成的驱动器的指令发送给驱动器，然后将FPGA采集的数据发送给上位机后返回外部中断4服务子程序。

4 FPGA采集模块软硬件设计

FPGA采集模块的硬件电路设计包括FPGA最小系统和编码器采集电路设计。其中FPGA最小系统主要包括：外部时钟、复位电路、电源电路、下载电路、FPGA芯片和JTAG接口电路等。编码器采集电路主要对五路伺服电机的增量式光电编码器信号进行采集。

FPGA采集程序是用Verilog硬件描述语言在ISE13.4软件开发平台编程实现的。FPGA数据采集程序主要包括同步时钟产生模块和五路增量式光电编码器采集模块。由于DSP的外部中断3和外部中断4中断的基准时钟信号是由FPGA产生的，也就是这里的FPGA产生的同步时钟信号。因此它不仅是DSP与FPGA双端RAM通信基准时钟，也是DSP控制器与上位机通信的基准时钟。由于本文所选择的交流伺服电机驱动器的电流环控制频率为10 kHz，因此同步时钟的频率也选择为10 kHz；而位置环的控制频率和增量式光电编码器的采集频率均采用1 kHz。在硬件电路设计时为FPGA工作时钟选为50 MHz，因此可以非常简单地通过Verilog语言编程对50 MHz分频得到10 kHz的同步信号，由于其程序比较简单，这里不再详细地介绍如何产生同步时钟信号。

本文所选的SEM交流伺服电机集成了增量式光电编码器输出的A、B、Z 3对差分脉冲信号，将3对差分信号经过单端信号转换芯片转换为3路单端信号A、B、Z送入FPGA芯片。其编码器的输出波形如图4 所示。

图4 增量式光电编码器输出波形

在FPGA采集程序中，通过判断A和B的相位关系确定伺服电机的转动方向；然后对A和B的脉冲计数来确定伺服电机转过的角度，并且在检查到Z相脉冲时对计数器清零。所以FPGA采集程序主要包括2个模块：滤波模块和解码计数模块。FPGA采集程序结构框图如图5所示。

图5 FPGA采集程序结构框图

设计滤波模块主要是消除外界环境产生的噪声信号对编码器信号的影响。该程序中的滤波模块主要是滤除高频噪声对编码器信号的干扰，就是将高于编码器正常工作时产生的脉冲频率的脉冲信号除去。

对编码器信号进行滤波后，信号进入解码计数模块。编码器输出的是正交脉冲信号，因此这里对编码器的A、B信号进行正交解码得到计数信号。计数信号是在A相和B相上升沿和下降沿各自产生一个脉冲信号，因此计数信号的频率为A相、B相频率的4倍。然后对其计数从而提高了编码器的测量精度。

在解码计数模块中将编码器的输出脉冲高电平用1表示，低电平用0表示。如图4所示，一个信号周期中AB有4组状态：10、11、01、00。由于A、B信号是正交脉冲信号，当电机正向转动时A相的相位超前B相90°，则AB两相的状态变化依次为：00、10、11、01、00；，当电机反向转动时B相的相位超前A相90°，则AB两相的状态变化依次为：00、01、11、10、00；因此可以根据AB两相的当前状态和上一状态的变化关系判断电机的运动：正向运动、反向运动、静止不动、错误4种结果。详细结果见表1。

表1 AB状态变化关系

在解码计数模块中按照表1对A和B两相的相位关系进行解码得到电机转动的方向信号。编码器采集程序具体实现过程如下[7]：首先将输入信号A和B组合为1个位宽为2位的wire型信号AB；然后在解码计数模块中定义2个位宽为2位的reg型变量cur_state和pre_state，分别存储上一状态和当前状态，并且将它们初始化为cur_state=2’b00、pre_state=2’b00。如果当前状态和上一状态一样时，则电机处于静止不动状态；当A或者B信号发生变化时，将当前状态存储在cur_state寄存器中，并且根据表1判断电机的转动方向，然后计数器模块进行相应的加1或者减1运算，并且将上一状态pre_state更新为当前状态cur_state；同时如果检测到Z相脉冲信号，则对计数器进行回零操作，并将过零计数器加1表示电机旋转了1圈。等待A、B、Z信号的变化触发下次计数，这样反复进行就可以完成对编码器信号的采集。

5 数据通信程序设计

5.1 DSP与驱动器CAN通信

DSP向驱动器发送信息的流程图如图6所示。首先对eCAN模块初始化，并且清除发送请求寄存器CANTRS，然后根据通讯协议初始化邮箱的标识寄存器MSGID；将协议中的数据分别写入消息数据寄存器CANMDL和CANMDH中后置位相应的TRS(1为启动发送，0为无操作)。等待发送应答寄存器相应的TA置为；最后复位发送应答寄存器相应的TA等待下次发送数据。

图6 CAN发送信息流程图

5.2 DSP与FPGA双端RAM通信

程序的设计思路是FPGA采集完所有的数据后存放在片内SRAM指定的地址中，其存储空间为4 K字，地址范围是0x004000～0x004FFF，并且片选信号为XZCS6，因此将此处的存储空间映射到了区域0。工作原理为：同步时钟是DSP与FPGA双端RAM通信基准时钟，其时钟的频率为10 kHz；由于增量式光电编码器的采集频率为1 kHz(也就是编码器的计数信号的存储频率)，因此需要将同步时钟分频得到1 kHz的编码器的采集时钟。而位置环的控制频率也是1 kHz，即外部中断的频率。当以同步时钟为基准时钟设定的中断定时到来时，将触发外部中断3和外部中断4；进入外部中断子程序，在这里DSP首先使能片选信号XZCS6，然后给出上述指定的地址，当读信号到来时，读取FPGA对5路编码器的采集数据。由于控制频率和编码器采集频率都是1 kHz，为了保证当读编码器的采集数据时，FPGA能够完成对5路编码器数据的采集并且存放在指定的地址中。所以这里在第k时刻读取第k-1时刻采集的数据。但是这样就造成电机的控制和上位机的数据显示造成一个时钟周期的延时，由于时间较短引起的延迟可以忽略。

5.3 DSP与上位机通信

DSP通过网络通信与上位机进行数据的传输。网络协议采用TCP传输控制协议，TCP是有连接的、可靠的传输协议。其中DSP作为服务端，上位机作为客户端。图7为DSP与上位机通信流程图，其中图7(a)上位机客户端程序通信程序流程图，图7(b)DSP服务端程序通信程序流程图。在网络通信中，DSP服务端需要首先开启网络服务；首先将DSP端的网络芯片W5300进行初始化，为其硬件地址、网关地址、子网掩码、网关地址、IP地址设置好相应的地址，并且配置TX和RX存储器的大小，以及将SOCKET0设置为TCP模式然后打开此端口。然后调用SOCKET_Listen(0)函数检测SOCKET0是否有上位机连接，循环等待上位机客户端连接。上位机客户端根据DSP服务端的IP地址和端口号设置异步通信回调函数AsynCallback()和实例化客户端TcpClient；通过判断回调函数返回的结果判断socket是否连接成功。由上位机客户端发起连接请求，如果DSP和上位机连接成功，则将连接标志位con_flag置为1。然后在DSP服务端调用数据接收函数Rx_Process()和数据发送函数Tx_Process()进行数据传输，在上位机客户端调用数据接收函数BinaryReader.Read()与数据发送函数BinaryWriter.Write()进行数据传输。若连接断开，则在服务端将SOCKET0关闭并将con_flag置为0；在客户端将实例化的客户端TcpClient关闭并将con_flag置为0。

图7 与上位机通信程序流程图

6 人机界面

上位机界面主要实现的功能是显示下料工业机器人每个关节的实时状态、对机器人进行示教、轨迹生成以及控制机器人的运行和停止等。如图8上位机人机界面所示。首先在通信区域单击“建立连接”按钮，向DSP服务端发送连接请求，如果连接成功，则在相应的textBox显示服务器的IP地址：192.168.0.20和端口号：50000。否则显示未连接。当连接成功时则DSP服务端将每个关节的实际位置发送给上位机，在显示关机实时位置区域中实时更新每个关节的实际位置。在示教区域单击相应关节的“正转”、“反转”，上位机则发送相应的指令给DSP，然后发送给相应关节的驱动器驱动关节运动；如果有单击“记录”按钮，上位机则发送记录指令，DSP记录此时各个关节的关节变量值并保存在指定位置；在示教结束前选择轨迹生成方式，单击示教“结束”按钮，上位机则将示教结束指令和轨迹生成方式发送给DSP，然后DSP按照记录的示教点和轨迹生成方式生成机器人的运动轨迹，并且将运动轨迹标志位置为1，即Traj=1。当有单击执行区域的“运行”按钮，则上位机发送运行指令给DSP，DSP然后去判断运动轨迹标志位，若Traj=1则DSP将之前生成的轨迹数据发送给驱动器驱动机器人运动，若Traj=0则机器人静止不动；当有单击执行区域的“停止”按钮，则上位机发送停止指令给DSP，DSP停止给驱动器发送轨迹数据。

图8 上位机人机界面

7 结语

设计了基于“DSP+FPGA+PC”架构的控制系统。然后用本文开发的控制系统和自主设计的工业机器人本体机械结构组成的五自由度工业机器人，并进行示教实验，实验结果表明初步实现了注塑机下料任务。同时为了使控制系统具有一定的开放性，在软件设置中可以对不同的机器人机械本体通过参数修改实现控制，并且在电路设计时预留了一些接口，例如：串口、6路编码器采集，6路电流采集等，方便以后扩展。