[发明专利]一种适用于3D打印控制系统的轨迹算法

权利要求书

1.一种适用于3D打印控制系统的轨迹算法，其特征在于：包括以下步骤：

S1、根据当前位置到预定位置的距离，计算矢量长度，并根据矢量长度创建节点；

S2、通过三个相邻节点得到相邻矢量线段，计算线段之间的角度和曲率半径，并根据线段之间的角度和曲率半径计算线段的起点最大速度和终点最大速度；

S3、根据线段的起点最大速度和终点最大速度判断当前线段的第一个节点是否是一条准直线的第一个节点，是则直接加入准直线节点链表并进入步骤S5；否则进入步骤S4；

S4、判断相邻线段的节点速度变化情况，若相邻线段的节点速度为上升，则将该节点加入准直线；若相邻线段的节点速度为下降，将节点合并至当前准直线的尾部，根据新准直线和当前准直线创建新节点链表；

S5、根据准直线节点链表和新节点链表更新节点链表数据，并修正当前准直线的运动速度，进而控制3D打印系统的轨迹；

所述通过三个相邻节点得到相邻矢量线段，计算线段之间的角度和曲率半径，并计算线段的起点最大速度和终点最大速度的方法包括方法①或方法②：

其中方法①为：将三个相邻的节点依次标记为P₁(x₁，y₁)、P₂(x₂，y₂)和P₃(x₃，y₃)，并根据这三个节点确定一个圆，该圆的半径R作为P₂点的曲率半径，则：

R＝L₁/(cosθ₁)，R＝L₂/(cosθ₂)，θ＝θ₁+θ₂

得到L₁×cos(θ-θ₁)＝L₂×cosθ₁

进而得到

根据公式

r＝sqrt((x-x₂)²+(y-y₂)²)

得到曲率半径，其中

y＝(d×c-a×f)/(b×d-e×a)，x＝(b×f-e×c)/(b×d-e×a)，f＝x₃²+y₃²-x₂²-y₂²，e＝2×(y₃一y₂)，d＝2×(x₃-x₂)，c＝x₂²+y₂²-x₁²-y₁²，b＝2×(y₂-y₁)，a＝2×(x₂-x₁)；

根据线段之间的角度和曲率半径计算线段的起点最大速度和终点最大速度得到节点的最大允许速度的公式为：

V′_c＝sqrt(r×A_a)

其中θ是相邻线段的夹角，V′_c是最大允许速度，L₁为P₁点到P₂点长度的一半，L₂为P₂点到P₃长度的一半；θ₁为∠P₁P₂O的大小，O为圆心；θ₂为∠P₃P₂O的大小；θ为∠P₁P₂P₃的大小；sqrt为平方根；A_a为常数；

方法②为：将三个相邻的节点依次标记为P₁(P_1x，P_1y)、P₂(P_2x，P_2y)和P₃(P_3x，P_3y)，根据这三个点的绝对坐标得到相邻线段的单位矢量：

矢量速度方向由变成得到X、Y轴产生的速度变化量：

根据X、Y轴许可的速度跳变量，按照X、Y轴产生的速度变化量逆推得到拐弯点的最大允许速度，根据公式

取其较小值作为拐弯点的最大允许速度；其中为X轴到拐点的最大允许速度；为Y轴到拐点的最大允许速度；v为最大允许速度；为的矢量速度方向；为的矢量速度方向；l₁为P₁点到P₂点长度的一半，l₂为P₂点到P₃长度的一半；α₁为与X轴的夹角；α₂为与X轴的夹角；sqrt为平方根；

根据线段的起点最大速度和终点最大速度判断当前线段的第一个节点是否是一条准直线的第一个节点的方法为：

根据规则

①C_i＞C_i-1，C_i＞C_i+1

②θ_i≤90°

当任意点P_i同时满足规则①和规则②，则判定此点为一条准直线的第一个节点；其中θ_i是和之间的夹角，C_i＝1/r_i是任意点P_i的曲率，C_i-1是点P_i-1的曲率，C_i+1是点P_i+1的曲率；P_i-1和P_i+1分别为点P_i前一个点和后一个点。