一种自动码垛机器人控制系统的制作方法

本发明涉及码垛机器人技术领域，尤其涉及一种自动码垛机器人控制系 统。

背景技术：

随着人力成本的上升，工业机器人自动码垛受到越来越多的重视。目 前国内的四轴码垛机器人市场主要由瑞士的ABB,德国的KUCA，日本的安 川和发那科所占有。

机器人控制系统是机器人核心组件，负责调度机器人的运行。国内自 主研发的机器人控制系统已投入市场的应用的比例非常少，大部分机器人 公司都采用国外成熟系统。国外通用的机器人控制系统，没有针对码垛进 行深度开发，因此需要操作人对机器人的功能特别熟悉。机器人相对其他 的工业设备来讲，功能复杂，需要编程实现码垛操作，掌握机器人的使用 功能门槛较高。

技术实现要素：

有鉴于此，本发明要解决的技术问题是提供一种自动码垛机器人控制 系统，能够降低机器人的使用门槛，操作简单、方便实用，满足工厂、车 间的自动码垛需求。

本发明的技术方案是这样实现的：

一种自动码垛机器人控制系统，包括示教器模块、机器人程序语言解析 模块、运动控制模块、IO模块、电机解码模块；

所述示教器模块包括输入单元和显示单元；所述输入单元包括机器人的 基本操作指令和自动码垛设置界面；所述显示单元用于显示所述码垛机器人 的工作状态，包括伺服的状态、IO状态和相关设备的运行信息；

所述机器人程序语言解析模块用于自定义机器人编程语言的语法结构； 检查机器人程序语言的语法错误；对符合正确语法规则的机器人程序，生成 对应的符号表和语法树；

所述运动控制模块包括通信单元、算法单元；所述通信单元用于与其他 模块进行数据交换；所述算法单元用于进行DH模型建立、关节角度正逆解 的求解和轨迹规划的插值运算；

所述电机解码模块用于根据所述轨迹规划程序和所述算法单元计算的插 值数据控制电机输出；

所述IO模块用于输入信号口状态，发送机器人输出状态至所述示教器模 块。

优选的，所述示教器模块包括基本的用户输入、机器人状态输出和自动 设置码垛界面：

用户输入包括：机器人程序编程框、机器人操作命令输入、摇杆操作、 机器人位置示教、工件坐标系示教、机器人校零、用户权限设置、时间设置；

机器人的状态输出包括：机器人的当前笛卡尔坐标值、关节角度、伺服 状态、机器人运行信息；

自动码垛设置界面包括：信号信息、垛位信息、品种信息；用户输入码 垛信息参数，以文件流的形式发送给运动控制模块，生成符合码垛现场的机 器人程序。

优选的，所述机器人程序语言解析模块包括前端、中间层和后端；

所述前端用于扫描程序字符串，将字符串标注为常量、变量、程序函数 名、数据类型名、关键字；

所述中间层用于将程序语言形成完整的语法树和符号表；

所述后端用于接收用户指令，遍历或阻塞或退出，语法树和查找相应的 符号表，逐步执行或挂起或退出中间层生成的机器人程序；

针对码垛系统，在解析器上设计永久变量、安全回原点、光幕信号监控 函数。

优选的，所述运动控制模块设有大负载机器人运动的轨迹规划算法。

优选的，所述IO模块，采用R485通信；为了保证IO通信的可靠性，在 通信数据上加入了校验位；为了滤除杂波信号，对数据提取进行了加窗操作。

本发明提出的自动码垛机器人控制系统，编程单元预先封装有轨迹规划 程序，当示教器模块输入码垛机器人工作参数后，可以自动生成轨迹规划程 序规划机器人的码垛轨迹，从而无需操作者进行编程，能够降低机器人的使 用门槛，操作简单、方便实用，满足工厂、车间的自动码垛需求。

附图说明

图1为本发明实施例提出的自动码垛机器人控制系统的结构框图；

图2为自动生成程序的结构流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行 清楚、完整地描述。

如图1所示，本发明实施例提出了一种自动码垛机器人控制系统，包括 示教器模块1、机器人程序语言解析模块2、运动控制模块3、IO模块4、电 机解码模块5；

示教器模块1包括输入单元和显示单元；输入单元用于输入码垛机器人 工作参数；显示单元用于显示码垛机器人的工作状态；

机器人相对于其他工业设备来讲，操作具有一定的复杂性，一般都是通 过示教编程来完成机器人运动的往返操作。需要使用者熟悉机器人的编程语 言、语法格式。然后用机器人程序语言将整个的码垛作业的逻辑关系，准确 的反应到程序中来。为了降低操作门槛，本发明在示教器输入端设置了自动 码垛输入界面。使用者设置码垛参数，如信号设置、垛位设置、品种设置等， 示教器将这些以xml文件的格式发送给运动控制模块。运动控制模块解析xml 文件，获取码垛的相关参数。根据码垛作业的抓取、抬高、旋转、放置等基 本动作流程，设置信号、垛型等。轨迹动作根据抓取点位置、放置点位置合 理计算，保证机器人的运行效率较高。自动生成程序的结构流程如图2所示。

其中过程定义就是各子函数定义，包括打开抓手、关闭抓手、托盘数据 定义、产品数据定义、获取产品、放置产品、计数、返回抓取线、优先抓取 点设置等码垛流程。

其中在获取产品、放置产品、返回抓取线的轨迹位置点的确定是通过抓 取点高度、物料的高度、放置托盘的工件坐标系高度、层数等信息计算得出。 这样确定的轨迹相对于手动编程的轨迹更为合理，整体码垛的速度会变快。 自动生成程序的逻辑在树形结构封装，这样避免了手动编程可能出现的逻辑 错误。此外，对于用户而言，只需要输入码垛的一些参数，就能生成码垛程 序，节省了大量的时间。

机器人程序语言解析模块2用于对机器人程序语言进行解析，构造为相 应的语法树和符号表；

前端负责扫描程序字符串，将字符串标注为常量、变量、程序函数名、 数据类型名、关键字等。

中间层将程序语言形成完整的语法树和符号表。程序语言的逻辑结构采 用树结构表示，每个子模块就是树的分支。不同的控制结构的语法树的逻辑 结构不同，如循环WHILE结构：WHILE由前端标注为关键字，生成一个树 节点，类型标注为循环，然后检查WHILE后面的表达式，表达式存储为循环 节点下的一个判断类型子节点；然后检查前端标注为关键字的DO，DO后面 的程序存储为循环节点下的一个过程类型的子节点。涉及到节点中的数值信 息通过生成的符号表查询，这样一层层构建，整个程序的逻辑框架就通过语 法树的结构存储起来了。

针对码垛系统，还可以设计一些特殊函数格式，如 movePallet,moveHomeSafe,setLoadWeight等。movePallet为系统函数，意义为 移动托盘，用户需要换品种的时候，调用这个函数，托盘数据就自动清空， 非常方便使用。moveHomeSafe为安全回原点，用户调用这个函数机器人就 自动回原点。码垛系统经常是需要记忆以往数据，如下个班次需要继续上个 班次继续码垛，机器人就需要掉电保存上个班次数据的功能。通过将需要掉 电保存的数据，标注为persistentInt。前端标注为persistentInt类型的数据，存 储在文件中，然后符号表通过找到相应文件取出相应数据。后端执行过程中 改变persistentInt数据，也相应的写到磁盘文件中。

后端通过中间层构建好的语法树和符号表中数据，根据示教器输入的指 令遍历或挂起或退出语法树，一步步的执行或暂停或停止程序。

运动控制模块3包括通信单元、编程单元和算法单元；通信单元用于与 示教器模块、机器人程序语言解析模块、IO模块和电机解码模块进行数据交 换；编程单元预先封装有轨迹规划程序，用于根据码垛机器人工作参数规划 运动轨迹；算法单元用于进行DH模型建立、关节角度正逆解的求解和轨迹 规划的插值；

运动控制算法包括DH模型的建立，关节角正逆解求解和轨迹规划算法。 其实现过程如下：

建立DH参数如下表：

其中：T60＝T10*T21*T32*T43*T54*T65

则有如下

其中:

px＝c1a5+c1c23a3+c1c2a2+c1a1

py＝s1a5+s1c23a3+s1c2a2+s1a1

pz＝-s23a3-s2a2+d1-d6

其中Px、Py、Pz就是末端坐标值。

L＝T01*T60＝T61＝R

T61＝T31*T63

根据R(2,4)＝0。得到：

-s1px+c1py＝0 (1)

所以

θ1＝arctan(py/px)+npi

根据：L(1,4)＝R(1,4)L(3,4)＝R(3,4)可得到：

c23a3+c2a2＝c1px+s1py-a1-a5 (2)

s23a3+s2a2＝d1-pz-d6 (3)

根据(2)的平方+(3)的平方+(1)的平方可以解得：

2a2a3c3＝(c1px+s1py-a1-a5)²+(d1-pz-d6)²-a3²-a2²

得到：

根据：L＝T03*T60＝T63＝R

T03＝T23*T12*T01

T63＝T43*T54*T65

根据L(1,4)＝R(1,4)，L(2,4)＝R(2,4)得到如下两式：

c23c1Px+c23s1Py-s23Pz+s23d1-c23a1-c3a2＝c23a5+s23d6+a3 (4)

-s23c1Px-s23s1Py-c23Pz+c23d1+s23a1+s3a2＝-s23a5+c23d6 (5)

解得方程如下：

θ2＝arccos(K)-θ3

上叙为机器人的正逆解过程。

通用机器人控制算法一般以时间最优或者能量最优，对轨迹进行数据训 练，得到最优的插值数据。这种方法训练数据时间较长，在4ms的数据交互 周期内难以满足实时性要求。为了满足实时性，也有采用三次样条曲线进行 估计规划，这种算法优点在于计算速度快，但只对速度、加速度进行了限制， 没考虑机械末端的震动。码垛机器人因为负载大，末端惯性比较大，所以必 须对末端进行震动抑制，本发明提出了一种震动抑制算法。振动抑制的决定 参数是机械的刚度和运动加速度的变化率,为此设计五次样条曲线,保证机器 人速度、加速度、jerk、jerk的倒数在起始点和结束点都为0。

在机器人轨迹规划过程中，能够知道起始点和末端点的位置，转化为求 解定位移,定Jerk,求解最大加速度和速度的过程。设置加减速度比为 η＝adown/aup(0＜η≤1),首先明确一点，速度是连续的，每段的加速度也 是连续的。设置位移为L。设置最大加速度为amax，最大速度为vmax，首先 根据距离L，求解出加速度。

根据速度相等，选择不同的加减速度比，则加减速度的j也不一样。

从上式可以看到，那么减速度的jerk就变成了加速度Jerk的η²倍了。

A：首先计算L内是否能达到最大加速度；

得：

如果amax＞a,计算速度是否超了最大速度

如果vmax＞v，则表明计算的加速度是允许的。如果不满足上述条件， 表明有匀速段。根据最大速度计算的最大加速度和给定的最大加速度选取一 个较小值am，然后计算匀速段时间。匀速段时间计算:

整个思路如下：

a：通过给定的vmax，计算出能够达到最大加速度的amax1，

令；amax1＝min(amax1,am)

b：根据距离L，计算出没有匀速段的加速度amax2；

c：若amax2＜amax1,表明没有匀速段；

d：若amax2＞amax1，表明有匀速段，匀速段时间是

以上算法保证的了运动过程中保证了速度，加速度、jerk小于一定的值， 并且连续可导；

运动停止的时候，jerk值为0，有效的消掉了冲击力。

该算法有数值解，能够满足是机器人实时性插值的要求，此外对于大负 载码垛机器人，其运动参数的多阶倒数数值都控制在一定范围内，且运动首 尾运动学参数值都为0，运动曲线平滑，非常适合大负载机器人的运动。

在本发明实施例中，为了实现闭环控制，机器人每到达一个点，就通过 电机解码器反馈实际的电机编码器数据，并根据编码器的实际数据，计算出 实际位置和理论位置的误差，然后在下一条语句的执行过程中，将误差进行 补偿，保证运动的精度。

IO模块4用于机器人和外部设备进行IO数据交互。IO板通信采用R485， 每20ms采集输入端口值，发送机器人状态到输出口。为了保证有效性，采用 CRC查表校验方式。针对IO信号，为了防止干扰信号，对IO信号进行了滤 波，具体是对信号进行了加窗操作，窗口步长为3，窗口内信号一致，如果有 杂波就滤除。为了保证数据的可靠性和实时性，设计了校验机制、重发机制、 丢弃机制、请求机制等。

电机解码模块5用于将算法计算数据转成电机输出；电机解码模块根据 算法的插值数据，根据机械减速比和电机的精度，转化为电机的输出。在这 个过程中，要保证各关节轴的同步和误差补偿。同步采用Ethercat总线通讯模 式保证。举例来说，4ms是一个交互周期。算法发送给电机的数据不可能是 电机驱动器最小刻度的整数值，为解决误差问题，电机解码器维护一个数据 栈，将每次不足最小刻度的数值进行累加，然后进行误差保存。因为机器人 数据交互周期为4ms，通过网络通信，算法不能及时将数据发送给电机解码 板。电机解码板的栈存储的数据，有效的保证了数据实时性的要求，保证了 机器人不会出现数据中断的问题。

可见，本发明实施例提出的自动码垛机器人控制系统，当示教器模块输 入码垛机器人工作参数后，可以自动生成轨迹规划程序规划机器人的码垛轨 迹，从而无需操作者进行编程，能够降低机器人的使用门槛，操作简单、方 便实用，满足工厂、车间的自动码垛需求。

针对码垛机器人的轨迹规划提出了一种高效、平滑的轨迹规划算法，降 低了机器人的振动。采用Ethercat总线通讯模式保证各电机轴的协同，保证轨 迹的精度。

针对码垛现场，在机器人程序语言解析器中加入了一些特殊函数和语言 格式，这些是通用机器人所不具备的，方便了现场的调试和使用。

最后需要说明的是：以上所述仅为本发明的实的技术方案，并非用于限 定本发明的保护范围。凡在本发明的精神和原则之内所做的任何修改、等同 替换、改进等，均包含在本发明的保护范围内。