CN109991932B - 一种物流搬运设备s曲线加减速速度规划与控制方法 - Google Patents

Abstract

本发明涉及一种物流搬运设备S曲线加减速速度规划与控制方法，获取目标位置，根据目标位置和约束条件，确定S曲线形式；如果S曲线为四段S曲线，则通过二分迭代法规划速度段；否则根据约束条件规划速度段；计算S曲线各分段点的位移和速度；根据位置反馈和分段位移比较，确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制。本发明仅通过PLC编程即可实现低成本的S曲线速度控制，不增加额外硬件投入，计算实时性强，控制效果稳定，对于难于实现实时位置反馈的应用场合，本发明避免了传统多段速开环控制速度切换时冲击大，低速定位段时间长的问题，通过闭环控制，保证了速度控制精度。

Description

一种物流搬运设备S曲线加减速速度规划与控制方法

技术领域

本发明涉及物流搬运设备运动控制技术领域，具体地说是一种物流搬运设备S曲线加减速速度规划与控制方法。

背景技术

在物流设备中，沿直线轨迹运行的机构非常普遍。出于机械安装限制或降低成本的考虑，很多采用了开环控制方式，即仅控制速度，通过认址片(孔)、编码器(尺)等认址装置完成位置定位。开环控制方式难以对全行程进行速度曲线规划，一般采用多段速调速，接近目标位置时采用低速定位。在这种方式下，速度控制不稳定，调速对设备和货物冲击大，定位过程时间长。

发明内容

针对现有技术的不足，本发明提供一种物流搬运设备S曲线加减速速度规划与控制方法，实现了无极调速，并实现了位置反馈对速度的闭环校验与控制，避免了对设备和货物的冲击，显著提高了定位效率，克服了传统的多轨迹段S曲线速度控制算法计算复杂，难于在PLC上实现的问题。

本发明为实现上述目的所采用的技术方案是：

一种物流搬运设备S曲线加减速速度规划与控制方法，包括以下步骤：

步骤1：获取目标位置，根据目标位置和约束条件规划速度曲线段，确定S曲线形式；如果S曲线为四段S曲线，则通过二分迭代法重新规划速度曲线段，执行步骤2；否则根据确定的S曲线形式执行步骤2；

步骤2：计算S曲线各速度曲线段分段点的位移和速度；

步骤3：根据位置反馈和分段位移比较，确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制。

所述约束条件包括加加速度J_max、加速度a_max、最大速度V_max以及运动路径长度S。

所述S曲线包括：四段S曲线、五段S曲线、六段S曲线和七段S曲线；其中

四段S曲线包括四个速度曲线段和速度曲线段后的低速定位段；速度曲线段依次为加加速段、减加速段、加减速段、减减速段，且加加速段与减减速段的位移相等，减加速段和加减速段的位移相等；

五段S曲线包括五个速度曲线段和速度曲线段后的低速定位段，速度曲线段依次为加加速段、减加速段、匀速段、加减速段、减减速段和低速定位段，且加加速段与减减速段的位移相等，减加速段和加减速段的位移相等；

六段S曲线包括六个速度曲线段和速度曲线段后的低速定位段，速度曲线段依次为加加速段、匀加速段、减加速段、加减速段、匀减速段、减减速段和低速定位段，且加加速段与减减速段的位移相等，匀加速段和匀减速段的位移相等，减加速段和加减速段的位移相等；

七段S曲线包括七个速度曲线段和速度曲线段后的低速定位段，速度曲线段依次为加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段和低速定位段，且加加速段与减减速段的位移相等，匀加速段和匀减速段的位移相等，减加速段和加减速段的位移相等。

所述确定S曲线形式包括以下过程：

(1)当S>2(S_Low+S_Mid+S_High)+S_Final时，为七段S曲线；

(2)当S＝2(S_Low+S_Mid+S_High)+S_Final时，为六段S曲线；

(3)当S<2(S_Low+S_Mid+S_High)+S_Final，且S≥2(S_Low+S_High)+S_Final时，为五段S曲线；

(4)当S<2(S_Low+S_High)+S_Final，且S>S_Final时，为四段S曲线；

(5)当S≤S_Final时，此时仅有低速定位段，不含S曲线段；

其中，S为运动路径长度，即当前位置与目标位置的距离；S_Low为加加速段或减减速段的位移，

S_Mid表示匀加速段或匀减速段的位移，

S_High表示减加速段或加减速段位移，

t表示插补点瞬时时间，T_Ramp为加减速时间，S_Final表示低速定位段的位移；J_max为加加速度；a_max为加速度，V_max为最大速度,T_Ramp为加减速时间：T_Ramp＝a_max/J_max,v_s为定位段速度，v₁为加加速段终速度：

v₃为匀速段速度：v₃＝V_max，v₂为匀加速段终速度：

所述通过二分迭代法规划速度段包括以下过程：

当S曲线为四段S曲线时，四段S曲线的加减速时间T_Ramp相等，其取值范围为(0,a_max/J_max)，加减速时间T_Ramp迭代的初值为(0+a_max/J_max)/2，代入S'＝2(S_Low+S_High)+S_Final中，每迭代一步按照如下公式计算：

当迭代误差绝对值|S'-S|满足给定的允许误差时，迭代完成，求出T_Ramp。

所述计算S曲线各速度曲线段分段点的位移和速度包括以下过程：

当S曲线为六段S曲线或七段S曲线时：

当S曲线为四段S曲线或五段S曲线时：

其中，S_Low为加加速段或减减速段的位移，S_Mid表示匀加速段或匀减速段的位移；S_Final表示减加速段或加减速段位移，S_V表示匀速段的位移，S_Final表示低速定位段的位移，J_max为加加速度，a_max为加速度，V_max为最大速度,T_Ramp为加减速时间，v_s为定位段速度，v₁为加加速段终速度，v₂为匀加速段终速度，v₃为匀速段速度。

所述确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制包括以下过程：

采用中点插补算法对曲线段进行插补，即在速度曲线上的两个点(T_N,V_N)和(T_N+2,V_N+2)中增加中点插补点(T_N+1,V_N+2)，设运动过程的总位移S_Sum＝2(S_Low+S_Mid+S_High)+S_V+S_Final；依次执行(1)、(2)、(3)、(4)、(5)、(6)、(7)、(8)、(9)；

(1)当S>2(S_Mid+S_High)+S_Low+S_V+S_Final时，为加加速段，其中：

设定位移

输出速度

(2)当不符合(1)条件，且S>2S_High+S_Mid+S_Low+S_V+S_Final时，为匀加速段，其中：

设定位移

输出速度V_t＝v₁+a_maxt+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(3)当不符合(1)和(2)条件，且S>S_High+S_Mid+S_Low+S_V+S_Final时，为减加速段，其中：

设定位移

输出速度

(4)当不符合(1)-(3)条件，且S>S_High+S_Mid+S_Low+S_Final时，为匀速段，其中：

设定位移S_t＝S_Low+S_Mid+S_High+v₃t；

输出速度V_t＝v₃+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(5)当不符合(1)-(4)条件，且S>S_Mid+S_Low+S_Final时，为加减速段，其中：

设定位移

输出速度

(6)当不符合(1)-(5)条件，且S>S_Low+S_Final时，为匀减速段，其中：

设定位移

输出速度V_t＝v₅-a_maxt+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(7)当不符合(1)-(6)条件，且S>S_Final时，为减减速段，其中：

设定位移

输出速度

(8)当不符合(1)-(7)条件，且S≤S_Final时，为低速定位段，其中：

输出速度V_t＝v_s；

(9)当实际位置与目标位置偏差绝对值满足给定的允许误差时，输出速度

V_t＝0，运动过程结束；否则返回(1)。

本发明具有以下有益效果及优点：

1.本发明仅通过PLC编程即可实现低成本的S曲线速度控制，不增加额外硬件投入，计算实时性强，控制效果稳定。

2.本发明提供了一种基于PLC、变频器、位置认址装置的低成本直线定位控制系统，其中PLC是控制的核心，用于速度曲线规划、位置数据的采集、无极闭环调速控制；变频器用于执行电机的调速；位置认址装置用于反馈位置数据。

3.本发明实现了根据加加速度、加速度、速度的设定值以及运动路径长度，自动预规划多轨迹段速度曲线，该方法计算量小，实时性强，克服了传统的多轨迹段S曲线速度控制算法计算复杂，难于在PLC上实现的问题。

4.本发明还提供了一种速度控制方法，该方法通过定时中断对规划的速度曲线进行实时插补，作为速度设定值；通过位置反馈环节修正速度设定值并判断跟随误差是否过大，通过反馈控制系统保证了速度曲线的跟踪速度和精度，实现了速度曲线的准确拟合。

5.本发明避免了传统多段速开环控制速度切换时冲击大，低速定位段时间长的问题，通过闭环控制，保证了速度控制精度。

附图说明

图1是系统控制结构框图；

图2是多种分段的S形速度曲线图；

图3是方法流程图。

具体实施方式

以下结合附图以及具体实施例，对本发明的技术方案进行详细描述。

图1示出了本发明的系统控制结构框图。

其硬件包括PLC、变频器、电机和位置认址装置。

PLC是控制的核心，用于速度曲线规划、位置数据的采集、无极闭环调速控制；

变频器用于执行电机的调速；

电机用于执行物流搬运设备的拖动与定位。

位置认址装置用于反馈位置数据。

位置环反馈环节采用比例(P)调节,Kp为位置环增益：即PLC实时采集位置认址装置反馈的位置数据，并与经规划速度曲线计算得出的设定位移值比较，其差值乘以增益Kp后叠加规划速度曲线的插补值作为速度输出设定值。

S曲线算法的核心思想是让加速度不产生突变，从而使被控对象的速度控制具有快速、平稳的特性。完整的S形速度曲线包括七段，如图2.a所示：加速过程包含加加速段(0～t₁)、匀加速段(t₁～t₂)及减加速段(t₂～t₃)；减速过程包含加减速段(t₄～t₅)、匀减速段(t₅～t₆)及减减速段(t₆～t₇)；加速和减速之间通过匀速段(t₃～t₄)衔接。

在实际应用中，由于被控对象约束条件的限制，S形曲线往往不能完整地包含七段，如图2.d所示，缺少匀速段，呈现六段速度曲线；如图2.c所示，缺少匀加速段和匀减速段，呈现五段速度曲线；如图2.b所示，缺少匀加速段、匀减速段和匀速段，呈现四段速度曲线。

为保证计算简便，应使加加速段和减加速段对称。且在加速过程中，应尽量满足使加速度a可达到a_max。因此，程序中强制使最大速度V_max不小于

和初速度V_S之和。其中J_max为加加速度。

为保证定位不超调，在S曲线分段外，增加了终定位的低速定位段，定位段距离可根据设备运动特性灵活配置。

当运动命令下达后，PLC首先根据约束条件：加加速度J_max、加速度a_max、最大速度V_max以及运动路径长度S(即当前位置与目标位置的距离值)确定S曲线形式,即确定曲线段是由七段、六段、五段还是四段构成及每段的含义。

因为加速段与减速段对称。加加速段与减减速段位移相等，以S_Low表示；匀加速段与匀减速段位移相等，以S_Mid表示；减加速段与加减速段位移相等，以S_High表示；匀速段以S_V表示；低速定位段以S_Final表示,通过将约束条件加加速度J_max、加速度a_max、最大速度V_max代入下述公式计算S_Low、S_Mid和S_High：

式中，T_Ramp为加减速时间，v_s为定位段速度，v₁为加加速段终速度，v₂为匀加速段终速度，v₃为匀速段速度。

根据计算结果，对S曲线的形式推论如下：

当S>2(S_Low+S_Mid+S_High)+S_Final时，此时为七段S曲线，如图2.a所示；

⑵当S＝2(S_Low+S_Mid+S_High)+S_Final时，此时为六段S曲线，不含匀速段，如图2.d所示；

⑶当S<2(S_Low+S_Mid+S_High)+S_Final，且S≥2(S_Low+S_High)+S_Final时，此时为五段S曲线，不含匀加速段与匀减速段,如图2.c所示；

⑷当S<2(S_Low+S_High)+S_Final，且S>S_Final时，此时为四段S曲线，不含匀加速段、匀减速段和匀速段,如图2.b所示，需要重新求解加减速时间T_Ramp。由于在某个特定的加减速时间T_Ramp下，四段加减速过程的总位移S是T_Ramp的单调增函数，因此迭代有限次后一定可求出满足误差范围的T_Ramp。

本发明通过二分迭代法求解，方法如下；

四段S曲线的加减速时间T_Ramp相等，其取值范围为(0,a_max/J_max)。加减速时间T_Ramp迭代的初值为(0+a_max/J_max)/2，代入S'＝2(S_Low+S_High)+S_Final中，每迭代一步按照如下公式计算：

当迭代误差绝对值|S'-S|满足给定的允许误差时，迭代完成，求出T_Ramp。

为防止迭代总时间超过PLC看门狗定时时间，从而触发系统故障，在PLC每次调用程序时仅执行一次迭代计算。

⑸当S≤S_Final时，此时仅有低速定位段，不含S曲线段。

如图2.a所示，加速段与减速段对称，因此分段点速度对应相等：即v₁＝v₆，v₂＝v₅，v₃＝v₄。因为六段、五段、四段S速度曲线均是七段速度曲线的特殊形式，即缺少的分段其首末分段点重合，因此本文中使用的分段点速度v₁～v₇均按照图2.a的七段S曲线定义。

在⑴和⑵的情况下，各分段位移和分段点速度在确定S曲线形式时，已经得

出。

在⑶和⑷的情况下，各分段位移和分段点速度需要重新计算：

其中，S_Low为加加速段或减减速段的位移，S_Mid表示匀加速段或匀减速段的位移；S_Final表示减加速段或加减速段位移，S_V表示匀速段的位移，S_Final表示低速定位段的位移，J_max为加加速度，a_max为加速度，V_max为最大速度,T_Ramp为加减速时间，v_s为定位段速度，v₁为加加速段终速度，v₂为匀加速段终速度，v₃为匀速段速度。

在得出S曲线各分段位移和分段点速度后，可对各分段曲线进行插补，控制输出速度。

为保证速度插补的实时性，将插补计算写入定时中断程序中。

本发明采用中点插补算法对曲线段进行插补，即在速度曲线上的两个点(T_N,V_N)和(T_N+2,V_N+2)中增加中点插补点(T_N+1,V_N+2)，该算法易于计算，位置偏差较小。

设运动过程的总位移S_Sum＝2(S_Low+S_Mid+S_High)+S_V+S_Final

⑴当S>2(S_Mid+S_High)+S_Low+S_V+S_Final时，为加加速段，其中：

设定位移

输出速度

⑵当不符合⑴条件,且S>2S_High+S_Mid+S_Low+S_V+S_Final时，为匀加速段，其中：

设定位移

输出速度V_t＝v₁+a_maxt+(S_t-S_Sum+S)K_P

⑶当不符合⑴-⑵条件,且S>S_High+S_Mid+S_Low+S_V+S_Final时，为减加速段，其中：

设定位移

输出速度

⑷当不符合⑴-⑶条件,且S>S_High+S_Mid+S_Low+S_Final时，为匀速段，其中：

设定位移S_t＝S_Low+S_Mid+S_High+v₃t

输出速度V_t＝v₃+(S_t-S_Sum+S)K_P

t∈[0,T_Ramp]

⑸当不符合⑴-⑷条件,且S>S_Mid+S_Low+S_Final时，为加减速段，其中：

设定位移

输出速度

⑹当不符合⑴-⑸条件,且S>S_Low+S_Final时，为匀减速段，其中：

设定位移

输出速度V_t＝v₅-a_maxt+(S_t-S_Sum+S)K_P

t∈[0,T_Ramp]

⑺当不符合⑴-⑹条件,且S>S_Final时，为减减速段，其中：

设定位移

输出速度

⑻当不符合⑴-⑺条件,且S≤S_Final时，为低速定位段，其中：

输出速度V_t＝v_s

⑼当实际位置与目标位置偏差绝对值满足给定的允许误差时，输出速度V_t＝0，运动过程结束。

以上，对本发明速度曲线规划和插补算法，及其在PLC硬件平台上的具体实现进行了详细介绍。本领域技术人员在本发明上做出的变型或延伸也属于本发明保护范围之内。

Claims (6)

1.一种物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：包括以下步骤：

步骤1：获取目标位置，根据目标位置和约束条件规划速度曲线段，确定S曲线形式；如果S曲线为四段S曲线，则通过二分迭代法重新规划速度曲线段，执行步骤2；否则根据确定的S曲线形式执行步骤2；

步骤2：计算S曲线各速度曲线段分段点的位移和速度；

步骤3：根据位置反馈和分段位移比较，确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制；

所述确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制包括以下过程：

采用中点插补算法对曲线段进行插补，即在速度曲线上的两个点(T_N,V_N)和(T_N+2,V_N+2)中增加中点插补点(T_N+1,V_N+2)，设运动过程的总位移S_Sum＝2(S_Low+S_Mid+S_High)+S_V+S_Final；依次执行(1)、(2)、(3)、(4)、(5)、(6)、(7)、(8)、(9)；

(1)当S＞2(S_Mid+S_High)+S_Low+S_V+S_Final时，为加加速段，其中：

设定位移

输出速度

t∈[0,T_Ramp]；

(2)当不符合(1)条件，且S＞2S_High+S_Mid+S_Low+S_V+S_Final时，为匀加速段，其中：

设定位移

输出速度V_t＝v₁+a_maxt+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

Kp为位置环增益；

(3)当不符合(1)和(2)条件，且S＞S_High+S_Mid+S_Low+S_V+S_Final时，为减加速段，其中：

设定位移

输出速度

t∈[T_Ramp,0]；

(4)当不符合(1)-(3)条件，且S＞S_High+S_Mid+S_Low+S_Final时，为匀速段，其中：

设定位移S_t＝S_Low+S_Mid+S_High+v₃t；

输出速度V_t＝v₃+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(5)当不符合(1)-(4)条件，且S＞S_Mid+S_Low+S_Final时，为加减速段，其中：

设定位移

输出速度

t∈[0,T_Ramp]；

(6)当不符合(1)-(5)条件，且S＞S_Low+S_Final时，为匀减速段，其中：

设定位移

输出速度V_t＝v₅-a_maxt+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(7)当不符合(1)-(6)条件，且S＞S_Final时，为减减速段，其中：

设定位移

输出速度

t∈[T_Ramp,0]；

(8)当不符合(1)-(7)条件，且S≤S_Final时，为低速定位段，其中：

输出速度V_t＝v_s；

(9)当实际位置与目标位置偏差绝对值满足给定的允许误差时，输出速度V_t＝0，运动过程结束；否则返回(1)。

2.根据权利要求1所述的物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：所述约束条件包括加加速度J_max、加速度a_max、最大速度V_max以及运动路径长度S。

3.根据权利要求1所述的物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：所述S曲线包括：四段S曲线、五段S曲线、六段S曲线和七段S曲线；其中

四段S曲线包括四个速度曲线段和速度曲线段后的低速定位段；速度曲线段依次为加加速段、减加速段、加减速段、减减速段，且加加速段与减减速段的位移相等，减加速段和加减速段的位移相等；

五段S曲线包括五个速度曲线段和速度曲线段后的低速定位段，速度曲线段依次为加加速段、减加速段、匀速段、加减速段、减减速段和低速定位段，且加加速段与减减速段的位移相等，减加速段和加减速段的位移相等；

六段S曲线包括六个速度曲线段和速度曲线段后的低速定位段，速度曲线段依次为加加速段、匀加速段、减加速段、加减速段、匀减速段、减减速段和低速定位段，且加加速段与减减速段的位移相等，匀加速段和匀减速段的位移相等，减加速段和加减速段的位移相等；

七段S曲线包括七个速度曲线段和速度曲线段后的低速定位段，速度曲线段依次为加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段和低速定位段，且加加速段与减减速段的位移相等，匀加速段和匀减速段的位移相等，减加速段和加减速段的位移相等。

4.根据权利要求1所述的物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：所述确定S曲线形式包括以下过程：

(1)当S＞2(S_Low+S_Mid+S_High)+S_Final时，为七段S曲线；

(2)当S＝2(S_Low+S_Mid+S_High)+S_Final时，为六段S曲线；

(3)当S＜2(S_Low+S_Mid+S_High)+S_Final，且S≥2(S_Low+S_High)+S_Final时，为五段S曲线；

(4)当S＜2(S_Low+S_High)+S_Final，且S＞S_Final时，为四段S曲线；

(5)当S≤S_Final时，此时仅有低速定位段，不含S曲线段；

其中，S为运动路径长度，即当前位置与目标位置的距离；S_Low为加加速段或减减速段的位移，

S_Mid表示匀加速段或匀减速段的位移，

S_High表示减加速段或加减速段位移，

t表示插补点瞬时时间，T_Ramp为加减速时间，S_Final表示低速定位段的位移；J_max为加加速度；a_max为加速度，V_max为最大速度,T_Ramp为加减速时间：T_Ramp＝a_max/J_max,v_s为定位段速度，v₁为加加速段终速度：

v₃为匀速段速度：v₃＝V_max，v₂为匀加速段终速度：

5.根据权利要求1所述的物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：所述通过二分迭代法规划速度段包括以下过程：

当S曲线为四段S曲线时，四段S曲线的加减速时间T_Ramp相等，其取值范围为(0,a_max/J_max)，加减速时间T_Ramp迭代的初值为(0+a_max/J_max)/2，代入S'＝2(S_Low+S_High)+S_Final中，每迭代一步按照如下公式计算：

当迭代误差绝对值|S'-S|满足给定的允许误差时，迭代完成，求出T_Ramp。

6.根据权利要求1所述的物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：所述计算S曲线各速度曲线段分段点的位移和速度包括以下过程：

当S曲线为六段S曲线或七段S曲线时：

当S曲线为四段S曲线或五段S曲线时：

其中，S_Low为加加速段或减减速段的位移，S_Mid表示匀加速段或匀减速段的位移；S_Final表示减加速段或加减速段位移，S_V表示匀速段的位移，S_Final表示低速定位段的位移，J_max为加加速度，a_max为加速度，V_max为最大速度,T_Ramp为加减速时间，v_s为定位段速度，v₁为加加速段终速度，v₂为匀加速段终速度，v₃为匀速段速度。

Images