一种局部减速控制方法、运动速度的规划方法、运动路径的规划方法和数据更新方法

摘要

本申请涉及一种局部减速控制方法、运动速度的规划方法、运动路径的规划方法和数据更新方法，该局部减速控制方法包括以下步骤：建立若干个分别对应有运行速度的相邻阈值区间；获取待减速曲线及其上的检测点；获取减速曲线在检测点上的曲率半径，并判断该曲率半径所处的阈值区间，以得到检测点对应的运行速度；基于检测点与待减速曲线对应的运行速度的相对大小从检测点中获得减速点；在减速点前后设置对应于减速点运行速度的减速区；建立对应于各个减速区的减速点集并写入减速区中的检测点；获取减速点集内各检测点对应的运行速度的最小值并作为减速区的运行速度。本申请具有让执行件在未预及的陡峭轨迹段减速以减少磨损的优点。

说明书

本发明是分案申请，分案申请基础是申请号为2020111146905、申请日为2020.10.16、发明名称是“一种运动速度的控制方法、系统、主机及存储介质”的发明申请案。

技术领域

本申请涉及自动化控制的领域，尤其是涉及一种局部减速控制方法、运动速度的规划方法、运动路径的规划方法和数据更新方法。

背景技术

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

通过动作示教，机械手可以按照固定的程序进行运动。不论运动轨迹简单还是复杂，都需要通过直线插补、圆弧插补或关节插补等方法，控制机器人完成轨迹运动。显然，教导的点位越多，机械手的运动轨迹精度越高。但点位越多，也就意味着线段越多，就需要频繁的进行加减速，以便完成线段之间的过渡，从而造成整体效率降低。目前，相关技术提出使用B样条对一次性将控制点拟合成连续的曲线，再在B样条拟合曲线上进行插补，从而实现平滑过渡。

但是发明人认为，在B样条拟合曲线上依然会存在曲率的变化，虽然可以在拟合曲线上均匀获取采样点来计算采样点处的曲率半径，以指导机械手的加减速，但是对于复杂图案而言，常会在短距离内存有多个波峰波谷，如果它们没有恰好被采样到，将会发生数据上的“抹平”，而无法进行相应的速度控制，容易造成机械手的加速磨损，影响使用寿命。

发明内容

为了让执行件在未预及的陡峭轨迹段减速，以降低执行件突然受力而加剧磨损的风险，本申请提供一种局部减速控制方法、运动速度的规划方法、运动路径的规划方法和数据更新方法。

第一方面，本申请提供的一种用于速度规划的局部减速控制方法，采用如下的技术方案：

一种用于速度规划的局部减速控制方法，包括以下步骤：

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

获取待减速曲线和位于待减速曲线上的检测点，其中，检测点的点集不含于待减速曲线的采样点的点集；

获取减速曲线在检测点上的曲率半径，并判断该曲率半径所处的阈值区间，以得到检测点对应的运行速度；

基于检测点对应的运行速度与待减速曲线的平均曲率半径所处阈值区间对应的运行速度的相对大小从检测点中获得减速点；

在减速点前后设置减速区，其中，减速区对应的运行速度为减速点对应的运行速度；

建立对应于各个减速区的减速点集，并将减速区中的检测点写入减速点集；

获取减速点集内各检测点对应的运行速度的最小值，并将该最小运行速度作为减速区的运行速度。

通过采用上述技术方案，选定需要减速的曲线，并在曲线上选取检测点，检测点用于计算曲线上未被采样过的点位的曲率半径。由于待减速曲线的曲率半径是由所有采样点处曲线的曲率半径平均而得，因此检测点的点集不为采样点的子集，则能够使得检测点避开采样点。

在实际工况中，曲线的波动常常是包括多个波峰波谷的一段，而罕见突兀的单个尖峰，因此当检测到减速点时，为了节约算力，本方案选择直接在减速点的前后设置减速区，而不是在减速点的前后再以更高的密度设置检测点。在有连续几个需要减速的检测点同时位于减速区内时，由于减速区范围较小，在减速区内进行频繁的加减速也会对机械臂造成磨损，因此采用各个检测点对应的最低运行速度作为减速区的统一运行速度，反而能够提高运行效率，避免减速度的连续设置和重复设置，节约算力，且同时对机械臂起到更好的保护效果。

可选的，检测点的选取步长小于采样点的选取步长。

通过采用上述技术方案，由于待减速曲线的局部平均曲率是基于采样点计算所得，因此选取不同步长的检测点与采样点，以使得检测点能够避开采样点以获得新的数据，防止数据重合而无法找出夹在两个采样点之间的波动。

可选的，所述待减速曲线在检测点上的曲率统计方法包括以下步骤：

在待减速曲线上均匀或非均匀地选取采样点；

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

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

通过采用上述技术方案，对待减速曲线进行多点采样，求得输入曲线在所有采样点上曲率半径的平均值，以表征输入曲线的弯曲程度，便于指导对机械臂移动速度的规划。

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

一种运动速度的规划方法，包括以下步骤：

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

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

基于统计策略计算未来轨迹的平均曲率半径，其中，当前轨迹为执行件当前位置对应的局部曲线，未来轨迹为与当前轨迹相连的下一段局部曲线；

获取未来轨迹的平均曲率半径所处的阈值区间，并将该阈值区间对应的运行速度作为未来速度，其中，未来速度为执行件在未来轨迹对应的插补轨迹上的移动速度；

在未来轨迹上等间距或变间距地选取若干检测点，并依次写入检测点集；

从检测点集依次读取检测点，并基于上述的局部减速控制方法在检测点处设置减速区；

基于规划策略对减速区和减速区外的部分的交界处进行速度规划。

通过采用上述技术方案，将正实数域划分为若干区间，用于对不同局部曲线的平均曲率半径进行分类，使得执行件在不同区间对应的线段上具有不同的运行速度。平滑的拟合轨迹可以为B样条曲线，或其它平滑的曲线等，以防止执行件在曲线上的某个节点发生速度突变。在这里对拟合轨迹进行切分若干较短的局部曲线，以使得局部曲线内部各点对应的曲率半径更为接近。对当前轨迹和未来轨迹的平均曲率半径进行计算，以判断在该平均曲率半径下对应的最高运行速度，以指导执行件在该轨迹上的速度规划。由机械手的结构特点可知，在弯曲程度越小的轨迹上移动时，在保证磨损相对较小的情况下，机械手的移动速度可以越高。由于运动惯性的存在，为避免执行件的运动轴启动、停止以及轨迹转接引起冲击、失步及振荡等现象,必须对执行件在相邻的局部线段的连接处进行加减速控制，在这里就体现为减速区和减速区外的部分的交界处的速度规划，合理的速度规划不仅能够提高运行精度，而且还能够有效的降低执行件的运行磨损，提高使用寿命。

可选的，还包括以下步骤：

基于统计策略计算当前轨迹的平均曲率半径；

获取当前轨迹的平均曲率半径所处的阈值区间，并将该阈值区间对应的运行速度作为当前速度，其中，当前速度为执行件在所处的插补轨迹上的移动速度；

基于规划策略驱使执行件在对当前轨迹和未来轨迹的交界处从当前速度平滑过渡到未来速度。

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

可选的，所述划分策略包括以下步骤：

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

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

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

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

通过采用上述技术方案，将拟合轨迹使用划分策略进行处理。对输入曲线进行等间距地选取分隔点，相当于对拟合轨迹进行多点采样。再计算拟合轨迹在各相邻分隔点上的曲率半径，如果在预设范围之内，则说明拟合轨迹的波动较小，分隔点分成的各线段可以作为局部曲线输出。如果超出预设范围，则说明拟合轨迹的波动较大，需要进一步划分。再使用迭代的方法将这两个分隔点之间的线段基于划分策略进行进一步划分，直至划分得到的线段均符合要求，则依次排序输入曲线被间隔点分隔成的线段进行并输出。通过这种划分策略，能够将拟合轨迹划分为多段各处曲率半径近似的局部线段，以便于后续处理。

可选的，所述规划策略包括以下步骤：

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

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

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

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

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

可选的，所述第二速度规划包括以下步骤：

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

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

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

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

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

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

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

在缓存区中建立执行区和暂存区；其中，执行区的读写速度＞暂存区的读写速度＞存储区的读写速度；

在执行区中建立有序的存储位；

获取存储区中的有序点集并基于调整策略录入暂存区；

将暂存区内的控制点按点序分组并逐组移动到执行区中，其中各组控制点的控制点数量小于等于存储位的数量；

对执行区内各控制点按点序进行B样条拟合，以产生拟合轨迹；

基于插补策略和由上述的运动速度的规划方法对拟合轨迹进行插补以产生插补轨迹。

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

在第一次拟合之前，先从存储区中申请有序点集并录入暂存区中。在第一次拟合开始时，定量或者不定量地从暂存区中的有序点集的头部申请一组控制点并写入执行区中，同时删除暂存区中的有序点集中被复制的控制点。然后按照写入顺序对执行区中控制点进行拟合。拟合过程中基于未来曲线对进行速度规划，并基于规划的速度对未来曲线进行插补，最后再控制执行件沿插补轨迹移动。

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

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

可选的，还包括以下步骤：

在缓存区中建立衍生数据区；

将拟合计算产生的中间数据写入衍生数据区；

将插补计算产生的中间数据写入衍生数据区。

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

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

在示教轨迹上均匀或非均匀地选取控制点；

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

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

可选的，所述在示教轨迹上选取控制点的步骤包括：

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

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

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

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

复制暂存区中有序点集的首位为至少k个重合点并并入暂存区内有序点集的头部；

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

其中，k为B样条拟合的阶数。

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

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

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

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

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

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

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

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

第四方面，本申请提供的一种基于执行件实时运动状态的数据更新方法，采用如下的技术方案：

一种基于执行件实时运动状态的数据更新方法，用于上述的运动路径的规划方法，包括以下步骤：

驱动执行件沿已有的插补轨迹移动；

实时监控执行件在插补轨迹上的位置以计算余点值，其中，以执行件在插补轨迹所经过的部分对应的控制点的数量为经点值，余点值为执行区中控制点的数量与经点值之差；

基于余点值与警戒值的相对大小，清除执行区中位于警戒节点前的控制点并将警戒节点后的控制点按原有顺序迁移至警戒节点前；其中，警戒节点为存储位序列中部的一个存储位，警戒值为警戒节点在执行区中的存储位序次；

按序取出暂存区中的控制点，并按序写入执行区中原有控制点之后的存储位中。

通过采用上述技术方案，随着执行件在插补轨迹上的移动，拟合轨迹相对于执行件未被经过的部分对应的控制点逐渐减少。当余点值小于警戒值时，清除执行区中警戒节点前的控制点数据，并将暂存区中有序点集的头部部分按序移动到执行区中位于警戒节点前的部分，且暂存区中被移出的控制点数据被清除，然后基于执行区中的控制点数据开始下一次拟合。

在各次拟合过程中均采用本数据更新方法，直至暂存区中的有序点集中的控制点被全部取出，即完成了执行件沿整条拟合轨迹的移动，最后销毁执行区并删除多余的中间数据。

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

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

可选的，所述警戒节点为执行区中序次大于B样条阶数的存储位，所述警戒值为一个小于存储位数量且大于等于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平面图像。

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

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

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

平均曲率半径高的说明其整体较为平缓，需要的控制点更少；平均曲率半径低的说明其整体较为弯曲，需要的控制点更多，才能更好地表现出示教轨迹的形状特征。举个例子，示教轨迹

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

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

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

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

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

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

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

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

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

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

参照图4，执行步骤至少包括以下五个步骤S1-S5，分别为：

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

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

举个例子，暂存区中的有序点集P＝{P

S2.对执行区中的各点按写入顺序进行B样条拟合，以更新拟合轨迹，并将拟合计算产生的中间数据写入衍生数据区。B样条曲线曲面具有几何不变性、凸包性、保凸性、变差减小性、局部支撑性等许多优良性质。举个例子，已知n+1个控制点P

其中N

这里规定

显然，基函数由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＝v

令Δt＝1，那么Δl＝v

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

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

对指引节点p

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

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

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

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

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

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

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

举个例子，机械臂末端位于控制点集{p

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

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样条拟合以更新拟合轨迹；

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

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

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

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

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

以上均为本申请的较佳实施例，并非依此限制本申请的保护范围，故：凡依本申请的结构、形状、原理所做的等效变化，均应涵盖于本申请的保护范围之内。