CN103358309A - 一种基于以太网的机械手运动控制系统 - Google Patents

Abstract

一种基于以太网的机械手运动控制系统，包括基于工控机的上层模块和以ARM处理器为核心的下层运动控制器模块，所述上层模块与所述下层运动控制器模块之间通过以太网方式进行通信连接；所述上层模块包括工控机，所述下层运动控制器模块包括ARM处理器、以太网通信接口、I\O口、外部存储器、驱动模块和步进电机，所述ARM处理器与以太网通信接口、I\O口、外部存储器、驱动模块连接，所述以太网通信接口与所述以太网连接，所述驱动模块与电机连接。本发明提出了一种实时性良好、高性能高精度的基于以太网的机械手运动控制系统。

Description

一种基于以太网的机械手运动控制系统

技术领域

本发明涉及机械手运动控制领域，具体涉及一种机械手运动控制系统。

背景技术

目前，国内机械手行业发展迅速，随着生产力的发展和装备技术的进步，对机械手控制技术的要求越来越高，传统结构的机械手控制系统已不能满足对机械手实施高性能控制的要求。高性能的运动控制系统需实时的通过运动规划和插补，将理想的运动曲线转变为电机的运动。控制单元与伺服驱动单元之间需要传输大量的数据，但是目前的机械手控制系统大多采用串口、运动轴卡的方式来连接，这种连接方式存在传输距离有限、数据交换速率低、存在信息交换瓶颈等缺点，所以插补数据一般只能通过离线方式发送给运动控制器，大大影响了机械手控制系统精度和性能的提升。近年来，网络技术发展迅速，商用以太网以其高速度、低成本、技术成熟等特点也慢慢走向了运动控制领域，运动控制技术与网络技术的相结合代表着未来运动控制的发展方向。

发明内容

为了克服目前机械手控制系统的传输速率低，不能满足高速高精度运动控制的需求等问题，本发明提出了一种实时性良好、高性能高精度的基于以太网的机械手运动控制系统。

本发明解决其技术问题所采用的技术方案是：

一种基于以太网的机械手运动控制系统，包括基于工控机的上层模块和以ARM处理器为核心的下层运动控制器模块，所述上层模块与所述下层运动控制器模块之间通过以太网方式进行通信连接；

所述上层模块包括工控机，所述工控机用以完成人机交互、输入数据处理和与下位机通信，所述下层运动控制器模块负责与上位机进行通讯、进一步处理加工数据、伺进行服控制、I\O模块输入输出等；

所述下层运动控制器模块包括ARM处理器、以太网通信接口、I\O口、外部存储器、驱动模块和步进电机，所述ARM处理器与以太网通信接口、I\O口、外部存储器、驱动模块连接，所述以太网通信接口与所述以太网连接，所述驱动模块与电机连接。

进一步，所述工控机中，对输入的数据进行运动轨迹规划，将复杂的运动曲线分为直线段和曲线段，实现粗插补运算，并通过实时高速以太网将粗插补数据发送给层运动控制器模块，在所述ARM处理器中进行精插补运算，实现微小直线和圆弧的插补，然后输出数据到驱动模块，从而控制机械手运动。

再进一步，所述下层运动控制器模块还包括编码器输入接口和编码器模块，所述ARM处理器与编码器输入接口、编码器模块连接，所述编码器输入接口与所述编码器模块连接，所述电机与所述编码器模块连接。

更进一步，通过所述编码器模块，对机械手实际运动位置和速度进行采集，机械手的位置、速度信息通过以太网实时的传回工控机，实时调整控制参数，实现机械手闭环控制。

在ARM处理器上搭载嵌入式实时操作系统μC/OS-II，并通过在μC/OS-II上移植开源的TCP/IP协议栈LwIP来实现。

在以太网MAC层上增加一个用以按报文紧急程度建立调度表的实时调度层。

本发明的有益效果主要表现在：将系统插补需要的大量运算一部分交给了上位机处理，精插补和最终插补命令的执行交给底层ARM实现，充分利用实时以太网高速实时大容量的数据传输能力，将上层位计算机强大的处理能力和下层ARM微处理器精确的强实时控制能力相结合，大大提高了机械手运动控制系统的速度和精度。此外，本发明可以很好的实现远程控制，使工控机远离恶劣的工业生产环境，提高系统可靠性，为生产操作人员提供良好的工作环境。

附图说明

图1是本发明的整体结构示意图。

图2是本发明控制系统软件结构示意图。

图3是本发明两级插补原理框图。

图4是本发明以太网通信模型。

具体实施方式

下面结合附图对本发明作进一步描述。

参照图1～图4，一种基于以太网的机械手运动控制系统，包括基于工控机的上层模块和以ARM处理器为核心的下层运动控制器模块，所述上层模块与所述下层运动控制器模块之间通过以太网方式进行通信连接。

所述上层模块包括工控机，所述工控机用以完成人机交互、输入数据处理和与下位机通信，所述下层运动控制器模块负责与上位机进行通讯、进一步处理加工数据、伺进行服控制、I\O模块输入输出等；

所述下层运动控制器模块包括ARM处理器、以太网通信接口、I\O口、外部存储器、驱动模块和步进电机，所述ARM处理器与以太网通信接口、I\O口、外部存储器、驱动模块连接，所述以太网通信接口与所述以太网连接，所述驱动模块与电机连接。

ARM微处理采用STM32F407控制器，该MCU主频可达168M，支持单周期DSP指令和浮点单元，能够很好完成数据插补，产生驱动电机的脉冲信号，可代替传统的DSP，节约成本，控制方便。STM32F407内含MAC控制器，配备DM9161A以太网物理接口芯片后，可实现以太网通信功能。

参照图2，该图为本发明控制系统软件结构示意图

用户软件为运行于系统上层工控机的应用软件，为上位机软件，完成人机交互功能。上位机对于用户输入的数据进行处理，当数据处理程序将一个程序数据段处理完毕后，控制系统就知道了这段曲线的种类、起点、终点、以及进给速度值。从而可在线段的起点和终点之间进行粗插补工作。然后上位机将粗处理的数据通过高速以太网发送到下位机的缓冲区，下位机从缓冲区中读取数据，一边进行精插补一边将插补结果输出，产生脉冲到驱动器实现对电机的精确控制。以太网通信功能的实现是在ARM平台上搭载嵌入式实时操作系统μC/OS-II，并通过在μC/OS-II上移植开源的TCP/IP协议栈LwIP来实现。

在μC/OS-II系统中我们通过建立相应的任务完成通信数据处理、，通信指令处理、插补算法模块、以太网通信等功能，其中插补运算模块的优先级最高，其次是通信数据处理模块和通信指令处理模块，最后是以太网通信模块。插补运算模块的任务定义每8ms处理一次，即定义该任务的挂起时间是8ms，提供较高的实时性。

参照图3，该图为两级插补原理框图。上层模块的粗插补采用数据采样插补算法中的时间分割法。下层模块采用DDA(数字积分法)圆弧插补。

设有一段运动轨迹P(t)，通过曲线拟合将P(t)分为ｎ个直线段和m个圆弧段。拟合后曲线

C(t)为拟合后曲线，L_i为第i条直线段，R_i为第i条圆弧段。

对于直线段用时间分割直线法进行粗插补，取其中一条直线段L_i,长为L，起点坐标（0,0），终点坐标（x_e，y_e）。设粗插补的插补周期为T_S，要求的进给速度为V_l，则可以得到进给步长为ΔL：

$\mathrm{\ΔL}=\frac{V_l{T}_s}{60}---\left(1\right)$

进而得到该插补周期内X，Y轴对应的位置增量Δx_i，Δy_i分别为：

${\mathrm{\Δx}}_i=\frac{\mathrm{\ΔL}}{L}{\·x}_e---\left(2\right)$

${\mathrm{\Δy}}_i=\frac{\mathrm{\ΔL}}{L}\·y_e---\left(3\right)$

因此可以得到插补点坐标（x_i，y_i）：

x_i＝x_i-1+Δx_i               （4）

y_i＝y_i-1+Δy_i               （5）

根据以上方法得到L_i上的n-1个插补点坐标，及其对应的n条插补线段：

对于圆弧插补的方式，其原理跟直线插补时类似的：

设圆弧当前点坐标为P_i（X_i，Y_i），下一插补点P_i+1（X_i+1，Y_i+1），圆弧P_iP_i+1对应的中心角为θ。根据如下公式我们可以得到圆弧上的插补点坐标。（X_i，Y_i）

X_i+1＝X_icosθ-Y_isinθ                    （6）

$Y_{i+1}=\±\sqrt{R^2-{X_{i+1}}^2}---\left(7\right)$

通过以太网将这些插补点坐标下发，下层控制器将其存入数据缓冲区，控制器从缓冲区中读取数据进行精插补运算。

精插补采用DDA(数字积分法)。对于直线段：取上层发下来的其中一个小直线段l₁，起点坐标（0，0），终点坐标为（x₁，y₁）。在内存中设定几个单元，分别用于存放x₁及其累加值Σx₁和y₁及其累加值Σy₁。Σx₁和Σy₁赋初值，在每次插补循环过程中，进行以下求和运算：

Σx₁+x₁→Σx₁，Σy₁+y₁→Σy₁

若Σx₁大于2^N则溢出脉冲1，否则为0；Σy₁大于则溢出脉冲1，否则为0，其中的N为累加寄存器的位数，进而将其作为脉冲输出来控制机床进给，就可走出所需的直线轨迹。

对于圆弧插补，与直线插补类似，但是坐标X_i，Y_i存入寄存器的对应关系与直线不同，恰好位置对调，X、Y方向插补时分别对X_i，Y_i进行累加。当X、Y方向了发送进给脉冲后，要修正（X_i，Y_i）的值。当X方向发脉冲后，Y轴累加器内容减1，当Y轴方向发脉冲后，X轴累加器内容减1。

参照图4，该图为以太网通信模型。传统以太网采用冲突检测载波监听多点访问（CSMA/CD）机制来解决通讯介质层的接入，并采用重发机制进行差错控制。以太网的这种随机竞争接入机制导致了非确定性的产生。以太网的这种通信不确定性会导致系统控制性能下降，控制效果不稳定，甚至会引起系统振荡；在有紧急事件发生时，还可能因报警信息不能得到及时响应而导致灾难事故的发生。所以本系统在以太网MAC（媒体接入控制器）层上增加一个实时调度层，按报文紧急程度建立调度表，保证报警、急停等非周期紧急数据优先处理。对于周期性插补数据。当数据包进入实时调度层，数据用队列进行缓冲。本系统对周期性数据和非周期性数据分别定义了缓冲队列。队列遵循FIFO的原则，主要采用指针数组的循环队列方式实现，数组中的指针指向等待发送的数据。对于在缓冲队列中的数据，将根据数据的紧急程度，按照调度表发送数据包。

Claims (6)

1.一种基于以太网的机械手运动控制系统，其特征在于：包括基于工控机的上层模块和以ARM处理器为核心的下层运动控制器模块，所述上层模块与所述下层运动控制器模块之间通过以太网方式进行通信连接；

所述上层模块包括工控机，所述下层运动控制器模块包括ARM处理器、以太网通信接口、I\O口、外部存储器、驱动模块和步进电机，所述ARM处理器与以太网通信接口、I\O口、外部存储器、驱动模块连接，所述以太网通信接口与所述以太网连接，所述驱动模块与电机连接。

2.如权利要求1所述的基于以太网的机械手运动控制系统，其特征在于：所述工控机中，对输入的数据进行运动轨迹规划，将复杂的运动曲线分为直线段和曲线段，实现粗插补运算，并通过实时高速以太网将粗插补数据发送给下层运动控制器模块，在所述ARM处理器中进行精插补运算，实现微小直线和圆弧的插补，然后输出数据到驱动模块，从而控制机械手运动。

3.如权利要求1或2所述的基于以太网的机械手运动控制系统，其特征在于：所述下层运动控制器模块还包括编码器输入接口和编码器模块，所述ARM处理器与编码器输入接口、编码器模块连接，所述编码器输入接口与所述编码器模块连接，所述电机与所述编码器模块连接。

4.如权利要求3所述的基于以太网的机械手运动控制系统，其特征在于：通过所述编码器模块，对机械手实际运动位置和速度进行采集，机械手的位置、速度信息通过以太网实时的传回工控机，实时调整控制参数，实现机械手闭环控制。

5.如权利要求1或2所述的基于以太网的机械手运动控制系统，其特征在于：在ARM处理器上搭载嵌入式实时操作系统μC/OS-II，并通过在μC/OS-II上移植开源的TCP/IP协议栈LwIP来实现。

6.如权利要求1或2所述的基于以太网的机械手运动控制系统，其特征在于：在以太网MAC层上增加一个用以按报文紧急程度建立调度表的实时调度层。

Images