一种软控制器连续小线段轨迹预处理方法与流程

1.本发明涉及数控加工技术领域，尤其是涉及一种软控制器连续小线段轨迹预处理方法。


背景技术：

2.随着企业对设备的自动化程度和产品质量要求越来越高，自动化设备已朝着高速度、高精度的方向发展。目前大多数自动化设备连续轨迹曲线加工工艺主要采用连续小线段轨迹加工方式，在给定误差的范围内，将待加工的复杂轨迹曲线分割成顺序相连且长度很短的微小直线段。这种轨迹从形状上逼近原直线或曲线轮廓，无法保证加工轮廓的光滑性。由于小线段太短，小线段衔接点速度为零，这将导致电机频繁启停从而使加工精度以及加工效率的降低。对于连续小线段轨迹的拟合，大多数拟合算法较复杂，不适合用在控制器中，且大多方法都是圆弧和直线拟合分开进行，这样导致某些适合拟合成圆弧的轨迹却拟合成多段短直线段，而适合拟合成长直线段的轨迹却拟合成了多段短圆弧，导致拟合结果存在较大的误差。


技术实现要素：

3.本发明要解决的技术问题是提供一种软控制器连续小线段轨迹预处理方法，算法计算复杂度较低，计算时间短，可以使加工后零件的表面更加的光滑，并提高加工的效率，提高企业生产效率。
4.为了解决上述技术问题，本发明提供一种连续小线段直线拟合与圆弧拟合方法，包括以下步骤：
5.步骤1、导入连续小线段数据点列p0(x0,y0),p1(x1,y1),
…
,p
n
(x
n
,y
n
)，确定参数erδ，erd，err；erδ为判断程序进入直线拟合还是圆弧拟合的阈值，err为圆弧拟合最大积分误差δr的允许误差，erd为直线的拟合精度；
6.步骤2、设步骤1中连续小线段中的当前拟合线段p
j
(x
j
,y
j
),p
j+1
(x
j+1
,y
j+1
),
…
,p
m
(x
m
,y
m
)的前三个点坐标p
j
(x
j
,y
j
)、p
j+1
(x
j+1
,y
j+1
)、p
j+2
(x
j+2
,y
j+2
)，计算向量和的夹角δ，以判断点p
j
、p
j+1
、p
j+2
是否在一条直线上；
7.步骤3、若δ≥erδ，则点p
j
、p
j+1
、p
j+2
三点不共线，由此进入步骤4圆弧小线段拟合过程拟合后续点；若δ＜erδ，则点p
j
、p
j+1
、p
j+2
位于同一条直线，由此进入步骤6直线小线段拟合过程拟合后续点；
8.步骤4、所述连续小线段圆弧拟合过程为：
9.步骤4.1、按带约束条件的最小二乘法计算p
j
、p
j+1
、p
j+2
三点所在圆弧拟合圆的参数圆心(a,b)和半径r；
10.步骤4.2、由步骤4.1所得圆心(a,b)和半径r，及拟合点列p
j
,p
j+1
,...,p
m
的坐标，计算最大积分误差δr：
[0011][0012]
步骤4.3、若δr≤err，则继续拟合下一点，使m＝m+1，回到步骤4.1计算圆弧参数；若δr＞err，输出上一点拟合的圆弧的参数，并将点p
m
‑1作为下一段圆弧拟合的起点；
[0013]
步骤4.4、直到拟合完最后一点，结束拟合；
[0014]
步骤5、若步骤4中计算的误差超过拟合精度，即δr≥err，则判断已经拟合的点数m
‑
j是否小于4，若m
‑
j≤4，则前面四个点不拟合成圆弧，进入步骤6的直线小线段拟合过程；若m
‑
j＞4，则将为拟合的圆弧段，点p
m
‑1作为后续拟合的初始点，从点p
m
‑1开始下一拟合；
[0015]
步骤6、所述直线小线段拟合过程为：
[0016]
步骤6.1、设初始参数值θ
max
＝+∞,θ
min
＝
‑
∞，[θ
min
,θ
max
]为拟合直线段与x轴的夹角区间范围；
[0017]
步骤6.2、对小线段初始点的后续任意点p
m
＝(x
m
,y
m
)，计算点p
j
与点p
m
连成的直线段p
j
p
m
与x轴的夹角θ：
[0018][0019]
步骤6.3、若步骤6.2中计算的θ是否满足θ∈[θ
min
,θ
max
]，则继续步骤6.4计算参数为[θ
min
,θ
max
]的修正系数；
[0020]
若θ不在允许范围内，即θ＜θ
min
或θ＞θ
max
，则将前一直线段p
j
p
m
‑1作为拟合直线段输出；将直线段p
j
p
m
‑1作为拟合直线段后，若后续还有小线段未拟合，即m＜n，则点p
m
‑1为下一拟合直线段初始点，回到步骤6.1开始拟合后续的小线段；若后续无剩余小线段，即m＝n，则结束拟合；
[0021]
步骤6.4、由下面公式计算参数
[0022][0023]
步骤6.5、修正参数θ
max
,θ
min
，
[0024]
步骤6.6、回到步骤6.2，继续拟合下一点；
[0025]
步骤7、直到所有点拟合完成，结束拟合。
[0026]
作为优选方式，所述步骤2中，由以下过程计算向量和的夹角δ:
[0027][0028][0029][0030]
[0031]
δ＝arccosδ。
[0032]
作为优选方式，所述步骤4.1中，通过带约束条件的最小二乘法计算拟合圆弧参数的过程如下：
[0033]
由圆的方程可确定目标函数的形式为：
[0034]
(其中，i为[j,m]中任意点)
[0035]
由拟合圆弧首、尾点的坐标可得限制条件为：
[0036][0037]
由以下公式求解a、b、c最优值：
[0038][0039]
将点数据带入以上公式整理得：
[0040][0041]
设中间变量：
[0042][0043][0044]
化简得：
[0045][0046]
解以上方程得到a、b、c的值。
[0047]
由a、b、c的值计算圆弧参数，圆心(a,b)和半径r：
[0048][0049]
本发明提供一种连续小线段直线拟合方法，包括以下步骤：
[0050]
步骤1、导入连续直线小线段点，设定空间直线的拟合精度erd；
[0051]
步骤2、设小线段的初始点p
j
＝(x
j
,y
j
)，设定初始参数值θ
max
＝+∞,θ
min
＝
‑
∞，[θ
min
,θ
max
]为拟合直线段与x轴的夹角区间范围；
[0052]
步骤3、对小线段初始点的后续任意点p
m
＝(x
m
,y
m
)，计算点p
j
与点p
m
连成的直线段p
j
p
m
与x轴的夹角θ：
[0053][0054]
步骤4、若步骤3中计算的θ是否满足θ∈[θ
min
,θ
max
]，则继续步骤5计算参数为[θ
min
,θ
max
]的修正系数；
[0055]
若θ不在允许范围内，即θ＜θ
min
或θ＞θ
max
，则将前一直线段p
j
p
m
‑1作为拟合直线段输出；将直线段p
j
p
m
‑1作为拟合直线段后，若后续还有小线段未拟合，即m＜n，则点p
m
‑1为下一拟合直线段初始点，回到步骤2开始拟合后续的小线段；若后续无剩余小线段，即m＝n，则结束拟合；
[0056]
步骤5、由下面公式计算参数
[0057][0058]
步骤6、修正参数θ
max
,θ
min
，
[0059]
步骤7、回到步骤1，继续拟合下一点；直到拟合完最后一点，结束拟合。
[0060]
本发发明提供一种连续小线段圆弧拟合方法，包括以下步骤：
[0061]
步骤1、导入连续小线段数据点列p0(x0,y0),p1(x1,y1),
…
,p
n
(x
n
,y
n
)，确定参数erδ，erd，err；erδ为判断程序进入直线拟合还是圆弧拟合的阈值，err为圆弧拟合最大积分误
差δr的允许误差，erd为直线的拟合精度；
[0062]
步骤2、设步骤1中连续小线段中的当前拟合圆弧段的首点坐标p
j
(x
j
,y
j
)，(0≤j≤n)，首先连续取三点p
j
,p
j+1
,p
j+2
拟合圆弧；
[0063]
步骤3、计算拟合圆的参数。设正被拟合成圆弧的数据点列为p
j
(x
j
,y
j
),p
j+1
(x
j+1
,y
j+1
),
…
,p
m
(x
m
,y
m
)，按带约束条件的最小二乘法计算拟合圆弧的参数，包括圆心(a,b)和半径r；
[0064]
步骤4、由步骤3所得圆心(a,b)和半径r，及拟合点列p
j
,p
j+1
,...,p
m
的坐标，计算最大积分误差δr：
[0065][0066]
步骤5、若δr≤err，则继续拟合下一点，使m＝m+1，回到步骤3计算圆弧参数；若δr＞err，输出上一点拟合的圆弧的参数，并将点p
m
‑1作为下一段圆弧拟合的起点；
[0067]
步骤6、直到拟合完最后一点，结束拟合；
[0068]
作为优选方式，所述步骤3中，通过带约束条件的最小二乘法计算拟合圆弧参数的过程如下：
[0069]
由圆的方程可确定目标函数的形式为：
[0070]
(其中，i为[j,m]中任意点)
[0071]
由拟合圆弧首、尾点的坐标可得限制条件为：
[0072][0073]
由以下公式求解a、b、c最优值：
[0074][0075]
将点数据带入以上公式整理得：
[0076][0077]
设中间变量：
[0078][0079][0080]
化简得：
[0081][0082]
解以上方程得到a、b、c的值。
[0083]
由a、b、c的值计算圆弧参数，圆心(a,b)和半径r：
[0084][0085]
本发明涉及一种软控制器连续小线段轨迹预处理方法，与现有设计相比，其优点在于：算法计算复杂度较低，计算时间短，采用一种带约束最小二乘圆弧拟合方法将连续小线段拟合成长圆弧段，将拟合误差减到最小的同时保证拟合圆弧衔接处的连贯性。利用圆弧+直线拟合连续小线段轨迹，可以使加工后零件的表面更加的光滑，并提高加工的效率，提高企业生产效率。
[0086]
将连续轨迹预处理算法植入基于rtx64的开放式运动控制器连续轨迹预处理阶段，同时结合实时工业以太网ethercat总线技术实现软控制器与电机伺服从站设备的通
讯，实现了对轴的精确控制，在不增加硬件成本的条件下，最终提高了设备的加工速度和加工精度，具有较高的应用价值。
附图说明
[0087]
图1为本发明实施例1导入的连续小线段直线拟合的示意图。
[0088]
图2为本发明实施例2导入的连续小线段圆弧拟合的示意图。
[0089]
图3为本发明连续小线段的直线拟合方法流程示意图。
[0090]
图4为本发明连续小线段的圆弧拟合方法流程示意图。
[0091]
图5为本发明连续小线段直线拟合与圆弧拟合方法结合的流程示意图。
具体实施方式
[0092]
下文结合说明书附图和具体实施例对本发明进行详细说明。
[0093]
实施例1
[0094]
本发明涉及一种连续小线段直线的拟合方法，如图3所示，包括以下步骤：
[0095]
步骤1、导入如图1所示的连续小线段点，设定直线拟合精度erd。
[0096]
步骤2、设小线段的初始点p
j
＝(x
j
,y
j
)，设初始参数值θ
max
＝+∞,θ
min
＝
‑
∞(其中，[θ
min
,θ
max
]为拟合直线段与x轴的夹角区间范围)。初始化参数值θ
max
＝+∞,θ
min
＝
‑
∞是为了算法参数的初始化，在后续步骤6中会将区间[θ
min
,θ
max
]进行修正，保证夹角区间的收敛性。
[0097]
步骤3、对小线段初始点的后续任意点p
m
＝(x
m
,y
m
)，计算点p
j
与点p
m
连成的直线段p
j
p
m
与x轴的夹角θ：
[0098][0099]
通过确保拟合的长直线段与x轴的夹角θ一直在收敛的区间范围[θ
min
,θ
max
]，以保证拟合直线的收敛性。
[0100]
步骤4、若所述步骤3中计算的θ在所述步骤2和步骤6设定的区间范围内，即θ∈[θ
min
,θ
max
]，则继续步骤5计算参数(用作步骤6中区间[θ
min
,θ
max
]的修正系数)。因为确保拟合的长直线段与x轴的夹角θ在收敛区间θ∈[θ
min
,θ
max
]，才能保证拟合的直线误差在允许范围。
[0101]
若θ不在允许范围内，即θ＜θ
min
或θ＞θ
max
，则将前一直线段p
j
p
m
‑1作为拟合直线段输出。将直线段p
j
p
m
‑1作为拟合直线段后，若后续还有小线段未拟合，即m＜n，则点p
m
‑1为下一拟合直线段初始点，回到步骤2开始拟合后续的小线段。若后续无剩余小线段，即m＝＝n，则结束拟合。因为，若θ不在允许范围内，则当前点p
m
拟合的误差太大，因此不拟合，所以输出前一点拟合成功的直线段p
j
p
m
‑1。又由于导入的连续小线段点较多时，并不是所有小线段全部拟合在一条长直线段，一般会拟合成多条长直线段，所以拟合完一条长直线段时，还需对剩余连续小线段点继续进行拟合。
[0102]
步骤5、由下面公式计算参数
[0103]
[0104]
由步骤1知，erd为设定的拟合精度，实际上经过以下公式转化为拟合直线段与x轴夹角的允许偏差值。
[0105]
步骤6、修正参数θ
max
,θ
min
。此步骤确保拟合直线段与x轴夹角区间一直是收敛的。
[0106]
步骤7、回到步骤3，继续拟合下一点。
[0107]
实施例2
[0108]
本发明涉及一种连续小线段圆弧拟合方法，如图4所示，包括以下步骤：
[0109]
步骤1、导入如图2所示的连续小线段点列p0(x0,y0),p1(x1,y1),
…
,p
n
(x
n
,y
n
)，确定允许圆弧拟合的误差err(err为点到圆心距离与半径差值的允许误差)。拟合过程判断连续小线段点是否位于拟合圆弧所在圆上，因此需确定圆弧拟合的点整体偏差是否在允许范围err内。
[0110]
步骤2、设步骤1中连续小线段中的当前拟合圆弧段的首点坐标p
j
(x
j
,y
j
)，(0≤j≤n)，首先连续取三点p
j
,p
j+1
,p
j+2
拟合圆弧。
[0111]
步骤3、计算拟合圆的参数。设正被拟合成圆弧的数据点列为p
j
(x
j
,y
j
),p
j+1
(x
j+1
,y
j+1
),
…
,p
m
(x
m
,y
m
)，其中按带约束条件的最小二乘法计算拟合圆弧的参数，包括圆心(a,b)和半径r，圆弧拟合参数计算过程如下：
[0112]
由圆的方程可确定目标函数的形式为：
[0113]
(i为[j,m]中任意点)
[0114]
由拟合圆弧首、尾点的坐标可得限制条件为：
[0115][0116]
由以下公式求解a、b、c最优值：
[0117][0118]
将点数据带入以上公式整理得：
[0119][0120]
设中间变量：
[0121][0122][0123]
化简得：
[0124][0125]
解以上方程得到a、b、c的值。
[0126]
由a、b、c的值计算圆弧参数，圆心(a,b)和半径r：
[0127][0128]
带约束最小二乘圆弧拟合原理是确保拟合圆弧上的点与圆弧的整体偏差值最小，可以求出最优的拟合圆弧，同时为保证拟合的圆弧段是衔接的，拟合圆弧的首点和末点需是此段拟合起始和结尾的小线段点，因此添加首、尾点限制条件。
[0129]
步骤4、由步骤3计算的圆弧圆心(a,b)和半径r，及拟合点列p
j
,p
j+1
,...,p
m
的坐标，计算最大积分误差δr：
[0130]
[0131]
通过计算拟合点列到圆心的距离与圆弧半径的差值的积分δr，来判断连续小线段点是否都位于空间圆弧上。
[0132]
步骤5、若δr≤err，则继续拟合下一点，使m＝m+1，回到步骤3计算圆弧参数。若δr＞err，输出上一点拟合的圆弧的参数，并将点p
m
‑1作为下一段圆弧拟合的起点。
[0133]
考虑连续小线段点列是否位于拟合圆弧上，最大积分误差δr需在允许拟合误差err内。
[0134]
步骤6、直到拟合完最后一点，结束拟合。
[0135]
实施例3
[0136]
本发明涉及一种连续小线段直线拟合与圆弧拟合方法，如图5所示，包括以下步骤：
[0137]
步骤1、导入连续小线段数据点列p0(x0,y0),p1(x1,y1),
…
,p
n
(x
n
,y
n
)，确定参数erδ，erd，err(其中，erδ为判断程序进入直线拟合还是圆弧拟合的阈值，err为圆弧拟合最大积分误差δr的允许误差，erd为直线的拟合精度)。
[0138]
步骤2、设步骤1中连续小线段中的当前拟合线段p
j
(x
j
,y
j
),p
j+1
(x
j+1
,y
j+1
),
…
,p
m
(x
m
,y
m
)(其中)的前三个点坐标p
j
(x
j
,y
j
)，p
j+1
(x
j+1
,y
j+1
)，p
j+2
(x
j+2
,y
j+2
)，向量向量由以下过程计算向量和的夹角δ:
[0139][0140][0141][0142][0143]
δ＝arccosδ
[0144]
通过计算向量和向量的夹角δ，以判断点p
j
、p
j+1
、p
j+2
是否在一条直线上。
[0145]
步骤3、若δ＜erδ，则可认为点p
j
、p
j+1
、p
j+2
位于同一条直线，由此进入步骤6直线拟合过程拟合后续点；若δ≥erδ，则点p
j
、p
j+1
、p
j+2
三点不共线，由此进入步骤4圆弧拟合拟合后续点。
[0146]
步骤4、所述连续圆弧小线段拟合过程为如下，与实施例2相同，对参与拟合的各点p
i
＝(x
i
,y
i
,z
i
)(其中，i为[j,m]中任意点)，圆弧拟合误差是否满足要求，即δr＜err。满足圆弧拟合要求则继续将后续点拟合成圆弧。
[0147]
步骤4.1、计算拟合圆的参数。正被拟合成圆弧的数据点列为p
j
(x
j
,y
j
),p
j+1
(x
j+1
,y
j+1
),
…
,p
m
(x
m
,y
m
)，其中按带约束条件的最小二乘法计算拟合圆弧的参数，包括圆心(a,b)和半径r，圆弧拟合参数计算过程如下：
[0148]
由圆的方程可确定目标函数的形式为：
[0149]
(其中，i为[j,m]中任意点)
[0150]
由拟合圆弧首、尾点的坐标可得限制条件为：
[0151][0152]
由以下公式求解a、b、c最优值：
[0153][0154]
将点数据带入以上公式整理得：
[0155][0156]
设中间变量：
[0157][0158][0159]
化简得：
[0160][0161]
解以上方程得到a、b、c的值。
[0162]
由a、b、c的值计算圆弧参数，圆心(a,b)和半径r：
[0163][0164]
步骤4.2、由步骤4.1计算的圆弧圆心(a,b)和半径r，及拟合点列p
j
,p
j+1
,...,p
m
的坐标，计算最大积分误差δr：
[0165][0166]
步骤4.3、若δr≤err，则继续拟合下一点，使m＝m+1，回到步骤4.1计算圆弧参数。若δr＞err，输出上一点拟合的圆弧的参数，并将点p
m
‑1作为下一段圆弧拟合的起点。
[0167]
步骤4.4、若拟合完最后一点，结束拟合。
[0168]
步骤5、若步骤4中计算的误差超过拟合精度，即δr≥err，则判断已经拟合的点数m
‑
j是否小于4，若m
‑
j≤4，则前面4个点不拟合成圆弧，进入步骤6的直线拟合过程。若m
‑
j＞4，则将为拟合的圆弧段。点p
m
‑1作为后续拟合的初始点，从点p
m
‑1开始下一拟合。
[0169]
拟合的圆弧少于四个点，大概率这四个点位于同一直线上，因进入直线拟合可拟合更多的点。
[0170]
步骤6、直线拟合过程如下，与实施例1相同，若进入直线拟合，若点p
m
满足拟合要求，则继续拟合下一点，不满足拟合要求则输出前一拟合直线段p
j
p
m
‑1。点p
m
‑1作为后续拟合的初始点，从点p
m
‑1开始下一拟合。
[0171]
步骤6.1、设当前拟合小线段的初始点p
j
＝(x
j
,y
j
)，设初始参数值θ
max
＝+∞,θ
min
＝
‑
∞(其中，[θ
min
,θ
max
]为拟合直线段与x轴的夹角区间范围)。
[0172]
步骤6.2、对小线段初始点的后续任意点p
m
＝(x
m
,y
m
)，计算点p
j
与点p
m
连成的直线段p
j
p
m
与x轴的夹角：
[0173][0174]
步骤6.3、若所述步骤6.2中计算的θ在所述步骤6.1和步骤6.5设定的区间范围内，
即θ∈[θ
min
,θ
max
]，则继续步骤6.4计算参数(用作步骤6.5中区间[θ
min
,θ
max
]的修正系数)。
[0175]
若θ不在允许范围内，即θ＜θ
min
或θ＞θ
max
，则将前一直线段p
j
p
m
‑1作为拟合直线段输出。将直线段p
j
p
m
‑1作为拟合直线段后，若后续还有小线段未拟合，即m＜n，则点p
m
‑1为下一拟合直线段初始点，回到步骤6.1开始拟合后续的小线段。若后续无剩余小线段，即m＝n，则结束拟合。
[0176]
步骤6.4、由下面公式计算参数
[0177][0178]
步骤6.5、修正参数θ
max
,θ
min
。此步骤确保拟合直线段与x轴夹角区间一直是收敛的。
[0179]
步骤6.6、回到步骤6.2，继续拟合下一点。
[0180]
步骤7、直到所有点拟合完成，结束拟合。