一种运动路径的规划方法、系统、主机及存储介质与流程

2021-01-19 14:01:56|

415|

起点商标网

本申请涉及自动化控制的领域，尤其是涉及一种运动路径的规划方法、系统、主机及存储介质。

背景技术：

近年来随着工业自动化的发展，自动化控制逐渐成为一门新兴学科，并得到了较快的发展。机械手是提高劳动生产率，改善劳动条件，减轻工人劳动强度和实现工业生产自动化的一个重要手段，国内外都十分重视它的应用和发展。机械手最早应用在汽车制造工业，常用于焊接、喷漆、上下料和搬运。机械手延伸和扩大了人的手足和大脑功能，它可替代人从事危险、有害、有毒、低温和高热等恶劣环境中的工作；代替人完成繁重、单调重复劳动，提高劳动生产率，保证产品质量。

通过动作示教，机械手可以按照固定的程序进行运动。不论运动轨迹简单还是复杂，都需要通过直线插补、圆弧插补或关节插补等方法，控制机器人完成轨迹运动。显然，教导的点位越多，机械手的运动轨迹精度越高。但点位越多，也就意味着线段越多，就需要频繁的进行加减速，以便完成线段之间的过渡，从而造成整体效率降低。目前，相关技术提出使用均匀b样条将控制点拟合成连续的曲线，从而实现平滑过渡的方法。但是发明人认为，这种拟合的方法随着控制点数量的增多，对机械手上芯片的算力要求和内存要求更高，因此难以推广到目前市场上大量存在的低配置机械手进行使用。

技术实现要素：

为了节约算力和内存，本申请提供一种运动路径的规划方法、系统、主机及存储介质。

第一方面，本申请提供的一种运动路径的规划方法，采用如下的技术方案：

一种运动路径的规划方法，包括：

录入步骤：

在缓存区中建立执行区和暂存区，其中，执行区具有固定数量的存储位，每个存储位至多容纳一个控制点；

基于调整策略向存储区申请有序点集并录入暂存区；

执行步骤：

s1.从暂存区中的有序点集中移出一组控制点，并将该组控制点逐点写入执行区的存储位中；

s2.对执行区中的各控制点按写入顺序进行b样条拟合，以更新拟合轨迹；

s3.基于插补策略对拟合轨迹进行插补以更新插补轨迹；

s4.驱动执行件沿插补轨迹移动，检测执行件与插补轨迹节点的相对位置关系，并查询执行区以检测剩余数量，其中，剩余数量为执行件最新经过的节点对应的控制点按写入顺序排列在后的控制点数量；判断剩余数量是否小于警戒值，若是，则清除执行区中执行件经过插补轨迹节点对应的控制点，并返回s1，直至暂存区中的有序点集中的控制点被全部取出；其中，警戒值为小于存储位数量的预设值；

s5.销毁执行区。

通过采用上述技术方案，人们择定示教样本，并通过示教样本选取一系列有序的控制点，以作为用为教导轨迹生成的数据。这些控制点有序地存入存储区中，存储区通常位于硬盘等外存储器，用于存储不活跃数据。当控制点全部录入时，缓存区建立执行区和暂存区，执行区具有固定数量的存储位，执行区可以为队列或其它数据结构，只需要使得有序的控制点在按序写入执行区后能够写入顺序读取即可。暂存区用于存储有序点集，以便于在有序点集不符合要求时对有序点集进行调整。存储位用于存储控制点数据，其定量设置决定了单次拟合所使用的控制点的上限。缓存区通常位于内存等内存储器，用于高速读写数据，价格和单位存储量之比远高于外存储器。

在第一次拟合之前，先从存储区中申请有序点集并录入暂存区中。在第一次拟合开始时，定量或者不定量地从暂存区中的有序点集的头部申请一组控制点并写入执行区中，同时删除暂存区中的有序点集中被复制的控制点。然后按照写入顺序对执行区中控制点进行拟合，插补，再控制执行件沿插补轨迹移动，执行件可以为机械臂末端或其它可精确移动装置。当随着执行件在插补轨迹上移动，插补轨迹上未被执行件经过的节点对应的控制点逐渐减少。当控制点的剩余数量小于警戒值时，清除执行区中执行件已经过轨迹对应的控制点数据，然后进行下一次拟合。

在下一次拟合时，重复上述步骤，直至暂存区中的有序点集中的控制点被全部取出，即完成了执行件沿整条拟合轨迹的移动。最后销毁执行区并删除多余的中间数据。

对于存储量的考虑，系统的内存有限。对于单次轨迹拟合，如果需要拟合的轨迹较长，且轨迹形状多变，比如绣花操作，为了保证拟合形状的近似，需要更多的控制点，比如上千个。如果进行一次拟合，势必要将所有的控制点录入系统的内存中，在进行拟合计算和插补计算时同时还将产生大量的中间数据，无疑对内存容量提出了很高的要求。本方案采用分散式计算的方式，将b样条分为若干较小的段落进行计算，单次只需录入较少量的控制点，从而产生较少的中间数据，从而降低了对内存容量的要求。

对于计算量的考虑，系统的算力是有限的。系统除了要进行拟合插补计算，还需要将有限算力分配到各个方面。本方案采用分散式计算的方式，单次对较少的控制点进行拟合插补，通过该点而计算出下一点的位置，然后通过下一点的位置和结构算法(底层逻辑)，而得出机械臂各个关节的运动量。对产生的数据进行实时调用，用过即删，从而将计算分散到各个运动的节拍中，从而避免某个时刻占据过多的算力而影响其它器件的工作，从而避免产生机械臂急停急启的现象，减少了机械臂运动发生的磨损。

优选的，所述录入步骤还包括：录入预设拟合阶数，设定b样条拟合的阶数为预设拟合阶数；所述存储位排序设置，并设置警戒节点，其中警戒节点为执行区中倒数第n个存储位，n为警戒值；

所述s1中控制点从暂存区中的有序点集中定量移出，并依序写入执行区；

所述s4包括以下步骤：

s41.驱动执行件沿插补轨迹移动；

s42.检测执行件与插补轨迹节点的相对位置关系，并查询执行区以检测剩余数量，判断剩余数量是否小于警戒值，若否，则返回s41；若是，则清除执行区中位于警戒节点之前的存储位数据；

s43.迁移执行区中剩余的控制点至执行区中序次靠前的存储位，并按原有次序排列；

s44.返回s1，直至暂存区中有序点集中的控制点被全部取出。

通过采用上述技术方案，执行区中的控制点进行拟合与插补后得到插补轨迹，警戒节点内所存储的控制点将在插补轨迹上具有相应的节点。监测执行件的世界坐标，当执行件的世界坐标到达该点位时，则清空该警戒点位之前的存储位数据，并将剩余的数据迁移至执行区中序次靠前的存储位，通常为从首位开始，并按照原有的次序排列。

如果执行点在完整地沿一条插补轨迹运动至终点后，系统才清空原来的执行区并从暂存区中的有序点集中取出新的执行点代入执行区中，并重新拟合插补出一条用于指导执行件移动的轨迹，则在这个计算过程中，执行件不可避免地将会中止运动，而发生急停。即使进行了速度规划，也将会增加整体的工作时长。因此本技术方案采用分散式计算的方式，能够有效地将对路径进行前瞻性的规划，并将计算过程分散在各个运动节拍中，降低了单次计算在计算过程中对算力的使用占比。

优选的，所述录入步骤还包括：在缓存区中划出用于容纳中间数据的衍生数据区；

所述s22还包括：将拟合计算产生的中间数据写入衍生数据区；

所述s3还包括：将插补计算产生的中间数据写入衍生数据区中，其中插补计算产生的中间数据与控制点相对应；

所述s42还包括：清除衍生数据区内警戒节点之前数据位存储的控制点相对应的中间数据。

通过采用上述技术方案，在使用控制点数据进行拟合时，控制点数据会根据系统内置的算法衍生出很多的中间数据，数量大约会是录入数据的十几倍，数据插补也将会产生大量的相关数据。且在进行运算的过程中，还会产生更多的临时变量。因此，将这些临时数据统一安放在衍生数据区中，不仅方便调用，而且随用随删，能够有效地缓解内存压力。

优选的，所述采样策略包括以下步骤：

在输入上均匀或非均匀地选取控制点；

将控制点按照选取顺序排序并作为有序点集。

通过采用上述技术方案，由于所需要的轨迹具有平缓和扭曲的区别，当轨迹各处的曲率变化较小时，比如说一条线段，则可以在线段上均匀地选取控制点。当曲率变化较为剧烈时，则在曲率变化较大且曲率较大的部分选取更密集的控制点，在曲率较小的部分选取较为稀疏的控制点。控制点选取过多将会影响拟合效率，控制点选取过少将会使得拟合曲线与原始轨迹的偏离较为明显。

优选的，所述调整策略包括以下步骤：

将暂存区中有序点集的首位复制为至少k个重合点并并入暂存区中有序点集的头部；将暂存区中有序点集的末位复制为至少k个重合点并并入暂存区中有序点集的尾部，其中，k＝预设拟合阶数-1。

通过采用上述技术方案，暂存区中的有序点集的第一个点和最后一点，分别对应于拟合轨迹的两端点。将这两个点分别复制为至少k个重合点，所形成的拟合组保证使得对应的拟合线段退化为一个点，从而使得整体的拟合轨迹与输入轨迹的首尾两端点重合。

优选的，所述采样策略包括以下步骤：

录入示教轨迹图像；

将示教轨迹划分为若干段局部示教轨迹，并计算各段局部示教轨迹的平均曲率半径；

在各段局部示教轨迹上分别以等间距选取控制点，其中，各段局部示教轨迹上控制点的间距与各段局部示教轨迹的平均曲率半径正相关。

通过采用上述技术方案，在不采用操控控制器以手动选取示教点的情况下，可以采用录入示教样本的方法。示教轨迹图像对应于示教样本，可以为cad图像。将示教轨迹均匀或非均匀地划分为若干段，划分步长可以根据图案的整体形状进行人工设定，以避免遗漏小而突兀的弯曲。计算各段局部示教轨迹的平均曲率半径，平均曲率半径高的说明其整体较为平缓，需要的控制点更少；平均曲率半径低的说明其整体较为弯曲，需要的控制点更多。对于某处小而突兀的弯曲，可以手动增添控制点，以达到拟合形状与示教轨迹更相似的目的。

优选的，所述插补策略包括以下步骤：

步骤一：获取拟合轨迹的首端点和末端点，并将首端点代入指引节点中，其中指引节点用于引导执行件的运动；

步骤二：将拟合轨迹在指引节点处进行一阶泰勒展开，并取得拟合轨迹在指引节点的一阶导数；

步骤三：基于指引节点和对应于该指引节点的一阶导数生成局部插补线段，并将局部插补线段远离该指引节点的一端的坐标值代入指引节点；

步骤四：基于局部插补轨迹更新插补轨迹；

步骤五：返回步骤二，直至指引节点为拟合轨迹的末端点。

通过采用上述技术方案，拟合轨迹不断地在指引节点处展开，得到局部插补线段，以更新插补轨迹，并获取新的指引节点。通过多次迭代，不断获得该拟合轨迹对应的局部插补线段，当机器人走到当前位置后，机器人能够通过插补算法获得下一时刻应该到达的位置的坐标。

第二方面，本申请提供的一种基于路径规划的移动系统，采用如下的技术方案：

一种基于路径规划的移动系统，包括：

录入模块，用于在示教样本上有序选取若干个控制点，并作为有序点集录入存储区；

缓存模块，包括位于缓存区中的执行区和用于储存从存储区中复制的有序点集的暂存区，所述执行区具有固定数量的存储位，每个存储位至多容纳一个控制点；

写入模块，用于从暂存区中的有序点集移出一组控制点，并将该组控制点逐点写入执行区的存储位中；

拟合模块，用于对执行区中的各控制点按写入顺序进行b样条拟合以更新拟合轨迹；

插补模块，用于对拟合轨迹进行插补以更新插补轨迹；

移动模块，用于驱动执行件沿插补轨迹移动；

循环判断模块，用于检测执行件与插补轨迹节点的相对位置关系，并查询执行区以检测剩余数量，当剩余数量小于警戒值时，清除执行区中执行件经过插补轨迹节点对应的控制点，并依次驱动拟合模块和插补模块和移动模块，直至暂存区中的有序点集中的控制点被全部取出，并销毁执行区。

第三方面，本申请提供的一种主机，采用如下的技术方案：

一种主机，包括存储器和处理器，存储器上存储有能够被处理器加载并执行上述运动路径规划方法的计算机程序。

第四方面，本申请提供的一种计算机可读存储介质，采用如下的技术方案：一种计算机可读存储介质，存储有能够被处理器加载并执行如上述方法的计算机程序。

附图说明

图1是本申请实施例中录入步骤的程序框图；

图2是本申请实施例中采样策略的程序框图；

图3是本申请实施例中离线编程模式的程序框图；

图4是本申请实施例中执行步骤的程序框图；

图5是本申请实施例中插补策略的程序框图；

图6是本申请实施例中s4子步骤的程序框图；

图7是本申请实施例中s3子步骤的程序框图；

图8是本申请实施例中划分策略的程序框图；

图9是本申请实施例中统计策略的程序框图；

图10是本申请实施例中规划策略的程序框图；

图11是本申请实施例中第二速度规划的程序框图；

图12是本申请实施例中局部减速策略的程序框图。

具体实施方式

以下结合附图1-12，对本申请作进一步详细说明。

通过动作示教，机械手可以按照固定的程序进行运动。不论运动轨迹简单还是复杂，都需要通过直线插补、圆弧插补或关节插补等方法，控制机器人完成轨迹运动。在驱动机械臂沿复杂轨迹移动时，如果简单地根据轨迹的形状录入控制点，并控制执行件沿控制点连成的小线段移动，相邻两条直线或者圆弧进行过渡时，需要进行加减速，且会有微小的停顿，频繁的进行加减速会延长工作周期，整个过程中卡顿感随着运行速度的提升而增强，使速度难以提升到较快的速度，限制了机器人的工作效率。当教导的点位越多，机械手的运动轨迹精度越高。但点位越多，也就意味着线段越多，加减速更为频繁，从而造成整体效率降低。

目前，常用的拟合方式有多项式拟合和最小二乘法拟合。多项式拟合通过预设相应的幂次，再将一定的控制点代入，即可得到连接这些控制点的平滑曲线。但是随着多项式拟合的幂次升高，单次拟合的运算的计算量也将会大幅提升。同时，龙格现象在高幂次多项式中也更为突显，相邻控制点之间的曲线斜率变化将变得非常大，不利于进行加减速控制。此外，当增加或减少一个控制点时，将会使得高阶曲线的整体形状发生巨大的改变，不利于对原轨迹的拟合。

使用最小二乘法拟合时，需要先预设一拟合曲线的基本函数，再最小化各控制点到拟合曲线距离方差之和，从而确定该函数的各参数值，获得最为接近的拟合曲线。这种方法所得到的拟合曲线很近似示教轨迹的形状，但是由于示教轨迹的形状通常多变，很难找到一个合适的基本函数，因此该方法的限制较大。

本申请实施例公开一种运动路径的规划方法。该规划方法是一种基于b样条拟合的方法，包括用于录入控制点的录入步骤、以及基于控制点生成b样条拟合轨迹并控制执行件沿该拟合轨迹运动的执行步骤。执行件为一具有多自由度的工作器件，可以为机械臂末端，也可以是一种桁架式控制装置，在本实施例中选用为机械臂。

参照图1，录入步骤至少包括两个步骤，分别为：

步骤一：选取示教样本，基于采样策略在示教样本上有序选取若干个控制点，并作为有序点集录入存储区；同时，录入预设拟合阶数k，设定预设拟合阶数k作为b样条拟合的阶数。

其中，参照图2，采样策略包括：

策略步骤一：在输入上均匀或非均匀地选取控制点；

示教样本可以为表面为平面的物体，也可以是一个表面具有三维曲面的物体。机械手通常具有多个自由度，能够在三维空间中调整机械手末端的位置和朝向，以与示教样本的表面各处进行贴合。示教样本可以是空白的，操作者根据需要控制机械手在示教样本上选取控制点。其中，控制点指用来控制和调整曲线形状的特殊点。

示教样本上也可以设置有用于指导人操控机械臂进行教导动作的标记线条，比如带颜色的曲线。在本实施例中，示教样本选取为平整的板状物体，其表面设置有黑色的标记线条。

控制点的选取模式包括手动模式或离线编程模式。手动模式就是使用控制手柄操控机器人手末端沿着示教样本上的轨迹移动，并在移动过程中不断调整机械手各关节的姿态，以使得机械手末端贴合于示教样本表面或与示教样本的表面保持一定距离移动。在移动过程中，按照操控者的需要依次录入控制点。

离线编程模式中，则不是使用手柄操作，而是基于数字图像进行控制点的选取。参照图3，该离线编程模式包括以下的子步骤：

子步骤一：录入示教轨迹图像。

该示教轨迹图像可以为通过离线编程软件将3d建模图形转化而成的3d轨迹图像，也可以是带有所需示教轨迹的2d平面图像。

子步骤二：将示教轨迹按照划分为若干段局部示教轨迹。示教轨迹可以进行自动分段，也就是按照固定步长进行划分，步长的大小根据具体图像而定。示教轨迹也可以根据人工进行分段，人工分段具有能够根据目测的方法将平缓处少分段，将陡峭处多分段的优点。实际操作中，也可以实用自动分段和人工分段相结合的方法进行分段。

分段后对各段进行平均曲率半径的计算，平均曲率半径能够大致表征该段局部轨迹的平缓情况。

子步骤三：在各段局部示教轨迹上分别以等间距选取控制点，其中，各段局部示教轨迹上控制点的间距与各段局部示教轨迹的平均曲率半径正相关。

平均曲率半径高的说明其整体较为平缓，需要的控制点更少；平均曲率半径低的说明其整体较为弯曲，需要的控制点更多，才能更好地表现出示教轨迹的形状特征。举个例子，示教轨迹其中也就是说，linea的平缓程度大于lineb，lineb的平缓程度大于linec。因此，linea上控制点的选取间隔大于lineb，lineb上控制点的选取间隔大于linec。对于某处小而突兀的弯曲，可以手动增添控制点，以达到拟合形状与示教轨迹更相似的目的，也视其重要程度选择将其忽视，以保持后期拟合轨迹的平整度。

策略步骤二：将控制点按照选取顺序排序并作为有序点集；

示教轨迹进行有序划分后，各段局部示教轨迹上的控制点也进行有序选取，从而使得所有的控制点在示教轨迹上有序排列。有序的排列各个控制点有序地写入有序点集中，以便于后期按照顺序地从有序点集中取出，从而使得拟合轨迹近似于示教轨迹。

步骤二：在缓存区中建立执行区、暂存区以及用于容纳中间数据的衍生数据区；其中，执行区具有固定数量的存储位，存储位排序设置且每个存储位至多容纳一个控制点；设定警戒值n，警戒值n为小于存储位数量且大于等于b样条阶数的预设值，在执行区中倒数第n个存储位设为警戒节点。

暂存区用于存储有序点集，以便于在有序点集不符合要求时对有序点集进行调整。存储位用于存储控制点数据，其定量设置决定了单次拟合所使用的控制点的上限。缓存区通常位于内存等内存储器，用于高速读写数据，价格和单位存储量之比远高于外存储器。

拟合组在进行拟合时，将会产生大量的中间数据，以得到最终的拟合公式，这些中间数据包括德布尔递推公式中计算出的各个基函数，计算基函数时产生的临时参数等。这些数据产生后被存储在衍生数据区中，以与存储控制点的执行区区分开来，使得对中间数据操作时不对控制点产生影响。执行区设置有固定的存储位，这意味着存储区只能存储有限个数的控制点数据。

步骤三：基于调整策略向存储区申请有序点集并录入暂存区。

其中，调整策略包括以下步骤：

将暂存区中有序点集的首位复制为至少k个重合点并并入暂存区中有序点集的头部，其中k为预设拟合阶数；

将暂存区中的有序点集的末位复制为至少k个重合点并并入暂存区中有序点集的尾部。

暂存区中的有序点集的第一个点和最后一点，分别对应于拟合轨迹的两端点。将这两个点分别复制为至少k个重合点，所形成的拟合组保证使得对应的拟合线段退化为一个点，从而使得整体的拟合轨迹与输入轨迹的首尾两端点重合。

参照图4，执行步骤至少包括以下五个步骤s1-s5，分别为：s1.从暂存区中的有序点集中移出一组控制点，并将该组控制点逐点写入执行区的存储位中。

在本实施例中，s1定量或不定量地从暂存区中的有序点集的头部移出一组控制点，将该组控制点依序写入执行区中序次靠前的存储位。

举个例子，暂存区中的有序点集p＝{p1,p2,p3,…,pn}，其中n为有限值，在本实施例中取n＝100为例。系统可以分多次定量地从暂存区中的有序点集中取点，若单次取点量无法除尽有序点集的控制点量，则在最后一次取点时将会少于单次取点量。举个例子，比如单次取点量为11个，则共取十次，最后一次为1个点。当然，系统也可以不规定单次取点量，比如在连续的几次取点过程中，分别以10个、9个、8个的数量进行取点。

s2.对执行区中的各点按写入顺序进行b样条拟合，以更新拟合轨迹，并将拟合计算产生的中间数据写入衍生数据区。b样条曲线曲面具有几何不变性、凸包性、保凸性、变差减小性、局部支撑性等许多优良性质。举个例子，已知n+1个控制点pi(i＝0,1,2,…,n)，节点向量u＝{u0,u1,…,um}，依次连接这些控制点可以构成一个特征多边形，可定义k+1阶(k次)b样条曲线的表达式为2≤k≤n+1，m＝n+k+1。

其中ni，k(u)是k次b样条基函数，也叫调和函数，或者k次规范b样条基函数，下面为其递归公式(cox-deboor公式)的定义：

这里规定ui∈[0,1]。

显然，基函数由u定义，其中基函数满足微分方程：

由于每次拟合时，计算机读取执行区内所有控制点，作为拟合运算的变量，从而得到中间数据和拟合曲线，所获得的中间数据存入衍生数据区中，以兹后续插补过程使用。

在拟合结束后，得到一整条平滑的曲线，由b样条曲线的局部性可知，当对其中某个控制点进行修改时，仅在局部发生形状改变。

另外，该处的拟合轨迹指的仅是此时执行区内所有控制点拟合得到的拟合轨迹，而非整个有序点集内所有控制点拟合得到的拟合轨迹。后续从下文可以得知，相邻两次拟合过程中得到的拟合轨迹具有重合部分。

s3.基于插补策略对拟合轨迹进行插补以更新插补轨迹。

其中，参照图5，插补策略包括以下步骤：

步骤一：获取拟合轨迹的首端点和末端点，并将首端点代入指引节点中，其中指引节点用于引导执行件的运动；

该拟合轨迹是对应于当前执行区内的控制点集的，当执行区内控制点发生更新时，拟合轨迹将发生改变，故代入指引节点的首端点也不同。

步骤二：将拟合轨迹在指引节点处进行一阶泰勒展开，并取得拟合轨迹在指引节点的一阶导数；

首先，节点u(t)可以看成是关于时间t的函数,b样条曲线上的坐标p(u)，即指引节点的坐标，可以看成节点u的函数。由于p(u)与u(t)并非线性关系，因此需要根据当前时刻的坐标和速度，推算下一时刻的坐标。可以选用一阶或多阶泰勒公式进行展开，在本实施例中，用一阶泰勒公式对节点u(t)函数进行展开，并用一阶泰勒公式对节点p(u)函数进行展开。

目前有p(u+δ)＝p(u)+p′(u)*δl这个关系式，现在需要确定下一时刻的移动距离(速度)δl对应的节点值。只有找到对应的节点值，那么|p(u+δ)-p(u)|的实际长度才能与我们期望的移动距离(速度)δl数值相近，否则机器运行速度就不受控制了。

现在我们用一阶泰勒公式对节点u(t)在t时刻处进行展开，可以得到以下关系，

已知b样条曲线对时间t的导数：

整理可得，

对于b样条曲线来说，微分关系为δl＝vi(t)·δt。

令δt＝1，那么δl＝vi(t)，也就是说，当知道移动距离δl，以及当前节点ui，便能得到下一时刻的节点值ui+1。知道节点值，便能得到该节点对应的b样条曲线上的坐标值。

步骤四：基于局部插补轨迹更新插补轨迹。

局部插补线段首尾依次相连，合并后即为连续的插补轨迹，用于指导执行件的移动。通过插补行为，当机器人走到当前位置后，机器人能够通过插补算法获得下一时刻应该到达的位置的坐标。

步骤五：返回步骤二，直至指引节点为拟合轨迹的末端点；

对指引节点pi(u)中的参数i不断地迭代，以沿着拟合轨迹不断获取指引节点pi+1(u)的坐标值，直至到达拟合轨迹的末端。

s4.驱动执行件沿插补轨迹移动，检测执行件与插补轨迹节点的相对位置关系，并查询执行区以检测剩余数量，其中，剩余数量为执行件最新经过的节点对应的控制点按写入顺序排列在后的控制点数量；判断剩余数量是否小于警戒值，若是，则清除执行区中执行件经过插补轨迹节点对应的控制点，并返回s1，直至暂存区中有序点集中的控制点被全部取出；其中，警戒值为小于存储位数量的预设值。

执行区可以为队列或其它数据结构，只需要使得有序的控制点在按序写入执行区后能够写入顺序读取即可。举个例子，在第一次进行拟合时，系统定量地从暂存区中的有序点集的头部复制一组控制点，并删除暂存区中的有序点集中被复制的控制点。由存储位的第一位开始排序录入控制点，然后对执行区中的控制点进行拟合，插补，再控制执行件沿插补轨迹移动。在靠近插补轨迹末端时，清除执行区中执行件已经过的轨迹对应的控制点数据，然后将剩余的控制点按照原有顺序迁移到执行区中序次靠前的存储位，通常为存储位的第一位及之后的存储位。然后进行下一次拟合。

具体的，参照图6，s4包括以下子步骤：

s41.驱动执行件沿插补轨迹移动；

s42.检测执行件与插补轨迹节点的相对位置关系，并查询执行区以检测剩余数量，判断剩余数量是否小于警戒值，若否，则返回s41；若是，则清除执行区中位于警戒节点之前的存储位数据，并清除衍生数据区内警戒节点之前数据位存储的控制点相对应的中间数据。

在录入步骤的步骤二中已设定警戒值n，警戒值n为小于存储位数量且大于等于b样条阶数的预设值，在执行区中倒数第n个存储位设为警戒节点。由于拟合计算的计算量相对较大，执行件移动到当前拟合轨迹末端时再计算，将会导致无法及时形成相应的插补轨迹，导致运动发生停滞，造成减速。警戒节点的设置，使得系统在执行件靠近拟合轨迹终点但尚未抵达时即开始拟合轨迹的更新，能够为后续的计算预留下充足的时间，使得机械臂整体动作流程运行，并有利于将计算量合理分散在各个节拍之中。

s43.迁移执行区中剩余的控制点至执行区中序次靠前的存储位，并按原有次序排列。

举个例子，机械臂末端位于控制点集{pi,pi+1,…,pi+n}对应的插补轨迹上移动，当机械臂末端移动至{pi+n-n+1,pi+n-n+2,…,pi+n}对应的插补轨迹上时，清除执行区中的控制点{pi,pi+1,…,pi+n-n}，并将{pi+n-n+1,pi+n-n+2,…,pi+n}从执行区首个存储位开始依次填入。

控制点拟合和插补时产生的中间节点可以即用即清，也可以在执行件抵达警戒节点时进行统一清除。

s44.判断暂存区中的有序点集中的控制点是否被全部取出，若否返回s1，若是则进入下一步。

紧接s43的例子，从暂存区中的有序点集中将提出i+n-n个控制点，并依次填入执行区剩余的存储位中，直至暂存区中的有序点集中的控制点被全部取出，此时即完成了执行件沿整个有序点集对应的拟合轨迹的移动。在最后一次提取控制点时，提取的控制点个数可以少于i+n-n个。

从存储量上看，系统的内存是有限的。对于单次轨迹拟合，如果需要拟合的轨迹较长，且轨迹形状多变，比如绣花操作，为了保证拟合形状的近似，需要更多的控制点。如果只进行一次拟合操作，势必要将所有的控制点录入系统的内存中，在进行拟合计算和插补计算时同时还将产生大量的中间数据，无疑对内存容量提出了很高的要求。本方案采用即时计算的方式，将b样条分为若干较小的段落进行计算，单次只需录入较少量的控制点，从而产生较少的中间数据，从而降低了对内存容量的要求。

从计算量上看，系统的算力是有限的。系统除了要进行拟合插补计算，还需要将有限算力分配到各个方面。本方案采用即算即动的方式，单次对较少的控制点进行拟合插补，通过该点而计算出下一点的位置，然后通过下一点的位置和结构算法(底层逻辑)，而得出机械臂各个关节的运动量。对产生的数据进行实时调用，用过即删，从而将计算分散到各个运动的节拍中，从而避免某个时刻占据过多的算力而影响其它器件的工作，从而避免产生机械臂急停急启的现象，减少了机械臂运动发生的磨损。

s5.销毁执行区。

在执行件完成运动后，销毁执行区以释放缓存区的空间。

本申请实施例还基于上述的运动路径规划方法，公开一种运动速度的控制方法，用于控制执行件在拟合轨迹对应的插补轨迹上的移动。

在录入步骤中，系统建立若干个相邻的阈值区间，各个阈值区间分别对应有运行速度，其中，所有的阈值区间组合成正实数域；

举个例子，比如正实数域划分为(0,1]、(1,4]、(4,25]、(25,+∞)，用于对不同线段的平均曲率半径进行分类，使得执行件在不同区间对应的线段上具有不同的运行速度。在这里这四个区间可以分别对应于运行速度1m/s，2m/s，4m/s，10m/s。

参照图7，上述的s3可以分为以下几个子步骤：

s31.基于插补策略将拟合轨迹转化为插补轨迹，将插补计算产生的中间数据写入衍生数据区中；其中插补计算产生的中间数据与控制点相对应。

需要注意的一点是，插补不是对拟合轨迹整体一次性进行插补，而是对于执行件所在的局部曲线进行插补。在后续过程中，将会根据执行件接下来将要经过的拟合轨迹局部线段进行速度规划，并根据速度规划的结果继续进行插补。

s32.将拟合轨迹等间距或变间距或基于划分策略分为若干段有序的局部曲线。

当前的拟合轨迹是基于执行区中控制点所生成的，由于控制点的选取间隔与示教轨迹的曲率有关，因此轨迹平滑且相对平缓。在这里进一步对拟合轨迹进行划分，以使得局部曲线内部各点对应的曲率半径更为接近。这里可以采用均分的方式，使得划分时无须产生过多的中间数据，也无须较多的算力。

可选的，可以采取划分策略对拟合曲线进行处理以划分为多段有序的局部曲线。

参照图8，划分策略包括以下步骤：

步骤一：在输入曲线上等间距地选取若干个分隔点；

步骤二：计算输入曲线在各个分隔点处的曲率半径；

步骤三：判断输入曲线在所有相邻间隔点的曲率半径的比值是否在预设范围内；若否，则对这两个分隔点之间的线段再次基于划分策略进行分段；

步骤四：依次排序输入曲线被间隔点分隔成的线段进行并输出。

举个例子，将拟合轨迹使用划分策略进行处理。在划分策略的步骤一中，对输入曲线进行等间距地选取分隔点，相当于对拟合轨迹进行多点采样。在步骤二和步骤三中，计算拟合轨迹在各相邻分隔点上的曲率半径，如果小于预设范围，比如预设范围为(0.9,1.1)，则说明拟合轨迹的波动较小，分隔点分成的各线段可以作为局部曲线输出。如果超出预设范围，比如拟合轨迹在两相邻分隔点上曲率半径的比值为2，大于预设范围的上限，则说明拟合轨迹的波动较大，需要进一步划分。步骤三中使用迭代的方法将这两个分隔点之间的线段基于划分策略进行进一步划分，直至划分得到的线段均符合要求，则进入步骤四，依次排序输入曲线被间隔点分隔成的线段进行并输出。

通过这种划分策略，能够将拟合轨迹划分为多段各处曲率半径近似的局部线段，以便于后续处理。

s33.基于统计策略计算当前轨迹的平均曲率半径，其中，当前轨迹为执行件当前所处插补轨迹对应的局部曲线；判断当前轨迹的平均曲率半径所处的阈值区间，并调整当前速度为该阈值区间对应的运行速度，其中，当前速度为执行件在所处的插补轨迹上的移动速度；

对当前所处的局部曲线的平均曲率半径进行计算，以判断在该平均曲率半径下对应的最高运行速度。由机械手的结构特点可知，在弯曲程度越小的轨迹上移动时，在保证冲击相对较小的情况下，机械手的移动速度可以越高。

举个例子，比如局部曲线的平均曲率半径为10cm，位于区间(4,25]中，区间内数值的单位为厘米，则该段局部曲线对应的最高运行速度为4m/s。执行件可以以不高于4m/s的速度在该段局部曲线上移动。

参照图9，统计策略包括以下步骤：

在输入曲线上均匀或非均匀地选取采样点；

计算输入曲线在各个采样点上的曲率半径；

计算输入曲线在所有采样点上曲率半径的平均值。

通过统计策略，对输入曲线进行采样，以求得输入曲线在所有采样点上曲率半径的平均值，以表征输入曲线的弯曲程度，便于指导对机械臂移动速度的规划。举个例子，局部曲线上均匀地取得5个采样点，该局部曲线在这5个采样点上的曲率半径分别为9.5cm,10.5cm,10.2cm,9.8cm,10.0cm，则局部曲线对应的平均曲率半径的平均值为10.0cm，对应的最高运行速度为4m/s。这里举例的是平均曲率半径相差不大的采样点，在同一个划分区间使用平均，若平均曲率半径相差较大时，则采用较小运行速度作为对应区间的对应曲率半径。

s34.基于统计策略计算未来轨迹的平均曲率半径，其中，未来轨迹为与当前轨迹相连的下一段局部曲线；判断未来轨迹的平均曲率半径所处的阈值区间，并设置未来速度为该阈值区间对应的运行速度，其中，未来速度为执行件在未来轨迹对应的插补轨迹上的移动速度；

类似的，对未来轨迹对应的平均曲率半径进行计算，并用于指导执行件在未来轨迹上的速度规划。

s35.判断当前速度和未来速度的相对大小，并基于规划策略驱使执行件在对当前轨迹和未来轨迹的交界处从当前速度平滑过渡到未来速度。

由于运动惯性的存在，为避免执行件的运动轴启动、停止以及轨迹转接引起冲击、失步及振荡等现象,必须对执行件在相邻的局部线段的连接处进行加减速控制，在这里就体现为当前轨迹和未来轨迹交界处的速度规划，合理的速度规划不仅能够提高运行精度，而且还能够有效的降低执行件的运行磨损，提高使用寿命。

参照图10，规划策略包括以下步骤：

步骤一：获取两条依次平滑连接的曲线，并作为第一线段和第二线段，并将第一线段和第二线段的交界处附近设置为调速区；

步骤二：获取执行件在第一线段上的计划移动速度和在第二线段上的计划移动速度，并分别作为执行件在调速区上运动的初速度和末速度；

步骤三：基于初速度和末速度对调速区进行匀变速的第一速度规划；

步骤四：对第一速度规划得到的加速度曲线做等积分面积的对称图形替换的第二速度规划。

规划策略的输入为两条平滑连接的线段，并将交界处作为调速区，当这两条线段的计划移动速度相等时，实际上调速区处的速度也等于该计划移动速度。当第一线段和第二线段的计划移动速度不同时，先粗略地将调速区的一端移动到另一端的路程设置为匀变速阶段，第一速度规划可以为匀加速也可以为匀减速，在该段加速度为定值，然后再进行第二速度规划，以使得在初速度变化到末速度的过程中消除加速度突变的现象。加速度的突变在力学上可以看成突然对执行件加力或者减力，容易对执行件造成冲击而产生磨损，甚至导致运动失准。

参照图11，第二速度规划包括以下步骤：

步骤一：获取第一速度规划中的匀变速阶段；

步骤二：采用对称且光滑的加速度曲线替换匀变速阶段的加速度曲线，其中对称且光滑的加速度曲线对时间的积分面积和匀变速阶段的加速度曲线对时间的积分面积大小相等；

步骤三：基于替换后的加速度曲线计算执行件在调速区的加加速度曲线、速度曲线和位移曲线。

对称且光滑的加速度曲线在替换匀变速阶段的加速度曲线后，新的加速度曲线的加加速度可以为固定，也可以为间断的，只要不出现无穷大或近于无穷大的间断点即可。因此此时加速度的曲线为连续可导的光滑曲线，具体表现为执行件的加力和减力为均匀变化，从而保护执行件。

s36.在未来轨迹上等间距或变间距地选取若干检测点，并依次写入检测点集；从检测点集依次读取检测点，并基于局部减速策略在检测点处设置减速区；基于规划策略对减速区和减速区外的部分的交界处进行速度规划。

在该步骤中，对未来轨迹进行的曲率半径进行复检，也可以略去该步骤，不进行复检。由于对局部曲线的划分是基于统计策略来计算平均曲率半径的，因此如果局部出现有速度波动而恰好未被采样到，举个例子，局部曲线为正弦曲线，采样点刚好均位于波峰处，则会被视为整条曲线的曲率不变，也就是采样到一条直线，这显然是不合理的。因此局部曲线将会不断代入未来轨迹中，通过对未来轨迹进行重复取样并基于取样结果决定是否进行速度调控。

参照图12，局部减速策略包括以下步骤：

步骤一：输入检测点和待减速曲线；

步骤二：计算待减速曲线在检测点上的曲率半径，并判断该曲率半径所处的阈值区间，以得到该检测点对应的运行速度；

步骤三：判断该检测点对应的运行速度是否小于待减速曲线的平均曲率半径所处阈值区间对应的运行速度，若是，则该点为减速点，并在减速点前后设置减速区，其中，减速区对应的运行速度为减速点对应的运行速度。

输入检测点和待减速曲线并计算减速曲线在检测点上的曲率半径，这里需要注意的是，在s36中检测点的选取步长应当与前述步骤的点的选取步长更小，以便于发现未来轨迹是否因为之前的统计误差而忽略了需要减速的区域。举个例子，比如待减速曲线对应的平均曲率半径的平均值为10.0cm，对应的最高运行速度为4m/s。但是待减速曲线内一个检测点的曲率半径为3.5cm，低于区间(4,25]的下限值，因此在该点则需要进行减速。待减速曲线在该检测点的曲率半径位于区间(1,4]，因此对应的最高运行速度为2m/s，故减速区对应的运行速度为2m/s。

步骤四：减速区中位于减速点后的检测点移至减速点集；

步骤五：判断减速点集内的检测点对应的运行速度是否小于减速点对应的运行速度，若是，则将对应有更小运行速度的检测点的对应运行速度作为减速区的运行速度，若否则结束。

步骤四和步骤五用于判断是否有连续几个需要减速的检测点同时位于第一个点决定的减速区内。当发现时，则将这几个点中对应的最小的运行速度作为减速区的运行速度，以避免减速度的连续设置或重复设置。

s37.基于上述速度规划，对拟合轨迹进行插补以生成相应的插补轨迹，并驱动执行件在基底材料上按控制点的顺序方向沿插补轨迹移动。

在速度规划完毕后，即可驱动执行件在基底材料上按控制点的顺序方向沿插补轨迹移动。本速度规划方法整体基于拟合-速度规划-插补-运动-继续速度规划-继续插补的逻辑进行，即时规划即时插补即时运动，由于速度规划具有前瞻性，因此不影响执行件在当前轨迹上的移动。

本申请实施例还公开一种运动路径的规划系统，包括：

录入模块，用于在示教样本上有序选取若干个控制点，并作为有序点集录入存储区；

写入模块，用于从暂存区中的有序点集移出一组控制点，并将该组控制点逐点写入执行区的存储位中；

拟合模块，用于对执行区中的各控制点按写入顺序进行b样条拟合以更新拟合轨迹；

插补模块，用于对拟合轨迹进行插补以更新插补轨迹；

移动模块，用于驱动执行件沿插补轨迹移动；

本实施例还公开一种主机，包括存储器和处理器，存储器上存储有能够被处理器加载并上述运动路径规划方法的计算机程序。