CN107783503B - 一种基于双线程的速度预处理算法 - Google Patents

Abstract

本发明公开了一种基于双线程的速度预处理算法，包括以下步骤：步骤1:建立直线或圆弧存储线段的循环队列，将队列中未加工线段的首段作为实时线程开始位置，将队列中未加工线段最后一段作为非实时线程开始位置；步骤2：将速度预处理算法分成实时部分和非实时部分两部分，其中实时部分放在实时线程中与速度规划算法一起运行，非实时部分放在非实时线程中与插入线段程序一起运行。本发明所算法可提高整个算法的实时性，并可减少程序计算量，减少实时线程负担，提高应用软件对应设备的加工效率，并可提高加工质量等问题。满足新的市场不断提高的需求，并可为企业创造出更多的经济效益和社会效益。

Description

一种基于双线程的速度预处理算法

技术领域

本发明涉及一种计算机算法，尤其是指一种基于双线程的速度预处理算法。

背景技术

目前，梯形图速度规划、S型速度规划、正弦速度规划等速度规划算法已广泛应用于数控系统、工业机器人操作作业中。速度规划算法的作用是对一段路径在给定首末速度、路径长度、最大速度、最大加速度以及最大加加速度的情况规划出时间最优速度曲线，且保证给定路径的平滑运动，从而提高运动控制的精度。难点在于首末速度的确定，以保证多段路径衔接点处的平滑过渡。现有技术多采用前瞻算法，且大多数将前瞻算法与速度规划算法一起运行在实时线程中，增加了实时程序的负担。此外，目前的速度规划算法将前瞻算法与规划算法运行在同一实时线程中，增加了实时程序的负担。采用的前瞻算法基本是改变首末速度或段间链接从而改变一段路径长度(段链总长)的方式来确保路径首末速度的可达性。但在连续小线段时，前瞻过程中采用改变首末速度的方式虽能保证锻间链接速度的可达性，但频繁改变首末速度(一般采用搜索法求解)将大量增加程序计算量，加大实时线程负担，且可能降低加工效率，而采用段间链接的方式虽然计算量少，但可能存在线段链接过多，规划速度远大于段间链接速度，从而影响加工质量等问题。故此，本申请人投入较大的研发，致力开发一种基于双线程的速度预处理算法采用双线程技术，将前瞻算法分成两部分，一部分与速度规划算法一起运行在实时线程中，一部分运行在非实时线程中，从而提高整个算法的实时性，以弥补现有技术的不足。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于双线程的速度预处理算法。

本发明解决现有技术的技术方案是：一种基于双线程的速度预处理算法，包括以下步骤：

步骤1:建立直线或圆弧存储线段的循环队列，将队列中未加工线段的首段作为实时线程开始位置，将队列中未加工线段最后一段作为非实时线程开始位置；

步骤2：将速度预处理算法分成实时部分和非实时部分两部分，其中实时部分放在实时线程中与速度规划算法一起运行，非实时部分放在非实时线程中与插入线段程序一起运行。

更具体为：所述速度预处理算法中实时部分实现过程如下：

步骤1：定义Vel等于一较大值(如10的6次方)，且定义队列中未加工线段第一段线段所在段链为cursor，进入步骤2；

步骤2：如果段链cursor中有线段标志last_flag＝1，则Vel＝0。进入步骤3；

步骤3：取plFinalVel为Vel和cursor末端速度finalVel之间的较小值，并判断cursor起始速度initFinal是否大于plFinalVel。如果是，则表明够长(由非实时部分确保)，进入步骤8，否则进入步骤4；

步骤4：测试cursor从起始速度initFinal到末端速度plFinalVel是否够长，如果是，则进入步骤8，否则进入步骤5；

步骤5：判断cursor段链中线段数量，如果大于给定的最大链接数量N，则进入步骤7，否则进入步骤6；

步骤6：定义cursor的前段链为nextCursor，并将cursor与nextCursor链接，返回步骤2；

步骤7：在区间[initFinal,plFinalVel]之间采用二值搜索法搜索速度newVel，使得cursor从起始速度initFinal到末端速度newVel刚好够长；设plFinalVel＝newVel，进入步骤8；

步骤8：设cursor末端速度为plFinalVel，退出；

所述速度预处理算法中非实时部分实现过程如下：

当插入一段新线段(定义为newseg)时，如果队列中只有这一段，则该段首末速度都为0，否则定义其前段链为prevseg。由于newseg和prevseg衔接，其衔接速度将由newseg和prevseg的夹角或曲率计算，设为cornerVel。prevseg末速改变(从0变为cornerVel)，需要测试其首末速是否可达，且需测试newseg首末速是否可达；分别定义函数sqBackLink()函数和sqLastBackLink()函数来处理prevseg首末速可达和newseg首末速可达问题。

再进一步，所述sqBackLink()函数实现过程如下：

步骤1：如果prevseg正在运行，即被实时部分调用，则返回-1，否则进入步骤2；

步骤2：如果prevseg起始速度initVec小于等于末端速度cornerVel，则返回0，否则进入步骤3；步骤3：测试prevseg从起始速度规划到末端速度是否够长。如果够长，则返回0，否则进入步骤4；

步骤4：定义prevseg的前段链为cursor，如果cursor正在运行，则返回-1，否则进入步骤5；

步骤5：测试cursor从起始速度规划到cornerVel是否够长。如果够长，则表明cursor足够长，prevseg不应该链接cursor，进入步骤6，否则表明cursor不够长，将prevseg与cursor链接，返回步骤2；

步骤6：在区间[cornerVel,initVec]采用二值搜索法搜索速度vel,使得prevseg以vel为起始速度，cornerVel为末端速度刚好够长。设vel为prevseg起始速度，且为cursor末端速度，并返回0。

再进一步，所述sqLastBackLink()函数实现过程如下：

步骤1：定义cursor＝newseg；

步骤2：测试从cursor起点速度到newseg末端速度0是否够长。如果够长，则进入步骤4，否则进入步骤3；

步骤3：定义cursor所在段链的前段链为prevseg，如果prevseg正在运行，newseg起始速度设为0，退出，否则令cursor＝prevseg，返回步骤2；

步骤4：设置cursor到newseg之间所有线段为末段标志，即标志位last_flag＝1，并将cursor之前所有标志last_flag为1的线段标志位清零，退出。

本发明所述一种基于双线程的速度预处理算法，将前瞻算法分成两部分，一部分与速度规划算法一起运行在实时线程中，一部分运行在非实时线程中，从而提高整个算法的实时性，并可减少程序计算量，减少实时线程负担，提高应用软件对应设备的加工效率，并可提高加工质量等问题。满足新的市场不断提高的需求，并可为企业创造出更多的经济效益和社会效益。

附图说明

图1是实施例1循环队列示意图；

图2是实施例1非实时部分实现过程图；

图3是实施例1sqBackLink函数实现流程图；

图4是实施例1sqLastBackLink函数实现流程图；

图5是实施例1实时部分实现流程图。

具体实施方式

实施例1

一种基于双线程的速度预处理算法，其方法包括以下步骤：

如图1所示，包括以下步骤：

步骤1:建立存储线段(所述存储线段可以为直线或圆弧)的循环队列，将队列中未加工线段的首段作为实时线程开始位置，将队列中未加工线段最后一段作为非实时线程开始位置；

步骤2：将速度预处理算法分成实时部分和非实时部分两部分，其中实时部分放在实时线程中与速度规划算法一起运行，非实时部分放在非实时线程中与插入线段程序一起运行；

如图5所示，所述速度预处理算法中实时部分实现过程如下：

步骤1：定义Vel等于一较大值(如10的6次方)，且定义队列中未加工线段第一段线段所在段链为cursor，进入步骤2；

步骤2：如果段链cursor中有线段标志last_flag＝1，则Vel＝0。进入步骤3；

步骤3：取plFinalVel为Vel和cursor末端速度finalVel之间的较小值，并判断cursor起始速度initFinal是否大于plFinalVel。如果是，则表明够长(由非实时部分确保)，进入步骤8，否则进入步骤4；

步骤4：测试cursor从起始速度initFinal到末端速度plFinalVel是否够长，如果是，则进入步骤8，否则进入步骤5；

步骤5：判断cursor段链中线段数量，如果大于给定的最大链接数量N，则进入步骤7，否则进入步骤6；

步骤6：定义cursor的前段链为nextCursor(图1所示从左向右看)，并将cursor与nextCursor链接，返回步骤2；

步骤7：在区间[initFinal,plFinalVel]之间采用二值搜索法搜索速度newVel，使得cursor从起始速度initFinal到末端速度newVel刚好够长；设plFinalVel＝newVel，进入步骤8；

步骤8：设cursor末端速度为plFinalVel，退出。

如图2所示所述速度预处理算法中非实时部分实现过程如下：

当插入一段新线段(定义为newseg)时，如果队列中只有这一段，则该段首末速度都为0，否则定义其前段链为prevseg(图1所示从右往左看)。由于newseg和prevseg衔接，其衔接速度将由newseg和prevseg的夹角或曲率计算，设为cornerVel。prevseg末速改变(从0变为cornerVel)，需要测试其首末速是否可达，且需测试newseg首末速是否可达；分别定义函数sqBackLink()函数和sqLastBackLink()函数来处理prevseg首末速可达和newseg首末速可达问题。

再进一步，如图3所示，所述sqBackLink()函数实现过程如下：

步骤1：如果prevseg正在运行，即被实时部分调用，则返回-1，否则进入步骤2；

步骤2：如果prevseg起始速度initVec小于等于末端速度cornerVel，则返回0，否则进入步骤3；步骤3：测试prevseg从起始速度规划到末端速度是否够长。如果够长，则返回0，否则进入步骤4；

步骤4：定义prevseg的前段链为cursor(图1所示从右往左看)，如果cursor正在运行，则返回-1，否则进入步骤5；

步骤5：测试cursor从起始速度规划到cornerVel是否够长。如果够长，则表明cursor足够长，prevseg不应该链接cursor，进入步骤6，否则表明cursor不够长，将prevseg与cursor链接，返回步骤2；

步骤6：在区间[cornerVel,initVec]采用二值搜索法搜索速度vel,使得prevseg以vel为起始速度，cornerVel为末端速度刚好够长。设vel为prevseg起始速度，且为cursor末端速度，并返回0。

再进一步，如图4所示，所述sqLastBackLink()函数实现过程如下：

步骤1：定义cursor＝newseg；

步骤2：测试从cursor起点速度到newseg末端速度0是否够长。如果够长，则进入步骤4，否则进入步骤3；

步骤3：定义cursor所在段链的前段链为prevseg(图1所示从右往左看)，如果prevseg正在运行，newseg起始速度设为0，退出，否则令cursor＝prevseg，返回步骤2；

步骤4：设置cursor到newseg之间所有线段为末段标志，即标志位last_flag＝1，并将cursor之前所有标志last_flag为1的线段标志位清零，退出。

以上所述，仅是本发明的较佳实施例而已，并非对本发明的结构作任何形式上的限制。凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明的技术方案的范围内。

Claims (4)

1.一种基于双线程的速度预处理算法，其特征在于:包括以下步骤：

步骤1:建立直线或圆弧存储线段的循环队列，将队列中未加工线段的首段作为实时线程开始位置，将队列中未加工线段最后一段作为非实时线程开始位置；

步骤2：将速度预处理算法分成实时部分和非实时部分两部分，其中实时部分放在实时线程中与速度规划算法一起运行，非实时部分放在非实时线程中与插入线段程序一起运行；

所述速度预处理算法中实时部分实现过程如下：

步骤1：定义Vel等于一较大值，且定义队列中未加工线段第一段线段所在段链为cursor，进入步骤2；

步骤2：如果段链cursor中有线段标志last_flag＝1，则Vel＝0，进入步骤3；

步骤3：取plFinalVel为Vel和cursor末端速度finalVel之间的较小值，并判断cursor起始速度initFinal是否大于plFinalVel，如果是，则表明够长，进入步骤8，否则进入步骤4；

步骤4：测试cursor从起始速度initFinal到末端速度plFinalVel是否够长，如果是，则进入步骤8，否则进入步骤5；

步骤5：判断cursor段链中线段数量，如果大于给定的最大链接数量N，则进入步骤7，否则进入步骤6；

步骤6：定义cursor的前段链为nextCursor，并将cursor与nextCursor链接，返回步骤2；

步骤7：在区间[initFinal,plFinalVel]之间采用二值搜索法搜索速度newVel，使得cursor从起始速度initFinal到末端速度newVel刚好够长；设plFinalVel＝newVel，进入步骤8；

步骤8：设cursor末端速度为plFinalVel，退出；

所述速度预处理算法中非实时部分实现过程如下：

当插入一段新线段并定义为newseg时，如果队列中只有这一段，则该段首末速度都为0，否则定义其前段链为prevseg，由于newseg和prevseg衔接，其衔接速度将由newseg和prevseg的夹角或曲率计算，设为cornerVel，prevseg末速改变，从0变为cornerVel，需要测试其首末速是否可达，且需测试newseg首末速是否可达；分别定义函数sqBackLink()函数和sqLastBackLink()函数来处理prevseg首末速可达和newseg首末速可达问题。

2.根据权利要求1所述一种基于双线程的速度预处理算法，其特征在于：所述Vel定义为10的6次方。

3.根据权利要求1所述一种基于双线程的速度预处理算法，其特征在于：所述sqBackLink()函数实现过程如下：

步骤1：如果prevseg正在运行，即被实时部分调用，则返回-1，否则进入步骤2；

步骤2：如果prevseg起始速度initVec小于等于末端速度cornerVel，则返回0，否则进入步骤3；步骤3：测试prevseg从起始速度规划到末端速度是否够长，如果够长，则返回0，否则进入步骤4；

步骤4：定义prevseg的前段链为cursor，如果cursor正在运行，则返回-1，否则进入步骤5；

步骤5：测试cursor从起始速度规划到cornerVel是否够长，如果够长，则表明cursor足够长，prevseg不应该链接cursor，进入步骤6，否则表明cursor不够长，将prevseg与cursor链接，返回步骤2；

步骤6：在区间[cornerVel,initVec]采用二值搜索法搜索速度vel,使得prevseg以vel为起始速度，cornerVel为末端速度刚好够长，设vel为prevseg起始速度，且为cursor末端速度，并返回0。

4.根据权利要求1所述一种基于双线程的速度预处理算法，其特征在于：所述sqLastBackLink()函数实现过程如下：

步骤1：定义cursor＝newseg；

步骤2：测试从cursor起点速度到newseg末端速度0是否够长，如果够长，则进入步骤4，否则进入步骤3；

步骤3：定义cursor所在段链的前段链为prevseg，如果prevseg正在运行，newseg起始速度设为0，退出，否则令cursor＝prevseg，返回步骤2；

步骤4：设置cursor到newseg之间所有线段为末段标志，即标志位last_flag＝1，并将cursor之前所有标志last_flag为1的线段标志位清零，退出。

Images