CN111596543B - 一种快速规划6阶s型运动曲线的方法 - Google Patents

Abstract

本发明公开一种快速规划6阶S型运动曲线的方法，通过已知的运动参数距离L_d、启动速度V_s、结束速度V_e、最大加加速J_m、最大允许速度V_m将待规划的S型速度曲线分为H(Head)加速阶段、B(Body)匀速阶段、T(Tail)减速阶段三种分段；对H与T段使用5阶贝塞尔曲线的方法来规划6阶S型速度曲线，同时设计了一种前向差分的计算法来提升计算效率；最后对速度曲线进行等间隔采样，分别设计了脉冲和总线型控制器的位置数据更新计算方法。

Description

一种快速规划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，输入待规划的运动曲线参数：轨迹长度L_d、启动速度V_s、结束速度V_e、最大加加速J_m、最大允许速度V_m。

步骤2，使用J_m与V_m作为约束条件，计算V_s、V_e、L_d之间的关系，将待规划的运动曲线分为加速阶段、匀速阶段和减速阶段，其中加速阶段记作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为本发明的对曲线上分段组合情况进行判断的流程图。

具体实施方式

为了使本领域技术人员更好地理解本发明的技术方案，下面对本发明进行抛开推到过程的使用描述，本部分的描述仅是示范性和解释性，不应对本发明的保护范围有任何限制作用

本发明采用的技术方案的步骤如下：

步骤一：输入待规划曲线的参数，轨迹长度L_d、启动速度V_s、结束速度V_e、最大加加速J_m、最大允许速度V_m。

步骤二：使用J_m与V_m作为约束条件，计算V_s、V_e、L_d之间的关系；将待规划的S型曲线按运动状态分段，分别为加速阶段、匀速阶段和减速阶段，其中加速阶段记作H分段，匀速阶段记作B分段，减速阶段记作T分段，待规划的S型曲线上会出现的运动状态组合情况为HBT、HB、BT、HT、H、T、B几种中的一种，对待规划的S型曲线上的运动状态组合进行判断。

步骤三：对于存在H分段与T分段的运动曲线，使用5阶贝塞尔曲线进行速度曲线的规划。

步骤四：对步骤三中规划出的速度曲线进行等间隔采样，等分为N份，每份时间间隔为h，根据需要控制的精细程度调节采样间隔，使用前向差分的方式计算此贝塞尔曲线，对应到S型运动曲线相当于使用线性的Pop作为速度规划，即6阶段控制，在不同硬件平台上实现6阶控制曲线，所述硬件平台是指脉冲型控制器和总线型控制器的硬件方案

所述步骤二中，对待规划的S型曲线上的运动状态分段组合进行判断，需要依据如下公式：

依据输入初始速度V_i与末速度V_f计算移动距离L_x的公式；

依据初始速度V_i与移动距离L_x计算末速度V_f的公式；

具体推倒过程如下，在推倒过程中使用固定的加加速J_m；

运动时间T的表达式为：

S曲线凹凸位置拐点加速度As的表达式为：

加速度变化率Ar的表达式为：

运动距离Lx的表达式为：

带入变量化简后得到：

由式(1)化简可得末速度V_f：

末速度V_f中用到了开3次方的计算，在资源有限的系统中计算可能会带入计算误差，使用牛顿-拉弗森方法来进一步逼近计算结果。

由以上推导过程构造0根等式，方便计算转换为Z(V_f)＝((V_f-V_i)³)/L_x ²-J_m。

对Z(x)求在x处的导数：

使用进行迭代计算即可逼近真实值，其中n为整数，在资源有限的系统中使用牛顿-拉弗森进行两次迭代即可达到可用计算精度。

步骤二中，对待规划的S型曲线上的运动状态分段组合进行判断，具体如下；

步骤2.1，如果V_s＝V_e，则该段为匀速运动状态过程，为B段模式，结束步骤二，退出曲线规划步骤，否则进入步骤2.2。

步骤2.2如果V_s≠V_e，将V_s与V_e代入式(1)，得到速度变化期间实际运动距离L_c：

步骤2.2.1，如果L_c≥L_d，且V_s<V_e时，说明需要实际运行更远的距离，速度才能从Vs变化成Ve，而此时Vs又小于Ve，则在设定的距离L_d内一直做加速运动，为H段模式，由公式(2)可计算得到该段实际的末速度V_c，

进入步骤三，否则进入步骤2.2.2。

步骤2.2.1，如果L_c≥L_d，且V_s>V_e时，说明需要实际运行更远的距离，速度才能从Vs变化成Ve，而此时Vs又大于Ve，则在设定的距离L_d内一直做减速运动，为T段，由公式(2)可计算得到该段实际的末速度V_c，进入步骤三，否则进入步骤2.2.3。

步骤2.2.3，如果L_c<L_d，且V_m＝V_s，将V_s与V_e代入式(1)，得到减速状态下运动的距离L_t。并且当L_t<L_d时，为BT分段，即该段为先以V_s的速度匀速运动距离L_b后再减速运动，其中L_b＝L_d-L_t。进入步骤三，对BT分段中的T分段进行速度曲线规划，否则进入步骤2.2.4。

步骤2.2.4，如果L_c<L_d，将V_s与V_m代入式(1)，得到加速距离L_h，如果L_h<L_d，且V_m＝V_e，为HB段模式，即该段为先加速到V_m后，再以V_m的速度运动。进入步骤三，对HB分段中的H分段进行速度曲线规划，否则进入步骤2.2.5。

步骤2.2.5，如果L_c<L_d，将V_s与V_m代入式(1)，得到加速状态下运动的距离L_h，将V_m与V_e代入式(1)，得到减速状态下运动的距离L_t；进入步骤2.2.5.1

步骤2.2.5.1，如果L_d<(L_h+L_t)，为HT分段，即该段曲线为先加速，然后立即进入减速运动模式，进入步骤三，否则进入步骤2.2.5.2；

步骤2.2.5.2，当L_d>(L_h+L_t)时，为HBT分段，曲线先加速，然后进入匀速，最后减速，中间匀速运状态下运动的距离为L_b＝L_d-(L_h+L_t)，进入步骤三。

所述步骤3中5阶贝塞尔曲线计算，采用前向差分的方法进行计算，算法计算推导过程：

根据贝塞尔曲线通用表达式：导出由5阶贝塞尔曲线生成的速度曲线表达式：

V(t)＝P₀B₀(t)+P₁B₁(t)+P₂B₂(t)+P₃B₃(t)+P₄B₄(t)+P₅B₅(t)(3)

其中0≤t≤1，V(t)为速度，P₀～P₅为贝塞尔控制点；

其中B₀(t)～B₅(t)为伯恩斯坦多项式

B₀(t)＝(1-t)⁵＝-t⁵+5t⁴-10t³+10t²-5t+1

B₁(t)＝5(1-t)⁴×t＝5t⁵-20t⁴+30t³-20t²+5t

B₂(t)＝10(1-t)³×t²＝-10t⁵+30t⁴-30t³+10t²

B₃(t)＝10(1-t)²×t³＝10t⁵-20t⁴+10t³

B₄(t)＝5(1-t)×t⁴＝-5t⁵+5t⁴

B₅(t)＝t⁵

根据式(3)速度曲线可以表达为：

V(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F

其中系数A～F可分别表达如下：

A＝-P₀+5P₁-10P₂+10P₃-5P₄+P₅

B＝5P₀-20P₁+30P₂-20P₃+5P₄

C＝-10P₀+30P₁-30P₂+10P₃

D＝10P₀-20P₁+10P₂

E＝-5P₀+5P₁

F＝P₀

H分段和T分段的运动曲线两端点处加速度和加加速都为零，H分段或T分段的速度曲线的初始速度记做Vi，末速度记作Vf，设置差分计算初始阶段P₀＝P₁＝P₂＝V_i，P0、P1和P2均等于初始速度V_i，P₃＝P₄＝P₅＝V_f，P3、P4和P5均等于末速度Vf。

带入化简A～F系数表达式

A＝-6V_i+6V_f

B＝15V_i-15V_f

C＝-10V_i+10V_f

D＝0

E＝0

F＝V_i

所以速度曲线可以表达为：

V(t)＝(-6V_i+6V_f)t⁵+(15V_i-15V_f)Bt⁴+(-10V_i+10V_f)t³+V_i

前向差分算法条件，假定从速度从V_i～V_f均分为N步，步长差分表达式F₁～F₅的初始值。

在已有初始条件下，使用如示公式迭代计算N次

V＝V+F₅

F₅＝F₅+F₄

F₄＝F₄+F₃

F₃＝F₃+F₂

F₂＝F₂+F₁

即可将整个速度曲线的离散V值算出。

初始值计算推导：为方便推到中系数迭代变换过程的表述，设定a＝A，b＝B，c＝C，d＝D，e＝E，f＝F；

F₅(t)＝at⁵+bt⁴+ct³+dt²+et+fF₅(t+h)-F₅(t)

＝(5ah)t⁴+(10ah²+4bh)t³+(10ah³+6bh²+3ch)t²

+(5ah⁴+4bh³+3ch²+2dh)t+ah⁵+bh⁴+ch³+dh²+eh

由上一步的推导设定

a＝5Ah,b＝10Ah²+4Bh，c＝10Ah³+6Bh²+3Ch；

d＝5Ah⁴+4Bh³+3Ch²+2Dh

F₄(t)＝at⁴+bt³+ct²+dt+e

F₄(t+h)-F₄(t)

＝(20ah²)t³+(60ah³+12bh²)t³+(70ah⁴+24bh³+6ch²)t+30ah⁵

+14bh⁴+6ch³+2dh²

由上一步的推导设定

a＝20Ah²,b＝60Ah³+12Bh²,c＝70Ah⁴+24Bh³+6Ch²

F₃(t)＝at³+bt²+ct+d

F₃(t+h)-F₃(t)＝(60ah³)t²+(180ah⁴+24bh³)t+150ah⁵+36bh⁴+6ch³

由上一步的推导设定

a＝60Ah³,b＝180Ah⁴+24Bh³

F₂(t)＝at²+bt+c

F₂(t+h)-F₂(t)＝(120ah⁴)t+240ah⁵+24bh⁴

由上一步的推导设定

a＝120Ah⁴

F₁(t)＝at+b

F₁(t+h)-F₁(t)＝120ah⁵

由于我们是将速度曲线以h为间隔均分，每一个速度点代表这一段内的速度，那么初始值计算中的初始时间点同时迭代次数变为N-1，带入初始时间点t₀，计算可得差分计算的初始值：

F₃(t₀)＝255Ah⁵+48Bh⁴+6Ch³

F₂(t₀)＝300Ah⁵+24Bh⁴

F₁(t₀)＝120Ah⁵

所述步骤4中等间隔采样，采样间隔为步骤三中定义的h，分段数为N-1。

脉冲型控制器，在每一个速度更新点上使用频率输出时长h的脉冲序列，直到迭代完成，即可完成控制输出；

总线型控制器，在每一个速度更新点上使用V(t+h)＝V(t)+F5，输出V(t+h)，直到迭代完成，即可完成控制输出，V(t+h)即为运动控制需要输出的控制位置。

对待规划的S型曲线上可能会出现的运动状态组合，分别进行速度规划的说明如下。

已知待规划的曲线的轨迹长度L_d、启动速度V_s、结束速度V_e、最大加加速J_m、最大允许速度V_m，经过步骤2中的判断后；

如果判断后该段为B分段曲线，B分段由于是匀速运动段故无须做速度曲线变换，B分段速度曲线为恒定值，B分段的速度值V＝V_s＝V_e；完成运行曲线规划；

如果判断后该段为H分段曲线，该段曲线的速度从V_s加速到实际的末速度V_c，根据式(2)可知实际末速度V_c的值其中通过本发明的步骤3可以规划出速度从V_s到实际末速度V_c的速度变化曲线，通过本发明的步骤4可以计算出所述曲线上V_s到V_c之间离散的速度值，完成运行曲线规划；

如果判断后该段为T分段曲线，该段曲线的速度从V_s减速到实际的末速度V_c，其中通过本发明的步骤3可以规划出速度从V_s到实际末速度V_c的速度变化曲线，通过本发明的步骤4可以计算出所述曲线上V_s到V_c之间离散的速度值，完成运行曲线规划；

如果判断后该段为BT分段曲线，该段曲线的速度开始时保持在V_s，再从V_s减速到V_e，其中对T分段部分采用本发明中步骤3和4的方法进行速度的计算，完成运行曲线规划；

如果判断后该段为HB分段曲线，该段曲线的速度从V_s加速到最大允许速度V_m，再以速度V_m一直匀速运动。其中通过本发明的步骤3规划出从速度从V_s到最大允许速度V_m之间的速度变化曲线，通过本发明的步骤4可以计算出所述速度变化曲线上V_s到V_m之间离散的速度值，完成运行曲线规划；

如果判断后该段为HT分段曲线，该段曲线的速度从V_s加速到V_m，再从V_m减速到V_e，其中对H和T分段采用本发明中步骤3和步骤4的方法进行速度的计算，完成运行曲线规划；

如果判断后该段为HBT分段曲线，该段曲线的速度从V_s加速到V_m，然后进入B分段以V_m匀速运行L_b长度后进入T分段从V_m减速到V_e，其中对H和T分段采用本发明中步骤3和4的方法进行速度的计算，完成运行曲线规划。

Claims (4)

1.一种快速规划6阶S型运动曲线的方法，其特征在于，包括如下步骤：

步骤1，输入待规划的运动曲线参数：轨迹长度L_d、启动速度V_s、结束速度V_e、最大加加速J_m、最大允许速度V_m；

步骤2，使用J_m与V_m作为约束条件，计算V_s、V_e、L_d之间的关系，将待规划的运动曲线分为加速阶段、匀速阶段和减速阶段，其中加速阶段记作H分段，匀速阶段记作B分段，减速阶段记作T分段，待规划的S型曲线上会出现的运动状态分段情况为H、T、B、BT、HB、HT、HBT几种分段中的一种，对待规划的S型曲线上的运动状态组合进行判断；

步骤3，对存在的H分段和T分段的运动曲线，使用5阶贝塞尔曲线规划速度曲线；具体方法为：

根据贝塞尔曲线通用表达式：导出由5阶贝塞尔曲线生成的速度曲线表达式：

V(t)＝P₀B₀(t)+P₁B₁(t)+P₂B₂(t)+P₃B₃(t)+P₄B₄(t)+P₅B₅(t)(3)

其中0≤t≤1，V(t)为速度，P₀～P₅为贝塞尔控制点，B₀(t)～B₅(t)为伯恩斯坦多项式；所以，式(3)也可写做：

V(t)＝At⁵+Bt⁴+Ct³+Dt²+Et+F(4)

其中系数A～F可分别表达如下：

A＝-P₀+5P₁-10P₂+10P₃-5P₄+P₅

B＝5P₀-20P₁+30P₂-20P₃+5P₄

C＝-10P₀+30P₁-30P₂+10P₃

D＝10P₀-20P₁+10P₂

E＝-5P₀+5P₁

F＝P₀

H分段或T分段的速度曲线两端点处加速度和加加速都为零，H分段或T分段的速度曲线的初始速度记做V_i，末速度记作V_f，设置差分计算初始阶段P₀＝P₁＝P₂＝V_i，P₀、P₁和P₂均等于初始速度V_i，P₃＝P₄＝P₅＝V_f，P₃、P₄和P₅均等于末速度V_f；

带入化简A～F系数表达式

A＝-6V_i+6V_f

B＝15V_i-15V_f

C＝-10V_i+10V_f

D＝0

E＝0

F＝V_i

将A～F系数代入式(4)，H分段或T分段速度曲线表达式为：

V(t)＝(-6V_i+6V_f)t⁵+(15V_i-15V_f)Bt⁴+(-10V_i+10V_f)t³+V_i(5)；

步骤4，对步骤三中规划出的速度曲线进行等间隔采样，将速度曲线等分为N份，每份时间间隔为h，采用向前差分的方式计算出速度曲线上的离散V值。

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

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

通过公式(1)和(2)计算，可对待规划的S型曲线上的运动状态分段组合进行判断，具体方法如下：

步骤2.1如果V_s＝V_e，则该段为匀速运动状态过程，为B分段，结束步骤二，否则进入步骤2.2；

步骤2.2将V_s与V_e代入式(1)，得到速度变化期间实际运动距离L_c，进入步骤2.2.1；

步骤2.2.1如果L_c≥L_d，且V_s<V_e时，说明需要实际运行更远的距离，速度才能从V_s变化成V_e，而此时V_s又小于V_e，则在设定的距离L_d内一直做加速运动，为H分段模式，由公式(2)可计算得到该段实际的末速度V_c，进入步骤三，否则进入步骤2.2.2；

步骤2.2.2如果L_c≥L_d，且V_s>V_e时，说明需要实际运行更远的距离，速度才能从V_s变化成V_e，而此时V_s又大于V_e，则在设定的距离L_d内一直做减速运动，为T分段，由公式(2)可计算得到该段实际的末速度V_c，进入步骤三，否则进入步骤2.2.3；

步骤2.2.3如果L_c<L_d，且V_m＝V_s，将V_s与V_e代入式(1)，得到减速状态下运动的距离L_t，并且当L_t<L_d时，为BT分段，即该段为先匀速后减速运动模式，进入步骤三，否则进入步骤2.2.4；

步骤2.2.4如果L_c<L_d，将V_s与V_m代入式(1)，得到加速距离L_h，如果L_h<L_d，且V_m＝V_e，为HB段模式，即该段为先加速后匀速运动模式，进入步骤三，否则进入步骤2.2.5；

步骤2.2.5如果L_c<L_d，将V_s与V_m代入式(1)，得到加速状态下运动的距离L_h，将V_m与V_e代入式(1)，得到减速状态下运动的距离L_t，进入步骤2.2.5.1；

步骤2.2.5.1如果L_d<(L_h+L_t)，为HT分段，即该段曲线为先加速，然后立即进入减速运动模式，进入步骤三，否则进入步骤2.2.5.2；

步骤2.2.5.2如果L_d>(L_h+L_t)，为HBT分段，曲线先加速，然后进入匀速，最后减速，进入步骤三。

3.根据权利要求1所述的一种快速规划6阶S型运动曲线的方法，其特征在于，步骤四中，采用前向差分的方式计算速度曲线上的离散V值的具体方法如下：

将速度曲线的计算设计为前向差分方式：

V＝V+F₅

F₅＝F₅+F₄

F₄＝F₄+F₃

F₃＝F₃+F₂

F₂＝F₂+F₁

其中F₁～F₅为各差分计算式；

前向差分算法的初始值计算方法：将速度曲线按照时间等分为N份，N为整数，每份间隔为h，然后使用如下公式分别计算差分式的初始值：

F₃(t₀)＝255Ah⁵+48Bh⁴+6Ch³

F₂(t₀)＝300Ah⁵+24Bh⁴

F₁(t₀)＝120Ah⁵

将计算的初始值带入向差分方式进行N-1次迭代计算即可得到完整的6阶S型运动曲线，每一次迭代计算都可以计算出一个实时的速度V。

4.根据权利要求2所述的一种快速规划6阶S型运动曲线的方法，其特征在于，

式(2)中求解末速度V_f过程中用到了开3次方的计算，使用牛顿-拉弗森方法来进一步逼近计算结果，

对式(2)构造0根等式，

为方便计算上式转换为，Z(V_f)＝((V_f-V_i)³)/L_x ²-J_m；

对Z(V_f)求在V_f处的导数：

使用进行迭代计算即可逼近真实值，其中n为整数。