CN112720472B - 一种机器人轨迹规划方法、装置、存储介质及机器人 - Google Patents

Abstract

本发明提供一种机器人轨迹规划方法、装置、存储介质及机器人，所述方法包括：对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数；根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度；在所述机器人运行所述程序时，控制所述机器人以所述最大可运行路径线速度运行。本发明提供的方案能够避免出现伺服驱动器报警、机器人“跑飞”等现象。

Description

一种机器人轨迹规划方法、装置、存储介质及机器人

技术领域

本发明涉及控制领域，尤其涉及一种机器人轨迹规划方法、装置、存储介质及机器人。

背景技术

为满足用户指定路径要求，工业机器人需在笛卡尔空间下进行轨迹规划。在对工业机器人进行笛卡尔空间下的轨迹规划时，不同笛卡尔路径的最高可运行速度不同，工业机器人直接采用用户设置的路径速度可能出现伺服驱动器报警、机器人“跑飞”等现象。

发明内容

本发明的主要目的在于克服上述现有技术的缺陷，提供一种机器人轨迹规划方法、装置、存储介质及机器人，以解决现有技术中工业机器人直接采用用户设置的路径速度可能出现伺服驱动器报警、机器人跑飞等现象的问题。

本发明一方面提供了一种机器人轨迹规划方法，包括：对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数；根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度；在所述机器人运行所述程序时，控制所述机器人以所述最大可运行路径线速度运行。

可选地，根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度，包括：根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度；通过机器人运动学逆解得到所述机器人运行时各关节轴的角度值，并根据各关节轴的角度值计算各关节轴的角速度值；判断得到的各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度；根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴；基于所述角速度超限幅度最大的关节轴计算进行降速的最大可运行线速度，以作为所述机器人运行所述程序的最大可运行线速度；其中，判断任一关节轴的角速度值是否超限，包括：判断所述关节轴的角速度值是否超过预设角速度约束值。

可选地，根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度，包括：基于所述运动参数采用设定的加减速策略进行路径规划，得到相应的规划路程；通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

可选地，根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴，包括：从储存的角速度超限时瞬时角速度值中提取出角速度超限的各关节轴的角速度超限最大值；根据各关节轴的角速度超限最大值及预设角速度约束值，利用如下公式(1)计算角速度超限的各关节轴的角速度超限比例系数k_{i,speed_ratio}；

其中，

为任一关节轴的预设角速度约束值；

为任一关节轴的角速度超限最大值；

根据各关节轴的角速度超限比例系数识别出角速度超限幅度最大的关节轴。

可选地，计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度，包括：根据所述角速度超限幅度最大的关节轴的角速度超限比例系数及其角速度超限最大值对应的角速度超限瞬时路径线速度，利用如下公式(2)计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度v_{TCP_speed_opimization}；

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max} (2)

其中，k_{p,speed_ratio}为角速度超限幅度最大的关节轴的角速度超限比例系数；v_{TCP_speed_max}为角速度超限幅度最大的关节轴的角速度超限最大值对应的角速度超限瞬时路径线速度。

本发明另一方面提供了一种机器人轨迹规划装置，包括：解析单元，用于对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数；处理单元，用于根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度；控制单元，用于在所述机器人运行所述程序时，控制所述机器人以所述最大可运行路径线速度运行。

可选地，所述处理单元，根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度，包括：根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度；通过机器人运动学逆解得到所述机器人运行时各关节轴的角度值，并根据各关节轴的角度值计算各关节轴的角速度值；判断得到的各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度；根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴；基于所述角速度超限幅度最大的关节轴计算进行降速的最大可运行线速度，以作为所述机器人运行所述程序的最大可运行线速度；其中，判断任一关节轴的角速度值是否超限，包括：判断所述关节轴的角速度值是否超过预设角速度约束值。

可选地，根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度，包括：基于所述运动参数采用设定的加减速策略进行路径规划，得到相应的规划路程；通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

可选地，根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴，包括：从储存的角速度超限时瞬时角速度值中提取出角速度超限的各关节轴的角速度超限最大值；根据各关节轴的角速度超限最大值及预设角速度约束值，利用如下公式(1)计算角速度超限的各关节轴的角速度超限比例系数k_{i,speed_ratio}；

其中，

为任一关节轴的预设角速度约束值；

为任一关节轴的角速度超限最大值；

根据各关节轴的角速度超限比例系数识别出角速度超限幅度最大的关节轴。

可选地，计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度，包括：根据所述角速度超限幅度最大的关节轴的角速度超限比例系数及其角速度超限最大值对应的角速度超限瞬时路径线速度，利用如下公式(2)计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度v_{TCP_speed_opimization}；

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max} (2)

其中，k_{p,speed_ratio}为角速度超限幅度最大的关节轴的角速度超限比例系数；v_{TCP_speed_max}为角速度超限幅度最大的关节轴的角速度超限最大值对应的角速度超限瞬时路径线速度。

本发明又一方面提供了一种存储介质，其上存储有计算机程序，所述程序被处理器执行时实现前述任一所述方法的步骤。

本发明再一方面提供了一种机器人，包括处理器、存储器以及存储在存储器上可在处理器上运行的计算机程序，所述处理器执行所述程序时实现前述任一所述方法的步骤。

本发明再一方面提供了一种机器人，包括前述任一所述的机器人轨迹规划装置。

根据本发明的技术方案，采用速度前瞻策略，在工业机器人插补笛卡尔路径前，提前分析出该路径下关节角速度幅度最大轴，并基于关节角速度约束计算出该路径下最大可运行路径线速度，当工业机器人开始插补时可采用此路径线速度运行。采用发明的轨迹规划方案，工业机器人可按照用户指定路径的最大路径速度运行，避免出现伺服驱动器报警、机器人“跑飞”等现象。另外，无需动态调整路径速度，保证了路径速度的匀速段无加速度变化。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明提供的机器人轨迹规划方法的一实施例的方法示意图；

图2是根据解析得到的所述运动参数进行速度前瞻的步骤的流程示意图；

图3示出了根据解析得到的所述运动参数进行速度前瞻一具体实施例的流程示意图；

图4示出了根据运动参数进行速度前瞻得到的机器人各关节轴的角度值及角速度值；

图5示出了根据运动参数进行速度前瞻得到的笛卡尔空间下的路径线速度、路径线加速度及路径线加加速度；

图6示出了基于本发明方案得到的最大可运行线速度，运行所述机器人得到的各关节轴的角度值及角速度值；

图7示出了基于本发明方案得到的最大可运行线速度，运行所述机器人得到的笛卡尔空间下路径线速度、路径线加速度及路径线加加速度；

图8是本发明提供的机器人轨迹规划装置的一实施例的结构框图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

图1是本发明提供的机器人轨迹规划方法的一实施例的方法示意图。该方法可以用于六轴工业机器人。

如图1所示，根据本发明的一个实施例，所述机器人轨迹规划方法至少包括步骤S110、步骤S120和步骤S130。

步骤S110，对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数。

所述运动参数具体可以包括：笛卡尔路径线速度、线加速度以及线加加速度，设定的路径线速度，起点、终点。例如，用户设定路径线速度为v_{TCP_user}＝2500mm/s，并设定起点及终点的坐标。

步骤S120，根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度。

具体地，根据解析得到的所述运动参数进行笛卡尔空间下的轨迹规划，从而得到机器人插补路径，并分析该插补路径下各关节轴中角速度超限的关节轴，得到角速度超限的关节轴中角速度超限幅度最大的关节轴，从而基于关节角速度约束计算出该路径下最大可运行路径线速度v_{TCP_opimization}。

图2是根据解析得到的所述运动参数进行速度前瞻的步骤一具体实施例的流程示意图。如图2所示，步骤S120包括步骤S121、步骤S122、步骤S123、步骤S124和步骤S125。

步骤S121，根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

具体地，基于所述运动参数采用设定的加减速策略进行路径规划，得到相应的规划路程；通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

所述设定的加减速策略例如包括S型加减速、五次多项式加减速、Sin加减速。得到相应的规划路程后，通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点(即插补点)、路径线速度v_TCP(t)以及路径线加速度a_TCP(t)。其中t表示时刻。

步骤S122，通过机器人运动学逆解得到所述机器人运行时各关节轴的角度值，并根据各关节轴的角度值计算各关节轴的角速度值。

例如，采用机器人运动学逆解获得关节轴m的角度值θ_m(t)，并计算出关节轴角速度值

其中m表示关节轴轴号，m＝1,2,3,…,r，t表示时刻，r表示工业机器人关节轴总个数。

步骤S123，判断得到的各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度。

判断任一关节轴的角速度值是否超限，包括：判断所述关节轴的角速度值是否超过预设角速度约束值。每个关节轴具有对应的预设角速度约束值。

具体地，若任一关节轴i在t＝a时刻的角速度值

超过预定义的关节轴角速度约束

则判定以用户设定的路径线速度v_{TCP_user}运行该路径存在角速度超限，则储存该角速度超限的关节轴的轴号i、角速度超限瞬时角速度值

角速度超限瞬时路径线速度v_TCP(a)；

步骤S124，根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴。

若计算得到的全部关节轴在t＝c时刻角速度值

均小于等于预定义的关节轴角速度约束

其中m＝1,2,3,…,r，储存角速度超限瞬时角速度值

角速度超限瞬时路径线速度v_TCP(c)。超限区域结束，可开始分析关节角速度超限幅度最大轴p，分析方法如下：

(1)从储存的角速度超限时瞬时角速度值

数据中提取出角速度超限的各关节轴的角速度超限最大值(超限的角速度值中的最大角速度值)

(2)根据各关节轴的角速度超限最大值

及预设角速度约束值

利用如下公式(1)计算角速度超限的各关节轴的角速度超限比例系数k_{i,speed_ratio}：

其中，

为任一关节轴的预设角速度约束值；

为任一关节轴的角速度超限最大值。

(3)由于可能存在多个轴超限的情况，i可能存在多个值，即i∈[1,6]，根据各关节轴的角速度超限比例系数k_{i,speed_ratio}识别出角速度超限幅度最大的关节轴p：

k_{p,speed_ratio}＝min({i∈[1,6]|k_{i,speed_ratio}})

即，角速度超限比例系数最小的关节轴的角速度超限幅度最大。

步骤S125，基于所述角速度超限幅度最大的关节轴计算进行降速的最大可运行线速度，以作为所述机器人运行所述程序的最大可运行线速度。

根据所述角速度超限幅度最大的关节轴p的角速度超限比例系数k_{p,speed_ratio}及其角速度超限最大值

对应的角速度超限瞬时路径线速度v_{TCP_speed_max}，利用如下公式(2)计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度v_{TCP_speed_opimization}；

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max} (2)

其中，v_{TCP_speed_max}表示关节角速度超限幅度最大轴p的超限最大值

对应的瞬时路径线速度。

经过以上步骤即可得到笛卡尔空间的插补路径下角速度超限幅度最大的关节轴的进行降速的最大可运行线速度v_{TCP_speed_opimization}，以作为所述机器人运行所述程序的最大可运行线速度v_{TCP_opimization}，即：

v_{TCP_opimization}＝v_{TCP_speed_opimization}

上述步骤还可以参考图3所示，图3示出了根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度一具体实施方式的流程示意图。

如图3所示，在一具体实施方式中，速度前瞻包括步骤2.1～步骤2.6.

步骤2.1：采用加减速策略(例如S型加减速)规划路径，获得规划路程；

步骤2.2：基于位置及姿态插补算法计算笛卡尔空间下的路径离散点，并计算出路径线速度v_TCP(t)；

步骤2.3：采用机器人运动学逆解获得关节轴m的角度值θ_m(t)，并计算出关节轴角速度值

其中，m表示关节轴轴号，m＝1,2,3,…,r，t表示时刻，r表示工业机器人关节轴总个数；

步骤2.4：判断各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度；

步骤2.5：根据储存的角速度超限时的瞬时角速度值分析关节角速度超限幅度最大轴p：

步骤2.6：计算角速度超限幅度最大的关节轴p进行降速的最大可运行线速度v_{TCP_speed_opimization}，作为该路径下工业机器人实际最大可运行路径线速度v_{TCP_opimization}。

步骤S130，在所述机器人运行所述程序时，控制所述机器人以所述最大可运行路径线速度运行。

也就是说，当工业机器人开始进行插补时，采用所述最大可运行路径线速度运行。

为清楚说明本发明技术方案，下面再以一个具体实施例对本发明提供的机器人轨迹规划方法的执行流程进行描述。

以下以六轴工业机器人运行一段含有四、六关节角速度超限的直线指令为例，对本发明作进一步详细说明。

定义六轴工业机器人笛卡尔空间的路径运动参数，包括线速度约束、线加速度约束以及线加加速度约束，如下表1所示：

表1

定义四、六关节轴运动参数(其余关节轴参数不再列出)，包括关节轴角速度约束、关节轴角加速度约束以及关节轴角加加速约束，如下

表2所示：

表2

步骤1：工业机器人读取直线指令程序并完成程序解析。

(1)用户设定的路径线速度为v_{TCP_user}＝2500mm/s；

(2)直线指令的起点及终点如下表3所示：

笛卡尔坐标	起点	终点
			X轴/mm	905.756	905.756
Y轴/mm	-724.669	737.845
			Z轴/mm	1173.648	1133.648
A角/°	-179.991	-179.991
			B角/°	-74.511	-74.511
C角/°	359.989	359.989

表3

步骤2：对直线指令程序进行速度前瞻，可分析出如下信息：

分析出该路径角速度超限为四、六轴，如图4所示，四、六关节轴角速度超限最大值

为：

四、六关节轴角速度超限比例系数k_{4,speed_ratio}、k_{6,speed_ratio}为：

由k_{p,speed_ratio}＝k_{6,speed_ratio}可得关节角速度超限幅度最大轴为第六轴，超限最大角速度

为：

图5示出了基于本发明方案得到的最大可运行线速度，运行所述机器人得到的各关节轴的角度值及角速度值。如图5所示，对应的瞬时路径速度v_{TCP_speed_max}为：

v_{TCP_speed_max}＝2306.282mm/s

以关节角速度超限幅度最大轴(六轴)进行降速的最大可运行线速度v_{TCP_speed_opimization}为：

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max}＝1142.849mm/s

实际最大可运行路径线速度v_{TCP_opimization}：

v_{TCP_opimization}＝v_{TCP_speed_opimization}＝1142.849mm/s

图5中：

合成速度：速度前瞻时采用某种加减速策略规划路径后得到的合成速度；

合成加速度：速度前瞻时采用某种加减速策略规划路径后得到的合成加速度；

合成加加速度：速度前瞻时采用某种加减速策略规划路径后得到的合成加加速度(学术上加加速度等同于捷度)；

自动运行程序笛卡尔线速度：速度前瞻时基于合成速度进行笛卡尔模式下直线位置插补计算得到的笛卡尔线速度；

自动运行程序笛卡尔线加速度：速度前瞻时基于合成速度进行笛卡尔模式下直线位置插补计算得到的笛卡尔线速度；

自动运行程序笛卡尔线捷度：速度前瞻时基于合成速度进行笛卡尔模式下直线位置插补计算得到的笛卡尔线捷度。

图4和图5表明，采用本发明方案进行速度前瞻可准确分析用户设定路径的运动数据，包括关节角度值、关节角速度、关节角加速度值、采用预设加减速策略规划路径后得到的合成速度、采用预设加减速策略规划路径后得到的合成加速度和采用预设加减速策略规划路径后得到的合成加加速度。

步骤3：六轴工业机器人以最大可运行路径线速度v_{TCP_opimization}运行。

图6示出了基于本发明方案得到的最大可运行线速度，运行所述机器人得到的各关节轴的角度值及角速度值。由图6可见，六轴工业机器人使用本发明的轨迹规划方法，保证了规划获得的四、六关节轴角速度值均未超过定义的角速度值约束，避免伺服驱动器报警。图6表明采用本发明方案进行速度前瞻后可求得最大可运行线速度，并以此速度运行关节角度值无突变、关节角速度和关节角加速度值连续变化。

图7示出了基于本发明方案得到的最大可运行线速度，运行所述机器人得到的笛卡尔空间下路径线速度、路径线加速度及路径线加加速度。其中，

合成速度：已求得最大可运行线速度后采用某种加减速策略规划路径后得到的合成速度；

合成加速度：已求得最大可运行线速度后采用某种加减速策略规划路径后得到的合成加速度；

合成加加速度：已求得最大可运行线速度后采用某种加减速策略规划路径后得到的合成加加速度(学术上加加速度等同于捷度)；

自动运行程序笛卡尔线速度：已求得最大可运行线速度后基于合成速度进行笛卡尔模式下直线位置插补计算得到的笛卡尔线速度；

自动运行程序笛卡尔线加速度：已求得最大可运行线速度后基于合成速度进行笛卡尔模式下直线位置插补计算得到的笛卡尔线速度；

自动运行程序笛卡尔线捷度：已求得最大可运行线速度后基于合成速度进行笛卡尔模式下直线位置插补计算得到的笛卡尔线捷度。

由图7可见，以最大可运行路径线速度v_{TCP_opimization}＝1142.849mm/s运行，第六关节轴角速度峰值达到350.006°/s，在保证运行速度的前提下，又确保匀速段无加速度变化，可适用于有匀速运行要求的加工制造领域。

图5和图7表明采用本发明方案进行速度前瞻后可求得最大可运行线速度，并以此速度运行规划得到的合成速度匀速段无加速度突变，合成加速度和合成捷度连续，基于所规划的合成速度进行笛卡尔模式直线位置插补计算得到的笛卡尔线速度匀速段无加速度突变，全过程线加速度连续变化，线捷度连续无突变。

图8是本发明提供的机器人轨迹规划装置的一实施例的结构示意图。该装置可以用于六轴工业机器人。

如图8所示，所述机器人轨迹规划装置包括解析单元110、处理单元120和控制单元130。

解析单元110用于对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数。

所述运动参数具体可以包括：笛卡尔路径线速度、线加速度以及线加加速度，设定的路径线速度，起点、终点。例如，用户设定路径线速度为v_{TCP_user}＝2500mm/s，并设定起点及终点的坐标。

处理单元120用于根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度。

具体地，处理单元120根据解析得到的所述运动参数进行笛卡尔空间下的轨迹规划，从而得到机器人插补路径，并分析该插补路径下各关节轴中角速度超限的关节轴，得到角速度超限的关节轴中角速度超限幅度最大的关节轴，从而基于关节角速度约束计算出该路径下最大可运行路径线速度v_{TCP_opimization}。

图2是处理单元120根据解析得到的所述运动参数进行速度前瞻的步骤一具体实施例的流程示意图。如图2所示，步骤S120包括步骤S121、步骤S122、步骤S123、步骤S124和步骤S125。

步骤S121，根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

具体地，基于所述运动参数采用设定的加减速策略进行路径规划，得到相应的规划路程；通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

所述设定的加减速策略例如包括S型加减速、五次多项式加减速、Sin加减速。得到相应的规划路程后，通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点(即插补点)、路径线速度v_TCP(t)以及路径线加速度a_TCP(t)。其中t表示时刻。

步骤S122，通过机器人运动学逆解得到所述机器人运行时各关节轴的角度值，并根据各关节轴的角度值计算各关节轴的角速度值。

例如，采用机器人运动学逆解获得关节轴m的角度值θ_m(t)，并计算出关节轴角速度值

其中m表示关节轴轴号，m＝1,2,3,…,r，t表示时刻，r表示工业机器人关节轴总个数。

步骤S123，判断得到的各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度。

判断任一关节轴的角速度值是否超限，包括：判断所述关节轴的角速度值是否超过预设角速度约束值。每个关节轴具有对应的预设角速度约束值。

具体地，若任一关节轴i在t＝a时刻的角速度值

超过预定义的关节轴角速度约束

则判定以用户设定的路径线速度v_{TCP_user}运行该路径存在角速度超限，则储存该角速度超限的关节轴的轴号i、角速度超限瞬时角速度值

角速度超限瞬时路径线速度v_TCP(a)；

步骤S124，根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴。

若计算得到的全部关节轴在t＝c时刻角速度值

均小于等于预定义的关节轴角速度约束

其中m＝1,2,3,…,r，储存角速度超限瞬时角速度值

角速度超限瞬时路径线速度v_TCP(c)。超限区域结束，可开始分析关节角速度超限幅度最大轴p，分析方法如下：

(1)从储存的角速度超限时瞬时角速度值

数据中提取出角速度超限的各关节轴的角速度超限最大值(超限的角速度值中的最大角速度值)

(2)根据各关节轴的角速度超限最大值

及预设角速度约束值

利用如下公式(1)计算角速度超限的各关节轴的角速度超限比例系数k_{i,speed_ratio}：

其中，

为任一关节轴的预设角速度约束值；

为任一关节轴的角速度超限最大值。

(3)由于可能存在多个轴超限的情况，i可能存在多个值，即i∈[1,6]，根据各关节轴的角速度超限比例系数k_{i,speed_ratio}识别出角速度超限幅度最大的关节轴p：

k_{p,speed_ratio}＝min({i∈[1,6]|k_{i,speed_ratio}})

即，角速度超限比例系数最小的关节轴的角速度超限幅度最大。

步骤S125，基于所述角速度超限幅度最大的关节轴计算进行降速的最大可运行线速度，以作为所述机器人运行所述程序的最大可运行线速度。

根据所述角速度超限幅度最大的关节轴p的角速度超限比例系数k_{p,speed_ratio}及其角速度超限最大值

对应的角速度超限瞬时路径线速度v_{TCP_speed_max}，利用如下公式(2)计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度v_{TCP_speed_opimization}；

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max} (2)

其中，v_{TCP_speed_max}表示关节角速度超限幅度最大轴p的超限最大值

对应的瞬时路径线速度。

经过以上步骤即可得到笛卡尔空间的插补路径下角速度超限幅度最大的关节轴的进行降速的最大可运行线速度v_{TCP_speed_opimization}，以作为所述机器人运行所述程序的最大可运行线速度v_{TCP_opimization}，即：

v_{TCP_opimization}＝v_{TCP_speed_opimization}

上述步骤还可以参考图3所示，图3示出了根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度一具体实施方式的流程示意图。

如图3所示，处理单元120据解析得到的所述运动参数进行速度前瞻包括步骤2.1～步骤2.6。

步骤2.1：采用加减速策略(例如S型加减速)规划路径，获得规划路程；

步骤2.2：基于位置及姿态插补算法计算笛卡尔空间下的路径离散点，并计算出路径线速度v_TCP(t)；

步骤2.3：采用机器人运动学逆解获得关节轴m的角度值θ_m(t)，并计算出关节轴角速度值

其中，m表示关节轴轴号，m＝1,2,3,…,r，t表示时刻，r表示工业机器人关节轴总个数；

步骤2.4：判断各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度；

步骤2.5：根据储存的角速度超限时的瞬时角速度值分析关节角速度超限幅度最大轴p：

步骤2.6：计算角速度超限幅度最大的关节轴p进行降速的最大可运行线速度v_{TCP_speed_opimization}，作为该路径下工业机器人实际最大可运行路径线速度v_{TCP_opimization}。

控制单元130用于在所述机器人运行所述程序时，控制所述机器人以所述最大可运行路径线速度运行。

也就是说，当工业机器人开始进行插补时，采用所述最大可运行路径线速度运行。

本发明还提供对应于所述机器人轨迹规划方法的一种存储介质，其上存储有计算机程序，所述程序被处理器执行时实现前述任一所述方法的步骤。

本发明还提供对应于所述机器人轨迹规划方法的一种机器人，包括处理器、存储器以及存储在存储器上可在处理器上运行的计算机程序，所述处理器执行所述程序时实现前述任一所述方法的步骤。所述机器人包括六轴机器人。

本发明还提供对应于所述机器人轨迹规划装置的一种机器人，包括前述任一所述的机器人装置。所述机器人包括六轴机器人。

据此，本发明提供的方案，采用速度前瞻策略，在工业机器人插补笛卡尔路径前提前分析出该路径下关节角速度幅度最大轴，并基于关节角速度约束计算出该路径下最大可运行路径线速度，当工业机器人开始插补时可采用此路径线速度运行。采用发明的轨迹规划方案，工业机器人可按照用户指定路径的最大路径速度运行，避免出现伺服驱动器报警、机器人“跑飞”等现象。另外，无需动态调整路径速度，保证了路径速度的匀速段无加速度变化。

工业机器人可预先判定出该路径下关节角速度幅度最大轴，并基于关节角速度约束计算出该路径下最大可运行路径线速度，当机器人高速轨迹插补时以此速度运行，关节轴角速度满足约束要求，从而避免出现伺服驱动器报警、机器人“跑飞”等现象。另外，该方法在速度前瞻计算出最大可运行路径线速度后便按照此速度进行插补，高速轨迹插补时无需动态调整路径速度，从而保证了路径速度的匀速段无加速度变化。

本文中所描述的功能可在硬件、由处理器执行的软件、固件或其任何组合中实施。如果在由处理器执行的软件中实施，那么可将功能作为一或多个指令或代码存储于计算机可读媒体上或经由计算机可读媒体予以传输。其它实例及实施方案在本发明及所附权利要求书的范围及精神内。举例来说，归因于软件的性质，上文所描述的功能可使用由处理器、硬件、固件、硬连线或这些中的任何者的组合执行的软件实施。此外，各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为控制装置的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本发明的实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。

Claims (10)

1.一种机器人轨迹规划方法，其特征在于，包括：

对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数；

根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度；

在所述机器人运行所述程序时，控制所述机器人以所述最大可运行线速度运行；

根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度，包括：

根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度；

通过机器人运动学逆解得到所述机器人运行时各关节轴的角度值，并根据各关节轴的角度值计算各关节轴的角速度值；

判断得到的各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度；

根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴；

基于所述角速度超限幅度最大的关节轴计算进行降速的最大可运行线速度，以作为所述机器人运行所述程序的最大可运行线速度；

其中，判断任一关节轴的角速度值是否超限，包括：判断所述关节轴的角速度值是否超过预设角速度约束值。

2.根据权利要求1所述的方法，其特征在于，根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度，包括：

基于所述运动参数采用设定的加减速策略进行路径规划，得到相应的规划路程；

通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

3.根据权利要求1所述的方法，其特征在于，根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴，包括：

从储存的角速度超限时瞬时角速度值中提取出角速度超限的各关节轴的角速度超限最大值；

根据各关节轴的角速度超限最大值及预设角速度约束值，利用如下公式(1)计算角速度超限的各关节轴的角速度超限比例系数k_{i,speed_ratio}；

其中，

为任一关节轴的预设角速度约束值；

为任一关节轴的角速度超限最大值；

根据各关节轴的角速度超限比例系数识别出角速度超限幅度最大的关节轴。

4.根据权利要求1-3任一项所述的方法，其特征在于，计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度，包括：

根据所述角速度超限幅度最大的关节轴的角速度超限比例系数及其角速度超限最大值对应的角速度超限瞬时路径线速度，利用如下公式(2)计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度v_{TCP_speed_opimization}；

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max} (2)

其中，k_{p,speed_ratio}为角速度超限幅度最大的关节轴的角速度超限比例系数；v_{TCP_speed_max}为角速度超限幅度最大的关节轴的角速度超限最大值对应的角速度超限瞬时路径线速度。

5.一种机器人轨迹规划装置，其特征在于，包括：

解析单元，用于对所述机器人所要运行的程序进行程序解析得到预先设定的运动参数；

处理单元，用于根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度；

控制单元，用于在所述机器人运行所述程序时，控制所述机器人以所述最大可运行线速度运行；

所述处理单元，根据解析得到的所述运动参数进行速度前瞻，得到所述机器人运行所述程序的最大可运行线速度，包括：

根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度；

通过机器人运动学逆解得到所述机器人运行时各关节轴的角度值，并根据各关节轴的角度值计算各关节轴的角速度值；

判断得到的各关节轴的角速度值是否超限，并储存角速度超限的关节轴的轴号、角速度超限时的瞬时角速度值以及角速度超限时的瞬时路径线速度；

根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴；

基于所述角速度超限幅度最大的关节轴计算进行降速的最大可运行线速度，以作为所述机器人运行所述程序的最大可运行线速度；

其中，判断任一关节轴的角速度值是否超限，包括：判断所述关节轴的角速度值是否超过预设角速度约束值。

6.根据权利要求5所述的装置，其特征在于，根据所述运动参数对所述机器人进行轨迹规划，得到笛卡尔空间下的路径离散点、路径线速度以及路径线加速度，包括：

基于所述运动参数采用设定的加减速策略进行路径规划，得到相应的规划路程；

通过位置及姿态插补算法计算所述规划路程在笛卡尔空间下的路径离散点、路径线速度以及路径线加速度。

7.根据权利要求5所述的装置，其特征在于，根据储存的角速度超限时的瞬时角速度值，确定角速度超限的关节轴中角速度超限幅度最大的关节轴，包括：

从储存的角速度超限时瞬时角速度值中提取出角速度超限的各关节轴的角速度超限最大值；

根据各关节轴的角速度超限最大值及预设角速度约束值，利用如下公式(1)计算角速度超限的各关节轴的角速度超限比例系数k_{i,speed_ratio}；

其中，

为任一关节轴的预设角速度约束值；

为任一关节轴的角速度超限最大值；

根据各关节轴的角速度超限比例系数识别出角速度超限幅度最大的关节轴。

8.根据权利要求5-7任一项所述的装置，其特征在于，计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度，包括：

根据所述角速度超限幅度最大的关节轴的角速度超限比例系数及其角速度超限最大值对应的角速度超限瞬时路径线速度，利用如下公式(2)计算所述角速度超限幅度最大的关节轴进行降速的最大可运行线速度v_{TCP_speed_opimization}；

v_{TCP_speed_opimization}＝k_{p,speed_ratio}·v_{TCP_speed_max} (2)

其中，k_{p,speed_ratio}为角速度超限幅度最大的关节轴的角速度超限比例系数；v_{TCP_speed_max}为角速度超限幅度最大的关节轴的角速度超限最大值对应的角速度超限瞬时路径线速度。

9.一种存储介质，其特征在于，其上存储有计算机程序，所述程序被处理器执行时实现权利要求1-4任一所述方法的步骤。

10.一种机器人，其特征在于，包括处理器、存储器以及存储在存储器上可在处理器上运行的计算机程序，所述处理器执行所述程序时实现权利要求1-4任一所述方法的步骤，包括如权利要求5-8任一所述的机器人轨迹规划装置。

Images