44] 3、增加控制点坐标
[0045] 物品由抓取位置Pi至移动开始位置P3的首段直线段和由移动结束位置P4至放置 位置Pe的末段直线段轨迹均存在直角过渡情况，为使用NURBS插补理论得到首末段的曲线 段运动轨迹，除将现有的关键点坐标作为控制点外，还需额外增加控制点坐标。在已给定的 6个控制点坐标的基础上，额外增加控制点Cl、C2、C3、C4。
[004引控制点。位于直线段PA的中间位置，坐标为C1 (XI，yi，Zi+Hi/。；控制点C2X3位 于中间直线段P3P4上，口3〔2= 03?4= kP3P4，k为比例系数，0《k《1，根据比例系数k的值， 可计算得到控制点C2的坐标：
[0047] 〔2 (Xi+Li+kI(Xi+Li) - (X厂l2)I，Yi+kIYi-YzI，Zi+Hi+kI(Zi+Hi)-(Z2+H2)I)、担制点〔3的 坐标：
[004引〔3 (Xjj-Lz-k I(Xi+Li) - (Xjj-Lz)I，y；；-k IYi-YzI，Zz+Hz-kI(Zi+Hi)- (Z2+H2) I);担审Ij 点〔4 位于PsPe的中间位置，坐标为C 4 (而，y2, Z2+H2/2)。
[0049] 4、运用NURBS插补理论进行曲线插补
[0050] 由抓取位置Pi至移动开始位置P3的首段运动轨迹为直线段，为得到平滑的曲线段 运动轨迹，运用NURBS曲线插补理论进行曲线插补。给定控制点口1、(：1、口2、口3八2，采用饥]1?8 曲线插补算法，得到一条经过首末控制点Pi和C2的平滑曲线，W此曲线轨迹取代原直线段 运动轨迹。
[0051] 移动开始位置P3至移动结束位置P4的轨迹采用直线段，如此可缩短轨迹位移。轨 迹插补过程中，可通过调整比例系数k的值，W保证曲线段与直线段连接处的平滑。
[0052] 由移动结束位置P4到放置位置Pe的末段运动轨迹为直线段，同样地，采用NURBS 插补算法，给定控制点C3、P4、Pe、C4、Pe，得到一条经过首末控制点C3和Pe的平滑曲线，并W 此曲线轨迹取代原直线段运动轨迹。
[005引 NURBS曲线插补的流程如图3所示，具体过程如下。
[0054] 第一步：计算节点向量
[0055] 已知m+p+1个控制点巧培T，其中m为所生成样条阶数，P为所构造的基函数次数。 控制点之间距离
[005引
（,/ .= 1，2足..泣乎滅，.其中Xj表示控制点PJ 的横轴坐标，y，表示控制点P，的纵轴坐标。
[0057] 根据哈德利一贾德方法递推公式可得：
[0058]
其
[0059] 中Ui表示第i个节点。根据上式可得节点向量U递推公式为：
[0060]
[0061] 求出的节点向量U的形式为：
[0062]
[006引第二步：根据节点向量计算NURBS基函数
[0064] 利用节点向量求得基函数，NURBS基函数有很多种构造形式，一般常用的构造形式 是由如下递推公式给出的，用下列递推方式确定的基函数Ni,p(u)称为相应于节点向量U的 P次NURBS基函数：
[0065]
[0066] 其中i是基函数的序列号，给定节点向量U，根据上面的递推公式就可W推导出所 需要的基函数。
[0067] 第立步：得到NURBS曲线表达式，根据控制点计算出插补点 [006引第i段NURBS曲线Pi(U)的表达式为：
[0069]
[0070] 参数UG[Ui+p，UiwJ均匀变化得到插补点数据。
[0071] 5、得到曲线段运动轨迹
[007引通过NURBS曲线插补算法后，由抓取位置P產C2的首段直线段和由C產P6间的 末段直线段轨迹均由获得的相应平滑曲线段代替。经过NURBS曲线插补算法得到的曲线段 运动轨迹如图4所示。
[0073] 曲线段运功轨迹由首末段曲线段和中间直线段=部分组成。相对初步运动轨迹的 直线段运动，曲线段运动轨迹可W提高码煤机械臂运动的平稳性。
【主权项】
1. 码垛机械臂末端的运动轨迹规划方法，其特征在于，包括以下步骤： 1) 确定关键点：以机械臂所要抓取的物品的原始位置为抓取位置P1，其在世界坐标系 中的坐标P1 (X1, Y1, Z1);以物品目标位置为放置位置P6,其坐标P6 (x2, y2, Z2); 抓取准备位置P2与抓取位置P 1垂直分布，高度差为H 1;移动开始位置P 3与抓取准备位 置P2水平分布，距离为L i;放置准备位置P 5与放置位置P 6垂直分布，高度差为H 2;移动结束 位置匕与放置准备位置P 5水平分布，距离为L 2;高度H ρΗ2和距离L i、L2由具体工作要求给 定； 因此，其余各关键点坐标分别为：P2 (Xi，yi，Zq+氏），P3 (X1+!^, Y1, ZfH1)，P4 (X2-L2, y2, z2+H 2)，P5 (x2, y2, z2+H2); 2) 确定初步运动轨迹：将上述各关键点依次连接获得直线段P1Pp P2P3、P3P4、P4P 5和 P5P6，以P1P2和P 2P3作为第一直角过渡段，P 3P4作为中间直线段，P 4P5和P 5P6作为第二直角过 渡段获得从P$|」P 6的初始运动轨迹； 3) 增加控制点：以直线段P1P2的中间位置为控制点C i，坐标为C1 (Xl，yi，a+H/2) ;P5P6 的中间位置为控制点(：4，坐标为(：4〇^72,22+!1 2/2);将控制点(：2、(：3设于中间直线段？丨4上， P3C2= C 3P4= kP 3P4, k 为比例系数，O < k < 1 ; 因此，控制点C2、C3的坐标分别为：4) 曲线插补：以P1ApPrPy C2作为控制点，采用NURBS曲线插补理论算法，得到经过 首末控制点PjP C 2的平滑曲线，以该曲线取代第一直角过渡段及P 3C2直线段；以C 3、P4、P5、 C4、P6作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点(：3和P 6的平滑曲线， 以此曲线轨迹取代C3P4直线段及第二直角过渡段；获得机械臂末端的最终运动轨迹。2. 根据权利要求1所述的码垛机械臂末端的运动轨迹规划方法，其特征在于，步骤4) 中所述的采用NURBS曲线插补理论算法，得到经过首末控制点的平滑曲线，具体方法为： 一、计算节点向量 已知m+p+1个控制点丨/U=纟，其中m为所生成样条的阶数，p为所构造的基函数次数； 控制点之间距离，其中Xj表示控 制点Pj的横轴坐标，y _j表示控制点P _j的纵轴坐标； 根据哈德利一贾德方法递推公式可得：其中U1表示第i个节点；根据上式可得节点向量U递推公式为：求出的节点向量U的形式为：二、 根据节点向量计算NURBS基函数 用下列递推方式确定基函数\p(u)，即相应于节点向量U的p次NURBS基函数：其中i是基函数的序列号； 三、 得到NURBS曲线表达式，根据控制点计算出插补点 第i段NURBS曲线P1 (u)的表达式为：参数u e [Ui+p，Ui+p+1]均匀变化得到插补点数据。
【专利摘要】本发明公开了一种码垛机械臂末端的运动轨迹规划方法，该方法是利用笛卡尔空间坐标系对码垛机械臂进行轨迹规划，根据具体工作要求给定关键控制点坐标，运用NURBS插补理论进行曲线插补，得到相应平滑曲线，通过用曲线段运动轨迹替代直线段运动轨迹，最终获得机械臂末端的运动轨迹，本发明采用曲线段运动轨迹可提高码垛机械臂的运行效率和稳定性。
【IPC分类】B25J9/16
【公开号】CN105171743
【申请号】
【发明人】史伟民, 葛宏伟, 杨亮亮, 许守金
【申请人】浙江理工大学
【公开日】2015年12月23日
【申请日】2015年7月29日