码垛机械臂末端的运动轨迹规划方法技术

本发明专利技术公开了一种码垛机械臂末端的运动轨迹规划方法，该方法是利用笛卡尔空间坐标系对码垛机械臂进行轨迹规划，根据具体工作要求给定关键控制点坐标，运用NURBS插补理论进行曲线插补，得到相应平滑曲线，通过用曲线段运动轨迹替代直线段运动轨迹，最终获得机械臂末端的运动轨迹，本发明专利技术采用曲线段运动轨迹可提高码垛机械臂的运行效率和稳定性。

【技术实现步骤摘要】
码垛机械臂末端的运动轨迹规划方法
本专利技术涉及机器人运动规划领域，尤其涉及一种码垛机械臂末端的运动轨迹规划方法。
技术介绍
码垛机械臂末端的运功轨迹根据工作要求不同而不同，以生产线上产品卸垛堆垛为例，工业生产中，需将流水线上的产品整齐的码放在托盘或推车上，这一动作简单而又重复，可用码垛机械臂完成。码垛机械臂在运动过程中，其运动轨迹具有重复性，通过有效的轨迹规划可以保证运行过程中的平稳和时间要求，进而可以提高码垛机械臂的运行效率和稳定性。
技术实现思路
本专利技术的目的在于针对现有技术的不足，提供一种码垛机械臂末端的运动轨迹规划方法，通过用曲线段运动轨迹替代直线段运动轨迹，可以提高码垛机械臂的运行效率和稳定性。本专利技术采用如下的技术方案：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本文档来自技高网...

【技术保护点】
码垛机械臂末端的运动轨迹规划方法，其特征在于，包括以下步骤：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直线段及第二直角过渡段；获得机械臂末端的最终运动轨迹。...

【技术特征摘要】
1.码垛机械臂末端的运动轨迹规划方法，其特征在于，包括以下步骤：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的平...

【专利技术属性】
技术研发人员：史伟民，葛宏伟，杨亮亮，许守金，
申请(专利权)人：浙江理工大学，
类型：发明
国别省市：浙江;33