CN106695787B - 一种速度规划方法 - Google Patents

Abstract

本发明涉及机器人控制技术领域，公开了一种速度规划方法。该速度规划方法包括，根据第一约束条件，对多轴机构的所有轴进行S型速度规划；其中，第一约束条件包括运动的路程、起点与末点速度、起点与末点加速度、运行速度、最大运行速度、最大加速度以及最大加加速度；从规划结果中选取时间最长的轴作为基准轴；根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划；其中，第二约束条件包括第一约束条件中除运行速度以外的所有约束条件以及基准轴的总运行时间。该方法可用于机器人各关节轴之间的同步，亦可用于位置与姿态之间的同步，可解决始末速度不为零的同步问题，并且，计算量较小，实时性较高。

Description

一种速度规划方法

技术领域

本发明涉及机器人控制技术领域，特别涉及一种速度规划方法。

背景技术

机器人属于多轴机构。在执行任务时，机器人各轴之间往往需要进行同步，即，同时到达终点。为了实现同步，往往先对所有轴按照各自的特性及系统要求分别进行速度规划，然后选取时间最长者作为基准轴，对其他轴按照基准轴进行同步。常见的用于机器人同步的方法主要有两种：(1)基于位移比例的同步方法，(2)给定时间的S型速度规划方法。

在实现本发明的过程中，发明人发现现有技术中至少存在以下问题：基于位移比例的同步方法，只能对单段进行同步，不同段之间的转折点速度及加速度需降为零，否则会引起速度突变，导致机械抖动，产生机械损伤。S型速度规划方法，需要大量的计算及迭代，实时性差。

发明内容

本发明实施方式的目的在于提供一种速度规划方法，该方法可用于机器人各关节轴之间的同步，亦可用于位置与姿态之间的同步，可解决同步过程中始末速度不为零的同步问题，并且，计算量较小，实时性较高。

为解决上述技术问题，本发明的实施方式提供了一种速度规划方法，包括：根据第一约束条件，对多轴机构的所有轴进行S型速度规划；其中，第一约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、运行速度、最大运行速度、最大加速度以及最大加加速度；从规划结果中，选取时间最长的轴作为基准轴；根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划；其中，第二约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、最大运行速度、最大加速度、最大加加速度以及基准轴的总运行时间。

本发明实施方式相对于现有技术而言，根据第二约束条件，以三次B样条曲线为规划曲线，对多轴机构的基准轴以外的其余的轴进行给定时间的速度规划，其中，第二约束条件可以包括运动的路程、起点速度等，这样，各轴不同段之间的转折点的速度以及加速度不需要降为零。这样，规划过程中的计算量较小，且实时性较高。

另外，根据第二约束条件，并以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划具体包括：根据所述第二约束条件，并以三次准均匀B样条曲线为规划曲线，对基准轴以外的其余的轴进行给定时间的速度规划。本实施方式中，以三次准均匀B样条曲线为规划曲线，使规划曲线更简单，速度、加速度更加平滑，规划过程更容易实现。

另外，规划曲线为六个控制点的三次准均匀B样条曲线；六个控制点的三次准均匀B样条曲线的表达式为：

其中，t为时间，s为路程，u为节点，B_i(u)为基函数，cp_i为控制点；i为控制点在规划曲线上的次序。本实施方式中，规划曲线上包括六个控制点，并且给出六个控制点的三次准均匀B样条曲线的表达式，使得规划过程中，计算量较小，规划过程更容易实现。

另外，根据第二约束条件，并以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划具体包括：根据第二约束条件与规划曲线，获取6个控制点信息；控制点信息包括控制点所对应的时间与路程；根据获取的控制点信息与规划曲线，获取各时刻所对应的位置、速度与加速度。本实施方式中，通过获取6个控制点信息，并根据获取的控制点信息与规划曲线，可以获取各时刻所对应的位置、速度与加速度，进而，可以获取多轴的位置、速度等信息，有利于多轴同步过程的实现。

附图说明

图1是根据本发明第一实施方式中的给定时间速度规划示意图；

图2是根据本发明第一实施方式的速度规划方法的流程图；

图3是根据本发明第二实施方式的速度规划方法的流程图；

图4是根据本发明第二实施方式的速度规划方法的流程图；

图5是根据本发明第三实施方式的起始点的加速度大于零时，可行域及最优解的示意图；

图6是根据本发明第三实施方式的起始点的加速度小于或者等于零时，可行域及最优解的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实施方式进行详细的阐述。然而，本领域的普通技术人员可以理解，在本发明各实施方式中，为了使读者更好地理解本申请而提出了许多技术细节。但是，即使没有这些技术细节和基于以下各实施方式的种种变化和修改，也可以实现本申请所要求保护的技术方案。

本发明实施方式中，时间t与位移s的规划曲线采用样条曲线，如图1所示，根据位移随时间的变化曲线101，可以得出包括控制点103的控制多边形102，图1中以六个控制点103为例。具体地说，在进行速度规划时，首先需要给出始末点的位置、速度、加速度及时刻，然后根据约束条件，求出控制点，进而，得到相应的时间与位移。

本发明的第一实施方式涉及一种速度规划方法，如图2所示，包括：

步骤201：根据第一约束条件，对多轴机构的所有轴进行S型速度规划，其中，第一约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、运行速度、最大运行速度、最大加速度以及最大加加速度。

步骤202：从规划结果中，选取时间最长的轴作为基准轴。具体地说，从规划结果中，选取出运行所用时间最长的轴，并将该轴作为基准轴。

步骤203：根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划，其中，第二约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、最大运行速度、最大加速度、最大加加速度以及基准轴的总运行时间。

具体地说，将基准轴的总运行时间作为约束时间，并以运动的路程、起点速度、末点速度、起点加速度、末点加速度、最大运行速度、最大加速度、最大加加速度以及基准轴的总运行时间作为第二约束条件，对其余的轴重新进行给定时间的速度规划。

本发明实施方式相对于现有技术而言，根据第二约束条件，以三次B样条曲线为规划曲线，对多轴机构的基准轴以外的其余的轴进行给定时间的速度规划，其中，第二约束条件可以包括运动的路程、起点速度等，这样，各轴不同段之间的转折点的速度以及加速度不需要降为零。这样，规划过程中的计算量较小，且实时性较高。

本发明的第二实施方式涉及一种速度规划方法，本实施方式在第一实施方式的基础上做了进一步细化，给出了一种更为具体的根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划的方法，如图3所示，包括：

步骤301：根据第一约束条件，对多轴机构的所有轴进行S型速度规划，其中，第一约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、运行速度、最大运行速度、最大加速度以及最大加加速度。

步骤302：从规划结果中，选取时间最长的轴作为基准轴。

具体地说，从规划结果中，选取出运行所用时间最长的轴，并将该轴作为基准轴。

步骤303：根据第二约束条件，以三次准均匀B样条曲线为规划曲线，对基准轴以外的其余的轴进行给定时间的速度规划。其中，规划曲线可以为六个控制点的三次准均匀B样条曲线。六个控制点的三次准均匀B样条曲线的表达式可以为：

其中，t为时间，s为路程，u为节点，B_i(u)为基函数，cp_i为控制点；i为控制点在所述规划曲线上的次序。

更具体地说，步骤303中还可以包括以下子步骤，具体如图4所示。

步骤401：根据节点的个数、控制点个数以及曲线次数之间的关系，获取规划曲线的节点矢量。

具体地说，根据节点u的个数与控制点个数、曲线次数之间的关系，可以得到规划曲线的节点矢量为：

U＝[0 0 0 0 1/3 2/3 1 1 1 1]

步骤402：根据节点矢量，获取节点分别为0与1时，基函数与基函数的导数值。其中，基函数的导数值包括：基函数的一阶导数值与基函数的二阶导数值。

具体地说，节点u为0与1时，基函数、基函数的一阶导数值以及基函数的二阶导数值如下表所示：

预设起点时刻、起点位置均为0，起点速度为v_s，起点加速度为a_s，末点时刻为t，末点位置为s，末点速度为v_e，末点加速度为0。对于起点，在节点u为0时，基函数值为(0,0)，基函数的一阶导数值为基函数的二阶导数值为

步骤403：根据约束条件、基函数及基函数的导数值，获取6个控制点信息。其中，控制点信息包括控制点所对应的时间与路程。

具体地说，根据约束条件、基函数、基函数的一阶导数与基函数的二阶导数，可以计算得出前三个控制点依次为：第一个控制点为(0,0)，第二个控制点为第三个控制点为

由于末点加速度为0，根据规划曲线中路程随着时间的增加而增加的规律，后三个控制点可以按照线性方法直接给定。后三个控制点可以依次为：第四个控制点为(s-k₁*v_e,t-k₁)，第五个控制点为(s-k₂*v_e,t-k₂)，其中，k₁、k₂为常数，k₁>k₂>0，第六个控制点为(s,t)。

步骤404：根据获取的控制点信息与规划曲线，获取各时刻所对应的位置、速度与加速度。

具体地说，根据获取的控制点信息与规划曲线，可以获取规划所得样条曲线，并且可以获取各时刻所对应的节点，进一步，将获取的节点带入规划所得样条曲线中，可以获得各时刻所对应的位置、速度与加速度。

更具体地说，由反函数与原函数之间的关系可得：

其中，与可直接由样条曲线得到，因此，根据二阶泰勒展式，可以得到节点u：

其中，u为当前时刻所对应的节点，u₀为当前时刻前一时刻的节点，Δt为这一时刻与上一时刻的时间差。进一步，可以将计算得出的节点u代入规划曲线，这样，就可以得出当前时刻所对应的位置，并且根据可以计算得出当前时刻所对应的速度，根据可以计算得出当前时刻所对应的加速度。

此外，当末点加速度不为0时，可以类似前三个控制点的求解方法，根据第二约束条件与规划曲线，求解得出第四个控制点、第五个控制点与第六个控制点。

此外，当起点加速度a_s为0时，可以根据规划曲线中路程随着时间的增加而增加的规律，按照线性方法，给定第一个控制点、第二个控制点与第三个控制点。

本发明实施方式中，以三次准均匀B样条曲线为规划曲线，使规划曲线更简单。规划曲线上包括六个控制点，并且给出六个控制点的三次准均匀B样条曲线的表达式，使得规划过程中，计算量较小，规划过程更容易实现。本实施方式通过获取6个控制点信息与规划曲线，获取各时刻所对应的位置、速度与加速度，从而，可以获取多轴的位置、速度等信息。这样，有利于多轴同步过程的实现。

本发明的第三实施方式涉及一种速度规划方法，第三实施方式在第二实施方式的基础上作了进一步的改进，改进之处主要在于：本发明第三实施方式，对控制点进行了进一步的优化，如图5及图6所示。

第二实施方式中获得的六个控制点依次为：第一个控制点为(0,0)，第二个控制点为第三个控制点为第四个控制点为(s-k₁*v_e,t-k₁)，第五个控制点为(s-k₂*v_e,t-k₂)，其中，k1、k2为常数，k1>k2>0，第六个控制点为(s,t)。由于上述控制点的表达式中与不唯一确定，因此，六个控制点的值同样不唯一确定。本实施方式中，根据规划曲线中路程随着时间的增加而增加的规律，对上述控制点作了进一步的优化。

根据第一个控制点与第二个控制点，得出第一不等式；其中，第一不等式为根据第二个控制点与第三个控制点，得出第二不等式与第三不等式；其中，第二不等式为第三不等式为根据第三个控制点与第四个控制点，得出第四不等式与第五不等式；其中，第四不等式为第五不等式为其中，第四个控制点为(cp4_t,cp4_s)，参见图5及图6，图5及图6中的编号①至编号⑤分别表示与第一不等式至第五不等式对应的可行域边界。

当起点加速度a_s大于0时，与的可行域如图5所示。

第一可行域的中心点的横坐标可以作为的第一最优解，第一可行域的中心点的纵坐标可以作为的第一最优解。其中，第一可行域为满足第一不等式、第二不等式与第五不等式的可行域。

第二可行域的中心点的横坐标可以作为的第一最优解，第二可行域的中心点的纵坐标可以作为的第一最优解。其中，第二可行域为满足第一不等式、第二不等式、第四不等式与第五不等式的可行域。

第三可行域的中心点的横坐标可以作为的第一最优解，第三可行域的中心点的纵坐标可以作为的第一最优解。其中，第三可行域为满足第一不等式、第二不等式与第四不等式的可行域。

当起点加速度a_s小于等于0时，与的可行域如图6所示。

第四可行域的中心点的横坐标可以作为的第一最优解，第四可行域的中心点的纵坐标可以作为的第一最优解。其中，第四可行域为满足第一不等式、第三不等式与第四不等式的可行域。

第五可行域的中心点的横坐标可以作为的第一最优解，第五可行域的中心点的纵坐标可以作为的第一最优解。其中，第五可行域为满足第一不等式、第三不等式、第四不等式与第五不等式的可行域。

第六可行域的中心点的横坐标可以作为的第一最优解，第六可行域的中心点的纵坐标可以作为的第一最优解。其中，第六可行域为满足第一不等式、第三不等式与第五不等式的可行域。

此外，上述最优解也可以采用其他优化方法对其他指标进行优化得到。

本发明实施方式中，根据规划曲线中路程随着时间的增加而增加的规律，获得多个不等式，进而，获得控制点需要满足的可行域，并选用可行域的中心点作为最优解。这样，得到的控制点唯一确定，且各个控制点相对分散，同时，得到的运行轨迹更加平滑。

此外，本发明实施方式中，还可以根据第二约束条件，并以三次非均匀B样条曲线为规划曲线，对多轴机构的基准轴以外的其余的轴进行给定时间的速度规划。

上面各种方法的步骤划分，只是为了描述清楚，实现时可以合并为一个步骤或者对某些步骤进行拆分，分解为多个步骤，只要包含相同的逻辑关系，都在本专利的保护范围内；对算法中或者流程中添加无关紧要的修改或者引入无关紧要的设计，但不改变其算法和流程的核心设计都在该专利的保护范围内。

本领域的普通技术人员可以理解，上述各实施方式是实现本发明的具体实施例，而在实际应用中，可以在形式上和细节上对其作各种改变，而不偏离本发明的精神和范围。

Claims (10)

1.一种速度规划方法，其特征在于，包括：

根据第一约束条件，对多轴机构的所有轴进行S型速度规划；其中，所述第一约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、运行速度、最大运行速度、最大加速度以及最大加加速度；

从规划结果中，选取时间最长的轴作为基准轴；

根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划；其中，所述第二约束条件包括运动的路程、起点速度、末点速度、起点加速度、末点加速度、最大运行速度、最大加速度、最大加加速度以及所述基准轴的总运行时间。

2.根据权利要求1所述的速度规划方法，其特征在于，所述根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划具体包括：

根据所述第二约束条件，并以三次准均匀B样条曲线为规划曲线，对所述基准轴以外的其余的轴进行给定时间的速度规划。

3.根据权利要求2所述的速度规划方法，其特征在于，所述规划曲线为六个控制点的三次准均匀B样条曲线；

所述六个控制点的三次准均匀B样条曲线的表达式为：

其中，t为时间，s为路程，u为节点，B_i(u)为基函数，cp_i为控制点；i为控制点在所述规划曲线上的次序。

4.根据权利要求3所述的速度规划方法，其特征在于，所述根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划具体包括：

根据所述第二约束条件与所述规划曲线，获取6个控制点信息；所述控制点信息包括控制点所对应的时间与路程；

根据获取的所述控制点信息与所述规划曲线，获取各时刻所对应的位置、速度与加速度。

5.根据权利要求4所述的速度规划方法，其特征在于，根据所述第二约束条件与所述规划曲线，获取6个控制点信息具体包括：

根据节点的个数、控制点个数以及曲线次数之间的关系，获取所述规划曲线的节点矢量；

根据所述节点矢量，获取节点分别为0与1时，基函数、所述基函数的一阶导数值、所述基函数的二阶导数值；

若起点时刻、起点位置均为0，起点速度为v_s，起点加速度为a_s，末点时刻为t，末点位置为s，末点速度为v_e，末点加速度为0，则得出起点的节点为0，基函数值为(0,0)，所述基函数的一阶导数值为所述基函数的二阶导数值为

计算得出第一个控制点为(0,0)，第二个控制点为第三个控制点为

根据所述规划曲线中路程随着时间的增加而增加的规律，按照线性方法给定第四个控制点为(s-k₁*v_e,t-k₁)，第五个控制点为(s-k₂*v_e,t-k₂)，其中，k₁、k₂为常数，k₁>k₂>0，第六个控制点为(s,t)。

6.根据权利要求5所述的速度规划方法，其特征在于，所述获取6个控制点信息具体包括：

根据所述第一个控制点与所述第二个控制点，得出第一不等式；其中，所述第一不等式为

根据所述第二个控制点与所述第三个控制点，得出第二不等式与第三不等式；其中，所述第二不等式为所述第三不等式为

根据所述第三个控制点与所述第四个控制点，得出第四不等式与第五不等式；其中，所述第四不等式为所述第五不等式为其中，所述第四个控制点为(cp4_t,cp4_s)；

根据所述第一不等式、所述第二不等式、所述第三不等式、所述第四不等式、所述第五不等式的可行域的中心点，确定所述与所述的最优解。

7.根据权利要求5所述的速度规划方法，其特征在于，所述根据所述第二约束条件与所述规划曲线，获取6个控制点信息具体包括：

根据所述第二约束条件与所述规划曲线，求解得出所述第四个控制点、所述第五个控制点与所述第六个控制点。

8.根据权利要求5所述的速度规划方法，其特征在于，所述根据所述第二约束条件与所述规划曲线，获取6个控制点信息具体包括：

若所述a_s为0，根据所述规划曲线中路程随着时间的增加而增加的规律，按照线性方法给定所述第一个控制点、所述第二个控制点与所述第三个控制点。

9.根据权利要求5所述的速度规划方法，其特征在于，还包括：

根据当前时刻计算对应的节点；其中，u为所述当前时刻所对应的节点，u₀为所述当前时刻前一时刻的节点，所述Δt为所述当前时刻与前一时刻的时间差，所述与所述由所述规划曲线得出；

将计算得出的节点代入所述规划曲线，得出所述当前时刻对应的位置，并根据计算得出所述当前时刻所对应的速度，根据计算得出所述当前时刻所对应的加速度。

10.根据权利要求1所述的速度规划方法，其特征在于，所述根据第二约束条件，以三次B样条曲线为规划曲线，对其余的轴重新进行给定时间的速度规划具体包括：

根据所述第二约束条件，并以三次非均匀B样条曲线为规划曲线，对所述多轴机构的基准轴以外的其余的轴进行给定时间的速度规划。