syms ve vs vmax amax jmax ld t1 t2 t3 t4 t5 t6 t7
n=1000;
%%以下三个参数为最大限制参数
vmax=1000;
amax=10;
jmax=20;
vs=708;%%初始速度
ve=76;%%截止速度
ld=500;%%目标位置
%%%%%%%%%%%%%%(1)
plotsubsection=zeros(1,7);
if vs>vmax||ve>vmax||ve<0||vs<0
errordlg('vs>vmax or ve>vmax or ld=0 or vs<0 or ve<0','err');
return;
end
%%%%%%%%%%%%%(2)
%%速度修正的原则是满足位移简体下还是以减小速度为原则,所以提示不是最优的方法的情况下,采用的方法都是这个
if vs==vmax&&ve==vmax
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase1( ld,vs,ve,vmax,amax,jmax,n );
elseif vs==vmax && 0<=ve && ve<vmax %%%%只有减速段且ve<vs
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase2( ld,vs,ve,vmax,amax,jmax,n );
elseif ve==vmax && 0<=vs && vs<vmax %%%%%%只有加速段且ve>vs
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase3( ld,vs,ve,vmax,amax,jmax,n );
elseif (0<=ve && ve<=vmax)&&(0<=vs && vs<=vmax)
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase4( ld,vs,ve,vmax,amax,jmax,n );
end
Fun_Painter(plotsubsection,St,Vt,At,t1,t2,t3,t4,t5,t6,t7);
function [ St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7 ] = FunScase1( ld,vs,ve,vmax,amax,jmax,n )
%UNTITLED2 Summary of this function goes here
% Detailed explanation goes here
t1=0;
t2=0;
t3=0;
t4=0;
t5=0;
t6=0;
t7=0;
vs=vmax;
ve=vmax;
t=0;
T4=abs(ld)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
plotsubsection=[0 0 0 1 0 0 0];
end
function [ St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7] = FunScase2( ld,vs,ve,vmax,amax,jmax,n )
t1=0;
t2=0;
t3=0;
t4=0;
t5=0;
t6=0;
t7=0;
vtemp=amax^2/jmax;
tm=amax/jmax;
%%%%%%%%%%得分情况1
if (vs-ve)>=vtemp %%%减速度可以达到最大值
tp=(vs-ve-vtemp)/(jmax*tm);
s=((ve+vs)*tp)/2+(ve+vs)*tm;
if ld>=s %%可分为匀速,加减速,匀减速,减减速
t=0;
T4=abs(ld-s)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,Vt{4}(n),t5-t,St{4}(n),0);
t=t+tm;
t6=linspace(t,t+tp,n);
[St{6},Vt{6},At{6}]=SProfileT6( tm,-jmax,Vt{5}(n),t6-t,St{5}(n));
t=t+tp;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),jmax,Vt{6}(n),t7-t,St{6}(n));
t=t+tm;
plotsubsection=[0 0 0 1 1 1 1];
else
vs=ve+vtemp;
s=(ve+vs)*tm;%%作为匀减速段存在的依据,这个是不存在匀减速段的位移临界值
if s<=ld %%这种情况是存在匀减速段的时候计算vs量
fun=@(vs)(-ld+((ve+vs)*((vs-ve-vtemp)/(jmax*tm)))/2+(ve+vs)*tm);
vs=fsolve(fun,1);
tp=(vs-ve-vtemp)/(jmax*tm);
t=0;
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,vs,t5-t,0,0);
t=t+tm;
t6=linspace(t,t+tp,n);
[St{6},Vt{6},At{6}]=SProfileT6( tm,-jmax,Vt{5}(n),t6-t,St{5}(n));
t=t+tp;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),jmax,Vt{6}(n),t7-t,St{6}(n));
t=t+tm;
plotsubsection=[0 0 0 0 1 1 1];
else
%%此时达到不了最大的加速度值,得重新开始算时间tm
fun=@(tm)(-ld+(2*ve+jmax*tm.^2)*tm);
tm=fsolve(fun,1);
t=0;
vs=ve+jmax*tm.^2
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,vs,t5-t,0,0);
t=t+tm;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{5}(n),jmax,Vt{5}(n),t7-t,St{5}(n));
t=t+tm;
plotsubsection=[0 0 0 0 1 0 1];
end
end
else
fun=@(tm)(vs-ve)-jmax*tm.^2;%%%减速度达到不了,按实际速度的位移去计算即可,作为判断条件
tm=fsolve(fun,1);
s=(ve+vs)*tm;
if ld>=s%%这种情况下,由于速度回超限,所以引入匀速进行处理
t=0;
T4=abs(ld-s)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,Vt{4}(n),t5-t,St{4}(n),0);
t=t+tm;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{5}(n),jmax,Vt{5}(n),t7-t,St{5}(n));
t=t+tm;
plotsubsection=[0 0 0 1 1 0 1];
else%%%%这种情况下由于位移小于实际速度变化tm对应的位移量了,只能修正vs,根据位移量去修正vs
fun=@(tm)(-ld+(2*ve+jmax*tm.^2)*tm);
tm=fsolve(fun,1);
t=0;
vs=ve+jmax*tm.^2
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,vs,t5-t,0,0);
t=t+tm;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{5}(n),jmax,Vt{5}(n),t7-t,St{5}(n));
t=t+tm;
plotsubsection=[0 0 0 0 1 0 1];
end
end
end
function [ St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7 ] = FunScase3( ld,vs,ve,vmax,amax,jmax,n )
%UNTITLED Summary of this function goes here
% Detailed explanation goes here
t1=0;
t2=0;
t3=0;
t4=0;
t5=0;
t6=0;
t7=0;
vtemp=amax^2/jmax;
tm=amax/jmax;
%%%%%%%%%%得分情况1
if (ve-vs)>=vtemp %%%加速度可以达到最大值
tp=(ve-vs-vtemp)/(jmax*tm);
s=((vs+ve)*tp)/2+(vs+ve)*tm;
if ld>=s %%可分为匀速,加减速,匀减速,减减速
t=0;
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t2=linspace(t,t+tp,n);
[St{2},Vt{2},At{2}]=SProfileT2( tm,jmax,Vt{1}(n),t2-t,St{1}(n));
t=t+tp;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),-jmax,Vt{2}(n),t3-t,St{2}(n));
t=t+tm;
T4=abs(ld-s)/ve;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),ve,t4-t);
t=t+T4;
plotsubsection=[1 1 1 1 0 0 0];
else
vt=ve+vtemp;
s=(ve+vt)*tm;
if s<=ld
fun=@(ve)(-ld+((vs+ve)*((ve-vs-vtemp)/(jmax*tm)))/2+(vs+ve)*tm);
ve=fsolve(fun,5);
tp=(ve-vs-vtemp)/(jmax*tm);
t=0;
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t2=linspace(t,t+tp,n);
[St{2},Vt{2},At{2}]=SProfileT2( tm,jmax,Vt{1}(n),t2-t,St{1}(n));
t=t+tp;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),-jmax,Vt{2}(n),t3-t,St{2}(n));
t=t+tm;
plotsubsection=[1 1 1 0 0 0 0];
else
%%此时达到不了最大的加速度值,得重新开始算时间tm
%fun=@(tm)(-ld+(2*(ve-jmax*tm.^2)+jmax*tm.^2)*tm);
%tm=fsolve(fun,1);
fun=@(tm)(-ld+(2*vs+jmax*tm.^2)*tm);
tm=fsolve(fun,1);
t=0;
ve=(vs+jmax*tm.^2);
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-jmax,Vt{1}(n),t3-t,St{1}(n));
t=t+tm;
plotsubsection=[1 0 1 0 0 0 0];
end
end
else
fun=@(tm)(ve-vs)-jmax*tm.^2;%%%加速度达到不了,按实际速度的位移去计算即可,作为判断条件
tm=fsolve(fun,1);
s=(ve+vs)*tm;
if ld>=s%%这种情况下,由于速度回超限,所以引入匀速进行处理
t=0;
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-jmax,Vt{1}(n),t3-t,St{1}(n));
t=t+tm;
T4=abs(ld-s)/ve;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),ve,t4-t);
t=t+T4;
plotsubsection=[1 0 1 1 0 0 0];
else%%%%这种情况下由于位移小于实际速度变化tm对应的位移量了,只能修正ve,根据位移量去修正ve
fun=@(tm)(-ld+(2*vs+jmax*tm.^2)*tm);
tm=fsolve(fun,1);
ve=vs+jmax*tm.^2;
t=0;
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-jmax,Vt{1}(n),t3-t,St{1}(n));
t=t+tm;
plotsubsection=[1 0 1 0 0 0 0];
end
end
end
function [ St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7 ] = FunScase4( ld,vs,ve,vmax,amax,jmax,n )
%UNTITLED2 Summary of this function goes here
% Detailed explanation goes here
t1=0;
t2=0;
t3=0;
t4=0;
t5=0;
t6=0;
t7=0;
vtemp=amax^2/jmax;
tm=amax/jmax;
%%%%%%%%%%得分情况1
if (vmax-vs)>=vtemp&&(vmax-ve)>=vtemp %%%减速度和加速度都可以达到最大值
tp1=(vmax-vs-vtemp)/(jmax*tm);
s1=(vmax+vs)*tm+((vmax+vs)*tp1)/2;
tp2=(vmax-ve-vtemp)/(jmax*tm);
s2=((ve+vmax)*tp2)/2+(ve+vmax)*tm;
s=s1+s2;
if ld>=s
t=0;
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t2=linspace(t,t+tp1,n);
[St{2},Vt{2},At{2}]=SProfileT2( tm,jmax,Vt{1}(n),t2-t,St{1}(n));
t=t+tp1;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),-jmax,Vt{2}(n),t3-t,St{2}(n));
t=t+tm;
T4=abs(ld-s)/vmax;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),Vt{3}(n),t4-t);
t=t+T4;
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,Vt{4}(n),t5-t,St{4}(n),0);
t=t+tm;
t6=linspace(t,t+tp2,n);
[St{6},Vt{6},At{6}]=SProfileT6( tm,-jmax,Vt{5}(n),t6-t,St{5}(n));
t=t+tp2;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),jmax,Vt{6}(n),t7-t,St{6}(n));
t=t+tm;
plotsubsection=[1 1 1 1 1 1 1];
else%%这个不是最优的,不过处理简单
if vs>ve%%a
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase2( ld,vs,ve,vmax,amax,jmax,n );
elseif vs==ve
t=0;
T4=abs(ld)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
plotsubsection=[0 0 0 1 0 0 0];
elseif vs<ve
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase3( ld,vs,ve,vmax,amax,jmax,n );
end
end
elseif (vmax-vs)>=vtemp&&(vmax-ve)<vtemp
tp1=(vmax-vs-vtemp)/(jmax*tm);
s1=(vmax+vs)*tm+((vmax+vs)*tp1)/2;
fun=@(tp2)(vmax-ve)-jmax*tp2.^2;%%%减速度达到不了,按实际速度的位移去计算即可,作为判断条件
tp2=fsolve(fun,1);
s2=(ve+vmax)*tp2;
s=s1+s2;
if ld>=s
t=0;
t1=linspace(t,t+tm,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tm;
t2=linspace(t,t+tp1,n);
[St{2},Vt{2},At{2}]=SProfileT2( tm,jmax,Vt{1}(n),t2-t,St{1}(n));
t=t+tp1;
t3=linspace(t,t+tm,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{2}(n),-jmax,Vt{2}(n),t3-t,St{2}(n));
t=t+tm;
T4=abs(ld-s)/vmax;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),Vt{3}(n),t4-t);
t=t+T4;
t5=linspace(t,t+tp2,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,Vt{4}(n),t5-t,St{4}(n),0);
t=t+tp2;
t7=linspace(t,t+tp2,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{5}(n),jmax,Vt{5}(n),t7-t,St{5}(n));
t=t+tp2;
plotsubsection=[1 1 1 1 1 0 1];
else%%%这个不是最优的,不过处理简单
if vs>ve%%a
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase2( ld,vs,ve,vmax,amax,jmax,n );
elseif vs==ve
t=0;
T4=abs(ld)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
plotsubsection=[0 0 0 1 0 0 0];
elseif vs<ve
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase3( ld,vs,ve,vmax,amax,jmax,n );
end
end
elseif (vmax-vs)<vtemp&&(vmax-ve)>=vtemp
fun=@(tp1)(vmax-vs)-jmax*tp1.^2;
tp1=fsolve(fun,1);
s1=(vs+vmax)*tp1;
tp2=(vmax-ve-vtemp)/(jmax*tm);
s2=((ve+vmax)*tp2)/2+(ve+vmax)*tm;
s=s1+s2;
if ld>=s
t=0;
t1=linspace(t,t+tp1,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tp1;
t3=linspace(t,t+tp1,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-jmax,Vt{1}(n),t3-t,St{1}(n));
t=t+tp1;
T4=abs(ld-s)/vmax;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),Vt{3}(n),t4-t);
t=t+T4;
t5=linspace(t,t+tm,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,Vt{4}(n),t5-t,St{4}(n),0);
t=t+tm;
t6=linspace(t,t+tp2,n);
[St{6},Vt{6},At{6}]=SProfileT6( tm,-jmax,Vt{5}(n),t6-t,St{5}(n));
t=t+tp2;
t7=linspace(t,t+tm,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{6}(n),jmax,Vt{6}(n),t7-t,St{6}(n));
t=t+tm;
plotsubsection=[1 0 1 1 1 1 1];
else%%%这个不是最优的,不过处理简单
if vs>ve%%a
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase2( ld,vs,ve,vmax,amax,jmax,n );
elseif vs==ve
t=0;
T4=abs(ld)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
plotsubsection=[0 0 0 1 0 0 0];
elseif vs<ve
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase3( ld,vs,ve,vmax,amax,jmax,n );
end
end
elseif (vmax-vs)<vtemp&&(vmax-ve)<vtemp
fun=@(tp1)(vmax-vs)-jmax*tp1.^2;
tp1=fsolve(fun,1);
s1=(vs+vmax)*tp1;
fun=@(tp2)(vmax-ve)-jmax*tp2.^2;%%%减速度达到不了,按实际速度的位移去计算即可,作为判断条件
tp2=fsolve(fun,1);
s2=(ve+vmax)*tp2;
s=s1+s2;
if ld>s
t=0;
t1=linspace(t,t+tp1,n);
[St{1},Vt{1},At{1}]=SProfileT1(0,jmax,vs,t1-t);
t=t+tp1;
t3=linspace(t,t+tp1,n);
[St{3},Vt{3},At{3}]=SProfileT32(At{1}(n),-jmax,Vt{1}(n),t3-t,St{1}(n));
t=t+tp1;
T4=abs(ld-s)/vmax;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(St{3}(n),Vt{3}(n),t4-t);
t=t+T4;
t5=linspace(t,t+tp2,n);
[St{5},Vt{5},At{5}]=SProfileT5(-jmax,Vt{4}(n),t5-t,St{4}(n),0);
t=t+tp2;
t7=linspace(t,t+tp2,n);
[St{7},Vt{7},At{7}]=SProfileT72(At{5}(n),jmax,Vt{5}(n),t7-t,St{5}(n));
t=t+tp2;
plotsubsection=[1 0 1 1 1 0 1];
else%%%这个不是最优的,不过处理简单
if vs>ve%%a
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase2( ld,vs,ve,vmax,amax,jmax,n );
elseif vs==ve
t=0;
T4=abs(ld)/vs;
t4=linspace(t,t+T4,n);
[St{4},Vt{4},At{4}]=SProfileT4(0,vs,t4-t);
t=t+T4;
plotsubsection=[0 0 0 1 0 0 0];
elseif vs<ve
[St,Vt,At,plotsubsection,t1,t2,t3,t4,t5,t6,t7]=FunScase3( ld,vs,ve,vmax,amax,jmax,n );
end
end
end
end
function [ output_args ] = Fun_Painter(plotsubsection,St,Vt,At,t1,t2,t3,t4,t5,t6,t7)
subsectionSum=0;
for i=1:size(plotsubsection,2)
subsectionSum=plotsubsection(i)+subsectionSum;
end
figure('name',strcat(num2str(subsectionSum),'段位移曲线'));
hold on
if plotsubsection(1)==1
plot(t1,St{1});
end
if plotsubsection(2)==1
plot(t2,St{2});
end
if plotsubsection(3)==1
plot(t3,St{3});
end
if plotsubsection(4)==1
plot(t4,St{4},'r');
end
if plotsubsection(5)==1
plot(t5,St{5},'b');
end
if plotsubsection(6)==1
plot(t6,St{6},'y');
end
if plotsubsection(7)==1
plot(t7,St{7},'r');
end
hold off
figure('name',strcat(num2str(subsectionSum),'段速度曲线'));
hold on
if plotsubsection(1)==1
plot(t1,Vt{1});
end
if plotsubsection(2)==1
plot(t2,Vt{2});
end
if plotsubsection(3)==1
plot(t3,Vt{3});
end
if plotsubsection(4)==1
plot(t4,Vt{4});
end
if plotsubsection(5)==1
plot(t5,Vt{5});
end
if plotsubsection(6)==1
plot(t6,Vt{6});
end
if plotsubsection(7)==1
plot(t7,Vt{7});
end
hold off
figure('name',strcat(num2str(subsectionSum),'段加速度曲线'));
hold on
if plotsubsection(1)==1
plot(t1,At{1});
end
if plotsubsection(2)==1
plot(t2,At{2});
end
if plotsubsection(3)==1
plot(t3,At{3});
end
if plotsubsection(4)==1
plot(t4,At{4});
end
if plotsubsection(5)==1
plot(t5,At{5});
end
if plotsubsection(6)==1
plot(t6,At{6});
end
if plotsubsection(7)==1
plot(t7,At{7});
end
end