S速度曲线规划算法总结
“之前有介绍过T形曲线，具体可以参考《一文教你快速搞懂速度曲线规划之T形曲线》，本文将在原先的基础上进行进一步扩展，另外由于介绍速度曲线的论文较多，本文会在具体引用的地方给出原文出处；先对比一下两者的差别；
1 前言
S形加减速的最重要特征是该算法的加速度/减速度曲线的形状如字母 S。S形加减速的速度曲线平滑 ,从而能够减少对控制过程中的冲击，并使插补过程具有柔性 [^1]。由于T形曲线在加速到匀速的切换过程中，实际中存在较大过冲，因此这里对比一下T曲线和7段S曲线的实际过程；
T形：加速 -> 匀速 -> 减速 S形：加加速() -> 匀加速() -> 减加速()-> 匀速()-> 加减速()-> 匀减速()-> 减减速()
上文在加速这块的文字描述可能读起来起来有点绕，下面看图：
2 理论分析
由于S曲线在加减速的过程中，其加速度是变化的，因此这里引入了新的一个变量 ，即加加速度。
因此对应上图的7段S速度曲线中，规定最大加速为，最小加速度为，则加速度的关系；
加加速()：逐渐增大； 此时 匀加速()：达到最大； 此时 减加速()：逐渐减小； 此时 匀速()：不变化； 此时 加减速()： 逐渐增大； 此时 匀减速()： 达到最大； 此时 减减速()： 逐渐减小； 此时
“为加速度的绝对值；其中
所以通常需要确定三个最基本的系统参数 :系统最大速度 ，最大加速度a_{max} ，加加速度，就可以可确定整个运行过程[^2] ；
最大速度：反映了系统的最大运行能力 ； 最大加速度：反映了系统的最大加减速能力 ； 加加速度：反映了系统的柔性； 柔性越大，过冲越大，运行时间越短； 柔性越小，过冲越小，运行时间越长；
2.1 加速度时间关系方程
整个加速度变化的过程具体如下图所示；
再次强调一下 和 的关系，另外这里再引入变量 ，
比如，当前时刻 ,即 位于区间 ，则如果将 作为初始点，则 为 相对于时刻的时间，则有：
下面可以得到加速度与时间的关系函数，具体如下：
根据 ① 式，将 代入 ② 式可以得到：
上式中 ；
2.2 速度时间关系方程
速度和加速度满足 ；加加速度和速度的关系满足：
结合加速度时间关系并结合② 式可以得到速度曲线关系，具体关系如下图所示；
进一步简化可以得到：
2.3 位移时间关系方程
位移 和加加速度 直接满足关系如下：
简单推导
因此可以得到：
“积分忘的差不多了，回去再复习一下；
最终位移的方程如下所示；
3 程序实现的思路
正如前面所提到的，S曲线规划需要确定三个最基本的系统参数 ：系统最大速度 ，最大加速度a_{max} ，加加速度，这样就可以确定这个运行过程。这里有一个隐性的条件，就是在运行的过程中可以达到最大速度，这样才是完整的7段S曲线，另外这里还有一些中间参数：
，因此有 ； 加加速度 ； ； ，用户给定整个运行过程所需要的时间；
但是通常实际过程中关心，，；
3.1 推导
理想状态假设存在 和，则推导过程如下：
因此可以得到：
简化之后得到：
根据②式可知：
最终得到：
“
下面可以根据位移时间关系方程进行离散化的程序编写。
假设可以到达最大速度，且用户给定了整个过程运行时间，则 的推导如下：
简化上式可以得到：
根据 代入上式可得：
3.2 的推导
这时候还剩下需要计算，通过已量 可以推导出来；首先位移之间满足关系如下：
其中加速区长度为 ；其中减速区长度为 ；
具体推导；[^2] 前面提到过，，因此在=0的时候，则
这里简单推导一下：
根据④，⑤最终简化得到：
“：为运行的总时间：为运行的总路程
详细推导过程如下：
因为：
因为：
所以，简化得到：
所以可以得到：
因为：
将其代入可以得到：
简化得到最终结果：
4 matlab 程序
matlab程序亲测可以运行，做了简单的修改，
因为这里直接给定了整个运行过程的时间，所以需要在SCurvePara函数中求出加加速度 的值，路程为 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('加速度曲线');
看到最终仿真结果和预期相同；
最后再看一下T形和S形速度曲线规划的效果对比：
5 总结
本文只对7段的S曲线规划做了详细的推导和介绍，matlab中的程序对于4段和5段都有做实现，很多是在理想情况下进行推导的，初始速度默认为0，终止速度也为0，并且假设加减速区域相互对称。最终运行结果符合预期效果。
“文中难免有错误和纰漏之处，请大佬们不吝赐教 创作不易，如果本文帮到了您；
请帮忙点个赞 👍👍👍；
请帮忙点个赞 👍👍👍；
请帮忙点个赞 👍👍👍；
6 参考
[^1]:陈友东 魏洪兴 王琦魁.数控系统的直线和 S 形加减速离散算法[D].北京:中国机械工程,2010.
[^2]:郭新贵 李从心 S 曲线加减速算法研究 上海交通大学国家模具 CAD 工程研究中心 , 200030
原创不易，欢迎转发、留言、点赞、分享给你的朋友，感谢您的支持！
长按识别二维码关注获取更多内容