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

摘要

本发明公开了一种码垛机械臂末端的运动轨迹规划方法，该方法是利用笛卡尔空间坐标系对码垛机械臂进行轨迹规划，根据具体工作要求给定关键控制点坐标，运用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直线段及第二直角过渡段；获得机械臂末端的最终运动轨迹。