一种速度规划方法与流程

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

背景技术：

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

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

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

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

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

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

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

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

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

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

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

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

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

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