[发明专利]基于三角函数的工业机器人轨迹规划升降速控制方法

摘要

本发明提供一种基于三角函数的工业机器人轨迹规划升降速控制方法，首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲线的起止点处的两个期望点的位置和速度参数；然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参数；最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线；最终确定期望的规划轨迹。本发明能够生成平滑连续的曲线，容易实现对期望轨迹的跟踪，适用于工业机器人的轨迹规划。

主权项

1.一种基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于，包括：首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲线的起止点处的两个期望点的位置和速度参数；然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参数；最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线；最终确定期望的规划轨迹；具体步骤为：步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速度的数学模型、加速度的数学模型和加加速度的数学模型；步骤二、给定机器人机械臂运动的起始速度Vs和末端速度Ve均为0、起始期望点位置Ps、轨迹规划期望速度Vel、末端期望点位置Pe，按最大加速度Amax和最大加加速度Jmax进行基于三角函数的机器人机械臂轨迹曲线的规划；步骤三、依据最短时间原则将机器人机械臂从起始期望点到末端期望点的轨迹分为加速段、匀速段、减速段，其中，加速段包括加加速段、匀加速段、减加速段，减速段包括加减速段、匀减速段、减减速段；步骤四、将加速段的加加速段、匀加速段、减加速段的边界条件的值代入步骤一的数学模型中列出方程组并求解数学模型的参数；步骤五、判断是否满足条件：其中，dp_min为规划的加速段最短距离，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，判断结果成立，则执行步骤六，否则执行步骤九；步骤六、判断是否满足条件：Vel≥dvmin，其中，dvmin为速度最小增量，Vel为机器人机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤七，否则执行步骤十四；步骤七、判断是否满足条件：Pe‑Ps≥2(D1+D2+D3)，其中，Ps为机器人机械臂运动的起始期望点位置，Pe为机器人机械臂运动的末端期望点位置，D1为加速段中的加加速段运行距离，D2为加速段中的匀加速段运行距离，D3为加速段中的减加速段运行距离，判断结果成立，则执行步骤八，否则执行步骤九；步骤八、基于三角函数的轨迹按最大加速度Amax和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；步骤九、判断是否满足条件：Vel≥dvmin，其中，dvmin为速度最小增量，Vel为机器人机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤十，否则执行步骤十一；步骤十、对三角函数的轨迹的最大加速度Amax和加速度从零加速到Amax的时间dtmax进行重置，即Areset和dtreset，继而进行规划，获得机器人机械臂期望的输出轨迹，从而实现基于三角函数的工业机器人轨迹规划升降速控制方法；步骤十一、对三角函数的轨迹的最大加速度Amax和加速度从零加速到Amax的时间dtmax进行重置，即Areset和dtreset，继而获得三角函数升降速控制的加速段重置的最大加速度Areset和加速度从零加速到Areset的时间dtreset；步骤十二、判断是否满足条件：判断结果成立，则执行步骤十三，否则执行步骤十四；步骤十三、基于三角函数的轨迹按重置后的最大加速度Areset、加速度从零加速到Areset的时间dtreset和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；步骤十四、基于三角函数的轨迹需要对最大加速度Areset和加速度从零加速到Areset的时间dtreset进行重置，轨迹按重置后的最大加速度Areset、加速度从零加速到Areset的时间dtreset和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出轨迹，实现基于三角函数的工业机器人轨迹规划升降速控制方法。