ELITE ROBOTS

基础码垛入门——门型框轨迹变种

门型框
码垛
基础
入门

文章《基础搬运入门：门型框式轨迹》介绍了以门型框轨迹制作简单的搬运程序的方法。本文将介绍如何以门型框轨迹的概念为核心，变化出简单的码垛程序。

轨迹上来看，码垛任务像是多个门型框重叠在了一起，所以只要制作很多个门型框就可以完成码垛程序的框架。这种方法虽然可行，但程序会变得十分冗长，不但需要示教大量点位，维护管理起来也十分麻烦。比起重叠多个门型框，不如制作每次运行都会改变一根门柱位置的程序。

本文将以下图为例，设计将12个箱子按3X2X2的方式进行摆放的码垛程序。箱子的大小为10cm X 10cm X 10cm。机器人的坐标系与码垛区域坐标系平行。

下图是一个门型框的变形，也是这个码垛程序的核心。关于P变量和V变量的使用理念还是没有变：需要能够精确调整位置，且移动距离较短的点采用V变量；长距离移动的点推荐使用P变量。V1点是熟悉的拿取位置，简单示教完V1后，垂直升起法兰，在垂直于V1上方合适高度出示教一个P1。而放置侧则增加了不少点位。V2是放置点，将会根据当前需要放置的箱子编号更新坐标。V3垂直于V2上方，是放置前的位置，高度需要保证不会撞到已经放置好的箱子，会跟着V2的坐标更新一起更新。P2是码垛区域上方的通用点位。而P2与V3之间的P3则是码垛任务中一个相当重要的过度点，也会根据V2的坐标变化而进行更新坐标。

如果在这个码垛应用中没有一个随机应变的P3点，可能会遇到以下两个问题：

1. 当机器人向某个距离较远的点放置箱子时，从V3向V2运动过程中因为关节旋转角度已经达到上限，导致无法到达目标点。

2. 机器人从P2向V3移动时，以奇怪姿态进入V3，导致持有的箱体撞到机器人自身。

有一个P3点可以在移动到V3之前先调整好关节方向可以有效解决这两个问题。

为便于理解接下来的内容，给箱子编上号码。编号采用从0开始的规则。第0层从右到左编号0-5，第1层则是6-11。这个编号也决定箱子的搬运顺序，这个顺序的关键在于先放置最远的箱子。否则从V3向V2运动过程中，机器人的姿势可能会碰到已经放好的箱子。

拿出上一篇文章的基础门型框型搬运程序，以此为基准开始码垛程序改造。注意：在实际运用中，标记了“//(Insert Pick Up Start Signal Here)”的地方需要插入“拿取物体”的命令信号，可以是箱子已经到达拿取地点的判断信号。

NOP
MOVJ P001 VJ=100% CR=0.0MM ACC=50 DEC=50
WHILE 1=1 DO
//Pick Up
//(Insert Pick Up Start Signal Here)
MOVL V001 AV=50.0MM/S PL=0
#CALL JOB:VG10_A_GRIP
TIMER T=0.5 S
MOVL P001 V=3000MM/S CR=100.0MM ACC=100 DEC=100
//Place
MOVJ P002 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVL V002 V=3000MM/S CR=0.0MM ACC=100 DEC=50
#CALL JOB:VG10_A_RELEASE
TIMER T=0.5 S
MOVL P002 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P001 VJ=100% CR=0.0MM ACC=100 DEC=100
ENDWHILE
END

1. 首先先修正循环条件，因为需要搬运的箱子只有12个，所以循环次数只需要12次。可以采用一个B变量做计数器，已经搬运的箱子个数。初始化B000 = 0，将While的条件改为WHILE B000< 12 DO，并在ENDWHILE前增加B000的数值。

NOP
SET B000 0
MOVJ P001 VJ=100% CR=0.0MM ACC=50 DEC=50
WHILE B000<12 DO
//Pick Up
//(Insert Pick Up Start Signal Here)
MOVL V001 AV=50.0MM/S PL=0
#CALL JOB:VG10_A_GRIP
TIMER T=0.5 S
MOVL P001 V=3000MM/S CR=100.0MM ACC=100 DEC=100
//Place
MOVJ P002 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVL V002 V=3000MM/S CR=0.0MM ACC=100 DEC=50
#CALL JOB:VG10_A_RELEASE
TIMER T=0.5 S
MOVL P002 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P001 VJ=100% CR=0.0MM ACC=100 DEC=100
INC B000
ENDWHILE
END

2. 用注释将程序分割成三个部分：初始化部分，计算部分，移动部分。其中计算部分和移动部分是每次循环都需要更新的。

NOP
//*********** Initialization ************
SET B000 0
MOVJ P001 VJ=100% CR=0.0MM ACC=50 DEC=50
//---------------------------------------
//
//************ Calculation **************
//---------------------------------------
//
//************** Movement ***************
WHILE B000<12 DO
//Pick Up
//(Insert Pick Up Start Signal Here)
MOVL V001 AV=50.0MM/S PL=0
#CALL JOB:VG10_A_GRIP
TIMER T=0.5 S
MOVL P001 V=3000MM/S CR=100.0MM ACC=100 DEC=100
//Place
MOVJ P002 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVL V002 V=3000MM/S CR=0.0MM ACC=100 DEC=50
#CALL JOB:VG10_A_RELEASE
TIMER T=0.5 S
MOVL P002 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P001 VJ=100% CR=0.0MM ACC=100 DEC=100
INC B000
//---------------------------------------
ENDWHILE
END

3. 在移动部分中，插入向V003和P003移动的指令。注意MOV指令的种类以及调整参数。

//************** Movement ***************
//Pick Up
//(Insert Pick Up Start Signal Here
MOVL V001 AV=50.0MM/S PL=0
#CALL JOB:VG10_A_GRIP
TIMER T=0.5 S
MOVL P001 V=3000MM/S CR=100.0MM ACC=100 DEC=100
//Place
MOVJ P002 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVJ P003 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVJ V003 V=3000MM/S CR=1.0MM ACC=100 DEC=50
MOVL V002 V=3000MM/S CR=0.0MM ACC=100 DEC=50
#CALL JOB:VG10_A_RELEASE
TIMER T=0.5 S
MOVL V003 V=3000MM/S CR=10.0MM ACC=100 DEC=100
MOVJ P003 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P002 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P001 VJ=100% CR=0.0MM ACC=100 DEC=100
INC B000
//---------------------------------------

4. 接着是关键的计算部分。首先要搞清楚当前箱子需要摆在第几层，第几行，第几位。使用几次除法就能获得需要的数值。B000的数值除以每层箱子总数6获得的商就是层数信息。比如搬运第六个箱子时，B000数值为5。5除以6得0，余数为5，也就是当前箱子处于第0层。而余数5除以每行总数3得1余2，也就是说位置为第1行第2位。

5. 于是在计算部分的程序中可以使用求余指令除运算指令将层数、行数、位数分别保存到变量B001，B002，B003中。

//************ Calculation **************
//Layer#
SET B001 B000
DIV B001 6
//Row#
SET B002 B000
MOD B002 6
SET B003 B002
DIV B002 3
//Column#
MOD B003 3
//---------------------------------------

6. 将第0层，第0行，第0位的坐标示教到一个其他V变量中，比如V010，方便将V002重置。已知箱子的大小是10cm X 10cm X10 cm，因此间距105mm左右相当合适。根据当前层数、行数、位数，利用乘法指令更新V002的坐标即可。注意在本示例中，V002(0)为x方向，根据箱子的位数调整，V002(1)为y方向，根据箱子的行数调整，V002(2)为Z轴方向，根据箱子的层数调整。根据XY的变化方向选择加法或减法指令，本示例中X方向位置根据箱子为位数的增加而减小；和Y方向位置根据箱子为行数的增加而减少。然后以更新后V002的坐标，利用加法指令更新V003的坐标。

//Column#
MOD B003 2
//
//Reset V002
SET V002 V010
//Z-axis
SET D000 105.000
MUL D000 B001
ADD V002(2) D000 
//Y-axis
SET D000 105.000
MUL D000 B002
SUB V002(1) D000 
//X-axis
SET D000 105.000
MUL D000 B003
SUB V002(0) D000
//
//Update V003
SET V003 V002
ADD V003(2) 105

7. 最后，门型框还需要的点就只剩下P2和P3了。一般来说，P2设置为码垛区域中心正上方，能不接触任意层高箱子的位置即可。接着开始测试程序，如果所有的V3、V2点都可以没有问题地到达，那么直接把P3的坐标设为同P2即可，如果存在需要增加中介点的才能到达的位置，那么这些中介点则需要一一保存到其他P变量中，以待需要时赋值给P3。本范例程序在测试时发现，为了顺利放置箱0和箱3需要一个共用中介点，箱6和箱9需要一个共用中介点，箱8和箱11需要一个共用中介点。这个范例也反映出需要中介点的位置一般都是集中在离机器人最远以及最近的区域附近。

8. 将这三个P3候补分别示教到P010，P011，P012。也可以在程序的初始化区域用SETJOINT指令进行设置，可以防止变量别其他人修改。（事实上，包含P1、V1、初始V2、P2在内的所有基准点位都建议通过SETJOINT或SETPOSE在初始化区域设置，否则变量一旦被其他用户修改将会导致机器人向错误位置移动。）

NOP
//*********** Initialization ************
SET B000 0
MOVJ P001 VJ=100% CR=0.0MM ACC=50 DEC=50
SETJOINT P010 19.354,-54.491,72.265,-109.329,90.500,8.810,0.0000,0.0000
SETJOINT P011 19.904,-73.585,72.264,-93.012,90.538,8.805,0.0000,0.0000
SETJOINT P012 19.878,-109.548,122.590,-107.113,90.541,8.805,0.0000,0.0000
//---------------------------------------
//

9. 接着在计算区域让机器人知道在搬运各个编号的箱子时需要使用的P3。用一个简单的IF指令即可完成。

箱子编号	P3调用	箱子编号	P3调用
0	P10	6	P11
1	P2	7	P2
2	P2	8	P12
3	P10	9	P11
4	P2	10	P2
5	P2	11	P12

//
//Update P003
IF B000 = 0 OR B000 = 3 THEN
SET P003 P010
ELSEIF B000 = 6 OR B000 = 9 THEN
SET P003 P011
ELSEIF B000 = 8 OR B000 = 11 THEN
SET P003 P012
ELSE
SET P003 P002
ENDIF

至此，一个码垛程序就完成了。下面是完整版。

NOP
//*********** Initialization ************
SET B000 0
MOVJ P001 VJ=100% CR=0.0MM ACC=50 DEC=50
SETJOINT P010 19.354,-54.491,72.265,-109.329,90.500,8.810,0.0000,0.0000
SETJOINT P011 19.904,-73.585,72.264,-93.012,90.538,8.805,0.0000,0.0000
SETJOINT P012 19.878,-109.548,122.590,-107.113,90.541,8.805,0.0000,0.0000
//---------------------------------------
//
//************ Calculation **************
WHILE B000<12 DO
//Layer#
SET B001 B000
DIV B001 6
//Row#
SET B002 B000
MOD B002 6
SET B003 B002
DIV B002 3
//Column#
MOD B003 3
//
//Reset V002
SET V002 V010
//Z-axis
SET D000 105.000
MUL D000 B001
ADD V002(2) D000 
//Y-axis
SET D000 105.000
MUL D000 B002
SUB V002(1) D000 
//X-axis
SET D000 105.000
MUL D000 B003
SUB V002(0) D000
//
//Update V003
SET V003 V002
ADD V003(2) 105
//
//Update P003
IF B000 = 0 OR B000 = 3 THEN
SET P003 P010
ELSEIF B000 = 6 OR B000 = 9 THEN
SET P003 P011
ELSEIF B000 = 8 OR B000 = 11 THEN
SET P003 P012
ELSE
SET P003 P002
ENDIF

//---------------------------------------
//
//************** Movement ***************
//Pick Up
//(Insert Pick Up Start Signal Here)
MOVL V001 AV=50.0MM/S PL=0
#CALL JOB:VG10_A_GRIP
TIMER T=0.5 S
MOVL P001 V=3000MM/S CR=100.0MM ACC=100 DEC=100
//Place
MOVJ P002 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVJ P003 VJ=100% CR=100.0MM ACC=100 DEC=100
MOVJ V003 V=3000MM/S CR=1.0MM ACC=100 DEC=50
MOVL V002 V=3000MM/S CR=0.0MM ACC=100 DEC=50
#CALL JOB:VG10_A_RELEASE
TIMER T=0.5 S
MOVL V003 V=3000MM/S CR=10.0MM ACC=100 DEC=100
MOVJ P003 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P002 V=3000MM/S CR=100.0MM ACC=100 DEC=100
MOVJ P001 VJ=100% CR=0.0MM ACC=100 DEC=100
INC B000
//---------------------------------------
ENDWHILE
END

在这个例子中，机器人的坐标系和码垛区域的坐标系是平行的，所以位置计算相对容易。如果坐标系不平行，那位数或行数的变化都会同时改变x轴和y轴上的坐标。下列代码是码垛区域坐标系与机器人坐标系有30度左右偏差的示范。根据测量得知位数的变化将会让x轴坐标减少90.932mm，让y轴坐标减少52.500mm；行数的变化将会让x轴坐标减少52.500mm，让y轴坐标减少90.932mm。

//Reset V002
SET V002 V010
//Z-axis
SET D000 105.000
MUL D000 B001
ADD V002(2) D000 
//Row Offset
SET D000 90.932
SET D001 52.500
MUL D000 B002
MUL D001 B002
SUB V002(0) D000
SUB V002(1) D001 
//Column Offset
SET D002 52.500
SET D003 90.932
MUL D002 B003
MUL D003 B002
SUB V002(0) D002
SUB V002(0) D003

如果不知道如何测量计算X轴和Y轴坐标需要的变化值，可以使用机器人进行测量。

如下图，可以使用机器人持有箱子的状态分别将0号位置，1号位置和3号位置分别记录在V100，V101，V103三个位置中。示教过程中可以不管高度偏差带来的影响，但要保证没有出现Rx，Ry，Rz3个方向的转动。