一种快速规划6阶S型运动曲线的方法技术

本发明专利技术公开一种快速规划6阶S型运动曲线的方法，通过已知的运动参数距离L

【技术实现步骤摘要】
一种快速规划6阶S型运动曲线的方法
本专利技术涉及精密运动控制，特别适用于对运动控制要求高速、平稳的速度曲线规划。
技术介绍
在带运动单元的自动化设备中，为了保证设备的运动单元在运动启动、停止、变速过程中避免产生冲击、失步、震荡和超程，以保证在高速运动情况下设备整体的平稳。输出给运动驱动器的控制信号必须要进行精细的过程规划，常见的运动过程规划有梯形与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)/Lx2-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本文档来自技高网...

【技术保护点】
1.一种快速规划6阶S型运动曲线的方法，其特征在于，包括如下步骤：/n步骤1，输入待规划的运动曲线参数：轨迹长度L

【技术特征摘要】
1.一种快速规划6阶S型运动曲线的方法，其特征在于，包括如下步骤：
步骤1，输入待规划的运动曲线参数：轨迹长度Ld、启动速度Vs、结束速度Ve、最大加加速Jm、最大允许速度Vm；
步骤2，使用Jm与Vm作为约束条件，计算Vs、Ve、Ld之间的关系，将待规划的运动曲线分为加速阶段、匀速阶段和减速阶段，其中加速阶段记作H分段，匀速阶段记作B分段，减速阶段记作T分段，待规划的S型曲线上会出现的运动状态分段情况为H、T、B、BT、HB、HT、HBT几种分段中的一种，对待规划的S型曲线上的运动状态组合进行判断；
步骤3，对存在的H分段和T分段的运动曲线，使用5阶贝塞尔曲线规划速度曲线；
步骤4，对步骤三中规划出的速度曲线进行等间隔采样，将速度曲线等分为N份，每份时间间隔为h，采用向前差分的方式计算出速度曲线上的离散V值。


2.根据权利要求1所述的一种快速规划6阶S型运动曲线的方法，其特征在于，S型运动曲线上已知某段的初始速度Vi与末速度Vf可计算移动距离Lx，具体公式如下；



S型运动曲线上已知某段的初始速度Vi与移动距离Lx计算该段的末速度Vf，具体公式如下；



通过公式(1)和(2)计算，可对待规划的S型曲线上的运动状态分段组合进行判断，具体方法如下：
步骤2.1如果Vs＝Ve，则该段为匀速运动状态过程，为B分段，结束步骤二，否则进入步骤2.2；
步骤2.2将Vs与Ve代入式(1)，得到速度变化期间实际运动距离Lc，进入步骤2.2.1；
步骤2.2.1如果Lc≥Ld，且Vs<Ve时，说明需要实际运行更远的距离，速度才能从Vs变化成Ve，而此时Vs又小于Ve，则在设定的距离Ld内一直做加速运动，为H分段模式，由公式(2)可计算得到该段实际的末速度Vc，进入步骤三，否则进入步骤2.2.2；
步骤2.2.2如果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分段，即该段为先匀速后减速运动模式，进入步骤三，否则进入步骤2.2.4；
步骤2.2.4如果Lc<Ld，将Vs与Vm代入式(1)，得到加速距离Lh，如果Lh<Ld，且Vm＝Ve，为HB段模式，即该段为先加速后匀速运动模式，进入步骤三，否则进入步骤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分段，曲线先加速，然后...

【专利技术属性】
技术研发人员：董杰楚，徐亦新，王飞，王志超，黄少强，
申请(专利权)人：嘉兴市像景智能装备有限公司，
类型：发明
国别省市：浙江;33