一种码垛机器人弧形运动轨迹算法
【技术领域】
[0001]本发明涉及码垛机器人，尤其是一种码垛机器人弧形运动轨迹算法。
【背景技术】
[0002]码垛机器人主要应用于大负载，高速环境替代人工作业的场合，其最突出的问题就是要节拍快，节能，柔性等。如图1所示，传统的码垛动作基本上都是基于“门”字形机构的动作轨迹。动作过程分为三步，第一步直线向下抓取，然后拉起来平移，到目标位置时垂直放下，然后按照相反的动作返回，继续下一个节拍。由于每次都是经过直线运动插补时机器人运动比较机械化，不够柔性，另外行走路径是直角中间有两个拐点，半个周期内要有三次加减速，所以此方式动作耗时速度慢，不够柔性。
【发明内容】
[0003]本发明所要解决的技术问题是提供一种码垛机器人弧形运动轨迹算法，减少机器人运动路程，使其减少每个周期的运动时间，同时使其运动过程更加柔性灵活，并通过减少运行时间降低电力使用成本。
[0004]为解决上述技术问题，本发明的技术方案是:一种码垛机器人弧形运动轨迹算法，包括以下步骤:
(1)设定码垛机器人当前位置的坐标Pl(Xl，yl，zl)，目标位置的坐标P3 (x3,y3,z3),过渡点的坐标P2 (X2，y2，z2);坐标点Pl为弧形运动轨迹的起点，坐标点P3为弧形运动轨迹的终点，坐标点P2 (x2, y2, z2)为坐标点Pl到坐标点P3的过渡点；坐标点P2到达由坐标点P1、P3形成的直线的垂直高度为H ；
(2)获取码垛机器人当前位置的坐标Pl(xl, yl, zl)和目标位置的坐标P3 (x3, y3,
z3)；
(3)码垛机器人读取输入的高度参数H，并将当前位置坐标Pl和目标位置坐标P3转化为世界坐标；
(4)将转化后的当前位置坐标P1、目标位置坐标P3和高度参数H代入底层算法函数中求出过渡坐标P2 ；
(5)将求取的过渡坐标P2转化为世界坐标系下坐标，并通过接口发送到应用层；
(6)在码垛机器人的用户应用层使用Circle指令运行规划圆弧运动轨迹。
[0005]由三维空间坐标点可以确定一个平面，在这个平面上规划一条圆弧曲线。例如Pl(xl，yl, zl), P2 (x2, y2，z2)，P3 (x3, y3，z3)，如果已知三个点的坐标，只要三个点不在一条直线上则可以算法同时经过三个点的空间圆弧，P2点为过渡点，Pl为起始点，P3点为终止点，圆弧轨迹的运动方向由Pl经过P2到P3，角度根据三点形成的圆弧圆心到Pl和P3的单位向量点乘求得。现场使用码垛机器人时抓取点Pl已知，放置点P3已知，现在要根据P2到达P1P3形成的直线的垂直高度H求取P2点的坐标。然后根据Pl，P2，P3点的坐标规划抛物线轨迹，使用机器人的Circle指令运行规划的轨迹。
[0006]作为改进,所述步骤(4)中，底层算法函数funct1n P2=FuncSolve ( PI, P3, h)，根据Pl，P3点求出中间的坐标:dist= (Pl+P3)/2 ;dist包含三个元素dist (I) X轴坐标，dist (2) Y轴坐标，dist (3) Z轴坐标；码垛机器人第四轴垂直于水平面，TCP运行的抛物线轨迹垂直于XOY平面，假设垂直于此平面的一个方向量是(l，k，0)，则k=( Pl(I)-P3(l) ) / ( P3(2) - Pl (2));然后再根据圆弧曲线在XOY平面投影的距离求取P2⑵即中间点Y轴的坐标；具体公式如下:
fun = @ (y) (point3 (I)+k*point3(2)-k*y_dist(I))'2+(y-dist(2))'2+...(((point3 (I)+k*point3(2)-k*y_dist(I)) *(point3(I)-pointI(I)) + (y-dist(2)) * (point3 (2)-pointI(2)))/(point3(3)-pointI(3)))'2-high'2P2 (2) = FSOLVE(fun,I)
其中FSOLVE是求变量为y，fun=l时y的值；
然后根据已知k，P2(2)的值求取P2(l)的值，公式如下:
P2(I) = point3(I)+k*point3(2)_k*Y最后求出P2(3):
P2 (3) =dist (3) - ((point3 (I) +k*point3 (2) -k*Y_dist (I)) * (point3 (I) -pointl (I)) +(Y-dist(2)) *(point3(2)-pointl(2)))/(point3(3)-pointl(3))。
[0007]作为改进，所述算法应用于平行杆码垛机器人，如公开号为103010764A的一种平行杆码垛机机器人。
[0008]本发明与现有技术相比所带来的有益效果是:
将码垛机器人的运动轨迹规划呈抛物线，减少机器人运动路程，使其减少每个周期的运动时间，同时使其运动过程更加柔性灵活，并通过减少运行时间降低电力使用成本。
【附图说明】
[0009]图1为现有码垛机器人运动轨迹。
[0010]图2为本发明码垛机器人运动轨迹。
[0011]图3为本发明算法流程图。
【具体实施方式】
[0012]下面结合说明书附图对本发明作进一步说明。
[0013]如图2、3所示，一种码垛机器人弧形运动轨迹算法，该码垛机器人为平行杆码垛机器人，算法包括以下步骤:
(1)设定码垛机器人当前位置的坐标Pl(Xl，yl，zl)，目标位置的坐标P3 (x3,y3,z3),过渡点的坐标P2 (X2，y2，z2);坐标点Pl为弧形运动轨迹的起点，坐标点P3为弧形运动轨迹的终点，坐标点P2 (x2, y2, z2)为坐标点Pl到坐标点P3的过渡点；坐标点P2到达由坐标点P1、P3形成的直线的垂直高度为H ；
(2)获取码垛机器人当前位置的坐标Pl(xl, yl, zl)和目标位置的坐标P3 (x3, y3,
z3)；
(3)码垛机器人读取输入的高度参数H，并将当前位置坐标Pl和目标位置坐标P3转化为世界坐标； (4)将转化后的当前位置坐标P1、目标位置坐标P3和高度参数H代入底层算法函数中求出过渡坐标P2 ；
(5)将求取的过渡坐标P2转化为世界坐标系下坐标，并通过接口发送到应用层；
(6)在码垛机器人的用户应用层使用Circle指令运行规划圆弧运动轨迹。
[0014] 底层算法函数funct1n P2=FuncSolve ( Pl, P3, h ),根据Pl, P3点求出中间的坐标:dist= (P1+P3)/2 ;dist包含三个元素dist (I) X轴坐标，dist (2) Y轴坐标，dist
(3)Z轴坐标；码垛机器人第四轴垂直于水平面，TCP运行的抛物线轨迹垂直于XOY平面，假设垂直于此平面的一个方向量是(1，k，0)，则k=( Pl(I) - P3(l) ) / ( P3(2) - Pl (2));然后再根据圆弧曲线在XOY平面投影的距离求取P2 (2)即中间点Y轴的坐标；具体公式如下:
fun = @ (y) (point3 (I)+k*point3(2)-k*y_dist(I))'2+(y-dist(2))'2+...(((point3 (I)+k*point3(2)-k*y_dist(I)) *(point3(I)-pointl(I)) + (y-dist(2)) * (point3 (2)-pointl(2)))/(point3(3)-pointl(3)))'2-high'2P2 (2) = FSOLVE(fun,I)
其中FSOLVE是求变量为y，fun=l时y的值；
然后根据已知k，P2(2)的值求取P2(l)的值，公式如下:
P2(I) = point3(I)+k*point3(2)_k*Y最后求出P2(3):
P2 (3) =dist (3) - ((point3 (I) +k*point3 (2) -k*Y_dist (I)) * (point3 (I) -pointl (I)) +(Y-dist(2)) *(point3(2)-pointl(2)))/(point3(3)-pointl(3))。
【主权项】
1.一种码垛机器人弧形运动轨迹算法，其特征在于，包括以下步骤: (1)设定码垛机器人当前位置的坐标Pl(Xl，yl，zl)，目标位置的坐标P3 (x3,y3,z3),过渡点的坐标P2 (X2，y2，z2);坐标点Pl为弧形运动轨迹的起点，坐标点P3为弧形运动轨迹的终点，坐标点P2 (x2, y2, z2)为坐标点Pl到坐标点P3的过渡点；坐标点P2到达由坐标点P1、P3形成的直线的垂直高度为H ； (2)获取码垛机器人当前位置的坐标Pl(xl, yl, zl)和目标位置的坐标P3 (x3, y3,z3)； (3)码垛机器人读取输入的高度参数H，并将当前位置坐标Pl和目标位置坐标P3转化为世界坐标； (4)将转化后的当前位置坐标P1、目标位置坐标P3和高度参数H代入底层算法函数中求出过渡坐标P2 ； (5)将求取的过渡坐标P2转化为世界坐标系下坐标，并通过接口发送到应用层； (6)在码垛机器人的用户应用层使用Circle指令运行规划圆弧运动轨迹。
2.根据权利要求1所述的一种码垛机器人弧形运动轨迹算法，其特征在于:所述步骤(4)中，底层算法函数funct1n P2=FuncSolve ( PI, P3, h ),根据PI, P3点求出中间的坐标:dist= (P1+P3) /2 ； dist包含三个元素dist (I) X轴坐标，dist (2) Y轴坐标，dist (3) Z轴坐标；码垛机器人第四轴垂直于水平面，TCP运行的抛物线轨迹垂直于XOY平面，假设垂直于此平面的一个方向量是(l，k，0)，则k=( Pl(I) - P3(l) ) / ( P3(2) - Pl (2));然后再根据圆弧曲线在XOY平面投影的距离求取P2 (2)即中间点Y轴的坐标；具体公式如下:fun = @ (y) (point3 (I)+k*point3(2)-k*y_dist(I))'2+(y-dist(2))'2+...(((point3 (I)+k*point3(2)-k*y_dist(I)) *(point3(I)-pointI(I)) + (y-dist(2)) * (point3 (2)-pointI(2)))/(point3(3)-pointI(3)))'2-high'2P2 (2) = FSOLVE(fun,I) 其中FSOLVE是求变量为y，fun=l时y的值； 然后根据已知k，P2(2)的值求取P2(l)的值，公式如下:P2(I) = point3(I)+k*point3(2)_k*Y最后求出P2(3): P2 (3) =dist (3) - ((point3 (I) +k*point3 (2) -k*Y_dist (I)) * (point3 (I) -pointl (I)) +(Y-dist(2)) *(point3(2)-pointl(2)))/(point3(3)-pointl(3))。
3.根据权利要求1所述的一种码垛机器人弧形运动轨迹算法，其特征在于:所述算法应用于平行杆码垛机器人。
【专利摘要】一种码垛机器人弧形运动轨迹算法，包括以下步骤：（2）获取码垛机器人当前位置的坐标P1（x1，y1，z1）和目标位置的坐标P3（x3，y3，z3）；（3）码垛机器人读取输入的高度参数H，并将当前位置坐标P1和目标位置坐标P3转化为世界坐标；（4）将转化后的当前位置坐标P1、目标位置坐标P3和高度参数H代入底层算法函数中求出过渡坐标P2；（5）将求取的过渡坐标P2转化为世界坐标系下坐标，并通过接口发送到应用层；（6）在码垛机器人的用户应用层使用Circle指令运行规划圆弧运动轨迹。将码垛机器人的运动轨迹规划呈抛物线，减少机器人运动路程，使其减少每个周期的运动时间，同时使其运动过程更加柔性灵活。
【IPC分类】G05B13-04
【公开号】CN104570735
【申请号】CN201410806960
【发明人】刘远强, 杨文发, 郭伟越
【申请人】广州市万世德智能装备科技有限公司, 佛山市万世德机器人技术有限公司
【公开日】2015年4月29日
【申请日】2014年12月23日