码垛机械臂末端的运动轨迹规划方法与流程

本发明涉及机器人运动规划领域，尤其涉及一种码垛机械臂末端的运动轨迹规划方法。

背景技术：
码垛机械臂末端的运功轨迹根据工作要求不同而不同，以生产线上产品卸垛堆垛为例，工业生产中，需将流水线上的产品整齐的码放在托盘或推车上，这一动作简单而又重复，可用码垛机械臂完成。码垛机械臂在运动过程中，其运动轨迹具有重复性，通过有效的轨迹规划可以保证运行过程中的平稳和时间要求，进而可以提高码垛机械臂的运行效率和稳定性。

技术实现要素：
本发明的目的在于针对现有技术的不足，提供一种码垛机械臂末端的运动轨迹规划方法，通过用曲线段运动轨迹替代直线段运动轨迹，可以提高码垛机械臂的运行效率和稳定性。本发明采用如下的技术方案：1)确定关键点：以机械臂所要抓取的物品的原始位置为抓取位置P1，其在世界坐标系中的坐标P1(x1，y1，z1)；以物品目标位置为放置位置P6，其坐标P6(x2，y2，z2)；抓取准备位置P2与抓取位置P1垂直分布，高度差为H1；移动开始位置P3与抓取准备位置P2水平分布，距离为L1；放置准备位置P5与放置位置P6垂直分布，高度差为H2；移动结束位置P4与放置准备位置P5水平分布，距离为L2；高度H1、H2和距离L1、L2由具体工作要求给定；因此，其余各关键点坐标分别为：P2(x1，y1，z1+H1)，P3(x1+L1，y1，z1+H1)，P4(x2-L2，y2，z2+H2)，P5(x2，y2，z2+H2)；2)确定初步运动轨迹：将上述各关键点依次连接获得直线段P1P2、P2P3、P3P4、P4P5和P5P6，以P1P2和P2P3作为第一直角过渡段，P3P4作为中间直线段，P4P5和P5P6作为第二直角过渡段，获得从P1到P6的初始运动轨迹；3)增加控制点：以直线段P1P2的中间位置为控制点C1，坐标为C1(x1，y1，z1+H1/2)；P5P6的中间位置为控制点C4，坐标为C4(x2，y2，z2+H2/2)；将控制点C2、C3设于中间直线段P3P4上，P3C2＝C3P4＝kP3P4，k为比例系数，0≤k≤1；因此，控制点C2、C3的坐标分别为：C2(x1+L1+k|(x1+L1)-(x2-L2)|，y1+k|y1-y2|，z1+H1+k|(z1+H1)-(z2+H2)|)C3(x2-L2-k|(x1+L1)-(x2-L2)|，y2-k|y1-y2，z2+H2-k|(z1+H1)-(z2+H2)|)；4)曲线插补：以P1、C1、P2、P3、C2作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点P1和C2的平滑曲线，以该曲线取代第一直角过渡段及P3C2直线段；以C3、P4、P5、C4、P6作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点C3和P6的平滑曲线，以此曲线轨迹取代C3P4直线段及第二直角过渡段；获得机械臂末端的最终运动轨迹。步骤4)中所述的采用NURBS曲线插补理论算法，得到经过首末控制点的平滑曲线，具体方法为：一、计算节点向量已知m+p+1个控制点其中m为所生成样条的阶数，p为所构造的基函数次数；控制点之间距离其中xj表示控制点Pj的横轴坐标，yj表示控制点Pj的纵轴坐标。根据哈德利—贾德方法递推公式可得：其中ui表示第i个节点；根据上式可得节点向量U递推公式为：求出的节点向量U的形式为：二、根据节点向量计算NURBS基函数用下列递推方式确定基函数Ni，p(u)，即相应于节点向量U的p次NURBS基函数：其中i是基函数的序列号；三、得到NURBS曲线表达式，根据控制点计算出插补点第i段NURBS曲线Pi(u)的表达式为：参数u∈[ui+p，ui+p+1]均匀变化得到插补点数据。本发明的有益效果在于：本发明的方法利用笛卡尔空间坐标系对码垛机械臂进行轨迹规划，根据具体工作要求给定关键控制点坐标，运用NURBS插补理论进行曲线插补，得到相应平滑曲线，通过用曲线段运动轨迹替代直线段运动轨迹，最终获得机械臂末端的运动轨迹，本发明采用用曲线段运动轨迹可提高码垛机械臂的运行效率和稳定性。附图说明图1是本发明的码垛机械臂末端的运动轨迹规划方法流程。图2是码垛机械臂末端的初步运动轨迹；(a)向低处放置物品的情况；(b)向高处放置物品的情况。图3是运用NURBS插补理论进行曲线插补的具体流程。图4是最终规划获得的码垛机械臂末端的运动轨迹；(a)向低处放置物品的情况；(b)向高处放置物品的情况。具体实施方式下面结合附图对本发明做进一步说明。本发明的码垛机械臂末端的运动轨迹规划方法流程如图1所示，规划开始时，根据工作要求给定关键点坐标，根据关键点坐标得到直线段运动轨迹，在首末段的直线段上选择控制点坐标，运用NURBS插补理论进行曲线插补，得到首末段处的曲线段运动轨迹，具体如下：1、确定关键点坐标码垛机械臂在搬运过程中，由生产流水线到推车存在多条运动轨迹，为实现物品在推车上的整齐码放，需要不断调整轨迹，推车上同一层的码放轨迹根据产品码放位置而定，不同层的码放轨迹仅在高度上存在差异。不同的运动轨迹中均存在几个关键点，包括抓取准备位置P2、抓取位置P1、移动开始位置P3、移动结束位置P4、放置准备位置P5、放置位置P6等，抓取位置P1与抓取准备位置P2垂直分布，高度为H1，抓取准备位置P2与移动开始位置P3水平分布，距离为L1；移动结束位置P4与放置准备位置P5水平分布，距离为L2，放置准备位置P5与放置位置P6垂直分布，高度为H2。高度H1、H2和距离L1、L2由实际工作要求提前给定。对于码放过程中不同的运动轨迹，首先获取抓取位置P1在世界坐标系中的坐标P1(x1，y1，z1)及放置位置P6在世界坐标系中的坐标P6(x2，y2，z2)；其次根据给定高度H1和H2的值计算抓取准备位置P2的坐标P2(x1，y1，z1+H1)、放置准备位置P5的坐标P5(x2，y2，z2+H2)，根据给定距离L1和L2的值计算移动开始位置P3的坐标P3(x1+L1，y1，z1+H1)、移动结束位置P4的坐标P4(x2-L2，y2，z2+H2)。至此运功轨迹中的几个关键点坐标均已给定。2、确定初步运动轨迹根据确定的关键点坐标，码垛机械臂末端的初步运动轨迹由抓取位置P1至移动开始位置P3的首段直线段、移动开始位置P3至移动结束位置P4的中间直线段、移动结束位置P4至放置位置P6的末段直线段三部分组成，如图2所示。因此，码垛机械臂末端的初步运动轨迹为直线段运动轨迹，由直线段P1P2、P2P3、P3P4、P4P5、P5P6组成，其中直线段P1P2和P2P3组成第一直角过渡段，P3P4为中间直线段，直线段P4P5和P5P6组成第二直角过渡段。3、增加控制点坐标物品由抓取位置P1至移动开始位置P3的首段直线段和由移动结束位置P4至放置位置P6的末段直线段轨迹均存在直角过渡情况，为使用NURBS插补理论得到首末段的曲线段运动轨迹，除将现有的关键点坐标作为控制点外，还需额外增加控制点坐标。在已给定的6个控制点坐标的基础上，额外增加控制点C1、C2、C3、C4。控制点C1位于直线段P1P2的中间位置，坐标为C1(x1，y1，z1+H1/2)；控制点C2、C3位于中间直线段P3P4上，P3C2＝C3P4＝kP3P4，k为比例系数，0≤k≤1，根据比例系数k的值，可计算得到控制点C2的坐标：C2(x1+L1+k|(x1+L1)-(x2-L2)|，y1+k|y1-y2|，z1+H1+k|(z1+H1)-(z2+H2)|)、控制点C3的坐标：C3(x2-L2-k|(x1+L1)-(x2-L2)|，y2-k|y1-y2|，z2+H2-k|(z1+H1)-(z2+H2)|)；控制点C4位于P5P6的中间位置，坐标为C4(x2，y2，z2+H2/2)。4、运用NURBS插补理论进行曲线插补由抓取位置P1至移动开始位置P3的首段运动轨迹为直线段，为得到平滑的曲线段运动轨迹，运用NURBS曲线插补理论进行曲线插补。给定控制点P1、C1、P2、P3、C2，采用NURBS曲线插补算法，得到一条经过首末控制点P1和C2的平滑曲线，以此曲线轨迹取代原直线段运动轨迹。移动开始位置P3至移动结束位置P4的轨迹采用直线段，如此可缩短轨迹位移。轨迹插补过程中，可通过调整比例系数k的值，以保证曲线段与直线段连接处的平滑。由移动结束位置P4到放置位置P6的末段运动轨迹为直线段，同样地，采用NURBS插补算法，给定控制点C3、P4、P5、C4、P6，得到一条经过首末控制点C3和P6的平滑曲线，并以此曲线轨迹取代原直线段运动轨迹。NURBS曲线插补的流程如图3所示，具体过程如下。第一步：计算节点向量已知m+p+1个控制点其中m为所生成样条阶数，p为所构造的基函数次数。控制点之间距离其中xj表示控制点Pj的横轴坐标，yj表示控制点Pj的纵轴坐标。根据哈德利—贾德方法递推公式可得：其中ui表示第i个节点。根据上式可得节点向量U递推公式为：求出的节点向量U的形式为：第二步：根据节点向量计算NURBS基函数利用节点向量求得基函数，NURBS基函数有很多种构造形式，一般常用的构造形式是由如下递推公式给出的，用下列递推方式确定的基函数Ni，p(u)称为相应于节点向量U的p次NURBS基函数：其中i是基函数的序列号，给定节点向量U，根据上面的递推公式就可以推导出所需要的基函数。第三步：得到NURBS曲线表达式，根据控制点计算出插补点第i段NURBS曲线Pi(u)的表达式为：参数u∈[ui+p，ui+p+1]均匀变化得到插补点数据。5、得到曲线段运动轨迹通过NURBS曲线插补算法后，由抓取位置P1至C2的首段直线段和由C3至P6间的末段直线段轨迹均由获得的相应平滑曲线段代替。经过NURBS曲线插补算法得到的曲线段运动轨迹如图4所示。曲线段运功轨迹由首末段曲线段和中间直线段三部分组成。相对初步运动轨迹的直线段运动，曲线段运动轨迹可以提高码垛机械臂运动的平稳性。