CN107598921B - 一种应用于六轴机械手臂的空间平滑停止控制方法 - Google Patents

Abstract

本发明提供一种应用于六轴机械手臂的空间平滑停止控制方法，包括以下步骤，测得并输入六轴机械手臂开始减速的初始点信息，包括位置，速度和加速度，并且假设六轴机械手臂经过时间段T，减速后停止点信息是速度为0，加速度为0。采用五次方程式作为减速阶段的位置插补算法，计算出位置，速度和加速度分别和任一插补采样时间点的关系，从而实现对六轴机械手臂的空间平滑停止控制。该控制方法采用5次线性方程推导出减速停止过程中机械手臂的减速运动曲线，使其从减速到停止更为平滑，更符合电机运转特性，对电机冲力较小，起到了很好的保护作用，延长了使用寿命。

Description

一种应用于六轴机械手臂的空间平滑停止控制方法

技术领域

本发明涉及机器人技术领域，更具体地，涉及一种应用于六轴机械手臂的空间平滑停止控制方法。

背景技术

六轴机械手臂是目前集运动控制算法、空间轨迹规划算法、动力学算法、伺服驱动控制及伺服电机、减速机等高科技含量的综合型智能设备。其中基于运动控制和轨迹规划为核心的控制器又是机械手臂的大脑中枢，能够控制机械手臂精准完成复杂、重复的动作，运动控制过程中运动控制的平滑性和轨迹规划的合理性会影响机械手臂相关硬件设备的寿命与用户使用体验。机器人关节空间平滑停止算法是全部六轴机械手臂众多运动控制算法中的一种，能够避免机械手臂在正常停止过程中因速度产生骤变而损坏电机设备。

目前六轴机械手臂从正常运转状态减速转变为正常停止时，由于机械手臂在运动过程中的惯性，停止时会产生较大的扭转力矩，这对电机承担的冲力较大，且容易损坏电机齿轮。使用此控制算法后，在停止时，会提前发送缓冲减速指令，在空间与时间允许的条件下减速停止。

发明内容

本发明要解决的技术问题在于，针对现有技术中机械手臂在运动过程中的惯性，停止时会产生较大的扭转力矩，这对电机承担的冲力较大，且容易损坏电机齿轮等缺陷，提供一种从减速到停止更为平滑，更符合电机运转特性，对电机冲力较小的应用于六轴机械手臂的空间平滑停止控制方法。

本发明解决其技术问题所采用的技术方案是：提供一种应用于六轴机械手臂的空间平滑停止控制方法，包括以下步骤：

测得并输入六轴机械手臂开始减速的初始点信息，位置P₀，速度V₀，加速度A₀，并且假设六轴机械手臂经过时间段T，减速后停止点信息是速度为0，加速度为0，位置P_T；

采用五次方程式作为减速阶段的位置插补算法，

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵， (1)

其中，t是减速时间段T内任一插补采样时间点，P(t)为任一插补采样时间点对应的位置，而a₀，a₁，a₂，a₃，a₄和a₅是系数；

假设位置曲线对称，则在时间t＝0时，机械手臂处于初始状态(P₀，V₀)，

在时间t＝2T时，机械手臂处于状态(P₀，-V₀)，

而在时间t＝T时，机械手臂处于状态(P_T，0)；

在时间段T内任一插补采样时间点t的位置P(t)、速度V(t)以及加速度A(t)如下，从而实现对六轴机械手臂的空间平滑停止控制：

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵， (2)

V(t)＝P’(t)＝a₁+2a₂t+3a₃t²+4a₄t³+5a₅t⁴， (3)

A(t)＝P”(t)＝2a₂+6a₃t+12a₄t²+20a₅t³， (4)

其中，a₀＝P₀， (5)

a₁＝V₀， (6)

优选地，基于公式(2)，(3)和(4)得到如下约束条件：

P(0)＝P₀， (11)

P’(0)＝V₀， (12)

P”(0)＝A₀， (13)

P’(T)＝0， (14)

P”(T)＝0， (15)

P’(2T)＝-V₀， (16)

根据公式(11)，(12)，(13)，(14)，(15)和(16)联立求解得到公式(5)，(6)，(7)，(8)，(9)和(10)。

优选地，在时间t＝T时，机械手臂处于状态(P_T，0)，该状态为减速到完全停止，速度为0的减速终止状态。

优选地，位置曲线对称的假设指的是六轴机械手臂从位置P₀开始减速并停止于位置P_T之后，又从位置P_T以大小相同，但方向相反的加速度开始加速并经过位置P₀，经过位置P₀时速度大小与从位置P₀开始减速时的速度大小相同，但是方向相反。

优选地，若在时间t＝T时，机械手臂处于状态(P_T，0)，且位置P_T超过六轴机械手臂的实际限位值，则重新设置减速后停止点的位置为实际限位值P_m，且速度和加速度仍然为0。

优选地，在位置P_T超过六轴机械手臂的实际限位值，且实际限位值为P_m的情况下，在时间t＝0时，机械手臂处于初始状态(P₀，V₀，A₀)；在时间t＝T时，机械手臂处于停止状态(P_m，0，0)，则在时间段T内任一插补采样时间点t的位置、速度以及加速度如公式(2)，(3)和(4)所示，实现对六轴机械手臂的空间平滑停止控制，但是系数如下：

a₀＝P₀， (17)

a₁＝V₀， (18)

优选地，在位置P_T超过六轴机械手臂的实际限位值，且实际限位值为P_m的情况下，基于公式(2)，(3)和(4)得到如下约束条件：

P(0)＝a₀＝P₀， (23)

P’(0)＝a₁＝V₀， (24)

P”(0)＝2a₂＝A₀， (25)

P(T)＝0， (26)

P’(T)＝0， (27)

P”(2T)＝-V₀， (28)

根据公式(23)，(24)，(25)，(26)，(27)和(28)联立求解得到公式(17)，(18)，(19)，(20)，(21)和(22)。

本发明的有益效果在于，该控制方法采用5次线性方程推导出减速停止过程中机械手臂的减速运动曲线，使其从减速到停止更为平滑，更符合电机运转特性，对电机冲力较小，起到了很好的保护作用，延长了使用寿命。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例的流程示意图；

图2是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中假设机械手臂加减速运动的位置曲线对称的示意图；

图3是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂基于平滑减速算法进行减速的位置变化曲线示意图；

图4是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂基于平滑减速算法进行减速的速度变化曲线示意图；

图5是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂基于平滑减速算法进行减速的加速度变化曲线示意图；

图6是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂基于常规通用减速算法进行减速的位置变化曲线示意图；

图7是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂基于常规通用减速算法进行减速的速度变化直线示意图；

图8是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂基于常规通用减速算法进行减速的加速度变化直线示意图；

图9是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂分别基于平滑减速算法和常规通用减速算法进行减速的加速度变化直线对比示意图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

图1是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例的流程示意图。该控制方法，包括以下步骤：测得并输入六轴机械手臂开始减速的初始点信息，位置P₀，速度V₀，加速度A₀，并且假设六轴机械手臂经过时间段T，减速后停止点信息是速度为0，加速度为0，位置P_T；采用五次方程式作为减速阶段的位置插补算法，公式如下所示。

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵， (1)

其中，t是减速时间段T内任一插补采样时间点，P(t)为任一插补采样时间点对应的位置，而a₀，a₁，a₂，a₃，a₄和a₅是系数。

图2是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中假设机械手臂加减速运动的位置曲线对称的示意图。假设机械手臂加速和减速运动的位置曲线对称，则该位置曲线的性质如图2所示，在时间t＝0时，机械手臂处于初始状态(P₀，V₀)，在时间t＝2T时，机械手臂处于状态(P₀，-V₀)，而在时间t＝T时，机械手臂处于状态(P_T，0)。

在时间段T内任一插补采样时间点t的位置P(t)、速度V(t)以及加速度A(t)如下，从而实现对六轴机械手臂的空间平滑停止控制：

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵， (2)

V(t)＝P’(t)＝a₁+2a₂t+3a₃t²+4a₄t³+5a₅t⁴， (3)

A(t)＝P”(t)＝2a₂+6a₃t+12a₄t²+20a₅t³， (4)

其中，a₀＝P₀， (5)

a₁＝V₀， (6)

然后，基于公式(2)，(3)和(4)得到如下约束条件：

P(0)＝P₀， (11)

P’(0)＝V₀， (12)

P”(0)＝A₀， (13)

P’(T)＝0， (14)

P”(T)＝0， (15)

P’(2T)＝-V₀， (16)

根据公式(11)，(12)，(13)，(14)，(15)和(16)联立求解得到公式(5)，(6)，(7)，(8)，(9)和(10)，从而求解得到各个系数。

在时间t＝T时，机械手臂处于状态(P_T，0)，该状态为减速到完全停止，速度为0的减速终止状态。

位置曲线对称的假设指的是六轴机械手臂从位置P₀开始减速并停止于位置P_T之后，又从位置P_T以大小相同，但方向相反的加速度开始加速并经过位置P₀，经过位置P₀时速度大小与从位置P₀开始减速时的速度大小相同，但是方向相反。

若在时间t＝T时，机械手臂处于状态(P_T，0)，且位置P_T超过六轴机械手臂的实际限位值，则重新设置减速后停止点的位置为实际限位值P_m，且速度和加速度仍然为0。

在位置P_T超过六轴机械手臂的实际限位值，且实际限位值为P_m的情况下，在时间t＝0时，机械手臂处于初始状态(P₀，V₀，A₀)；在时间t＝T时，机械手臂处于停止状态(P_m，0，0)，则在时间段T内任一插补采样时间点t的位置、速度以及加速度如公式(2)，(3)和(4)所示，实现对六轴机械手臂的空间平滑停止控制，但是系数如下：

a₀＝P₀， (17)

a₁＝V₀， (18)

在位置P_T超过六轴机械手臂的实际限位值，且实际限位值为P_m的情况下，基于公式(2)，(3)和(4)得到如下约束条件：

P(0)＝a₀＝P₀， (23)

P’(0)＝a₁＝V₀， (24)

P”(0)＝2a₂＝A₀， (25)

P(T)＝0， (26)

P’(T)＝0， (27)

P”(2T)＝-V₀， (28)

根据公式(23)，(24)，(25)，(26)，(27)和(28)联立求解得到公式(17)，(18)，(19)，(20)，(21)和(22)，从而求解达到各个系数。

在一个优选实施例中，假设六轴机械手臂在某一轴上匀速运动状态下，需要立刻减速。此时机器人的位置P₀＝2rad，速度V₀＝1rad/s，加速度A₀＝0rad/s²，减速时间要求是T＝0.5s。将以上的数据代入平滑减速算法的系数计算公式(11)，(12)，(13)，(14)，(15)和(16)中，可以计算求解得到如下六个系数：

a₀＝P₀＝2

a₁＝V₀＝1

将计算得到的a₀，a₁，a₂，a₃，a₄，a₅代入减速阶段的五次插补方程中，可以得到任一插补采样时间点t的位置P(t)、速度V(t)以及加速度A(t)如下，从而实现对六轴机械手臂的空间平滑停止控制：

位置：P(t)＝2+t-5.3333t³+8t⁴-3.2t⁵，t∈[0，0.5]

速度：P′(t)＝1-15.9999t²+32t³-16t⁴，t∈[0，0.5]

加速度：P″(t)＝-31.9998t+96t²-64t³，t∈[0，0.5]

假设以0.001s为基础时间间隔进行插补采样，控制机械手臂的减速，将插补时间t代入方程，即可求得每个插补点期望的位置、速度、加速度的一组输出值，在0.5秒内得到500个插补点，共500组数值。分别对0.5秒内500个插补采样点的位置、速度、加速度的数值通过图形描绘，生成图3，图4和图5所示的曲线。

如果使用常规普通的梯形减速算法，得到任一插补采样点t的位置P(t)、速度V(t)以及加速度A(t)如下：

位置：

速度：P′(t)＝V₀+At

加速度：

将该减速运动过程中的初始状态代入以上方程，可以得到：

位置：P(t)＝2+t-t²，t∈[0，0.5]

速度：P′(t)＝1-2t，t∈[0，0.5]

加速度：P″(t)＝-2，t∈[0，0.5]

同样假设以0.001s为基础时间间隔进行插补采样，控制机械手臂的减速，将插补采样点t代入方程，即可求得每个插补采样点期望的位置、速度、加速度的一组输出值，在0.5秒内得到500个插补采样点，共500组数值。分别对0.5秒内500个插补采样点的位置、速度、加速度的数值通过图形描绘，生成图6，图7和图8所示的曲线。

图3是机械手臂基于平滑减速算法进行减速的位置变化曲线示意图；而图6是机械手臂基于常规通用减速算法进行减速的位置变化曲线示意图。

图4是机械手臂基于平滑减速算法进行减速的速度变化曲线示意图；而图7是机械手臂基于常规通用减速算法进行减速的速度变化直线示意图。

图5是机械手臂基于平滑减速算法进行减速的加速度变化曲线示意图；而图8是机械手臂基于常规通用减速算法进行减速的加速度变化直线示意图。

基于以上图中曲线的对比可以看出，运用本发明的平滑减速算法从速度1rad/s减速到停止的减速距离为0.2333rad，而应用常规普通减速算法从速度1rad/s减速到停止的减速距离为0.25rad。首先从减速距离上来说，本发明的平滑减速算法所需的减速距离显著少于常规普通减速算法。另外，平滑减速算法进行减速的速度变化曲线更为平滑和顺畅，这意味着对电机更小的冲击和损害。另外重要的是，初速度V₀越大、减速时间段T越短，本发明算法的优势越明显，即本发明平滑减速算法的减速距离与常规普通算法的减速距离之间的差距也会越大。在较为极限的情况下，减速距离之间的差距可达10％左右。

而且，本发明算法可以在减速距离更短的情况下，同时保证更平滑的速度减速曲线，通过加速度大小的自动变化，最大程度减少对电机的冲击和磨损。

图9是本发明应用于六轴机械手臂的空间平滑停止控制方法优选实施例中机械手臂分别基于平滑减速算法和常规通用减速算法进行减速的加速度变化直线对比示意图。从图9中可以明显地看出，本发明平滑减速算法所得到的加速度以平滑的方式变化，减速过程中的加速度由小变化到大，又由大变化到小，该种方式尽量减少了因为突然出现大的加速度而对电机产生的冲击，大大改善了整个减速过程的平稳性，避免过冲现象的发生，保护电机并延长了其使用寿命。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (7)

1.一种应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，包括以下步骤：

测得并输入六轴机械手臂开始减速的初始点信息，位置P₀，速度V₀，加速度A₀，并且假设六轴机械手臂经过时间段T，减速后停止点信息是速度为0，加速度为0，位置P_T；

采用五次方程式作为减速阶段的位置插补算法，

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵, (1)

其中，t是减速时间段T内任一插补采样时间点，P(t)为任一插补采样时间点对应的位置，而a₀，a₁，a₂，a₃，a₄和a₅是系数；

假设位置曲线对称，则在时间t＝0时，机械手臂处于初始状态(P₀，V₀),

在时间t＝2T时，机械手臂处于状态(P₀，-V₀)，

而在时间t＝T时，机械手臂处于状态(P_T，0)；

在时间段T内任一插补采样时间点t的位置P(t)、速度V(t)以及加速度A(t)如下，从而实现对六轴机械手臂的空间平滑停止控制：

P(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴+a₅t⁵, (2)

V(t)＝P’(t)＝a₁+2a₂t+3a₃t²+4a₄t³+5a₅t⁴, (3)

A(t)＝P”(t)＝2a₂+6a₃t+12a₄t²+20a₅t³, (4)

其中，a₀＝P₀, (5)

a₁＝V₀， (6)

2.根据权利要求1所述的应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，基于公式(2)，(3)和(4)得到如下约束条件：

P(0)＝P₀， (11)

P’(0)＝V₀， (12)

P”(0)＝A₀， (13)

P’(T)＝0， (14)

P”(T)＝0， (15)

P’(2T)＝-V₀， (16)

根据公式(11)，(12)，(13)，(14)，(15)和(16)联立求解得到公式(5)，(6)，(7)，(8)，(9)和(10)。

3.根据权利要求2所述的应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，在时间t＝T时，机械手臂处于状态(P_T，0)，该状态为减速到完全停止，速度为0的减速终止状态。

4.根据权利要求3所述的应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，位置曲线对称的假设指的是六轴机械手臂从位置P₀开始减速并停止于位置P_T之后，又从位置P_T以大小相同，但方向相反的加速度开始加速并经过位置P₀，经过位置P₀时速度大小与从位置P₀开始减速时的速度大小相同，但是方向相反。

5.根据权利要求2所述的应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，若在时间t＝T时，机械手臂处于状态(P_T，0)，且位置P_T超过六轴机械手臂的实际限位值，则重新设置减速后停止点的位置为实际限位值P_m，且速度和加速度仍然为0。

6.根据权利要求5所述的应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，在位置P_T超过六轴机械手臂的实际限位值,且实际限位值为P_m的情况下，在时间t＝0时，机械手臂处于初始状态(P₀，V₀，A₀)；在时间t＝T时，机械手臂处于停止状态(P_m，0，0)，则在时间段T内任一插补采样时间点t的位置、速度以及加速度如公式(2)，(3)和(4)所示，实现对六轴机械手臂的空间平滑停止控制，但是系数如下：

a₀＝P₀, (17)

a₁＝V₀， (18)

7.根据权利要求6所述的应用于六轴机械手臂的空间平滑停止控制方法，其特征在于，在位置P_T超过六轴机械手臂的实际限位值,且实际限位值为P_m的情况下，基于公式(2)，(3)和(4)得到如下约束条件：

P(0)＝a₀＝P₀， (23)

P’(0)＝a₁＝V₀， (24)

P”(0)＝2a₂＝A₀， (25)

P(T)＝0， (26)

P’(T)＝0， (27)

P”(2T)＝-V₀， (28)

根据公式(23)，(24)，(25)，(26)，(27)和(28)联立求解得到公式(17)，(18)，(19)，(20)，(21)和(22)。