CN113791585A - 一种圆弧轨迹速度自适应控制方法 - Google Patents

Abstract

本发明公开了一种圆弧轨迹速度自适应控制方法，包括：遍历闭合曲线的各坐标点，计算基于各相邻坐标点所组成的线段的斜率，根据斜率计算出相邻线段速度方向变化的夹角作为拐角；根据相邻线段所对应的拐角对拐点速度进行约束；提取闭合曲线中的圆弧轨迹，并计算各个圆弧轨迹的半径、圆心、线速度，根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新；根据更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度，利用速度倒推法对拐点速度进行约束更新；利用加减速算法对闭合曲线进行插补，并限制圆弧轨迹的最大速度，完成闭合曲线的速度规划。本发明能有效提高切割轨迹中圆弧轨迹的切割效率及切割精度。

Description

一种圆弧轨迹速度自适应控制方法

技术领域

本申请属于运动控制技术领域，具体涉及一种圆弧轨迹速度自适应控制方法。

背景技术

在裁床、模板机等设备的运动控制中，运动轨迹是由上位机发送的PLT文件决定，PLT文件由构成轨迹的一系列坐标点和指令组成，控制器对PLT文件进行解析并通过速度规划算法和插补算法控制电机运动。

目前传统的速度规划算法主要是通过坐标点组成线段所构成的夹角大小对拐角速度进行规划。对于切割轨迹中长直线与圆弧衔接的这类轨迹，衔接点的拐角过小，利用传统的速度规划方法则会造成衔接处的速度过大，导致过切现象；并且对于切割轨迹中半径不同的圆形轨迹，利用传统的速度规划算法则会造成各种圆形轨迹的拐角速度相同，导致切割精度与效率降低。

发明内容

本申请的目的在于提供一种圆弧轨迹速度自适应控制方法，有效提高切割轨迹中圆弧轨迹的切割效率及切割精度。

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

一种圆弧轨迹速度自适应控制方法，所述圆弧轨迹速度自适应控制方法，包括：

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

步骤2、以每一坐标点作为拐点，根据相邻线段所对应的拐角对拐点速度进行约束；

步骤3、提取闭合曲线中的圆弧轨迹，并计算各个圆弧轨迹的半径、圆心、线速度，根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新；

步骤4、根据步骤3更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度，利用速度倒推法对拐点速度进行约束更新；

步骤5、利用加减速算法对闭合曲线进行插补，并限制圆弧轨迹的最大速度，完成闭合曲线的速度规划。

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

作为优选，所述计算基于各相邻坐标点所组成的线段的斜率，根据斜率计算出相邻线段速度方向变化的夹角作为拐角，包括：

已知PLT文件中任意连续三点坐标为P_i(x_i,y_i)，P_i+1(x_i+1,y_i+1)，P_i+2(x_i+2,y_i+2)，则计算相邻线段P_i P_i+1和P_i+1P_i+2的拐角θ_i+1如下：

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

其中，角α为经过拐点P_i+1前的速度与X轴正方向的夹角，角β为经过拐点P_i+1后的速度与X轴正方向的夹角。

作为优选，所述根据相邻线段所对应的拐角对拐点速度进行约束，包括：

令经过拐点前的速度为v，经过拐点后的速度为v'，且速度v和速度v'大小相同、方向不同，速度变化量在一个插补周期内，即速度变化量Δv为：

Δv＝a_maxT

式中，a_max为X轴和Y轴的最大合加速度，T为插补周期；

设速度v与X轴正方向的夹角为α，速度v'与X轴正方向的夹角为β，则速度v和速度v'在X、Y轴方向上的速度分量的大小为：

v_x＝v cosα

v'_x＝v'cosβ

v_y＝v sinα

v'_y＝v'sinβ

设a_{max_x}、a_{max_y}分别为X轴和Y轴最大允许加速度，则经过拐点前后的速度在X、Y轴方向上的速度变化为：

v'_x-v_x＝v'cosβ-v cosα＝a_{max_x}T

v'_y-v_y＝v'sinβ-v sinα＝a_{max_y}T

由于速度v'和速度v大小相等，方向不同，则有v'＝v，设拐点处的速度为v_m，计算出v_m的速度为：

且进一步调整拐点速度v_m为：

式中，v_turn为用户设定的拐弯系数。

作为优选，所述提取闭合曲线中的圆弧轨迹，并计算各个圆弧轨迹的半径、圆心、线速度，根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新，包括：

步骤3.1、取PLT文件中的连续三个坐标点P_i、P_i+1、P_i+2，令初始值i＝0，i最大值为N-3，N为闭合曲线中坐标点的总个数，计算这三个点所组成的两条线段P_i P_i+1和P_i+1P_i+2的差值为Δl_i和对应的拐角为θ_i+1，判断θ_i+1是否小于拐角阈值θ_angle，若满足转到步骤3.2，若不满足，则i＝i+1，并转到步骤3.1；

步骤3.2、判断圆弧点的数量pointSum是否为0，若pointSum为0，则圆弧起始点序号Startnum＝i，pointSum＝pointSum+1，圆弧线段长度差值D_value＝Δl_i，且i＝i+1并转到步骤3.1；若pointSum不为0，转到步骤3.3；

步骤3.3、判断Δl_i与D_value的差值的绝对值是否小于设定阈值L_dis，若满足，转到步骤3.4；若不满足，转到步骤3.6；

步骤3.4、判断当前圆弧半径R是否为0，若R为0，则继续判断pointSum是否大于6，若pointSum大于6，取序号为Startnum、Startnum+2、Startnum+5三点坐标确定圆弧，计算圆弧的半径R、圆心坐标CentrePoint，并跳到步骤3.5，若pointSum小于6，则pointSum＝pointSum+1，i＝i+1，转到步骤3.1；若R不为0，跳到步骤3.5；

步骤3.5、判断当前拐点P_i与圆心CentrePoint的距离是否在半径R的阈值内，若满足，则pointSum＝pointSum+1，i＝i+1，转到步骤3.1；若不满足，转到步骤3.6；

步骤3.6、判断pointSum是否大于6，若大于6，圆弧终点序号Endnum＝i，取序号为Startnum、

三点坐标确定圆弧，计算圆弧的线速度Vel，Vel＝2πnR+V_start，n为转速，V_start为起跳速度，并转到步骤3.7；若pointSum小于6，则更新Startnum＝i，pointSum＝1，D_value＝Δl_i，当前圆弧半径R为0，i＝i+1，转到步骤3.1；

步骤3.7、根据圆弧的线速度Vel对此圆弧的各个拐点速度进行约束，并保存此圆弧段的最大速度V_max：

V_min＝min(v_mk)(k＝Startnum...Endnum)

式中，v_mk为步骤2中由拐角大小约束的拐点速度；

对拐点速度进行约束更新：

v_mk＝min(Vel,V_min)(k＝Startnum...Endnum)

保存此圆弧的最大速度：

V_max＝v_mk

式中，Startnum为此圆弧起始点序号，Endnum为此圆弧终点序号；

且i＝i+1，转到步骤3.1。

作为优选，所述利用速度倒推法对拐点速度进行约束更新，包括：

步骤4.1、令相邻两个坐标点组成的线段长度为L₀、L₁、…、L_N-2，其中N为闭合曲线中坐标点的总个数，取步骤3约束更新后各个拐点P₀、P₁、…、P_N-1的拐点速度为v_m0、v_m1、…、v_m(N-1)，且起点和终点的速度相等，即v_m0＝v_m(N-1)＝V_start，V_start为起跳速度；

步骤4.2、假设当前点速度为v_mb，b＝N-1,...,3,2,1，从当前点开始以S型速度曲线加速，当加速距离达到L_b-1时的速度为v'_m(b-1)，则更新拐点P_b-1的速度为v_m(b-1)＝min(v_m(b-1),v'_m(b-1))，继续以v_m(b-1)作为当前点速度以S型速度曲线加速，直到速度倒推至v₀。

作为优选，所述利用加减速算法对闭合曲线进行插补，并限制圆弧轨迹的最大速度，包括：

步骤5.1、取由matlab制定生成的S型速度曲线，并离散成f阶，构成加速度表Acc_Tab＝[a_t1,a_t2...a_t(j-1)、a_tj...a_t(f-1),a_tf]、加速步长表Len_Tab＝[L_t1,L_t2...L_t(j-1)、L_tj...L_t(f-1),L_tf]，j＝1...f，用户设定的最大切割速度为V_user，切割加速度为A_user，计算速度表V_Tab＝[V_t1,V_t2...V_t(j-1)、V_tj...V_t(f-1),V_tf]，其中速度表计算如下：

V_tj＝V_start+L_t1*a_t1*A_user，j＝1

V_tj＝V_t(j-1)+L_tj*a_tj*A_user，j≠1

式中，V_start为预设定的起跳速度，且若V_tj＞V_user，则V_tj＝V_user；

步骤5.2：根据当前速度V_cur、目标速度V_tar、相邻两个坐标点之间的长度L_c，计算加减速距离如下：

取当前速度V_cur、目标速度V_tar中的较小值作为初始速度，以S型速度曲线加速至两速度中的较大值，加速距离为L'_acc，若加速距离L'_acc≥L_c，则L'_acc＝L_c；

(1)、若V_cur＜V_tar：

则加速距离

(2)、若V_cur≥V_tar：

则加速距离

则计算得到减速距离为L_dec＝L_c-L_acc；

步骤5.3：根据计算得到的加速距离和减速距离判断当前插补位置处于加速段或减速段并在每个插补周期内进行相应的加减速，且如果当前相邻两个坐标点之间的线段不为圆弧段，则限制当前插补位置的最大速度为预设的最大允许速度；如果当前相邻两个坐标点之间的线段为圆弧段，则限制当前插补位置的最大速度为圆弧的最大速度V_max。

本申请提供的圆弧轨迹速度自适应控制方法，根据拐角的大小对拐点速度进行规划，然后鉴别出切割轨迹中的圆弧轨迹，根据圆弧轨迹的半径通过线速度自适应调整圆弧中的拐点速度，最后利用速度倒推法确保拐点速度的可达性，并利用加减速算法对切割轨迹进行插补，能有效的提高切割轨迹中圆弧、圆形轨迹的切割效率及切割精度。

附图说明

图1为本申请的圆弧轨迹速度自适应控制方法的流程图；

图2为本申请计算拐角的示意图；

图3为本申请长直线与圆弧衔接类型以及多圆弧衔接类型的轨迹示意图。

具体实施方式

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

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

其中一个实施例中，提供一种圆弧轨迹速度自适应控制方法，以克服了切割轨迹中圆弧与长直线衔接处的过切现象，提高不同半径圆形轨迹的切割精度和切割效率。

如图1所示，本实施例的圆弧轨迹速度自适应控制方法，包括：

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

如图2所示，已知PLT文件中任意连续三点坐标为P_i(x_i,y_i)，P_i+1(x_i+1,y_i+1)，P_i+2(x_i+2,y_i+2)，则计算相邻线段P_i P_i+1和P_i+1P_i+2的拐角θ_i+1如下：

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

其中，角α为经过拐点P_i+1前的速度与X轴正方向的夹角，角β为经过拐点P_i+1后的速度与X轴正方向的夹角。拐角θ_i+1为对应于坐标P_i+1(即拐点P_i+1)的拐角，令闭合曲线中坐标点的总个数为N，则i的取值范围为0～N-3，为了计算得到各个拐点对应的拐点，在i的取值范围之外，还要额外计算拐点P_N-1对应的拐角，取坐标P₀，P_N-1，P_N-2进行计算。

步骤2、以每一坐标点作为拐点，根据相邻线段所对应的拐角对拐点速度进行约束。本实施例中的速度约束应理解为确定速度最大值。

令经过拐点前的速度为v，经过拐点后的速度为v'，且速度v和速度v'大小相同、方向不同，速度变化量在一个插补周期内，即速度变化量Δv为：

Δv＝a_maxT

式中，a_max为X轴和Y轴(在轨迹切割中通常基于X轴和Y轴控制刀头完成运行，本实施例基于X轴和Y轴进行规划，但不限于整个切割中只存在X轴和Y轴)的最大合加速度，T为插补周期；

设速度v与X轴正方向的夹角为α，速度v'与X轴正方向的夹角为β，则速度v和速度v'在X、Y轴方向上的速度分量的大小为：

v_x＝v cosα

v'_x＝v'cosβ

v_y＝v sinα

v'_y＝v'sinβ

设a_{max_x}、a_{max_y}分别为X轴和Y轴最大允许加速度，则经过拐点前后的速度在X、Y轴方向上的速度变化为：

v'_x-v_x＝v'cosβ-v cosα＝a_{max_x}T

v'_y-v_y＝v'sinβ-v sinα＝a_{max_y}T

由于速度v'和速度v大小相等，方向不同，则有v'＝v，设拐点处的速度为v_m，计算出v_m的速度为：

且为了便于用户对拐点速度可控，进一步调整拐点速度v_m为：

式中，v_turn为用户设定的拐弯系数。需要说明的是，本实施例中的速度为具有大小和方向的速度，但是在以上计算过程中仅取速度的大小进行计算，即公式中的v，v'，v_m，v_turn表示相应速度的大小。

容易理解的是，以上计算的拐点速度v_m为统一表述方式，在具体计算时为了便于建立每一拐点对应的拐点速度，可基于v_m进一步添加下标标识进行区分。

步骤3、提取闭合曲线中的圆弧轨迹，并计算各个圆弧轨迹的半径、圆心、线速度，根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新。

步骤3.1、取PLT文件中的连续三个坐标点P_i、P_i+1、P_i+2，令初始值i＝0，i最大值为N-3，N为闭合曲线中坐标点的总个数，计算这三个点所组成的两条线段P_i P_i+1和P_i+1P_i+2的差值为Δl_i和对应的拐角为θ_i+1，判断θ_i+1是否小于拐角阈值θ_angle，若满足转到步骤3.2，若不满足，则i＝i+1，并转到步骤3.1；

步骤3.2、判断圆弧点的数量pointSum是否为0，若pointSum为0，则圆弧起始点序号Startnum＝i，pointSum＝pointSum+1，圆弧线段长度差值D_value＝Δl_i，且i＝i+1并转到步骤3.1；若pointSum不为0，转到步骤3.3；

步骤3.3、判断Δl_i与D_value的差值的绝对值是否小于设定阈值L_dis，若满足，转到步骤3.4；若不满足，转到步骤3.6；

步骤3.4、判断当前圆弧半径(即步骤3.2中确定了圆弧起始点序号的圆弧)R是否为0，若R为0，则继续判断pointSum是否大于6，若pointSum大于6，取序号为Startnum、Startnum+2、Startnum+5三点坐标确定圆弧，计算圆弧的半径R、圆心坐标CentrePoint，并跳到步骤3.5，若pointSum小于6，则pointSum＝pointSum+1，i＝i+1，转到步骤3.1；若R不为0，跳到步骤3.5；

步骤3.5、判断当前拐点P_i与圆心CentrePoint的距离是否在半径R的阈值内(这里的半径R的阈值应理解为基于半径R确定的判断范围，例如半径R的阈值为0.9R～1.1R)，若满足，则pointSum＝pointSum+1，i＝i+1，转到步骤3.1；若不满足，转到步骤3.6；

步骤3.6、判断pointSum是否大于6，若大于6，圆弧终点序号Endnum＝i，取序号为Startnum、

三点坐标确定圆弧，计算圆弧的线速度Vel，Vel＝2πnR+V_start，n为转速，V_start为起跳速度，并转到步骤3.7；若pointSum小于6，则更新Startnum＝i，pointSum＝1，D_value＝Δl_i，当前圆弧半径R为0，i＝i+1，转到步骤3.1；

步骤3.7、根据圆弧的线速度Vel对此圆弧的各个拐点速度进行约束，并保存此圆弧段的最大速度V_max：

V_min＝min(v_mk)(k＝Startnum...Endnum)

式中，v_mk为步骤2中由拐角大小约束的拐点速度；

对拐点速度进行约束更新：

v_mk＝min(Vel,V_min)(k＝Startnum...Endnum)

保存此圆弧的最大速度：

V_max＝v_mk

式中，Startnum为此圆弧起始点序号，Endnum为此圆弧终点序号；容易理解的是，步骤3.7中限定圆弧段的最大速度时以V_max进行统一表述，在具体计算时为了便于建立不同圆弧对应的最大速度，可基于V_max进一步添加下标标识进行区分。

且i＝i+1，转到步骤3.1。

步骤4、根据步骤3更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度，利用速度倒推法对拐点速度进行约束更新。

步骤4.1、令相邻两个坐标点组成的线段长度为L₀、L₁、…、L_N-2，其中N为闭合曲线中坐标点的总个数，取步骤3约束更新后各个拐点P₀、P₁、…、P_N-1的拐点速度为v_m0、v_m1、…、v_m(N-1)，且起点和终点的速度相等，即v_m0＝v_m(N-1)＝V_start，V_start为起跳速度；

步骤4.2、假设当前点速度为v_mb，b＝N-1,...,3,2,1，从当前点开始以S型速度曲线加速，当加速距离达到L_b-1时的速度为v'_m(b-1)，则更新拐点P_b-1的速度为v_m(b-1)＝min(v_m(b-1),v'_m(b-1))，继续以v_m(b-1)作为当前点速度以S型速度曲线加速，直到速度倒推至v₀。

在速度倒推法中，以v_m(N-1)起始开始进行倒推，在倒推时取倒推距离为拐点P_N-1和拐点P_N-2之间的间距L_N-2，当加速距离达到L_N-2时得到一个倒推速度v'_m(N-2)，而在步骤3中约束更新后拐点P_N-2对应的速度为v_m(N-2)，因此需要重新更新约束，更新为v_m(N-2)＝min(v_m(N-2),v'_m(N-2))，完成由速度v_m(N-1)至v_m(N-2)的倒推约束更新。接下去继续以v_m(N-2)开始进行倒推，同理类推，直至完成由速度v_m1至v_m0的倒推约束更新

本实施例中对拐点速度进行约束时，先后采取了三种不同的约束方法，即先基于拐角进行速度约束，后基于圆弧的参数对全部或部分拐点速度约束进行更新，最后基于最新的速度约束利用速度倒推法继续对全部或部分拐点速度进行更新，以此综合考虑了拐点速度的各个影响因素，使得步骤4最终得到的拐点速度具有有效性和可行性。

步骤5、利用加减速算法对闭合曲线进行插补，并限制圆弧轨迹的最大速度，完成闭合曲线的速度规划。

步骤5.1、取由matlab制定生成的S型速度曲线，并离散成f阶，构成加速度表Acc_Tab＝[a_t1,a_t2...a_t(j-1)、a_tj...a_t(f-1),a_tf]、加速步长表Len_Tab＝[L_t1,L_t2...L_t(j-1)、L_tj...L_t(f-1),L_tf]，j＝1...f，用户设定的最大切割速度为V_user，切割加速度为A_user，计算速度表V_Tab＝[V_t1,V_t2...V_t(j-1)、V_tj...V_t(f-1),V_tf]，其中速度表计算如下：

V_tj＝V_start+L_t1*a_t1*A_user，j＝1

V_tj＝V_t(j-1)+L_tj*a_tj*A_user，j≠1

式中，V_start为预设定的起跳速度，且若V_tj＞V_user，则V_tj＝V_user；

步骤5.2：根据当前速度V_cur、目标速度V_tar、相邻两个坐标点之间的长度L_c，计算加减速距离如下：

取当前速度V_cur、目标速度V_tar中的较小值作为初始速度，以S型速度曲线加速至两速度中的较大值，加速距离为L'_acc，若加速距离L'_acc≥L_c，则L'_acc＝L_c；

(1)、若V_cur＜V_tar：

则加速距离

(2)、若V_cur≥V_tar：

则加速距离

则计算得到减速距离为L_dec＝L_c-L_acc；

由于实际加速段累积插补长度可能与加速距离不重合，即实际运动过程中以加速距离L_acc为目标基准进行运动，但由于运动误差的影响导致实际运动距离与L_acc相比具有一定偏差，因此需要对加速距离进行更新，更新实际加速距离为

(其中v_ci为加速段在每个插补周期的实际速度，T为插补周期)，所以实际减速距离为L_decactual＝L_c-L_accactual。

步骤5.3：根据计算得到的加速距离和减速距离判断当前插补位置处于加速段或减速段并在每个插补周期内进行相应的加减速，且如果当前相邻两个坐标点之间的线段不为圆弧段，则限制当前插补位置的最大速度为预设的最大允许速度；如果当前相邻两个坐标点之间的线段为圆弧段，则限制当前插补位置的最大速度为圆弧的最大速度V_max。

即本申请的速度规划控制后，整个闭合曲线的运行中直线按照预设的最大允许速度进行运动，圆弧按照限制的每段圆弧的最大速度进行运动，并且所有拐点按照步骤4最终更新后的拐点速度进行运动，以保证运动精度和速度。并且本申请的速度规划在如图3所示的长直线与圆弧衔接类型以及多圆弧衔接类型中均具有优异的应用效果。

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

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

Claims (6)

1.一种圆弧轨迹速度自适应控制方法，其特征在于，所述圆弧轨迹速度自适应控制方法，包括：

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

步骤2、以每一坐标点作为拐点，根据相邻线段所对应的拐角对拐点速度进行约束；

步骤3、提取闭合曲线中的圆弧轨迹，并计算各个圆弧轨迹的半径、圆心、线速度，根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新；

步骤4、根据步骤3更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度，利用速度倒推法对拐点速度进行约束更新；

步骤5、利用加减速算法对闭合曲线进行插补，并限制圆弧轨迹的最大速度，完成闭合曲线的速度规划。

2.如权利要求1所述的圆弧轨迹速度自适应控制方法，其特征在于，所述计算基于各相邻坐标点所组成的线段的斜率，根据斜率计算出相邻线段速度方向变化的夹角作为拐角，包括：

已知PLT文件中任意连续三点坐标为P_i(x_i,y_i)，P_i+1(x_i+1,y_i+1)，P_i+2(x_i+2,y_i+2)，则计算相邻线段P_iP_i+1和P_i+1P_i+2的拐角θ_i+1如下：

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

其中，角α为经过拐点P_i+1前的速度与X轴正方向的夹角，角β为经过拐点P_i+1后的速度与X轴正方向的夹角。

3.如权利要求1所述的圆弧轨迹速度自适应控制方法，其特征在于，所述根据相邻线段所对应的拐角对拐点速度进行约束，包括：

令经过拐点前的速度为v，经过拐点后的速度为v'，且速度v和速度v'大小相同、方向不同，速度变化量在一个插补周期内，即速度变化量Δv为：

Δv＝a_maxT

式中，a_max为X轴和Y轴的最大合加速度，T为插补周期；

设速度v与X轴正方向的夹角为α，速度v'与X轴正方向的夹角为β，则速度v和速度v'在X、Y轴方向上的速度分量的大小为：

v_x＝vcosα

v'_x＝v'cosβ

v_y＝vsinα

v'_y＝v'sinβ

设a_{max_x}、a_{max_y}分别为X轴和Y轴最大允许加速度，则经过拐点前后的速度在X、Y轴方向上的速度变化为：

v'_x-v_x＝v'cosβ-vcosα＝a_{max_x}T

v'_y-v_y＝v'sinβ-vsinα＝a_{max_y}T

由于速度v'和速度v大小相等，方向不同，则有v'＝v，设拐点处的速度为v_m，计算出v_m的速度为：

且进一步调整拐点速度v_m为：

式中，v_turn为用户设定的拐弯系数。

4.如权利要求1所述的圆弧轨迹速度自适应控制方法，其特征在于，所述提取闭合曲线中的圆弧轨迹，并计算各个圆弧轨迹的半径、圆心、线速度，根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新，包括：

步骤3.1、取PLT文件中的连续三个坐标点P_i、P_i+1、P_i+2，令初始值i＝0，i最大值为N-3，N为闭合曲线中坐标点的总个数，计算这三个点所组成的两条线段P_iP_i+1和P_i+1P_i+2的差值为Δl_i和对应的拐角为θ_i+1，判断θ_i+1是否小于拐角阈值θ_angle，若满足转到步骤3.2，若不满足，则i＝i+1，并转到步骤3.1；

步骤3.2、判断圆弧点的数量pointSum是否为0，若pointSum为0，则圆弧起始点序号Startnum＝i，pointSum＝pointSum+1，圆弧线段长度差值D_value＝Δl_i，且i＝i+1并转到步骤3.1；若pointSum不为0，转到步骤3.3；

步骤3.3、判断Δl_i与D_value的差值的绝对值是否小于设定阈值L_dis，若满足，转到步骤3.4；若不满足，转到步骤3.6；

步骤3.4、判断当前圆弧半径R是否为0，若R为0，则继续判断pointSum是否大于6，若pointSum大于6，取序号为Startnum、Startnum+2、Startnum+5三点坐标确定圆弧，计算圆弧的半径R、圆心坐标CentrePoint，并跳到步骤3.5，若pointSum小于6，则pointSum＝pointSum+1，i＝i+1，转到步骤3.1；若R不为0，跳到步骤3.5；

步骤3.5、判断当前拐点P_i与圆心CentrePoint的距离是否在半径R的阈值内，若满足，则pointSum＝pointSum+1，i＝i+1，转到步骤3.1；若不满足，转到步骤3.6；

步骤3.6、判断pointSum是否大于6，若大于6，圆弧终点序号Endnum＝i，取序号为Startnum、

三点坐标确定圆弧，计算圆弧的线速度Vel，Vel＝2πnR+V_start，n为转速，V_start为起跳速度，并转到步骤3.7；若pointSum小于6，则更新Startnum＝i，pointSum＝1，D_value＝Δl_i，当前圆弧半径R为0，i＝i+1，转到步骤3.1；

步骤3.7、根据圆弧的线速度Vel对此圆弧的各个拐点速度进行约束，并保存此圆弧段的最大速度V_max：

V_min＝min(v_mk)(k＝Startnum...Endnum)

式中，v_mk为步骤2中由拐角大小约束的拐点速度；

对拐点速度进行约束更新：

v_mk＝min(Vel,V_min)(k＝Startnum...Endnum)

保存此圆弧的最大速度：

V_max＝v_mk

式中，Startnum为此圆弧起始点序号，Endnum为此圆弧终点序号；

且i＝i+1，转到步骤3.1。

5.如权利要求4所述的圆弧轨迹速度自适应控制方法，其特征在于，所述利用速度倒推法对拐点速度进行约束更新，包括：

步骤4.1、令相邻两个坐标点组成的线段长度为L₀、L₁、…、L_N-2，其中N为闭合曲线中坐标点的总个数，取步骤3约束更新后各个拐点P₀、P₁、…、P_N-1的拐点速度为v_m0、v_m1、…、v_m(N-1)，且起点和终点的速度相等，即v_m0＝v_m(N-1)＝V_start，V_start为起跳速度；

步骤4.2、假设当前点速度为v_mb，b＝N-1,...,3,2,1，从当前点开始以S型速度曲线加速，当加速距离达到L_b-1时的速度为v'_m(b-1)，则更新拐点P_b-1的速度为v_m(b-1)＝min(v_m(b-1),v'_m(b-1))，继续以v_m(b-1)作为当前点速度以S型速度曲线加速，直到速度倒推至v₀。

6.如权利要求5所述的圆弧轨迹速度自适应控制方法，其特征在于，所述利用加减速算法对闭合曲线进行插补，并限制圆弧轨迹的最大速度，包括：

步骤5.1、取由matlab制定生成的S型速度曲线，并离散成f阶，构成加速度表Acc_Tab＝[a_t1,a_t2...a_t(j-1)、a_tj...a_t(f-1),a_tf]、加速步长表Len_Tab＝[L_t1,L_t2...L_t(j-1)、L_tj...L_t(f-1),L_tf]，j＝1...f，用户设定的最大切割速度为V_user，切割加速度为A_user，计算速度表V_Tab＝[V_t1,V_t2...V_t(j-1)、V_tj...V_t(f-1),V_tf]，其中速度表计算如下：

V_tj＝V_start+L_t1*a_t1*A_user，j＝1

V_tj＝V_t(j-1)+L_tj*a_tj*A_user，j≠1

式中，V_start为预设定的起跳速度，且若V_tj＞V_user，则V_tj＝V_user；

步骤5.2：根据当前速度V_cur、目标速度V_tar、相邻两个坐标点之间的长度L_c，计算加减速距离如下：

取当前速度V_cur、目标速度V_tar中的较小值作为初始速度，以S型速度曲线加速至两速度中的较大值，加速距离为L'_acc，若加速距离L'_acc≥L_c，则L'_acc＝L_c；

(1)、若V_cur＜V_tar：

则加速距离

(2)、若V_cur≥V_tar：

则加速距离

则计算得到减速距离为L_dec＝L_c-L_acc；

步骤5.3：根据计算得到的加速距离和减速距离判断当前插补位置处于加速段或减速段并在每个插补周期内进行相应的加减速，且如果当前相邻两个坐标点之间的线段不为圆弧段，则限制当前插补位置的最大速度为预设的最大允许速度；如果当前相邻两个坐标点之间的线段为圆弧段，则限制当前插补位置的最大速度为圆弧的最大速度V_max。

Images