一种快速规划6阶s型运动曲线的方法
阅读说明:本技术 一种快速规划6阶s型运动曲线的方法 (Method for rapidly planning 6-order S-shaped motion curve ) 是由 董杰楚 徐亦新 王飞 王志超 黄少强 于 2020-06-10 设计创作,主要内容包括:本发明公开一种快速规划6阶S型运动曲线的方法,通过已知的运动参数距离L<Sub>d</Sub>、启动速度V<Sub>s</Sub>、结束速度V<Sub>e</Sub>、最大加加速J<Sub>m</Sub>、最大允许速度V<Sub>m</Sub>将待规划的S型速度曲线分为H(Head)加速阶段、B(Body)匀速阶段、T(Tail)减速阶段三种分段;对H与T段使用5阶贝塞尔曲线的方法来规划6阶S型速度曲线,同时设计了一种前向差分的计算法来提升计算效率;最后对速度曲线进行等间隔采样,分别设计了脉冲和总线型控制器的位置数据更新计算方法。(The invention discloses a method for rapidly planning a 6-order S-shaped motion curve, which is implemented by using a known motion parameter distance L d Starting speed V s End velocity V e Maximum acceleration J m Maximum allowable speed V m Dividing an S-shaped speed curve to be planned into three segments, namely an H (head) acceleration stage, a B (body) constant speed stage and a T (Tail) deceleration stage; planning a 6-order S-shaped speed curve by using a 5-order Bezier curve method for H and T sections, and simultaneously designing a forward difference calculation method to improve the calculation efficiency; and finally, sampling the speed curve at equal intervals, and designing position data updating and calculating methods of the pulse controller and the bus controller respectively.)
技术领域
本发明涉及精密运动控制,特别适用于对运动控制要求高速、平稳的速度曲线规划。
背景技术
在带运动单元的自动化设备中,为了保证设备的运动单元在运动启动、停止、变速过程中避免产生冲击、失步、震荡和超程,以保证在高速运动情况下设备整体的平稳。输出给运动驱动器的控制信号必须要进行精细的过程规划,常见的运动过程规划有梯形与S型曲线两种,梯形曲线计算量少,易于实现,但由于加速轨迹无平滑过程,存在力矩冲击,工程中比较少应用。S型曲线加入了加速度的控制,能有效减少力矩的突变,能更好的控制电机的力矩输出,可以让系统在达到更高加速度和控制精度的同时减少机械上的冲击提高设备的可靠性。
S曲线根据其控制的精细度分体现在对位置曲线求导阶数上,1阶Velocity,2阶Acceleration,3阶Jerk,4阶Snap,5阶Crackle,6阶Pop,阶数越高轨迹规划算法越复杂,控制效果越好,目前市面商用运动控制器多为3阶曲线控制,部分特殊行业专用设备能做到4阶曲线,使用传统方法计算S曲线依赖非常细致的分段,例如4阶S型曲线就需要分为14段进行计算,分段条件繁杂这使得轨迹解析非常繁复,实际中使用容易出错,对实现的硬件的要求也非常高,6阶曲线用传统方法属难于实现的技术。
发明内容
本发明针对当前6阶S型运动曲线规划难以可靠实现的问题,提供一种快速规划6阶S型运动曲线的方法,所述方法是基于等间隔采样贝塞尔曲线来实现6阶运动曲线规划的方法,易于多平台实现和移植。
本发明的一种快速规划6阶S型运动曲线的方法,包括如下步骤:
步骤1,输入待规划的运动曲线参数:轨迹长度Ld、启动速度Vs、结束速度Ve、最大加加速Jm、最大允许速度Vm。
步骤2,使用Jm与Vm作为约束条件,计算Vs、Ve、Ld之间的关系,将待规划的运动曲线分为加速阶段、匀速阶段和减速阶段,其中加速阶段记作H分段,匀速阶段记作B分段,减速阶段记作T分段,待规划的S型曲线上会出现的运动状态分段情况为HBT、HB、BT、HT、H、T、B几种中的一种,对待规划的S型曲线上的运动状态组合进行判断;
步骤3,对存在的H分段和T分段的运动曲线,使用5阶贝塞尔曲线规划速度曲线;
步骤4,对步骤三中规划出的速度曲线进行等间隔采样,将速度曲线等分为N份,每份时间间隔为h,采用向前差分的方式计算出速度曲线上的离散V值,即可在不同硬件平台上实现6阶控制曲线。
有益效果:
本发明的一种快速规划6阶S型运动曲线的方法,采用前向差分计算的贝塞尔曲线方法,将算法计算量降低,使得多平台可实现;抛弃传统方法中将多阶运动控制曲线分解为多段,使用众多条件判断来分段执行的思路,采用简易分类将运动曲线分为加速,匀速,减速三种类型或其结合,对加速或减速过程直接规划出速度曲线,规划阶段简单直观,大大增加了算法的鲁棒性。
附图说明
图1为本发明的整体步骤流程图。
图2为本发明的对曲线上分段组合情况进行判断的流程图
具体实施方式
为了使本领域技术人员更好地理解本发明的技术方案,下面对本发明进行抛开推到过程的使用描述,本部分的描述仅是示范性和解释性,不应对本发明的保护范围有任何限制作用
本发明采用的技术方案的步骤如下:
步骤一:输入待规划曲线的参数,轨迹长度Ld、启动速度Vs、结束速度Ve、最大加加速Jm、最大允许速度Vm。
步骤二:使用Jm与Vm作为约束条件,计算Vs、Ve、Ld之间的关系;将待规划的S型曲线按运动状态分段,分别为加速阶段、匀速阶段和减速阶段,其中加速阶段记作H分段,匀速阶段记作B分段,减速阶段记作T分段,待规划的S型曲线上会出现的运动状态组合情况为HBT、HB、BT、HT、H、T、B几种中的一种,对待规划的S型曲线上的运动状态组合进行判断。
步骤三:对于存在H分段与T分段的运动曲线,使用5阶贝塞尔曲线进行速度曲线的规划。
步骤四:对步骤三中规划出的速度曲线进行等间隔采样,等分为N份,每份时间间隔为h,根据需要控制的精细程度调节采样间隔,使用前向差分的方式计算此贝塞尔曲线,对应到S型运动曲线相当于使用线性的Pop作为速度规划,即6阶段控制,在不同硬件平台上实现6阶控制曲线,所述硬件平台是指脉冲型控制器和总线型控制器的硬件方案
所述步骤二中,对待规划的S型曲线上的运动状态分段组合进行判断,需要依据如下公式:
依据输入初始速度Vi与末速度Vf计算移动距离Lx的公式;
依据初始速度Vi与移动距离Lx计算末速度Vf的公式;
具体推倒过程如下,在推倒过程中使用固定的加加速Jm;
运动时间T的表达式为:
S曲线凹凸位置拐点加速度As的表达式为:
加速度变化率Ar的表达式为:
运动距离Lx的表达式为:
带入变量化简后得到:
由式(1)化简可得末速度Vf:
末速度Vf中用到了开3次方的计算,在资源有限的系统中计算可能会带入计算误差,使用牛顿-拉弗森方法来进一步逼近计算结果。
由以上推导过程构造0根等式,方便计算转换为Z(Vf)=((Vf-Vi)3)/Lx 2-Jm。
对Z(x)求在x处的导数:
使用进行迭代计算即可逼近真实值,其中n为整数,在资源有限的系统中使用牛顿-拉弗森进行两次迭代即可达到可用计算精度。
步骤二中,对待规划的S型曲线上的运动状态分段组合进行判断,具体如下;
步骤2.1,如果Vs=Ve,则该段为匀速运动状态过程,为B段模式,结束步骤二,退出曲线规划步骤,否则进入步骤2.2。
步骤2.2如果Vs≠Ve,将Vs与Ve代入式(1),得到速度变化期间实际运动距离Lc:
步骤2.2.1,如果Lc≥Ld,且Vs<Ve时,说明需要实际运行更远的距离,速度才能从Vs变化成Ve,而此时Vs又小于Ve,则在设定的距离Ld内一直做加速运动,为H段模式,由公式(2)可计算得到该段实际的末速度Vc,
进入步骤三,否则进入步骤2.2.2。
步骤2.2.1,如果Lc≥Ld,且Vs>Ve时,说明需要实际运行更远的距离,速度才能从Vs变化成Ve,而此时Vs又大于Ve,则在设定的距离Ld内一直做减速运动,为T段,由公式(2)可计算得到该段实际的末速度Vc,进入步骤三,否则进入步骤2.2.3。
步骤2.2.3,如果Lc<Ld,且Vm=Vs,将Vs与Ve代入式(1),得到减速状态下运动的距离Lt。并且当Lt<Ld时,为BT分段,即该段为先以Vs的速度匀速运动距离Lb后再减速运动,其中Lb=Ld-Lt。进入步骤三,对BT分段中的T分段进行速度曲线规划,否则进入步骤2.2.4。
步骤2.2.4,如果Lc<Ld,将Vs与Vm代入式(1),得到加速距离Lh,如果Lh<Ld,且Vm=Ve,为HB段模式,即该段为先加速到Vm后,再以Vm的速度运动。进入步骤三,对HB分段中的H分段进行速度曲线规划,否则进入步骤2.2.5。
步骤2.2.5,如果Lc<Ld,将Vs与Vm代入式(1),得到加速状态下运动的距离Lh,将Vm与Ve代入式(1),得到减速状态下运动的距离Lt;进入步骤2.2.5.1
步骤2.2.5.1,如果Ld<(Lh+Lt),为HT分段,即该段曲线为先加速,然后立即进入减速运动模式,进入步骤三,否则进入步骤2.2.5.2;
步骤2.2.5.2,当Ld>(Lh+Lt)时,为HBT分段,曲线先加速,然后进入匀速,最后减速,中间匀速运状态下运动的距离为Lb=Ld-(Lh+Lt),进入步骤三。
所述步骤3中5阶贝塞尔曲线计算,采用前向差分的方法进行计算,算法计算推导过程:
根据贝塞尔曲线通用表达式:导出由5阶贝塞尔曲线生成的速度曲线表达式:
V(t)=P0B0(t)+P1B1(t)+P2B2(t)+P3B3(t)+P4B4(t)+P5B5(t) (3)
其中0≤t≤1,V(t)为速度,P0~P5为贝塞尔控制点;
其中B0(t)~B5(t)为伯恩斯坦多项式
B0(t)=(1-t)5=-t5+5t4-10t3+10t2-5t+1
B1(t)=5(1-t)4×t=5t5-20t4+30t3-20t2+5t
B2(t)=10(1-t)3×t2=-10t5+30t4-30t3+10t2
B3(t)=10(1-t)2×t3=10t5-20t4+10t3
B4(t)=5(1-t)×t4=-5t5+5t4
B5(t)=t5
根据式(3)速度曲线可以表达为:
V(t)=At5+Bt4+Ct3+Dt2+Et+F
其中系数A~F可分别表达如下:
A=-P0+5P1-10P2+10P3-5P4+P5
B=5P0-20P1+30P2-20P3+5P4
C=-10P0+30P1-30P2+10P3
D=10P0-20P1+10P2
E=-5P0+5P1
F=P0
H分段和T分段的运动曲线两端点处加速度和加加速都为零,H分段或T分段的速度曲线的初始速度记做Vi,末速度记作Vf,设置差分计算初始阶段P0=P1=P2=Vi,P0、P1和P2均等于初始速度Vi,P3=P4=P5=Vf,P3、P4和P5均等于末速度Vf。
带入化简A~F系数表达式
A=-6Vi+6Vf
B=15Vi-15Vf
C=-10Vi+10Vf
D=0
E=0
F=Vi
所以速度曲线可以表达为:
V(t)=(-6Vi+6Vf)t5+(15Vi-15Vf)Bt4+(-10Vi+10Vf)t3+Vi
前向差分算法条件,假定从速度从Vi~Vf均分为N步,步长差分表达式F1~F5的初始值。
在已有初始条件下,使用如示公式迭代计算N次
V=V+F5
F5=F5+F4
F4=F4+F3
F3=F3+F2
F2=F2+F1
即可将整个速度曲线的离散V值算出。
初始值计算推导:为方便推到中系数迭代变换过程的表述,设定a=A,b=B,c=C,d=D,e=E,f=F;
F5(t)=at5+bt4+ct3+dt2+et+f
F5(t+h)-F5(t)
=(5ah)t4+(10ah2+4bh)t3+(10ah3+6bh2+3ch)t2
+(5ah4+4bh3+3ch2+2dh)t+ah5+bh4+ch3+dh2+eh
由上一步的推导设定
a=5Ah,b=10Ah2+4Bh,c=10Ah3+6Bh2+3Ch;
d=5Ah4+4Bh3+3Ch2+2Dh
F4(t)=at4+bt3+ct2+dt+e
F4(t+h)-F4(t)
=(20ah2)t3+(60ah3+12bh2)t3+(70ah4+24bh3+6ch2)t
+30ah5+14bh4+6ch3+2dh2
由上一步的推导设定
a=20Ah2,b=60Ah3+12Bh2,c=70Ah4+24Bh3+6Ch2
F3(t)=at3+bt2+ct+d
F3(t+h)-F3(t)=(60ah3)t2+(180ah4+24bh3)t+150ah5+36bh4+6ch3
由上一步的推导设定
a=60Ah3,b=180Ah4+24Bh3
F2(t)=at2+bt+c
F2(t+h)-F2(t)=(120ah4)t+240ah5+24bh4
由上一步的推导设定
a=120Ah4
F1(t)=at+b
F1(t+h)-F1(t)=120ah5
由于我们是将速度曲线以h为间隔均分,每一个速度点代表这一段内的速度,那么初始值计算中的初始时间点同时迭代次数变为N-1,带入初始时间点t0,计算可得差分计算的初始值:
F3(t0)=255Ah5+48Bh4+6Ch3
F2(t0)=300Ah5+24Bh4
F1(t0)=120Ah5
所述步骤4中等间隔采样,采样间隔为步骤三中定义的h,分段数为N-1。
脉冲型控制器,在每一个速度更新点上使用频率输出时长h的脉冲序列,直到迭代完成,即可完成控制输出;
总线型控制器,在每一个速度更新点上使用V(t+h)=V(t)+F5,输出V(t+h),直到迭代完成,即可完成控制输出,V(t+h)即为运动控制需要输出的控制位置。
对待规划的S型曲线上可能会出现的运动状态组合,分别进行速度规划的说明如下。
已知待规划的曲线的轨迹长度Ld、启动速度Vs、结束速度Ve、最大加加速Jm、最大允许速度Vm,经过步骤2中的判断后;
如果判断后该段为B分段曲线,B分段由于是匀速运动段故无须做速度曲线变换,B分段速度曲线为恒定值,B分段的速度值V=Vs=Ve;完成运行曲线规划;
如果判断后该段为H分段曲线,该段曲线的速度从Vs加速到实际的末速度Vc,根据式(2)可知实际末速度Vc的值其中通过本发明的步骤3可以规划出速度从Vs到实际末速度Vc的速度变化曲线,通过本发明的步骤4可以计算出所述曲线上Vs到Vc之间离散的速度值,完成运行曲线规划;
如果判断后该段为T分段曲线,该段曲线的速度从Vs减速到实际的末速度Vc,其中通过本发明的步骤3可以规划出速度从Vs到实际末速度Vc的速度变化曲线,通过本发明的步骤4可以计算出所述曲线上Vs到Vc之间离散的速度值,完成运行曲线规划;
如果判断后该段为BT分段曲线,该段曲线的速度开始时保持在Vs,再从Vs减速到Ve,其中对T分段部分采用本发明中步骤3和4的方法进行速度的计算,完成运行曲线规划;
如果判断后该段为HB分段曲线,该段曲线的速度从Vs加速到最大允许速度Vm,再以速度Vm一直匀速运动。其中通过本发明的步骤3规划出从速度从Vs到最大允许速度Vm之间的速度变化曲线,通过本发明的步骤4可以计算出所述速度变化曲线上Vs到Vm之间离散的速度值,完成运行曲线规划;
如果判断后该段为HT分段曲线,该段曲线的速度从Vs加速到Vm,再从Vm减速到Ve,其中对H和T分段采用本发明中步骤3和步骤4的方法进行速度的计算,完成运行曲线规划;
如果判断后该段为HBT分段曲线,该段曲线的速度从Vs加速到Vm,然后进入B分段以Vm匀速运行Lb长度后进入T分段从Vm减速到Ve,其中对H和T分段采用本发明中步骤3和4的方法进行速度的计算,完成运行曲线规划。
- 上一篇:一种医用注射器针头装配设备
- 下一篇:一种基于模糊PI的频率跟踪搜索方法