1.原理
五段s曲线相较于三段s曲线而言加速度也是连续变化的,能适用于平稳性要求更高的场合。分为加加速、加减速、匀速、减加速、减减速这五段。
设除匀速段以为,其余四段的时间相等都为
Ta
T
a
,总时间为
T
T
,匀速段速度为,四个变速段斜率大小都为
A
A
,整段轨迹的总位移、加加速段位移
L1
L
1
、加减速段位移
L2
L
2
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪Ta=vsA−−√L1=16AT3aL2=56AT3aT=4Ta+L−2L1−2L2vs
{
T
a
=
v
s
A
L
1
=
1
6
A
T
a
3
L
2
=
5
6
A
T
a
3
T
=
4
T
a
+
L
−
2
L
1
−
2
L
2
v
s
则加速度分段函数为
a=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪At,(0≤t≤Ta)−A(t−2Ta),(Ta≤t≤2Ta)0,(2Ta≤t≤T−2Ta)−A[t−(T−2Ta)],(T−2Ta≤t≤T−Ta)A(t−T),(T−Ta≤t≤T)
a
=
{
A
t
,
(
0
≤
t
≤
T
a
)
−
A
(
t
−
2
T
a
)
,
(
T
a
≤
t
≤
2
T
a
)
0
,
(
2
T
a
≤
t
≤
T
−
2
T
a
)
−
A
[
t
−
(
T
−
2
T
a
)
]
,
(
T
−
2
T
a
≤
t
≤
T
−
T
a
)
A
(
t
−
T
)
,
(
T
−
T
a
≤
t
≤
T
)
对加速度积分可得
v=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪12At2,(0≤t≤Ta)−12A(t−2Ta)2+AT2a,(Ta≤t≤2Ta)vs,(2Ta≤t≤T−2Ta)−12A(t−T+2Ta)2+AT2a,(T−2Ta≤t≤T−Ta)12A(t−T)2,(T−Ta≤t≤T)
v
=
{
1
2
A
t
2
,
(
0
≤
t
≤
T
a
)
−
1
2
A
(
t
−
2
T
a
)
2
+
A
T
a
2
,
(
T
a
≤
t
≤
2
T
a
)
v
s
,
(
2
T
a
≤
t
≤
T
−
2
T
a
)
−
1
2
A
(
t
−
T
+
2
T
a
)
2
+
A
T
a
2
,
(
T
−
2
T
a
≤
t
≤
T
−
T
a
)
1
2
A
(
t
−
T
)
2
,
(
T
−
T
a
≤
t
≤
T
)
对速度积分可得到位移s的分段函数
s=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪16At3,(0≤t≤Ta)−16A(t−2Ta)3+AT2at−AT3a,(Ta≤t≤2Ta)AT2at−AT3a,(2Ta≤t≤T−2Ta)−16A(t−T+2Ta)3+AT2at−AT2a,(T−2Ta≤t≤T−Ta)16A(t−T)3−2AT3a+AT2bT,(T−Ta≤t≤T)
s
=
{
1
6
A
t
3
,
(
0
≤
t
≤
T
a
)
−
1
6
A
(
t
−
2
T
a
)
3
+
A
T
a
2
t
−
A
T
a
3
,
(
T
a
≤
t
≤
2
T
a
)
A
T
a
2
t
−
A
T
a
3
,
(
2
T
a
≤
t
≤
T
−
2
T
a
)
−
1
6
A
(
t
−
T
+
2
T
a
)
3
+
A
T
a
2
t
−
A
T
a
2
,
(
T
−
2
T
a
≤
t
≤
T
−
T
a
)
1
6
A
(
t
−
T
)
3
−
2
A
T
a
3
+
A
T
b
2
T
,
(
T
−
T
a
≤
t
≤
T
)
3.matlab代码实现
指定位置、速度、斜率
clc;
clear;
%初始条件
x_arry=[0,10,20,30];
v_arry=[2,2,2];
A_arry=[3,3,3];
weiyi=[x_arry(1)];sudu=[0];shijian=[0];timeall=0;jiasudu=[0]
for i=1:1:length(x_arry)-1;
%清空
a=[];v=[];s=[];
%计算加减速段的时间和位移
L=x_arry(i+1)-x_arry(i);
A=A_arry(i);
vs=v_arry(i);
Ta=sqrt(vs/A);
L1=A*(Ta^3)/6;
L2=A*(Ta^3)*(5/6);
%计算整段轨迹的总位移
T=4*Ta+(L-2*L1-2*L2)/vs;
for t=0:0.001:T
if t<=Ta;%加加速度阶段
ad=A*t;
vd=0.5*A*t^2;
sd=(1/6)*A*t^3;
a=[a,ad];v=[v,vd];s=[s,sd];
elseif t>Ta && t<=2*Ta;%加减速阶段
ad=-A*(t-2*Ta);
vd=-0.5*A*(t-2*Ta)^2+A*Ta^2;
sd=-(1/6)*A*(t-2*Ta)^3+A*Ta^2*t-A*Ta^3;
a=[a,ad];v=[v,vd];s=[s,sd];
elseif t>2*Ta && t<=T-2*Ta;%匀速阶段
ad=0;
vd=vs;
sd=A*Ta^2*t-A*Ta^3;
a=[a,ad];v=[v,vd];s=[s,sd];
elseif t>T-2*Ta && t<=T-Ta;%减加度阶段
ad=-A*(t-(T-2*Ta));
vd=-0.5*A*(t-T+2*Ta)^2+A*Ta^2;
sd=-(1/6)*A*(t-T+2*Ta)^3+A*Ta^2*t-A*Ta^3;
a=[a,ad];v=[v,vd];s=[s,sd];
elseif t>T-Ta && t<=T;%减减阶段
ad=A*(t-T);
vd=0.5*A*(t-T)^2;
sd=(1/6)*A*(t-T)^3-2*A*Ta^3+A*Ta^2*T;
a=[a,ad];v=[v,vd];s=[s,sd];
end
end
%时间
time=[timeall:0.001:timeall+T];
timeall=timeall+T;
%连接每一段轨迹
weiyi=[weiyi,s(2:end)+x_arry(i)];
sudu=[sudu,v(2:end)];
jiasudu=[jiasudu,a(2:end)];
shijian=[shijian,time(2:end)];
end
subplot(3,1,1),plot(shijian,weiyi,'r');xlabel('t'),ylabel('position');grid on;
subplot(3,1,2),plot(shijian,sudu,'b');xlabel('t'),ylabel('velocity');grid on;
subplot(3,1,3),plot(shijian,jiasudu,'g');xlabel('t'),ylabel('accelerate');grid on;
结果如下