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

基于三角函数的工业机器人轨迹规划升降速控制方法
【专利摘要】本发明提供一种基于三角函数的工业机器人轨迹规划升降速控制方法，首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲线的起止点处的两个期望点的位置和速度参数；然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参数；最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线；最终确定期望的规划轨迹。本发明能够生成平滑连续的曲线，容易实现对期望轨迹的跟踪，适用于工业机器人的轨迹规划。
【专利说明】
基于三角函数的工业机器人轨迹规划升降速控制方法
技术领域
[0001 ]本发明涉及一种基于三角函数的工业机器人轨迹规划升降速控制方法，属于机器 人轨迹规划领域。
【背景技术】
[0002] 现代制造业对机器人性能的要求也越来越高，而机器人运动轨迹规划算法在机器 人控制中占有重要地位，直接影响着机器人末端执行器在加工过程中的性能和效率。
[0003] 通常情况下，机器人的期望轨迹是事先给定一系列笛卡尔或者关节空间的点，且 给定通过该点的速度或两点间的时间，另外还会限制机器人运动允许的最大速度。轨迹规 划的目的就是根据这些约束条件，建立通过这些点的平滑轨迹。为了缩短运动路径，通常规 划这些点之间通过直线相连，并且在连续点处通过曲线过渡。最为常用的是梯形升降速与s 型曲线升降速控制轨迹规划。
[0004] 梯形升降速控制拟合的线性段轨迹包括加速段、匀速段和减速段，在考虑最大运 行速度的情况下达到经历时间最短，可很好的保证轨迹的位置的连续，如图2-1至图2-4所 示，但该方法在过渡点^、^处存在速度的较小突变和加速度突变，以及无穷大的加速度导 数值，这意味着驱动力矩及驱动力矩一阶导数突然增大，由此生成的轨迹并不容易跟踪，并 且造成机器人振动，导致加工出的零部件表面不光滑;S型曲线升降速控制拟合的线性段轨 迹包括加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，如图3-1至 图3-4所示，该升降速模式没有速度突变，过渡较平滑，而加速度变化连续但在处仍存 在突变，仍会对机器人关节造成一定程度的柔性冲击。

【发明内容】

[0005] 本发明的目的是为解决现有机器人轨迹采用梯形升降速和S型曲线升降速拟合的 线性段轨迹的方法存在加速度的阶跃突变，速度变化对关节冲击较大，使驱动力矩及驱动 力矩一阶导数突然增大，从而导致轨迹不容易跟踪的问题，提供了一种基于三角函数的工 业机器人轨迹规划升降速控制方法。
[0006] 本发明的技术解决方案是：
[0007] -种基于三角函数的工业机器人轨迹规划升降速控制方法，包括：
[0008] 首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲 线的起止点处的两个期望点的位置和速度参数；
[0009] 然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参 数；
[0010] 最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速 度和加加速度来限制升降速控制曲线；
[0011] 最终确定期望的规划轨迹。
[0012] 进一步地，具体步骤为：
[0013] 步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速 度的数学模型、加速度的数学模型和加加速度的数学模型；
[0014] 步骤二、给定机器人机械臂运动的起始速度Vs和末端速度Ve均为0、起始期望点位 置P s、轨迹规划期望速度li、末端期望点位置Pe，按最大加速度Amax和最大加加速度J max进行 基于三角函数的机器人机械臂轨迹曲线的规划；
[0015] 步骤三、依据最短时间原则将机器人机械臂从起始期望点到末端期望点的轨迹分 为加速段、匀速段、减速段，其中，加速段包括加加速段、匀加速段、减加速段，减速段包括加 减速段、勾减速段、减减速段；
[0016] 步骤四、将加速段的加加速段、匀加速段、减加速段的边界条件的值代入步骤一的 数学模型中列出方程组并求解数学模型的参数；
[0017] 步骤五、判断是否满足条件
其中，dpmin为规划的加速段最短距离， Ps为机器人机械臂运动的起始期望点位置，Pe为机器人机械臂运动的末端期望点位置，判断 结果成立，则执行步骤六，否则执行步骤九；
[0018] 步骤六、判断是否满足条件:Vel 2 dVmin，其中，dVmin为速度最小增量，Vel为机器人 机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤七，否则执行步骤十四；
[0019]步骤七、判断是否满足条件:Pe-Ps 2 2^+02+03)，其中，Ps为机器人机械臂运动的 起始期望点位置，Pe为机器人机械臂运动的末端期望点位置，Di为加速段中的加加速段运行 距离，D2为加速段中的匀加速段运行距离，D 3为加速段中的减加速段运行距离，判断结果成 立，则执行步骤八，否则执行步骤九；
[0020] 步骤八、基于三角函数的轨迹按最大加速度Amax和最大加加速度Jmax进行规划，获 得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控 制方法；
[0021] 步骤九、判断是否满足条件:Vel 2 dVmin，其中，dVmin为速度最小增量，Vel为机器人 机械臂运动的轨迹规划期望速度，判断结果成立，则执行步骤十，否则执行步骤十一；
[0022] 步骤十、对三角函数的轨迹的最大加速度Amax和加速度从零加速到Amax的时间dt max 进行重置，即Areset和dtreset，继而进行规划，获得机器人机械臂期望的输出轨迹，从而实现 基于三角函数的工业机器人轨迹规划升降速控制方法；
[0023] 步骤^^一、对三角函数的轨迹的最大加速度Amax和加速度从零加速到Amax的时间 duax?行重置，即Areset和dtreset，继而获得三角函数升降速控制的加速段重置的最大加速 度A re3S(rt和加速度从零加速到Are3se3t的时间dt reset ；
[0024] 步骤十二、判断是否满足条件
判断结果成立，则执行步骤十 三，否则执行步骤十四；
[0025] 步骤十三、基于三角函数的轨迹按重置后的最大加速度Areset、加速度从零加速到 Areset的时间dtreset和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出轨迹，继而 实现基于三角函数的工业机器人轨迹规划升降速控制方法；
[0026] 步骤十四、基于三角函数的轨迹按最大加速度Amax和加速度从零加速到Amax的时间 dtmax?行重置，即Areset和dtreset，继而进行规划，获得机器人机械臂期望的输出轨迹，实现 基于三角函数的工业机器人轨迹规划升降速控制方法。
[0027]进一步地，步骤一中，基于三角函数的工业机器人轨迹规划升降速控制的加加速 度即加速度的一阶导数的数学模型J(t)为：
[0029]
为系数，dtmax为所述余弦周期即加速度从零上升到最大加 速度AmaJ^时间，dtmu为加速段中的的匀加速段时间；
[0030] 基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加加速度的数学模 型进行积分，获得其加速度的数学模型A(t)为：
[0032] 式（2)中，Amax为最大加速度：为系数，dtmax为所述余弦周期即加速度 从零上升到最大加速度AmaJ^时间；
[0033]基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加速度的数学模型 进行积分，获得其速度的数学模型V( t)为：
[0035] 式(3)中，Amax为最大加速度
-为系数，dtmax为所述余弦周期即加速度 从零上升到最大加速度AmaJ^时间，Vs为轨迹规划的起始速度，Va为加加速段的最大速度同 时也是匀加速段的起始速度，Vb为匀加速段的最大速度同时也是减加速段的起始速度； [0036]基于三角函数的工业机器人轨迹规划升降速控制的加速段，对速度的数学模型进 行积分，获得其位置的数学模型P (t)为：
[
[0038] 式(4)中，Amax为最大加速度
为系数，dtmax为所述余弦周期即加速度 从零上升到最大加速度AmaJ^时间，Vs为轨迹规划的起始速度，Va为加加速段的最大速度同 时也是匀加速段的起始速度，Vb为匀加速段的最大速度同时也是减加速段的起始速度，Ps* 起始期望点位置，〇:为加加速段位移增量，出为匀加速段位移增量。
[0039] 进一步地，步骤三具体为：
[0040]速度倾斜上升的加加速段的时间区间为[0，dtmax]，速度由Vs增加到V a，运行距离为 Di，加速度达到最大值Amax;
[0041 ] 速度倾斜上升的匀加速段的时间区间为[dtmax，dtmax+dt mu]，速度由Va均匀增加到 Vb，运行距离为D2，加速度保持最大值Amax，加速度的导数加加速度保持为0 ;
[0042] 速度倾斜上升的减加速段的时间区间为[dtmax+dtmu，2dt max+dtmu]，速度由Vb增加到 轨迹规划期望速度Vel，运行距离为D3，加速度由最大值Amax下降到0;
[0043] 速度平稳的匀速段的时间区间为[2dtmax+dtmu，2dt max+dtmu+dtu]，速度保持轨迹规 划期望速度Vel，运行距离为D4,加速度保持为0,加速度的导数加加速度保持为0;
[0044] 速度倾斜下降的减加速段的时间区间为[2(11：11^+(11：11111+(11：11，3(11：11^+(11：11111+(11：11]，速度 由轨迹规划期望速度v el下降到速度Vb，运行距离为D5，加速度由0下降到反方向最大加速度 值 _Amax ;
[0045] 速度倾斜下降的勾减速段的时间区间为[3dtmax+dtmu+dtu，3dtmax+2dtmu+dtu]，速度 由Vb均匀下降到速度乂 3，运行距离为D6,加速度保持反方向最大加速度值-Amax，加速度的导 数加加速度保持为〇;
[0046] 速度倾斜下降的减减速段的时间区间为[3dtmax+2dtmu+dtu，4dtmax+2dtmu+dtu ]，速 度由匀下降到速度0,运行距离为D7,加速度由反方向最大加速度值-Amax下降到0。
[0047] 进一步地，步骤四所述的求解数学模型参数的方法为：
[0048]加速度在加加速段满足在dtmax时间达到加速度的最大值Amax，所述基于三角函数 的工业机器人轨迹规划曲线在时间t = 0时，位置P(0)、速度V(0)、加速度A(0)、加加速度J (0)满足的约束条件为：
[0050]式(6)中，Ps为机器人机械臂运动的起始期望点位置，Vs为轨迹规划的起始速度； [0051 ] 基于三角函数的工业机器人轨迹规划曲线在时间t = dtmaJt，位置P(dtmax)、速度V (dtmax)、加速度A(dtmax)、加加速度J(dtmax)满足的约束条件为：
[0053]式(7)中，Ps为机器人机械臂运动的起始期望点位置，Di为加速段中的加加速段运 行距离，vs为轨迹规划的起始速度，va*加加速段的最大速度同时也是匀加速段的起始速 度；
[0054]将式(6)和式(7)代入式（1)至式(4)中，获得速度倾斜上升的加加速段末端的最大 速度Va和加加速段的运行距离增量Di:
[0056]式(8)中，Di为加速段中的加加速段运行距离，Vs为机器人机械臂运动的轨迹规划 的起始速度，Amax为最大加速度，Va为加加速段的最大速度同时也是匀加速段的起始速度； [0057]由匀加速段末端速度V b加速到减加速段末端速度Vei的时间同样为dtmax，则匀加速 段末端速度V b为：
[0059]匀加速段的运行距离增量出和减加速度段的运行距离增量D3为：
[0061]式（10)中，Amax为最大加速度，Va为加加速段的最大速度同时也是匀加速段的起始 速度，Vb为匀加速段的最大速度同时也是减加速段的起始速度；
[0062]在加速段中，当匀加速段的运行距离增量D2 = 0时，且Va = Vb，整个加速段只有加加 速段和减加速段，此时规划的加速段距1?最短为dpmin，速度增量dvmin最小，即：
[0064]式（10)中，Amax为最大加速度，dpmin为规划的加速段最短距离，dVmin为速度最小增 量，Vs为轨迹规划的起始速度。
[0065] 进一步地，步骤十、步骤^--中，对最大加速度Amax和加速度从零上升到Amax的时间 (1"^进行重置，获得Areset和dtreset的方法为：
[0066] 基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程 组：
[0068]获得三角函数升降速控制的加速段重置的最大加速度Are3se3t和加速度从零加速到 Aresert的时间dt reset:
[0070] 式（13)、（14)中，Ps为机器人机械臂运动的起始期望点位置，Pe为机器人机械臂运 动的期望终点位置，Jmx为最大加加速度。
[0071] 进一步地，步骤十四中，对最大加速度Amax和加速度从零上升到Amax的时间dtmax进 行重置，获得A res et 和 dt reset 的方法为：
[0072] 基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程 组：
[0074]获得三角函数升降速控制的加速段重置的最大加速度Are3se3t和加速度从零加速到 Aresert的时间dt reset:
[0076] 式（15)、（16)中，Vel为机器人机械臂运动的轨迹规划期望速度，Ps为机器人机械臂 运动的起始期望点位置，P e为机器人机械臂运动的期望终点位置，Jmax为最大加加速度。
[0077] 进一步地，步骤八中，匀速段位移量D4 = Pe-Ps-2X (Di+D2+D3)，其中，Ps为机器人机 械臂运动的起始期望点位置，Pe为机器人机械臂运动的末端期望点位置，〇:为加速段中的加 加速段运行距离，D 2为加速段中的匀加速段运行距离，D3为加速段中的减加速段运行距离， 继而获得机器人机械臂期望的输出轨迹。
[0078] 进一步地，步骤二中，加速度从零上升到最大加速度Amax的时间由式(5)获得，具体 为：
[0080] 式(5)中，Jmax为最大加加速度，dtmax为加速度从0加速到最大加速度A maJ^时间。
[0081] 本发明的有益效果是:本发明可计算出一条位置、速度、加速度可导，并且加速度 的导数，即加加速度在限定范围内为连续平滑轨迹。由此方法生成的轨迹，平滑连续，更容 易跟踪，可大大降低机器人在运动过程中的柔性冲击，减少机械振动，并可降低机器人运动 过程中的运动跟踪误差。
【附图说明】
[0082]图1是本发明基于三角函数的工业机器人轨迹规划升降速控制方法的流程示意 图。
[0083] 图2-1是梯形加减速曲线拟合的线性轨迹-加加速度曲线，图2-2是梯形加减速曲 线拟合的线性轨迹-加速度曲线，图2-3是梯形加减速曲线拟合的线性轨迹-速度曲线，图2-4是梯形加减速曲线拟合的线性轨迹-位置曲线。
[0084] 图3-1是S形加减速曲线拟合的线性轨迹-加加速度曲线，图3-2是S形加减速曲线 拟合的线性轨迹-加速度曲线，图3-3是S形加减速曲线拟合的线性轨迹-速度曲线，图3-4是 S形加减速曲线拟合的线性轨迹-位置曲线。
[0085]图4是轨迹规划分段示意图，图中A、B、C表示三种速度类型，E表示匀速拼平稳段，D 表不速度升降的倾斜段。
[0086]图5-1是基于三角函数加减速曲线拟合的加速段的线性轨迹规划-加加速度曲线， 图5-2是基于三角函数加减速曲线拟合的加速段的线性轨迹规划-加速度曲线，图5-3是基 于三角函数加减速曲线拟合的加速段的线性轨迹规划-速度曲线，图5-4是基于三角函数加 减速曲线拟合的加速段的线性轨迹规划-位置曲线。
[0087]图6-1是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-加 加速度曲线，图6-2是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-加速度曲线，图6-3是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-速度曲线，图6-4是基于三角函数加减速曲线拟合的加减速完整曲线段的线性轨迹规划-位 置曲线。
[0088]图7-1是基于三角函数加减速曲线拟合的加速段运行距离不能满足要求的线性轨 迹规划-加加速度曲线，图7-2是基于三角函数加减速曲线拟合的加速段运行距离不能满足 要求的线性轨迹规划-加速度曲线，图7-3是基于三角函数加减速曲线拟合的加速段运行距 离不能满足要求的线性轨迹规划-速度曲线，图7-4是基于三角函数加减速曲线拟合的加速 段运行距离不能满足要求的线性轨迹规划-位置曲线。
[0089]图8-1是基于三角函数加减速曲线拟合的加减速完整曲线段运行距离不能满足要 求的线性轨迹规划-加加速度曲线，图8-2是基于三角函数加减速曲线拟合的加减速完整曲 线段运行距离不能满足要求的线性轨迹规划-加速度曲线，图8-3是基于三角函数加减速曲 线拟合的加减速完整曲线段运行距离不能满足要求的线性轨迹规划-速度曲线，图8-4是基 于三角函数加减速曲线拟合的加减速完整曲线段运行距离不能满足要求的线性轨迹规划-位置曲线。
[0090]图9-1是基于三角函数加减速曲线拟合的加速段速度和运行距离均不能满足要求 的线性轨迹规划-加加速度曲线，图9-2是基于三角函数加减速曲线拟合的加速段速度和运 行距离均不能满足要求的线性轨迹规划-加速度曲线，图9-3是基于三角函数加减速曲线拟 合的加速段速度和运行距离均不能满足要求的线性轨迹规划-速度曲线，图9-4是基于三角 函数加减速曲线拟合的加速段速度和运行距离均不能满足要求的线性轨迹规划-位置曲 线。
[0091] 图10-1是基于三角函数的加减速曲线拟合的加减速完整曲线段速度和运行距离 均不能满足要求的线性轨迹规划-加加速度曲线，图10-2是基于三角函数的加减速曲线拟 合的加减速完整曲线段速度和运行距离均不能满足要求的线性轨迹规划-加速度曲线，图 10-3是基于三角函数的加减速曲线拟合的加减速完整曲线段速度和运行距离均不能满足 要求的线性轨迹规划-速度曲线，图10-4是基于三角函数的加减速曲线拟合的加减速完整 曲线段速度和运行距离均不能满足要求的线性轨迹规划-位置曲线。
[0092] 图11-1是基于三角函数加减速曲线拟合的加速段速度不能满足要求的线性轨迹 规划-加加速度曲线，图11-2是基于三角函数加减速曲线拟合的加速段速度不能满足要求 的线性轨迹规划-加速度曲线，图11-3是基于三角函数加减速曲线拟合的加速段速度不能 满足要求的线性轨迹规划-速度曲线，图11-4是基于三角函数加减速曲线拟合的加速段速 度不能满足要求的线性轨迹规划-位置曲线。
[0093] 图12-1是基于三角函数加减速曲线拟合的加减速完整曲线段速度不能满足要求 的线性轨迹规划-加加速度曲线，图12-2是基于三角函数加减速曲线拟合的加减速完整曲 线段速度不能满足要求的线性轨迹规划-加速度曲线，图12-3是基于三角函数加减速曲线 拟合的加减速完整曲线段速度不能满足要求的线性轨迹规划-速度曲线，图12-4是基于三 角函数加减速曲线拟合的加减速完整曲线段速度不能满足要求的线性轨迹规划-位置曲 线。
【具体实施方式】
[0094]下面结合附图详细说明本发明的优选实施例。
[0095] 实施例
[0096] 本发明能够生成平滑连续的曲线，容易实现对期望轨迹的跟踪，适用于工业机器 人的轨迹规划。实施例解决了工业机器人轨迹规划采用梯形升降速控制拟合的线性段轨迹 的方法存在的加速度阶跃变化，以及在突变处存在无穷大的加速度导数值，从而导致轨迹 不容易跟踪的问题;而采用S型曲线升降速控制拟合的线性段轨迹的方法存在的加加速度 阶跃变化，仍会对机器人关节造成一定程度的柔性冲击的问题。
[0097] 实施例首先给定基于三角函数的位置、速度、加速度和加加速度的数学模型，给定 轨迹规划曲线的起止点处的两个期望点的位置和速度参数，然后，将边界条件的参数值带 入到所建立的数学模型中，列出方程组并求出模型的参数。最后，根据起止点处的两个期望 点的位置和速度参数关系及加速度和加加速度来限制升降速控制曲线。最终确定期望的规 划轨迹。
[0098]实施例基于三角函数的工业机器人轨迹规划升降速控制方法，如图1，具体包括以 下步骤：
[0099]步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速 度的数学模型、加速度的数学模型和加加速度的数学模型；
[0100] 通常将两点间的轨迹根据控制参数(位置、速度和加速度)的变化分为表示速度倾 斜段D、速度平稳的匀速段E，参见图4所示。控制参数速度发生变化的阶段为倾斜段D，主要 有两种类型:一是表示速度上升的倾斜段D，包括:加加速段、匀加速段、减加速段;二是表示 速度下降的倾斜段D，包括:加减速段、匀减速段、减减速段。控制参数速度不发生变化的阶 段为匀速平稳段E。
[0101] 在图4中，控制参数速度经过加速倾斜段D直接转为减速倾斜段D为速度类型B;经 历加速倾斜段D、匀速平稳段E和减速倾斜段D为速度类型A或者C，而速度类型C又为未能达 到指定速度值Vu的类型。
[0102] 基于三角函数的工业机器人轨迹规划升降速控制的加速段，而对于减速段的加加 速度的数学模型与加速段加加速度的数学模型相同只是符号相反，参见图5-1，其加加速度 即加速度的一阶导数的数学模型J(t)为：
[0104]
为系数，dtmax为所述余弦周期即加速度从零上升到最大加速度 Amax的时间，dtmu为加速段中的的匀加速段时间。
[0105] 对公式(1)进行积分获得加速度的数学模型。
[0106] 基于三角函数的工业机器人轨迹规划升降速控制的加速段，参见图5-2,其加速度 的数学模型A(t)为：
[0108] 式中:Amax为最大加速度。
[0109] 对公式(2)进行积分获得速度的数学模型。
[0110] 基于三角函数的工业机器人轨迹规划升降速控制的加速段，参见图5-3,其速度的 数学模型V(t)为：
[0112]式中:VS为轨迹规划的起始速度、Va为加加速段的最大速度同时也是匀加速段的起 始速度、Vb为匀加速段的最大速度同时也是减加速段的起始速度。
[0113] 对公式(3)进行积分获得位置的数学模型。
[0114] 基于三角函数的工业机器人轨迹规划升降速控制的加速段，参见图5-4,其位置的 数学模型P(t)为：
[0116]式中：PS为起始期望点位置、Di为加加速段运行距离增量、D2为匀加速段运行距离 增量。
[0117] 步骤二、给定机器人机械臂运动的期望起始点起始速度Vs和期望终点速度Ve、起始 期望点位置Ps、轨迹规划期望速度Vei、期望终点位置Pe，按最大加速度A max和最大加加速度 Jmax进行基于三角函数的机器人机械臂轨迹曲线的规划。
[0118] 最大加速度Amax和最大加加速度Jmax之间存在如下关系：
[01 20]最大加速度Amax是由机器人机械臂的最大输出力矩的性能决定，为已知量，最大加 加速度Jmax为最大加速度Amax的一阶导数也为已知量，由公式（1 )可推导出加速度从0加速到 最大加速度Amax的时间dtmax，由公式(1)和公式⑵可推导出公式(5)。
[0121] 步骤三、依据速度控制轨迹平滑过渡的要求和时间最优原则将机器人机械臂从起 始期望点到末端期望点的轨迹分为速度倾斜上升的加速段(加加速段、匀加速段、减加速 段）、速度平稳的匀速段、速度倾斜下降的减速段(加减速段、匀减速段、减减速段），参见图 6-1~图6-4,具体为：
[0122] 速度倾斜上升的加加速段的时间区间为[0，dtmax]，速度由Vs增加到Va，运行距离为 Di，加速度达到最大值Amax;
[0123] 速度倾斜上升的匀加速段的时间区间为[dtmax，dtmax+dtmu]，速度由V a均匀增加到 Vb，运行距离为D2，加速度保持最大值Amax，加速度的导数加加速度保持为0。
[0124] 速度倾斜上升的减加速段的时间区间为[dtmax+dtmu，2dt max+dtmu]，速度由Vb增加到 轨迹规划期望速度Vel，运行距离为D 3,加速度由最大值Amax下降到0。速度平稳的匀速段的时 间区间为[2dtmax+dtm U，2dtmaX+dtmU+dtu]，速度保持轨迹规划期望速度Vel，运行距离为D4，加 速度保持为〇，加速度的导数加加速度保持为〇。
[0125] 速度倾斜下降的减加速段的时间区间为[2(11：11^+(11：11111+(11：11，3(11：11^+(11：11111+(11：11]，速度 由轨迹规划期望速度v el下降到速度Vb，运行距离为D5，加速度由0下降到反方向最大加速度 值 _Amax 〇
[0126] 速度倾斜下降的勾减速段的时间区间为[3dtmax+dtmu+dtu，3dtmax+2dtmu+dtu]，速度 由Vb均匀下降到速度乂 3，运行距离为D6,加速度保持反方向最大加速度值-Amax，加速度的导 数加加速度保持为0。
[0127] 速度倾斜下降的减减速段的时间区间为[3dtmax+2dtmu+dtu，4dtmax+2dtmu+dtu ]，速 度由匀下降到速度0,运行距离为D7,加速度由反方向最大加速度值-Amax下降到0。
[0128] 给定的起始期望点的位置Ps和速度Vs，终点期望点的位置和速度，且满足> Ps，Ve>Vs，限定最大加速度和最大加加速度分别为A max和Jmax。在最短的时间内完成从起始 期望位置Ps到终点期望位置P e的过渡，因此最为理想的轨迹规划将是以Amax和Jmax为约束，让 速度在最短的时间内达到期望的轨迹规划速度V el(最大速度），并快速达到期望终点位置 Pe〇
[0129] 步骤四、将速度倾斜上升的加速段的各段边界条件代入步骤一所述的数学模型中 进行处理，列出方程组并求解，获得所述数学模型的参数。
[0130]加速度在加加速段满足在dtmax时间达到加速度的最大值Amax，所述基于三角函数 的工业机器人轨迹规划曲线在时间t = 0时，位置P(0)、速度V(0)、加速度A(0)、加加速度J (0)满足的约束条件为：
[0132] 基于三角函数的工业机器人轨迹规划曲线在时间t = dtmaJt，位置P(dtmax)、速度V (dtmax)、加速度A(dtmax)、加加速度J(dtmax)满足的约束条件为：
[0134]将方程组(6)和方程组(7)代入公式（1)至公式(4)中，获得速度倾斜上升的加加速 段末端的最大速度va和加加速段的运行距离增量m:
[0136]由匀加速段末端速度Vb加速到减加速段末端速度Vei的时间同样为dtmax，则匀加速 段末端速度Vb为：
[0138]匀加速段的运行距离增量出和减加速度段的运行距离增量D3为：
[0140]在加速段中，当匀加速段的运行距离增量D2 = 0时，且Va = Vb，整个加速段只有加加 速段和减加速段，此时规划的加速段距1?最短为dpmin，速度增量dvmin最小，即：

[0142] 步骤五、判断是否满足条件： 判断结果成立，则执行步骤六，否则执 9 行步骤九。
[0143] 步骤六、判断是否满足条件:Vel 2 Vmin，判断结果成立，则执行步骤七，否则执行步 骤十四。
[0144] 步骤七、判断是否满足条件： 判断结果成立，则执行步骤八， ，. 否则执行步骤十。
[0145] 步骤八、基于三角函数的轨迹按最大加速度Amax和最大加加速度Jmax进行规划，获 得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控 制方法。
[0146] 本步骤所述的是满足以上三种限定条件的情况，如图6-1~图6-4所示，本步骤所 述的基于三角函数的轨迹规划包括加加速段、匀加速段、减加速段、匀速段、加减速段、匀减 速段、减减速段七段，其中加加速段、匀加速段、减加速段分别与加减速段、匀减速段、减减 速段的运行距离增量对应相同，则匀速段运行距离增量D 4为：
[0147] D4=Pe-Ps-2X(Di+D2+D3) (12)
[0148] 根据公式(5)获得加速度从零上升到最大加速度Amax的时间，根据公式(9)获得加 加速段末端速度￥ 3，匀加速段末端速度Vb，以及加加速段、匀加速段、减加速段和匀速段运行 距离增量D:\D4,继而获得机器人机械臂期望的输出轨迹；
[0149] 步骤九、判断是否满足条件2 Vmin，判断结果成立，则执行步骤十，否则执行步 骤i ；
[0150] 步骤十、基于三角函数的轨迹按重置后最大加速度Areset和最大加加速度Jmax进行 规划，获得机器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划 升降速控制方法；
[0151] 本步骤所述情况为：
[0153] 即在最大加速度Amax和最大加加速度Jmax的限制条件下，运行距离不能满足要求的 情况，表示加速段加速至规划速度^时加速段运行距离超出规划期望距离的一半，因此，要 想使轨迹规划的终点期望点能够满足要求，需要对基于三角函数的工业机器人轨迹规划升 降速控制的最大加速度A MX和加速度从零上升到Amax的时间dtmax进行重置，即Areset和 dtreset，继而进行重新规划，获得机器人机械臂期望的输出轨迹，从而实现基于三角函数的 工业机器人轨迹规划升降速控制方法。
[0154] 本步骤的基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，加速段 轨迹如图7-1~图7-4所示，且满足以下方程组：
[0156]获得三角函数升降速控制的加速段重置的最大加速度Are3se3t和加速度从零加速到 Aresert的时间dt reset:
[0158] 基于三角函数的轨迹按重置后的最大加速度Areset与加速度从零加速到Areset的时 间dtreset和最大加加速度J max进行规划，继而获得机器人机械臂期望的输出轨迹，如图8-1~ 图8-4所示。
[0159] 步骤十一、基于三角函数的轨迹按重置后最大加速度Are3se3t、重置后加速度从零加 速到Areset的时间dtreset和最大加加速度J max进行规划，获得机器人机械臂期望的输出轨迹， 继而实现基于三角函数的工业机器人轨迹规划升降速控制方法。
[0160] 本步骤所述情况为：
[0162] 即在最大加速度Amax和最大加加速度Jmax的限制条件下运行距离不能满足期望运 行距离的要求，并且速度不能满足条件的情况，表示加速段加速最短时间2dt max的末端速度 将超出至规划速度Vel，同时加速段运行距离将超出期望规矩距离的一般，将不能到达期望 距离P e处，因此，要想使轨迹规划的终点期望点能够满足要求，需要对基于三角函数的工业 机器人轨迹规划升降速控制的最大加速度A max和加速度从零上升到Amax的时间dtmax进行重 置，即Areset和dt reset 〇
[0163] 本步骤所述的基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，加 速段轨迹如图9-1~图9-4所示，且满足以下方程组，如式（13)所示，继而获得三角函数升降 速控制的加速段重置的最大加速度A rase3t和加速度从零加速到Are3se3t的时间dtr^t，如式（14) 所示。
[0164] 步骤十二、判断是否满足条件:Vei2Aresetdtreset，判断结果成立，则执行步骤十三， 否则执行步骤十四。
[0165] 步骤十三、基于三角函数的轨迹按步骤十一重置后的最大加速度Areset、加速度从 零加速到Areset的时间dt reset和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出 轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方法，如图10-1~图10-4 所示。
[0166] 步骤十四、基于三角函数的轨迹需要对最大加速度Amax和加速度从零上升到Amax的 时间扣^进行重置，轨迹按重置后最大加速度Are3se3t、重置后加速度从零加速到Arase3t的时间 dtreset和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出轨迹，继而实现基于三 角函数的工业机器人轨迹规划升降速控制方法。
[0167]本步骤所述情况为：
[0169] 即在最大加速度Amax和最大加加速度Jmax的限制条件下速度不能满足条件的情况， 表示加速段加速最短时间2dt max的末端速度将超出规划速度Vel，也不能达到期望终点位置， 也将不能到达期望距离h处，因此，要想使轨迹规划的终点期望点能够满足要求，需要对基 于三角函数的工业机器人轨迹规划升降速控制的最大加速度A max和加速度从零上升到Amax 的时间dUax进行重置，即Areset和dt reset ；
[0170] 基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，加速段轨迹如图 11-1~图11-4所示，且满足以下方程组：
[0172]获得三角函数升降速控制的加速段重置的最大加速度Are3se3t和加速度从零加速到 Aresert的时间dt reset:
[0174]根据三角函数升降速控制的加速段与减速段相对称，可同时得到减速段重置的最 大加速度Areset和加速度从零加速到Areset的时间dtreset，继而获得机器人机械臂期望的输出 轨迹，实现基于三角函数的工业机器人轨迹规划升降速控制方法，如图12-1~图12-4所示。
【主权项】
1. 一种基于三角函数的工业机器人轨迹规划升降速控制方法，其特征在于，包括： 首先给定基于三角函数的位置、速度、加速度和加加速度的模型，给定轨迹规划曲线的 起止点处的两个期望点的位置和速度参数； 然后，将边界条件的参数值带入到所建立的模型中，列出方程组并求出模型的参数； 最后，根据轨迹规划曲线的起止点处的两个期望点的位置和速度参数关系及加速度和 加加速度来限制升降速控制曲线； 最终确定期望的规划轨迹。2. 如权利要求1所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征 在于，具体步骤为： 步骤一、建立基于三角函数的工业机器人机械臂轨迹曲线的位置的数学模型、速度的 数学模型、加速度的数学模型和加加速度的数学模型； 步骤二、给定机器人机械臂运动的起始速度Vs和末端速度Ve均为0、起始期望点位置Ps、 轨迹规划期望速度11、末端期望点位置Pe，按最大加速度Amax和最大加加速度Jmax进行基于 三角函数的机器人机械臂轨迹曲线的规划； 步骤三、依据最短时间原则将机器人机械臂从起始期望点到末端期望点的轨迹分为加 速段、匀速段、减速段，其中，加速段包括加加速段、匀加速段、减加速段，减速段包括加减速 段、勾减速段、减减速段； 步骤四、将加速段的加加速段、匀加速段、减加速段的边界条件的值代入步骤一的数学 模型中列出方程组并求解数学模型的参数； 步骤五、判断是否满足条件〖中，dpmin为规划的加速段最短距离，pss -， 机器人机械臂运动的起始期望点位置，Pe为机器人机械臂运动的末端期望点位置，判断结 果成立，则执行步骤六，否则执行步骤九； 步骤六、判断是否满足条件dVmin，其中，dVmin为速度最小增量，Vel为机器人机械臂 运动的轨迹规划期望速度，判断结果成立，则执行步骤七，否则执行步骤十四； 步骤七、判断是否满足条件:Pe-Ps 2 2^+02+03)，其中，Ps为机器人机械臂运动的起始 期望点位置，Pe为机器人机械臂运动的末端期望点位置，〇:为加速段中的加加速段运行距 离，D2为加速段中的匀加速段运行距离，D 3为加速段中的减加速段运行距离，判断结果成立， 则执行步骤八，否则执行步骤九； 步骤八、基于三角函数的轨迹按最大加速度Amax和最大加加速度Jmax进行规划，获得机 器人机械臂期望的输出轨迹，继而实现基于三角函数的工业机器人轨迹规划升降速控制方 法； 步骤九、判断是否满足条件2 dvmin，其中，dvmin为速度最小增量，Vei为机器人机械臂 运动的轨迹规划期望速度，判断结果成立，则执行步骤十，否则执行步骤十一； 步骤十、对三角函数的轨迹的最大加速度Amax和加速度从零加速到Amax的时间dtmax进行 重置，即Areset和dtreset，继而进行规划，获得机器人机械臂期望的输出轨迹，从而实现基于 三角函数的工业机器人轨迹规划升降速控制方法； 步骤十一、对三角函数的轨迹的最大加速度Amax和加速度从零加速到Amax的时间dt ma^ 行重置，即Areset和dtreset，继而获得三角函数升降速控制的加速段重置的最大加速度Areset 和加速度从零加速到Areset的时间dtreset;步骤十二、判断是否满足条件 判断结果成立，则执行步骤十三，否则 执行步骤十四； 步骤十三、基于三角函数的轨迹按重置后的最大加速度Areset、加速度从零加速到Areset 的时间dtreset和最大加加速度Jmax进行规划，获得机器人机械臂期望的输出轨迹，继而实现 基于三角函数的工业机器人轨迹规划升降速控制方法； 步骤十四、基于三角函数的轨迹需要对最大加速度Ar^t和加速度从零加速到Are3se3t的时 间dtreset进行重置，轨迹按重置后的最大加速度Areset、加速度从零加速到Areset的时间dtreset 和最大加加速度JmaX进行规划，获得机器人机械臂期望的输出轨迹，实现基于三角函数的工 业机器人轨迹规划升降速控制方法。3.如权利要求2所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征 在于:步骤一中，基于三角函数的工业机器人轨迹规划升降速控制的加加速度即加速度的 一阶导数的数学模型J(t)为：式（1)中I数，dtmax为所述余弦周期即加速度从零上升到最大加速度UlCtA. Amax的时间，dtmu为加速段中的的匀加速段时间； 基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加加速度的数学模型进 行积分，获得其加速度的数学模型A(t)为：式(2)中，Amax为最大加速度，3系数，dtmax为所述余弦周期即加速度从零 上升到最大加速度AmaJ^时间； 基于三角函数的工业机器人轨迹规划升降速控制的加速段，对加速度的数学模型进行 积分，获得其速度的数学模型V( t)为：式(3)中，Amax为最大加速度％系数，dtmax为所述余弦周期即加速度从零 上升到最大加速度Amax的时间，VS为轨迹规划的起始速度，Va*加加速段的最大速度同时也 是匀加速段的起始速度，Vb为匀加速段的最大速度同时也是减加速段的起始速度； 基于三角函数的工业机器人轨迹规划升降速控制的加速段，对速度的数学模型进行积 分，获得其位置的数学模型P (t)为：式(4)中，Amax为最大加速度：3系数，dtmax为所述余弦周期即加速度从零 上升到最大加速度Amax的时间，VS为轨迹规划的起始速度，Va*加加速段的最大速度同时也 是匀加速段的起始速度，Vb为匀加速段的最大速度同时也是减加速段的起始速度，Ps为起始 期望点位置，〇:为加加速段位移增量，出为匀加速段位移增量。4.如权利要求2所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征 在于:步骤二具体为： 速度倾斜上升的加加速段的时间区间为[〇，dtmax]，速度由Vs增加到Va，运行距离为Di， 加速度达到最大值Amax; 速度倾斜上升的匀加速段的时间区间为[dtmax，dtmax+dtmu]，速度由V a均匀增加到Vb，运 行距离为D2，加速度保持最大值Amax，加速度的导数加加速度保持为0 ; 速度倾斜上升的减加速段的时间区间为[扣_^(1^11，2(1^^+(^_]，速度由￥0曾加到轨迹 规划期望速度Vel，运行距离为D3，加速度由最大值Amax下降到0; 速度平稳的匀速段的时间区间为[2dtmax+dtmu，2dtmax+dt mu+dtu]，速度保持轨迹规划期 望速度Vel，运行距离为D4，加速度保持为0，加速度的导数加加速度保持为0; 速度倾斜下降的减加速段的时间区间为[2dtmax+dtmU+dtu，3dtmax+dtm U+dtu ]，速度由轨 迹规划期望速度vel下降到速度Vb，运行距离为D5，加速度由0下降到反方向最大加速度值- Amax ； 速度倾斜下降的勾减速段的时间区间为[3扣1^+(^11111+(^11，3(^11^+2(^1111 1+(^11]，速度由\^ 均匀下降到速度￥3，运行距离为D6,加速度保持反方向最大加速度值_Amax，加速度的导数加 加速度保持为ο; 速度倾斜下降的减减速段的时间区间为[3(1"^+2(^_+(^11，4(11^+2(^_+(^11]，速度由￥ 3 均匀下降到速度0,运行距离为D7,加速度由反方向最大加速度值-Amax下降到0。5.如权利要求4所述的基于三角函数的工业机器人轨迹规划升降速控制方法，其特征 在于:步骤四所述的求解数学模型参数的方法为： 加速度在加加速段满足在dtmax时间达到加速度的最大值Amax，所述基于三角函数的工 业机器人轨迹规划曲线在时间t = 0时，位置P(0)、速度V(0)、加速度A(0)、加加速度J(0)满 足的约束条件为：式(6)中，Ps为机器人机械臂运动的起始期望点位置，Vs为轨迹规划的起始速度； 基于三角函数的工业机器人轨迹规划曲线在时间t = dtmax时，位置P(dtmax)、速度V (dtmax)、加速度A(dtmax)、加加速度J(dtmax)满足的约束条件为：式(7)中，Ps为机器人机械臂运动的起始期望点位置，0:为加速段中的加加速段运行距 离，Vs为轨迹规划的起始速度，Va为加加速段的最大速度同时也是匀加速段的起始速度； 将式(6)和式(7)代入式（1)至式(4)中，获得速度倾斜上升的加加速段末端的最大速度 va和加加速段的运行距离增量m:式(8)中，Di为加速段中的加加速段运行距离，Vs为机器人机械臂运动的轨迹规划的起 始速度，Amax为最大加速度，Va为加加速段的最大速度同时也是匀加速段的起始速度； 由匀加速段末端速度Vb加速到减加速段末端速度Vei的时间同样为dtmax，则勾加速段末 端速度Vb为： 一v y j λ. 匀加速段的运行距离增量D2和减加速度段的运行距离增量d3为：式(10)中，Amax为最大加速度，Va为加加速段的最大速度同时也是匀加速段的起始速度， Vb为匀加速段的最大速度同时也是减加速段的起始速度； 在加速段中，当匀加速段的运行距离增量D2 = 0时，且Va = Vb，整个加速段只有加加速段 和减加速段，此时规划的加速段距1?最短为dpmin，速度增量dVmin最小，即：式（10)中，Amax为最大加速度，dpmin为规划的加速段最短距离，dVmin为速度最小增量，Vs 为轨迹规划的起始速度。6. 如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方 法，其特征在于，步骤十、步骤十一中，对最大加速度A max和加速度从零上升到Amax的时间 (1"^进行重置，获得Areset和dtreset的方法为： 基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程组：获得三角函数升降速控制的加速段重置的最大加速度Are3se3t和加速度从零加速到Are3se3t 的时间dt reset:式（13)、（14)中，Ps为机器人机械臂运动的起始期望点位置，Pe为机器人机械臂运动的 期望终点位置，Jmx为最大加加速度。7. 如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方 法，其特征在于:步骤十四中，对最大加速度Amax和加速度从零上升到Amax的时间dtmax进行重 置，获得A res et 和 dt reset 的方法为： 基于三角函数的加速段轨迹规划包括加加速段和减加速段两段，且满足以下方程组：获得三角函数升降速控制的加速段重置的最大加速度A re3se3t和加速度从零加速到Are3se3t 的时间dt reset:式（15)、（16)中，Vel为机器人机械臂运动的轨迹规划期望速度，PS为机器人机械臂运动 的起始期望点位置，Pe为机器人机械臂运动的期望终点位置，Jmax为最大加加速度。8. 如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方 法，其特征在于：步骤八中，匀速段位移量D4 = Pe-PS_2 X (DX+D2+D3)，其中，Ps为机器人机械 臂运动的起始期望点位置，Pe为机器人机械臂运动的末端期望点位置，Di为加速段中的加加 速段运行距离，D 2为加速段中的匀加速段运行距离，D3为加速段中的减加速段运行距离，继 而获得机器人机械臂期望的输出轨迹。9. 如权利要求2-5任一项所述的基于三角函数的工业机器人轨迹规划升降速控制方 法，其特征在于:步骤二中，加速度从零上升到最大加速度A max的时间由式(5)获得，具体为：(5) 式(5)中，Jmax为最大加加速度，dtmax为加速度从0加速到最大加速度Amax的时间。
【文档编号】G05D1/02GK105867376SQ201610226001
【公开日】2016年8月17日
【申请日】2016年4月12日
【发明人】李宏胜, 汪允鹤
【申请人】南京工程学院