码垛机器人运动学分析和控制系统设计

周根荣, 姜　平, 钟永彦

(南通大学 电气工程学院，江苏 南通　226019)



码垛机器人运动学分析和控制系统设计

周根荣, 姜平, 钟永彦

(南通大学 电气工程学院，江苏 南通226019)

针对一类关节坐标四自由度码垛机器人，对其机械传动结构进行分析，构建其运动学坐标体系；根据码垛生产特点，运动路径规划采用设定点位法，其中一些点位的脉冲坐标通过示教法直接确定，其余点位脉冲坐标则通过正、反运动学求解获取；为降低运动学求解复杂程度，将三维空间坐标体系的求解，先分解到两个平面坐标中单独进行，然后再加以合成；控制系统以PLC为核心，各轴驱动采用高精度交流伺服系统，通过原点回归确定各轴原点，根据目标点位实时计算各轴运动脉冲数和频率，采用绝对定位驱动方式实现机器人各轴联动；实际运行效果表明，运动学求解复杂程度的降低，提高了系统运算速度和实时性，而绝对定位方式可避免相对方式下的累积误差，从而提高了系统的运动精度。

码垛机器人；运动学分析；伺服系统；绝对定位

0　引言

当前，伴随着人力成本上升、装备产业升级等因素，具有高度柔性制造能力的工业机器人已越来越多应用在生产过程中。码垛机器人是一种应用在生产、物流自动化领域的工业机器人，可按照一定模式，实现物料的搬运、存储和装卸[1]，本文介绍一种四自由度码垛机器人的运动学分析和控制系统设计。

1　机器人结构及运动轨迹

一类关节坐标机器人结构如图1所示，有腰部、大臂、小臂、腕部四个自由度。大臂电机运动经丝杆传动实现x轴方向前后运动；小臂电机运动经连动杆以A为支点实现z轴方向旋转运动；腰部电机则以Oy为轴心实现y轴方向旋转运动；腕部电机通常跟随腰部电机做反方向同步运动，以保证夹具处于固定不变的角度。机器人原点设在O点(x轴方向前限位点)，D点为目标点位(夹具中心点)。

图1　机器人结构简图

1.1码垛路径规划

码垛机器人一般采用点位控制法，即保证码垛过程中能准确到达几个设定点位，而相邻点位之间的运动轨迹一般不需要太高精度[2]。一种简单的码垛路径点位示意图如图2所示。

机器人开始时和一次码垛过程结束后，都处在起始点位P，当需要将物品从P2(抓取点)搬运至Q(码垛点)时，可沿着PP1→P1P2→P2P1→P1P→PQ2→Q2Q1→Q1Q→QQ1→Q1Q2→Q2P运动路线，共6个点位，10个阶段。

图2码垛路径点位示意图

之所以在抓取侧和码垛侧各设置一个中间点，是保证机器人抓取、码垛过程中基本上走垂直线，以图2中抓取侧为例，如果不设置中间点，由于起始点

P

和抓取点

P

2

的

x

坐标相同，从

P

→

P

2

过程中，

x

方向电机保持不动，如此在以

A

为支点的

z

方向小臂电机运动下，

P

→

P

2

就会走出图中所示的一条弧线，基于安全生产考虑、以及在有些空间狭窄的作业场合，这种运动轨迹是不允许的，因此通常要设置一个中间点

P

1

(

z

坐标为0)，分成

PP

1

、

P

1

P

2

二段运动轨迹，

PP

1

段时，

x

方向电机后退(

A

→

A

1

)，以保证

PP

1

基本垂直，而

P

1

P

2

段时，

x

方向电机前行(

A

1

→

A

2

)，以保证

P

1

P

2

基本垂直。同理，在码垛侧也设置一个中间点

Q

1

。

1.2点位坐标确定

在码垛过程中，抓取点、起始点通常为固定的一点，可采用示教法直接确定；而码垛点、码垛上方点均有多点(如图3所示，每层3*3个物品，共3层，则有27个码垛点、27个码垛上方点)。

图3　码垛模式示意图

显然，实际操作中是不会逐一采用示教法来确定如此多点位的[3]，而通常是按下步骤来确定(以码垛点为例)：

(1)对1#物品码垛点采用示教法，直接确定各轴脉冲坐标；

(2)根据各轴脉冲坐标，通过反运动学求解，求出1#物品码垛点的距离坐标；

(3)其余物品则根据与1#物品之间的x、y、z距离偏差，先由下式确定出其距离坐标：

(1)

其中：n为码垛物品序号，lx、ly、lz为相邻码垛点在x、y、z方向上距离(通常是定值)。

(4)再通过正运动学求解，由物品距离坐标确定出其脉冲坐标。

之所以先确定距离坐标，再确定脉冲坐标，是由于y、z二轴是旋转运动，且此二轴运动又会影响到x轴坐标(见下文描述)，因而各点的距离坐标和脉冲坐标不是简单的线性关系，要确定码垛侧各点脉冲坐标，必须要通过正运动学求解。

另外，为避免码垛各阶段运动过程中产生累积误差，各点位坐标采用绝对坐标体系，即相对于原点的绝对距离位置(距离坐标)，与此对应，各轴伺服电机采用绝对定位方式，即相对于原点的绝对脉冲位置(脉冲坐标)。

2　正、反运动学求解

为确定码垛侧各点脉冲坐标，需进行正、反运动学求解，下面详细介绍求解过程。

2.1正运动学求解

已知目标点位D的距离坐标(xd,yd,zd)，求解相应的脉冲坐标(xpls,ypls,zpls)。

z、y方向均是旋转运动，不受其它方向运动的影响，因而z、y方向的转动角度可方便求出；但z、y方向的旋转运动都会影响到x方向坐标，因此，具体计算x方向运动行进距离时，可分解到x-z和x-y两个坐标系中，分别计算在x-z和x-y坐标系中的行进距离xz和xy。

另外，为分析方便，目标点D在x-z和x-y两个坐标系中，分别称为Dxz、Dxy。

2.1.1x-z坐标计算

图4为求解示意图，为分析方便，假设x、z方向不是同步运动，而是按如下分两步实现：

图4　x-z坐标正运动学求解

第1步：z方向转动角度θ1，显然：

(2)

其中：ze是计算目标点B’xz的z坐标值。

之所以引入计算目标点B’xz，是由于腕部电机、安装座及夹具部分(图中阴影框部分)与地面始终保持垂直，所以实际目标点Dxz到小臂支点A之间距离不是定值，而是随θ1变化而变化，因此无法作为计算参照，而计算目标点B’xz到A点之间距离是定值(即小臂半径R)，因而计算时采用B’xz点为参照，B’xz与Dxz点之间关系如下：

(3)

其中：Rx、Rz分别为实际目标点Dxz和计算目标点B’xz在x、z方向的距离。

由式(2)、(3)得，z方向电机转动角度θ1为：

(4)

第2步：在第1步基础上，计算x方向运动在x-z坐标系中行进距离xz，显然：

xz=OE-AE=xe-Rcosθ1，因此：

(5)

2.1.2x-y坐标计算

图5为求解示意图，为分析方便，假设x、y方向不是同步运动，而是按如下顺序分两步实现。

图5　x-y坐标正运动学求解

第3步：在上述第1、2步基础上，x方向运动再前行距离xy，使之从Dxz点前行到C点，之所以如此，目的是经第4步y轴沿腰部轴心Oy旋转θ2后，最终满足实际目标点Dxy的y坐标要求。显然，由xy=OyC-OyDxz=OyDxy--OyDxz，可得：

(6)

其中：Rxy是原点O与腰部轴心Oy之间距离。

第4步：y方向电机转动角度θ2，到达目标点Dxy，可得：

(7)

2.1.3x轴运动总行进距离xdis计算

解出xz、xy后，即可得出：

(8)

2.1.4目标点位D脉冲坐标计算

经上述步骤，求得z、y方向电机转动角度θ1、θ2(rad)，及x方向运动的行进距离xdis(mm)后，即可求出目标点D脉冲坐标(xpls,ypls,zpls)：

(9)

其中：zdiv、ydiv为z、y轴电机行进分度值(pls/rad，每弧度对应脉冲数)；xdiv为x轴电机行进分度值(pls/mm，每mm对应脉冲数)。

2.2反运动学求解

已知目标点位D的脉冲坐标(xpls,ypls,zpls)，求解相应的距离坐标(xd,yd,zd)。按如下步骤计算(公式中各符号含义见2.1节)：

2.2.1目标点z轴位置坐标

先求出z方向电机行进角度θ1：

(10)

再由图4和式(3)，可得：

(11)

2.2.2目标点x轴位置坐标

先求出y方向电机行进角度θ2：

(12)

再由图5，可知：

(13)

将上式代入式(6)，可得：

(14)

将式(5)、式(14)代入式(8)，有：

即：

因此：

(15)

由式(9)求出xdis，代入式(15)，即可求得目标点x轴位置坐标：

(6)

2.2.3目标点y轴位置坐标

求得xd后，由式(13)即可求出yd。

2.3实现指令

系统控制核心采用日本三菱电机的FX3U-48MT小型PLC，具有丰富的浮点运算功能指令，如三角函数、反三角函数、及浮点加减乘除等，另外还有浮点数→整数、整数→浮点数的转换指令，非常方便编程实现上述的正、反运动学求解过程。

3　控制系统

系统控制核心FX3U-48MT小型PLC，本体具有3路最高为100 k的高速输出I/O口；各轴驱动采用三菱的MR-J4-A系列交流伺服系统[4]，其内部配置22位高分辨率编码器(4194304 ppr)，具有位置控制、速度控制和转矩控制三种控制模式，本系统中使用位置控制模式。系统主要输入输出I/O信号如图6所示。

大臂x、小臂z、腰部y轴这三轴伺服系统的脉冲输入配置为“脉冲串+符号”形式(通过伺服驱动器PA13参数设置)，PP为脉冲串输入引脚，NP为脉冲方向引脚，直接由FX3U-48MT本体的Y0、Y1、Y2高速输出口输出脉冲串、Y10、Y11、Y12则控制脉冲方向。

图6　主要输入输出I/O信号

腕部w轴伺服系统的脉冲输入配置也是“脉冲串+符号”形式(PA13参数)，由FX2N-1PG脉冲发生器模块控制，可以实现1路最大100 k高速输出，1PG模块脉冲输出配置为“带方向的脉冲”(BFM#3单元的b8位=1)，即FP输出脉冲串，RP则为方向输出。

3.1原点回归

系统采用绝对脉冲坐标，即各点脉冲值是相对于原点的绝对脉冲位置，因而系统上电运行后，必须先对各轴执行原点回归[5]，以获得各轴原点位置。以x轴为例，结合图7说明原点回归过程。

图7　原点回归示意图

从A点开始执行原点回归，按设定加速时间加速到设定的回归速度(B点)后，按回归速度朝原点运动；当检测到C点的DOG信号(近原点信号)后，按设定减速时间减速到设定的爬行速度(D点)后，按爬行速度缓慢朝原点运动；当检测到E点的原点信号后，在1 ms之内从F点开始减速到零(O点)，电机停止，脉冲当前值寄存器(D8341-D8340)清零，原点回归完成。

具体指令格式如下[6]：

[DSZR X000 X001 Y000 M200]

其中：DSZR是带DOG搜索的原点回归，X0为DOG信号输入I/O口，X1为零点/原点信号输入I/O口，Y0为高速脉冲输出I/O口，M200由PLC自身判断旋转方向，PLC程序再根据M200状态控制伺服系统方向输入信号。

加减速时间、最高速度、回归速度和爬行速度等通过D8342～D8348特殊数据寄存器设定。

另外从示意图可知，通常要使原点回归的开始点(A点)，位于DOG信号的右边，而当机器人遇到断电或紧急停止等特殊情况时，并不能保证当前位置在DOG信号的右边。为此，原点回归开始前，程序先执行PLSY脉冲输出指令，控制电机运动，使当前位置处在x轴后限位，然后再执行原点回归。PLSY具体指令格式如下[6]：

[DPLSY D0 D10 Y000]

其中：DPLSY是32位脉冲输出指令，D1-D0为输出脉冲频率，D11-D10为输出脉冲数，因为上电后无法得知当前位置距x轴后限位的具体脉冲数值，所以此单元设为0(此时Y0以设定脉冲频率一直输出脉冲)。运动过程中检测到x轴后限位时，即可停止本指令执行，转而进行原点回归。

3.2各轴联动脉冲数、脉冲频率确定

在运动学求解过程中，假设各轴电机不是同步运动的，但这仅仅是为了计算分析方便，实际运动时各轴当然需联动控制：即从当前点位向目标点位运动时，各轴电机需同时启动、且同时到达目标点位、并同时停止。显然各轴距目标点位的行进脉冲数是不同的，为了使各轴运动能同时到达目标点位，必然要求各轴电机的行进速度要匹配，计算过程如下：

(1)计算目标点位的脉冲坐标，标记如下：

(2)根据当前点位脉冲坐标，标记如下：

先确定最大行进脉冲数，然后按下式确定到达目标点位所需时间：

(17)

其中：fmax是系统设定的各轴伺服系统脉冲输入的最大频率。

确定到达目标点位所需时间t后，即可按下式确定各轴伺服系统脉冲输入频率：

(18)

3.3各轴绝对定位驱动

各轴行进脉冲数、脉冲频率确定后，即可执行绝对定位指令，控制各轴伺服电机行进到目标点位。绝对定位示意图如图8所示。

图8　原点回归示意图

假设起点1、起点2当前脉冲坐标值为100、375，目标点坐标值均为225，显然对于起点1情形下，行进方向应向右，而对于起点2情形下，行进方向应向左。FX3UPLC的绝对定位指令具有自动判断运动方向的功能，因而系统使用绝对坐标体系和绝对定位驱动，通过绝对定位指令自动判断运动方向，极大降低了程序复杂程度。

以x方向伺服为例，指令格式如下[6]：

[DDRVA D100 D200 Y000 M100]

其中：DDRVA是双字绝对定位指令，D101-D100存放经计算所得的脉冲坐标，D201-D200存放脉冲频率，Y0为高速脉冲输出I/O口，本指令根据起点和目标点坐标，自动判断运动方向，并对M100置位或清零，PLC程序再根据M100状态控制伺服系统方向输入信号。

4　结论

介绍了一类关节坐标码垛机器人的机械结构和控制系统，根据码垛生产特点，轨迹规划采用设定点位法，对起始点、抓取点、1#物品码垛点等点位采用示教法直接确定，其余点位则根据正、反运动学求解获取。各轴驱动采用交流伺服系统，为避免相对定位方式下产生累积误差，机器人运动采用绝对坐标体系，为此需先通过原点回归确定各轴原点。码垛过程中，根据目标点位实时计算各轴联动所需的脉冲数和频率，采用绝对定位驱动方式实现机器人各轴联动，同时利用伺服自身配备的高精度编码器检测各轴是否到位，以提高系统运动精度。

[1] 李成伟, 朱秀丽, 贠超. 码垛机器人机构设计与控制系统研究[J]. 机电工程, 2008,25(12):81-84.

[2] 叶辰雷, 刘晓平, 贠超. 码垛机器人的轨迹规划与仿真分析[J]. 机械研究与应用, 2013,26(5): 26-30.

[3] 张明, 何庆中, 王志鹏,等. 关节型码垛机器人的工作空间分析[J]. 组合机床与自动化加工技术, 2013(7):68-70.

[4] 三菱电机自动化(中国)有限公司. MR-J4-A伺服放大器技术资料集，SH030103-A[EB/OL]. (2012-01)[2015-06].http://cn.mitsubishielectric.com/fa/zh/download/dwn_idx_manual.asp.

[5] 三菱电机自动化(中国)有限公司. FX3U系列微型可编程控制器用户手册[定位控制篇]，JY997D19501G [EB/OL]. (2014-04)[2015-06] . http://cn.mitsubishielectric.com/fa/zh/download/dwn_idx_manual.asp.

[6] 三菱电机自动化(中国)有限公司. FX3U系列微型可编程控制器编程手册，JY997D19401M[EB/OL].(2014-03)[2015-06].http://cn.mitsubishielectric.com/fa/zh/download/dwn_idx_manual.asp.

Kinematics analysis and control system design of palletizing robot

Zhou Genrong，Jiang Ping，Zhong Yongyan

(School of Electrical Engineering, Nantong University, Nantong226019, China)

This paper analyzes the transmission structure of a kind of four DOF palletizing robot, and constructs the kinematics coordinate system. According to the production characteristics of palletizing, the point-setting method is used for trajectory planning, that some points are determined through teaching method directly, the remaining points are obtained by the forward and reverse kinematics solving. In order to reduce the complexity of solving, the solution in the three-dimensional coordinate system is first decomposed into two plane coordinate system, and then solved separately, finally integrated together. PLC is used as controller in this system, each axis is driven by AC servo system, and the origin of each axis is determined by return-to-zero-point instruction. According to the target point coordinate, the driving pulse numbers and frequency of each axis are calculated in real time, the linkage of all axises is realized by the absolute positioning method. The practical results show that the calculation speed and the real-time performance of the system are improved due to the reduction of the complexity of the solution. Compared with the relative positioning method, absolute positioning method avoids the accumulated error and improves the accuracy of motion.

palletizing robot; kinematics analysis; AC servo system; absolute positioning

2015-10-09；

2015-11-12。

江苏省产学研合作前瞻性联合研究项目(BY2015047-06)。

周根荣(1971-)，男，浙江金华人，副教授，主要从事自动控制、检测与传感技术方向的研究。

1671-4598(2016)01-0081-04

10.16526/j.cnki.11-4762/tp.2016.01.022

TP249

A