CN112965443A - 一种裁床拐角轨迹跟踪高精度插值控制方法 - Google Patents

Abstract

本发明公开了一种裁床拐角轨迹跟踪高精度插值控制方法，对60°～120°、120°～180°的拐角进行轨迹优化，将闭合曲线的拐角均优化至0°～60°以内后再进行拐点的速度规划，克服了传统拐角采用圆弧曲线过渡及圆弧插补的复杂性，同时又保留了圆弧过渡的平滑性，提供了一种小线段逼近圆弧的过渡方法，对拐角进行高精度插值，能够使裁床加工路径中拐角衔接处的速度更为平滑，提高裁床的加工效率。

Description

一种裁床拐角轨迹跟踪高精度插值控制方法

技术领域

本申请属于数控机床运动控制技术领域，具体涉及一种裁床拐角轨迹跟踪高精度插值控制方法。

背景技术

在裁床的运动控制中，拐角速度在裁床的稳定性及加工效率中起着关键性的作用，拐角速度过快，则会造成过切、机械抖动等问题，拐角速度过慢则会限制裁床的加工效率。本申请针对不同的拐角大小以及拐点前后线段长度等因素对拐角轨迹跟踪进行高精度插值，并对插值后的拐角进行速度规划，能够有效的提高裁床的稳定性及加工效率。

发明内容

本申请的目的在于提供一种裁床拐角轨迹跟踪高精度插值控制方法，使裁床加工路径中拐角衔接处的速度更为平滑，提高裁床的加工效率。

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

一种裁床拐角轨迹跟踪高精度插值控制方法，所述裁床拐角轨迹跟踪高精度插值控制方法，包括：

步骤1、遍历切割文件中闭合曲线的各坐标点，求出各坐标点所组成的线段的斜率，根据斜率计算出相邻线段沿速度变化方向的夹角作为拐角；

步骤2、将各相邻线段组成的拐角进行分类，分成0°～60°、60°～120°、120°～180°三类拐角；

步骤3、将60°～120°的拐角对应的两条相邻线段优化成四条连续的线段：首先在两条相邻线段之间建立圆弧过渡模型，则得到拐角插值点依次为：圆弧接入点、圆弧中点、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算三个拐角插值点的坐标，并利用计算得到的三个拐角插值点代替原来的两条相邻线段的交点，得到优化后的四条连续的线段，此时新的相邻两条线段的拐角范围为0°～60°；

步骤4、将120°～180°的拐角对应的两条相邻线段优化成六条连续的线段：首先在两条相邻线段之间建立圆弧过渡模型，取均分圆弧四等分的三个点作为圆弧插入点，则得到拐角插值点依次为：圆弧接入点、圆弧插入点1、圆弧插入点2、圆弧插入点3、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算五个拐角插值点的坐标，并利用计算得到的五个拐角插值点代替原来的两条相邻线段的交点，得到优化后的六条连续的线段，此时新的相邻两条线段的拐角范围为0°～60°；

步骤5、待所有60°～120°、120°～180°的拐角均优化完成后，重新遍历更新后的切割文件中闭合曲线的各坐标点，根据拐角的大小及轴速度的方向对各坐标点的速度进行规划，对各坐标点组成的线段进行直线插补及加减速规划；

其中，根据拐角的大小及轴速度的方向对各坐标点的速度进行规划如下：

(1)若坐标点P_i处X轴或/和Y轴的速度方向反向改变，则坐标点P_i的速度V_i为：

式中A_max为最大加速度，T为插补周期，I_i,x、I_i,y为线段P_i-1P_i的方向矢量，P_i为切割文件中的第i个坐标点，x_i、y_i为坐标点P_i的坐标；

(2)若坐标点P_i处X轴和Y轴的速度方向均不变，则坐标点P_i的速度V_i为：

式中，θ为坐标点P_i所在位置对应的拐角的角度。

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

作为优选，所述得到拐角插值点依次为：圆弧接入点、圆弧中点、圆弧转出点，根据预设的弓高误差以及原来的两条相邻线段上的三个坐标点，计算三个拐角插值点的坐标，包括：

已知原来的两条相邻线段上的三个坐标点为P_i-1、P_i、P_i+1，其中P_i为拐点，E_i为弓高误差，θ为拐角，可算出圆弧的过渡距离L为：

取

其中S_i-1、S_i+1为原来的两条相邻线段的长度，则计算三个拐角插值点的坐标的过程如下：

(1)当L_m≥L时，此时E_i取预设的系统最大弓高误差，则R的值以及P_a、P_b、P_c三点坐标为：

(2)当L_m＜L时，此时E_i受L_m的影响，则R、E_i的值以及P_a、P_b、P_c三点坐标为：

式中，R为圆弧的半径，P_a为圆弧接入点，P_b为圆弧中点，P_c为圆弧转出点。

作为优选，所述得到拐角插值点依次为：圆弧接入点、圆弧插入点1、圆弧插入点2、圆弧插入点3、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算五个拐角插值点的坐标，包括：

已知原来的两条相邻线段上的三个坐标点为P_i-1、P_i、P_i+1，其中P_i为拐点，E_i为弓高误差，θ为拐角，可算出圆弧的过渡距离L为：

取

其中S_i-1、S_i+1为原来的两条相邻线段的长度，则计算五个拐角插值点的坐标的过程如下：

(1)当L_m≥L时，此时E_i取预设的系统最大弓高误差，则R的值以及P_a、P_b、P_c、P_d、P_e五点坐标为：

(2)当L_m＜L时，此时R、E_i受L_m的影响，则R、E_i的值以及P_a、P_b、P_c、P_d、P_e五点坐标为：

式中，P_a为圆弧接入点，P_b为圆弧插入点1，P_c为圆弧插入点2，P_d为圆弧插入点3，P_e为圆弧转出点，Q₀、Q₁分别为P_i-1P_i、P_iP_i+1上的坐标点，Q₀ Q₁过点P_c且平行于P_aP_e。

作为优选，所述加减速规划，包括：

取各坐标点组成的线段，对线段的长度小于阈值的线段采用T型加减速进行速度规划，对线段的长度大于等于阈值的线段采用S型加减速进行速度规划。

作为优选，所述采用T型加减速进行速度规划，包括：

假设坐标点P_i的速度为V_i，坐标点P_i+1的速度为V_i+1，且V_i<V_i+1，线段P_iP_i+1的长度为L_i，则速度规划如下：

确定V_i在速度表中的位置Pos_i，取加速度表中Pos_i位置的值与设定的加速度值的乘积即为速度变化量ΔV，以ΔV进行定周期加速得到实时速度V_c，ΔV与插补周期T的乘积即为一个周期内的插补步长，在每个周期内判断V_c在速度表中的位置，并更新速度变化值ΔV、实时速度V_c、累积插补步长L_sum，根据V_i+1及L_i的值确定加速距离；根据加速距离和L_i判断是否存在匀速、减速，如果存在减速，则继续确定是否包含匀速，根据是否存在匀速、减速确定减速位置，当累积插补的步长L_sum达到线段的长度L_i时，则此线段的加减速规划及插补完成。

本申请提供的裁床拐角轨迹跟踪高精度插值控制方法，克服了传统拐角采用圆弧曲线过渡及圆弧插补的复杂性，同时又保留了圆弧过渡的平滑性，提供了一种小线段逼近圆弧的过渡方法，对拐角进行高精度插值，能够使裁床加工路径中拐角衔接处的速度更为平滑，提高裁床的加工效率。

附图说明

图1为本申请的裁床拐角轨迹跟踪高精度插值控制方法的流程图；

图2为本申请计算拐角的一种实施例示意图；

图3为本申请针对60°～120°的拐角优化的一种实施例示意图；

图4为本申请拐角的过渡距离在公共小线段上不重叠的示意图；

图5为本申请拐角的过渡距离在公共小线段上重叠的示意图；

图6为本申请针对120°～180°的拐角优化的一种实施例示意图。

具体实施方式

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

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

如图1所示，本实施例提供一种裁床拐角轨迹跟踪高精度插值控制方法，包括以下步骤：

步骤1、遍历切割文件中闭合曲线的各坐标点，求出各坐标点所组成的线段的斜率，根据斜率计算出相邻线段沿速度变化方向的夹角作为拐角。其中切割文件为裁床运动控制中的已知文件，记录裁床运动轨迹等信息。

如图2所示，已知切割文件中任意连续三点坐标P₁(x₁,y₁)，P₂(x₂,y₂)，P₃(x₃,y₃)，则求解相邻线段速度变化方向的夹角θ如下：

θ＝acos(cosα*cosβ+sinα*sinβ)*(180/π)

步骤2、将各相邻线段组成的拐角进行分类，分成0°～60°、60°～120°、120°～180°三类拐角。

步骤3、将60°～120°的拐角对应的两条相邻线段优化成四条连续的线段：首先在两条相邻线段之间建立圆弧过渡模型，则得到拐角插值点依次为：圆弧接入点、圆弧中点、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算三个拐角插值点的坐标，并利用计算得到的三个拐角插值点代替原来的两条相邻线段的交点，得到优化后的四条连续的线段，此时新的相邻两条线段的拐角范围为0°～60°。

具体计算过程如图3所示，在建立圆弧过渡模型时必须满足约束条件为：拐点的前一段线段的过渡距离L_i-1等于拐点后一段小线段的过渡距离L_i+1。

则已知原来的两条相邻线段上的三个坐标点为P_i-1、P_i、P_i+1，其中P_i为拐点，E_i为弓高误差，θ为拐角，可算出圆弧的过渡距离L为：

因为△P_aP_iP_c与△P_aOP_c都为等腰三角形，所以拐角θ等于

又因为：

取过渡阈值

其中S_i-1、S_i+1为原来的两条相邻线段(线段P_i-1P_i，线段P_iP_i+1)的长度，由于过渡距离L受到弓高误差和小线段自身长度的约束，当弓高误差过大时，在切割文件中有些相邻拐点之间存在特别短的公共小线段，会造成两拐角的过渡距离在公共小线段上形成重叠现象。针对这种现象，弓高误差则通过相邻线段的长度做出限定，避免重叠现象的发生。则计算三个拐角插值点的坐标的过程分为如下两种情况：

(1)当L_m≥L时，说明不会发生重叠现象，如图4所示，此时E_i取预设的系统最大弓高误差，则R的值以及P_a、P_b、P_c三点坐标为：

(2)当L_m＜L时，说明发生重叠现象，如图5所示，ΔL为重叠部分，此时E_i受L_m的影响，则R、E_i的值以及P_a、P_b、P_c三点坐标为：

式中，R为圆弧的半径，P_a为圆弧接入点，P_b为圆弧中点，P_c为圆弧转出点。

需要说明的是，上述公式是为了计算坐标点，但是为了便于表示将坐标点采用其对于的位置点进行替代表示，即式中的P_a实质上为(x_a,y_a)，P_i实质上为(x_i,y_i)，其余的P_b，P_c，P_i+1，P_i-1同理理解。

经过上述优化在进行速度规划时，降低了拐点夹角大小突变时对拐点速度的影响，使各拐点的速度不会产生较大的突变，从而使加工速度在拐角处平滑过渡。相对于传统的圆弧过渡，保留了其速度过渡的平滑性，同时极大的降低了圆弧插补的复杂性。

步骤4、将120°～180°的拐角对应的两条相邻线段优化成六条连续的线段：首先在两条相邻线段之间建立圆弧过渡模型，取均分圆弧四等分的三个点作为圆弧插入点，则得到拐角插值点依次为：圆弧接入点、圆弧插入点1、圆弧插入点2、圆弧插入点3、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算五个拐角插值点的坐标，并利用计算得到的五个拐角插值点代替原来的两条相邻线段的交点，得到优化后的六条连续的线段，此时新的相邻两条线段的拐角范围为0°～60°。

如图6所示，其具体计算过程如下：已知原来的两条相邻线段上的三个坐标点为P_i-1、P_i、P_i+1，其中P_i为拐点，E_i为弓高误差，θ为拐角，可算出圆弧的过渡距离L为：

取

其中S_i-1、S_i+1为原来的两条相邻线段的长度，则计算五个拐角插值点的坐标的过程如下：

(1)当L_m≥L时，，说明不会发生重叠现象此时E_i取预设的系统最大弓高误差，则R的值以及P_a、P_b、P_c、P_d、P_e五点坐标为：

(2)当L_m＜L时，说明会发生重叠现象，此时R、E_i受L_m的影响，则R、E_i的值以及P_a、P_b、P_c、P_d、P_e五点坐标为：

式中，P_a为圆弧接入点，P_b为圆弧插入点1，P_c为圆弧插入点2，P_d为圆弧插入点3，P_e为圆弧转出点，Q₀、Q₁分别为P_i-1P_i、P_iP_i+1上的坐标点，Q₀ Q₁过点P_c且平行于P_aP_e。

进行优化后的拐角过渡被优化成逼近圆弧的六段连续的线段，且线段间的拐角范围均为0°～60°，在进行速度规划时，降低了拐点夹角大小突变时对拐点速度的影响，使各拐点的速度不会产生较大的突变，从而使加工速度在拐角处平滑过渡。相对于传统的圆弧过渡，保留了其速度过渡的平滑性，同时极大的降低了圆弧插补的复杂性。

步骤5、待所有60°～120°、120°～180°的拐角均优化完成后，重新遍历更新后的切割文件中闭合曲线的各坐标点，根据拐角的大小及轴速度的方向对各坐标点的速度进行规划，对各坐标点组成的线段进行直线插补及加减速规划；

其中，根据拐角的大小及轴速度的方向对各坐标点的速度进行规划如下：

(1)若坐标点P_i处X轴或/和Y轴的速度方向反向改变(X轴和Y轴为裁床中需要速度规划的轴，但不限定裁床仅含有X轴和Y轴，速度方向反向改变指单X轴的速度方向反向改变，或单Y轴的速度方向反向改变，或X轴和Y轴的速度方向均反向改变均视为速度方向改变。其中坐标点P_i处是否速度方向改变可以根据坐标点P_i-1、P_i、P_i+1三个点计算线段斜率，根据斜率判断方向是否改变)，则坐标点P_i的速度V_i为：

式中A_max为最大加速度，T为插补周期，I_i,x、I_i,y为线段P_i-1P_i的方向矢量，P_i为切割文件中的第i个坐标点，x_i、y_i为坐标点P_i的坐标，I_i+1,x、I_i+1,y为线段P_iP_i+1的方向矢量，P_i-1为切割文件中的第i-1个坐标点，x_i-1、y_i-1为坐标点P_i-1的坐标，P_i+1为切割文件中的第i+1个坐标点，x_i+1、y_i+1为坐标点P_i+1的坐标；

(2)若坐标点P_i处X轴和Y轴的速度方向均不变，则坐标点P_i的速度V_i为：

式中，θ为坐标点P_i所在位置对应的拐角的角度。

考虑到步骤3、4对夹角的优化可能会形成特别短的小线段，尤其是优化较大的拐角时，因此再对线段轨迹进行直线插补及加减速规划时，取各坐标点组成的线段，对线段的长度小于阈值(例如小于20个脉冲)的线段(小线段)采用T型加减速进行速度规划，能够减小加减速距离；对线段的长度大于等于阈值的线段采用S型加减速进行速度规划，从而有效的提升了系统的稳定性及加工效率。

其中T型加减速进行速度规划的过程如下：

1、在Matlab仿真出T型加速度曲线、T型速度曲线。

2、将速度曲线离散成n段，构成加速度值a_i及对应的加速长度Len_i，其中0≤i≤n，对应的a_i及加速长度Len_i即为加速度表、加速长度表，加速度值与对应加速长度、用户设定的加速度的乘积即为当前段以A_i加速Len_i长度的速度变化值ΔV。

3、根据设定的起始速度V_s、系统最大速度V_max、加速度表、加速长度表、用户设定的加速度Acc，计算速度表：如速度表中第一阶速度V₀＝V_s+a₀*Len₀*Acc，第二阶速度V₁＝V₀+a₁*Len₁*Acc，以此类推，并限定最大速度与最小速度。

4、假设坐标点P_i的速度为V_i，坐标点P_i+1的速度为V_i+1，且V_i<V_i+1，线段P_iP_i+1的长度为L_i，则速度规划如下：

确定V_i在速度表中的位置Pos_i，取加速度表中Pos_i位置的值与设定的加速度值的乘积即为速度变化量ΔV，以ΔV进行定周期加速得到实时速度V_c，ΔV与插补周期T的乘积即为一个周期内的插补步长，在每个周期内判断V_c在速度表中的位置，并更新速度变化值ΔV、实时速度V_c、累积插补步长L_sum，根据V_i+1及L_i的值确定加速距离；根据加速距离和L_i判断是否存在匀速、减速，如果存在减速，则继续确定是否包含匀速，根据是否存在匀速、减速的情况确定减速位置，当累积插补的步长L_sum达到线段的长度L_i时，则此线段的加减速规划及插补完成。

需要说明的是，根据V_i+1及L_i的值确定加速距离、确定减速位置均为T型加减速规划中的成熟步骤，本实施例中不展开描述。

对于S型加减速也采用上述方法，与T型加减速的区别在于S型的加速度曲线由加加速度曲线决定，离散分段值n不同，其他步骤一致。

本申请提供的裁床拐角轨迹跟踪高精度插值控制方法，克服了传统拐角采用圆弧曲线过渡及圆弧插补的复杂性，同时又保留了圆弧过渡的平滑性，提供了一种小线段逼近圆弧的过渡方法；有效的提高了对各类角度过渡的平滑性，增加了系统的稳定性及加工效率。

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

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

Claims (5)

1.一种裁床拐角轨迹跟踪高精度插值控制方法，其特征在于，所述裁床拐角轨迹跟踪高精度插值控制方法，包括：

步骤1、遍历切割文件中闭合曲线的各坐标点，求出各坐标点所组成的线段的斜率，根据斜率计算出相邻线段沿速度变化方向的夹角作为拐角；

步骤2、将各相邻线段组成的拐角进行分类，分成0°～60°、60°～120°、120°～180°三类拐角；

步骤3、将60°～120°的拐角对应的两条相邻线段优化成四条连续的线段：首先在两条相邻线段之间建立圆弧过渡模型，则得到拐角插值点依次为：圆弧接入点、圆弧中点、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算三个拐角插值点的坐标，并利用计算得到的三个拐角插值点代替原来的两条相邻线段的交点，得到优化后的四条连续的线段，此时新的相邻两条线段的拐角范围为0°～60°；

步骤4、将120°～180°的拐角对应的两条相邻线段优化成六条连续的线段：首先在两条相邻线段之间建立圆弧过渡模型，取均分圆弧四等分的三个点作为圆弧插入点，则得到拐角插值点依次为：圆弧接入点、圆弧插入点1、圆弧插入点2、圆弧插入点3、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算五个拐角插值点的坐标，并利用计算得到的五个拐角插值点代替原来的两条相邻线段的交点，得到优化后的六条连续的线段，此时新的相邻两条线段的拐角范围为0°～60°；

步骤5、待所有60°～120°、120°～180°的拐角均优化完成后，重新遍历更新后的切割文件中闭合曲线的各坐标点，根据拐角的大小及轴速度的方向对各坐标点的速度进行规划，对各坐标点组成的线段进行直线插补及加减速规划；

其中，根据拐角的大小及轴速度的方向对各坐标点的速度进行规划如下：

(1)若坐标点P_i处X轴或/和Y轴的速度方向反向改变，则坐标点P_i的速度V_i为：

式中A_max为最大加速度，T为插补周期，I_i,x、I_i,y为线段P_i-1P_i的方向矢量，P_i为切割文件中的第i个坐标点，x_i、y_i为坐标点P_i的坐标；

(2)若坐标点P_i处X轴和Y轴的速度方向均不变，则坐标点P_i的速度V_i为：

式中，θ为坐标点P_i所在位置对应的拐角的角度。

2.如权利要求1所述的裁床拐角轨迹跟踪高精度插值控制方法，其特征在于，所述得到拐角插值点依次为：圆弧接入点、圆弧中点、圆弧转出点，根据预设的弓高误差以及原来的两条相邻线段上的三个坐标点，计算三个拐角插值点的坐标，包括：

已知原来的两条相邻线段上的三个坐标点为P_i-1、P_i、P_i+1，其中P_i为拐点，E_i为弓高误差，θ为拐角，可算出圆弧的过渡距离L为：

取

其中S_i-1、S_i+1为原来的两条相邻线段的长度，则计算三个拐角插值点的坐标的过程如下：

(1)当L_m≥L时，此时E_i取预设的系统最大弓高误差，则R的值以及P_a、P_b、P_c三点坐标为：

(2)当L_m＜L时，此时E_i受L_m的影响，则R、E_i的值以及P_a、P_b、P_c三点坐标为：

式中，R为圆弧的半径，P_a为圆弧接入点，P_b为圆弧中点，P_c为圆弧转出点。

3.如权利要求1所述的裁床拐角轨迹跟踪高精度插值控制方法，其特征在于，所述得到拐角插值点依次为：圆弧接入点、圆弧插入点1、圆弧插入点2、圆弧插入点3、圆弧转出点，根据弓高误差以及原来的两条相邻线段上的三个坐标点，计算五个拐角插值点的坐标，包括：

已知原来的两条相邻线段上的三个坐标点为P_i-1、P_i、P_i+1，其中P_i为拐点，E_i为弓高误差，θ为拐角，可算出圆弧的过渡距离L为：

取

其中S_i-1、S_i+1为原来的两条相邻线段的长度，则计算五个拐角插值点的坐标的过程如下：

(1)当L_m≥L时，此时E_i取预设的系统最大弓高误差，则R的值以及P_a、P_b、P_c、P_d、P_e五点坐标为：

(2)当L_m＜L时，此时R、E_i受L_m的影响，则R、E_i的值以及P_a、P_b、P_c、P_d、P_e五点坐标为：

式中，P_a为圆弧接入点，P_b为圆弧插入点1，P_c为圆弧插入点2，P_d为圆弧插入点3，P_e为圆弧转出点，Q₀、Q₁分别为P_i-1P_i、P_iP_i+1上的坐标点，Q₀ Q₁过点P_c且平行于P_aP_e。

4.如权利要求1所述的裁床拐角轨迹跟踪高精度插值控制方法，其特征在于，所述加减速规划，包括：

取各坐标点组成的线段，对线段的长度小于阈值的线段采用T型加减速进行速度规划，对线段的长度大于等于阈值的线段采用S型加减速进行速度规划。

5.如权利要求4所述的裁床拐角轨迹跟踪高精度插值控制方法，其特征在于，所述采用T型加减速进行速度规划，包括：

假设坐标点P_i的速度为V_i，坐标点P_i+1的速度为V_i+1，且V_i<V_i+1，线段P_iP_i+1的长度为L_i，则速度规划如下：

确定V_i在速度表中的位置Pos_i，取加速度表中Pos_i位置的值与设定的加速度值的乘积即为速度变化量ΔV，以ΔV进行定周期加速得到实时速度V_c，ΔV与插补周期T的乘积即为一个周期内的插补步长，在每个周期内判断V_c在速度表中的位置，并更新速度变化值ΔV、实时速度V_c、累积插补步长L_sum，根据V_i+1及L_i的值确定加速距离；根据加速距离和L_i判断是否存在匀速、减速，如果存在减速，则继续确定是否包含匀速，根据是否存在匀速、减速确定减速位置，当累积插补的步长L_sum达到线段的长度L_i时，则此线段的加减速规划及插补完成。

Images