码垛机械臂末端的运动轨迹规划方法
【技术领域】
[0001] 本发明设及机器人运动规划领域，尤其设及一种码煤机械臂末端的运动轨迹规划 方法。
【背景技术】
[0002] 码煤机械臂末端的运功轨迹根据工作要求不同而不同，W生产线上产品卸煤堆煤 为例，工业生产中，需将流水线上的产品整齐的码放在托盘或推车上，运一动作简单而又重 复，可用码煤机械臂完成。
[0003] 码煤机械臂在运动过程中，其运动轨迹具有重复性，通过有效的轨迹规划可W保 证运行过程中的平稳和时间要求，进而可W提高码煤机械臂的运行效率和稳定性。
【发明内容】
[0004] 本发明的目的在于针对现有技术的不足，提供一种码煤机械臂末端的运动轨迹规 划方法，通过用曲线段运动轨迹替代直线段运动轨迹，可W提高码煤机械臂的运行效率和 稳定性。
[0005] 本发明采用如下的技术方案：
[0006] 1)确定关键点：W机械臂所要抓取的物品的原始位置为抓取位置Pi，其在世界坐 标系中的坐标Pl(Xl，yl，Zl);W物品目标位置为放置位置P6，其坐标P6(X2，y2，Z2);
[0007] 抓取准备位置P2与抓取位置P1垂直分布，高度差为H1;移动开始位置P3与抓取准 备位置P2水平分布，距离为L1;放置准备位置Pe与放置位置Pe垂直分布，高度差为H2;移动 结束位置P4与放置准备位置P冰平分布，距离为L2;高度H1、&和距离Li、L2由具体工作要 求给定；
[000引因此，其余各关键点坐标分别为：P2(Xi，yi，Zi+Hi)，P3(Xi+Li，yi，Zi+Hi)，P4(X2-L2,72， Z2+H2) , 口5(而，72,Z2+H2);
[000引。确定初步运动轨迹：将上述各关键点依次连接获得直线段PA、P2P3、P3P4、P4P5和PsPe，WP1P2和P2P3作为第一直角过渡段，P3P4作为中间直线段，P4P5和PsPe作为第二直 角过渡段，获得从Pi到Pe的初始运动轨迹；
[0010]如增加控制点拟直线段PA的中间位置为控制点Ci，坐标为Ci(Xi，yi，Zi+Hi/。； P5P6的中间位置为控制点C4,坐标为C4(而，y2,Z2+H2/2);将控制点C2、C3设于中间直线段P3P4 上，口3〔2=〇3口4=1^^口4，1^为比例系数，0《1^《1;
[0011] 因此，控制点C2、C3的坐标分别为：
[0012] 〔2(Xi+Li+k(Xi+Li) -(Xz-Lz) ,y1+kyi_y2,Z1+Hi+k(Z1+Hi)- (Z2+恥）） C3(x；j-L2-k| (Xi+Li)-(x；j-L2)I，y；；-k|yi-y2,Zz+Hz-kl(Zi+Hi)-(Z2+H2)I);
[001引 4)曲线插补：WPi、Cl、PzA、Cz作为控制点，采用NURBS曲线插补理论算法，得到 经过首末控制点Pi和C2的平滑曲线，W该曲线取代第一直角过渡段及P3C2直线段；WC3、 ?4、Ps、C4、Pe作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点C3和Pe的平 滑曲线，w此曲线轨迹取代C3P4直线段及第二直角过渡段；获得机械臂末端的最终运动轨 迹。
[0014] 步骤4)中所述的采用NURBS曲线插补理论算法，得到经过首末控制点的平滑曲 线，具体方法为：
[001引一、计算节点向量
[001引已知m+p+1个控制点巧乂;立巧中m为所生成样条的阶数，P为所构造的基函数次 数；控制点之间距离
，其中Xj表 示控制点P，的横轴坐标，y，表示控制点P，的纵轴坐标。
[0017] 根据哈德利一贾德方法递推公式可得：
[0018]
[0019] 其中Ui表示第i个节点；根据上式可得节点向量U递推公式为：
[0020]
[0021] 求出的节点向量U的形式为：
[0022]
[002引二根据节点向量计算NURBS基函数
[0024] 用下列递推方式确定基函数Ni,p(u)，即相应于节点向量U的P次NURBS基函数：
[00巧]
[0026] 其中i是基函数的序列号；
[0027] S、得到NURBS曲线表达式，根据控制点计算出插补点 [002引第i段NURBS曲线Pi(U)的表达式为：
[0029]
[0030] 参数UG[Ui+p，UiwJ均匀变化得到插补点数据。
[0031] 本发明的有益效果在于：本发明的方法利用笛卡尔空间坐标系对码煤机械臂进行 轨迹规划，根据具体工作要求给定关键控制点坐标，运用NURBS插补理论进行曲线插补，得 到相应平滑曲线，通过用曲线段运动轨迹替代直线段运动轨迹，最终获得机械臂末端的运 动轨迹，本发明采用用曲线段运动轨迹可提高码煤机械臂的运行效率和稳定性。
【附图说明】
[0032]图1是本发明的码煤机械臂末端的运动轨迹规划方法流程。
[0033] 图2是码煤机械臂末端的初步运动轨迹；(a)向低处放置物品的情况；化）向高处 放置物品的情况。
[0034] 图3是运用NURBS插补理论进行曲线插补的具体流程。
[0035] 图4是最终规划获得的码煤机械臂末端的运动轨迹；(a)向低处放置物品的情况； 化）向高处放置物品的情况。
【具体实施方式】
[0036] 下面结合附图对本发明做进一步说明。
[0037] 本发明的码煤机械臂末端的运动轨迹规划方法流程如图1所示，规划开始时，根 据工作要求给定关键点坐标，根据关键点坐标得到直线段运动轨迹，在首末段的直线段上 选择控制点坐标，运用NURBS插补理论进行曲线插补，得到首末段处的曲线段运动轨迹，具 体如下：
[0038] 1、确定关键点坐标
[0039] 码煤机械臂在搬运过程中，由生产流水线到推车存在多条运动轨迹，为实现物品 在推车上的整齐码放，需要不断调整轨迹，推车上同一层的码放轨迹根据产品码放位置而 定，不同层的码放轨迹仅在高度上存在差异。不同的运动轨迹中均存在几个关键点，包括抓 取准备位置P2、抓取位置Pi、移动开始位置P3、移动结束位置P4、放置准备位置Pe、放置位置 Pe等，抓取位置P1与抓取准备位置P2垂直分布，高度为H1，抓取准备位置P2与移动开始位 置P3水平分布，距离为L1;移动结束位置P4与放置准备位置Pe水平分布，距离为L2,放置准 备位置Pe与放置位置Pe垂直分布，高度为H2。高度Hi、&和距离L1、L2由实际工作要求提 前给定。
[0040] 对于码放过程中不同的运动轨迹，首先获取抓取位置Pi在世界坐标系中的坐标 Pi(xi，yi，zi)及放置位置P庙世界坐标系中的坐标Pe(X2,72,Z2);其次根据给定高度Hi和H2 的值计算抓取准备位置的坐标P2 (Xl，yi，Zi+Hi)、放置准备位置Pg的坐标P5 (而，72,Z2+H2)， 根据给定距离Li和L2的值计算移动开始位置P3的坐标P3(Xi+Li，Yi，Zi+Hi)、移动结束位置 ?4的坐标P4 (X2-L2,y2,Z2+H2)。至此运功轨迹中的几个关键点坐标均已给定。
[0041] 2、确定初步运动轨迹
[0042] 根据确定的关键点坐标，码煤机械臂末端的初步运动轨迹由抓取位置Pi至移动开 始位置P3的首段直线段、移动开始位置P3至移动结束位置P4的中间直线段、移动结束位置 P4至放置位置Pe的末段直线段=部分组成，如图2所示。
[0043] 因此，码煤机械臂末端的初步运动轨迹为直线段运动轨迹，由直线段P1P2、P2P3、 P3P4、P4P5、PsPe组成，其中直线段P化和P2P3组成第一直角过渡段，P3P4为中间直线段，直线 段PaPs和P乐e组成第二直角过渡段。
[00