CN109434841B - 一种工业机器人动态调速的全局时间最优轨迹规划方法 - Google Patents

Abstract

本发明提出一种工业机器人动态调速的全局时间最优轨迹规划方法，基于固定搜索范围迭代后移的搜索机制提前检测可能超限的轨迹点，根据超限情况分为3类并以非线性问题分段线性化的思想提出相应算法，尤其关节减速度超限时使用回推挪动轨迹点与消峰填谷算法结合调速。通过算法动态调速使超限轨迹点满足约束要求，并不断下发搜索范围的起始点去执行，保证搜索检测、动态调速、轨迹执行的同步运行。通过同时对机器人笛卡尔空间和关节空间的运动速度和加速度进行平滑和限制调节，保证原有轨迹并以合理速度在约束条件范围内运动，实现动态调速的时间最优，确保了机器人在整个运动空间平滑、高效的运动，提高了机器人的精度和运行效率。

Description

一种工业机器人动态调速的全局时间最优轨迹规划方法

技术领域

本发明属于工业机器人技术领域，尤其是一种工业机器人动态调速的全局时间最优轨迹规划方法。

背景技术

工业机器人轨迹规划，是根据作业任务要求，计算规划机器人运动过程中每个时刻的位置、速度、加速度信息。工业机器人即可在笛卡尔空间轨迹规划，也可以在关节空间进行规划。笛卡尔空间轨迹规划的主要优点是空间轨迹确定，能够完成特定的作业需要，缺点是与关节空间中的各关节速度、加速度是非线性关系，且容易发生奇异。而关节空间轨迹规划的主要优点就是不会发生奇异，轨迹规划相对容易，缺点是笛卡尔空间的轨迹无法确定。

已知的机器人轨迹规划技术主要单独集中于笛卡尔空间轨迹规划或集中于关节空间轨迹规划，解决其中的技术难题，很少有将关节空间和笛卡尔空间联合起来进行轨迹规划作业。但由于工业机器人是由多个关节串联或并联组成，每个关节都有固定的运动范围和转动能力。而且由于工业机器人笛卡尔空间与关节空间的非线性关系，当机器人在工作空间作业时，关节空间的各关节的速度、加速度是否超限及超限后如何动态调速避免其超限是一项重要难题。

在专利公开号为108333968A的机器人单步运动轨迹规划方法中，提出了提前检测轨迹点的位置、速度、加速度，遇到关节超限故障能够安全停止，避免机器人的硬停止现象。该方法的优点是可以提前检测关节空间是否超限，并通过算法将其安全停止，但缺点是只是提前检测并安全停止，并没有实现轨迹的动态调速让其避免超限并正常运行的功能。

发明内容

本发明所解决的技术问题在于提供一种工业机器人动态调速的全局时间最优轨迹规划方法，随着轨迹点的计算运行，根据提出的搜索范围实时检测后面轨迹点的位置、速度、加速度信息，检测到关节速度、加速度超限后，在线动态调速并优化之前的轨迹，使其在保证轨迹不变的前提下，同时满足笛卡尔空间和关节空间的运动约束条件，避免机器人运动过程中的超限问题。

实现本发明目的的技术解决方案为：

一种工业机器人动态调速的全局时间最优轨迹规划方法，包括以下步骤：

S1：输入工业机器人运动轨迹的起始点和终止点数据，输入笛卡尔空间最大线速度Max_Velo、笛卡尔空间最大加速度Max_Accel、关节空间最大角速度Max_jointVelo、关节空间最大角加速度Max_jointAccel信息作为机器人运动的约束条件；

S2：根据上述约束条件对机器人的运动轨迹进行预计算：采用梯形加减速方法或s型加减速方法对运动轨迹进行预计算并进行离散处理，工业机器人末端点位姿由位置矢量(x,y,z)和欧拉角RPY姿态矢量(α,β,γ)共同表示，得到一组由6自由度的位姿矢量(x,y,z,α,β,γ)表示的笛卡尔空间轨迹点；

S3：采用固定距离迭代搜索方法对运行轨迹进行超限搜索：以搜索范围为单位，计算搜索范围内的轨迹点是否存在关节超限点，若该搜索范围内无关节超限点，则转到S7；若搜索到该搜索范围内的第一个超限点，则判断该超限点的关节角速度和关节角加速度的超限情况，若关节角速度未超限、关节角加速度超限则转到S4，若关节角速度未超限、关节角减速度超限则转到S5，若关节角速度超限则转到S6，其中，超限指的是不满足S1中的约束条件；

S4：关节角速度未超限、关节角加速度超限发生在笛卡尔空间运动轨迹加速段，采用非线性问题分段线性化的方法计算机器人六个轴的角加速度的超限比例和角速度的动态调速比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点；

S5：关节角速度未超限、关节角减速度超限发生在笛卡尔空间运动轨迹减速段，实时调速具体包括：

S5-1：回推挪动轨迹点：计算角减速度的超限比例和角速度的动态调速比例，从超限点回推并刷新轨迹点，使轨迹点满足最大角加速度Max_jointAccel的约束，直到回推到设定的最大角速度；

S5-2：对挪动后轨迹点的关节角速度突变点根据突变程度的不同进行消峰填谷处理：将速度突变点与周围轨迹点的偏差平分给速度突变点之前的轨迹点；

S6：关节角速度超限时，计算关节角速度的超限比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点，对新的轨迹点进行角加速度验算，若角加速度超限则转到S4，若角减速度超限则转到S5，若不超限则转S7；

S7：将该搜索范围的起始点下发给机器人运行，若运动轨迹未结束则迭代后移搜索范围，转到S3，否则规划计算结束。

进一步的，本发明的工业机器人动态调速的全局时间最优轨迹规划方法，S3中的搜索范围为位移ss1和位移ss2中的最大值，其中，ss1为笛卡尔空间的线速度从0变化到最大线速度所运行的位移，

ss2为关节空间的角速度从0变化到最大角速度时笛卡尔空间所运行的位移，

进一步的，本发明的工业机器人动态调速的全局时间最优轨迹规划方法，S4具体为：

S4-1：通过

计算6个轴的角加速度的超限比例，其中joint_accel_i表示第i个关节的实时角加速度，ka_i表示第i个关节的角加速度超限比例，选择角加速度超限最严重轴的超限比例，即ka_i中绝对值最大值ka，且ka＞1；

S4-2：通过

计算角速度动态调速比例，其中

表示第i个关节的第n个插补点的角速度，ka表示各关节角加速度最大超限比例，k_i表示第i个关节的角速度动态调速比例，n表示关节空间插补点的序号，选择k_i中最大值k，且k＞1，将机器人最优动态调速比例k代入后面公式进行调速计算；

S4-3：根据

得到插补点的位置，m表示笛卡尔空间插补点的序号,S_m表示笛卡尔空间第m个轨迹点的位置矢量[x,y,z]^T，Δs表示笛卡尔空间第m个轨迹点与第m-1个轨迹点之间的距离矢量，k为机器人最优动态调速比例。

进一步的，本发明的工业机器人动态调速的全局时间最优轨迹规划方法，S5-1具体为：

S5-1-1：通过

计算6个轴的角减速度的超限比例，其中joint_accel_i表示第i个关节的实时角减速度，Max_jointAccel表示限定的关节最大角减速度，ka_i表示第i个关节的角减速度超限比例，选择角减速度超限最严重轴的超限比例，即ka_i中绝对值最大值ka，且ka＞1；

S5-1-2：通过

计算角速度动态调速比例，其中

表示第i个关节的第n个插补点的角速度，ka表示各关节角减速度最大超限比例，k_i表示第i个关节的角速度动态调速比例，n表示关节空间插补点的序号，选择k_i中最大值k，且k＞1；

S5-1-3：从超限点回推刷新轨迹点，根据

得到超限点回推的第二个轨迹点的位置，m表示笛卡尔空间插补点的序号,S_m-1表示笛卡尔空间第m-1个轨迹点的位置矢量[x,y,z]^T，Δs表示笛卡尔空间第m-1个轨迹点与第m-2个轨迹点之间的距离矢量，k为机器人最优动态调速比例。

进一步的，本发明的工业机器人动态调速的全局时间最优轨迹规划方法，S5-2具体为：

将速度突变点的速度与左右两边的速度进行做商比较，确定速度突变点的突变程度，通过四舍五入确定插点个数：插点个数的小数点位在0-0.5时形成凸峰速度，将凸峰速度与周围轨迹点速度的偏差平分给凸峰所在轨迹点前面的轨迹点；当插点个数的小数点位在0.5-1时形成凹谷速度，将凹谷速度与周围轨迹点速度的偏差平分给凹谷所在轨迹点前面的轨迹点。

进一步的，本发明的工业机器人动态调速的全局时间最优轨迹规划方法，S6具体为：

S6-1：根据

计算关节角速度的超限比例，其中joint_velo_i表示第i个关节的实时角速度，k_vi表示第i个关节的关节角速度超限比例，选择k_vi中最大值k_v，且k_v＞1；

S6-2：根据

Δs＝S_m-S_m-1得到插补点位置，m表示笛卡尔空间插补点的序号，S_m表示笛卡尔空间第m个轨迹点的位置矢量[x,y,z]^T，Δs表示笛卡尔空间第m个轨迹点与第m-1个轨迹点之间的距离矢量；

S6-3：计算插补点的角加速度，并判断其是否超限，若角加速度超限则转到S4，若角减速度超限则转到S5，若不超限则转S7。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明的方法首次在工业机器人轨迹规划时同时考虑笛卡尔空间和关节空间的多重约束，并实现在关节超限时不需要被迫急停而是对超限轨迹点动态调速使其满足约束条件继续运行的效果。采用笛卡尔空间轨迹预插补和迭代搜索关节空间是否有超限点及动态调速的方法，最终实现了预插补轨迹点动态修正；能同时对机器人笛卡尔空间和关节空间的运动速度和加速度进行平滑和限制调节，在满足机器人笛卡尔空间运动速度和加速度平滑不超限的同时，对其关节空间中各关节速度和加速度也得到很好的控制，在保证原有轨迹的同时，确保了机器人在整个运动空间平滑、高效的运动，提高了机器人的运行精度和效率。并且避免了机器人超限时只能硬急停的现象。

2、本发明中提出的固定距离迭代搜索方法，不仅可以提前计算到关节空间会发生超限的位置，使系统提前动态调节超限点。由于搜索机制总是下发搜索范围的起始点一个点去运行，所以搜索计算、动态局部调速运算总是超前于运行点位，实现搜索超限、局部动态调速、实际执行的同步运行。相较于其他领域全局读取数据点并进行全局调速的方法，本发明针对机器人大量离散轨迹点实现在线局部调速、在线同步运行的“边搜索-边调速-边运行”效果，极大缩短了动态调速时间，提升了算法的执行效率与机器人的整体工作效率，实现轨迹点超限时动态调速的时间最优。

3、本发明提出的分类动态调速策略，弥补了目前技术中检测到超限后只能被迫停止的技术难题。在本发明中，当发生关节空间的关节运动参数超限后进行分类动态调节使轨迹点满足约束条件并继续正常运行，实现笛卡尔空间与关节空间中运动参数的协同调控。其中以非线性问题分段线性化的思想，首次针对各类超限情况，分别提出相应的调速策略，尤其在关节减速度超限的复杂情况下，提出了回推挪动轨迹点与削峰填谷算法的配合调速策略，该策略可以成功实现关节减速度超限的动态调速。

附图说明

图1是本发明提供的超限搜索机制示意图。

图2是本发明提供的回推挪动轨迹点的算法示意图。

图3是本发明提供的回推挪动轨迹点前后的速度对比图。

图4是本发明提供的消峰算法插入新点的处理效果图。

图5是本发明提供的消峰算法调速前后的速度对比图。

图6是本发明提供的填谷算法插入新点的处理效果图。

图7是本发明提供的填谷算法调速前后的速度对比图。

图8是本发明提供的工业机器人动态调速全局时间最优轨迹规划算法流程图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

一种工业机器人动态调速的全局时间最优轨迹规划方法，如图8所示，包括以下步骤：

S1：输入工业机器人运动轨迹的起始点和终止点数据，输入笛卡尔空间最大线速度Max_Velo、笛卡尔空间最大加速度Max_Accel、关节空间最大角速度Max_jointVelo、关节空间最大角加速度Max_jointAccel信息作为机器人运动的约束条件；

S2：根据上述约束条件对机器人的运动轨迹进行预计算：采用梯形加减速方法或s型加减速方法对运动轨迹进行预计算并进行离散处理，工业机器人末端点位姿由位置矢量(x,y,z)和欧拉角RPY姿态矢量(α,β,γ)共同表示，得到一组由6自由度的位姿矢量(x,y,z,α,β,γ)表示的笛卡尔空间轨迹点；

S3：采用固定距离迭代搜索方法对运行轨迹进行超限搜索：以搜索范围为单位，计算搜索范围内的轨迹点是否存在关节超限点，若该搜索范围内无关节超限点，则转到S7；若搜索到该搜索范围内的第一个超限点，则判断该超限点的关节角速度和关节角加速度的超限情况，若关节角速度未超限、关节角加速度超限则转到S4，若关节角速度未超限、关节角减速度超限则转到S5，若关节角速度超限则转到S6，其中，超限指的是不满足S1中的约束条件；

S4：关节角速度未超限、关节角加速度超限发生在笛卡尔空间运动轨迹加速段，采用非线性问题分段线性化的方法计算机器人六个轴的角加速度的超限比例和角速度的动态调速比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点；

S5：关节角速度未超限、关节角减速度超限发生在笛卡尔空间运动轨迹减速段，实时调速具体包括：

S5-1：回推挪动轨迹点：计算角减速度的超限比例和角速度的动态调速比例，从超限点回推并刷新轨迹点，使轨迹点满足最大角加速度Max_jointAccel的约束，直到回推到设定的最大角速度；

S5-2：对挪动后轨迹点的关节角速度突变点根据突变程度的不同进行消峰填谷处理：将速度突变点与周围轨迹点的偏差平分给速度突变点之前的轨迹点；

S6：关节角速度超限时，计算关节角速度的超限比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点，对新的轨迹点进行角加速度验算，若角加速度超限则转到S4，若角减速度超限则转到S5，若不超限则转S7；

S7：将该搜索范围的起始点下发给机器人运行，若运动轨迹未结束则迭代后移搜索范围，转到S3，否则规划计算结束。

实施例1

本实施例以工业机器人笛卡尔空间直线规划为例进行详细说明。一种工业机器人动态调速的全局时间最优轨迹规划方法，包括以下步骤：

S1：输入工业机器人运动轨迹的起始点和终止点数据，输入笛卡尔空间最大线速度Max_Velo、笛卡尔空间最大加速度Max_Accel、关节空间最大角速度Max_jointVelo、关节空间最大角加速度Max_jointAccel信息作为机器人运动的约束条件。

S2：根据上述约束条件对机器人的运动轨迹进行预计算：本实施例采用梯形加减速方法对运动轨迹进行预计算工业机器人末端点位姿由位置矢量(x,y,z)和欧拉角RPY姿态矢量(α,β,γ)共同表示，得到一组由6自由度的位姿矢量(x,y,z,α,β,γ)表示的笛卡尔空间轨迹点。

通过梯形加减速规划直线路径，其中姿态矢量通过归一化方法与位置矢量关联，最后将预计算的轨迹进行离散处理，得到一组用位姿矢量表示的轨迹点。

S3：采用固定距离迭代搜索方法对运行轨迹进行超限搜索。

为了实现轨迹规划中在线检测超限并在线动态调速避免超限，采用固定距离迭代搜索方法。该方法以搜索范围为单位，正常计算搜索范围内的轨迹点是否存在关节超限点，搜索范围一直迭代后移直到轨迹运行结束。

根据预计算的笛卡尔空间轨迹点的速度、加速度实时解算关节空间的关节角速度、关节角加速度。通过建立机器人的DH参数，来构建机器人运动学正逆解，同时可以解算出雅克比矩阵。工业机器人的笛卡尔空间速度与关节空间角速度具有映射关系：

表示笛卡尔空间的广义速度；

为关节速度；J(q)是6×n的偏导数矩阵，称为机器人的雅克比矩阵。

关节空间角速度可通过角度差分(即单位时间内关节角度变化量)计算或速度雅克比矩阵进行计算，角加速度通过角速度差分获得。控制算法实时检测关节空间的角速度、角加速度信息，观测哪个变量会超限。

为保证下发的搜索范围初始点不论任何情况都是满足各项约束条件的，需要合理选择搜索范围的大小。搜索范围选定为位移ss1和位移ss2中的最大值SS_max，其中，ss1为笛卡尔空间的线速度从0变化到最大线速度所运行的位移，

ss2为关节空间的角速度从0变化到最大角速度时笛卡尔空间所运行的位移，

如图1所示，以确定的搜索范围SS_max为固定搜索距离，每次迭代搜索该范围内的轨迹点超限情况，待该段距离搜索结束后，将搜索范围后移一个轨迹点继续前面的搜索操作，搜索范围一直迭代后移直到轨迹运行结束。

若该搜索范围内无关节超限点，则转到S7；若搜索到该搜索范围内的第一个超限点，则判断该超限点的关节角速度和关节角加速度的超限情况，若关节角速度未超限、关节角加速度超限则转到S4，若关节角速度未超限、关节角减速度超限则转到S5，若关节角速度超限则转到S6，其中，超限指的是不满足S1中的约束条件。

S4：关节角速度未超限、关节角加速度超限发生在笛卡尔空间运动轨迹加速段，该动态调速方法采用非线性问题分段线性化的思路，将机器人六个轴的角加速度的超限比例和角速度的动态调速比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点，保证轨迹精度的同时满足速度要求。具体为：

通过

计算6个轴的角加速度的超限比例，其中joint_accel_i表示第i个关节的实时角加速度，ka_i表示第i个关节的角加速度超限比例，选择角加速度超限最严重轴的超限比例，即ka_i中绝对值最大值ka，且ka＞1；因为轨迹点数据密集，在调速过程中其余五轴都按最大超限比例ka进行调速，做线性化处理；

通过

即通过前一插补点速度根据ka调速后的速度与当前实际速度做比，计算角速度动态调速比例，其中

表示第i个关节的第n个插补点的角速度，ka表示各关节角加速度最大超限比例，k_i表示第i个关节的角速度动态调速比例，n表示关节空间插补点的序号，选择k_i中最大值k，且k＞1，将机器人最优动态调速比例k代入后面公式进行调速计算；

根据

得到插补点的位置，m表示笛卡尔空间插补点的序号,S_m表示笛卡尔空间第m个轨迹点的位置矢量[x,y,z]^T，Δs表示笛卡尔空间第m个轨迹点与第m-1个轨迹点之间的距离矢量，k为机器人最优动态调速比例。

根据上面的思想，将满足角加速度的调速比例k带入到笛卡尔空间更改这个插入点，分析这个新点与旧点的距离，来判断是否插入新的点以及插点个数，通过插入新的点来降低关节角加速度。

S5：关节角速度未超限、关节角减速度超限发生在笛卡尔空间运动轨迹减速段，进行实时调速需要两种算法配合调速，具体包括：

S5-1：采用回推挪动轨迹点算法进行预处理。这类情况发生在笛卡尔空间减速段，速度降得太快，故提出回推挪动插补点的方式来将速度往后倒推刷新满足Max_jointAccel的要求。在往后重新刷新旧的插补点的过程中会将前面点的速度降下来，角减速度也就满足了要求。

通过

计算6个轴的角减速度的超限比例，其中joint_accel_i表示第i个关节的实时角减速度，Max_jointAccel表示限定的关节最大角减速度，ka_i表示第i个关节的角减速度超限比例，选择角减速度超限最严重轴的超限比例，即ka_i中绝对值最大值ka，且ka＞1，其余五轴随该比例变化；通过

计算角速度动态调速比例，其中

表示第i个关节的第n个插补点的角速度，ka表示各关节角减速度最大超限比例，k_i表示第i个关节的角速度动态调速比例，n表示关节空间插补点的序号，选择k_i中最大值k，且k＞1，根据该公式逐渐将轨迹点从n向1的方向回推，调节之前这些点的角速度；将k带入

m表示笛卡尔空间插补点的序号,S_m-1表示笛卡尔空间第m-1个轨迹点的位置矢量[x,y,z]^T，Δs表示笛卡尔空间第m-1个轨迹点与第m-2个轨迹点之间的距离矢量，k为机器人最优动态调速比例，以此来动态调节笛卡尔空间中超限点回推的第二个轨迹点的位置来满足关节空间的速度要求。如图2所示，P6为关节角减速度超限轨迹点位置，移动P4点到P4^*,缩短了P4与P5之间的距离，消除了两个轨迹点之间关节角速度下降太快的问题并且满足Max_jointAccel的要求，以此为例一直回推挪动到P1点，达到关节最大速度限制时停止，得到回推挪动轨迹点前后的速度对比如图3所示。

S5-2：对挪动后轨迹点关节角速度突变点根据突变程度的不同进行消峰填谷处理：将速度突变点与周围轨迹点的偏差平分给速度突变点之前的轨迹点。上一步骤的回推算法是以关节空间合理角加速度一直回推刷新之前的轨迹点，回推的速度会越来越大，当回推到设定的最大关节速度时停止，此时有一个速度突变点，该突变采用消峰填谷算法进行平滑。

具体为：将速度突变点的速度与左右两边的速度进行做商比较，确定速度突变点的突变程度，通过四舍五入确定插点个数：插点个数的小数点位在0-0.5时形成凸峰速度，将凸峰速度与周围轨迹点速度的偏差平分给凸峰所在轨迹点前面的轨迹点，如图4、5所示；当插点个数的小数点位在0.5-1时形成凹谷速度，将凹谷速度与周围轨迹点速度的偏差平分给凹谷所在轨迹点前面的轨迹点，如图6、7所示。最后平滑速度突变并保证关节速度不再超限，轨迹没有偏移。

S6：关节角速度超限时，计算关节角速度的超限比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点，具体为：

根据

计算关节角速度的超限比例，其中joint_velo_i表示第i个关节的实时角速度，k_vi表示第i个关节的关节角速度超限比例，选择k_vi中最大值k_v，且k_v＞1；根据

得到插补点位置，m表示笛卡尔空间插补点的序号，S_m表示笛卡尔空间第m个轨迹点的位置矢量[x,y,z]^T，Δs表示笛卡尔空间第m个轨迹点与第m-1个轨迹点之间的距离矢量。

对新的轨迹点进行角速度验算，若角加速度超限则转到S4，若角减速度超限则转到S5，若不超限则转S7。

S7：将该搜索范围的起始点下发给机器人运行，若运动轨迹未结束则迭代后移搜索范围，转到S3，否则规划计算结束。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进，这些改进应视为本发明的保护范围。

Claims (6)

1.一种工业机器人动态调速的全局时间最优轨迹规划方法，其特征在于，包括以下步骤：

S1：输入工业机器人运动轨迹的起始点和终止点数据，输入笛卡尔空间最大线速度Max_Velo、笛卡尔空间最大加速度Max_Accel、关节空间最大角速度Max_jointVelo、关节空间最大角加速度Max_jointAccel信息作为机器人运动的约束条件；

S2：根据上述约束条件对机器人的运动轨迹进行预计算：采用梯形加减速方法或s型加减速方法对运动轨迹进行预计算并进行离散处理，工业机器人末端点位姿由位置矢量(x,y,z)和欧拉角RPY姿态矢量(α,β,γ)共同表示，得到一组由6自由度的位姿矢量(x,y,z,α,β,γ)表示的笛卡尔空间轨迹点；

S3：采用固定距离迭代搜索方法对运行轨迹进行超限搜索：以搜索范围为单位，计算搜索范围内的轨迹点是否存在关节超限点，若该搜索范围内无关节超限点，则转到S7；若搜索到该搜索范围内的第一个超限点，则判断该超限点的关节角速度和关节角加速度的超限情况，若关节角速度未超限、关节角加速度超限则转到S4，若关节角速度未超限、关节角减速度超限则转到S5，若关节角速度超限则转到S6，其中，超限指的是不满足S1中的约束条件；

S4：关节角速度未超限、关节角加速度超限发生在笛卡尔空间运动轨迹加速段，采用非线性问题分段线性化的方法计算机器人六个轴的角加速度的超限比例和角速度的动态调速比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点；

S5：关节角速度未超限、关节角减速度超限发生在笛卡尔空间运动轨迹减速段，实时调速具体包括：

S5-1：回推挪动轨迹点：计算角减速度的超限比例和角速度的动态调速比例，从超限点回推并刷新轨迹点，使轨迹点满足最大角加速度Max_jointAccel的约束，直到回推到设定的最大角速度；

S5-2：对挪动后轨迹点的关节角速度突变点根据突变程度的不同进行消峰填谷处理：将速度突变点与周围轨迹点的偏差平分给速度突变点之前的轨迹点；

S6：关节角速度超限时，计算关节角速度的超限比例，映射到笛卡尔空间，确定插补点的位置和个数，并直接插入插补点作为新的轨迹点，对新的轨迹点进行角加速度验算，若角加速度超限则转到S4，若角减速度超限则转到S5，若不超限则转S7；

S7：将该搜索范围的起始点下发给机器人运行，若运动轨迹未结束则迭代后移搜索范围，转到S3，否则规划计算结束。

2.根据权利要求1所述的工业机器人动态调速的全局时间最优轨迹规划方法，其特征在于，S3中的搜索范围为位移ss1和位移ss2中的最大值，其中，ss1为笛卡尔空间的线速度从0变化到最大线速度所运行的位移，

ss2为关节空间的角速度从0变化到最大角速度时笛卡尔空间所运行的位移，

3.根据权利要求1所述的工业机器人动态调速的全局时间最优轨迹规划方法，其特征在于，S4具体为：

S4-1：通过

计算6个轴的角加速度的超限比例，其中joint_accel_i表示第i个关节的实时角加速度，ka_i表示第i个关节的角加速度超限比例，选择角加速度超限最严重轴的超限比例，即ka_i中绝对值最大值ka，且ka＞1；

S4-2：通过

计算角速度动态调速比例，其中

表示第i个关节的第n个插补点的角速度，ka表示各关节角加速度最大超限比例，k_i表示第i个关节的角速度动态调速比例，n表示关节空间插补点的序号，选择k_i中最大值k，且k＞1，将机器人最优动态调速比例k代入后面公式进行调速计算；

S4-3：根据

得到插补点的位置，m表示笛卡尔空间插补点的序号，S_m表示笛卡尔空间第m个轨迹点的位置矢量[x，y，z]^T，Δs表示笛卡尔空间第m个轨迹点与第m-1个轨迹点之间的距离矢量，k为机器人最优动态调速比例。

4.根据权利要求1所述的工业机器人动态调速的全局时间最优轨迹规划方法，其特征在于，S5-1具体为：

S5-1-1：通过

计算6个轴的角减速度的超限比例，其中joint_accel_i表示第i个关节的实时角减速度，Max_jointAccel表示限定的关节最大角减速度，ka_i表示第i个关节的角减速度超限比例，选择角减速度超限最严重轴的超限比例，即ka_i中绝对值最大值ka，且ka＞1；

S5-1-2：通过

计算角速度动态调速比例，其中

表示第i个关节的第n个插补点的角速度，ka表示各关节角减速度最大超限比例，k_i表示第i个关节的角速度动态调速比例，n表示关节空间插补点的序号，选择k_i中最大值k，且k＞1；

S5-1-3：从超限点回推刷新轨迹点，根据

得到超限点回推的第二个轨迹点的位置，m表示笛卡尔空间插补点的序号，S_m-1表示笛卡尔空间第m-1个轨迹点的位置矢量[x，y，z]^T，Δs表示笛卡尔空间第m-1个轨迹点与第m-2个轨迹点之间的距离矢量，k为机器人最优动态调速比例。

5.根据权利要求1所述的工业机器人动态调速的全局时间最优轨迹规划方法，其特征在于，S5-2具体为：

将速度突变点的速度与左右两边的速度进行做商比较，确定速度突变点的突变程度，通过四舍五入确定插点个数：插点个数的小数点位在0-0.5时形成凸峰速度，将凸峰速度与周围轨迹点速度的偏差平分给凸峰所在轨迹点前面的轨迹点；当插点个数的小数点位在0.5-1时形成凹谷速度，将凹谷速度与周围轨迹点速度的偏差平分给凹谷所在轨迹点前面的轨迹点。

6.根据权利要求1所述的工业机器人动态调速的全局时间最优轨迹规划方法，其特征在于，S6具体为：

S6-1：根据

计算关节角速度的超限比例，其中joint_velo_i表示第i个关节的实时角速度，k_vi表示第i个关节的关节角速度超限比例，选择k_vi中最大值k_v，且k_v＞1；

S6-2：根据

Δs＝S_m-S_m-1得到插补点位置，m表示笛卡尔空间插补点的序号，S_m表示笛卡尔空间第m个轨迹点的位置矢量[x，y，z]^T，Δs表示笛卡尔空间第m个轨迹点与第m-1个轨迹点之间的距离矢量；

S6-3：计算插补点的角加速度，并判断其是否超限，若角加速度超限则转到S4，若角减速度超限则转到S5，若不超限则转S7。

Images