[发明专利]一种基于双线程的速度预处理算法

权利要求书

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的线段标志位清零，退出。