一文教你快速搞懂速度曲線規划之S形曲線（超詳細+圖文+推導+附件代碼）

 

本文介紹了運動控制終的S曲線，通過matlab和C語言實現並進行仿真；本文篇幅較長，請自備茶水；
請幫忙點個贊 👍👍👍；
請幫忙點個贊 👍👍👍；
請幫忙點個贊 👍👍👍；

之前有介紹過T形曲線，具體可以參考《一文教你快速搞懂速度曲線規划之T形曲線》，本文將在原先的基礎上進行進一步擴展，另外由於介紹速度曲線的論文較多，本文會在具體引用的地方給出原文出處；先對比一下兩者的差別；

網圖侵刪

文章目錄

• 1 前言
• 2 理論分析
• 2.1 加速度時間關系方程
• 2.2 速度時間關系方程
• 2.3 位移時間關系方程
• 3 程序實現的思路
• 3.1 $T^k$ 推導
• 3.2 $J$ 的推導
• 4 matlab 程序
• 5 總結
• 6 參考

 

1 前言

S形加減速的最重要特征是該算法的加速度/減速度曲線的形狀如字母 S。S形加減速的速度曲線平滑 ,從而能夠減少對控制過程中的沖擊，並使插補過程具有柔性 ¹。
由於T形曲線在加速到勻速的切換過程中，實際中存在較大過沖，因此這里對比一下T曲線和7段S曲線的實際過程；

• T形：加速 -> 勻速 -> 減速
• S形：加加速($T^1$) -> 勻加速($T^2$) -> 減加速($T^3$)-> 勻速($T^4$)-> 加減速($T^5$)-> 勻減速($T^6$)-> 減減速($T^7$)

上文在加速這塊的文字描述可能讀起來起來有點繞，下面看圖：

2 理論分析

由於S曲線在加減速的過程中，其加速度是變化的，因此這里引入了新的一個變量 $J$，即加加速度。
$$J=\genfrac{}{}{0ex}{}{d^a}{d^t}$$
因此對應上圖的7段S速度曲線中，規定最大加速為$a^{max}$，最小加速度為$-a^{max}$，則加速度的關系；

• 加加速($T^1$)：$a$逐漸增大，此時$a=J{T}^1,0<t\le t^1;$
• 勻加速($T^2$)：$a$達到最大，此時$a=a^{max},t^1<t\le t^2;$
• 減加速($T^3$)：$a$逐漸減小，此時$a=a^{max}-J{T}^3,t^2<t\le t^3;$
• 勻速($T^4$)：$a$不變化，此時$a=0,t^3<t\le t^4;$
• 加減速($T^5$)：$|a|$ 逐漸減小，此時$a=-J{T}^5,t^4<t\le t^5;$
• 勻減速($T^6$)：$|a|$ 達到最小，此時$a=-a^{max},t^5<t\le t^6;$
• 減減速($T^7$)：$|a|$ 達到最小，此時$a=-a^{max}+-J{T}^7,t^6<t\le t^7;$

其中 $T^k=t^k-t^{k-1}(k=1,...,7)$

所以通常需要確定三個最基本的系統參數 :系統最大速度 $v^{max}$ ，最大加速度a_{max} ，加加速度$J$，就可以可確定整個運行過程² ；

• 最大速度：反映了系統的最大運行能力 ；
• 最大加速度：反映了系統的最大加減速能力 ；
• 加加速度：反映了系統的柔性；
  • 柔性越小，過沖越大，運行時間越短；
  • 柔性越大，過沖越小，運行時間越長；

2.1 加速度時間關系方程

整個加速度變化的過程具體如下圖所示；


再次強調一下 $T$ 和 $t$ 的關系，$T^k=t^k-t^{k-1}(k=1,...,7)$
另外這里再引入變量 $\tau$，
$${\tau }^k=t-t^{k-1}\cdots ①$$
比如，當前時刻 $t^2<t\le t^3$ ,即 $t$ 位於區間 $T^3$，則如果將 $t^2$ 作為初始點，則 ${\tau }^3$w為 $t$ 相對於$t^2$時刻的時間，則有：
$${\tau }^3=t-t^2$$
下面可以得到加速度與時間的關系函數，具體如下：
$$\begin{array}{r}a(t)=\{\begin{array}{ll}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{array}\end{array}\cdots ②$$

根據 ① 式，將 $\tau$ 代入 ② 式可以得到：
$$\begin{array}{r}a(t)=\{\begin{array}{ll}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{array}\end{array}$$

上式中 $J>0$；

2.2 速度時間關系方程

速度和加速度滿足 $v=at$ ；加加速度和速度的關系滿足：
$$v=\genfrac{}{}{0ex}{}{1}{2}J{t}^2$$

結合加速度時間關系並結合② 式可以得到速度曲線關系，具體關系如下圖所示；





進一步簡化可以得到：

2.3 位移時間關系方程

位移 $S$ 和加加速度 $J$ 直接滿足關系如下：
$$S=\genfrac{}{}{0ex}{}{1}{6}J{t}^3$$

簡單推導
$$\{\begin{array}{l}S={\int }^{0}vdt\\ v=\genfrac{}{}{0ex}{}{1}{2}J{t}^2\end{array}$$
因此可以得到：
$$S={\int }^0\genfrac{}{}{0ex}{}{1}{2}J{t}^{2}dt=\genfrac{}{}{0ex}{}{1}{6}J{t}^3{|}^0$$

積分忘的差不多了，回去再復習一下；

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

3 程序實現的思路

正如前面所提到的，S曲線規划需要確定三個最基本的系統參數 ：系統最大速度 $v^{max}$ ，最大加速度a_{max} ，加加速度$J$，這樣就可以確定這個運行過程。
這里有一個隱性的條件，就是在運行的過程中可以達到最大速度，這樣才是完整的7段S曲線，另外這里還有一些中間參數：

• $t^m=\genfrac{}{}{0ex}{}{v^{max}}{a^{max}}$，因此有 $T^1=T^3=T^5=T^7$；
• 加加速度 $J=J^1=J^3=J^5=J^7=\genfrac{}{}{0ex}{}{a^{max}}{t^m}$；
• $T^2=T^6$；
• $T^f$，用戶給定整個運行過程所需要的時間；

但是通常實際過程中關心$a^{max}$，$v^{max}$，$T^f$；

3.1 $T^k$ 推導

理想狀態假設存在 $T^2$和$T^6$，則推導過程如下：

$$\{\begin{array}{l}{v}^1=\genfrac{}{}{0ex}{}{1}{2}J{T}^1\\ v^2=v^1+a^{max}{T}^2\\ v^3=v^2+\genfrac{}{}{0ex}{}{1}{2}J{T}^3\\ v^3=v^{max}\\ T^1=T^3\end{array}$$

因此可以得到：
$$v^{max}=\genfrac{}{}{0ex}{}{1}{2}J{T}^1+a^{max}{T}^2+\genfrac{}{}{0ex}{}{1}{2}J{T}^1$$

簡化之后得到：
$$v^{max}=J{T}^1+a^{max}{T}^2$$

根據②式可知：$a^{max}=J{T}^1$

最終得到：
$$T^2=T^6=\genfrac{}{}{0ex}{}{V^{max}-V^s}{a^{max}}-T^1$$

$V^s為初始速度；$

下面可以根據位移時間關系方程進行離散化的程序編寫。

假設可以到達最大速度，且用戶給定了整個過程運行時間$T^f$，則 $T^4$ 的推導如下：
$$\{\begin{array}{l}{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{array}$$
簡化上式可以得到：
$$T^4=T^f-2T^2-4T^1$$
根據 $T^1=\genfrac{}{}{0ex}{}{a^{max}}{J}$代入上式可得：
$$T^4=T^f-2\genfrac{}{}{0ex}{}{v^{max}}{a^{max}}-2\genfrac{}{}{0ex}{}{a^{max}}{J}$$

3.2 $J$ 的推導

這時候還剩下$J$需要計算，通過已量 $T^f,v^{max},a^{max}$可以推導出來；
首先位移之間滿足關系如下：
$$S^{ref}=S^a+S^4+S^d$$
其中加速區長度為 $S^a$；
其中減速區長度為 $S^d$；
$$\{\begin{array}{l}{S}^a=v^s(2T^1+T^2)+\genfrac{}{}{0ex}{}{1}{2}J{T}^1(2T^1+3T^1{T}^2+T^2)\\ S^b=v^3(2T^5+T^6)-\genfrac{}{}{0ex}{}{1}{2}J{T}^5(2T^5+3T^5{T}^6+T^6)\end{array}$$
具體推導；²
前面提到過$T^1=T^3=T^5=T^7$，$T^2=T^6$，因此在$V^s$=0的時候，則
$$S^a+S^b=v^3(2T^5+T^6)=v^1(2T^1+T^2)\cdots ④$$

這里簡單推導一下：
$$\{\begin{array}{l}{S}^4=v^4{T}^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=\genfrac{}{}{0ex}{}{a^{max}}{J}\\ T^2=T^6=\genfrac{}{}{0ex}{}{V^{max}}{a^{max}}-T^1\\ S^{r}ef=S^a+S^4+S^b\end{array}\cdots ⑤$$
根據④，⑤最終簡化得到：
$$J=\genfrac{}{}{0ex}{}{2a^{max}{v}^{max}}{v^{max}{T}^f-v^{max}-S^{ref}{a}^{max}}$$

$T^f$：為運行的總時間
$S^{ref}$：為運行的總路程

4 matlab 程序

matlab程序親測可以運行，做了簡單的修改，
因為這里直接給定了整個運行過程的時間，所以需要在SCurvePara函數中求出加加速度 $J$ 的值，路程$S$為 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 參考

---

1. 陳友東 魏洪興 王琦魁.數控系統的直線和 S 形加減速離散算法[D].北京:中國機械工程,2010. ↩︎

2. 郭新貴 李從心 S 曲線加減速算法研究 上海交通大學國家模具 CAD 工程研究中心 , 200030 ↩︎ ↩︎