川崎机器人码垛程序指令【大全】
首先将机器人码垛动作与码垛位置计算程序分开处理。码垛样式可以随便更改，甚至可以选择多样式。这里就有三种码垛样式。
码垛动作
码垛动作就是解决机器人动作与其他IO信号之间的配合问题。相对码垛位置是封装程度比较高的有。
对于同时码放两个卡板时，机器人动作的处理。a卡板，b卡板同时进行，但两个卡板的节拍是不一样的，a快或b快。这时候要考虑两者信号对于机器人流程的竞争问题。可以将其一卡板的优先级设高，也可以将两个卡板的优先级设定为一样。
机器人的码垛动作流程无非是几个，home位，过渡位1，抓取位，过渡位2，码垛上方位，码垛位，码垛上方位，过渡位3，home位。过渡位用一个子程序基本可以完成计算，抓取位一般是固定的，码垛位和码垛上方位是计算出来的。后文有解释。
码垛位置计算
point put[n]=trans(x,y,z,o,a,t)。
其中trans是决定现实中箱子的绝对位置坐标以及方向，put[n]中的n是码垛的顺序。因为同一个卡板上箱子的位置是不变的，变得是箱子码放的顺序。有些顺序比较合理，有些就不是特别合理。
以广州**拆垛码垛项目为例。
a.main程序是机器人的动作流程程序，这时候不管码垛拆垛的位置如何，都应该是同样的动作流程。
.PROGRAM a.main();主程序
ACCURACY 10 ALWAYS
SPEED 100 ALWAYS
;机器人自动回home点，这个思路和过渡点子程序的思路相似。
HERE .p1
POINT .zhome = #HOME(1)
POINT/Z .p1 = .zhome
SPEED 20
LMOVE .p1
HOME
;拆垛
XMOVE home1 TILL 2001
IF SIG(1007) AND n1<total and n3<2 THEN
UTIMER @jp = 0
SIGNAL 15,-16;真空吸盘开
;提前计算过渡点，后面开启pc运动的时候机器人动作就会比较流畅。如果在两个机器人位姿变量之间插入其他的计算，那么pc运动不起作用，机器人动作就不流畅,会影响节拍。
;任意点到pick的过渡点
CALL a.midpoint(&HERE,0,&pick[n1],ch,&.mp1,)
;pick点到drop的过渡点
CALL a.midpoint(&pick[n1],ch,&drop,ch,&.mp2,)
;动作流程
LMOVE .mp1
LAPPRO pick[n1],ch
LMOVE pick[n1]
LDEPART,ch
LMOVE .mp2
LAPPRO drop,ch
SWAIT 1009;允许机器人放箱
LMOVE drop
SIGNAL -15,16;真空吸盘关
LDEPART,ch
n1=n1+1
n3=n3+1
jp1 = UTIMER(@jp)
END
IF n1>=total THEN
SIGNAL 11;拆垛完成
ELSE
SIGNAL -11
END
IF SIG(1010) THEN
n1=0
SIGNAL 13;拆垛清零完成
ELSE
SIGNAL -13
END
;码垛
IF SIG(1008,1012) AND n2<total THEN
UTIMER @jp = 0
SIGNAL 15,-16;真空吸盘开
;任意点到get的过渡点
CALL a.midpoint(&HERE,0,&get,ch,&.mp3,)
;get到rdput的过渡点
CALL a.midpoint(&get,ch,&rdput[n2],0,&.mp4,)
;rdput到home1的过渡点
CALL a.midpoint(&rdput[n2],0,&home1,0,&.mp5,)
;动作流程
LMOVE .mp3
LAPPRO get,ch
LMOVE get
LDEPART,ch
LMOVE .mp4
LMOVE rdput[n2]
LMOVE put[n2]
SIGNAL -15,16;真空吸盘关
LMOVE rdput[n2]
LMOVE .mp5
n2=n2+1
n3=n3-1
jp2 = UTIMER(@jp)
END
IF n2>=total THEN
SIGNAL 12;码垛完成
ELSE
SIGNAL -12
END
IF SIG(1011) THEN
n1=0
SIGNAL 14;码垛清零完成
ELSE
SIGNAL -14
END
GOTO loop1
.END
下面的程序例子是过渡点子程序。
.PROGRAM a.midpoint(.&hp,.hzip,.&dp,.dzip,.&mp) #367;过渡点子程序
;计算位于hp位姿上方hz高度的点与位于dp位姿上方dz高度的点之间的过渡点
;输入参数 当前位姿.hp及hz高度,目的位姿.dp及dz高度。
;返回参数过渡点mp，
POINT .#hp = .hp
POINT .#dp = .dp
;工具坐标的角度为什么要转换成360圆周角度。因为机器人位姿有时候是从0度转到180度（-180度），角度过大，机器人有可能会出现报警（oat数据不一致）或者随机顺时针/逆时针转动。但是拆垛码垛的时候因为气管的存在不允许随机转动。转换位360圆周角度后就容易计算起始位姿与终点位姿之间的中间角度，用以过渡，并以此控制机器人的工具坐标的转动角度，避免报警。
;工具角度转360圆周角度
IF DEXT(.dp,4)<0 THEN
.d4 = DEXT(.dp,4)+360
ELSE
.d4 = DEXT(.dp,4)
END
IF DEXT(.hp,4)<0 THEN
.h4 = DEXT(.hp,4)+360
ELSE
.h4 = DEXT(.hp,4)
END
.hd4 = (.d4+.h4)/2
;360圆周角度工具角度
.hd4 = .hd4-360
.hz = DZ(.hp)+.hzip
.dz = DZ(.dp)+.dzip
IF .hz<.dz THEN
POINT .mp = TRANS(DX(.hp),DY(.hp),.dz,.hd4,-180,0)
;POINT .mp = SHIFT(.hp BY 0,0,.dz-.hz+.hzip)
ELSE
POINT .mp = TRANS(DX(.dp),DY(.dp),.hz,.hd4,-180,0)
;POINT .mp = SHIFT(.dp BY 0,0,.hz-.dz+.dzip)
END
IF DY(.mp)>1500 THEN
POINT .mp = SHIFT(.mp BY 0,1500-DY(.mp),)
END
.END
以下是码垛位姿生成子程序
;核心思想在POINT put[3+.m*.num] = TRANS(.xl+.yp,.yl-.xp,.zl+(1+.m).h,-90,-180,0)，put[3+.m.num]中括号中表示的码垛数组的序号，从1 to n。trans函数后的对应的就是某位置箱子的代数位置。上面的思路就是给某位置的箱子安排序号，或者可以理解为给某序号的箱子安排位置。这样就可以随意根据编排码垛拆垛的顺序了。
为什么要rdput这个位姿变量呢？因为从码垛上方位直上直下的话，因为误差的原因，箱子有可能会直接相撞，如下图所示。经过计算，rdput位姿稍微偏移一点，斜着靠近已码垛好的箱子，这样那怕位置有误差，运动中的箱子也不会直接相撞，而是蹭着已码好的箱子就位。
利用相对位姿的思路，箱子的位置就是机器人相对卡板中心的位置+箱子相对卡板中心的位置。可以先算好箱子相对卡板中心的位置，然后到现场后，利用机器人定位，告诉机器人，卡板中心相对机器人的位置。这样就减少了在现场调试的时间了。顺带提一句，机器人视觉中，对相机输送线的定位也是这个思路。只不过定位用的tcp的精度要比较高，一般都是用一根椎体的顶点作为tcp。
.PROGRAM a.models(.l,.w,.h,.layer,.&location1,.&location2,.t) #17;标签方向ok
;输入参数，箱子长宽高.l,.w,.h,码垛卡板定位位姿变量.location1，拆垛卡板定位位姿location2
;返回参数，位姿变量数组拆垛picko[]，码垛put[]以及rdput[]，数组元素总数.t,
.wh = 2000-150;最高码垛高度
;根据最大高度层计算码垛层数
;.layer = INT(.wh/.h);层
.num = 4
.t = .layer*.num
;卡板中心相对机器人底座中心距离，.xl .yl .zl
.xl = DX(.location1)-310
.yl = DY(.location1)+445
.zl = DZ(.location1)-20
;1箱子距离卡板中心
.xp = -0.5*.l
.yp = -0.5*.w
;奇数层
;3箱子的中心相对机器人位置
FOR .m = 0 TO .layer-1 STEP 2
POINT put[3+.m*.num] = TRANS(.xl+.yp,.yl-.xp,.zl+(1+.m).h,-90,-180,0);2象限
END
;4箱子的中心相对机器人位置
FOR .m = 0 TO .layer-1 STEP 2
POINT put[4+.m.num] = TRANS(.xl+.xp,.yl+.yp,.zl+(1+.m).h,0,-180,0);3象限
END
;1箱子的中心相对机器人位置
FOR .m = 0 TO .layer-1 STEP 2
POINT put[1+.m.num] = TRANS(.xl-.xp,.yl-.yp,.zl+(1+.m).h,-180,-180,0);1象限
END
;2箱子的中心相对机器人位置
FOR .m = 0 TO .layer-1 STEP 2
POINT put[2+.m.num] = TRANS(.xl-.yp,.yl+.xp,.zl+(1+.m).h,90,-180,0);4象限
END
;偶数层
;3箱子的中心相对机器人位置
FOR .m = 1 TO .layer-1 STEP 2
POINT put[3+.num.m] = TRANS(.xl+.xp,.yl-.yp,.zl+(1+.m).h,-180,-180,0);2象限
END
;4箱子的中心相对机器人位置
FOR .m = 1 TO .layer-1 STEP 2
POINT put[4+.num.m] = TRANS(.xl+.yp,.yl+.xp,.zl+(1+.m).h,-90,-180,0);3象限
END
;1箱子的中心相对机器人位置
FOR .m = 1 TO .layer-1 STEP 2
POINT put[1+.num.m] = TRANS(.xl-.yp,.yl-.xp,.zl+(1+.m).h,90,-180,0);1象限
END
;2箱子的中心相对机器人位置
FOR .m = 1 TO .layer-1 STEP 2
POINT put[2+.num.m] = TRANS(.xl-.xp,.yl+.yp,.zl+(1+.m).h,0,-180,0);4象限
END
;码垛rdput
FOR .n = 1 TO .t
POINT rdput[.n] = SHIFT(put[.n] BY -100,-100,.h)
END
;拆垛位姿pick
.dxx = -DX(.location1)+DX(.location2)
FOR .n = 1 TO .t
POINT picko[.n] = put[.n]
POINT picko[.n] = SHIFT(picko[.n] BY .dxx,82)
END
.END
.PROGRAM a.modelt(.l,.w,.h,.layer,.&location1,.&location2,.t) #2;标签方向ok
;输入参数，箱子长宽高.l,.w,.h,，卡板定位位姿变量.location，
;返回参数，位姿变量数组拆垛picko[]，码垛put[]以及rdput[]，数组元素总数.t,
.wh = 2000-150;最高码垛高度
;根据最大高度层计算码垛层数
;.layer = INT(.wh/.h);层
.num = 5
.t = .layer.num
;卡板中心相对机器人底座中心距离，.xl .yl .zl
.xl = DX(.location1)-310
.yl = DY(.location1)+445
.zl = DZ(.location1)-20
;1箱子距离卡板中心
.xp1 = -0.5*.l
.yp1 = -0.5*.l
;3箱子距离卡板中心
.xp3 = 0.5*.w
.yp3 = .w
;奇数层
;5箱子的中心相对机器人位置
FOR .m = 0 TO .layer-1 STEP 2
POINT put[5+.m*.num] = TRANS(.xl+.xp1,.yl+.yp1,.zl+(1+.m).h,0,-180,0)
END
;4箱子的中心相对机器人位置
FOR .m = 0 TO .layer-1 STEP 2
POINT put[4+.m.num] = TRANS(.xl+.xp1,.yl-.yp1,.zl+(1+.m).h,180,-180,0)
END
;1箱子的中心相对机器人位置
FOR .n = 0 TO 2
FOR .m = 0 TO .layer-1 STEP 2
POINT put[1+.n+.m.num] = TRANS(.xl+.xp3,.yl+.yp3-.n*.w,.zl+(1+.m).h,90,-180,0)
END
END
;偶数层
;3箱子的中心相对机器人位置
FOR .n = 0 TO 2
FOR .m = 1 TO .layer-1 STEP 2
POINT put[3+.n+.m.num] = TRANS(.xl-.xp3,.yl+.yp3-.n*.w,.zl+(1+.m).h,-90,-180,0)
END
END
;1箱子的中心相对机器人位置
FOR .m = 1 TO .layer-1 STEP 2
POINT put[1+.m.num] = TRANS(.xl-.xp1,.yl-.yp1,.zl+(1+.m).h,-180,-180,0)
END
;2箱子的中心相对机器人位置
FOR .m = 1 TO .layer-1 STEP 2
POINT put[2+.m.num] = TRANS(.xl-.xp1,.yl+.yp1,.zl+(1+.m).h,0,-180,0)
END
;码垛rdput
FOR .n = 1 TO .t
POINT rdput[.n] = SHIFT(put[.n] BY -100,-100,.h)
END
;拆垛位姿pick
.dxx = -DX(.location1)+DX(.location2)
FOR .n = 1 TO .t
POINT picko[.n] = put[.n]
POINT picko[.n] = SHIFT(picko[.n] BY .dxx,82)
END
.END
.PROGRAM a.modelx(.l,.w,.h,.layer,.&location1,.&location2,.t) #2;标签方向ok
;输入参数，箱子长宽高.l,.w,.h,，卡板定位位姿变量.location，
;返回参数，位姿变量数组拆垛picko[]，码垛put[]以及rdput[]，数组元素总数.t,
.wh = 2000-150;最高码垛高度
;根据最大高度层计算码垛层数
;.layer = INT(.wh/.h);层
.num = 4
.t = .layer.num
;卡板中心相对机器人底座中心距离，.xl .yl .zl
.xl = DX(.location1)-310
.yl = DY(.location1)+445
.zl = DZ(.location1)-20
;1箱子距离卡板中心
.xp = -0.5*.w
.yp = 0.5*.l
;1箱子的中心相对机器人位置
FOR .n = 0 TO 3 step 2
FOR .m = 0 TO .layer-1
POINT put[1+.n+.m*.num] = TRANS(.xl-.xp-.n*.w,.yl+.yp,.zl+(1+.m).h,-180,-180,0)
END
END
;3箱子的中心相对机器人位置
FOR .n = 0 TO 3 step 2
FOR .m = 0 TO .layer-1
POINT put[3+.n+.m.num] = TRANS(.xl-.xp-.n*.w,.yl-.yp,.zl+(1+.m)*.h,0,-180,0)
END
END
;码垛rdput
FOR .n = 1 TO .t
POINT rdput[.n] = SHIFT(put[.n] BY -100,-100,.h)
END
;拆垛位姿pick
.dxx = -DX(.location1)+DX(.location2)
FOR .n = 1 TO .t
POINT picko[.n] = put[.n]
POINT picko[.n] = SHIFT(picko[.n] BY .dxx,80)
END
.END
————————————————
声明：本文为转载「阿石石石石」
