机械臂运动的速度规划方法及装置与流程

本发明涉及机器人领域，具体而言，涉及一种机械臂运动的速度规划方法及装置。

背景技术：

相关技术中的机械臂运动轨迹规划算法有三次多项式轨迹规划、五次多项式轨迹规划、直线加抛物线组合轨迹规划等，其中连续插补时三次多项式规划方法存在速度难以平滑连续、加速度骤变等问题，直线加抛物线组合轨迹规划存在加速度骤变的问题，导致运动不平稳。五次多项式规划存在无法对其最大速度进行限制的问题。由于实际运用中关节电机存在最大速度的限制或是机械臂操作者需要设定运动最大速度值，则五次多项式规划就不再适用，此时一般会用到梯形速度规划或者s型速度规划，但是梯形速度规划出来的加速度存在骤变，且速度不平滑，s型速度规划存在计算量大的问题。

针对上述的问题，目前尚未提出有效的解决方案。

技术实现要素：

本发明实施例提供了一种机械臂运动的速度规划方法及装置，以至少解决相关技术中机械臂的速度规划方法，无法限制速度，存在加速度骤变，速度曲线不平滑连续，导致机械臂运动不平稳的技术问题。

根据本发明实施例的一个方面，提供了一种机械臂运动的速度规划方法，包括：获取机械臂的初始速度规划曲线；判断所述初始速度规划曲线是否存在大于速度阈值的超速区域；在所述超速区域存在的情况下，获取所述初始速度规划曲线中大于速度容忍值的待规划速度曲线，所述待规划速度曲线位于超速区域处，所述速度容忍值小于所述速度阈值；对所述待规划速度曲线进行重新规划得到更新速度曲线，所述更新速度曲线和所述待规划速度曲线覆盖区域的位移量相等，所述更新速度曲线和所述待规划速度曲线的起始点和终止点的速度与加速度相同，所述更新速度曲线的速度极大值不大于所述速度阈值；将所述更新速度曲线替换所述待规划速度曲线得到最终速度规划曲线。

可选的，所述更新速度曲线包括呈直线状的第一子更新曲线，以及对称设置在第一子更新曲线两端且呈抛物线形状的第二子更新曲线和第三子更新曲线。

可选的，对所述待规划速度曲线进行重新规划得到更新速度曲线包括：根据所述第二子更新曲线的起始点的速度和加速度分别与所述待规划速度曲线的起始点的速度和加速度相同，所述第二子更新曲线的终止点的速度为所述速度阈值，以及所述第二子更新曲线的终止点的加速度为零，规划出第二子更新曲线；根据所述第三子更新曲线的终止点的速度和加速度分别与所述待规划速度曲线的终止点的速度和加速度相同，所述第三子更新曲线的起始点的速度为所述速度阈值，以及所述第三子更新曲线的起始点的加速度为零，规划出所述第三子更新曲线；根据所述第一子更新曲线的速度等于所述速度阈值，以及所述更新曲线和所述待规划速度曲线覆盖区域的位移量相等，规划出第一子更新曲线。

可选的，将所述更新速度曲线替换待规划速度曲线得到最终速度规划曲线包括：计算所述更新速度曲线和所述待规划速度曲线的时间差；根据所述时间差将第二初始速度规划曲线沿时间方向整体向右平移，第一初始速度规划曲线和所述第二初始速度规划曲线分别为所述初始速度规划曲线中位于所述待规划速度曲线左侧和右侧的速度规划曲线；连接所述更新速度曲线的起始点和所述第一初始速度规划曲线的终止点，以及连接所述更新速度曲线的终止点和向右平移后的第二初始速度规划曲线的起始点，得到所述最终速度规划曲线。

可选的，所述速度阈值与所述速度容忍值的差为0.1至0.4倍速度阈值。

可选的，所述速度曲线为所述机械臂的一个轴的速度曲线，所述机械臂的轴的数量为多个；将所述更新速度曲线替换所述待规划速度曲线得到最终速度规划曲线之后，还包括：分别对所述机械臂的多个轴的速度曲线进行速度规划。

可选的，获取机械臂的初始速度规划曲线包括：获取所述机械臂的输入参数，其中，所述输入参数包括：所述机械臂运动起始点的速度和加速度，所述机械臂运动终止点的速度和加速度，所述运动起始点和所述运动终止点的位姿，从所述运动起始点到所述运动终止点的时间，所述速度阈值；通过五次多项式根据所述输入参数，确定所述机械臂的初始速度规划曲线。

根据本发明实施例的另一方面，还提供了一种机械臂运动的速度规划装置，包括：第一获取模块，用于获取机械臂的初始速度规划曲线；判断模块，用于判断所述初始速度规划曲线是否存在大于速度阈值的超速区域；第二获取模块，用于在所述超速区域存在的情况下，获取所述初始速度规划曲线中大于速度容忍值的待规划速度曲线，所述待规划速度曲线位于超速区域处，所述速度容忍值小于所述速度阈值；规划模块，用于对所述待规划速度曲线进行重新规划得到更新速度曲线，所述更新速度曲线和所述待规划速度曲线覆盖区域的位移量相等，所述更新速度曲线和所述待规划速度曲线的起始点和终止点的速度与加速度相同，所述更新速度曲线的速度极大值不大于所述速度阈值；替换模块，用于将所述更新速度曲线替换所述待规划速度曲线得到最终速度规划曲线。

可选的，所述更新速度曲线包括呈直线状的第一子更新曲线，以及对称设置在第一子更新曲线两端且呈抛物线形状的第二子更新曲线和第三子更新曲线。

可选的，所述规划模块包括：第一规划单元，用于根据所述第二子更新曲线的起始点的速度和加速度分别与所述待规划速度曲线的起始点的速度和加速度相同，所述第二子更新曲线的终止点的速度为所述速度阈值，以及所述第二子更新曲线的终止点的加速度为零，规划出第二子更新曲线；第二规划单元，用于根据所述第三子更新曲线的终止点的速度和加速度分别与所述待规划速度曲线的终止点的速度和加速度相同，所述第三子更新曲线的起始点的速度为所述速度阈值，以及所述第三子更新曲线的起始点的加速度为零，规划出所述第三子更新曲线；第三规划单元，用于根据所述第一子更新曲线的速度等于所述速度阈值，以及所述更新曲线和所述待规划速度曲线覆盖区域的位移量相等，规划出第一子更新曲线。

可选的，所述替换模块包括：计算单元，用于计算所述更新速度曲线和所述待规划速度曲线的时间差；平移单元，用于根据所述时间差将第二初始速度规划曲线沿时间方向整体向右平移，第一初始速度规划曲线和所述第二初始速度规划曲线分别为所述初始速度规划曲线中位于所述待规划速度曲线左侧和右侧的速度规划曲线；连接单元，用于连接所述更新速度曲线的起始点和所述第一初始速度规划曲线的终止点，以及连接所述更新速度曲线的终止点和向右平移后的第二初始速度规划曲线的起始点，得到所述最终速度规划曲线。

根据本发明实施例的另一方面，还提供了一种计算机存储介质，所述计算机存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机存储介质所在设备执行上述中任意一项所述的机械臂运动的速度规划方法。

根据本发明实施例的另一方面，还提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述中任意一项所述的机械臂运动的速度规划方法。

在本发明实施例中，采用获取机械臂的初始速度规划曲线；判断初始速度规划曲线是否存在大于速度阈值的超速区域；在超速区域存在的情况下，获取初始速度规划曲线中大于速度容忍值的待规划速度曲线，待规划速度曲线位于超速区域处，速度容忍值小于速度阈值；对待规划速度曲线进行重新规划得到更新速度曲线，更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值；将更新速度曲线替换待规划速度曲线得到最终速度规划曲线的方式，通过将机械臂在预设运动轨迹的速度曲线中，超出预设速度阈值的部分进行删除，并重新进行规划，达到了使重新规划后的曲线，在预设速度阈值之内的目的，从而实现了保证速度曲线在预设速度曲线范围内，且速度曲线平滑过渡，提高机械臂运动稳定性的技术效果，进而解决了相关技术中机械臂的速度规划方法，无法限制速度，存在加速度骤变，速度曲线不平滑连续，导致机械臂运动不平稳的技术问题。

附图说明

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

图1是根据本发明实施例的一种机械臂运动的速度规划方法的流程图；

图2是根据本发明实施方式的初始速度规划曲线与速度限制的示意图；

图3是根据本发明实施方式的初始速度规划曲线与最终速度规划曲线的示意图；

图4是根据本发明实施方式的速度曲线重新规划的流程图；

图5是根据本发明实施方式的初始速度规划曲线与速度限制和速度容忍值的示意图；

图6是根据本发明实施方式的最终速度规划曲线的示意图；

图7是根据本发明实施方式的另一应用实例的最终速度规划曲线的示意图；

图8是根据本发明实施例的一种机械臂运动的速度规划装置的示意图。

具体实施方式

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

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

根据本发明实施例，提供了一种机械臂运动的速度规划方法的方法实施例，需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

图1是根据本发明实施例的一种机械臂运动的速度规划方法的流程图，如图1所示，根据本发明实施例的另一方面，还提供了一种信号发送方法，该方法包括以下步骤：

步骤s102，获取机械臂的初始速度规划曲线；

步骤s104，判断初始速度规划曲线是否存在大于速度阈值的超速区域；

步骤s106，在超速区域存在的情况下，获取初始速度规划曲线中大于速度容忍值的待规划速度曲线，待规划速度曲线位于超速区域处，速度容忍值小于速度阈值；

步骤s108，对待规划速度曲线进行重新规划得到更新速度曲线，更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值；

步骤s110，将更新速度曲线替换待规划速度曲线得到最终速度规划曲线。

通过上述步骤，采用获取机械臂的初始速度规划曲线；判断初始速度规划曲线是否存在大于速度阈值的超速区域；在超速区域存在的情况下，获取初始速度规划曲线中大于速度容忍值的待规划速度曲线，待规划速度曲线位于超速区域处，速度容忍值小于速度阈值；对待规划速度曲线进行重新规划得到更新速度曲线，更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值；将更新速度曲线替换待规划速度曲线得到最终速度规划曲线的方式，通过将机械臂在预设运动轨迹的速度曲线中，超出预设速度阈值的部分进行删除，并重新进行规划，达到了使重新规划后的曲线，在预设速度阈值之内的目的，从而实现了保证速度曲线在预设速度曲线范围内，且速度曲线平滑过渡，提高机械臂运动稳定性的技术效果，进而解决了相关技术中机械臂的速度规划方法，无法限制速度，存在加速度骤变，速度曲线不平滑连续，导致机械臂运动不平稳的技术问题。

上述机械臂的初始速度规划曲线可以为上述机械臂根据预设的轨迹点，确定运动轨迹需要经过的轨迹点。根据轨迹点的位姿、到达五个位姿点的时间、起始点速度和加速度、终止点速度和加速度，通过五次多项式，确定上述机械臂经过上述轨迹点的六轴方向的速度曲线，也即是机械臂的初始速度规划曲线。

上述在超速区域存在的情况下，获取初始速度规划曲线中大于速度容忍值的待规划速度曲线，待规划速度曲线位于超速区域处，速度容忍值小于速度阈值。由于速度阈值为速度曲线不可超过的阈值，若以超速区域直接规划，会导致超速区域前段的速度曲线与规划的速度曲线产生拐点，导致速度突变，设置一个速度容忍值，可以从超过速度容忍值的初始规划速度曲线进行规划，使得可以在速度容忍值与速度阈值之间的初始规划速度曲线进行平滑过渡，从而防止速度突变，使得速度规划更合理更科学。

上述对轴的初始速度规划曲线进行确定是在关节空间中进行确定，上述方法还可以对机械臂末端的笛卡尔空间速度曲线进行确定，将上述在关节空间中的轴的初始速度规划曲线替换为上述机械臂末端的tcp(工具中心点，toolcenterpoint)在笛卡尔空间中的速度曲线，同理可以对机械臂末端的笛卡尔空间速度曲线进行重新规划。

上述对待规划速度曲线进行重新规划得到更新速度曲线，更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值。可以根据更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值的约束条件建立更新速度曲线的多元方程，对多元方程求解，就可以得到更新速度曲线的表达式和时间范围，进而确定更新速度曲线。

上述更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，包括了更新速度曲线的起始点的速度和待规划速度曲线的起始点的速度相同，更新速度曲线的终止点的速度和待规划速度曲线的终止点的速度相同，更新速度曲线的起始点的速度和待规划速度曲线的起始点的加速度相同，更新速度曲线的终止点的速度和待规划速度曲线的终止点的加速度相同。

确定初始速度规划曲线是否大于速度阈值的超速区域，其中，速度阈值为预设的最大速度的限制值，上述机械臂的电机为规划的轴对应的电机，在多轴机械臂中，多个轴可能对应多个电机。通过上述速度阈值对超速区域的速度曲线进行重新规划，将机械臂在初始速度规划曲线中，超出速度阈值的部分进行删除，并重新进行规划，达到了使重新规划后的最终速度规划曲线，在预设速度阈值之内的目的，从而实现了保证速度曲线在预设速度曲线范围内，且速度曲线平滑过渡，提高机械臂运动稳定性的技术效果，进而解决了相关技术中机械臂的速度规划方法，无法限制速度，存在加速度骤变，速度曲线不平滑连续，导致机械臂运动不平稳的技术问题。

可选的，更新速度曲线包括呈直线状的第一子更新曲线，以及对称设置在第一子更新曲线两端且呈抛物线形状的第二子更新曲线和第三子更新曲线。

上述更新曲线可以为多种预设的形状，例如，单抛物线，直线等，本实施例中，使用抛物线+直线+抛物线进行拟合衔接，也即是，更新曲线两端的呈抛物线形状的第二子更新曲线和第三子更新曲线，两段抛物线中间的呈直线状的第一子更新曲线，第二子更新曲线和第三子更新曲线对应的时长相等，第一子更新曲线为速度等于速度阈值。第二子更新曲线的起始点的速度和第三子更新曲线中重点的速度等于上述速度容忍值。从而保持速度曲线的平滑过渡，并保证重新规划更新后的速度曲线处于预设速度阈值内。

可选的，对待规划速度曲线进行重新规划得到更新速度曲线包括：根据第二子更新曲线的起始点的速度和加速度分别与待规划速度曲线的起始点的速度和加速度相同，第二子更新曲线的终止点的速度为速度阈值，以及第二子更新曲线的终止点的加速度为零，规划出第二子更新曲线；根据第三子更新曲线的终止点的速度和加速度分别与待规划速度曲线的终止点的速度和加速度相同，第三子更新曲线的起始点的速度为速度阈值，以及第三子更新曲线的起始点的加速度为零，规划出第三子更新曲线；根据第一子更新曲线的速度等于速度阈值，以及更新曲线和待规划速度曲线覆盖区域的位移量相等，规划出第一子更新曲线。

根据第二子更新曲线的起始点的速度和加速度分别与待规划速度曲线的起始点的速度和加速度相同，第二子更新曲线的终止点的速度为速度阈值，以及第二子更新曲线的终止点的加速度为零，规划出第二子更新曲线可以包括：

根据第二子更新曲线的起始点和终止点的速度特性，确定第二子更新曲线的表达式，以及第二子更新曲线分别对应的时长。具体的，根据第二子更新曲线的起始点和终止点的速度特性，确定第二子更新曲线的四个边界条件；根据四个边界条件，建立四元方程组；对四元方程组进行求解，确定第二子更新曲线的表达式的三个系数，以及第二子更新曲线分别对应的时长。

具体的第二子更新曲线的四个边界条件可以为：

第二子更新曲线的起始点的速度值与待规划速度曲线的起始点的速度值相等；其中，待规划速度曲线的起始点的速度，等于超速区域前段的初始规划速度曲线的终止点的速度，具体等于速度容忍值；

第二子更新曲线的起始点的加速度值与待规划速度曲线的起始点的加速度值相等；其中，待规划速度曲线的起始点的加速度，等于超速区域前段的初始规划速度曲线的终止点的加速度，具体可以根据初始规划速度曲线确定；

第二子更新曲线的终止点的速度值与第一子更新曲线的速度相等；具体等于速度阈值；

第二子更新曲线的终止点的速度值对时间的导数为零。

例如，建立新的坐标系对第二子更新曲线进行独立规划：以vstartpoint在t轴的投影点为原点，新坐标系t轴与原坐标系t轴方向一致，新坐标系v轴垂直于t轴，方向为从投影点指向vstartpoint。

在新坐标系中，设第二子更新曲线的表达式为：

v1＝a1*t²+b1*t+c1

由上述边界条件可得到四个方程：

v′startpoint＝2*a1*t0+b1

v′end1＝2*a1*t1f+b1

其中vstartpoint及其对应的时间可由初始速度规划曲线表达式得到，vend1为第二子更新曲线的终止点的速度，vend1＝maxvel，v′startpoint可由初始速度规划曲线表达式再对时间求导得到，v′end1＝0，t0＝0。联立求解得出第二子更新曲线的表达式及其对应的时长。

根据第三子更新曲线的终止点的速度和加速度分别与待规划速度曲线的终止点的速度和加速度相同，第三子更新曲线的起始点的速度为速度阈值，以及第三子更新曲线的起始点的加速度为零，规划出第三子更新曲线可以包括：

根据第三子更新曲线的起始点和终止点的速度特性，确定第三子更新曲线的表达式，以及第三子更新曲线分别对应的时长。具体的，根据第三子更新曲线的起始点和终止点的速度特性，确定第三子更新曲线的四个边界条件；根据四个边界条件，建立四元方程组；对四元方程组进行求解，确定第三子更新曲线的表达式的三个系数，以及第三子更新曲线分别对应的时长。

具体的第三子更新曲线的四个边界条件可以为：

第三子更新曲线的终止点的速度值与待规划速度曲线的终止点的速度值相等；其中，待规划速度曲线的终止点的速度，等于超速区域后段的初始规划速度曲线的起始点的速度，具体等于速度容忍值；

第三子更新曲线的终止点的加速度值与待规划速度曲线的终止点的加速度值相等；其中，待规划速度曲线的终止点的加速度，等于超速区域后段的初始规划速度曲线的起始点的加速度，具体可以根据初始规划速度曲线确定；

第三子更新曲线的起始点的速度值与第一子更新曲线的速度相等；具体等于速度阈值；

第三子更新曲线的起始点的速度值对时间的导数为零。

例如，建立新的坐标系对第三子更新曲线进行独立规划：以第三子更新曲线最左侧起始点在t轴的投影点为原点，新坐标系t轴与原坐标系t轴方向一致，新坐标系v轴垂直于t轴，方向为从投影点指向第三子更新曲线最左侧起始点。

在新坐标系中，设第三子更新曲线的表达式为：

v2＝a2*t²+b2*t+c2

由上述边界条件可得到四个方程：

v′start2＝2*a2*t0+b1

v′endpoint＝2*a2*t2f+b2

其中vendpoint及其对应的时间可由初始速度规划曲线表达式得到，vstart2为第三子更新曲线的起始点速度，vstart2＝maxvel，v′endpoint可由初始速度规划曲线表达式再对时间求导得到，v′start2＝0，t0＝0。联立求解得出第三子更新曲线的表达式及其对应的时长。

根据第一子更新曲线的速度等于速度阈值，以及更新曲线和待规划速度曲线覆盖区域的位移量相等，规划出第一子更新曲线可以包括：

确定待规划速度曲线覆盖区域的第一位移量，根据第一位移量，第二子更新曲线和第三子更新曲线分别对应的第二位移量和第三位移量，确定第一子更新曲线的位移量，其中，第二子更新曲线和第三子更新曲线分别对应的第二位移量和第三位移量为第二子更新曲线和第三子更新曲线的表达式在时间上求积分得到的。根据第一子更新曲线的位移量，且已知第一子更新曲线为速度等于速度阈值的直线，确定第一自更新曲线的时长。

确定待规划速度曲线覆盖区域的第一位移量可以通过下式确定：

endpoint和startpoint分别为待规划速度曲线的终止点和起始点。

第一子更新曲线的表达式即为y＝maxvel。对应的时长t3f由以下限定条件解得：

x1＝x2

式中，x2为更新曲线的位移量，v1为第二子更新曲线的表达式，end1为第二子更新曲线的终止点，v2为第三子更新曲线的表达式，start2为第三子更新曲线的起始点，endpoint和startpoint分别为待规划速度曲线的终止点和起始点，也即是第三子更新曲线的终止点和第二子更新曲线的起始点。

需要说明的是，重新规划的更新速度曲线由于位移量不变，但是曲线形状发生变化，因此，其对应的时间与原待规划速度曲线对应的时间不一致，通常更新速度曲线对应的时间大于待规划速度曲线对应的时间。就需要将待规划速度曲线之后的初始规划曲线进行平移，使得与更新速度曲线平滑连接。

上述待规划速度曲线的第一位移量可以通过该超速区域的待规划速度曲线求积分确定，该待规划速度曲线对应的位移，在实际的机械臂运动轨迹中不变，通过位移不变，改变速度曲线，对速度曲线进行重新规划，使得重新规划后的曲线，在预设速度阈值之内的目的，从而实现了保证速度曲线在预设速度曲线范围内，且速度曲线平滑过渡，提高机械臂运动稳定性的技术效果，进而解决了相关技术中机械臂的速度规划方法，无法限制速度，存在加速度骤变，速度曲线不平滑连续，导致机械臂运动不平稳的技术问题。

可选的，将更新速度曲线替换待规划速度曲线得到最终速度规划曲线包括：计算更新速度曲线和待规划速度曲线的时间差；根据时间差将第二初始速度规划曲线沿时间方向整体向右平移，第一初始速度规划曲线和第二初始速度规划曲线分别为初始速度规划曲线中位于待规划速度曲线左侧和右侧的速度规划曲线；连接更新速度曲线的起始点和第一初始速度规划曲线的终止点，以及连接更新速度曲线的终止点和向右平移后的第二初始速度规划曲线的起始点，得到最终速度规划曲线。

待规划速度曲线的左侧不需要重新规划的速度曲线不变，为了保证重新规划前后位移量不变，规划后更新速度曲线以后的初始速度规划曲线形状在规划前后保持不变，在时间上整体后延，最终能够准确地到达既定目标点。

可选的，速度阈值与速度容忍值的差为0.1至0.4倍速度阈值。

可以有效在速度容忍值和速度阈值之间保留足够进行速度缓冲进行速度平滑过渡的时间。

可选的，速度曲线为机械臂的一个轴的速度曲线，机械臂的轴的数量为多个；将更新速度曲线替换待规划速度曲线得到最终速度规划曲线之后，还包括：分别对机械臂的多个轴的速度曲线进行速度规划。

初始速度规划曲线为机械臂的一个轴的速度曲线，机械臂的轴的速度方向包括顺时针方向，逆时针方向；在机械臂为多轴机械臂的情况下，例如，3轴机械臂，4轴机械臂，5轴机械臂，6轴机械臂，在对一个轴的速度曲线确定之后，对其他轴的速度曲线进行确定，直至机械臂的所有轴的速度曲线完成确定。

可选的，获取机械臂的初始速度规划曲线包括：获取机械臂的输入参数，其中，输入参数包括：机械臂运动起始点的速度和加速度，机械臂运动终止点的速度和加速度，运动起始点和运动终止点的位姿，从运动起始点到运动终止点的时间，速度阈值；通过五次多项式根据输入参数，确定机械臂的初始速度规划曲线。

确定机械臂的初始速度规划曲线可以包括：获取机械臂的运动轨迹需要经过的预设的轨迹点；根据轨迹点确定机械臂在轴方向的插补曲线；根据插补曲线对时间求导，确定速度曲线。例如，获取输入的运动轨迹的起始点速度和加速度、终止点的速度和加速度、轨迹点的位姿、到达各轨迹点的时间、最大速度限制值maxvel。运用机械臂逆运动学解获得这些轨迹点分别对应的6轴角度。对每一个轴的角度变化单独进行五次多项式规划，n+1个轨迹点(包含起始点和结束点)需要规划n条五次多项式曲线。规划中间轨迹点(不包括起始点和终止点)的速度和加速度。分别对每一段曲线进行单独规划，运用五次多项式解求出五次多项式的6个参数，进而得到第m个轴的第n段曲线方程。完成第m轴的n条曲线的单独规划后将其整合，将得到一条轨迹、速度平滑的插补曲线。将第m轴的插补曲线对时间求导，得到初始速度规划曲线。

需要说明的是，本实施例还提供了一种可选的实施方式，下面对该实施方式进行详细说明。

本实施方式将五次多项式规划算法进行改进，令其能够人为设定最大运动速度值，同时保证速度曲线的平滑过渡。

本实施方式最主要/关键的技术问题：改进了五次多项式轨迹规划无法对其进行速度限制的缺点，实现了带有速度限制的五次多项式规划算法。次要问题：利用抛物线+直线+抛物线方式拟合超出速度限制区域；利用积分运算合理设置各拟合段时长；对速度超出限制的区段重新规划的同时保证速度的连续平滑；保证重新规划前后的总“位移量”不变。

使用本实施方式进行五次多项式规划，用户/使用者可以依据需求或者电机最大速度等，在算法的输入中添加最大速度限制maxvel。本发明算法能够将速度规划时的最大速度限制在maxvel，同时保证速度曲线的平滑过渡，通过积分法合理分配过渡时间以保证进行轨迹插补时能够到达预计的轨迹点。

以某一种构型的6轴机械臂为例，使用它在笛卡尔空间进行五次多项式轨迹规划，人为设置了起始点和终止点，希望生成一条速度和加速度平滑、无突变的直线轨迹。

使用一般的五次多项式做轨迹规划，输入参数有：起始点和终止点的位姿、到达终止点的时间、起始点速度和加速度、终止点速度和加速度。可以获得平滑的tcp(末端工具中心)速度曲线，也即是初始速度规划曲线，但如果初始速度规划曲线中某一段的速度超出了速度限制maxvel(本实施方式中设为83mm/s)，不会对之进行处理而继续执行。图2是根据本发明实施方式的初始速度规划曲线与速度限制的示意图，如图2所示，一般五次多项式规划的速度曲线存在超出限制速度的部分。

本实施方式的改进五次多项式规划方法当某段的速度超出了maxvel，会将超出的部分和周边区域“切除”并重新进行规划，使用抛物线+直线+抛物线进行拟合衔接，保持速度曲线的平滑，保证最大速度为设定的maxvel。同时通过积分计算原“切除区域”的位移量x1，也即是待规划速度曲线的第一位移量，原位移量x1应当等于重新规划后的“切除区域”的位移量x2，也即是更新速度曲线的位移量，以保证机械臂能够在规划后准确到达轨迹点。图3是根据本发明实施方式的初始速度规划曲线与最终速度规划曲线的示意图，如图3所示，通过对比可见，超出速度限制的区段发生了变化，它的峰值都被限制在maxvel，在一定时间内以maxvel进行运动，同时也做到了重新规划的曲线和其他曲线的平滑连接以及保证了规划前后的位移量一致。

机械臂运动轨迹规划是建立在机械臂正逆运动学解已经获得的前提下，本步骤以某一六轴机械臂为例，使用它进行基于具有速度限制的五次多项式的轨迹规划。

图4是根据本发明实施方式的速度曲线重新规划的流程图，如图4所示，主要步骤如下：

1.获取输入：运动的起始点速度和加速度、终止点的速度和加速度、起始点和终止点的位姿、到达终止点的时间、最大速度限制值maxvel。

2.运用五次多项式进行初步速度规划。

3.遍历找出速度曲线中速度绝对值大于maxvel-tol(tol<maxvel)的区段，其中tol为容许度，使得重新规划的区段的起始点值为maxvel–tol，它决定了抛物线段的长短，该参数可以由用户设置或者算法中给定，大小建议设置在区间[0.1*maxvel，0.4*maxvel]内。

4.利用积分法求出速度值大于等于maxvel-tol的区段的曲线包络面积x1，即该区段速度曲线对应的“位移量”，然后将该区段的速度曲线“切除掉”(抛弃)并重新规划。

5.求解重新规划的第一段抛物线及运动时长。抛物线为二次多项式，其求解3个参数需要3个独立的方程，求解抛物线速度运动时长需要额外的1个独立方程。故共需四个边界条件。边界条件如下：1)第一段抛物线起始点位置与切除的最左端点衔接相等；2)第一段抛物线起始点加速度(对时间的导数)与切除的最左端点加速度相等；3)第一段抛物线终止点的值为maxvel；4)第一段抛物线在终止点对时间的导数为0。可得到四个方程联立求解得出第一段抛物线的表达式。

6.求解重新规划的第二段抛物线及运动时长。同理其求解4个参数需要4个独立的方程。根据以下条件求解：1)第二段抛物线起始点值为maxvel；2)第二段抛物线在起始点对时间的导数为0；3)第二段抛物线终止点的位置与切除的最右端点衔接相等；4)第二段抛物线在终止点的加速度与切除的最右端点加速度相等。可得到4个方程联立求解得出第二段抛物线的表达式。

7.求解直线表达式及运动时长。直线表达式即为v＝maxvel。直线速度运动时间由以下限定条件求解：重新规划前后的总位移量一致。重新规划后的直线+抛物线+直线速度曲线对应的位移量x2应当等于x1，以保证重新规划后机械臂能准确到达终止点。

8.得到具有最大速度限制的五次多项式连续轨迹规划曲线。

具体的步骤如下：

1.获取输入。

输入为起始点和终止点，本实施方式中实现从起始点到终止点的直线轨迹。起始点速度加速度为0，终止点速度加速度为0，预期的插补时间tf设为5s，最大速度限制值maxvel设为83mm/s。

2.运用五次多项式进行初步速度规划。

先求出起始点到终止点的欧氏距离len＝331.663mm。

五次多项式一般式为：

x＝a0+a1*t+a2*t²+a3*t³+a4*t⁴+a5*t⁵

需要利用输入的边界条件来求解出a0至a5共六个系数，故需要六个边界条件，即起始点和终止点的位置、速度、加速度。

转化为矩阵方程形式：quinticmatrix*coefficients＝boundaryconditon

其中：

t＝tf＝5s

则coefficients＝quinticmatrix^-1*boundaryconditon，从而可求出五次多项式六个系数。从而确定初始速度规划曲线。

3.遍历初始速度规划曲线找出速度绝对值大于速度容忍值maxvel-tol的区段。此例中maxvel-tol设置为0.1*maxvel＝8.3mm/s。此例中找出一段，图5是根据本发明实施方式的初始速度规划曲线与速度限制和速度容忍值的示意图，如图5所示，记为[startpoint，endpoint]。

4.利用积分法求出速度值大于等于maxvel-tol的区段的待规划速度曲线的包络面积x1也即是第一位移量，即该区段速度曲线对应的“位移量”，然后将该区段的速度曲线“切除掉”并重新规划。

endpoint和startpoint分别为该待规划速度曲线的终止点和起始点，接下来求取重新规划的抛物线+直线+抛物线速度段。

5.求解重新规划的第一段抛物线也即是上述第二更新曲线，及运动时长t1f。

为求取方便，建立新的坐标系对第二更新曲线进行独立规划：以vstartpoint在t轴的投影点为原点，新坐标系t轴与原坐标系t轴方向一致，新坐标系v轴垂直于t轴，方向为从投影点指向vstartpoint。

在新坐标系中，设第二更新曲线的表达式为：

v1＝a1*t²+b1*t+c1

由边界条件可得到四个方程：

v′startpoint＝2*a1*t0+b1

v′end1＝2*a1*t1f+b1

其中vstartpoint及其对应的时间可由初始速度规划曲线表达式得到，vend1为第二更新曲线的终止点速度，vend1＝maxvel，v′startpoint可由初始速度规划曲线表达式再对时间求导得到，v′end1＝0，t0＝0。联立求解得出第二更新曲线的表达式。

6.求解重新规划的第二段抛物线，也即是第三更新曲线，及运动时长t2f。

为求取方便，建立新的坐标系对第三更新曲线进行独立规划：以第三更新曲线最左侧起始点在t轴的投影点为原点，新坐标系t轴与原坐标系t轴方向一致，新坐标系v轴垂直于t轴，方向为从投影点指向第三更新曲线最左侧起始点。

在新坐标系中，设第三更新曲线的表达式为：

v2＝a2*t²+b2*t+c2

由边界条件可得到四个方程：

v′start2＝2*a2*t0+b1

v′endpoint＝2*a2*t2f+b2

其中vendpoint及其对应的时间可由初始速度规划曲线表达式得到，vstart2为第三更新曲线的起始点速度，vstart2＝maxvel，v′endpoint可由初始速度规划曲线表达式再对时间求导得到，v′start2＝0，t0＝0。联立求解得出第三更新曲线的表达式。

7.求解直线，也即是第一更新曲线，表达式及运动时长t3f。

第一更新曲线的表达式即为y＝maxvel。对应的时长t3f由以下限定条件解得：

x1＝x2

式中，x2为更新曲线的位移量，v1为第二子更新曲线的表达式，end1为第二子更新曲线的终止点，v2为第三子更新曲线的表达式，start2为第三子更新曲线的起始点，endpoint和startpoint分别为待规划速度曲线的终止点和起始点，也即是第三子更新曲线的终止点和第二子更新曲线的起始点。

8.得到具有最大速度限制的五次多项式轨迹规划曲线。

将重新规划的抛物线+直线+抛物线速度接入到步骤四中被“切除掉”的区段，最终得到具有最大速度限制的五次多项式轨迹规划曲线，图6是根据本发明实施方式的最终速度规划曲线的示意图，如图6所示。其中startpoint左侧不需要重新规划的速度曲线不变，为了保证重新规划前后位移量不变，规划后endpoint以后的速度曲线形状在规划前后保持不变，在时间上整体后延，最终能够准确地到达既定目标点。

图7是根据本发明实施方式的另一应用实例的最终速度规划曲线的示意图，如图7所示，为一个新的应用示例，该示例用本发明实现连续轨迹插补，设置的输入为3个点，机械臂需要由起始点运动到中间目标点，再运动到终止点，期间运动速度不得大于用户设置的最大速度。

如图7可知，endpoint之后的速度曲线在不需要重新规划时保持形状不变，而在时间上整体后延，同时能够准确到达中间目标点以及终止点。本示例也验证了本发明适用于输入多个中间点的连续轨迹插补。

本实施方式的算法适用于大多数六轴机械臂的运动学规划，包括工业机械臂和协作机械臂。本实施方式是将五次多项式重新规划运用到点的直线轨迹插补，此外，该方法也可以用于多个插补点的连续轨迹插补。本实施方式的插补点的起始点和终止点的速度以及加速度可不为0。

图8是根据本发明实施例的一种机械臂运动的速度规划装置的示意图，如图8所示，根据本发明实施例的另一方面，还提供了一种机械臂运动的速度规划装置，包括：第一获取模块802，判断模块804，第二获取模块806，规划模块808和替换模块810，下面对该装置进行详细说明。

第一获取模块802，用于获取机械臂的初始速度规划曲线；判断模块804，与上述第一获取模块802相连，用于判断初始速度规划曲线是否存在大于速度阈值的超速区域；第二获取模块806，与上述判断模块804相连，用于在超速区域存在的情况下，获取初始速度规划曲线中大于速度容忍值的待规划速度曲线，待规划速度曲线位于超速区域处，速度容忍值小于速度阈值；规划模块808，与上述第二获取模块806相连，用于对待规划速度曲线进行重新规划得到更新速度曲线，更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值；替换模块810，与上述规划模块808相连，用于将更新速度曲线替换待规划速度曲线得到最终速度规划曲线。

通过上述装置，采用第一获取模块802获取机械臂的初始速度规划曲线；判断模块804判断初始速度规划曲线是否存在大于速度阈值的超速区域；第二获取模块806在超速区域存在的情况下，获取初始速度规划曲线中大于速度容忍值的待规划速度曲线，待规划速度曲线位于超速区域处，速度容忍值小于速度阈值；规划模块808对待规划速度曲线进行重新规划得到更新速度曲线，更新速度曲线和待规划速度曲线覆盖区域的位移量相等，更新速度曲线和待规划速度曲线的起始点和终止点的速度与加速度相同，更新速度曲线的速度极大值不大于速度阈值；替换模块810将更新速度曲线替换待规划速度曲线得到最终速度规划曲线的方式，通过将机械臂在预设运动轨迹的速度曲线中，超出预设速度阈值的部分进行删除，并重新进行规划，达到了使重新规划后的曲线，在预设速度阈值之内的目的，从而实现了保证速度曲线在预设速度曲线范围内，且速度曲线平滑过渡，提高机械臂运动稳定性的技术效果，进而解决了相关技术中机械臂的速度规划方法，无法限制速度，存在加速度骤变，速度曲线不平滑连续，导致机械臂运动不平稳的技术问题。

可选的，更新速度曲线包括呈直线状的第一子更新曲线，以及对称设置在第一子更新曲线两端且呈抛物线形状的第二子更新曲线和第三子更新曲线。

可选的，规划模块包括：第一规划单元，用于根据第二子更新曲线的起始点的速度和加速度分别与待规划速度曲线的起始点的速度和加速度相同，第二子更新曲线的终止点的速度为速度阈值，以及第二子更新曲线的终止点的加速度为零，规划出第二子更新曲线；第二规划单元，用于根据第三子更新曲线的终止点的速度和加速度分别与待规划速度曲线的终止点的速度和加速度相同，第三子更新曲线的起始点的速度为速度阈值，以及第三子更新曲线的起始点的加速度为零，规划出第三子更新曲线；第三规划单元，用于根据第一子更新曲线的速度等于速度阈值，以及更新曲线和待规划速度曲线覆盖区域的位移量相等，规划出第一子更新曲线。

可选的，替换模块包括：计算单元，用于计算更新速度曲线和待规划速度曲线的时间差；平移单元，用于根据时间差将第二初始速度规划曲线沿时间方向整体向右平移，第一初始速度规划曲线和第二初始速度规划曲线分别为初始速度规划曲线中位于待规划速度曲线左侧和右侧的速度规划曲线；连接单元，用于连接更新速度曲线的起始点和第一初始速度规划曲线的终止点，以及连接更新速度曲线的终止点和向右平移后的第二初始速度规划曲线的起始点，得到最终速度规划曲线。

根据本发明实施例的另一方面，还提供了一种计算机存储介质，计算机存储介质包括存储的程序，其中，在程序运行时控制计算机存储介质所在设备执行上述中任意一项的机械臂运动的速度规划方法。

根据本发明实施例的另一方面，还提供了一种处理器，处理器用于运行程序，其中，程序运行时执行上述中任意一项的机械臂运动的速度规划方法。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

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

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

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

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

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。