CN116330299B - 一种基于加速度平滑的工业机器人柔顺运动控制方法 - Google Patents

Abstract

本发明公开了一种基于加速度平滑的工业机器人柔顺运动控制方法，属于运动控制技术领域。本发明根据加速度对待处理的速度曲线进行插补处理；计算插补处理后每一速度阶段中各插补周期内的脉冲个数；以速度阶段为界，对每一速度阶段中各插补周期进行加速度平滑处理；根据每个插补周期中每个脉冲对应的加速度细值进行速度控制，实现工业机器人柔顺运动控制。本发明采用基于加速度平滑的方式对每个插补周期的脉冲数进行了优化，增加了速度的平滑性，减少了机械的震动，较好地实现了优化目标。

Description

一种基于加速度平滑的工业机器人柔顺运动控制方法

技术领域

本发明属于运动控制技术领域，具体涉及一种基于加速度平滑的工业机器人柔顺运动控制方法。

背景技术

在工业加工控制领域，脉冲控制是一种常见的步进电机、伺服电机位置控制方法。而工业机器人是面向工业领域的多关节机械手或多自由度的机器装置，它能自动执行工作，是靠自身动力和控制能力来实现各种功能的一种机器。在实际应用中，很多工业机器人考虑到成本原因都会采用例如STM32等嵌入式芯片作为运动控制器，但是其运算力较低，在进行速度控制时，通常会将连续的速度运算离散化，以降低实时计算量。

将连续的速度运算离散化通常通过插补操作实现，插补(Interpolation)，即机床数控系统依照一定方法确定刀具运动轨迹的过程。也可以说，已知曲线上的某些数据，按照某种算法计算已知点之间的中间点的方法，也称为数据点的密化过程。

在采用脉冲控制时，每个插补周期内的脉冲频率决定了当前插补周期的速度，而现有的技术方案通常为基于每个加减速阶段的路程来计算不同速度阶段的插补周期数，然后根据不同插补周期间的速度变化来调整插补周期内的速度，且在计算时通常按照插补周期进行速度调整，即在当前插补周期结束时一次性增加一个加速度值。例如在采用例如梯形和S形加减速控制时，可以根据整个加速阶段的路程长度和不同速度阶段的加速度来计算得出不同速度阶段的插补周期数，再根据每个插补周期对应的加速度变化，对速度进行加减速控制，最终完成整个插补过程。

然而在速度快速变化时，目前常规的插补方法会导致相邻插补周期的速度出现阶梯型跳变的问题，即当前插补周期内最后一个脉冲在过渡到下一插补周期第一个脉冲时频率发生较大跳变，在加速度较大时这种跳变现象更为突出。而这种速度跳变问题会使得机械产生振动，影响机器的加工精度，因此需要提出一种基于插补的更加柔顺的电机控制方法。

发明内容

本发明的目的在于提供一种基于加速度平滑的工业机器人柔顺运动控制方法，采用基于加速度平滑的方式对每个插补周期的脉冲数进行了优化，增加了速度的平滑性，减少了机械的震动，较好地实现了优化目标。

为实现上述目的，本发明所采取的技术方案为：

一种基于加速度平滑的工业机器人柔顺运动控制方法，所述基于加速度平滑的工业机器人柔顺运动控制方法，包括：

步骤1、根据加速度对待处理的速度曲线进行插补处理，所述速度曲线包括多个速度阶段；

步骤2、计算插补处理后每一速度阶段中各插补周期内的脉冲个数；

步骤3、以速度阶段为界，对每一速度阶段中各插补周期进行加速度平滑处理，包括：

步骤3.1、读取当前插补周期的加速度；

步骤3.2、基于当前插补周期内的脉冲个数对当前插补周期的加速度进行分割处理，得到多个加速度细值；

步骤3.3、将加速度细值有序分配至当前插补周期内的每个脉冲，完成加速度平滑处理；

步骤4、根据每个插补周期中每个脉冲对应的加速度细值进行速度控制，实现工业机器人柔顺运动控制。

以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。

作为优选，所述计算插补处理后每一速度阶段中各插补周期内的脉冲个数，包括：

计算速度曲线中速度阶段的第/>个插补周期的速度/>如下：

；

式中，为速度阶段/>的起始速度，/>为速度阶段/>的第/>个插补周期的加速度，为速度阶段/>的插补周期总数，/>为第/>个插补周期的时长，；

根据速度得到自动装载寄存器在速度阶段/>的第/>个插补周期的/>值为：

；

式中，为自动装载寄存器在速度阶段/>的第/>个插补周期的/>值，/>为自动装载寄存器的最大/>值；

因此，得到速度曲线中速度阶段的第/>个插补周期的脉冲个数如下：

；

式中，为速度曲线中速度阶段/>的第/>个插补周期的脉冲个数。

作为优选，速度曲线中速度阶段的第/>个插补周期的脉冲个数进行简化处理，包括：

；

式中，表示简化处理后的速度阶段/>的第/>个插补周期的脉冲个数，/>表示向下取整处理。

作为优选，所述基于当前插补周期内的脉冲个数对当前插补周期的加速度进行分割处理，得到多个加速度细值，包括：

计算速度阶段的第/>个插补周期内每一个脉冲均分得到的平均加速度：

；

式中，表示速度阶段/>的第/>个插补周期内的平均加速度，/>为速度阶段/>的第/>个插补周期的加速度，/>表示简化处理后的速度阶段/>的第/>个插补周期的脉冲个数，为速度阶段/>的插补周期总数，/表示相除取整操作，/>；

计算速度阶段的第/>个插补周期内的剩余加速度：

；

式中，表示速度阶段/>的第/>个插补周期内的剩余加速度，/>表示相除取余操作，/>表示向下取整处理；

因此，得到多个加速度细值如下：

；

式中，表示速度阶段/>的第/>个插补周期内的第/>个加速度细值。

作为优选，所述将加速度细值有序分配至当前插补周期内的每个脉冲，包括：将加速度细值分配至速度阶段/>的第/>个插补周期内的第/>个脉冲。

作为优选，所述速度曲线为七段S形加减速曲线，所述七段S形加减速曲线包括的速度阶段为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段，则所述根据加速度对待处理的速度曲线进行插补处理，包括：

步骤1.1、计算从最小加速度到最大加速度/>的脉冲周期数，即插补周期数/>，计算公式为：

；

其中，为加加速度常量；

步骤1.2、计算从最小加速度到最大加速度的速度变化量：

；

步骤1.3、计算匀加速阶段的插补周期数如下：

；

式中，为最大速度，/>为最小速度，/>表示相除取整操作；

步骤1.4、根据七段S形加减速曲线的对称性可以得出：加加速阶段和加减速阶段的插补周期数为，减加速阶段和减减速阶段的插补周期数为/>，匀加速阶段和匀减速阶段的插补周期数为/>；

步骤1.5、根据总脉冲周期数与插补周期的关系，计算匀速阶段的插补周期：

；

式中，表示七段S形加减速曲线的总脉冲周期数。

作为优选，所述速度曲线为梯形加减速曲线，所述梯形加减速曲线包括的速度阶段为匀加速阶段、匀速阶段和匀减速阶段，则所述根据加速度对待处理的速度曲线进行插补处理，包括：

步骤1.1、计算匀加速阶段的插补周期数：

；

式中，为最大速度，/>为梯形加减速曲线的起始速度，/>为最大加速度；

步骤1.2、计算匀减速阶段的插补周期数：

；

式中，为梯形加减速曲线的停止速度；

步骤1.3、计算匀速阶段的插补周期数：

；

式中，为匀加速阶段的轨迹长度，/>为匀减速阶段的轨迹长度；

；

；

式中，为梯形加减速曲线的总轨迹长度，/>为匀速阶段的轨迹长度。

本发明提供的一种基于加速度平滑的工业机器人柔顺运动控制方法，与现有技术相比，具有以下有益效果：

（1）结合插补处理提出一种加速度平滑方法，能够有效解决在进行速度插补时相邻插补周期之间产生的速度跳变问题，增加了速度控制精度，提升了工业机器人在运行时的稳定性。

（2）本发明提出的加速度平滑方法可以和不同的速度曲线（例如S形加减速曲线、梯形加减速曲线、抛物线型加减速曲线等）相结合，具有很高的适用性。

（3）本发明提出的加速度平滑方法计算量小，能够快速完成计算，增加了基于加速度平滑的工业机器人柔顺运动控制方法在计算时的实时性。

附图说明

图1为本发明基于加速度平滑的工业机器人柔顺运动控制方法的流程图；

图2为本发明七段S形加减速曲线规划下加加速阶段的插补速度跳变问题示意图；

图3为本发明七段S形加减速曲线规划下加加速阶段进行加速度平滑处理后的效果示意图；

图4为本发明梯形加减速曲线规划下加速度平滑前后对比示意图；

图5 为本发明梯形加减速曲线规划下匀加速阶段加速度平滑前后对比示意图。

具体实施方式

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

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本发明。

为了克服现有技术中插补处理后，相邻两个插补周期之间存在速度跳变的问题，本实施例提出一种基于加速度平滑的工业机器人柔顺运动控制方法，在插补处理后融合加速度平滑处理，弱化速度跳变问题，提高工业机器人运动稳定性。

如图1所示，本实施例的一种基于加速度平滑的工业机器人柔顺运动控制方法，包括以下步骤：

步骤1、根据加速度对待处理的速度曲线进行插补处理。

为了使机器人末端执行器从起始端到末端平滑过渡，需要对机器人的整个运行过程进行加减速控制，即设定机器人整个运行路径的速度变化过程，此时就需要用到速度曲线。常用的速度曲线有梯形加减速曲线（又称梯形速度曲线控制算法）、S形加减速曲线（又称S型速度曲线控制算法）、抛物线型加减速曲线（又称抛物线型加减速控制算法）。本实施例在插补处理时，基于任意速度曲线均可执行，不做限制。

为了便于理解，本实施例给出两种常用的速度曲线进行插补处理说明。

（1）速度曲线为七段S形加减速曲线。

七段S形加减速曲线将整个运动过程划分为7个速度阶段，即加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段，不同阶段速度衔接处加速度连续，且加速度的变化率可控。

基于七段S形加减速曲线，本实施例的插补处理包括以下步骤：

步骤1.1、计算从最小加速度到最大加速度/>的脉冲周期数，即插补周期数/>，计算公式为：

；

其中，为加加速度常量，/>和/>为整个速度曲线中的最小加速度和最大加速度。为了得到整数的插补周期数，本实施例在计算/>之后取整作为/>，这里的取整可以是向下取整，即插补周期数/>为整数。

步骤1.2、计算从最小加速度到最大加速度的速度变化量：

；

步骤1.3、计算匀加速阶段的插补周期数如下：

；

式中，为整个速度曲线中的最大速度，/>为整个速度曲线中的最小速度，/>为相除取整操作，以保证插补周期数/>为一个整数。

步骤1.4、根据七段S形加减速曲线的对称性可以得出：加加速阶段和加减速阶段的插补周期数为，减加速阶段和减减速阶段的插补周期数为/>，匀加速阶段和匀减速阶段的插补周期数为/>。

步骤1.5、根据总脉冲周期数与插补周期的关系，计算匀速阶段的插补周期：

；

式中，表示七段S形加减速曲线的总脉冲周期数。

在七段S形加减速曲线中，从起始速度到最大速度这一过程中一共经历三个阶段，即加加速阶段、匀加速阶段和减加速阶段。其中，在加加速阶段的加速度为：

；

式中，为加加速阶段的加速度，/>表示第/>个插补周期的时长，/>为加加速阶段的插补周期总数，当加速度达到最大加速度时，加加速阶段结束，开始匀加速阶段，匀加速阶段的加速度为：

；

式中，，为匀加速阶段的加速度，/>为匀加速阶段的插补周期总数，当最大速度减去当前速度的变化量等于/>时匀加速阶段结束，开始加减速阶段，加减速阶段的加速度为：

；

式中，为加减速阶段的加速度。

七段S形加减速曲线的插补处理中，加速度变化的阶段只有加速段（加加速阶段、匀加速阶段、减加速阶段）和减速段（加减速阶段、匀减速阶段、减减速阶段）且两段具有对称性，因此本实施例以加加速阶段、匀加速阶段和减加速阶段为例说明加速度的插补过程，加减速阶段、匀减速阶段、减减速阶段的加速度根据对称性即可得到，而匀速段的加速度为0 无需进行规划。

通过以上计算方式，获得加减速控制过程中的每个阶段的插补周期数，再根据每个插补周期对应的加速度变化，对速度进行加减速控制，最终完成整个插补过程。

如图2所示，在插补过程中，每个插补周期都对应一个恒定速度，即插补周期内每个脉冲频率是恒定的。只有当进入到下一插补周期时，才会改变插补周期内的脉冲频率从而来改变当前插补周期的速度，因此相邻两个插补周期之间的速度是存在阶跃跳变问题的。

（2）速度曲线为梯形加减速曲线。

梯形加减速曲线将整个运动过程分为匀加速阶段、匀速阶段和匀减速阶段三个阶段，在变速过程中，加速度始终为设定的一固定值，匀加速时为正值，匀减速时为负值。

在匀加速阶段，梯形的速度曲线表达式为：

；

式中，表示/>时刻对应的速度，/>为/>时刻对应的速度，/>表示梯形加减速曲线加速时的加速度，/>表示/>时刻到/>时刻的时长，/>表示匀加速段的截止时间。

在匀速阶段，梯形的速度曲线表达式为：

；

式中，表示达到匀速阶段时的最大速度，/>表示匀速阶段的截止时间，即/>为匀速阶段的时长。

在匀减速阶段，梯形的速度曲线表达式为：

；

式中，表示匀减速阶段的截止时间，即/>为匀减速阶段的时长。由于梯形加减速曲线中的加速度比较简单，为一个固定值（在加速时为正数，在减速时为负数）和0，因此无需进行规划。

基于梯形加减速曲线，本实施例的插补处理包括以下步骤：

步骤1.1、计算计算匀加速阶段的插补周期数：

；

式中，为最大速度，/>为梯形加减速曲线的起始速度，/>为最大加速度。

步骤1.2、计算匀减速阶段的插补周期数：

；

式中，为梯形加减速曲线的停止速度。

步骤1.3、计算匀速阶段的插补周期数：

；

式中，为匀加速阶段的轨迹长度，/>为匀减速阶段的轨迹长度。根据速度与轨迹长度关系计算出匀速阶段的插补周期数如下：

；

；

式中式中，为梯形加减速曲线的总轨迹长度，/>为匀速阶段的轨迹长度。

步骤2、计算插补处理后每一速度阶段中各插补周期内的脉冲个数。

在进行插补时，插补周期时间设置为定值，此时使用插补周期数来代表时间，因此计算速度曲线中速度阶段的第/>个插补周期的速度/>如下：

；

式中，为速度阶段/>的起始速度，/>为速度阶段/>的第/>个插补周期的加速度，为速度阶段/>的插补周期总数，/>为第/>个插补周期的时长。为了提高加速度平滑效果，本实施例以速度阶段为界，以一个速度阶段为一个整体进行一次计算，计算后得到该速度阶段中每一插补周期内的脉冲个数。

容易理解的是，在以速度阶段为整体进行一次计算时，可以是针对每一速度阶段均进行一次计算，也可以根据速度曲线的对称性，仅计算具有对称性的速度阶段中的一个，然后根据对称性得到剩余速度阶段中每一插补周期内的脉冲个数，即本实施例中的速度阶段为速度曲线中待计算的速度阶段。

在用脉冲控制电机运动时，脉冲的频率代表了速度，则加速度表示脉冲频率的变化值，设物理意义上的加速度值为，则采用脉冲频率变化量来表示加速度/>时，它们之间的转换公式如下：

；

式中，表示电机每转一圈走过的距离，/>表示电机转动一圈需要的脉冲个数。

在定时器中断中，若选择向上计数模式，则计数器从0开始增加，一旦到达自动装载寄存器(TIMx_ARR)里定义的值，就会重新从0开始计数并且产生上溢事件（定时器中断）。因此自动装载寄存器中的值可以理解为用来确定中断频率的，即控制脉冲发送频率的关键在于自动装载寄存器中的值。以STM32系列芯片为例，根据速度/>得到自动装载寄存器在速度阶段/>的第/>个插补周期的/>值为：

；

式中，为自动装载寄存器在速度阶段/>的第/>个插补周期的/>值，/>为自动装载寄存器的最大/>值。当以起始速度/>运行时，/>表示一个插补周期时间内所发脉冲数为1的最大/>计数值。从公式中可以看出/>值与速度呈反比关系，随着速度不断增大，/>的值逐渐减小。此时进中断时间缩短，在一个插补周期内所发脉冲数变多，脉冲频率变快。

因此，得到速度曲线中速度阶段的第/>个插补周期的脉冲个数如下：

；

式中，为速度曲线中速度阶段/>的第/>个插补周期的脉冲个数。为了避免脉冲个数出现非整数的情况，本实施例对/>进行简化处理，包括：

；

式中，表示简化处理后的速度阶段/>的第/>个插补周期的脉冲个数，/>表示向下取整处理。在其他实施例中，脉冲个数的简化处理还可以采取其他方式，例如综合一个速度阶段中的多个/>，然后根据综合值重新为每个插补周期分配为整数的脉冲个数。

步骤3、以速度阶段为界，对每一速度阶段中各插补周期进行加速度平滑处理。由于插补处理后相邻插补周期的速度存在阶跃问题，因此本实施例提出了一种加速度平滑方法，用于降低或消除上述的速度阶跃问题。

步骤3.1、读取当前插补周期的加速度。在插补处理中针对每一速度阶段进行了插补周期数的计算，并且针对每一插补周期中的加速度也进行了规划，因此在加速度平滑时直接基于插补处理规划的加速度进行执行。

步骤3.2、基于当前插补周期内的脉冲个数对当前插补周期的加速度进行分割处理，得到多个加速度细值。

本实施例将每一插补周期内的脉冲个数进行优化分配，优化分配时将读取的加速度细分到当前插补周期内的每一个脉冲上，即将加速度合理分配到每发一次的脉冲上。

首先进行取整操作，计算速度阶段的第/>个插补周期内每一个脉冲均分得到的平均加速度：

；

式中，表示速度阶段/>的第/>个插补周期内的平均加速度，/>为速度阶段/>的第/>个插补周期的加速度，/>表示简化处理后的速度阶段/>的第/>个插补周期的脉冲个数，为速度阶段/>的插补周期总数，/表示相除取整操作。

然后进行取余操作，计算速度阶段的第/>个插补周期内的剩余加速度：

；

式中，表示速度阶段/>的第/>个插补周期内的剩余加速度，/>表示相除取余操作，/>表示向下取整处理。

因此，得到多个加速度细值如下：

；

式中，表示速度阶段/>的第/>个插补周期内的第/>个加速度细值，/>表示1到中的数值。

步骤3.3、将加速度细值有序分配至当前插补周期内的每个脉冲，完成加速度平滑处理。以个脉冲个数进行描述，在有序分配加速度细值时，将加速度细值/>分配至速度阶段/>的第/>个插补周期内的第/>个脉冲。因此对于/>个脉冲，前/>个脉冲分配的加速度为/>，剩余的脉冲分配的加速度为/>，该分配方式提升速度变化平滑性。

以图2中所示的七段S形加减速曲线中加加速阶段的插补处理结果为例，在进行加速度平滑处理后得到如图3所示的效果，图2中时间段74-76之间的柱状体表示该插补周期内的速度为32.22mm/s，将一个柱状体分为两种填充方式是为了表示顶部填充方式表示的区域即为当前插补周期与上一插补周期的速度阶跃区域，也就是前一个插补周期待优化的区域。时间段74-76之后的时间段对应的柱状体同理理解。图3中加速度平滑处理后使得每一时间段（最后一个未示意）对应的柱状体顶部出现多个小柱状体，小柱状体即为加速度平滑处理后基于原始速度（即原图2中的柱状体）得到的速度增量。对比图3中前一个插补周期内柱状体顶部的小柱状体和图2中后一个插补周期内柱状体的顶部填充方式表示的区域，可以看出第一个插补周期内的脉冲通过细分加速度的方式平稳递增过渡到下一插补时刻。而下一个插补周期内的脉冲以同样方式，再进行合理分配加速度。通过类似的均匀分配，最终解决了在加减速控制阶段中当前插补周期内对应的速度不变，再进入下一插补周期时速度突变的问题。

另外，本实施例以梯形加减速曲线为例进行试验，梯形加减速曲线插补处理后的速度变化示意图如图4中的（a）所示，存在明显的速度阶跃性表现。基于图4中的（a）进行加速度平滑处理后，得到如图4中的（b）所示的速度变化示意图，可以看出经过加速度平滑处理后速度柔顺性有效提升。如图5所示，以梯形加减速曲线规划下匀加速阶段的速度阶跃问题为例，本实施例在加速度细化后采取两段方式为脉冲分配加速度，该加速度分配方式使得两段对应的速度能够平稳过渡，从速度变化的整体上来看，速度平滑性得到了有效提高，现有插补方法中存在的速度阶跃问题得到了良好解决。

步骤4、根据每个插补周期中每个脉冲对应的加速度细值进行速度控制，实现工业机器人柔顺运动控制。本实施例为每个脉冲分配了对应的加速度细值，在用脉冲控制电机运动时，根据加速度细值完成控制，具体控制过程为现有的脉冲控制过程，本实施例不进行限制。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。

Claims (4)

1.一种基于加速度平滑的工业机器人柔顺运动控制方法，其特征在于，所述基于加速度平滑的工业机器人柔顺运动控制方法，包括：

步骤1、根据加速度对待处理的速度曲线进行插补处理，所述速度曲线包括多个速度阶段；

所述速度曲线为七段S形加减速曲线，所述七段S形加减速曲线包括的速度阶段为加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段，则所述根据加速度对待处理的速度曲线进行插补处理，包括：

步骤1.1、计算从最小加速度到最大加速度/>的脉冲周期数，即插补周期数，计算公式为：

；

其中，为加加速度常量；

步骤1.2、计算从最小加速度到最大加速度的速度变化量：

；

步骤1.3、计算匀加速阶段的插补周期数如下：

；

式中，为最大速度，/>为最小速度，/>表示相除取整操作；

步骤1.4、根据七段S形加减速曲线的对称性可以得出：加加速阶段和加减速阶段的插补周期数为，减加速阶段和减减速阶段的插补周期数为/>，匀加速阶段和匀减速阶段的插补周期数为/>；

步骤1.5、根据总脉冲周期数与插补周期的关系，计算匀速阶段的插补周期：

；

式中，表示七段S形加减速曲线的总脉冲周期数；

或者，所述速度曲线为梯形加减速曲线，所述梯形加减速曲线包括的速度阶段为匀加速阶段、匀速阶段和匀减速阶段，则所述根据加速度对待处理的速度曲线进行插补处理，包括：

步骤1.1、计算匀加速阶段的插补周期数：

；

式中，为最大速度，/>为梯形加减速曲线的起始速度，/>为最大加速度；

步骤1.2、计算匀减速阶段的插补周期数：

；

式中，为梯形加减速曲线的停止速度；

步骤1.3、计算匀速阶段的插补周期数：

；

式中，为匀加速阶段的轨迹长度，/>为匀减速阶段的轨迹长度；

；

；

式中，为梯形加减速曲线的总轨迹长度，/>为匀速阶段的轨迹长度；

步骤2、计算插补处理后每一速度阶段中各插补周期内的脉冲个数，包括：

计算速度曲线中速度阶段的第/>个插补周期的速度/>如下：

；

式中，为速度阶段/>的起始速度，/>为速度阶段/>的第/>个插补周期的加速度，/>为速度阶段/>的插补周期总数，/>为第/>个插补周期的时长，/>；

根据速度得到自动装载寄存器在速度阶段/>的第/>个插补周期的/>值为：

；

式中，为自动装载寄存器在速度阶段/>的第/>个插补周期的/>值，/>为自动装载寄存器的最大/>值；

因此，得到速度曲线中速度阶段的第/>个插补周期的脉冲个数如下：

；

式中，为速度曲线中速度阶段/>的第/>个插补周期的脉冲个数；

步骤3、以速度阶段为界，对每一速度阶段中各插补周期进行加速度平滑处理，包括：

步骤3.1、读取当前插补周期的加速度；

步骤3.2、基于当前插补周期内的脉冲个数对当前插补周期的加速度进行分割处理，得到多个加速度细值；

步骤3.3、将加速度细值有序分配至当前插补周期内的每个脉冲，完成加速度平滑处理；

步骤4、根据每个插补周期中每个脉冲对应的加速度细值进行速度控制，实现工业机器人柔顺运动控制。

2.如权利要求1所述的基于加速度平滑的工业机器人柔顺运动控制方法，其特征在于，速度曲线中速度阶段的第/>个插补周期的脉冲个数进行简化处理，包括：

；

式中，表示简化处理后的速度阶段/>的第/>个插补周期的脉冲个数，/>表示向下取整处理。

3.如权利要求1所述的基于加速度平滑的工业机器人柔顺运动控制方法，其特征在于，所述基于当前插补周期内的脉冲个数对当前插补周期的加速度进行分割处理，得到多个加速度细值，包括：

计算速度阶段的第/>个插补周期内每一个脉冲均分得到的平均加速度：

；

式中，表示速度阶段/>的第/>个插补周期内的平均加速度，/>为速度阶段/>的第/>个插补周期的加速度，/>表示简化处理后的速度阶段/>的第/>个插补周期的脉冲个数，/>为速度阶段/>的插补周期总数，/表示相除取整操作，/>；

计算速度阶段的第/>个插补周期内的剩余加速度：

；

式中，表示速度阶段/>的第/>个插补周期内的剩余加速度，/>表示相除取余操作，表示向下取整处理；

因此，得到多个加速度细值如下：

；

式中，表示速度阶段/>的第/>个插补周期内的第/>个加速度细值。

4.如权利要求3所述的基于加速度平滑的工业机器人柔顺运动控制方法，其特征在于，所述将加速度细值有序分配至当前插补周期内的每个脉冲，包括：将加速度细值分配至速度阶段/>的第/>个插补周期内的第/>个脉冲。