CN105867376A - 基于三角函数的工业机器人轨迹规划升降速控制方法 - Google Patents

Abstract

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

Description

基于三角函数的工业机器人轨迹规划升降速控制方法

技术领域

本发明涉及一种基于三角函数的工业机器人轨迹规划升降速控制方法，属于机器人轨迹规划领域。

背景技术

现代制造业对机器人性能的要求也越来越高，而机器人运动轨迹规划算法在机器人控制中占有重要地位，直接影响着机器人末端执行器在加工过程中的性能和效率。

通常情况下，机器人的期望轨迹是事先给定一系列笛卡尔或者关节空间的点，且给定通过该点的速度或两点间的时间，另外还会限制机器人运动允许的最大速度。轨迹规划的目的就是根据这些约束条件，建立通过这些点的平滑轨迹。为了缩短运动路径，通常规划这些点之间通过直线相连，并且在连续点处通过曲线过渡。最为常用的是梯形升降速与S型曲线升降速控制轨迹规划。

梯形升降速控制拟合的线性段轨迹包括加速段、匀速段和减速段，在考虑最大运行速度的情况下达到经历时间最短，可很好的保证轨迹的位置的连续，如图2-1至图2-4所示，但该方法在过渡点t₁、t₂处存在速度的较小突变和加速度突变，以及无穷大的加速度导数值，这意味着驱动力矩及驱动力矩一阶导数突然增大，由此生成的轨迹并不容易跟踪，并且造成机器人振动，导致加工出的零部件表面不光滑；S型曲线升降速控制拟合的线性段轨迹包括加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，如图3-1至图3-4所示，该升降速模式没有速度突变，过渡较平滑，而加速度变化连续但在t₁、t₂处仍存在突变，仍会对机器人关节造成一定程度的柔性冲击。

发明内容

本发明的目的是为解决现有机器人轨迹采用梯形升降速和S型曲线升降速拟合的线性段轨迹的方法存在加速度的阶跃突变，速度变化对关节冲击较大，使驱动力矩及驱动力矩一阶导数突然增大，从而导致轨迹不容易跟踪的问题，提供了一种基于三角函数的工业机器人轨迹规划升降速控制方法。

本发明的技术解决方案是：

一种基于三角函数的工业机器人轨迹规划升降速控制方法，包括：

首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲线的起止点处的两个期望点的位置和速度参数；

然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参数；

最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线；

最终确定期望的规划轨迹。

进一步地，具体步骤为：

步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速度的数学模型、加速度的数学模型和加加速度的数学模型；

步骤二、给定机器人机械臂运动的起始速度V_s和末端速度V_e均为0、起始期望点位置P_s、轨迹规划期望速度V_el、末端期望点位置P_e，按最大加速度A_max和最大加加速度J_max进行基于三角函数的机器人机械臂轨迹曲线的规划；

步骤三、依据最短时间原则将机器人机械臂从起始期望点到末端期望点的轨迹分为加速段、匀速段、减速段，其中，加速段包括加加速段、匀加速段、减加速段，减速段包括加减速段、匀减速段、减减速段；

步骤四、将加速段的加加速段、匀加速段、减加速段的边界条件的值代入步骤一的数学模型中列出方程组并求解数学模型的参数；

步骤五、判断是否满足条件：其中，dp_min为规划的加速段最短距离，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，判断结果成立，则执行步骤六，否则执行步骤九；

步骤六、判断是否满足条件：V_el≥dv_min，其中，dv_min为速度最小增量，V_el为机器人机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤七，否则执行步骤十四；

步骤七、判断是否满足条件：P_e-P_s≥2(D₁+D₂+D₃)，其中，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，D₁为加速段中的加加速段运行距离，D₂为加速段中的匀加速段运行距离，D₃为加速段中的减加速段运行距离，判断结果成立，则执行步骤八，否则执行步骤九；

步骤八、基于三角函数的轨迹按最大加速度A_max和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

步骤九、判断是否满足条件：V_el≥dv_min，其中，dv_min为速度最小增量，V_el为机器人机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤十，否则执行步骤十一；

步骤十、对三角函数的轨迹的最大加速度A_max和加速度从零加速到A_max的时间dt_max进行重置，即A_reset和dt_reset，继而进行规划，获得机器人机械臂期望的输出轨迹，从而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

步骤十一、对三角函数的轨迹的最大加速度A_max和加速度从零加速到A_max的时间dt_max进行重置，即A_reset和dt_reset，继而获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset；

步骤十二、判断是否满足条件：判断结果成立，则执行步骤十三，否则执行步骤十四；

步骤十三、基于三角函数的轨迹按重置后的最大加速度A_reset、加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

步骤十四、基于三角函数的轨迹按最大加速度A_max和加速度从零加速到A_max的时间dt_max进行重置，即A_reset和dt_reset，继而进行规划，获得机器人机械臂期望的输出轨迹，实现基于三角函数的工业机器人轨迹规划升降速控制方法。

进一步地，步骤一中，基于三角函数的工业机器人轨迹规划升降速控制的加加速度即加速度的一阶导数的数学模型J(t)为：

$J\left(t\right)=\left\{\begin{array}{cc}{k}_1\cos (\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})& (0\&le;t<{\mathrm{dt}}_{\max })\\ 0& (0\&le;t<{\mathrm{dt}}_{\max })\\ -k_1\cos (\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})& (0\&le;t<{\mathrm{dt}}_{\max })\end{array}\right.---\left(1\right)$

式(1)中，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，dt_mu为加速段中的的匀加速段时间；

基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加加速度的数学模型进行积分，获得其加速度的数学模型A(t)为：

$A\left(t\right)=\left\{\begin{array}{cc}\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}& (0\&le;t<{\mathrm{dt}}_{max})\\ A_{\max }& (0\&le;t<{\mathrm{dt}}_{mu})\\ -\frac{k_1{\mathrm{dt}}_{\max }}{\mathrm{\&pi;}}sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(2\right)$

式(2)中，A_max为最大加速度，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间；

基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加速度的数学模型进行积分，获得其速度的数学模型V(t)为：

$V\left(t\right)=\left\{\begin{array}{cc}-\frac{k_1{\mathrm{dt}}_{\max }^2}{{\mathrm{\&pi;}}^2}cos(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}t+V_s& (0\&le;t<{\mathrm{dt}}_{max})\\ A_{\max }t+V_a& (0\&le;t<{\mathrm{dt}}_{mu})\\ \frac{k_1{\mathrm{dt}}_{\max }^2}{{\mathrm{\&pi;}}^2}cos(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{\max }}{\mathrm{\&pi;}}t+V_b& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(3\right)$

式(3)中，A_max为最大加速度，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，V_s为轨迹规划的起始速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度，V_b为匀加速段的最大速度同时也是减加速段的起始速度；

基于三角函数的工业机器人轨迹规划升降速控制的加速段，对速度的数学模型进行积分，获得其位置的数学模型P(t)为：

$P\left(t\right)=\left\{\begin{array}{cc}-\frac{k_1{\mathrm{dt}}_{\max }^3}{{\mathrm{\&pi;}}^3}\&lsqb;sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+1\&rsqb;+\frac{k_1{\mathrm{dt}}_{max}}{2\mathrm{\&pi;}}{t}^2+V_{s}t+P_s& (0\&le;t<{\mathrm{dt}}_{max})\\ \frac{1}{2}{A}_{max}{t}^2+V_{a}t+P_s+D_1& (0\&le;t<{\mathrm{dt}}_{mu})\\ \frac{k_1{\mathrm{dt}}_{\max }^3}{{\mathrm{\&pi;}}^3}\&lsqb;sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+1\&rsqb;+\frac{k_1{\mathrm{dt}}_{\max }}{2\mathrm{\&pi;}}{t}^2+V_{b}t+P_s+D_1+D_2& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(4\right)$

式(4)中，A_max为最大加速度，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，V_s为轨迹规划的起始速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度，V_b为匀加速段的最大速度同时也是减加速段的起始速度，P_s为起始期望点位置，D₁为加加速段位移增量，D₂为匀加速段位移增量。

进一步地，步骤三具体为：

速度倾斜上升的加加速段的时间区间为[0,dt_max]，速度由V_s增加到V_a，运行距离为D₁，加速度达到最大值A_max；

速度倾斜上升的匀加速段的时间区间为[dt_max,dt_max+dt_mu]，速度由V_a均匀增加到V_b，运行距离为D₂，加速度保持最大值A_max，加速度的导数加加速度保持为0；

速度倾斜上升的减加速段的时间区间为[dt_max+dt_mu,2dt_max+dt_mu]，速度由V_b增加到轨迹规划期望速度V_el，运行距离为D₃，加速度由最大值A_max下降到0；

速度平稳的匀速段的时间区间为[2dt_max+dt_mu,2dt_max+dt_mu+dt_u]，速度保持轨迹规划期望速度V_el，运行距离为D₄，加速度保持为0，加速度的导数加加速度保持为0；

速度倾斜下降的减加速段的时间区间为[2dt_max+dt_mu+dt_u,3dt_max+dt_mu+dt_u]，速度由轨迹规划期望速度V_el下降到速度V_b，运行距离为D₅，加速度由0下降到反方向最大加速度值-A_max；

速度倾斜下降的匀减速段的时间区间为[3dt_max+dt_mu+dt_u,3dt_max+2dt_mu+dt_u]，速度由V_b均匀下降到速度V_a，运行距离为D₆，加速度保持反方向最大加速度值-A_max，加速度的导数加加速度保持为0；

速度倾斜下降的减减速段的时间区间为[3dt_max+2dt_mu+dt_u,4dt_max+2dt_mu+dt_u]，速度由V_a均匀下降到速度0，运行距离为D₇，加速度由反方向最大加速度值-A_max下降到0。

进一步地，步骤四所述的求解数学模型参数的方法为：

加速度在加加速段满足在dt_max时间达到加速度的最大值A_max，所述基于三角函数的工业机器人轨迹规划曲线在时间t＝0时，位置P(0)、速度V(0)、加速度A(0)、加加速度J(0)满足的约束条件为：

$\left\{\begin{array}{c}P\left(0\right)=P_s\\ V\left(0\right)=V_s\\ A\left(0\right)=0\\ J\left(0\right)=0\end{array}\right.---\left(6\right)$

式(6)中，P_s为机器人机械臂运动的起始期望点位置，V_s为轨迹规划的起始速度；

基于三角函数的工业机器人轨迹规划曲线在时间t＝dt_max时，位置P(dt_max)、速度V(dt_max)、加速度A(dt_max)、加加速度J(dt_max)满足的约束条件为：

$\left\{\begin{array}{c}P\left({\mathrm{dt}}_{\max }\right)=P_s+D_1\\ V\left({\mathrm{dt}}_{\max }\right)=V_s+V_a\\ A\left({\mathrm{dt}}_{\max }\right)=A_{\max }\\ J\left({\mathrm{dt}}_{\max }\right)=0\end{array}\right.---\left(7\right),$

式(7)中，P_s为机器人机械臂运动的起始期望点位置，D₁为加速段中的加加速段运行距离，V_s为轨迹规划的起始速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度；

将式(6)和式(7)代入式(1)至式(4)中，获得速度倾斜上升的加加速段末端的最大速度V_a和加加速段的运行距离增量D₁：

$\left\{\begin{array}{c}{V}_a=V_s+\frac{A_{\max }{\mathrm{dt}}_{max}}{2}\\ D_1=A_{max}{\mathrm{dt}}_{max}^2(\frac{1}{4}-\frac{1}{{\mathrm{\&pi;}}^2})+V_s{\mathrm{dt}}_{max}\end{array}\right.---\left(8\right),$

式(8)中，D₁为加速段中的加加速段运行距离，V_s为机器人机械臂运动的轨迹规划的起始速度，A_max为最大加速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度；

由匀加速段末端速度V_b加速到减加速段末端速度V_el的时间同样为dt_max，则匀加速段末端速度V_b为：

$V_b=V_{el}-\frac{A_{max}{\mathrm{dt}}_{max}}{2}---\left(9\right),$

匀加速段的运行距离增量D₂和减加速度段的运行距离增量D₃为：

$\left\{\begin{array}{c}{D}_2=\frac{V_b^2-V_a^2}{2A_{max}}\\ D_3=A_{max}{\mathrm{dt}}_{max}^2(\frac{1}{4}+\frac{1}{{\mathrm{\&pi;}}^2})+V_b{\mathrm{dt}}_{max}\end{array}\right.---\left(10\right)$

式(10)中，A_max为最大加速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度，V_b为匀加速段的最大速度同时也是减加速段的起始速度；

在加速段中，当匀加速段的运行距离增量D₂＝0时，且V_a＝V_b，整个加速段只有加加速段和减加速段，此时规划的加速段距离最短为dp_min，速度增量dv_min最小，即：

$\left\{\begin{array}{c}{\mathrm{dp}}_{\min }=A_{max}{\mathrm{dt}}_{max}^2+2V_s{\mathrm{dt}}_{max}\\ {\mathrm{dv}}_{\min }=A_{max}{\mathrm{dt}}_{max}\end{array}\right.---\left(11\right)$

式(10)中，A_max为最大加速度，dp_min为规划的加速段最短距离，dv_min为速度最小增量，V_s为轨迹规划的起始速度。

进一步地，步骤十、步骤十一中，对最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，获得A_reset和dt_reset的方法为：

基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程组：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}=\frac{2J_{max}{\mathrm{dt}}_{reset}^3}{\mathrm{\&pi;}}+2V_s{\mathrm{dt}}_{reset}\\ J_{max}=\frac{{\mathrm{\&pi;A}}_{reset}}{2{\mathrm{dt}}_{reset}}\end{array}\right.---\left(13\right)$

获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset：

$\left\{\begin{array}{c}{\mathrm{dt}}_{reset}=\sqrt[3]{\frac{\mathrm{\&pi;}(P_e-P_s)}{4J_{max}}}\\ A_{reset}=\frac{2J_{\max }{\mathrm{dt}}_{reset}}{\mathrm{\&pi;}}\end{array}\right.---\left(14\right)$

式(13)、(14)中，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的期望终点位置，J_max为最大加加速度。

进一步地，步骤十四中，对最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，获得A_reset和dt_reset的方法为：

基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程组：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}=\frac{2J_{max}{\mathrm{dt}}_{reset}^2}{\mathrm{\&pi;}}\\ J_{max}=\frac{{\mathrm{\&pi;A}}_{reset}}{2{\mathrm{dt}}_{reset}}\end{array}\right.---\left(15\right)$

获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset：

$\left\{\begin{array}{c}{\mathrm{dt}}_{reset}=\sqrt{\frac{{\mathrm{\&pi;V}}_{el}}{2J_{max}}}\\ A_{reset}=\frac{2J_{max}{\mathrm{dt}}_{reset}}{\mathrm{\&pi;}}\end{array}\right.---\left(16\right)$

式(15)、(16)中，V_el为机器人机械臂运动的轨迹规划期望速度，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的期望终点位置，J_max为最大加加速度。

进一步地，步骤八中，匀速段位移量D₄＝P_e-P_s-2×(D₁+D₂+D₃)，其中，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，D₁为加速段中的加加速段运行距离，D₂为加速段中的匀加速段运行距离，D₃为加速段中的减加速段运行距离，继而获得机器人机械臂期望的输出轨迹。

进一步地，步骤二中，加速度从零上升到最大加速度A_max的时间由式(5)获得，具体为：

$J_{max}=\frac{{\mathrm{\&pi;A}}_{max}}{2{\mathrm{dt}}_{max}}---\left(5\right)$

式(5)中，J_max为最大加加速度，dt_max为加速度从0加速到最大加速度A_max的时间。

本发明的有益效果是：本发明可计算出一条位置、速度、加速度可导，并且加速度的导数，即加加速度在限定范围内为连续平滑轨迹。由此方法生成的轨迹，平滑连续，更容易跟踪，可大大降低机器人在运动过程中的柔性冲击，减少机械振动，并可降低机器人运动过程中的运动跟踪误差。

附图说明

图1是本发明基于三角函数的工业机器人轨迹规划升降速控制方法的流程示意图。

图2-1是梯形加减速曲线拟合的线性轨迹-加加速度曲线，图2-2是梯形加减速曲线拟合的线性轨迹-加速度曲线，图2-3是梯形加减速曲线拟合的线性轨迹-速度曲线，图2-4是梯形加减速曲线拟合的线性轨迹-位置曲线。

图3-1是S形加减速曲线拟合的线性轨迹-加加速度曲线，图3-2是S形加减速曲线拟合的线性轨迹-加速度曲线，图3-3是S形加减速曲线拟合的线性轨迹-速度曲线，图3-4是S形加减速曲线拟合的线性轨迹-位置曲线。

图4是轨迹规划分段示意图，图中A、B、C表示三种速度类型，E表示匀速拼平稳段，D表示速度升降的倾斜段。

图5-1是基于三角函数加减速曲线拟合的加速段的线性轨迹规划-加加速度曲线，图5-2是基于三角函数加减速曲线拟合的加速段的线性轨迹规划-加速度曲线，图5-3是基于三角函数加减速曲线拟合的加速段的线性轨迹规划-速度曲线，图5-4是基于三角函数加减速曲线拟合的加速段的线性轨迹规划-位置曲线。

图6-1是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-加加速度曲线，图6-2是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-加速度曲线，图6-3是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-速度曲线，图6-4是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-位置曲线。

图7-1是基于三角函数加减速曲线拟合的加速段运行距离不能满足要求的线性轨迹规划-加加速度曲线，图7-2是基于三角函数加减速曲线拟合的加速段运行距离不能满足要求的线性轨迹规划-加速度曲线，图7-3是基于三角函数加减速曲线拟合的加速段运行距离不能满足要求的线性轨迹规划-速度曲线，图7-4是基于三角函数加减速曲线拟合的加速段运行距离不能满足要求的线性轨迹规划-位置曲线。

图8-1是基于三角函数加减速曲线拟合的加减速完整曲线段运行距离不能满足要求的线性轨迹规划-加加速度曲线，图8-2是基于三角函数加减速曲线拟合的加减速完整曲线段运行距离不能满足要求的线性轨迹规划-加速度曲线，图8-3是基于三角函数加减速曲线拟合的加减速完整曲线段运行距离不能满足要求的线性轨迹规划-速度曲线，图8-4是基于三角函数加减速曲线拟合的加减速完整曲线段运行距离不能满足要求的线性轨迹规划-位置曲线。

图9-1是基于三角函数加减速曲线拟合的加速段速度和运行距离均不能满足要求的线性轨迹规划-加加速度曲线，图9-2是基于三角函数加减速曲线拟合的加速段速度和运行距离均不能满足要求的线性轨迹规划-加速度曲线，图9-3是基于三角函数加减速曲线拟合的加速段速度和运行距离均不能满足要求的线性轨迹规划-速度曲线，图9-4是基于三角函数加减速曲线拟合的加速段速度和运行距离均不能满足要求的线性轨迹规划-位置曲线。

图10-1是基于三角函数的加减速曲线拟合的加减速完整曲线段速度和运行距离均不能满足要求的线性轨迹规划-加加速度曲线，图10-2是基于三角函数的加减速曲线拟合的加减速完整曲线段速度和运行距离均不能满足要求的线性轨迹规划-加速度曲线，图10-3是基于三角函数的加减速曲线拟合的加减速完整曲线段速度和运行距离均不能满足要求的线性轨迹规划-速度曲线，图10-4是基于三角函数的加减速曲线拟合的加减速完整曲线段速度和运行距离均不能满足要求的线性轨迹规划-位置曲线。

图11-1是基于三角函数加减速曲线拟合的加速段速度不能满足要求的线性轨迹规划-加加速度曲线，图11-2是基于三角函数加减速曲线拟合的加速段速度不能满足要求的线性轨迹规划-加速度曲线，图11-3是基于三角函数加减速曲线拟合的加速段速度不能满足要求的线性轨迹规划-速度曲线，图11-4是基于三角函数加减速曲线拟合的加速段速度不能满足要求的线性轨迹规划-位置曲线。

图12-1是基于三角函数加减速曲线拟合的加减速完整曲线段速度不能满足要求的线性轨迹规划-加加速度曲线，图12-2是基于三角函数加减速曲线拟合的加减速完整曲线段速度不能满足要求的线性轨迹规划-加速度曲线，图12-3是基于三角函数加减速曲线拟合的加减速完整曲线段速度不能满足要求的线性轨迹规划-速度曲线，图12-4是基于三角函数加减速曲线拟合的加减速完整曲线段速度不能满足要求的线性轨迹规划-位置曲线。

具体实施方式

下面结合附图详细说明本发明的优选实施例。

实施例

本发明能够生成平滑连续的曲线，容易实现对期望轨迹的跟踪，适用于工业机器人的轨迹规划。实施例解决了工业机器人轨迹规划采用梯形升降速控制拟合的线性段轨迹的方法存在的加速度阶跃变化，以及在突变处存在无穷大的加速度导数值，从而导致轨迹不容易跟踪的问题；而采用S型曲线升降速控制拟合的线性段轨迹的方法存在的加加速度阶跃变化，仍会对机器人关节造成一定程度的柔性冲击的问题。

实施例首先给定基于三角函数的位置、速度、加速度和加加速度的数学模型，给定轨迹规划曲线的起止点处的两个期望点的位置和速度参数，然后，将边界条件的参数值带入到所建立的数学模型中，列出方程组并求出模型的参数。最后，根据起止点处的两个期望点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线。最终确定期望的规划轨迹。

实施例基于三角函数的工业机器人轨迹规划升降速控制方法，如图1，具体包括以下步骤：

步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速度的数学模型、加速度的数学模型和加加速度的数学模型；

通常将两点间的轨迹根据控制参数(位置、速度和加速度)的变化分为表示速度倾斜段D、速度平稳的匀速段E，参见图4所示。控制参数速度发生变化的阶段为倾斜段D，主要有两种类型：一是表示速度上升的倾斜段D，包括：加加速段、匀加速段、减加速段；二是表示速度下降的倾斜段D，包括：加减速段、匀减速段、减减速段。控制参数速度不发生变化的阶段为匀速平稳段E。

在图4中，控制参数速度经过加速倾斜段D直接转为减速倾斜段D为速度类型B；经历加速倾斜段D、匀速平稳段E和减速倾斜段D为速度类型A或者C，而速度类型C又为未能达到指定速度值V_el的类型。

基于三角函数的工业机器人轨迹规划升降速控制的加速段，而对于减速段的加加速度的数学模型与加速段加加速度的数学模型相同只是符号相反，参见图5-1，其加加速度即加速度的一阶导数的数学模型J(t)为：

$J\left(t\right)=\left\{\begin{array}{cc}{k}_1\cos (\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})& (0\&le;t<{\mathrm{dt}}_{\max })\\ 0& (0\&le;t<{\mathrm{dt}}_{\max })\\ -k_1\cos (\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})& (0\&le;t<{\mathrm{dt}}_{\max })\end{array}\right.---\left(1\right)$

式中：为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，dt_mu为加速段中的的匀加速段时间。

对公式(1)进行积分获得加速度的数学模型。

基于三角函数的工业机器人轨迹规划升降速控制的加速段，参见图5-2，其加速度的数学模型A(t)为：

$A\left(t\right)=\left\{\begin{array}{cc}\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}& (0\&le;t<{\mathrm{dt}}_{max})\\ A_{\max }& (0\&le;t<{\mathrm{dt}}_{mu})\\ -\frac{k_1{\mathrm{dt}}_{\max }}{\mathrm{\&pi;}}sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(2\right)$

式中：A_max为最大加速度。

对公式(2)进行积分获得速度的数学模型。

基于三角函数的工业机器人轨迹规划升降速控制的加速段，参见图5-3，其速度的数学模型V(t)为：

$V\left(t\right)=\left\{\begin{array}{cc}-\frac{k_1{\mathrm{dt}}_{\max }^2}{{\mathrm{\&pi;}}^2}cos(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}t+V_s& (0\&le;t<{\mathrm{dt}}_{max})\\ A_{\max }t+V_a& (0\&le;t<{\mathrm{dt}}_{mu})\\ \frac{k_1{\mathrm{dt}}_{\max }^2}{{\mathrm{\&pi;}}^2}cos(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{\max }}{\mathrm{\&pi;}}t+V_b& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(3\right)$

式中：V_s为轨迹规划的起始速度、V_a为加加速段的最大速度同时也是匀加速段的起始速度、V_b为匀加速段的最大速度同时也是减加速段的起始速度。

对公式(3)进行积分获得位置的数学模型。

基于三角函数的工业机器人轨迹规划升降速控制的加速段，参见图5-4，其位置的数学模型P(t)为：

$P\left(t\right)=\left\{\begin{array}{cc}-\frac{k_1{\mathrm{dt}}_{\max }^3}{{\mathrm{\&pi;}}^3}\&lsqb;sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+1\&rsqb;+\frac{k_1{\mathrm{dt}}_{max}}{2\mathrm{\&pi;}}{t}^2+V_{s}t+P_s& (0\&le;t<{\mathrm{dt}}_{max})\\ \frac{1}{2}{A}_{max}{t}^2+V_{a}t+P_s+D_1& (0\&le;t<{\mathrm{dt}}_{mu})\\ \frac{k_1{\mathrm{dt}}_{\max }^3}{{\mathrm{\&pi;}}^3}\&lsqb;sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+1\&rsqb;+\frac{k_1{\mathrm{dt}}_{\max }}{2\mathrm{\&pi;}}{t}^2+V_{b}t+P_s+D_1+D_2& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(4\right)$

式中：P_s为起始期望点位置、D₁为加加速段运行距离增量、D₂为匀加速段运行距离增量。

步骤二、给定机器人机械臂运动的期望起始点起始速度V_s和期望终点速度V_e、起始期望点位置P_s、轨迹规划期望速度V_el、期望终点位置P_e，按最大加速度A_max和最大加加速度J_max进行基于三角函数的机器人机械臂轨迹曲线的规划。

最大加速度A_max和最大加加速度J_max之间存在如下关系：

$J_{max}=\frac{{\mathrm{\&pi;A}}_{max}}{2{\mathrm{dt}}_{max}}---\left(5\right)$

最大加速度A_max是由机器人机械臂的最大输出力矩的性能决定，为已知量，最大加加速度J_max为最大加速度A_max的一阶导数也为已知量，由公式(1)可推导出加速度从0加速到最大加速度A_max的时间dt_max，由公式(1)和公式(2)可推导出公式(5)。

步骤三、依据速度控制轨迹平滑过渡的要求和时间最优原则将机器人机械臂从起始期望点到末端期望点的轨迹分为速度倾斜上升的加速段(加加速段、匀加速段、减加速段)、速度平稳的匀速段、速度倾斜下降的减速段(加减速段、匀减速段、减减速段)，参见图6-1～图6-4，具体为：

速度倾斜上升的加加速段的时间区间为[0,dt_max]，速度由V_s增加到V_a，运行距离为D₁，加速度达到最大值A_max；

速度倾斜上升的匀加速段的时间区间为[dt_max,dt_max+dt_mu]，速度由V_a均匀增加到V_b，运行距离为D₂，加速度保持最大值A_max，加速度的导数加加速度保持为0。

速度倾斜上升的减加速段的时间区间为[dt_max+dt_mu,2dt_max+dt_mu]，速度由V_b增加到轨迹规划期望速度V_el，运行距离为D₃，加速度由最大值A_max下降到0。速度平稳的匀速段的时间区间为[2dt_max+dt_mu,2dt_max+dt_mu+dt_u]，速度保持轨迹规划期望速度V_el，运行距离为D₄，加速度保持为0，加速度的导数加加速度保持为0。

速度倾斜下降的减加速段的时间区间为[2dt_max+dt_mu+dt_u,3dt_max+dt_mu+dt_u]，速度由轨迹规划期望速度V_el下降到速度V_b，运行距离为D₅，加速度由0下降到反方向最大加速度值-A_max。

速度倾斜下降的匀减速段的时间区间为[3dt_max+dt_mu+dt_u,3dt_max+2dt_mu+dt_u]，速度由V_b均匀下降到速度V_a，运行距离为D₆，加速度保持反方向最大加速度值-A_max，加速度的导数加加速度保持为0。

速度倾斜下降的减减速段的时间区间为[3dt_max+2dt_mu+dt_u,4dt_max+2dt_mu+dt_u]，速度由V_a均匀下降到速度0，运行距离为D₇，加速度由反方向最大加速度值-A_max下降到0。

给定的起始期望点的位置P_s和速度V_s，终点期望点的位置P_e和速度V_e，且满足P_e＞P_s，V_e＞V_s，限定最大加速度和最大加加速度分别为A_max和J_max。在最短的时间内完成从起始期望位置P_s到终点期望位置P_e的过渡，因此最为理想的轨迹规划将是以A_max和J_max为约束，让速度在最短的时间内达到期望的轨迹规划速度V_el(最大速度)，并快速达到期望终点位置P_e。

步骤四、将速度倾斜上升的加速段的各段边界条件代入步骤一所述的数学模型中进行处理，列出方程组并求解，获得所述数学模型的参数。

加速度在加加速段满足在dt_max时间达到加速度的最大值A_max，所述基于三角函数的工业机器人轨迹规划曲线在时间t＝0时，位置P(0)、速度V(0)、加速度A(0)、加加速度J(0)满足的约束条件为：

$\left\{\begin{array}{c}P\left(0\right)=P_s\\ V\left(0\right)=V_s\\ A\left(0\right)=0\\ J\left(0\right)=0\end{array}\right.---\left(6\right)$

基于三角函数的工业机器人轨迹规划曲线在时间t＝dt_max时，位置P(dt_max)、速度V(dt_max)、加速度A(dt_max)、加加速度J(dt_max)满足的约束条件为：

$\left\{\begin{array}{c}P\left({\mathrm{dt}}_{\max }\right)=P_s+D_1\\ V\left({\mathrm{dt}}_{\max }\right)=V_s+V_a\\ A\left({\mathrm{dt}}_{\max }\right)=A_{\max }\\ J\left({\mathrm{dt}}_{\max }\right)=0\end{array}\right.---\left(7\right)$

将方程组(6)和方程组(7)代入公式(1)至公式(4)中，获得速度倾斜上升的加加速段末端的最大速度V_a和加加速段的运行距离增量D₁：

$\left\{\begin{array}{c}{V}_a=V_s+\frac{A_{\max }{\mathrm{dt}}_{max}}{2}\\ D_1=A_{max}{\mathrm{dt}}_{max}^2(\frac{1}{4}-\frac{1}{{\mathrm{\&pi;}}^2})+V_s{\mathrm{dt}}_{max}\end{array}\right.---\left(8\right)$

由匀加速段末端速度V_b加速到减加速段末端速度V_el的时间同样为dt_max，则匀加速段末端速度V_b为：

$V_b=V_{el}-\frac{A_{max}{\mathrm{dt}}_{max}}{2}---\left(9\right)$

匀加速段的运行距离增量D₂和减加速度段的运行距离增量D₃为：

$\left\{\begin{array}{c}{D}_2=\frac{V_b^2-V_a^2}{2A_{max}}\\ D_3=A_{max}{\mathrm{dt}}_{max}^2(\frac{1}{4}+\frac{1}{{\mathrm{\&pi;}}^2})+V_b{\mathrm{dt}}_{max}\end{array}\right.---\left(10\right)$

在加速段中，当匀加速段的运行距离增量D₂＝0时，且V_a＝V_b，整个加速段只有加加速段和减加速段，此时规划的加速段距离最短为dp_min，速度增量dv_min最小，即：

$\left\{\begin{array}{c}{\mathrm{dp}}_{\min }=A_{max}{\mathrm{dt}}_{max}^2+2V_s{\mathrm{dt}}_{max}\\ {\mathrm{dv}}_{\min }=A_{max}{\mathrm{dt}}_{max}\end{array}\right.---\left(11\right)$

步骤五、判断是否满足条件：判断结果成立，则执行步骤六，否则执行步骤九。

步骤六、判断是否满足条件：V_el≥V_min,判断结果成立，则执行步骤七，否则执行步骤十四。

步骤七、判断是否满足条件：判断结果成立，则执行步骤八，否则执行步骤十。

步骤八、基于三角函数的轨迹按最大加速度A_max和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法。

本步骤所述的是满足以上三种限定条件的情况，如图6-1～图6-4所示，本步骤所述的基于三角函数的轨迹规划包括加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段七段，其中加加速段、匀加速段、减加速段分别与加减速段、匀减速段、减减速段的运行距离增量对应相同，则匀速段运行距离增量D₄为：

D₄＝P_e-P_s-2×(D₁+D₂+D₃) (12)

根据公式(5)获得加速度从零上升到最大加速度A_max的时间，根据公式(9)获得加加速段末端速度V_a，匀加速段末端速度V_b，以及加加速段、匀加速段、减加速段和匀速段运行距离增量D₁～D₄，继而获得机器人机械臂期望的输出轨迹；

步骤九、判断是否满足条件：V_el≥V_min,判断结果成立，则执行步骤十，否则执行步骤十一；

步骤十、基于三角函数的轨迹按重置后最大加速度A_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

本步骤所述情况为：

或

即在最大加速度A_max和最大加加速度J_max的限制条件下，运行距离不能满足要求的情况，表示加速段加速至规划速度V_el时加速段运行距离超出规划期望距离的一半，因此，要想使轨迹规划的终点期望点能够满足要求，需要对基于三角函数的工业机器人轨迹规划升降速控制的最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，即A_reset和dt_reset，继而进行重新规划，获得机器人机械臂期望的输出轨迹，从而实现基于三角函数的工业机器人轨迹规划升降速控制方法。

本步骤的基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，加速段轨迹如图7-1～图7-4所示，且满足以下方程组：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}=\frac{2J_{max}{\mathrm{dt}}_{reset}^3}{\mathrm{\&pi;}}+2V_s{\mathrm{dt}}_{reset}\\ J_{max}=\frac{{\mathrm{\&pi;A}}_{reset}}{2{\mathrm{dt}}_{reset}}\end{array}\right.---\left(13\right)$

获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset：

$\left\{\begin{array}{c}{\mathrm{dt}}_{reset}=\sqrt[3]{\frac{\mathrm{\&pi;}(P_e-P_s)}{4J_{max}}}\\ A_{reset}=\frac{2J_{\max }{\mathrm{dt}}_{reset}}{\mathrm{\&pi;}}\end{array}\right.---\left(14\right)$

基于三角函数的轨迹按重置后的最大加速度A_reset与加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，继而获得机器人机械臂期望的输出轨迹，如图8-1～图8-4所示。

步骤十一、基于三角函数的轨迹按重置后最大加速度A_reset、重置后加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法。

本步骤所述情况为：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}<d{p}_{min}\\ V_{el}<V_{min}\end{array}\right.$

即在最大加速度A_max和最大加加速度J_max的限制条件下运行距离不能满足期望运行距离的要求，并且速度不能满足条件的情况，表示加速段加速最短时间2dt_max的末端速度将超出至规划速度V_el，同时加速段运行距离将超出期望规矩距离的一般，将不能到达期望距离P_e处，因此，要想使轨迹规划的终点期望点能够满足要求，需要对基于三角函数的工业机器人轨迹规划升降速控制的最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，即A_reset和dt_reset。

本步骤所述的基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，加速段轨迹如图9-1～图9-4所示，且满足以下方程组，如式(13)所示，继而获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset，如式(14)所示。

步骤十二、判断是否满足条件：V_el≥A_resetdt_reset,判断结果成立，则执行步骤十三，否则执行步骤十四。

步骤十三、基于三角函数的轨迹按步骤十一重置后的最大加速度A_reset、加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法，如图10-1～图10-4所示。

步骤十四、基于三角函数的轨迹需要对最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，轨迹按重置后最大加速度A_reset、重置后加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法。

本步骤所述情况为：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}\&GreaterEqual;d{p}_{min}\\ V_{el}<V_{min}\end{array}\right.$

即在最大加速度A_max和最大加加速度J_max的限制条件下速度不能满足条件的情况，表示加速段加速最短时间2dt_max的末端速度将超出规划速度V_el，也不能达到期望终点位置，也将不能到达期望距离P_e处，因此，要想使轨迹规划的终点期望点能够满足要求，需要对基于三角函数的工业机器人轨迹规划升降速控制的最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，即A_reset和dt_reset；

基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，加速段轨迹如图11-1～图11-4所示，且满足以下方程组：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}=\frac{2J_{max}{\mathrm{dt}}_{reset}^2}{\mathrm{\&pi;}}\\ J_{max}=\frac{{\mathrm{\&pi;A}}_{reset}}{2{\mathrm{dt}}_{reset}}\end{array}\right.---\left(15\right)$

获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset：

$\left\{\begin{array}{c}{\mathrm{dt}}_{reset}=\sqrt{\frac{{\mathrm{\&pi;V}}_{el}}{2J_{max}}}\\ A_{reset}=\frac{2J_{max}{\mathrm{dt}}_{reset}}{\mathrm{\&pi;}}\end{array}\right.---\left(16\right)$

根据三角函数升降速控制的加速段与减速段相对称，可同时得到减速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset，继而获得机器人机械臂期望的输出轨迹，实现基于三角函数的工业机器人轨迹规划升降速控制方法，如图12-1～图12-4所示。

Claims (9)

1.一种基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于，包括：

首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲线的起止点处的两个期望点的位置和速度参数；

然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参数；

最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线；

最终确定期望的规划轨迹。

2.如权利要求1所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于，具体步骤为：

步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速度的数学模型、加速度的数学模型和加加速度的数学模型；

步骤二、给定机器人机械臂运动的起始速度V_s和末端速度V_e均为0、起始期望点位置P_s、轨迹规划期望速度V_el、末端期望点位置P_e，按最大加速度A_max和最大加加速度J_max进行基于三角函数的机器人机械臂轨迹曲线的规划；

步骤三、依据最短时间原则将机器人机械臂从起始期望点到末端期望点的轨迹分为加速段、匀速段、减速段，其中，加速段包括加加速段、匀加速段、减加速段，减速段包括加减速段、匀减速段、减减速段；

步骤四、将加速段的加加速段、匀加速段、减加速段的边界条件的值代入步骤一的数学模型中列出方程组并求解数学模型的参数；

步骤五、判断是否满足条件：其中，dp_min为规划的加速段最短距离，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，判断结果成立，则执行步骤六，否则执行步骤九；

步骤六、判断是否满足条件：V_el≥dv_min，其中，dv_min为速度最小增量，V_el为机器人机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤七，否则执行步骤十四；

步骤七、判断是否满足条件：P_e-P_s≥2(D₁+D₂+D₃)，其中，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，D₁为加速段中的加加速段运行距离，D₂为加速段中的匀加速段运行距离，D₃为加速段中的减加速段运行距离，判断结果成立，则执行步骤八，否则执行步骤九；

步骤八、基于三角函数的轨迹按最大加速度A_max和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

步骤九、判断是否满足条件：V_el≥dv_min，其中，dv_min为速度最小增量，V_el为机器人机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤十，否则执行步骤十一；

步骤十、对三角函数的轨迹的最大加速度A_max和加速度从零加速到A_max的时间dt_max进行重置，即A_reset和dt_reset，继而进行规划，获得机器人机械臂期望的输出轨迹，从而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

步骤十一、对三角函数的轨迹的最大加速度A_max和加速度从零加速到A_max的时间dt_max进行重置，即A_reset和dt_reset，继而获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset；

步骤十二、判断是否满足条件：判断结果成立，则执行步骤十三，否则执行步骤十四；

步骤十三、基于三角函数的轨迹按重置后的最大加速度A_reset、加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法；

步骤十四、基于三角函数的轨迹需要对最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset进行重置，轨迹按重置后的最大加速度A_reset、加速度从零加速到A_reset的时间dt_reset和最大加加速度J_max进行规划，获得机器人机械臂期望的输出轨迹，实现基于三角函数的工业机器人轨迹规划升降速控制方法。

3.如权利要求2所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于：步骤一中，基于三角函数的工业机器人轨迹规划升降速控制的加加速度即加速度的一阶导数的数学模型J(t)为：

$J\left(t\right)=\left\{\begin{array}{cc}{k}_{1}cos(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})& (0\&le;t<{\mathrm{dt}}_{max})\\ 0& (0\&le;t<{\mathrm{dt}}_{mu})\\ -k_{1}cos(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(1\right)$

式(1)中，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，dt_mu为加速段中的的匀加速段时间；

基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加加速度的数学模型进行积分，获得其加速度的数学模型A(t)为：

$A\left(t\right)=\left\{\begin{array}{cc}\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}& (0\&le;t<{\mathrm{dt}}_{max})\\ A_{max}& (0\&le;t<{\mathrm{dt}}_{mu})\\ -\frac{k_1{\mathrm{dt}}_{\max }}{\mathrm{\&pi;}}sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(2\right)$

式(2)中，A_max为最大加速度，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间；

基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加速度的数学模型进行积分，获得其速度的数学模型V(t)为：

$V\left(t\right)=\left\{\begin{array}{cc}-\frac{k_1{\mathrm{dt}}_{\max }^2}{{\mathrm{\&pi;}}^2}\cos (\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}t+V_s& (0\&le;t<{\mathrm{dt}}_{max})\\ A_{max}t+V_a& (0\&le;t<{\mathrm{dt}}_{mu})\\ \frac{k_1{\mathrm{dt}}_{\max }^2}{{\mathrm{\&pi;}}^2}\cos (\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+\frac{k_1{\mathrm{dt}}_{max}}{\mathrm{\&pi;}}t+V_b& (0\&le;t<{\mathrm{dt}}_{max})\end{array}\right.---\left(3\right)$

式(3)中，A_max为最大加速度，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，V_s为轨迹规划的起始速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度，V_b为匀加速段的最大速度同时也是减加速段的起始速度；

基于三角函数的工业机器人轨迹规划升降速控制的加速段，对速度的数学模型进行积分，获得其位置的数学模型P(t)为：

$P\left(t\right)=\left\{\begin{array}{cc}-\frac{k_1{\mathrm{dt}}_{\max }^3}{{\mathrm{\&pi;}}^3}\&lsqb;sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{max}}t-\frac{\mathrm{\&pi;}}{2})+1\&rsqb;+\frac{k_1{\mathrm{dt}}_{max}}{2\mathrm{\&pi;}}{t}^2+V_{s}t+P_s& (0\&le;t<{\mathrm{dt}}_{max})\\ \frac{1}{2}{A}_{\max }{t}^2+V_{a}t+P_s+D_1& (0\&le;t<{\mathrm{dt}}_{mu})\\ \frac{k_1{\mathrm{dt}}_{\max }^3}{{\mathrm{\&pi;}}^3}\&lsqb;sin(\frac{\mathrm{\&pi;}}{{\mathrm{dt}}_{\max }}t-\frac{\mathrm{\&pi;}}{2})+1\&rsqb;+\frac{k_1{\mathrm{dt}}_{\max }}{2\mathrm{\&pi;}}{t}^2+V_{b}t+P_s+D_1+D_2& (0\&le;t<{\mathrm{dt}}_{\max })\end{array}\right.---\left(4\right)$

式(4)中，A_max为最大加速度，为系数，dt_max为所述余弦周期即加速度从零上升到最大加速度A_max的时间，V_s为轨迹规划的起始速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度，V_b为匀加速段的最大速度同时也是减加速段的起始速度，P_s为起始期望点位置，D₁为加加速段位移增量，D₂为匀加速段位移增量。

4.如权利要求2所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于：步骤三具体为：

速度倾斜上升的加加速段的时间区间为[0,dt_max]，速度由V_s增加到V_a，运行距离为D₁，加速度达到最大值A_max；

速度倾斜上升的匀加速段的时间区间为[dt_max,dt_max+dt_mu]，速度由V_a均匀增加到V_b，运行距离为D₂，加速度保持最大值A_max，加速度的导数加加速度保持为0；

速度倾斜上升的减加速段的时间区间为[dt_max+dt_mu,2dt_max+dt_mu]，速度由V_b增加到轨迹规划期望速度V_el，运行距离为D₃，加速度由最大值A_max下降到0；

速度平稳的匀速段的时间区间为[2dt_max+dt_mu,2dt_max+dt_mu+dt_u]，速度保持轨迹规划期望速度V_el，运行距离为D₄，加速度保持为0，加速度的导数加加速度保持为0；

速度倾斜下降的减加速段的时间区间为[2dt_max+dt_mu+dt_u,3dt_max+dt_mu+dt_u]，速度由轨迹规划期望速度V_el下降到速度V_b，运行距离为D₅，加速度由0下降到反方向最大加速度值-A_max；

速度倾斜下降的匀减速段的时间区间为[3dt_max+dt_mu+dt_u,3dt_max+2dt_mu+dt_u]，速度由V_b均匀下降到速度V_a，运行距离为D₆，加速度保持反方向最大加速度值-A_max，加速度的导数加加速度保持为0；

速度倾斜下降的减减速段的时间区间为[3dt_max+2dt_mu+dt_u,4dt_max+2dt_mu+dt_u]，速度由V_a均匀下降到速度0，运行距离为D₇，加速度由反方向最大加速度值-A_max下降到0。

5.如权利要求4所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于：步骤四所述的求解数学模型参数的方法为：

加速度在加加速段满足在dt_max时间达到加速度的最大值A_max，所述基于三角函数的工业机器人轨迹规划曲线在时间t＝0时，位置P(0)、速度V(0)、加速度A(0)、加加速度J(0)满足的约束条件为：

$\left\{\begin{array}{c}P\left(0\right)=P_s\\ V\left(0\right)=V_s\\ A\left(0\right)=0\\ J\left(0\right)=0\end{array}\right.---\left(6\right)$

式(6)中，P_s为机器人机械臂运动的起始期望点位置，V_s为轨迹规划的起始速度；

基于三角函数的工业机器人轨迹规划曲线在时间t＝dt_max时，位置P(dt_max)、速度V(dt_max)、加速度A(dt_max)、加加速度J(dt_max)满足的约束条件为：

$\left\{\begin{array}{c}P\left({\mathrm{dt}}_{max}\right)=P_s+D_1\\ V\left({\mathrm{dt}}_{\max }\right)=V_s+V_a\\ A\left({\mathrm{dt}}_{\max }\right)=A_{\max }\\ J\left({\mathrm{dt}}_{max}\right)=0\end{array}\right.---\left(7\right),$

式(7)中，P_s为机器人机械臂运动的起始期望点位置，D₁为加速段中的加加速段运行距离，V_s为轨迹规划的起始速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度；

将式(6)和式(7)代入式(1)至式(4)中，获得速度倾斜上升的加加速段末端的最大速度V_a和加加速段的运行距离增量D₁：

$\left\{\begin{array}{c}{V}_a=V_s+\frac{A_{\max }{\mathrm{dt}}_{max}}{2}\\ D_1=A_{max}{\mathrm{dt}}_{max}^2(\frac{1}{4}-\frac{1}{{\mathrm{\&pi;}}^2})+V_s{\mathrm{dt}}_{max}\end{array}\right.---\left(8\right),$

式(8)中，D₁为加速段中的加加速段运行距离，V_s为机器人机械臂运动的轨迹规划的起始速度，A_max为最大加速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度；

由匀加速段末端速度V_b加速到减加速段末端速度V_el的时间同样为dt_max，则匀加速段末端速度V_b为：

$V_b=V_{el}-\frac{A_{max}{\mathrm{dt}}_{max}}{2}---\left(9\right),$

匀加速段的运行距离增量D₂和减加速度段的运行距离增量D₃为：

$\left\{\begin{array}{c}{D}_2=\frac{V_b^2-V_a^2}{2A_{max}}\\ D_3=A_{max}{\mathrm{dt}}_{max}^2(\frac{1}{4}+\frac{1}{{\mathrm{\&pi;}}^2})+V_b{\mathrm{dt}}_{max}\end{array}\right.---\left(10\right)$

式(10)中，A_max为最大加速度，V_a为加加速段的最大速度同时也是匀加速段的起始速度，V_b为匀加速段的最大速度同时也是减加速段的起始速度；

在加速段中，当匀加速段的运行距离增量D₂＝0时，且V_a＝V_b，整个加速段只有加加速段和减加速段，此时规划的加速段距离最短为dp_min，速度增量dv_min最小，即：

$\left\{\begin{array}{c}{\mathrm{dp}}_{\min }=A_{max}{\mathrm{dt}}_{max}^2+2V_s{\mathrm{dt}}_{max}\\ {\mathrm{dv}}_{\min }=A_{max}{\mathrm{dt}}_{max}\end{array}\right.---\left(11\right)$

式(10)中，A_max为最大加速度，dp_min为规划的加速段最短距离，dv_min为速度最小增量，V_s为轨迹规划的起始速度。

6.如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于，步骤十、步骤十一中，对最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，获得A_reset和dt_reset的方法为：

基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程组：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}=\frac{2J_{max}{\mathrm{dt}}_{reset}^3}{\mathrm{\&pi;}}+2V_s{\mathrm{dt}}_{reset}\\ J_{max}=\frac{{\mathrm{\&pi;A}}_{reset}}{2{\mathrm{dt}}_{reset}}\end{array}\right.---\left(13\right)$

获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset：

$\left\{\begin{array}{c}{\mathrm{dt}}_{reset}=\sqrt[3]{\frac{\mathrm{\&pi;}(P_e-P_s)}{4J_{max}}}\\ A_{reset}=\frac{2J_{\max }{\mathrm{dt}}_{reset}}{\mathrm{\&pi;}}\end{array}\right.---\left(14\right)$

式(13)、(14)中，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的期望终点位置，J_max为最大加加速度。

7.如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于：步骤十四中，对最大加速度A_max和加速度从零上升到A_max的时间dt_max进行重置，获得A_reset和dt_reset的方法为：

基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程组：

$\left\{\begin{array}{c}\frac{P_e-P_s}{2}=\frac{2J_{max}{\mathrm{dt}}_{reset}^2}{\mathrm{\&pi;}}\\ J_{max}=\frac{{\mathrm{\&pi;A}}_{reset}}{2{\mathrm{dt}}_{reset}}\end{array}\right.---\left(15\right)$

获得三角函数升降速控制的加速段重置的最大加速度A_reset和加速度从零加速到A_reset的时间dt_reset：

$\left\{\begin{array}{c}{\mathrm{dt}}_{reset}=\sqrt{\frac{{\mathrm{\&pi;V}}_{el}}{2J_{max}}}\\ A_{reset}=\frac{2J_{max}{\mathrm{dt}}_{reset}}{\mathrm{\&pi;}}\end{array}\right.---\left(16\right)$

式(15)、(16)中，V_el为机器人机械臂运动的轨迹规划期望速度，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的期望终点位置，J_max为最大加加速度。

8.如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于：步骤八中，匀速段位移量D₄＝P_e-P_s-2×(D₁+D₂+D₃)，其中，P_s为机器人机械臂运动的起始期望点位置，P_e为机器人机械臂运动的末端期望点位置，D₁为加速段中的加加速段运行距离，D₂为加速段中的匀加速段运行距离，D₃为加速段中的减加速段运行距离，继而获得机器人机械臂期望的输出轨迹。

9.如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于：步骤二中，加速度从零上升到最大加速度A_max的时间由式(5)获得，具体为：

$J_{max}=\frac{{\mathrm{\&pi;A}}_{max}}{2{\mathrm{dt}}_{max}}---\left(5\right)$

式(5)中，J_max为最大加加速度，dt_max为加速度从0加速到最大加速度A_max的时间。