CN103713581A

CN103713581A - 运动控制系统s曲线加减速的实现方法

Info

Publication number: CN103713581A
Application number: CN201310683011.XA
Authority: CN
Inventors: 张建华; 李宏胜; 葛红宇; 王建红; 樊红梅; 邵祥兵
Original assignee: Nanjing Institute of Technology
Current assignee: Nanjing Institute of Technology
Priority date: 2013-12-12
Filing date: 2013-12-12
Publication date: 2014-04-09
Anticipated expiration: 2033-12-12
Also published as: CN103713581B

Abstract

一种运动控制系统S曲线加减速的实现方法，1）S曲线加减速的实现条件：S曲线加减速控制过程的基本限定条件公式如公式（6）：

完整的S曲线加减速过程应满足公式（7），

2）S曲线加减速的递推公式：设初始速度为V₀；当前采样时刻k，对应的加加速度J_k、加速度a_k以及运动速度V_k，则基于前一时刻的各递推公式分别为公式（8）、（9）与（10），

Description

运动控制系统S曲线加减速的实现方法

技术领域

本技术方案属于运动控制领域，具体是运动控制系统S曲线加减速的实现方法。

背景技术

加减速控制是机器人、高性能数控系统等的关键技术。相较于其它方法，S曲线加减速控制具有加速度连续，速度曲线光滑、均匀，运动平稳、无冲击等优势，能够获得较理想的运动控制效果。结合高性能数控、机器人等的实际需求，先后出现了S曲线加减速的嵌套式前瞻快速算法、实时软件等成果，但其速度规划模型、求解算法及实时性等是该领域研究亟待解决的问题。

相对而言，现有的S曲线加减速控制方法多采用高性能计算机，通过软件的方式实现。加减速控制过程中，计算机根据指令参数，通过复杂、繁琐的不等式求解运算，确定加减速边界条件与约束条件；而后，根据得到的约束条件，进行速度规划，求解高次方程或不等式，确定各运动段的各时间点、运动速度、加速度等参数。运算量大，对系统实时性，尤其是嵌入式处理器为主的运动控制系统，存在较大影响。

发明内容

结合数控系统、机器人等领域的实际应用，本发明研究分析利用S曲线加减速过程中速度、加速度、加加速度曲线的几何特征及其相互之间的数学关系，通过对其过程及运算的合理规划，适当简化，借助分布式存储与计算等手段，避免现有S曲线加减速控制的复杂、繁琐运算，探索实时性高、便于实现的S曲线加减速控制方法与电路。

本发明结合高性能数控、机器人等领域的实际控制需求，本技术方案提出了S曲线加减速的速度规划模型及硬件实现方法，以及相关高性能实时控制电路及逻辑控制芯片，以简化相关运动控制系统的硬、软件，提高运动控制系统实时性、集成度与可靠性，提供高性能的运动控制解决方案。

本技术方案的原理说明如下：

1）S曲线加减速过程的运动分析：

运动控制过程中，平稳、光滑、均匀的S型速度曲线要求运动段的速度-时间变化曲线处处连续、可导；同时要求运动段的加加速度、加速度与速度变化曲线相互之间满足严格的积分关系：

1.1）加加速曲线

S曲线加减速的加加速度曲线为依赖运动时间的分段曲线（加减速过程的加加速度曲线如图1所示），假定加加速度大小为J_M，假定当前时间t、加加速段终点时间t₀、匀加速段终点时间t₁、减加速段终点时间t₂、匀速段终点时间t₃、加减速段终点时间t₄、匀减速段终点时间t₅与减减速段终点时间t₆，根据加加速度曲线，得到加加速度J的计算公式（1）：

J = \{\begin{matrix} J_{M} & 0 \leq t \leq t_{0}, t_{5} < t \leq t_{6} \\ 0 & t_{0} < t \leq t_{1}, t_{2} < t \leq t_{3}, t_{4} < t \leq t_{5} \\ - J_{M} & t_{1} < t \leq t_{2}, t_{3} < t \leq t_{4} \end{matrix} - - - (1)

其中，加加速段0-t₀的加加速度为J_M；匀加速段t₀-t₁的加加速度为0；减加速段t₁-t₂的加加速度为-J_M；匀速段t₂-t₃的加加速度为0；加减速段t₃-t₄的加加速度为-J_M；匀减速段t₄-t₅的加加速度为0；减减速段t₅-t₆的加加速度为J_M；

1.2）加速度曲线

S曲线加减速过程的加速度为加加速度的一次积分；（变化曲线如图2所示）。

加减速过程的加速度曲线处处连续，假定采样时间Δt，同时当前时间t、加加速段终点时间t₀、匀加速段终点时间t₁、减加速段终点时间t₂、匀速段终点时间t₃、加减速段终点时间t₄、匀减速段终点时间t₅与减减速段终点时间t₆分别对应于采样时刻n、n_t0、n_t1、n_t2、n_t3、n_t4、n_t5与n_t6，得到各运动段加速度a的计算公式（2）：

a = \{\begin{matrix} {&Integral;}_{0}^{t} J (t) dt = Σ_{0}^{n} J_{M} Δt, 0 \leq t \leq t_{0}, 0 \leq n \leq n_{t_{0}} \\ a_{M}, t_{0} < t \leq t_{1}, n_{t_{0}} < n \leq n_{t_{1}} \\ a_{M} - {&Integral;}_{t_{1}}^{t} J (t) dt = a_{M} - Σ_{n_{t_{1}}}^{n} J_{M} Δt, t_{1} < t \leq t_{2}, n_{t_{1}} < n \leq n_{t_{2}} \\ 0, t_{2} < t \leq t_{3}, n_{t_{2}} < n \leq n_{t_{3}} \\ {&Integral;}_{t_{3}}^{t} J (t) dt = {- Σ}_{n_{t_{3}}}^{n} J_{M} Δt, t_{3} \leq t \leq t_{4}, n_{t_{3}} \leq n \leq n_{t_{4}} \\ - a_{M}, t_{4} < t \leq t_{5}, n_{t_{4}} < n \leq n_{t_{5}} \\ {- a}_{M} + {&Integral;}_{t_{5}}^{t} J (t) dt = {- a}_{M} + Σ_{n_{t_{5}}}^{n} J_{M} Δt, t_{5} < t \leq t_{6}, n_{t_{5}} < n \leq n_{t_{6}} \end{matrix} - - - (2)

其中，加速度最大值a_M为：

a_{M} = {&Integral;}_{0}^{t_{0}} J (t) dt = Σ_{0}^{n_{t_{0}}} J_{M} Δt - - - (3)

加加速时间0-t₀、减加速时间t₁-t₂、加减速时间t₃-t₄与减减速时间t₅-t₆相等；最大加速度与最小加速度大小相等、符号相反；加加速段与减加速段、加减速段与减减速段加速度曲线对称；

1.3）速度曲线

S曲线加减速过程的速度为加速度的积分、加加速的二次积分（变化曲线如图3所示）；

S曲线的速度处处连续、可导，速度曲线具有对称性，加加速段与匀减速段的终止速度相等，均为V_t0；匀加速段与加减速段终止速度相等，均为V_t1；利用前文所述假设条件，根据图3所示的速度曲线，得到各运动段的即时速度V的计算公式（4）：

V = \{\begin{matrix} {&Integral;}_{0}^{t} {&Integral;}_{0}^{t} J (τ) dτdt = Σ_{n_{τ} = 0}^{n} (Σ_{0}^{n_{τ}} J_{M} Δτ) Δt, 0 \leq t \leq t_{0}, 0 \leq n \leq n_{t_{0}} \\ V_{t_{0}} + {&Integral;}_{t_{0}}^{t} a_{M} dt = V_{t_{0}} + Σ_{n_{t_{0}}}^{n} a_{M} Δt, t_{0} < t \leq t_{1}, n_{t_{0}} < n \leq n_{t_{1}} \\ V_{t_{1}} + {&Integral;}_{t_{1}}^{t} (a_{M} - {&Integral;}_{t_{1}}^{t} J_{M} dτ) dt = V_{t_{1}} + Σ_{n_{τ} = n_{t_{1}}}^{n} (a_{M} - Σ_{n_{t_{1}}}^{n_{τ}} J_{M} Δτ) Δt, t_{1} < t \leq t_{2}, n_{t_{1}} < n \leq n_{t_{2}} \\ V_{t_{2}}, t_{2} < t \leq t_{3}, n_{t_{2}} < n \leq n_{t_{3}} \\ V_{t_{2}} - {&Integral;}_{t_{3}}^{t} ({&Integral;}_{t_{3}}^{t} J_{M} dτ) dt = V_{t_{2}} - Σ_{n_{τ} = n_{t_{3}}}^{n} (Σ_{n_{t_{3}}}^{n_{τ}} J_{M} Δτ) Δt, t_{3} < t \leq t_{4}, n_{t_{3}} < n \leq n_{t_{4}} \\ V_{t_{1}} + {&Integral;}_{t_{4}}^{t} a (t) dt = V_{t_{1}} - Σ_{n_{t_{4}}}^{n} a_{M} Δt, t_{4} < t \leq t_{5}, n_{t_{4}} < n \leq n_{t_{5}} \\ V_{t_{0}} - {&Integral;}_{t_{5}}^{t} (a_{M} - {&Integral;}_{t_{5}}^{t} J_{M} dτ) dt = V_{t_{0}} - Σ_{n_{τ} = n_{t_{5}}}^{n} (a_{M} - Σ_{n_{t_{5}}}^{n_{τ}} J_{M} Δτ) Δt, t_{5} < t \leq t_{6}, n_{t_{5}} < n \leq n_{t_{6}} \end{matrix} - - - (4)

此处，Δt与Δτ取同值。其中，最大加速度a_M同上，加加速段终止速度V_t0、匀加速段终止速度V_t1与减加速段终止速度V_t2根据公式（5）求得：

\{\begin{matrix} V_{t_{0}} = Σ_{n_{τ} = 0}^{n_{0}} (Σ_{0}^{n_{τ}} J_{M} Δt) Δt \\ V_{t_{1}} = V_{t_{0}} + Σ_{n_{t_{0}}}^{n_{t_{1}}} (Σ_{0}^{n_{t_{0}}} J_{M} Δt) Δt \\ V_{t_{2}} = V_{t_{1}} + {&Integral;}_{t_{1}}^{t_{2}} a (t) dt = V_{t_{1}} + Σ_{{n_{τ} = n}_{t_{1}}}^{n_{t_{2}}} (Σ_{0}^{n_{t_{0}}} J_{M} Δt - Σ_{n_{t 1}}^{n_{τ}} J_{M} Δt) Δt \end{matrix} - - - (5)

基于上述原理分析，本发明提出S曲线加减速的实现方法，具体如下：

一种运动控制系统S曲线加减速的实现方法：

S曲线加减速的完整过程分为7个运动段，它们是加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段与减减速段；

本方法中用到的参数定义为：加加速度大小为J_M、当前时间t、加加速段终点时间t₀、匀加速段终点时间t₁、减加速段终点时间t₂、匀速段终点时间t₃、加减速段终点时间t₄、匀减速段终点时间t₅与减减速段终点时间t₆；采样时间Δt；加速度最大值a_M；

t₀、t₁、t₂、t₃、t₄、t₅与t₆分别对应的采样时刻n_t0、n_t1、n_t2、n_t3、n_t4、n_t5与n_t6；

1）S曲线加减速的实现条件：

S曲线加减速控制过程的基本限定条件公式如公式（6），即S曲线加减速过程的加加速、减加速、加减速与减减速时间相等，加速度变化量与速度变化量大小相等，符号不同；

\{\begin{matrix} t_{0} = t_{2} - t_{1} = t_{4} - t_{3} = t_{6} - t_{5} \\ a_{t_{0}} = a_{t_{1}} = - a_{t_{4}} = - a_{t_{5}} \\ V_{t_{0}} = V_{t_{2}} - V_{t_{1}} = V_{t_{3}} - V_{t_{4}} = V_{t_{5}} - V_{t_{6}} \end{matrix} - - - (6)

其中，a_t0为加加速段终点时间t₀的加速度、a_t1为匀加速段终点时间t₁的加速度、a_t4为加减速段终点时间t₄的加速度、a_t5为匀减速段终点时间t₅的加速度；V_t3为匀速段的终止速度、V_t4为加减速段的终止速度、V_t5为匀减速段的终止速度、V_t6为减减速段的终止速度；

设S曲线加减速过程中，指令速度V_L，指令位移L，定义时间变量为：

Δt_1-2=t₂-t₁,Δt_3-4=t₄-t₃,Δt_5-6=t₆-t₅

则完整的S曲线加减速过程应满足公式（7），其中a_Max为最大容许加速度：

\{\begin{matrix} L / V_{L} &GreaterEqual; t_{0} + {Δt}_{1 - 2} + {Δt}_{3 - 4} + {Δt}_{5 - 6} \\ a_{M} \leq a_{Max} \end{matrix} - - - (7)

2）S曲线加减速的递推公式

采用递推方法，利用前一时刻的运动状态，获得当前时刻的运动控制参数：

设初始速度为V₀；当前采样时刻k，对应的加加速度J_k、加速度a_k以及运动速度V_k，则基于前一时刻的各递推公式分别为公式（8）、（9）与（10），此处k取0、1、2……：

J_{k} = \{\begin{matrix} J_{M}, & 0 \leq t \leq t_{0}, t_{5} < t \leq t_{6}, 0 \leq k \leq n_{t_{0}}, n_{t_{5}} < k \leq n_{t_{6}} \\ 0, & t_{0} < t \leq t_{1}, t_{2} < t \leq t_{3}, t_{4} < t \leq t_{5}, n_{t_{0}} < k \leq n_{t_{1}}, n_{t_{2}} < k \leq n_{t_{3}}, \\ - J_{M} & t_{1} < t \leq t_{2}, t_{3} < t \leq t_{4}, n_{t_{1}} < k \leq n_{t_{2}}, n_{t_{3}} < k \leq n_{t_{4}} \end{matrix} n_{t_{4}} < k \leq n_{t_{5}}- - - - (8)

\{\begin{matrix} a_{0} = 0 \\ a_{k} = a_{k - 1} + J_{k} Δk \end{matrix} - - - (9)

\{\begin{matrix} V_{0} = 0 \\ V_{k} = V_{k - 1} + a_{k} Δk \end{matrix} - - - (10)

最后，由运动控制系统根据运动控制参数控制被控对象的S曲线加减速动作。

基于该方法的运动控制系统S曲线加减速的运动规划如下：

1）S曲线加减速的运动规划：

1.1）S曲线加减速过程利用速度及其变化量作为各运动段转换的控制条件：

运动规划时，要求加加速段、减加速段、加减速段、减减速段速度变化量相等，且都小于整个加速过程或减速过程速度变化总量的50%，从而使加减速过程包含S曲线加减速的所有运动段，实现平滑的运动效果；

1.2）按速度变化将S曲线加减速分为均速运动、加速运动与减速运动三种类型，分别加以实现；

均速运动指令的构成包括速度与位移；均速运动的加减速过程包含S曲线加减速的所有运动段；运动经加加速、匀加速与减加速达到指令速度然后开始匀速运动；而后经加减速、匀减速与减减速恢复初始速度，同时达到指令位移；

加速运动指令的构成包括起始速度、终止速度与位移；加速运动的加减速过程为左半S曲线，运动由起始速度开始，经加加速、匀加速与减加速增至终止速度，然后保持匀速运动，最终达到指令位移；

减速运动指令的构成包括起始速度、终止速度与位移，减速运动的加减速过程为右半S曲线，运动由起始速度开始，经加减速、匀减速与减减速等运动段，速度降至终止速度并保持匀速运动，最终达到指令位移；

2）异常处理：

情形a、在均速运动的加加速段、加速运动的加加速段以及减速运动的加减速段，如果其加速度达到a_Max或-a_Max，则S曲线加减速运动不再等到加加速段或加减速段的速度变化量达到加加速段或加减速段速度变化量的预定值，而是直接进入匀加速段或匀减速段；

情形b、在均速运动的加加速段、匀加速段或减加速段，如果当前位移达到指令位移量的50%，S曲线加减速运动不再等到加加速段、匀加速段或减加速段的速度变化量达到预定值，而是直接进入相应的减减速段、匀减速段或加减速段。

情形c、如果当前位移达到指令位移，则结束本次加减速控制。

所述运动规划中：

所述1.1）S曲线加减速过程利用速度及其变化量作为各运动段转换的控制条件中：

加加速段以及减加速段速度变化量都取加速过程速度变化总量的1/4、3/8、7/16或15/32；

加减速段以及减减速段速度变化量都取减速过程速度变化总量的1/4、3/8、7/16或15/32；

匀加速段的速度变化量都取加速过程速度变化总量的1/2、1/4、1/8或1/16；

匀减速段的速度变化量都取减速过程速度变化总量的1/2、1/4、1/8或1/16；

所述2）异常处理中：

情形a中，所述预定值是

加加速段速度变化量取加速过程速度变化总量的1/4、3/8、7/16或15/32；

加减速段速度变化量取减速过程速度变化总量的1/4、3/8、7/16或15/32；

情形b中，所述预定值是

减加速段速度变化量取加速过程速度变化总量的1/4、3/8、7/16或15/32；

匀加速段的速度变化量取加速过程速度变化总量的1/2、1/4、1/8或1/16；

基于该方法的S曲线加减速控制方法，其特征是：

设初始速度为V₀；当前采样时刻k，对应的加加速度J_k、加速度a_k以及运动速度V_k；

（1）速度控制：

设脉宽计数值cnt₀，脉间计数值cnt，基准时钟周期T_CK，则通用的驱动脉冲频率f的算式（11）：

f = \frac{1}{(cnt + {cnt}_{0}) T_{CK}} - - - (11)

（2）运动参数的确定：

便于S曲线加减速控制电路描述与计算，对式（8）～（10）进行的修订，定义

式（14）为新的通用加速度递推算式：

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} Δt \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (14)

其中，sigmaJ_kΔt为0-k时刻加速度变化量，即加加速度的一次积分；

定义

修订后的通用速度递推算式（16）：

\{\begin{matrix} V_{k} = V_{0} + ({ka}_{0} + sigma A_{k} Δt) Δt \\ sigma A_{k} = sigma A_{k - 1} + {sigmaJ}_{k} \\ a_{0} = 0, V_{0} = 0 \end{matrix} - - - (16)

其中，(ka₀+sigmaA_kΔt)Δt为0-k时刻速度变化总量，即加速度的一次积分。

所述S曲线加减速控制方法所控制的运动通过伺服电机实现，采用驱动脉冲结合方向控制信号的方式，脉冲频率控制运动速度，脉冲数量控制位移量大小；通过专用定时器实现驱动脉冲的脉宽与脉间，调节该定时器计数值，进而实现脉宽、脉间与脉冲频率的控制。

所述S曲线加减速控制方法中，步当量δ取1μm，基准时钟周期50ns即20MHz，驱动脉冲脉宽取定值1μs，则脉宽计数值cnt₀为20，则：

（1）对于速度控制：

由此得到速度v（单位mm/s）与计数值cnt的关系式（12）：

v = fδ = \frac{δ}{(cnt + {cnt}_{0}) T_{CK}} = \frac{1 \times 10^{- 3}}{(cnt + 20) 50 \times 10^{- 9}} = \frac{20 \times 10^{3}}{cnt + 20} - - - (12)

则速度控制量cnt的算式（13）为：

cnt = \frac{20 \times 10^{3}}{v} - 20 - - - (13)

（2）对于运动参数的确定：

设置采样时间Δt不变，保持为10μs，得到专用加速度递推算式（15）；式中加速度与加加速度单位分别为mm/s²与mm/s³；

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} / 10^{5} \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (15)

采样时间Δt为10μs，得到专用速度递推算式（17）；其中加速度与速度单位分别为mm/s²与mm/s；

\{\begin{matrix} V_{k} = V_{0} + sigma A_{k} / 10^{10} \\ sigma A_{k} = sigma A_{k - 1} + sigma J_{k} \\ V_{0} = 0 \end{matrix} - - - (17)

式中10¹⁰除运算需要较多的软硬件资源，因此计算时把速度分为整数与余数部分，sigmaA_k用作余数寄存器；每次运动采样执行一次积分运算后，若sigmaA_k≥10¹⁰，即时速度V的整数部分执行加1运算，而后将sigmaA_k-10¹⁰得到的新余数存入sigmaA_k，供下次速度计算使用；反之，sigmaA_k不变。

对于所述S曲线加减速控制方法，采用该方法的控制系统的工作过程如下：

加减速控制分为三种运动模式，分别为：均速运动模式、加速运动模式与减速运动模式。

上电以后，控制系统进入空闲状态；有加减速指令送入，控制系统获取指令参数，指令参数包括加加速度、平均速度、指令位移、起始速度和终止速度，然后进入运动初始化状态；初始化状态下，控制系统根据指令参数判别加减速模式，初算各段速度变化量与匀加速段的起始速度V_f、终止速度V_s；

运动初始化完成，系统开始S加减速控制过程，加加速度与加速度定时积分，求取即时加速度、速度并计算速度控制量cnt，由脉冲发生电路产生相应驱动脉冲，实现相应的运动段；

初始化结束后，若指令为均速运动模式或加速运动模式，控制系统转入加加速状态；若为减速运动模式，控制系统转加减速状态；

加加速状态下，情形1，即时加速度a≥a_Max，控制系统将即时速度V作为新的匀加速段起始速度V_f存储，同时重算并存储匀加速段终止速度V_s，而后转入匀加速状态；情形2，若a＜a_Max且V≥V_f，则控制系统直接进入匀加速状态；

匀加速状态，即时速度V≥V_s，控制系统进入减加速状态；而后的减加速状态下，当加速度减至0值，控制系统存储即时位移s作为匀速段起始位移L_f，进入匀速运动状态；匀速运动状态下，若为均速运动模式且位移s≥L-L_f，控制系统转加减速状态；若为加速运动模式且即时位移s≥L，控制系统转空闲态，加减速控制结束；

加减速状态，控制系统等到即时速度V降至V_s后进入匀减速状态；然而，如果加减速状态下，出现a≤-a_Max的情况，控制系统将即时速度V作为V_S，同时重算速度V_f并存储，直接转入匀减速状态；匀减速状态，即时速度V≤V_f，系统进入减减速状态；最后，减减速状态下，即时位移s≥L，控制系统转空闲态，加减速控制结束；

均速运动模式下，在加加速段、匀加速段、减加速段的任何时刻，若当前位移是指令位移的50%，则控制系统直接进入加减速段；减速运动模式下，在减速运动过程的任意时刻，若当前位移达到指令位移，则控制系统直接进入空闲状态，加减速控制结束；加速运动模式下，在加速运动过程的任意时刻，若当前位移达到指令位移，控制系统直接进入空闲状态，加减速控制结束。

一种实现运动控制系统S曲线加减速的控制电路，在使用时候，本S曲线加减速控制电路接收来自外部（例如控制计算机）的运动控制指令，根据指令参数计算在S曲线加减速运动段的各运动时刻的加速度、速度以及速度控制量；而后，根据速度控制量，动态调节脉间与脉冲频率，产生符合控制指令的伺服电机驱动脉冲；

S曲线加减速控制电路包括：CPU总线接口电路、指令参数寄存器、初始化电路、加加速度积分电路、加速度积分电路、执行寄存器、状态转换与控制电路、驱动脉冲发生电路、位移控制电路和驱动脉冲脉间计算电路；

CPU总线接口电路包括并行总线接口电路与SPI总线接口电路；

所述指令参数寄存器包括加加速度寄存器、平均速度寄存器、起始速度寄存器、终止速度寄存器与指令位移寄存器；

所述执行寄存器包括位移寄存器、速度寄存器、加速度寄存器和运动状态寄存器；

各部分功能及作用如下：

CPU总线接口电路把加加速度、平均速度、起始速度、终止速度与指令位移参数被写入指令参数寄存器中的相应寄存器，本S曲线加减速控制电路同时响应外部请求，送出本S曲线加减速控制电路的工作状态；

初始化电路读取指令参数寄存器中的指令参数，计算速度变化总量并合理规划各段速度变化量；

加加速度积分电路和加速度积分电路执行积分运算，分别求取即时加速度和速度，并写入执行寄存器中的相应寄存器；

驱动脉冲脉间计算电路读取速度寄存器，根据即时速度计算驱动脉冲的脉间计数值；脉冲发生电路利用脉间计数值对基准时钟计数，实现所需要的驱动脉冲；

位移控制电路对输出的驱动脉冲计数，计算输出位移并写入位移寄存器；

状态转换与控制电路响应基准时钟，根据运行状态、即时速度、加速度修改本S曲线加减速控制电路运行状态，控制本S曲线加减速控制电路有序协调动作。

本控制电路的设计中的参数要求如下：

（1）速度控制：

设脉宽计数值cnt₀，脉间计数值cnt，基准时钟周期T_CK，则以下为通用的驱动脉冲频率f的算式（11）：

f = \frac{1}{(cnt + {cnt}_{0}) T_{CK}} - - - (11)

步当量δ取1μm，基准时钟周期50ns即20MHz，驱动脉冲脉宽取定值1μs，则脉宽计数值cnt₀为20，则对于（1）速度控制：

由此得到速度v（单位mm/s）与计数值cnt的关系式（12）：

v = fδ = \frac{δ}{(cnt + {cnt}_{0}) T_{CK}} = \frac{1 \times 10^{- 3}}{(cnt + 20) 50 \times 10^{- 9}} = \frac{20 \times 10^{3}}{cnt + 20} - - - (12)

则速度控制量cnt的算式（13）为：

cnt = \frac{20 \times 10^{3}}{v} - 20 - - - (13)

（2）运动参数的确定：

便于S曲线加减速控制电路描述与计算，对式（8）～（10）进行的修订，设

式（14）为新的通用加速度递推算式：

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} Δt \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (14)

设置采样时间Δt不变，为10μs，得到适用于S曲线加减速控制电路的专用加速度递推算式（15）；式中加速度与加加速度单位分别为mm/s²与mm/s³；

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} / 10^{5} \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (15)

设得到修订后的通用速度递推算式（16）：

\{\begin{matrix} V_{k} = V_{0} + ({ka}_{0} + sigma A_{k} Δt) Δt \\ sigma A_{k} = sigma A_{k - 1} + {sigmaJ}_{k} \\ a_{0} = 0, V_{0} = 0 \end{matrix} - - - (16)

其中，(ka0+sigmaA_kΔt)Δt为0-k时刻速度变化总量，即加速度的一次积分。

采样时间Δt为10μs，得到适用于S曲线加减速控制电路的专用速度递推算式（17）；其中加速度与速度单位分别为mm/s²与mm/s；

\{\begin{matrix} V_{k} = V_{0} + sigma A_{k} / 10^{10} \\ sigma A_{k} = sigma A_{k - 1} + sigma J_{k} \\ V_{0} = 0 \end{matrix} - - - (17)

式中10¹⁰除运算需要较多的硬件资源，因此计算时把速度分为整数与余数部分，sigmaA_k用作余数寄存器；每次运动采样执行一次积分运算后，若sigmaA_k≥10¹⁰，即时速度V的整数部分执行加1运算，而后将sigmaA_k-10¹⁰得到的新余数存入sigmaA_k，供下次速度计算使用；反之，sigmaA_k不变。

本S曲线加减速控制电路运行状态定义与状态转换条件如下：

本加减速控制电路的设置10种运行状态，见表1：

表1加减速控制电路的运行状态

状态

说明

状态

说明

状态

说明

状态

说明

状态

说明

IDLE

空闲态

RST

运动初始化

CAC

匀加速

ZAC

匀速

UDC

匀减速

DTR

数据传送

AAC

加加速

RAC

减加速

ADC

加减速

RDC

减减速

实现本S曲线加减速控制电路时，定义加减速过程的当前位移s、速度变化总量Δ_v、匀速段起始位移L_f、匀加速段起始速度V_f、匀加速段终止速度V_s，速度变化系数k_v（0<k_v≤0.5）；同时定义运动模式m，均速运动m=0，加速运动m=1，减速运动m=2。假定加加速段、减加速段、加减速段、减减速段速度变化k_vΔ_v，匀加速段、匀减速段速度变化(1-2k_v)Δ_v；k_v取1/4、3/8、7/16或15/32，得到的状态转换条件见表2：

表2状态转换条件及含义

还包括段间转换寄存器，段间转换寄存器包括匀速段起始位移寄存器、匀加速终止速度寄存器和匀加速起始速度寄存器；

本加减速控制电路工作过程如下：

上电以后，本S曲线加减速控制电路进入空闲状态；

写信号有效，电路转数据传送状态，总线接口响应写信号WR，依次写入加加速度、平均速度、指令位移、起始速度和终止速度；

加减速使能信号有效，运动初始化状态被激活，电路读取指令参数寄存器，根据写入的参数判别加减速模式，初算各段速度变化量与匀加速段的起始速度V_f、终止速度V_s；

空闲状态下，加减速使能信号也会激活运动初始化状态，此时电路直接读取指令参数寄存器进行初始化；

运动初始化完成后，本S曲线加减速控制电路开始S曲线加减速控制过程，加加速度与加速度积分电路10μs积分一次，求取即时加速度、速度并计算速度控制量cnt，由脉冲发生电路产生相应驱动脉冲，实现相应的运动段；

初始化结束，若指令为均速运动或加速运动(m=0，1)，本S曲线加减速控制电路转入加加速状态；若指令为减速运动模式(m=2)，本S曲线加减速控制电路转加减速态；

加加速状态，若即时加速度a≥a_Max，本S曲线加减速控制电路将即时速度V作为新的匀加速段起始速度V_f存入段间转换寄存器，同时重算并存储匀加速段终止速度V_s，而后转入匀加速状态；若a＜a_Max且V≥V_f，电路直接进入匀加速状态；匀加速状态，即时速度V≥V_s，电路进入减加速态；

而后的减加速状态，当加速度减至0值，本S曲线加减速控制电路存储即时位移s作为匀速段起始位移L_f，进入匀速运动状态；

匀速状态下，若为均速运动（m=0）且位移s≥L-L_f，电路转加减速态；若为加速运动（m=1）且即时位移s≥L，本S曲线加减速控制电路转空闲态，完成加减速控制；

加减速状态，电路等到即时速度V降至V_s后进入匀减速状态。然而，如果加减速状态下，出现a≤-a_Max的情况，电路将即时速度V作为V_S，同时重算速度V_f并存储，直接转入匀减速状态；

匀减速状态，即时速度V≤V_f，电路进入减减速状态；

最后，减减速状态下，即时位移s≥L，本S曲线加减速控制电路转空闲态，完成加减速控制；

转换条件

为均速运动（m=0）模式下的异常处理，在加加速、匀加速、减加速段的任何时刻，若当前位移达到指令位移的50%，本S曲线加减速控制电路直接越过正常处理进入加减速段；

转换条件

为减速运动（m=2）模式下的异常处理，在运动的任意时刻，若当前位移达到指令位移，本S曲线加减速控制电路直接进入空闲状态，完成加减速控制；

转换条件

为加速运动（m=1）模式下的异常处理，在运动的任意时刻，若当前位移达到指令位移，本S曲线加减速控制电路直接进入空闲状态，完成加减速控制。

本发明的有益效果是，相对于软件加减速控制，S曲线加减速的硬件控制方法由专用硬件或集成电路实现，不依赖于控制系统的计算机与控制软件。因此，控制计算机在实现速度规划、控制时无需复杂、繁琐的求解运算，其输入/输出操作的实时性要求也有所降低，控制与计算任务极大简化。同时，由于采用专用集成电路与控制芯片，S曲线加减速运动的容许速度、实时性可以得到极大提高，运动控制系统对CPU的速度、运算要求极大降低，软件结构与复杂程度极大简化，有助于高速、多轴、复杂运动控制系统的研究与开发。

附图说明

图1是S曲线加减速过程的加加速度曲线；

图2是S曲线加减速过程的加速度曲线；

图3是S曲线加减速过程的速度曲线；

图4是加减速控制电路结构示意图；

图5是加减速控制电路状态转换示意图；

图6是控制电路仿真测试波形图；

图7a）～图7c）所示为电路仿真中得到的速度曲线；其中，图7a）为上述加速运动指令的速度仿真曲线，图7b）为减速运动指令的速度仿真曲线，图7c）为均速运动指令的速度仿真曲线；

图8a）～图8c）所示为图7a）～图7c）的3条运动指令执行过程的电路时序仿真结果波形图；其中，图8a）对应的是30mm/s～100mm/s加速运动指令的信号仿真波形，图8b）对应的是100mm/s～30mm/s减速运动指令的信号仿真波形，图8c）对应的是100mm/s均速运动指令的信号仿真波形。

具体实施方式

下面结合附图与具体实施方式对本技术方案进一步说明如下：

1.S曲线加减速过程的运动分析

运动控制过程中，平稳、光滑、均匀的S型速度曲线要求运动段的速度-时间变化曲线处处连续、可导；同时要求运动段的加加速度、加速度与速度变化曲线相互之间满足严格的积分关系。

1）加加速曲线

S曲线加减速的加加速度曲线为依赖运动时间的分段曲线，假定加加速度大小为J_M，加减速过程的加加速度曲线如图1所示。

假定当前时间t、加加速段终点t₀、匀加速段终点t₁、减加速段终点t₂、匀速段终点t₃、加减速段终点t₄、匀减速段终点t₅与减减速段终点t₆，根据图1所示的加加速度曲线，得到加加速度J的计算公式（1）：

J = \{\begin{matrix} J_{M} & 0 \leq t \leq t_{0}, t_{5} < t \leq t_{6} \\ 0 & t_{0} < t \leq t_{1}, t_{2} < t \leq t_{3}, t_{4} < t \leq t_{5} \\ - J_{M} & t_{1} < t \leq t_{2}, t_{3} < t \leq t_{4} \end{matrix} - - - (1)

其中，加加速段0-t₀的加加速度为J_M；匀加速段t₀-t₁的加加速度为0；减加速段t₁-t₂的加加速度为-J_M；匀速段t₂-t₃的加加速度为0；加减速段t₃-t₄的加加速度为-J_M；匀减速段t₄-t₅的加加速度为0；减减速段t₅-t₆的加加速度为J_M。

2）加速度曲线

S曲线加减速过程的加速度为加加速度的一次积分，变化曲线如图2所示。

加减速过程的加速度曲线处处连续，假定采样时间Δt，同时当前时间t、加加速段终点t₀、匀加速段终点t₁、减加速段终点t₂、匀速段终点t₃、加减速段终点t₄、匀减速段终点t₅与减减速段终点t₆分别对应于采样时刻n、n_t0、n_t1、n_t2、n_t3、n_t4、n_t5与n_t6，得到各运动段加速度a的计算公式（2）：

a = \{\begin{matrix} {&Integral;}_{0}^{t} J (t) dt = Σ_{0}^{n} J_{M} Δt, 0 \leq t \leq t_{0}, 0 \leq n \leq n_{t_{0}} \\ a_{M}, t_{0} < t \leq t_{1}, n_{t_{0}} < n \leq n_{t_{1}} \\ a_{M} - {&Integral;}_{t_{1}}^{t} J (t) dt = a_{M} - Σ_{n_{t_{1}}}^{n} J_{M} Δt, t_{1} < t \leq t_{2}, n_{t_{1}} < n \leq n_{t_{2}} \\ 0, t_{2} < t \leq t_{3}, n_{t_{2}} < n \leq n_{t_{3}} \\ {&Integral;}_{t_{3}}^{t} J (t) dt = {- Σ}_{n_{t_{3}}}^{n} J_{M} Δt, t_{3} \leq t \leq t_{4}, n_{t_{3}} \leq n \leq n_{t_{4}} \\ - a_{M}, t_{4} < t \leq t_{5}, n_{t_{4}} < n \leq n_{t_{5}} \\ {- a}_{M} + {&Integral;}_{t_{5}}^{t} J (t) dt = {- a}_{M} + Σ_{n_{t_{5}}}^{n} J_{M} Δt, t_{5} < t \leq t_{6}, n_{t_{5}} < n \leq n_{t_{6}} \end{matrix} - - - (2)

其中，加速度最大值a_M为：

a_{M} = {&Integral;}_{0}^{t_{0}} J (t) dt = Σ_{0}^{n_{t_{0}}} J_{M} Δt - - - (3)

加加速时间0-t₀、减加速时间t₁-t₂、加减速时间t₃-t₄与减减速时间t₅-t₆相等；最大加速度与最小加速度大小相等、符号相反；加加速段与减加速段、加减速段与减减速段加速度曲线对称。

3）速度曲线

S曲线加减速过程的速度为加速度的积分、加加速的二次积分，变化曲线如图3所示。

S曲线的速度处处连续、可导，速度曲线具有对称性，加加速段与匀减速段的终止速度相等，均为V_t0；匀加速段与加减速段终止速度相等，均为V_t1。利用前文所述假设条件，根据图3所示的速度曲线，得到各运动段的即时速度V的计算公式（4）：

V = \{\begin{matrix} {&Integral;}_{0}^{t} {&Integral;}_{0}^{t} J (τ) dτdt = Σ_{n_{τ} = 0}^{n} (Σ_{0}^{n_{τ}} J_{M} Δτ) Δt, 0 \leq t \leq t_{0}, 0 \leq n \leq n_{t_{0}} \\ V_{t_{0}} + {&Integral;}_{t_{0}}^{t} a_{M} dt = V_{t_{0}} + Σ_{n_{t_{0}}}^{n} a_{M} Δt, t_{0} < t \leq t_{1}, n_{t_{0}} < n \leq n_{t_{1}} \\ V_{t_{1}} + {&Integral;}_{t_{1}}^{t} (a_{M} - {&Integral;}_{t_{1}}^{t} J_{M} dτ) dt = V_{t_{1}} + Σ_{n_{τ} = n_{t_{1}}}^{n} (a_{M} - Σ_{n_{t_{1}}}^{n_{τ}} J_{M} Δτ) Δt, t_{1} < t \leq t_{2}, n_{t_{1}} < n \leq n_{t_{2}} \\ V_{t_{2}}, t_{2} < t \leq t_{3}, n_{t_{2}} < n \leq n_{t_{3}} \\ V_{t_{2}} - {&Integral;}_{t_{3}}^{t} ({&Integral;}_{t_{3}}^{t} J_{M} dτ) dt = V_{t_{2}} - Σ_{n_{τ} = n_{t_{3}}}^{n} (Σ_{n_{t_{3}}}^{n_{τ}} J_{M} Δτ) Δt, t_{3} < t \leq t_{4}, n_{t_{3}} < n \leq n_{t_{4}} \\ V_{t_{1}} + {&Integral;}_{t_{4}}^{t} a (t) dt = V_{t_{1}} - Σ_{n_{t_{4}}}^{n} a_{M} Δt, t_{4} < t \leq t_{5}, n_{t_{4}} < n \leq n_{t_{5}} \\ V_{t_{0}} - {&Integral;}_{t_{5}}^{t} (a_{M} - {&Integral;}_{t_{5}}^{t} J_{M} dτ) dt = V_{t_{0}} - Σ_{n_{τ} = n_{t_{5}}}^{n} (a_{M} - Σ_{n_{t_{5}}}^{n_{τ}} J_{M} Δτ) Δt, t_{5} < t \leq t_{6}, n_{t_{5}} < n \leq n_{t_{6}} \end{matrix} - - - (4)

\{\begin{matrix} V_{t_{0}} = Σ_{n_{τ} = 0}^{n_{0}} (Σ_{0}^{n_{τ}} J_{M} Δt) Δt \\ V_{t_{1}} = V_{t_{0}} + Σ_{n_{t_{0}}}^{n_{t_{1}}} (Σ_{0}^{n_{t_{0}}} J_{M} Δt) Δt \\ V_{t_{2}} = V_{t_{1}} + {&Integral;}_{t_{1}}^{t_{2}} a (t) dt = V_{t_{1}} + Σ_{{n_{τ} = n}_{t_{1}}}^{n_{t_{2}}} (Σ_{0}^{n_{t_{0}}} J_{M} Δt - Σ_{n_{t 1}}^{n_{τ}} J_{M} Δt) Δt \end{matrix} - - - (5)

2.S曲线加减速的实现方法

1）S曲线加减速的实现条件

参考S曲线加减速的运动过程分析，结合运动过程中速度、加速度与加加速度曲线的几何特征，可以得到S曲线加减速控制过程的基本限定条件公式（6）。即S曲线加减速过程的加加速段、减加速段、加减速段与减减速段时间相等，加速度变化量与速度变化量大小相等，符号不同。

\{\begin{matrix} t_{0} = t_{2} - t_{1} = t_{4} - t_{3} = t_{6} - t_{5} \\ a_{t_{0}} = a_{t_{1}} = - a_{t_{4}} = - a_{t_{5}} \\ V_{t_{0}} = V_{t_{2}} - V_{t_{1}} = V_{t_{3}} - V_{t_{4}} = V_{t_{5}} - V_{t_{6}} \end{matrix} - - - (6)

假定设S曲线加减速过程中，指令速度V_L，指令位移L，定义时间变量：

Δt_1-2=t₂-t₁,Δt_3-4=t₄-t₃,Δt_5-6=t₆-t₅

则完整的S曲线加减速过程应满足公式（7），其中a_Max为系统的最大容许加速度：

\{\begin{matrix} L / V_{L} &GreaterEqual; t_{0} + {Δt}_{1 - 2} + {Δt}_{3 - 4} + {Δt}_{5 - 6} \\ a_{M} \leq a_{Max} \end{matrix} - - - (7)

2）S曲线加减速的递推公式

利用上述条件，结合公式（1）～（5）可以直接规划S曲线加减速各运动段，确定运动段的各运动参数；从简化计算、方便实现的角度出发，本发明并不采用上述方法实现S曲线加减速的控制，而是在上述公式基础上，采用递推公式，利用前一时刻的运动状态，获得当前时刻的运动参数。假定当前采样时刻k，加加速度J_k、加速度a_k以及运动速度V_k，基于前一时刻的各递推公式分别见公式（8）与（9）与（10）：

J_{k} = \{\begin{matrix} J_{M}, & 0 \leq t \leq t_{0}, t_{5} < t \leq t_{6}, 0 \leq k \leq n_{t_{0}}, n_{t_{5}} < k \leq n_{t_{6}} \\ 0, & t_{0} < t \leq t_{1}, t_{2} < t \leq t_{3}, t_{4} < t \leq t_{5}, n_{t_{0}} < k \leq n_{t_{1}}, n_{t_{2}} < k \leq n_{t_{3}}, \\ - J_{M} & t_{1} < t \leq t_{2}, t_{3} < t \leq t_{4}, n_{t_{1}} < k \leq n_{t_{2}}, n_{t_{3}} < k \leq n_{t_{4}} \end{matrix} n_{t_{4}} < k \leq n_{t_{5}}- - - - (8)

\{\begin{matrix} a_{0} = 0 \\ a_{k} = a_{k - 1} + J_{k} Δk \end{matrix} - - - (9)

\{\begin{matrix} V_{0} = 0 \\ V_{k} = V_{k - 1} + a_{k} Δk \end{matrix} - - - (10)

3）S曲线加减速的运动规划

S曲线加减速过程利用速度及其变化量作为各运动段转换的控制条件。考虑系统加减速性能与运动的平稳性，运动规划时，要求加加速、减加速、加减速、减减速段速度变化量相等，且小于整个加速过程或减速过程速度变化总量的50%。为简化数据处理中的运算，加加速、减加速、加减速、减减速段速度变化量取加速或减速过程速度变化总量的1/4、3/8、7/16或15/32。由于数值的1/4运算可以通过右移2位实现，数值的3/8运算（即1/4+1/8）可以通过数值右移2位+数值右移3位实现，数值的7/16（即1/4+1/8+1/16）运算可以通过数值右移2位+数值右移3位+数值右移4位实现，数值的15/32（即1/4+1/8+1/16+1/32）运算可以通过数值右移2位+数值右移3位+数值右移4位+数值右移5位实现，速度变化量计算中的乘除运算被速度更快、复杂程度更低的“移位”运算与“加”运算替代，运算复杂程度极大降低，同时有助于提高加减速控制的运算速度与实时性；相应的，匀加速或匀减速段的速度变化量则取加/减速过程速度变化总量的1/2、1/4、1/8或1/16，速度变化量的计算也可以通过右移1位、2位、3位与4位实现，简化运算，同时使加减速过程包含S曲线加减速的所有7个运动段，实现平滑的运动效果。

结合运动控制的实际应用，按速度变化将S曲线加减速分为三种类型，分别加以实现：均速运动、加速运动与减速运动。均速运动指令包括指令速度与指令位移，均速运动的加减速过程包括S曲线加减速的所有7个运动段。运动经加加速、匀加速与减加速达到指令速度并开始匀速运动，而后经加减速、匀减速与减减速恢复初始速度，同时达到指令位移；加速运动指令包括起始、终止速度与指令位移，加速运动的加减速过程为左半S曲线，运动由指令起始速度开始，经加加速、匀加速与减加速增至指令的终止速度并保持匀速运动；减速运动指令同样包括起始、终止速度与指令位移，区别在于其起始速度大于终止速度，减速运动的加减速过程为右半S曲线，运动由起始速度开始，经加减速、匀减速与减减速三个运动段，速度降至指令的终止速度并保持匀速运动，最终达到指令位移。

4）异常处理

受到最大容许加速度a_Max、指令位移L等的限制，部分极端条件下的运动指令，其S曲线加减速过程需要适当修正，才能继续执行。在均速运动或加速运动的加加速段、减速运动的加减速段，如果加速度达到容许加速度a_Max或-a_Max，运动不再等到速度变化量达到加/减速过程速度变化总量的1/4、3/8、7/16或15/32，而是直接进入匀加速段或匀减速段，以保证加速度不超过容许值；在均速运动的加加速、匀加速或减加速段，若当前位移达到指令位移量的50%，运动也不再等到速度变化量达到预定值，而是直接进入相应的减减速、匀减速或加减速段，执行相应的控制策略。

3.S曲线加减速的控制方法及电路

1）运动控制方法

（1）速度控制

本发明所需要的运动通过伺服电机实现，采用驱动脉冲结合方向控制信号的方式，脉冲频率控制运动速度，脉冲数量控制位移量大小。通过内部专用定时器实现驱动脉冲的脉宽与脉间，调节定时器计数值，可以实现脉宽、脉间与脉冲频率的控制。假定脉宽计数值cnt₀，脉间计数值cnt，基准时钟周期T_CK，得到驱动脉冲频率f的算式（11）：

f = \frac{1}{(cnt + {cnt}_{0}) T_{CK}} - - - (11)

本发明中步当量δ取1μm，基准时钟周期50ns（20MHz），驱动脉冲脉宽取定值1μs，则脉宽计数值cnt₀为20，由此得到速度v（单位mm/s）与计数值cnt的关系式（12）：

v = fδ = \frac{δ}{(cnt + {cnt}_{0}) T_{CK}} = \frac{1 \times 10^{- 3}}{(cnt + 20) 50 \times 10^{- 9}} = \frac{20 \times 10^{3}}{cnt + 20} - - - (12)

则速度控制量cnt的算式（13）为：

cnt = \frac{20 \times 10^{3}}{v} - 20 - - - (13)

（2）运动参数的确定

便于电路描述与计算，对式（8）～（10）进行一定的修订，假定

式（14）为新的加速度递推算式：

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} Δt \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (14)

其中，sigmaJ_kΔt为0-k时刻加速度变化量，即加加速度的一次积分。设置采样时间Δt不变，保持10μs，得到电路设计中所用的加速度计算公式（15）。式中加速度与加加速度单位分别为mm/s²与mm/s³。

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} / 10^{5} \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (15)

同样，假定

修订后的速度递推算式为公式（16）：

\{\begin{matrix} V_{k} = V_{0} + ({ka}_{0} + sigma A_{k} Δt) Δt \\ sigma A_{k} = sigma A_{k - 1} + {sigmaJ}_{k} \\ a_{0} = 0, V_{0} = 0 \end{matrix} - - - (16)

其中，(ka0+sigmaA_kΔt)Δt为0-k时刻速度变化总量，即加速度的一次积分。采样时间Δt同上，得到电路设计中所用的加速度计算公式（17）。其中加速度与速度单位分别为mm/s²与mm/s。

\{\begin{matrix} V_{k} = V_{0} + sigma A_{k} / 10^{10} \\ sigma A_{k} = sigma A_{k - 1} + sigma J_{k} \\ V_{0} = 0 \end{matrix} - - - (17)

式中10¹⁰除运算需要较多的硬件资源，因此计算时把速度分为整数与余数部分，sigmaA_k用作余数寄存器。每次运动采样执行一次积分运算后，若sigmaA_k≥10¹⁰，速度V的整数部分执行加1运算，而后将sigmaA_k-10¹⁰得到的新余数存入sigmaA_k，供下次速度计算使用；反之，sigmaA_k不变。

2）控制电路结构

S曲线加减速控制电路从控制计算机接收运动控制指令，根据指令参数计算运动段各运动时刻的加速度、速度以及速度控制量cnt；而后，根据速度控制量，动态调节脉间与脉冲频率，产生符合控制指令的伺服电机驱动脉冲。根据功能，加减速控制电路主要由CPU总线接口、指令参数寄存器、初始化电路、加加速度积分电路、加速度积分电路、执行寄存器、状态转换与控制电路、驱动脉冲发生电路、位移控制电路等构成，组成框架如图4。

总线接口提供与电路的数据传输接口，包括并行接口与SPI协议，总线接口将加加速度、平均速度、起始速度、终止速度与指令位移等指令参数写入相应寄存器，同时响应外部请求，送出电路工作状态；初始化电路读取指令参数，计算速度变化总量并合理规划各段速度变化量；加加速度、加速度积分电路执行积分预算，求取即时加速度、速度并写入相应的执行寄存器；脉间计算电路读取速度寄存器，根据即时速度计算驱动脉冲的脉间计数值；利用计算得到脉间计数值，脉冲发生电路对基准时钟计数，实现所需要的驱动脉冲；位移控制电路对输出脉冲计数，计算输出位移写入位移寄存器；状态转换与控制响应基准时钟，根据运行状态、即时速度、加速度等修改电路运行状态，控制电路有序协调动作。

3）电路状态转换

根据S曲线加减速的控制过程，结合控制电路必要的通信、运动初始化等功能，设置加减速控制电路的10种运行状态，见表1。

表1加减速控制电路的运行状态

状态

说明

状态

说明

状态

说明

状态

说明

状态

说明

IDLE

空闲态

RST

运动初始化

CAC

匀加速

ZAC

匀速

UDC

匀减速

DTR

数据传送

AAC

加加速

RAC

减加速

ADC

加减速

RDC

减减速

实现电路时，方便描述，定义加减速过程的当前位移s、速度变化总量Δ_v、匀速段起始位移L_f、匀加速段起始速度V_f、匀加速段终止速度V_s，速度变化系数k_v（0<k_v≤0.5）；同时定义运动模式m，均速运动m=0，加速运动m=1，减速运动m=2。假定加加速段、减加速段、加减速段、减减速段的速度变化k_vΔ_v，匀加速段、匀减速段的速度变化(1-2k_v)Δ_v。根据前文所述的运动规划，k_v取1/4、3/8、7/16或15/32，得到图5的状态转换条件见表2。

表2状态转换条件及含义

4）电路工作过程

上电以后，电路进入空闲状态；写信号有效，电路转数据传送状态，总线接口响应信号WR，依次写入加加速度、平均速度、指令位移、起始速度、终止速度；加减速使能信号有效，运动初始化状态被激活，电路读取指令参数寄存器，根据写入的参数判别加减速模式，初算各段速度变化量与匀加速段的起始速度V_f、终止速度V_s。空闲状态下，加减速使能信号也会激活运动初始化状态，此时电路直接读取指令参数寄存器进行初始化。

运动初始化完成后，电路开始S曲线加减速控制过程，加加速度与加速度积分电路10μs积分一次，求取即时加速度、速度并计算速度控制量cnt，由脉冲发生电路产生相应驱动脉冲，实现相应的运动段。

初始化结束，若指令为均速运动或加速运动(m=0,1)，电路转入加加速状态；同时，若为减速运动模式(m=2)，电路转加减速态；加加速状态下，即时加速度a≥a_Max，电路将即时速度V作为新的匀加速段起始速度V_f存入段间转换寄存器，同时重算并存储匀加速段终止速度V_s，而后转入匀加速状态；反之，若a≤a_Max且V≥V_f，电路直接进入匀加速状态。

匀加速状态，即时速度V≥V_s，电路进入减加速态。而后，当加速度减至0值，电路存储即时位移s作为匀速段起始位移L_f，进入匀速运动状态；匀速状态下，若为均速运动（m=0）且位移s≥L-L_f，电路转加减速态；若为加速运动（m=1）且即时位移s≥L，电路转空闲态。

加减速状态，电路等到即时速度V降至V_s后进入匀减速状态。然而，如果加减速状态下，出现a≤-a_Max的情况，电路将即时速度V作为V_S，同时重算速度V_f并存储，直接转入匀减速状态；匀减速状态，即时速度V≤V_f，电路进入减减速状态；最后，减减速状态下，即时位移s≥L，电路转空闲态。

图5中的转换条件

为均速运动（m=0）模式下的异常处理，在加加速、匀加速、减加速段的任何时刻，若当前位移达到指令位移的50%，电路直接越过正常处理进入加减速段；转换条件

为减速运动（m=2）模式下的异常处理，在运动的任意时刻，若当前位移达到指令位移，电路直接进入空闲状态；转换条件

为加速运动（m=1）模式下的异常处理，在控制的任意时刻，若当前位移达到指令位移，电路直接进入空闲状态。

4.电路测试与仿真

S曲线加减速控制电路测试与仿真结果见图6。图中，步当量1μm，速度变化系数k_v取1/4，减速运动指令的初始速度100mm/s，终止速度30mm/s，加加速度7000mm/s³，指令位移16000μm（输出驱动脉冲数16000）；加速运动指令的初始速度30mm/s，终止速度100mm/s，加加速度设定值仍为7000mm/s³，指令位移16000μm；均速运动指令速度100mm/s，加加速度设定值7000mm/s³，指令位移30000μm。

初始速度、终止速度、平均速度、加加速度与指令位移参数寄存器占用3位地址，地址依次为100B、101B、110B、111B与011B，波形图中的各信号及端口定义见表3。

表3电路仿真信号及定义

信号

定义

信号

定义

信号

定义

信号

定义

A

地址总线

BSY

状态信号

MovMod

运动模式

sDstCnt

当前位移计数值

D

16位数据总线

Cnt

脉间计数值

MovSec

当前运动段

DstCnt

指令位移计数值

CK

基准时钟，周期50ns

CntOrg

脉间计数初值

Jerk

加加速设定值

xSpdAvr

平均速度设定值

WR

写信号

CP

输出驱动脉冲

sSigmaJ

加加速度积分

xSpdOrg

初始速度设定值

GT

使能信号，高有效

RD

CPU读信号

sSigmaA

加速度积分

xSpdEnd

终止速度设定值

Acc

加速度，mm/S2

Spd

即时速度，mm/s

Cnt10us

10us定时计数值

xSpdFir

加加速段终止速度

MovMod表示运动模式，其值为0，按均速运动，需设定加加速、平均速度及指令位移；模式为1，加速运动，需设定加加速度、初始速度、终止速度及位移；模式为2，减速运动，需设定加加速度、初始速度、终止速度及位移；MovSec表示当前所处运动段，其值0-6分别对应加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速与减减速段。

图7所示为电路仿真中得到的速度曲线。其中，图7a）为上述加速运动指令的速度仿真曲线，图7b）为减速运动指令的速度仿真曲线，图7c）为均速运动指令的速度仿真曲线，符合S曲线加减速速度变化特征。

图8a）～c）所示为上述3条运动指令执行过程的电路时序仿真结果，其中的图8a）、图8b）、图8c）分别为30mm/s～100mm/s加速运动指令、100mm/s～30mm/s减速运动指令与100mm/s均速运动指令的信号仿真波形。

图8a）中电路经加加速、匀加速与减加速，进入匀速运动状态，保持指令的终止速度。加加速段，加速度随时间持续增大，达到488mm/s²后进入匀加速段，减加速段加速度持续减小，直至0加速度；相应地，指令时间内，即时速度Spd持续增大，直至匀速段的100mm/s。同时，加加速段内，即时速度变化逐渐加快，进入匀加速段后，速度变化的快慢固定不变；进入减加速段后，速度变化逐渐变慢，直到最终的匀速段。

图8b）中减速运动指令中，指令时间内，即时速度Spd持续减小，直至减减速段的31mm/s。同时，加减速段内，即时速度变化逐渐加快，进入匀减速段后，速度变化的快慢固定不变；进入减减速段后，速度变化逐渐变慢，直到最终速度并保持不变。此时存在1mm/s的速度误差。

图8c）为完整的S曲线加减速指令，具有典型S曲线加减速完整的7个运动段，经过加加速、匀加速与减加速，即时速度Spd达到99mm/s，进入匀速段。同样，此处存在1mm/s的计算误差；而后，电路进入加减速段，经加减速、匀减速与减减速，速度减至1mm/s。本发明的加减速过程中，速度低于1mm/s后，可以将速度直接减至0值，停止运动，而不是持续根据加速度减速。同样，启动时，速度也从1mm/s开始加速，见图8c。

5.结论

本发明对S曲线加减速开展了系统深入的探讨，结合数控系统、机器人控制等领域的工程实际，给出了适于硬件描述与电路实现的S曲线加减速数据处理过程及具体方法，研制了S曲线加减速的高实时控制电路及集成控制芯片，主要包括：

1）全面分析S曲线加减速的运动过程，推导了S曲线加减速各运动参数对时间的运动参数控制方程式，并对其离散化，得到离散化的S曲线加减速运动参数控制方程式。

2）根据S曲线加减速运动参数曲线的几何特征，提出S加加速的控制条件；结合数字实现，提出各运动参数控制的递推公式。

3）结合机器人、数控系统等的具体应用，将S曲线加减速分为加速、减速与均速运动控制三种模式，提出了基于速度变化量、运动段间转换速度与运动段间转换位移的运动规划模型及异常处理机制。

4）提出基于上述算法与模型的S曲线加减速控制电路、电路工作状态及转换过程、电路状态转换条件以及相关的电路工作过程。

5）基于上述模型与方法，采用大规模可编程逻辑器件，实现了S曲线加减速的专用集成电路并进行了仿真测试与验证。

Claims

1.一种运动控制系统S曲线加减速的实现方法，其特征是

1）S曲线加减速的实现条件：

\{\begin{matrix} t_{0} = t_{2} - t_{1} = t_{4} - t_{3} = t_{6} - t_{5} \\ a_{t_{0}} = a_{t_{1}} = - a_{t_{4}} = - a_{t_{5}} \\ V_{t_{0}} = V_{t_{2}} - V_{t_{1}} = V_{t_{3}} - V_{t_{4}} = V_{t_{5}} - V_{t_{6}} \end{matrix} - - - (6)

Δt_1-2=t₂-t₁,Δt_3-4=t₄-t₃,Δt_5-6=t₆-t₅

\{\begin{matrix} L / V_{L} &GreaterEqual; t_{0} + {Δt}_{1 - 2} + {Δt}_{3 - 4} + {Δt}_{5 - 6} \\ a_{M} \leq a_{Max} \end{matrix} - - - (7)

2）S曲线加减速的递推公式

J_{k} = \{\begin{matrix} J_{M}, & 0 \leq t \leq t_{0}, t_{5} < t \leq t_{6}, 0 \leq k \leq n_{t_{0}}, n_{t_{5}} < k \leq n_{t_{6}} \\ 0, & t_{0} < t \leq t_{1}, t_{2} < t \leq t_{3}, t_{4} < t \leq t_{5}, n_{t_{0}} < k \leq n_{t_{1}}, n_{t_{2}} < k \leq n_{t_{3}}, \\ - J_{M} & t_{1} < t \leq t_{2}, t_{3} < t \leq t_{4}, n_{t_{1}} < k \leq n_{t_{2}}, n_{t_{3}} < k \leq n_{t_{4}} \end{matrix} n_{t_{4}} < k \leq n_{t_{5}}- - - - (8)

\{\begin{matrix} a_{0} = 0 \\ a_{k} = a_{k - 1} + J_{k} Δk \end{matrix} - - - (9)

\{\begin{matrix} V_{0} = 0 \\ V_{k} = V_{k - 1} + a_{k} Δk \end{matrix} - - - (10)

2.根据权利要求1所述运动控制系统S曲线加减速的实现方法，其特征是基于该方法的运动控制系统S曲线加减速的运动规划如下：

1）S曲线加减速的运动规划：

2）异常处理：

3.根据权利要求2所述运动控制系统S曲线加减速的实现方法，其特征是所述运动规划中：

所述2）异常处理中：

情形a中，所述预定值是

情形b中，所述预定值是

匀加速段速度变化量取加速过程速度变化总量的1/2、1/4、1/8或1/16；

4.根据权利要求2所述运动控制系统S曲线加减速的实现方法，其特征是基于该方法的S曲线加减速控制方法，其特征是：

（1）速度控制：

f = \frac{1}{(cnt + {cnt}_{0}) T_{CK}} - - - (11)

（2）运动参数的确定：

式（14）为新的通用加速度递推算式：

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} Δt \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (14)

定义

修订后的通用速度递推算式（16）：

\{\begin{matrix} V_{k} = V_{0} + ({ka}_{0} + sigma A_{k} Δt) Δt \\ sigma A_{k} = sigma A_{k - 1} + {sigmaJ}_{k} \\ a_{0} = 0, V_{0} = 0 \end{matrix} - - - (16)

5.根据权利要求4所述运动控制系统S曲线加减速的实现方法，其特征是所述S曲线加减速控制方法所控制的运动通过伺服电机实现，采用驱动脉冲结合方向控制信号的方式，脉冲频率控制运动速度，脉冲数量控制位移量大小；通过专用定时器实现驱动脉冲的脉宽与脉间，调节该定时器计数值，进而实现脉宽、脉间与脉冲频率的控制。

6.根据权利要求4或5所述运动控制系统S曲线加减速的实现方法，其特征是所述S曲线加减速控制方法中，步当量δ取1μm，基准时钟周期50ns即20MHz，驱动脉冲脉宽取定值1μs，则脉宽计数值cnt₀为20，则：

（1）对于速度控制：

由此得到速度v（单位mm/s）与计数值cnt的关系式（12）：

v = fδ = \frac{δ}{(cnt + {cnt}_{0}) T_{CK}} = \frac{1 \times 10^{- 3}}{(cnt + 20) 50 \times 10^{- 9}} = \frac{20 \times 10^{3}}{cnt + 20} - - - (12)

则速度控制量cnt的算式（13）为：

cnt = \frac{20 \times 10^{3}}{v} - 20 - - - (13)

（2）对于运动参数的确定：

\{\begin{matrix} a_{k} = a_{0} + sigma J_{k} / 10^{5} \\ sigma J_{k} = sigma J_{k - 1} + J_{k} \\ a_{0} = 0 \end{matrix} - - - (15)

\{\begin{matrix} V_{k} = V_{0} + sigma A_{k} / 10^{10} \\ sigma A_{k} = sigma A_{k - 1} + sigma J_{k} \\ V_{0} = 0 \end{matrix} - - - (17)

7.根据权利要求4所述运动控制系统S曲线加减速的实现方法，其特征是对于所述S曲线加减速控制方法，采用该方法的控制系统的工作过程如下：