CN109773778A - 一种工业机器人关节空间同步运动的规划方法 - Google Patents

Abstract

本发明涉及一种工业机器人关节空间同步运动的规划方法。该方法不仅关节姿态是平滑过渡的，其速度、加速度也是平滑过渡的，没有阶跃和突变，抗抖动性能好。实现该方法包括以下步骤：【1】启动运动控制器；【2】获取期望的机器人运动的起始点和末端点；【3】计算规划所需的最大时长MaxT；【4】计算各关节运动规划所需的5个系数；【5】判断运行模式，若为正常模式，则执行步骤【6】，若为减速停止模式，则执行步骤【7】；【6】正常运行模式下的规划；【7】减速停止模式下的规划；【8】结束。

Description

一种工业机器人关节空间同步运动的规划方法

技术领域

本发明涉及一种工业机器人关节空间同步运动的规划方法。

背景技术

工业机器人的构型是多关节耦合的，如何实现多关节在空间中的同步运动，是工业机器人控制的关键问题之一。当前公开的文献资料的技术多采用T型或三次多项式加减速的方法进行关节轨迹规划，这种方法规划出的运动过程各关节的运动速度和加速度不是平滑过渡的，其抗抖动性能差。

发明内容

为了解决背景技术中的问题，本发明提出了一种工业机器人关节空间同步运动的规划方法。采用该方法不仅关节姿态是平滑过渡的，其速度、加速度也是平滑过渡的，没有阶跃和突变，抗抖动性能更好。

为了实现发明目的，本发明采用的具体技术方案是：

一种工业机器人关节空间同步运动的规划方法，主要包括以下步骤：

【1】启动运动控制器；

【2】获取期望的机器人运动的起始点和末端点；

【3】计算规划所需的最大时长MaxT；

【4】计算机器人各关节运动规划所需的5个系数；

【5】判断运行模式，若为正常模式，则执行步骤【6】，若为减速停止模式，则执行步骤【7】；

【6】正常运行模式下的规划；

【7】减速停止模式下的规划；

【8】结束。

进一步地，所述步骤【3】的具体计算过程是：

【3.1】根据已知的起始点和末端点，计算每个运动关节需转动的角度θ_i，具体公式为：

θ_i＝sEndP.A_i-sStaP.A_i

其中，sEndP.A_i为第i关节运动的末端点，sStaP.A_i为第i关节运动的起始点；

【3.2】计算每个运动关节达到期望的关节运动特性需要的加速时间Ta_Acci和减速时间Ta_Deci，具体公式为：

其中，lrAcc[i]为期望的第i关节的加速度，lrVel[i]为期望的第i关节的速度，lrDec[i]为期望的第i关节的减速度；

【3.3】计算每个运动关节按照期望的关节运动特性需要的加速和减速时转动的角度，具体公式为：

θa_Acci为第i关节加速时转动的角度，θa_Deci为第i关节减速时转动的角度；

【3.4】计算每个运动关节总的运动时长，具体公式为：

其中，MaxV_i代表第i关节实际所能到达的最大速度；

【3.5】在T_i中找出最大的一个值，即就是最大时长MaxT。

进一步地，所述步骤【4】的具体计算公式为：

其中，a_0i、a_1i、a_2i、a_3i、a_4i代表第i关节运动规划所需的5个系数。

进一步地，所述步骤【6】中正常模式下的规划具体为：

计算正常模式下各个运动关节第j个插值点的角度MidAngles.A_i；

其中j＝1,2,…是插值点的序列数，计算过程中

lrCycleTime×j≤MaxT，

若，lrCycleTime×j＞MaxT，则规划完成。

进一步地，所述步骤【7】中减速停止模式下的规划流程如下：

【7.1】计算当前各个运动关节的转动速度υ_i，具体公式为：

【7.2】计算各个运动关节从当前速度减速到停止需要的时间，具体公式为：

【7.3】在Tstopi中获取最大停止时间MaxT_stop；

【7.4】计算各个运动关节的角度，具体公式为：

A、当在MaxT_stop＞lrCycleTime的情况下，

其中θ_ci是当前的各关节的角度；j＝1,2,…是插值点的序列数；计算过程中需满足lrCycleTime×j≤MaxT_stop，若lrCycleTime×j＞MaxT_stop时，规划结束；

B、当在MaxT_stop≤lrCycleTime情况下，规划结束。

本发明的有益效果是：

本发明实现了正常运行和减速停两种模式下的关节空间同步运动的轨迹规划，该方法不仅关节姿态是平滑过渡的，其速度、加速度也是平滑过渡的，没有阶跃和突变，抗抖动性能更好，该方法简单，步骤清晰，容易实现。

附图说明

图1为本发明方法的流程框图；

图2为计算最大时长MaxT的流程框图；

图3为减速停止模式的流程框图。

具体实施方式

下面结合附图对本发明的方法做进一步的详述：

在机器人运动控制器中运行该方法是已知用户期望的机器人各关节运动的起始点和末端点，规划出这两点之间每一个时刻机器人各关节的运动状态，保证机器人各关节同步运动，并且其速度、加速度平滑过渡。

如图1所示，该方法的是在运动控制器内运行的程序指令，其运行的流程是：

步骤【1】启动运动控制器；

步骤【2】获取期望的机器人各关节运动的末端点sEndP.A_i和起始点sStaP.A_i；

步骤【3】根据已知的起始点和末端点，计算规划所需的最大时长MaxT；具体步骤是：

步骤【3.1】计算每个运动关节需转动的角度θ_i，具体公式为：

θ_i＝sEndP.A_i-sStaP.A_i

其中，sEndP.A_i为第i关节运动的末端点，sStaP.A_i为第i关节运动的起始点；

步骤【3.2】计算每个运动关节达到期望的关节运动特性需要的加速时间Ta_Acci和减速时间Ta_Deci，具体公式为：

其中，lrAcc[i]为期望的第i关节的加速度，lrVel[i]为期望的第i关节的速度，lrDec[i]为期望的第i关节的减速度；

步骤【3.3】计算每个运动关节按照期望的关节运动特性需要的加速和减速时转动的角度，具体公式为：

θa_Acci为第i关节加速时转动的角度，θa_Deci为第i关节减速时转动的角度；

步骤【3.4】计算每个运动关节总的运动时长，具体公式为：

其中，MaxV_i代表第i关节实际所能到达的最大速度；

步骤【3.5】在T_i中找出最大的一个值，就是最大时长MaxT。

步骤【4】计算机器人各关节运动规划所需的5个系数；

其中，a_0i、a_1i、a_2i、a_3i、a_4i代表第i关节运动规划所需的5个系数；

步骤【5】判断运行模式，若为正常模式，则执行步骤【6】，若为减速停止模式，则执行步骤【7】；需要说明的是：会出现减速停止模式的情况一般是需要机器人突然停止下来所需的特殊处理形式，一般情况下机器人均是按照正常模式下的规划进行；

步骤【6】正常模式下的规划；

计算正常模式下各个运动关节第j个插值点的角度MidAngles.A_i；

MidAngles.A_i＝a_0i+a_1i(lrCycleTime×j)⁴+a_2i(lrCycleTime×j)⁵

+a_3i(lrCycleTime×j)⁶+a_4i(lrCycleTime×j)⁷

其中j＝1,2,…是插值点的序列数，计算过程中

lrCycleTime×j≤MaxT，

若，lrCycleTime×j＞MaxT，则规划完成，跳转至步骤【8】。

步骤【7】减速停止模式下的规划；

其具体流程是：

步骤【7.1】计算当前各个运动关节的转动速度υ_i，具体公式为：

步骤【7.2】计算各个运动关节从当前速度减速到停止需要的时间，具体公式为：

步骤【7.3】在Tstopi中获取最大停止时间MaxT_stop；

步骤【7.4】计算各个运动关节的角度，具体公式为：

A、当在MaxT_stop＞lrCycleTime的情况下，

其中θ_ci是当前的各关节的角度；j＝1,2,…是插值点的序列数；计算过程中需满足lrCycleTime×j≤MaxT_stop，若lrCycleTime×j＞MaxT_stop时，规划结束；

B、当在MaxT_stop≤lrCycleTime情况下，规划结束。

步骤【8】结束。

Claims (5)

1.一种工业机器人关节空间同步运动的规划方法，其特征在于，包括以下步骤：

【1】启动运动控制器；

【2】获取期望的机器人运动的起始点和末端点；

【3】计算规划所需的最大时长MaxT；

【4】计算各关节运动规划所需的5个系数；

【5】判断运行模式，若为正常模式，则执行步骤【6】，若为减速停止模式，则执行步骤【7】；

【6】正常运行模式下的规划；

【7】减速停止模式下的规划；

【8】结束。

2.根据权利要求1所述的工业机器人关节空间同步运动的规划方法，其特征在于：所述步骤【3】的具体计算过程是：

【3.1】根据已知的机器人各关节运动的起始点和末端点，计算每个运动关节需转动的角度θ_i，具体公式为：

θ_i＝sEndP.A_i-sStaP.A_i

其中，sEndP.A_i为第i关节运动的末端点，sStaP.A_i为第i关节运动的起始点；

【3.2】计算每个运动关节要达到期望的关节运动特性所需的加速时间Ta_Acci和从期望的关节速度减速到零的减速时间Ta_Deci，具体公式为：

其中，lrAcc[i]为期望的第i关节的加速度，lrVel[i]为期望的第i关节的速度，lrDec[i]为期望的第i关节的减速度；

【3.3】计算每个运动关节按照期望的关节运动特性需要的加速和减速时转动的角度，具体公式为：

θa_Acci为第i关节加速时转动的角度，θa_Deci为第i关节减速时转动的角度；

【3.4】计算每个运动关节总的运动时长，具体公式为：

其中，MaxV_i代表第i关节实际所能到达的最大速度；

【3.5】在T_i中找出最大的一个值，即就是最大时长MaxT。

3.根据权利要求1所述的工业机器人关节空间同步运动的规划方法，其特征在于：

所述步骤【4】的具体计算公式为：

其中，a_0i、a_1i、a_2i、a_3i、a_4i代表第i关节运动规划所需的5个系数。

4.根据权利要求1所述的工业机器人关节空间同步运动的规划方法，其特征在于：所述步骤【6】中正常模式下的规划具体为：

计算正常模式下各个运动关节第j个插值点的角度MidAngles.A_i；

其中，lrCycleTime为插值的间隔时间，j＝1,2,…为插值点的序列数，

计算过程中lrCycleTime×j≤MaxT，若，lrCycleTime×j＞MaxT，则规划完成。

5.根据权利要求1所述的工业机器人关节空间同步运动的规划方法，其特征在于：

所述步骤【7】中减速停止模式下的规划流程如下：

【7.1】计算当前各个运动关节的转动速度υ_i，具体公式为：

【7.2】计算各个运动关节从当前速度减速到停止需要的时间，具体公式为：

【7.3】在Tstopi中获取最大停止时间MaxT_stop；

【7.4】计算各个运动关节的角度，具体公式为：

A、当在MaxT_stop＞lrCycleTime的情况下，

其中θ_ci是当前的各关节的角度；j＝1,2,…是插值点的序列数；计算过程中需满足lrCycleTime×j≤MaxT_stop，若lrCycleTime×j＞MaxT_stop时，规划结束；

B、当在MaxT_stop≤lrCycleTime情况下，规划结束。