[发明专利]码垛机械臂末端的运动轨迹规划方法

权利要求书

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]均匀变化得到插补点数据。