一、需求与硬件
个人观点，码垛和拆垛本质上是一样，把流水线上的产品按规律整齐堆叠到托盘里叫码垛，从整齐排列有物料的托盘取料然后放到流水线上叫拆垛。取放料操作一般由机器人完成，我习惯按物料流向将设备分为码盘机(码垛)和上料机(拆垛)，以下程序实例中的机器人为汇川的Scara机器人。
曾在师傅项目上参与过上料机的机器人程序维护，自动线有多台上料机，生产不同型号产品时物料的摆盘方向又不尽相同，为方便维护，于是想把所有拆码垛机器人程序设计整合成一套。下面介绍物料摆盘方式：
以上十三种物料及摆盘方式，我们忽略行数和列数来找规律，只看物料方向，其实只三种摆盘方式：X单向Y单向、X单向Y双向、X双向Y单向。标定一个全单向托盘需要4个坐标点，如果托盘是平行四边形则也可用3点法标定，第四个点由系统自动解算。
由于兼容物料型号太多，机器人手臂上安装有多种夹爪，导致取放托盘边沿的物料时发生机构干涉，这时程序逻辑需要变更为无干涉情况和非边沿的物料正取正放、有干涉情况的边沿区的物料反取反放。
综上，本文及程序支持的常规托盘是指上述三种摆盘方式下的平行四边形托盘。程序核心在于用五个托盘标定点去兼容三种摆盘方向的码垛、用五个标定点解算出正向码垛的四角坐标、使用码垛点时分情况增加正反取料换向偏移和边沿干涉避让偏移。
二、设计程序
选择托盘标定点最重要的是能直接或间接计算得到托盘内每个物料的正向码垛点和反向码垛点。观察托盘物料：X/Y单向摆料时，若夹爪与托盘边沿无干涉则只需两个标定点就能均分后确定整列/行的所有物料坐标，若有干涉则需要三个标定点才能确定所有物料的有效坐标。X/Y双向摆料又分为对半反向或交替反向，双向摆料时不需考虑托盘边沿干涉，标定点最少需要3个才能计算得到所有物料坐标。
综上，X和Y方向各需要3个标定点才能确定整列或行所有物料的坐标，然后原点为X和Y方向共用，所以只需要5个标定点就能完成托盘的标定。
这里解释一下为什么不选Y点而一定要用XY点来做第5个点？首要原因是这样排布可以将5个标定点尽可能分散开，尽可能地跟随托盘变形带来的高度变化。其次是由于XY点和O点一样是稳定点，X单向无干涉或XY任意一方双向时XY点都不受影响，即使在XY都单向且存在托盘边沿干涉情况下，XY点反向标定，此时XY点与X点方向同向，两者的差值也能保证托盘四角解算点的在y方向的准确，而Y点受托盘边沿干涉避让影响更大。
由标定点计算托盘其他点坐标的原理是对平行四边形的边做矢量伸缩与平移。
个人建议将上料机或码盘机的机器人程序设计成主程序只做取放料判断和运动控制指令，而将本文中涉及的托盘坐标计算及码垛设定、获取单个物料的码垛点位与是否偏移做成两个函数，供主程序调用，这样既可以精简主程序结构、避免运动指令多重启动错误，也便于托盘设定部分程序标准化和移植到其他品牌机器人上使用。
下面讲一下托盘内哪些位置的物料的坐标需要做偏移。为便于理解与实际编程，同时由于X与Y不能同时双向，所以我们约定X方向一定为单向摆料，且仅在X方向上考虑托盘边沿干涉避让问题，约定Y方向分单向和双向摆料，则含三种情况：全单00000000、对半00001111(包含0000-0000)、交替01010101。我们计算后设定的码垛为全正向码垛，如果需要反向则在正向码垛点的基础加上换向偏移，如果需要托盘边沿存在干涉则加上避让偏移。
三、编写代码
以下代码是我上一个项目中写的汇川机器人码垛设定程序，部分点位名称可能有些上文有差异，希望对有同样需求的朋友有所帮助：
######## 汇川机器人 ########
############################## 常规托盘 #############################
##托盘按摆盘方向(单/双)区分，可能有以下组合：行单列单、行双列单、行单列双、行双列双(舍去)；
###对于常规规则托盘，单/双方向包含：全单00000000、半双00001111(兼容0000-0000)、全双01010101 (按数值大小编码为1、2、3)
####同时，在某些托盘应用中，托盘边沿存在干涉，需要使用正向取反向放或反向取正向放等类似操作来避让；
#####为简化程序，本程序规定：行方向可以为单方向或双方向，列方向一定为单方向，且仅列方向上可能有避让；
######为尝试兼容上述中的多种摆盘与避让需求，我们在数学上规划出最少的6个标定点；
#######这6个标定点不直接对应托盘设定点位，但可以在数学上计算出一个m行*n列的码垛、列方向上的夹爪正反偏移、行方向上的奇偶转换偏移；
########解算出的托盘数据使用逻辑为：托盘顺序为先列后行，奇数行用码垛点、偶数行用码垛点+换向偏移、如有干涉避让=第1列全正向+最后1列全反向(码垛点+避让偏移)；
## m行*n列的托盘6个标定点位依次为：
########## Pallet_M —— —— —— —— —— —— —— —— —— —— Pallet_XY
########## | —— —— —— —— —— —— —— —— —— —— —— —— |
########## | —— —— —— —— —— —— —— —— —— —— —— —— |
########## Pallet_M2 —— —— —— —— —— —— —— —— —— —— —— |
########## | —— —— —— —— —— —— —— —— —— —— —— —— |
########## Pallet_O —— —— —— —— —— —— —— Pallet_N1 Pallet_N --->列方向：全单向000000或需托盘避让的单向000001
##注：Pallet_N1为列方向倒数第2个点，如果最后1列不需要避让，可设定与Pallet_N重复；
## Pallet_M2为行方向上正向最远点，如果全单向则可设定与Pallet_M重复，如果半双向则在第m/2行，如果为全双向则在第m-1行(这样设定是为了尽可能缩小计算偏差)；
Func normal_pallet()
##
########三点法设定码垛
#### P3 —— —— —— —— ^ 行M
#### | —— —— —— —— | |
#### | —— —— —— —— | |
#### P1 —— —— —— —— P2 ---->列N
##
#####################托盘参数##########################
Int n; ##托盘行数
Int m; ##托盘列数
Int e; ##托盘层数
Int k; ##托盘摆盘方式编码
n = Pallet_columns;
m = Pallet_rows;
e = Pallet_layers;
k = Pallet_types;
####托盘摆盘类型
Print "当前托盘参数 = " + m + "行" + n + "列" + e + "层";
print "托盘摆盘方式 = " + k;
Switch k
Case 1:
print "摆盘方式 = 行单+列单+无避让";
Break;
Case 2:
print "摆盘方式 = 行半双+列单+无避让";
Break;
Case 3:
print "摆盘方式 = 行全双+列单+无避让";
Break;
Case 6:
print "摆盘方式 = 行单+列单+有避让";
Break;
Case 7:
print "摆盘方式 = 行半双+列单+有避让";
Break;
Case 8:
print "摆盘方式 = 行全双+列单+有避让";
Break;
EndSwitch;
Delay T[1];
######################正码垛########################
####解算P1点
LP[1] = Pallet_O;
####解算P2点(列方向)
If n > 2 And k > 5 ##托盘有很多列且最后1列需要避让
LPR[1] = Msft(Pallet_O,Pallet_N1); ##计算第1列指向第n-1列的矢量差
##LPR[2] = LPR[1] / (n - 2); ##计算列间距
LPR[2].X = LPR[1].X / (n - 2);
LPR[2].Y = LPR[1].Y / (n - 2);
LPR[2].Z = LPR[1].Z / (n - 2);
LPR[2].C = LPR[1].C / (n - 2);
LP[2] = Offset(Pallet_N1,LPR[2]); ##在第n-1列的基础上，平移一个列间距，计算得到第n列正向坐标
Else
LP[2] = Pallet_N;
EndIf;
####解算P3点(行方向)
If k ==1 Or k == 6 ##全单向00000000
##全单向的行数可奇可偶
LP[3] = Pallet_M;
ElseIf m > 2 And (k == 3 Or k == 8) ##全双向01010101
##全双向的行数一定是偶数，如4、6、8等，此时Pallet_M2应在第m-1行
LPR[1] = Msft(Pallet_O,Pallet_M2); ##计算第1行指向第m-1行的矢量差
##LPR[2] = LPR[1] / (m - 2); ##计算行间距
LPR[2].X = LPR[1].X / (m - 2);
LPR[2].Y = LPR[1].Y / (m - 2);
LPR[2].Z = LPR[1].Z / (m - 2);
LPR[2].C = LPR[1].C / (m - 2);
LP[3] = Offset(Pallet_M2,LPR[2]); ##在第m-1行的基础上，平移一个行间距，计算得到第m行的正向坐标
Else ##半双向00001111
##半双向的行数一定是偶数，此时Pallet_M2应在第m/2行
If m == 2 ##仅2行
LPR[1] = Msft(LP[2],Pallet_XY); ##计算第1行指向第m=2行的矢量差(使用第n行的数据，是因为XY点标定时一定不受托盘避让影响)
LPR[1].C = 0; ##清除角度偏移
LP[3] = Offset(Pallet_O,LPR[1]); ##计算得到第m行的正向坐标
Else ##4行及以上
LPR[1] = Msft(Pallet_O,Pallet_M2); ##计算第1行指向第m/2行的矢量差
LP[3] = Offset(Pallet_M2,LPR[1]); ##在第m/2行基础上平移得到第m-1行的正向坐标
LPR[2] = Msft(Pallet_O,LP[3]); ##计算第1行指向第m-1行的矢量差
##LPR[3] = LPR[2] / (m -2); ##计算行间距
LPR[3].X = LPR[2].X / (m - 2);
LPR[3].Y = LPR[2].Y / (m - 2);
LPR[3].Z = LPR[2].Z / (m - 2);
LPR[3].C = LPR[2].C / (m - 2);
LP[3] = Offset(LP[3],LPR[3]); ##在第m-1行的基础上，平移一个行间距，计算得到第m行的正向坐标
EndIf;
EndIf;
Delay T[0.2];
########设定正码垛
Pallet 1,LP[1],LP[2],LP[3],n,m,e,0;
Delay T[0.2];
######################偏移数据########################
##注意指令规定托盘点都是从(0,0,0)开始的，且不得等于n,m
####列方向
LP[4] = Pallet(1,n-1,0,0); ##获取等1行n列号正向坐标
PR[11] = Msft(LP[4],Pallet_N); ##计算列方向托盘避让偏移(如果托盘无干涉避让，则偏移为0)
Print "列方向避让偏移 = " + PR[11];
####行方向
LP[4] = Pallet(1,n-1,m-1,0); ##获取等m行n列号正向坐标(使用对角点，是因为XY点标定时一定不受托盘避让影响)
PR[12] = Msft(LP[4],Pallet_XY); ##计算行方向换向偏移(如果行方向为单向，则偏移为0)
Print "行方向换向偏移 = " + PR[12];
EndFunc;
Func get_pallet_point()
##注意指令规定托盘点都是从(0,0,0)开始的，且不得等于n,m
########获取基础托盘点坐标
Print "本次托盘位置 = " + PutIN_pointer + " = " + PUT_rows + "行" + PUT_columns + "列" + PUT_layers + "层";
LP[0] = Pallet(1,PUT_columns,PUT_rows,PUT_layers);
########判断是否偏移
Clear PR[13],1; ##选择并清零一个PR[13]存最终偏移量
Switch Pallet_types
Case 1:
#摆盘方式 = 行单+列单+无避让
Break;
Case 2:
##摆盘方式 = 行半双+列单+无避让
If PUT_rows >= (Pallet_rows / 2) ##半双型托盘需在后半盘作换向偏移
PR[13] = PR[13] + PR[12];
Print "本次坐标已作换向偏移";
EndIf;
Break;
Case 3:
##摆盘方式 = 行全双+列单+无避让
If PUT_rows % 2 == 1 ##全双型托盘需在偶数行作换向偏移
PR[13] = PR[13] + PR[12];
Print "本次坐标已作换向偏移";
EndIf;
Break;
Case 6:
##摆盘方式 = 行单+列单+有避让
If PUT_columns == (Pallet_columns - 1) ##全单型托盘需在第n列作避让偏移
PR[13] = PR[13] + PR[11];
Print "本次坐标已作避让偏移";
EndIf;
Break;
Case 7:
##摆盘方式 = 行半双+列单+有避让
If PUT_rows < (Pallet_rows / 2) ##半双型托盘的前半盘
If PUT_columns == (Pallet_columns - 1) ##在前半盘的第n列作避让偏移
PR[13] = PR[13] + PR[11];
Print "本次坐标已作避让偏移";
EndIf;
Else ##半双型托盘的后半盘
PR[13] = PR[13] + PR[12]; ##在后半盘需作换向偏移
Print "本次坐标已作换向偏移";
If PUT_columns == 0 ##在后半盘的第1列作避让偏移
PR[13] = PR[13] + PR[11];
Print "本次坐标已作避让偏移";
EndIf;
EndIf;
Break;
Case 8:
##摆盘方式 = 行全双+列单+有避让
If PUT_rows % 2 == 0 ##全双型托盘的奇数行(从0行递增)
If PUT_columns == (Pallet_columns - 1) ##奇数行的第n列作避让偏移
PR[13] = PR[13] + PR[11];
Print "本次坐标已作避让偏移";
EndIf;
Else ##全双型托盘的偶数行
PR[13] = PR[13] + PR[12]; ##在偶数行需作换向偏移
Print "本次坐标已作换向偏移";
If PUT_columns == 0 ##偶数行的第1列作避让偏移
PR[13] = PR[13] + PR[11];
Print "本次坐标已作避让偏移";
EndIf;
EndIf;
Break;
EndSwitch;
########保存最终托盘点
put_pallet_point = Offset(LP[0],PR[13]);
EndFunc;四、编译、运行与调试
略。
五、其他说明
1、上述程序是五点法兼容常规托盘码垛的第二版，由于我的上一个项目只涉及到X单向无避让+Y对半双向情况，所以只测试了这种情况程序无问题。而上述的十三种摆盘方式是我师傅的项目需求，我那时刚实习结束并转正，并无实际项目，只是跟着师傅学习，师傅负责PLC程序和机器人程序，用的凯宝机器人，我协助跟线时在他指导下写出了五点法兼容常规码垛的第一版，师傅则重点写CCD视觉引导抓取部分。
2、第二版程序在理论上与第一版并未做改动，更多的是机器人指令变动、变量命名和注释方面的精简，第一版的确兼容了上述多种摆盘方式，想来第二版也应该问题不大。只是第一版程序复杂很多，而且是双夹爪需要考虑挨着取料和间隔取料情况，双夹爪常需要跳跃取料，这部分程序的写法是分情况设定一个控制字，然后循环右移来控制夹爪是否夹取动作。
版权声明：本文为博主作者：风海winds原创文章，版权归属原作者，如果侵权，请联系我们删除！
原文链接：https://blog.csdn.net/qq_43653371/article/details/133434654
