CN105171743B - 码垛机械臂末端的运动轨迹规划方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

码垛机械臂末端的运功轨迹根据工作要求不同而不同，以生产线上产品卸垛堆垛为例，工业生产中，需将流水线上的产品整齐的码放在托盘或推车上，这一动作简单而又重复，可用码垛机械臂完成。

码垛机械臂在运动过程中，其运动轨迹具有重复性，通过有效的轨迹规划可以保证运行过程中的平稳和时间要求，进而可以提高码垛机械臂的运行效率和稳定性。

发明内容

本发明的目的在于针对现有技术的不足，提供一种码垛机械臂末端的运动轨迹规划方法，通过用曲线段运动轨迹替代直线段运动轨迹，可以提高码垛机械臂的运行效率和稳定性。

本发明采用如下的技术方案：

1)确定关键点：以机械臂所要抓取的物品的原始位置为抓取位置P₁，其在世界坐标系中的坐标P₁(x₁，y₁，z₁)；以物品目标位置为放置位置P₆，其坐标P₆(x₂，y₂，z₂)；

抓取准备位置P₂与抓取位置P₁垂直分布，高度差为H₁；移动开始位置P₃与抓取准备位置P₂水平分布，距离为L₁；放置准备位置P₅与放置位置P₆垂直分布，高度差为H₂；移动结束位置P₄与放置准备位置P₅水平分布，距离为L₂；高度H₁、H₂和距离L₁、L₂由具体工作要求给定；

因此，其余各关键点坐标分别为：P₂(x₁，y₁，z₁+H₁)，P₃(x₁+L₁，y₁，z₁+H₁)，P₄(x₂-L₂，y₂，z₂+H₂)，P₅(x₂，y₂，z₂+H₂)；

2)确定初步运动轨迹：将上述各关键点依次连接获得直线段P₁P₂、P₂P₃、P₃P₄、P₄P₅和P₅P₆，以P₁P₂和P₂P₃作为第一直角过渡段，P₃P₄作为中间直线段，P₄P₅和P₅P₆作为第二直角过渡段，获得从P₁到P₆的初始运动轨迹；

3)增加控制点：以直线段P₁P₂的中间位置为控制点C₁，坐标为C₁(x₁，y₁，z₁+H₁/2)；P₅P₆的中间位置为控制点C₄，坐标为C₄(x₂，y₂，z₂+H₂/2)；将控制点C₂、C₃设于中间直线段P₃P₄上，P₃C₂＝C₃P₄＝kP₃P₄，k为比例系数，0≤k≤1；

因此，控制点C₂、C₃的坐标分别为：

C₂(x₁+L₁+k|(x₁+L₁)-(x₂-L₂)|，y₁+k|y₁-y₂|，z₁+H₁+k|(z₁+H₁)-(z₂+H₂)|)C₃(x₂-L₂-k|(x₁+L₁)-(x₂-L₂)|，y₂-k|y₁-y₂，z₂+H₂-k|(z₁+H₁)-(z₂+H₂)|)；

4)曲线插补：以P₁、C₁、P₂、P₃、C₂作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点P₁和C₂的平滑曲线，以该曲线取代第一直角过渡段及P₃C₂直线段；以C₃、P₄、P₅、C₄、P₆作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点C₃和P₆的平滑曲线，以此曲线轨迹取代C₃P₄直线段及第二直角过渡段；获得机械臂末端的最终运动轨迹。

步骤4)中所述的采用NURBS曲线插补理论算法，得到经过首末控制点的平滑曲线，具体方法为：

一、计算节点向量

已知m+p+1个控制点其中m为所生成样条的阶数，p为所构造的基函数次数；控制点之间距离其中x_j表示控制点P_j的横轴坐标，y_j表示控制点P_j的纵轴坐标。

根据哈德利—贾德方法递推公式可得：

其中u_i表示第i个节点；根据上式可得节点向量U递推公式为：

求出的节点向量U的形式为：

二、根据节点向量计算NURBS基函数

用下列递推方式确定基函数N_i，p(u)，即相应于节点向量U的p次NURBS基函数：

其中i是基函数的序列号；

三、得到NURBS曲线表达式，根据控制点计算出插补点

第i段NURBS曲线P_i(u)的表达式为：

参数u∈[u_i+p，u_i+p+1]均匀变化得到插补点数据。

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

附图说明

图1是本发明的码垛机械臂末端的运动轨迹规划方法流程。

图2是码垛机械臂末端的初步运动轨迹；(a)向低处放置物品的情况；(b)向高处放置物品的情况。

图3是运用NURBS插补理论进行曲线插补的具体流程。

图4是最终规划获得的码垛机械臂末端的运动轨迹；(a)向低处放置物品的情况；(b)向高处放置物品的情况。

具体实施方式

下面结合附图对本发明做进一步说明。

本发明的码垛机械臂末端的运动轨迹规划方法流程如图1所示，规划开始时，根据工作要求给定关键点坐标，根据关键点坐标得到直线段运动轨迹，在首末段的直线段上选择控制点坐标，运用NURBS插补理论进行曲线插补，得到首末段处的曲线段运动轨迹，具体如下：

1、确定关键点坐标

码垛机械臂在搬运过程中，由生产流水线到推车存在多条运动轨迹，为实现物品在推车上的整齐码放，需要不断调整轨迹，推车上同一层的码放轨迹根据产品码放位置而定，不同层的码放轨迹仅在高度上存在差异。不同的运动轨迹中均存在几个关键点，包括抓取准备位置P₂、抓取位置P₁、移动开始位置P₃、移动结束位置P₄、放置准备位置P₅、放置位置P₆等，抓取位置P₁与抓取准备位置P₂垂直分布，高度为H₁，抓取准备位置P₂与移动开始位置P₃水平分布，距离为L₁；移动结束位置P₄与放置准备位置P₅水平分布，距离为L₂，放置准备位置P₅与放置位置P₆垂直分布，高度为H₂。高度H₁、H₂和距离L₁、L₂由实际工作要求提前给定。

对于码放过程中不同的运动轨迹，首先获取抓取位置P₁在世界坐标系中的坐标P₁(x₁，y₁，z₁)及放置位置P₆在世界坐标系中的坐标P₆(x₂，y₂，z₂)；其次根据给定高度H₁和H₂的值计算抓取准备位置P₂的坐标P₂(x₁，y₁，z₁+H₁)、放置准备位置P₅的坐标P₅(x₂，y₂，z₂+H₂)，根据给定距离L₁和L₂的值计算移动开始位置P₃的坐标P₃(x₁+L₁，y₁，z₁+H₁)、移动结束位置P₄的坐标P₄(x₂-L₂，y₂，z₂+H₂)。至此运功轨迹中的几个关键点坐标均已给定。

2、确定初步运动轨迹

根据确定的关键点坐标，码垛机械臂末端的初步运动轨迹由抓取位置P₁至移动开始位置P₃的首段直线段、移动开始位置P₃至移动结束位置P₄的中间直线段、移动结束位置P₄至放置位置P₆的末段直线段三部分组成，如图2所示。

因此，码垛机械臂末端的初步运动轨迹为直线段运动轨迹，由直线段P₁P₂、P₂P₃、P₃P₄、P₄P₅、P₅P₆组成，其中直线段P₁P₂和P₂P₃组成第一直角过渡段，P₃P₄为中间直线段，直线段P₄P₅和P₅P₆组成第二直角过渡段。

3、增加控制点坐标

物品由抓取位置P₁至移动开始位置P₃的首段直线段和由移动结束位置P₄至放置位置P₆的末段直线段轨迹均存在直角过渡情况，为使用NURBS插补理论得到首末段的曲线段运动轨迹，除将现有的关键点坐标作为控制点外，还需额外增加控制点坐标。在已给定的6个控制点坐标的基础上，额外增加控制点C₁、C₂、C₃、C₄。

控制点C₁位于直线段P₁P₂的中间位置，坐标为C₁(x₁，y₁，z₁+H₁/2)；控制点C₂、C₃位于中间直线段P₃P₄上，P₃C₂＝C₃P₄＝kP₃P₄，k为比例系数，0≤k≤1，根据比例系数k的值，可计算得到控制点C₂的坐标：

C₂(x₁+L₁+k|(x₁+L₁)-(x₂-_L2)|，y₁+k|y₁-y₂|，z₁+H₁+k|(z₁+H₁)-(z₂+H₂)|)、控制点C₃的坐标：

C₃(x₂-L₂-k|(x₁+L₁)-(x₂-L₂)|，y₂-k|y₁-y₂|，z₂+H₂-k|(z₁+H₁)-(z₂+H₂)|)；控制点C₄位于P₅P₆的中间位置，坐标为C₄(x₂，y₂，z₂+H₂/2)。

4、运用NURBS插补理论进行曲线插补

由抓取位置P₁至移动开始位置P₃的首段运动轨迹为直线段，为得到平滑的曲线段运动轨迹，运用NURBS曲线插补理论进行曲线插补。给定控制点P₁、C₁、P₂、P₃、C₂，采用NURBS曲线插补算法，得到一条经过首末控制点P₁和C₂的平滑曲线，以此曲线轨迹取代原直线段运动轨迹。

移动开始位置P₃至移动结束位置P₄的轨迹采用直线段，如此可缩短轨迹位移。轨迹插补过程中，可通过调整比例系数k的值，以保证曲线段与直线段连接处的平滑。

由移动结束位置P₄到放置位置P₆的末段运动轨迹为直线段，同样地，采用NURBS插补算法，给定控制点C₃、P₄、P₅、C₄、P₆，得到一条经过首末控制点C₃和P₆的平滑曲线，并以此曲线轨迹取代原直线段运动轨迹。

NURBS曲线插补的流程如图3所示，具体过程如下。

第一步：计算节点向量

已知m+p+1个控制点其中m为所生成样条阶数，p为所构造的基函数次数。控制点之间距离

其中x_j表示控制点P_j的横轴坐标，y_j表示控制点P_j的纵轴坐标。

根据哈德利—贾德方法递推公式可得：

其

中u_i表示第i个节点。根据上式可得节点向量U递推公式为：

求出的节点向量U的形式为：

第二步：根据节点向量计算NURBS基函数

利用节点向量求得基函数，NURBS基函数有很多种构造形式，一般常用的构造形式是由如下递推公式给出的，用下列递推方式确定的基函数N_i，p(u)称为相应于节点向量U的p次NURBS基函数：

其中i是基函数的序列号，给定节点向量U，根据上面的递推公式就可以推导出所需要的基函数。

第三步：得到NURBS曲线表达式，根据控制点计算出插补点

第i段NURBS曲线P_i(u)的表达式为：

参数u∈[u_i+p，u_i+p+1]均匀变化得到插补点数据。

5、得到曲线段运动轨迹

通过NURBS曲线插补算法后，由抓取位置P₁至C₂的首段直线段和由C₃至P₆间的末段直线段轨迹均由获得的相应平滑曲线段代替。经过NURBS曲线插补算法得到的曲线段运动轨迹如图4所示。

曲线段运功轨迹由首末段曲线段和中间直线段三部分组成。相对初步运动轨迹的直线段运动，曲线段运动轨迹可以提高码垛机械臂运动的平稳性。

Claims (2)

1.码垛机械臂末端的运动轨迹规划方法，其特征在于，包括以下步骤：

1)确定关键点：以机械臂所要抓取的物品的原始位置为抓取位置P₁，其在世界坐标系中的坐标P₁(x₁,y₁,z₁)；以物品目标位置为放置位置P₆，其坐标P₆(x₂,y₂,z₂)；

抓取准备位置P₂与抓取位置P₁垂直分布，高度差为H₁；移动开始位置P₃与抓取准备位置P₂水平分布，距离为L₁；放置准备位置P₅与放置位置P₆垂直分布，高度差为H₂；移动结束位置P₄与放置准备位置P₅水平分布，距离为L₂；高度H₁、H₂和距离L₁、L₂由具体工作要求给定；

因此，其余各关键点坐标分别为：P₂(x₁,y₁,z₁+H₁)，P₃(x₁+L₁,y₁,z₁+H₁)，P₄(x₂-L₂,y₂,z₂+H₂)，P₅(x₂,y₂,z₂+H₂)；

2)确定初步运动轨迹：将上述各关键点依次连接获得直线段P₁P₂、P₂P₃、P₃P₄、P₄P₅和P₅P₆，以P₁P₂和P₂P₃作为第一直角过渡段，P₃P₄作为中间直线段，P₄P₅和P₅P₆作为第二直角过渡段获得从P₁到P₆的初始运动轨迹；

3)增加控制点：以直线段P₁P₂的中间位置为控制点C₁，坐标为C₁(x₁,y₁,z₁+H₁/2)；P₅P₆的中间位置为控制点C₄，坐标为C₄(x₂,y₂,z₂+H₂/2)；将控制点C₂、C₃设于中间直线段P₃P₄上，P₃C₂＝C₃P₄＝kP₃P₄，k为比例系数，0≤k≤1；

因此，控制点C₂、C₃的坐标分别为：

C₂(x₁+L₁+k|(x₁+L₁)-(x₂-L₂)|,y₁+k|y₁-y₂|,z₁+H₁+k|(z₁+H₁)-(z₂+H₂)|)

C₃(x₂-L₂-k|(x₁+L₁)-(x₂-L₂)|,y₂-k|y₁-y₂|,z₂+H₂-k|(z₁+H₁)-(z₂+H₂)|)；

4)曲线插补：以P₁、C₁、P₂、P₃、C₂作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点P₁和C₂的平滑曲线，以该曲线取代第一直角过渡段及P₃C₂直线段；以C₃、P₄、P₅、C₄、P₆作为控制点，采用NURBS曲线插补理论算法，得到经过首末控制点C₃和P₆的平滑曲线，以此曲线轨迹取代C₃P₄直线段及第二直角过渡段；获得机械臂末端的最终运动轨迹。

2.根据权利要求1所述的码垛机械臂末端的运动轨迹规划方法，其特征在于，步骤4)中所述的采用NURBS曲线插补理论算法，得到经过首末控制点的平滑曲线，具体方法为：

一、计算节点向量

已知m+p+1个控制点其中m为所生成样条的阶数，p为所构造的基函数次数；控制点之间距离 $d_j=\sqrt{{(x_j-x_{j-1})}^2+{(y_j-y_{j-1})}^2},(j=1,2,3...m+p),$ 其中x_j表示控制点P_j的横轴坐标，y_j表示控制点P_j的纵轴坐标；

根据哈德利—贾德方法递推公式可得：

$u_i-u_{i-1}=(m+1)\underset{j=i-p}{\overset{j-1}{\Σ}}{d}_j/\left(\underset{i=p+1}{\overset{m+1}{\Σ}}\underset{j=i-p}{\overset{i-1}{\Σ}}{d}_j\right),(i=p+1,p+2,...m+p),$

其中u_i表示第i个节点；根据上式可得节点向量U递推公式为：

$\{\begin{array}{c}{u}_p=0\\ u_i=(m+1)\underset{j=p+1}{\overset{i}{\mathrm{\Σ}}}(u_j-u_{j-1})\\ u_{m+p+1}=m+1\end{array},(i=p+1,p+2,...m+p)$

求出的节点向量U的形式为：

二、根据节点向量计算NURBS基函数

用下列递推方式确定基函数N_i,p(u)，即相应于节点向量U的p次NURBS基函数：

其中i是基函数的序列号；

三、得到NURBS曲线表达式，根据控制点计算出插补点

第i段NURBS曲线P_i(u)的表达式为：

$P_i\left(u\right)=\underset{n=0}{\overset{p}{\Σ}}{W}_{i+n}{P}_{i+n}{N}_{n-p,p}\left(u\right)/\underset{n=0}{\overset{p}{\Σ}}{W}_{i+n}{N}_{n-p,p}\left(u\right),(i=0,1,2,3...m)$

参数u∈[u_i+p,u_i+p+1]均匀变化得到插补点数据。