一文教你快速搞懂速度曲线规划之S形曲线（超详细+图文+推导+附件代码）

本文介绍了运动控制终的S曲线，通过matlab和C语言实现并进行仿真；本文篇幅较长，请自备茶水；
请帮忙点个赞；
请帮忙点个赞；
请帮忙点个赞；

之前有介绍过T形曲线，具体可以参考《一文教你快速搞懂速度曲线规划之T形曲线》，本文将在原先的基础上进行进一步扩展，另外由于介绍速度曲线的论文较多，本文会在具体引用的地方给出原文出处；先对比一下两者的差别；

网图侵删

文章目录

1 前言
2 理论分析

2.1 加速度时间关系方程
2.2 速度时间关系方程
2.3 位移时间关系方程

3 程序实现的思路

3.1 TkT_kTk 推导
3.2 JJJ 的推导

4 matlab 程序
5 总结
6 参考

1 前言

S形加减速的最重要特征是该算法的加速度/减速度曲线的形状如字母 S。S形加减速的速度曲线平滑 ,从而能够减少对控制过程中的冲击，并使插补过程具有柔性 ¹。
由于T形曲线在加速到匀速的切换过程中，实际中存在较大过冲，因此这里对比一下T曲线和7段S曲线的实际过程；

T形：加速 -> 匀速 -> 减速
S形：加加速(T1T_1T1) -> 匀加速(T2T_2T2) -> 减加速(T3T_3T3)-> 匀速(T4T_4T4)-> 加减速(T5T_5T5)-> 匀减速(T6T_6T6)-> 减减速(T7T_7T7)

上文在加速这块的文字描述可能读起来起来有点绕，下面看图：

2 理论分析

由于S曲线在加减速的过程中，其加速度是变化的，因此这里引入了新的一个变量 JJJ，即加加速度。
J=dadt J = \cfrac{d_a}{d_t} J=dtda
因此对应上图的7段S速度曲线中，规定最大加速为amaxa_{max}amax，最小加速度为−amax-a_{max}−amax，则加速度的关系；

加加速(T1T_1T1)：aaa逐渐增大，此时a=JT1,0<t≤t1;a = JT_1,\qquad 0<t\le t_1;a=JT1,0<t≤t1;
匀加速(T2T_2T2)：aaa达到最大，此时a=amax,t1<t≤t2;a = a_{max}, \qquad t_1<t\le t_2;a=amax,t1<t≤t2;
减加速(T3T_3T3)：aaa逐渐减小，此时a=amax−JT3,t2<t≤t3;a = a_{max}-JT_3, \qquad t_2<t\le t_3;a=amax−JT3,t2<t≤t3;
匀速(T4T_4T4)：aaa不变化，此时a=0,t3<t≤t4;a = 0, \qquad t_3<t\le t_4;a=0,t3<t≤t4;
加减速(T5T_5T5)：∣a∣|a|∣a∣ 逐渐减小，此时a=−JT5,t4<t≤t5;a = -JT_5, \qquad t_4<t\le t_5;a=−JT5,t4<t≤t5;
匀减速(T6T_6T6)：∣a∣|a|∣a∣ 达到最小，此时a=−amax,t5<t≤t6;a = -a_{max}, \qquad t_5<t\le t_6;a=−amax,t5<t≤t6;
减减速(T7T_7T7)：∣a∣|a|∣a∣ 达到最小，此时a=−amax+−JT7,t6<t≤t7;a = -a_{max}+-JT_7, \qquad t_6<t\le t_7;a=−amax+−JT7,t6<t≤t7;

其中 Tk=tk−tk−1(k=1,...,7)T_k =t_k - t_{k -1} (k =1 , ..., 7)Tk=tk−tk−1(k=1,...,7)

所以通常需要确定三个最基本的系统参数 :系统最大速度 vmaxv_{max}vmax ，最大加速度a_{max} ，加加速度JJJ，就可以可确定整个运行过程² ；

最大速度：反映了系统的最大运行能力；
最大加速度：反映了系统的最大加减速能力；
加加速度：反映了系统的柔性；
- 柔性越小，过冲越大，运行时间越短；
- 柔性越大，过冲越小，运行时间越长；

2.1 加速度时间关系方程

整个加速度变化的过程具体如下图所示；

再次强调一下 TTT 和 ttt 的关系，Tk=tk−tk−1(k=1,...,7)T_k =t_k - t_{k -1} (k =1 , ..., 7)Tk=tk−tk−1(k=1,...,7)
另外这里再引入变量 τ\tauτ，
τk=t−tk−1⋯①\tau_k = t - t_{k-1}\quad \cdots \quad①τk=t−tk−1⋯①
比如，当前时刻 t2<t≤t3t_2<t\le t_3t2<t≤t3 ,即 ttt 位于区间 T3T_3T3，则如果将 t2t_2t2 作为初始点，则 τ3\tau_3τ3w为 ttt 相对于t2t_2t2时刻的时间，则有：
τ3=t−t2 \tau_3 = t - t_2 τ3=t−t2
下面可以得到加速度与时间的关系函数，具体如下：
a(t)={Jt,0<t≤t1amax,t1<t≤t2amax−J(t−t2),t2<t≤t30,t3<t≤t4−J(t−t4),t4<t≤t5−amax,t5<t≤t6−amax−J(t−t7),t6<t≤t7⋯② \begin{aligned} a(t)=\begin{cases} Jt, & 0<t\le t_1 \\ a_{max}, & t_1<t\le t_2 \\ a_{max}-J(t-t_2), & t_2<t\le t_3 \\ 0,& t_3<t\le t_4 \\ -J(t-t_4),& t_4<t\le t_5 \\ -a_{max},& t_5<t\le t_6 \\ -a_{max}-J(t-t_7),& t_6<t\le t_7 \end{cases} \end{aligned} \quad \cdots \quad②a(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Jt,amax,amax−J(t−t2),0,−J(t−t4),−amax,−amax−J(t−t7),0<t≤t1t1<t≤t2t2<t≤t3t3<t≤t4t4<t≤t5t5<t≤t6t6<t≤t7⋯②

根据 ① 式，将 τ\tauτ 代入 ② 式可以得到：
a(t)={Jτ1,0<t≤t1amax,t1<t≤t2amax−Jτ3,t2<t≤t30,t3<t≤t4−Jτ3,t4<t≤t5−amax,t5<t≤t6−amax+Jτ7,t6<t≤t7 \begin{aligned} a(t)=\begin{cases} J\tau_1, & 0<t\le t_1 \\ a_{max}, & t_1<t\le t_2 \\ a_{max}-J\tau_3, & t_2<t\le t_3 \\ 0,& t_3<t\le t_4 \\ -J\tau_3,& t_4<t\le t_5 \\ -a_{max},& t_5<t\le t_6 \\ -a_{max}+J\tau_7,& t_6<t\le t_7 \end{cases} \end{aligned}a(t)=⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Jτ1,amax,amax−Jτ3,0,−Jτ3,−amax,−amax+Jτ7,0<t≤t1t1<t≤t2t2<t≤t3t3<t≤t4t4<t≤t5t5<t≤t6t6<t≤t7

上式中 J>0J > 0J>0；

2.2 速度时间关系方程

速度和加速度满足 v=atv=atv=at ；加加速度和速度的关系满足：
v=12Jt2v = \cfrac{1}{2} Jt^2v=21Jt2

结合加速度时间关系并结合② 式可以得到速度曲线关系，具体关系如下图所示；

进一步简化可以得到：

2.3 位移时间关系方程

位移 SSS 和加加速度 JJJ 直接满足关系如下：
S=16Jt3 S = \cfrac{1}{6} Jt^3 S=61Jt3

简单推导
{S=∫0tvdtv=12Jt2\begin{cases} S = \int_{0}^{t}vdt \\ v = \cfrac{1}{2} Jt^2 \end{cases}⎩⎨⎧S=∫0tvdtv=21Jt2
因此可以得到：
S=∫0t12Jt2dt=16Jt3∣0t S = \int_{0}^{t}\cfrac{1}{2}Jt^2dt = \cfrac{1}{6}Jt^3|_0^t\\ S=∫0t21Jt2dt=61Jt3∣0t

积分忘的差不多了，回去再复习一下；

最终位移的方程如下所示；

3 程序实现的思路

正如前面所提到的，S曲线规划需要确定三个最基本的系统参数：系统最大速度 vmaxv_{max}vmax ，最大加速度a_{max} ，加加速度JJJ，这样就可以确定这个运行过程。
这里有一个隐性的条件，就是在运行的过程中可以达到最大速度，这样才是完整的7段S曲线，另外这里还有一些中间参数：

tm=vmaxamaxt_m = \cfrac{v_{max}}{a_{max}}tm=amaxvmax，因此有 T1=T3=T5=T7T_1=T_3=T_5=T_7T1=T3=T5=T7；
加加速度 J=J1=J3=J5=J7=amaxtmJ = J_1= J_3= J_5= J_7=\cfrac{a_{max}}{t_m}J=J1=J3=J5=J7=tmamax；
T2=T6T_2 = T_6T2=T6；
TfT_fTf，用户给定整个运行过程所需要的时间；

但是通常实际过程中关心amaxa_{max}amax，vmaxv_{max}vmax，TfT_fTf；

3.1 TkT_kTk 推导

理想状态假设存在 T2T_2T2和T6T_6T6，则推导过程如下：

{v1=12JT12v2=v1+amaxT2v3=v2+12JT32v3=vmaxT1=T3\begin{cases} v_1 = \cfrac{1}{2}JT_1^2 \\ v_2 = v_1 + a_{max}T_2\\ v_3 = v_2 + \cfrac{1}{2}JT_3^2 \\ v_3 = v_{max} \\ T_1 = T_3 \end{cases}⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧v1=21JT12v2=v1+amaxT2v3=v2+21JT32v3=vmaxT1=T3

因此可以得到：
vmax=12JT12+amaxT2+12JT12 v_{max} = \cfrac{1}{2}JT_1^2 + a_{max}T_2 +\cfrac{1}{2}JT_1^2vmax=21JT12+amaxT2+21JT12

简化之后得到：
vmax=JT12+amaxT2 v_{max} = JT_1^2 + a_{max}T_2vmax=JT12+amaxT2

根据②式可知：amax=JT1a_{max} = JT_1amax=JT1

最终得到：
T2=T6=Vmax−Vsamax−T1T_2 = T_6 = \cfrac{V_{max} - V_s}{a_{max}} - T_1 T2=T6=amaxVmax−Vs−T1

Vs为初始速度；V_s为初始速度；Vs为初始速度；

下面可以根据位移时间关系方程进行离散化的程序编写。

假设可以到达最大速度，且用户给定了整个过程运行时间TfT_fTf，则 T4T_4T4 的推导如下：
{Tf=T1+T2+T3+T4+T5+T6+T7T2=T6T1=T3=T5=T7\begin{cases} T_f = T_1 + T_2 + T_3 + T_4 + T_5 + T_6 + T_7 \\ T_2 = T_6 \\ T_1 = T_3 = T_5 = T_7 \\ \end{cases}⎩⎪⎨⎪⎧Tf=T1+T2+T3+T4+T5+T6+T7T2=T6T1=T3=T5=T7
简化上式可以得到：
T4=Tf−2T2−4T1 T_4 = T_f - 2T_2 - 4T_1 T4=Tf−2T2−4T1
根据 T1=amaxJT_1 = \cfrac{a_{max}}{J}T1=Jamax代入上式可得：
T4=Tf−2vmaxamax−2amaxJ T_4 = T_f - 2\cfrac{v_{max}}{a_{max}}-2\cfrac{a_{max}}{J} T4=Tf−2amaxvmax−2Jamax

3.2 JJJ 的推导

这时候还剩下JJJ需要计算，通过已量 Tf,vmax,amaxT_f,v_{max},a_{max}Tf,vmax,amax可以推导出来；
首先位移之间满足关系如下：
Sref=Sa+S4+SdS_{ref} = S_a + S_4 + S_dSref=Sa+S4+Sd
其中加速区长度为 SaS_aSa；
其中减速区长度为 SdS_dSd；
{Sa=vs(2T1+T2)+12JT1(2T12+3T1T2+T22)Sb=v3(2T5+T6)−12JT5(2T52+3T5T6+T62) \begin{cases} S_a = v_s(2T_1 + T_2)+\cfrac{1}{2}JT_1(2T_1^2 + 3T_1T_2 + T_2^2) \\ S_b = v_3(2T_5 + T_6)-\cfrac{1}{2}JT_5(2T_5^2 + 3T_5T_6 + T_6^2) \end{cases} ⎩⎪⎨⎪⎧Sa=vs(2T1+T2)+21JT1(2T12+3T1T2+T22)Sb=v3(2T5+T6)−21JT5(2T52+3T5T6+T62)
具体推导；²
前面提到过T1=T3=T5=T7T_1 = T_3 = T_5 = T_7T1=T3=T5=T7，T2=T6T_2 = T_6T2=T6，因此在VsV_sVs=0的时候，则
Sa+Sb=v3(2T5+T6)=v1(2T1+T2)⋯④S_a + S_b = v_3(2T_5 + T_6) = v_1(2T_1 + T_2) \cdots ④Sa+Sb=v3(2T5+T6)=v1(2T1+T2)⋯④

这里简单推导一下：
{S4=v4T4T4=Tf−(T1+T2+T3+T4+T5+T6)T1=T3=T5=T7=amaxJT2=T6=Vmaxamax−T1Sref=Sa+S4+Sb⋯⑤\begin{cases} S_4 = v_4T_4 \\ T_4 = T_f - (T_1 + T_2 + T_3 + T_4 + T_5 + T_6) \\ T_1 = T_3 = T_5 = T_7 = \cfrac{a_{max}}{J} \\ T_2 = T_6 = \cfrac{V_{max}}{a_{max}} - T_1 \\ S_ref = S_a + S_4 + S_b \end{cases} \cdots ⑤ ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧S4=v4T4T4=Tf−(T1+T2+T3+T4+T5+T6)T1=T3=T5=T7=JamaxT2=T6=amaxVmax−T1Sref=Sa+S4+Sb⋯⑤
根据④，⑤最终简化得到：
J=2amaxvmaxvmaxTf−vmax2−Srefamax J = \cfrac{2a_{max}v_{max}}{v_{max}T_f-v_{max}^2-S_{ref}a_{max}} J=vmaxTf−vmax2−Srefamax2amaxvmax

TfT_fTf：为运行的总时间
SrefS_{ref}Sref：为运行的总路程

4 matlab 程序

matlab程序亲测可以运行，做了简单的修改，
因为这里直接给定了整个运行过程的时间，所以需要在SCurvePara函数中求出加加速度 JJJ 的值，路程SSS为 1：

SCurvePara

 function [Tf1,V,A,J,T] = SCurvePara(Tf, v, a)

 T = zeros(1,7);

for i=1:1000

    % 加加速度 J

    J = (a^2 * v) / (Tf*v*a - v^2 - a);

    % Tk

    T(1) = a / J;

    T(2) = v / a - a / J; % t2 = v / a - t1;

    T(3) = T(1);

    T(4) = Tf - 2 * a / J - 2 * v / a;    % t4 = Tf - 4*t1 - 2*t2;

    T(5) = T(3);

    T(6) = T(2);

    T(7) = T(1);

    % 根据T2和T4判断S曲线的类型

    if T(2) < -1e-6

        a = sqrt(v*J);

        display('t2<0');

    elseif T(4) < -1e-6

        v = Tf*a/2 - a*a/J;

        display('t4<0');

    elseif J < -1e-6

        Tf = (v^2 + a) / (v*a) + 1e-1;

        display('J<0');

    else

        break;

    end

end

 A = a;

 V = v;

 Tf1 = Tf;

 end

SCurveScaling

 function s = SCurveScaling(t,V,A,J,T,Tf)

% J = (A^2 * V) / (Tf*V*A - V^2 - A);

% T(1) = A / J;

% T(2) = V / A - A / J; % T(2) = V / A - T(1);

% T(3) = T(1);

% T(4) = Tf - 2 * A / J - 2 * V / A;    % T(4) = Tf - 4*T(1) - 2*T(2);

% T(5) = T(3);

% T(6) = T(2);

% T(7) = T(1);

%%

if (t >= 0 && t <= T(1))

    s = 1/6 * J * t^3;

elseif (  t > T(1) && t <= T(1)+T(2) )

    dt = t - T(1);

    s = 1/2 * A * dt^2 + A^2/(2*J) * dt...

        + A^3/(6*J^2);

elseif ( t > T(1)+T(2) && t <= T(1)+T(2)+T(3) )

     dt = t - T(1) - T(2);

     s = -1/6*J*dt^3 + 1/2*A*dt^2 + (A*T(2) + A^2/(2*J))*dt ...

         + 1/2*A*T(2)^2 + A^2/(2*J)*T(2) + A^3/(6*J^2);

elseif ( t > T(1)+T(2)+T(3) && t <= T(1)+T(2)+T(3)+T(4) )

     dt = t - T(1) - T(2) - T(3);

     s = V*dt ...

         +  (-1/6*J*T(3)^3) + 1/2*A*T(3)^2 + (A*T(2) + A^2/(2*J))*T(3) + 1/2*A*T(2)^2 + A^2/(2*J)*T(2) + A^3/(6*J^2);

elseif ( t > T(1)+T(2)+T(3)+T(4) && t <= T(1)+T(2)+T(3)+T(4)+T(5) )

     t_temp = Tf - t;

     dt = t_temp - T(1) - T(2);

     s = -1/6*J*dt^3 + 1/2*A*dt^2 + (A*T(2) + A^2/(2*J))*dt ...

         + 1/2*A*T(2)^2 + A^2/(2*J)*T(2) + A^3/(6*J^2);

     s = 1 - s;

elseif ( t > T(1)+T(2)+T(3)+T(4)+T(5) && t <= T(1)+T(2)+T(3)+T(4)+T(5)+T(6) )

     t_temp = Tf - t;

     dt = t_temp - T(1);

     s = 1/2 * A * dt^2 + A^2/(2*J) * dt + A^3/(6*J^2);

     s = 1 - s;

elseif ( t > T(1)+T(2)+T(3)+T(4)+T(5)+T(6) && t <= T(1)+T(2)+T(3)+T(4)+T(5)+T(6)+T(7) + 1e5 )

     t_temp = Tf - t;

     s = 1/6 * J * t_temp^3;

     s = 1 - s;

end

end

测试的代码如下：
TEST

%%

N = 500;

ThetaStart = 0;

ThetaEnd = 90;

VTheta = 90;    %1

ATheta = 135;   %1.5

Tf = 1.8;

v = VTheta/(ThetaEnd - ThetaStart);

a = ATheta/(ThetaEnd - ThetaStart);

v = abs(v);

a = abs(a);

Theta = zeros(1,N);

s = zeros(1,N);

sd = zeros(1,N);

sdd = zeros(1,N);

[TF,V,A,J,T] = SCurvePara(Tf, v, a);

display(J, 'J:');

display(TF,'Tf:');

display(V,'v:');

display(A, 'da:');

display(TF-Tf,'dTf:');

display(V-v,'dv:');

display(A-a, 'da:');

t=linspace(0,TF,N);

dt = t(2) - t(1);

for i = 1:N

    if i == N

        a = a;

    end

    s(i) = SCurveScaling(t(i),V,A,J,T,TF);

    Theta(i) = ThetaStart + s(i) * (ThetaEnd - ThetaStart);

    if i>1

        sd(i-1) = (s(i) - s(i-1)) / dt;

    end

    if i>2

        sdd(i-2) = (sd(i-1) - sd(i-2)) / dt;

    end

end

subplot(3,1,1);

legend('Theta');

xlabel('t');

subplot(3,1,1);

plot(t,s)

legend('位移');

xlabel('t');

title('位置曲线');

subplot(3,1,2);

plot(t,sd);

legend('速度');

xlabel('t');

title('速度曲线');

subplot(3,1,3);

plot(t,sdd);

legend('加速度');

xlabel('t');

title('加速度曲线');

看到最终仿真结果和预期相同；

5 总结

本文只对7段的S曲线规划做了详细的推导和介绍，matlab中的程序对于4段和5段都有做实现，很多是在理想情况下进行推导的，初始速度默认为0，终止速度也为0，并且假设加减速区域相互对称。最终运行结果符合预期效果。

文中难免有错误和纰漏之处，请大佬们不吝赐教
创作不易，如果本文帮到了您；
请帮忙点个赞；
请帮忙点个赞；
请帮忙点个赞；

6 参考

陈友东魏洪兴王琦魁.数控系统的直线和 S 形加减速离散算法[D].北京:中国机械工程,2010. ︎
郭新贵李从心 S 曲线加减速算法研究上海交通大学国家模具 CAD 工程研究中心 , 200030 ︎ ︎