CN113791585A - 一种圆弧轨迹速度自适应控制方法 - Google Patents
一种圆弧轨迹速度自适应控制方法 Download PDFInfo
- Publication number
- CN113791585A CN113791585A CN202110938026.0A CN202110938026A CN113791585A CN 113791585 A CN113791585 A CN 113791585A CN 202110938026 A CN202110938026 A CN 202110938026A CN 113791585 A CN113791585 A CN 113791585A
- Authority
- CN
- China
- Prior art keywords
- speed
- inflection point
- point
- circular arc
- velocity
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
- G05B19/4163—Adaptive control of feed or cutting velocity
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/36—Nc in input of data, input key till input tape
- G05B2219/36521—Select by combination of detected force, acceleration, speed, work rate
Landscapes
- Engineering & Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种圆弧轨迹速度自适应控制方法,包括:遍历闭合曲线的各坐标点,计算基于各相邻坐标点所组成的线段的斜率,根据斜率计算出相邻线段速度方向变化的夹角作为拐角;根据相邻线段所对应的拐角对拐点速度进行约束;提取闭合曲线中的圆弧轨迹,并计算各个圆弧轨迹的半径、圆心、线速度,根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新;根据更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度,利用速度倒推法对拐点速度进行约束更新;利用加减速算法对闭合曲线进行插补,并限制圆弧轨迹的最大速度,完成闭合曲线的速度规划。本发明能有效提高切割轨迹中圆弧轨迹的切割效率及切割精度。
Description
技术领域
本申请属于运动控制技术领域,具体涉及一种圆弧轨迹速度自适应控制方法。
背景技术
在裁床、模板机等设备的运动控制中,运动轨迹是由上位机发送的PLT文件决定,PLT文件由构成轨迹的一系列坐标点和指令组成,控制器对PLT文件进行解析并通过速度规划算法和插补算法控制电机运动。
目前传统的速度规划算法主要是通过坐标点组成线段所构成的夹角大小对拐角速度进行规划。对于切割轨迹中长直线与圆弧衔接的这类轨迹,衔接点的拐角过小,利用传统的速度规划方法则会造成衔接处的速度过大,导致过切现象;并且对于切割轨迹中半径不同的圆形轨迹,利用传统的速度规划算法则会造成各种圆形轨迹的拐角速度相同,导致切割精度与效率降低。
发明内容
本申请的目的在于提供一种圆弧轨迹速度自适应控制方法,有效提高切割轨迹中圆弧轨迹的切割效率及切割精度。
为实现上述目的,本申请所采取的技术方案为:
一种圆弧轨迹速度自适应控制方法,所述圆弧轨迹速度自适应控制方法,包括:
步骤1、遍历PLT文件中闭合曲线的各坐标点,计算基于各相邻坐标点所组成的线段的斜率,根据斜率计算出相邻线段速度方向变化的夹角作为拐角;
步骤2、以每一坐标点作为拐点,根据相邻线段所对应的拐角对拐点速度进行约束;
步骤3、提取闭合曲线中的圆弧轨迹,并计算各个圆弧轨迹的半径、圆心、线速度,根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新;
步骤4、根据步骤3更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度,利用速度倒推法对拐点速度进行约束更新;
步骤5、利用加减速算法对闭合曲线进行插补,并限制圆弧轨迹的最大速度,完成闭合曲线的速度规划。
以下还提供了若干可选方式,但并不作为对上述总体方案的额外限定,仅仅是进一步的增补或优选,在没有技术或逻辑矛盾的前提下,各可选方式可单独针对上述总体方案进行组合,还可以是多个可选方式之间进行组合。
作为优选,所述计算基于各相邻坐标点所组成的线段的斜率,根据斜率计算出相邻线段速度方向变化的夹角作为拐角,包括:
已知PLT文件中任意连续三点坐标为Pi(xi,yi),Pi+1(xi+1,yi+1),Pi+2(xi+2,yi+2),则计算相邻线段Pi Pi+1和Pi+1Pi+2的拐角θi+1如下:
θi+1=acos(cosα*cosβ+sinα*sinβ)*(180/π)
其中,角α为经过拐点Pi+1前的速度与X轴正方向的夹角,角β为经过拐点Pi+1后的速度与X轴正方向的夹角。
作为优选,所述根据相邻线段所对应的拐角对拐点速度进行约束,包括:
令经过拐点前的速度为v,经过拐点后的速度为v',且速度v和速度v'大小相同、方向不同,速度变化量在一个插补周期内,即速度变化量Δv为:
Δv=amaxT
式中,amax为X轴和Y轴的最大合加速度,T为插补周期;
设速度v与X轴正方向的夹角为α,速度v'与X轴正方向的夹角为β,则速度v和速度v'在X、Y轴方向上的速度分量的大小为:
vx=v cosα
v'x=v'cosβ
vy=v sinα
v'y=v'sinβ
设amax_x、amax_y分别为X轴和Y轴最大允许加速度,则经过拐点前后的速度在X、Y轴方向上的速度变化为:
v'x-vx=v'cosβ-v cosα=amax_xT
v'y-vy=v'sinβ-v sinα=amax_yT
由于速度v'和速度v大小相等,方向不同,则有v'=v,设拐点处的速度为vm,计算出vm的速度为:
且进一步调整拐点速度vm为:
式中,vturn为用户设定的拐弯系数。
作为优选,所述提取闭合曲线中的圆弧轨迹,并计算各个圆弧轨迹的半径、圆心、线速度,根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新,包括:
步骤3.1、取PLT文件中的连续三个坐标点Pi、Pi+1、Pi+2,令初始值i=0,i最大值为N-3,N为闭合曲线中坐标点的总个数,计算这三个点所组成的两条线段Pi Pi+1和Pi+1Pi+2的差值为Δli和对应的拐角为θ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=Δli,且i=i+1并转到步骤3.1;若pointSum不为0,转到步骤3.3;
步骤3.3、判断Δli与D_value的差值的绝对值是否小于设定阈值Ldis,若满足,转到步骤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、判断当前拐点Pi与圆心CentrePoint的距离是否在半径R的阈值内,若满足,则pointSum=pointSum+1,i=i+1,转到步骤3.1;若不满足,转到步骤3.6;
步骤3.6、判断pointSum是否大于6,若大于6,圆弧终点序号Endnum=i,取序号为Startnum、三点坐标确定圆弧,计算圆弧的线速度Vel,Vel=2πnR+Vstart,n为转速,Vstart为起跳速度,并转到步骤3.7;若pointSum小于6,则更新Startnum=i,pointSum=1,D_value=Δli,当前圆弧半径R为0,i=i+1,转到步骤3.1;
步骤3.7、根据圆弧的线速度Vel对此圆弧的各个拐点速度进行约束,并保存此圆弧段的最大速度Vmax:
Vmin=min(vmk)(k=Startnum...Endnum)
式中,vmk为步骤2中由拐角大小约束的拐点速度;
对拐点速度进行约束更新:
vmk=min(Vel,Vmin)(k=Startnum...Endnum)
保存此圆弧的最大速度:
Vmax=vmk
式中,Startnum为此圆弧起始点序号,Endnum为此圆弧终点序号;
且i=i+1,转到步骤3.1。
作为优选,所述利用速度倒推法对拐点速度进行约束更新,包括:
步骤4.1、令相邻两个坐标点组成的线段长度为L0、L1、…、LN-2,其中N为闭合曲线中坐标点的总个数,取步骤3约束更新后各个拐点P0、P1、…、PN-1的拐点速度为vm0、vm1、…、vm(N-1),且起点和终点的速度相等,即vm0=vm(N-1)=Vstart,Vstart为起跳速度;
步骤4.2、假设当前点速度为vmb,b=N-1,...,3,2,1,从当前点开始以S型速度曲线加速,当加速距离达到Lb-1时的速度为v'm(b-1),则更新拐点Pb-1的速度为vm(b-1)=min(vm(b-1),v'm(b-1)),继续以vm(b-1)作为当前点速度以S型速度曲线加速,直到速度倒推至v0。
作为优选,所述利用加减速算法对闭合曲线进行插补,并限制圆弧轨迹的最大速度,包括:
步骤5.1、取由matlab制定生成的S型速度曲线,并离散成f阶,构成加速度表AccTab=[at1,at2...at(j-1)、atj...at(f-1),atf]、加速步长表LenTab=[Lt1,Lt2...Lt(j-1)、Ltj...Lt(f-1),Ltf],j=1...f,用户设定的最大切割速度为Vuser,切割加速度为Auser,计算速度表VTab=[Vt1,Vt2...Vt(j-1)、Vtj...Vt(f-1),Vtf],其中速度表计算如下:
Vtj=Vstart+Lt1*at1*Auser,j=1
Vtj=Vt(j-1)+Ltj*atj*Auser,j≠1
式中,Vstart为预设定的起跳速度,且若Vtj>Vuser,则Vtj=Vuser;
步骤5.2:根据当前速度Vcur、目标速度Vtar、相邻两个坐标点之间的长度Lc,计算加减速距离如下:
取当前速度Vcur、目标速度Vtar中的较小值作为初始速度,以S型速度曲线加速至两速度中的较大值,加速距离为L'acc,若加速距离L'acc≥Lc,则L'acc=Lc;
(1)、若Vcur<Vtar:
(2)、若Vcur≥Vtar:
则计算得到减速距离为Ldec=Lc-Lacc;
步骤5.3:根据计算得到的加速距离和减速距离判断当前插补位置处于加速段或减速段并在每个插补周期内进行相应的加减速,且如果当前相邻两个坐标点之间的线段不为圆弧段,则限制当前插补位置的最大速度为预设的最大允许速度;如果当前相邻两个坐标点之间的线段为圆弧段,则限制当前插补位置的最大速度为圆弧的最大速度Vmax。
本申请提供的圆弧轨迹速度自适应控制方法,根据拐角的大小对拐点速度进行规划,然后鉴别出切割轨迹中的圆弧轨迹,根据圆弧轨迹的半径通过线速度自适应调整圆弧中的拐点速度,最后利用速度倒推法确保拐点速度的可达性,并利用加减速算法对切割轨迹进行插补,能有效的提高切割轨迹中圆弧、圆形轨迹的切割效率及切割精度。
附图说明
图1为本申请的圆弧轨迹速度自适应控制方法的流程图;
图2为本申请计算拐角的示意图;
图3为本申请长直线与圆弧衔接类型以及多圆弧衔接类型的轨迹示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
除非另有定义,本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的,不是在于限制本申请。
其中一个实施例中,提供一种圆弧轨迹速度自适应控制方法,以克服了切割轨迹中圆弧与长直线衔接处的过切现象,提高不同半径圆形轨迹的切割精度和切割效率。
如图1所示,本实施例的圆弧轨迹速度自适应控制方法,包括:
步骤1、遍历PLT文件中闭合曲线的各坐标点,计算基于各相邻坐标点所组成的线段的斜率,根据斜率计算出相邻线段速度方向变化的夹角作为拐角。
如图2所示,已知PLT文件中任意连续三点坐标为Pi(xi,yi),Pi+1(xi+1,yi+1),Pi+2(xi+2,yi+2),则计算相邻线段Pi Pi+1和Pi+1Pi+2的拐角θi+1如下:
θi+1=acos(cosα*cosβ+sinα*sinβ)*(180/π)
其中,角α为经过拐点Pi+1前的速度与X轴正方向的夹角,角β为经过拐点Pi+1后的速度与X轴正方向的夹角。拐角θi+1为对应于坐标Pi+1(即拐点Pi+1)的拐角,令闭合曲线中坐标点的总个数为N,则i的取值范围为0~N-3,为了计算得到各个拐点对应的拐点,在i的取值范围之外,还要额外计算拐点PN-1对应的拐角,取坐标P0,PN-1,PN-2进行计算。
步骤2、以每一坐标点作为拐点,根据相邻线段所对应的拐角对拐点速度进行约束。本实施例中的速度约束应理解为确定速度最大值。
令经过拐点前的速度为v,经过拐点后的速度为v',且速度v和速度v'大小相同、方向不同,速度变化量在一个插补周期内,即速度变化量Δv为:
Δv=amaxT
式中,amax为X轴和Y轴(在轨迹切割中通常基于X轴和Y轴控制刀头完成运行,本实施例基于X轴和Y轴进行规划,但不限于整个切割中只存在X轴和Y轴)的最大合加速度,T为插补周期;
设速度v与X轴正方向的夹角为α,速度v'与X轴正方向的夹角为β,则速度v和速度v'在X、Y轴方向上的速度分量的大小为:
vx=v cosα
v'x=v'cosβ
vy=v sinα
v'y=v'sinβ
设amax_x、amax_y分别为X轴和Y轴最大允许加速度,则经过拐点前后的速度在X、Y轴方向上的速度变化为:
v'x-vx=v'cosβ-v cosα=amax_xT
v'y-vy=v'sinβ-v sinα=amax_yT
由于速度v'和速度v大小相等,方向不同,则有v'=v,设拐点处的速度为vm,计算出vm的速度为:
且为了便于用户对拐点速度可控,进一步调整拐点速度vm为:
式中,vturn为用户设定的拐弯系数。需要说明的是,本实施例中的速度为具有大小和方向的速度,但是在以上计算过程中仅取速度的大小进行计算,即公式中的v,v',vm,vturn表示相应速度的大小。
容易理解的是,以上计算的拐点速度vm为统一表述方式,在具体计算时为了便于建立每一拐点对应的拐点速度,可基于vm进一步添加下标标识进行区分。
步骤3、提取闭合曲线中的圆弧轨迹,并计算各个圆弧轨迹的半径、圆心、线速度,根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新。
步骤3.1、取PLT文件中的连续三个坐标点Pi、Pi+1、Pi+2,令初始值i=0,i最大值为N-3,N为闭合曲线中坐标点的总个数,计算这三个点所组成的两条线段Pi Pi+1和Pi+1Pi+2的差值为Δli和对应的拐角为θ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=Δli,且i=i+1并转到步骤3.1;若pointSum不为0,转到步骤3.3;
步骤3.3、判断Δli与D_value的差值的绝对值是否小于设定阈值Ldis,若满足,转到步骤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、判断当前拐点Pi与圆心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+Vstart,n为转速,Vstart为起跳速度,并转到步骤3.7;若pointSum小于6,则更新Startnum=i,pointSum=1,D_value=Δli,当前圆弧半径R为0,i=i+1,转到步骤3.1;
步骤3.7、根据圆弧的线速度Vel对此圆弧的各个拐点速度进行约束,并保存此圆弧段的最大速度Vmax:
Vmin=min(vmk)(k=Startnum...Endnum)
式中,vmk为步骤2中由拐角大小约束的拐点速度;
对拐点速度进行约束更新:
vmk=min(Vel,Vmin)(k=Startnum...Endnum)
保存此圆弧的最大速度:
Vmax=vmk
式中,Startnum为此圆弧起始点序号,Endnum为此圆弧终点序号;容易理解的是,步骤3.7中限定圆弧段的最大速度时以Vmax进行统一表述,在具体计算时为了便于建立不同圆弧对应的最大速度,可基于Vmax进一步添加下标标识进行区分。
且i=i+1,转到步骤3.1。
步骤4、根据步骤3更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度,利用速度倒推法对拐点速度进行约束更新。
步骤4.1、令相邻两个坐标点组成的线段长度为L0、L1、…、LN-2,其中N为闭合曲线中坐标点的总个数,取步骤3约束更新后各个拐点P0、P1、…、PN-1的拐点速度为vm0、vm1、…、vm(N-1),且起点和终点的速度相等,即vm0=vm(N-1)=Vstart,Vstart为起跳速度;
步骤4.2、假设当前点速度为vmb,b=N-1,...,3,2,1,从当前点开始以S型速度曲线加速,当加速距离达到Lb-1时的速度为v'm(b-1),则更新拐点Pb-1的速度为vm(b-1)=min(vm(b-1),v'm(b-1)),继续以vm(b-1)作为当前点速度以S型速度曲线加速,直到速度倒推至v0。
在速度倒推法中,以vm(N-1)起始开始进行倒推,在倒推时取倒推距离为拐点PN-1和拐点PN-2之间的间距LN-2,当加速距离达到LN-2时得到一个倒推速度v'm(N-2),而在步骤3中约束更新后拐点PN-2对应的速度为vm(N-2),因此需要重新更新约束,更新为vm(N-2)=min(vm(N-2),v'm(N-2)),完成由速度vm(N-1)至vm(N-2)的倒推约束更新。接下去继续以vm(N-2)开始进行倒推,同理类推,直至完成由速度vm1至vm0的倒推约束更新
本实施例中对拐点速度进行约束时,先后采取了三种不同的约束方法,即先基于拐角进行速度约束,后基于圆弧的参数对全部或部分拐点速度约束进行更新,最后基于最新的速度约束利用速度倒推法继续对全部或部分拐点速度进行更新,以此综合考虑了拐点速度的各个影响因素,使得步骤4最终得到的拐点速度具有有效性和可行性。
步骤5、利用加减速算法对闭合曲线进行插补,并限制圆弧轨迹的最大速度,完成闭合曲线的速度规划。
步骤5.1、取由matlab制定生成的S型速度曲线,并离散成f阶,构成加速度表AccTab=[at1,at2...at(j-1)、atj...at(f-1),atf]、加速步长表LenTab=[Lt1,Lt2...Lt(j-1)、Ltj...Lt(f-1),Ltf],j=1...f,用户设定的最大切割速度为Vuser,切割加速度为Auser,计算速度表VTab=[Vt1,Vt2...Vt(j-1)、Vtj...Vt(f-1),Vtf],其中速度表计算如下:
Vtj=Vstart+Lt1*at1*Auser,j=1
Vtj=Vt(j-1)+Ltj*atj*Auser,j≠1
式中,Vstart为预设定的起跳速度,且若Vtj>Vuser,则Vtj=Vuser;
步骤5.2:根据当前速度Vcur、目标速度Vtar、相邻两个坐标点之间的长度Lc,计算加减速距离如下:
取当前速度Vcur、目标速度Vtar中的较小值作为初始速度,以S型速度曲线加速至两速度中的较大值,加速距离为L'acc,若加速距离L'acc≥Lc,则L'acc=Lc;
(1)、若Vcur<Vtar:
(2)、若Vcur≥Vtar:
则计算得到减速距离为Ldec=Lc-Lacc;
由于实际加速段累积插补长度可能与加速距离不重合,即实际运动过程中以加速距离Lacc为目标基准进行运动,但由于运动误差的影响导致实际运动距离与Lacc相比具有一定偏差,因此需要对加速距离进行更新,更新实际加速距离为(其中vci为加速段在每个插补周期的实际速度,T为插补周期),所以实际减速距离为Ldecactual=Lc-Laccactual。
步骤5.3:根据计算得到的加速距离和减速距离判断当前插补位置处于加速段或减速段并在每个插补周期内进行相应的加减速,且如果当前相邻两个坐标点之间的线段不为圆弧段,则限制当前插补位置的最大速度为预设的最大允许速度;如果当前相邻两个坐标点之间的线段为圆弧段,则限制当前插补位置的最大速度为圆弧的最大速度Vmax。
即本申请的速度规划控制后,整个闭合曲线的运行中直线按照预设的最大允许速度进行运动,圆弧按照限制的每段圆弧的最大速度进行运动,并且所有拐点按照步骤4最终更新后的拐点速度进行运动,以保证运动精度和速度。并且本申请的速度规划在如图3所示的长直线与圆弧衔接类型以及多圆弧衔接类型中均具有优异的应用效果。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (6)
1.一种圆弧轨迹速度自适应控制方法,其特征在于,所述圆弧轨迹速度自适应控制方法,包括:
步骤1、遍历PLT文件中闭合曲线的各坐标点,计算基于各相邻坐标点所组成的线段的斜率,根据斜率计算出相邻线段速度方向变化的夹角作为拐角;
步骤2、以每一坐标点作为拐点,根据相邻线段所对应的拐角对拐点速度进行约束;
步骤3、提取闭合曲线中的圆弧轨迹,并计算各个圆弧轨迹的半径、圆心、线速度,根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新;
步骤4、根据步骤3更新后的各个拐点的拐点速度、各相邻坐标点所构成的线段长度,利用速度倒推法对拐点速度进行约束更新;
步骤5、利用加减速算法对闭合曲线进行插补,并限制圆弧轨迹的最大速度,完成闭合曲线的速度规划。
3.如权利要求1所述的圆弧轨迹速度自适应控制方法,其特征在于,所述根据相邻线段所对应的拐角对拐点速度进行约束,包括:
令经过拐点前的速度为v,经过拐点后的速度为v',且速度v和速度v'大小相同、方向不同,速度变化量在一个插补周期内,即速度变化量Δv为:
Δv=amaxT
式中,amax为X轴和Y轴的最大合加速度,T为插补周期;
设速度v与X轴正方向的夹角为α,速度v'与X轴正方向的夹角为β,则速度v和速度v'在X、Y轴方向上的速度分量的大小为:
vx=vcosα
v'x=v'cosβ
vy=vsinα
v'y=v'sinβ
设amax_x、amax_y分别为X轴和Y轴最大允许加速度,则经过拐点前后的速度在X、Y轴方向上的速度变化为:
v'x-vx=v'cosβ-vcosα=amax_xT
v'y-vy=v'sinβ-vsinα=amax_yT
由于速度v'和速度v大小相等,方向不同,则有v'=v,设拐点处的速度为vm,计算出vm的速度为:
且进一步调整拐点速度vm为:
式中,vturn为用户设定的拐弯系数。
4.如权利要求1所述的圆弧轨迹速度自适应控制方法,其特征在于,所述提取闭合曲线中的圆弧轨迹,并计算各个圆弧轨迹的半径、圆心、线速度,根据各个圆弧轨迹的线速度对构成圆弧轨迹的拐点对应的拐点速度进行约束更新,包括:
步骤3.1、取PLT文件中的连续三个坐标点Pi、Pi+1、Pi+2,令初始值i=0,i最大值为N-3,N为闭合曲线中坐标点的总个数,计算这三个点所组成的两条线段PiPi+1和Pi+1Pi+2的差值为Δli和对应的拐角为θ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=Δli,且i=i+1并转到步骤3.1;若pointSum不为0,转到步骤3.3;
步骤3.3、判断Δli与D_value的差值的绝对值是否小于设定阈值Ldis,若满足,转到步骤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、判断当前拐点Pi与圆心CentrePoint的距离是否在半径R的阈值内,若满足,则pointSum=pointSum+1,i=i+1,转到步骤3.1;若不满足,转到步骤3.6;
步骤3.6、判断pointSum是否大于6,若大于6,圆弧终点序号Endnum=i,取序号为Startnum、三点坐标确定圆弧,计算圆弧的线速度Vel,Vel=2πnR+Vstart,n为转速,Vstart为起跳速度,并转到步骤3.7;若pointSum小于6,则更新Startnum=i,pointSum=1,D_value=Δli,当前圆弧半径R为0,i=i+1,转到步骤3.1;
步骤3.7、根据圆弧的线速度Vel对此圆弧的各个拐点速度进行约束,并保存此圆弧段的最大速度Vmax:
Vmin=min(vmk)(k=Startnum...Endnum)
式中,vmk为步骤2中由拐角大小约束的拐点速度;
对拐点速度进行约束更新:
vmk=min(Vel,Vmin)(k=Startnum...Endnum)
保存此圆弧的最大速度:
Vmax=vmk
式中,Startnum为此圆弧起始点序号,Endnum为此圆弧终点序号;
且i=i+1,转到步骤3.1。
5.如权利要求4所述的圆弧轨迹速度自适应控制方法,其特征在于,所述利用速度倒推法对拐点速度进行约束更新,包括:
步骤4.1、令相邻两个坐标点组成的线段长度为L0、L1、…、LN-2,其中N为闭合曲线中坐标点的总个数,取步骤3约束更新后各个拐点P0、P1、…、PN-1的拐点速度为vm0、vm1、…、vm(N-1),且起点和终点的速度相等,即vm0=vm(N-1)=Vstart,Vstart为起跳速度;
步骤4.2、假设当前点速度为vmb,b=N-1,...,3,2,1,从当前点开始以S型速度曲线加速,当加速距离达到Lb-1时的速度为v'm(b-1),则更新拐点Pb-1的速度为vm(b-1)=min(vm(b-1),v'm(b-1)),继续以vm(b-1)作为当前点速度以S型速度曲线加速,直到速度倒推至v0。
6.如权利要求5所述的圆弧轨迹速度自适应控制方法,其特征在于,所述利用加减速算法对闭合曲线进行插补,并限制圆弧轨迹的最大速度,包括:
步骤5.1、取由matlab制定生成的S型速度曲线,并离散成f阶,构成加速度表AccTab=[at1,at2...at(j-1)、atj...at(f-1),atf]、加速步长表LenTab=[Lt1,Lt2...Lt(j-1)、Ltj...Lt(f-1),Ltf],j=1...f,用户设定的最大切割速度为Vuser,切割加速度为Auser,计算速度表VTab=[Vt1,Vt2...Vt(j-1)、Vtj...Vt(f-1),Vtf],其中速度表计算如下:
Vtj=Vstart+Lt1*at1*Auser,j=1
Vtj=Vt(j-1)+Ltj*atj*Auser,j≠1
式中,Vstart为预设定的起跳速度,且若Vtj>Vuser,则Vtj=Vuser;
步骤5.2:根据当前速度Vcur、目标速度Vtar、相邻两个坐标点之间的长度Lc,计算加减速距离如下:
取当前速度Vcur、目标速度Vtar中的较小值作为初始速度,以S型速度曲线加速至两速度中的较大值,加速距离为L'acc,若加速距离L'acc≥Lc,则L'acc=Lc;
(1)、若Vcur<Vtar:
(2)、若Vcur≥Vtar:
则计算得到减速距离为Ldec=Lc-Lacc;
步骤5.3:根据计算得到的加速距离和减速距离判断当前插补位置处于加速段或减速段并在每个插补周期内进行相应的加减速,且如果当前相邻两个坐标点之间的线段不为圆弧段,则限制当前插补位置的最大速度为预设的最大允许速度;如果当前相邻两个坐标点之间的线段为圆弧段,则限制当前插补位置的最大速度为圆弧的最大速度Vmax。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110938026.0A CN113791585B (zh) | 2021-08-16 | 2021-08-16 | 一种圆弧轨迹速度自适应控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110938026.0A CN113791585B (zh) | 2021-08-16 | 2021-08-16 | 一种圆弧轨迹速度自适应控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113791585A true CN113791585A (zh) | 2021-12-14 |
CN113791585B CN113791585B (zh) | 2023-09-22 |
Family
ID=79181823
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110938026.0A Active CN113791585B (zh) | 2021-08-16 | 2021-08-16 | 一种圆弧轨迹速度自适应控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113791585B (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114985929A (zh) * | 2022-06-30 | 2022-09-02 | 广东金橙子科技有限公司 | 激光运动轨迹处理方法、装置和激光设备 |
CN115534133A (zh) * | 2022-11-11 | 2022-12-30 | 泉州市大鲨鱼机械科技有限公司 | 绳锯切割装置及切割方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH04268609A (ja) * | 1990-12-10 | 1992-09-24 | Philips Gloeilampenfab:Nv | 機械加工装置 |
JPH0764622A (ja) * | 1993-08-26 | 1995-03-10 | Matsushita Electric Ind Co Ltd | ロボットの軌道補間装置 |
CN101751008A (zh) * | 2008-12-18 | 2010-06-23 | 陈学恭 | 一种插补方法 |
CN110355365A (zh) * | 2019-06-13 | 2019-10-22 | 西安交通大学 | 一种协同840d数控***的ebm变方向成形动态切片方法 |
CN110879569A (zh) * | 2019-12-18 | 2020-03-13 | 湖南戈人自动化科技有限公司 | 一种前瞻控制方法 |
CN111897290A (zh) * | 2020-08-06 | 2020-11-06 | 南京工程学院 | 一种轴加加速度光滑的拐角过渡平滑方法 |
CN112965443A (zh) * | 2021-02-08 | 2021-06-15 | 浙江工业大学 | 一种裁床拐角轨迹跟踪高精度插值控制方法 |
-
2021
- 2021-08-16 CN CN202110938026.0A patent/CN113791585B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH04268609A (ja) * | 1990-12-10 | 1992-09-24 | Philips Gloeilampenfab:Nv | 機械加工装置 |
JPH0764622A (ja) * | 1993-08-26 | 1995-03-10 | Matsushita Electric Ind Co Ltd | ロボットの軌道補間装置 |
CN101751008A (zh) * | 2008-12-18 | 2010-06-23 | 陈学恭 | 一种插补方法 |
CN110355365A (zh) * | 2019-06-13 | 2019-10-22 | 西安交通大学 | 一种协同840d数控***的ebm变方向成形动态切片方法 |
CN110879569A (zh) * | 2019-12-18 | 2020-03-13 | 湖南戈人自动化科技有限公司 | 一种前瞻控制方法 |
CN111897290A (zh) * | 2020-08-06 | 2020-11-06 | 南京工程学院 | 一种轴加加速度光滑的拐角过渡平滑方法 |
CN112965443A (zh) * | 2021-02-08 | 2021-06-15 | 浙江工业大学 | 一种裁床拐角轨迹跟踪高精度插值控制方法 |
Non-Patent Citations (2)
Title |
---|
HUI DONG: "A LADRC based fuzzy PID approach to contour error control of networked motion control system with time‐varying delays", 《ASIAN JOURNAL OF CONTROL》 * |
康磊: "裁床运动控制***的开发与实现", 《中国优秀硕士学位论文全文数据库工程科技Ⅰ辑》 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114985929A (zh) * | 2022-06-30 | 2022-09-02 | 广东金橙子科技有限公司 | 激光运动轨迹处理方法、装置和激光设备 |
CN114985929B (zh) * | 2022-06-30 | 2023-01-17 | 广东金橙子科技有限公司 | 激光运动轨迹处理方法、装置和激光设备 |
CN115534133A (zh) * | 2022-11-11 | 2022-12-30 | 泉州市大鲨鱼机械科技有限公司 | 绳锯切割装置及切割方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113791585B (zh) | 2023-09-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109623820B (zh) | 一种机器人空间轨迹过渡方法 | |
CN108549328B (zh) | 自适应速度规划方法及*** | |
CN113791585A (zh) | 一种圆弧轨迹速度自适应控制方法 | |
CN107436555B (zh) | 非零初始状态s型停止曲线速度规则及其在线求解方法 | |
US20200004228A1 (en) | A feedrate scheduling method for five-axis dual-spline curve interpolation | |
CN110900612B (zh) | 一种位姿同步的六轴工业机器人轨迹平顺方法 | |
JP6740477B2 (ja) | 自動駐車制御装置 | |
CN104160617B (zh) | 电动机控制装置 | |
CN106926840A (zh) | 一种无人车辆极限动力学轨迹跟踪控制*** | |
WO2017113416A1 (zh) | 加工轨迹平滑转接的方法及加工装置 | |
CN108940759B (zh) | 连续加工路径的恒定胶量的控制方法及*** | |
CN111806467A (zh) | 一种基于车辆行驶规律的变速动态换道轨迹规划方法 | |
CN112965443B (zh) | 一种裁床拐角轨迹跟踪高精度插值控制方法 | |
CN109048091B (zh) | 激光切割速度规划方法、装置、存储介质及计算机设备 | |
CN111158318B (zh) | 一种非对称性四次曲线柔性加减速规划方法 | |
CN108594815A (zh) | 一种分阶段的轮式机器人移动路径规划方法 | |
CN109814496B (zh) | 一种s型加减速轨迹规划中多轴时间同步方法 | |
CN110789530B (zh) | 一种四轮独立转向-独立驱动车辆轨迹跟踪方法和*** | |
CN110456801B (zh) | 一种自动驾驶汽车的跟车控制方法、装置及汽车 | |
CN110162073B (zh) | 一种适应边界约束的分段正弦姿态机动轨迹规划方法 | |
CN110543193B (zh) | 一种用于指向机构的在线加减速控制方法、***及介质 | |
CN113190021A (zh) | 一种工业机器人小线段轨迹规划算法 | |
CN108594757A (zh) | 一种基于位置和姿态约束的机器人小线段前瞻规划方法 | |
CN116300698A (zh) | 一种基于动态规划的螺纹切削加工方法 | |
CN113741336A (zh) | 基于实时运动的笛卡尔空间轨迹规划方法和*** |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
TA01 | Transfer of patent application right |
Effective date of registration: 20230814 Address after: 201306 building C, No. 888, Huanhu West 2nd Road, Lingang New District, China (Shanghai) pilot Free Trade Zone, Pudong New Area, Shanghai Applicant after: Shanghai ruishenglian Information Technology Co.,Ltd. Address before: Room 312, Research Building 1 # -1, Xianhua Base, No. 589 Longtan Road, Qiubin Street, Wucheng District, Jinhua City, Zhejiang Province, 321016 Applicant before: Jinhua Kaihui Intelligent Technology Co.,Ltd. |
|
TA01 | Transfer of patent application right | ||
GR01 | Patent grant | ||
GR01 | Patent grant |