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

摘要

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

主权项

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