基于目标行程的S型速度曲线规划
默认：加加速度为一个常数，当然是可正可负的；
最大加速度受限； 最大速度受限；
说明：在参数确定的条件下，根据行程的远近可以将S型速度曲线划分为三类：
七段式、六段式、四段式
MATLAB仿真代码如下：
%SDí?ù?è?ú??μ?éú3é￡?7??ê?￡?6??ê?ò??°4??ê??￡
%ê?è?2?êy?μ?÷￡?????2?êy·?±eê??ó?ó?ù?è￡?×?′ó?ó?ù?è￡?×?′ó?ù?è￡??·3ì %?òóD??è?μ?ì??tv_max>a_max^2/h;
function s_curve( h ,a_max,v_max ,s)
%UNTITLED Summary of this function goes here % Detailed explanation goes here
s1=(v_max^2*h+v_max*a_max^2)/(2*a_max*h); s2=(a_max^3)/(h^2); double s1; double s2;
double a_max_sub; double v_max_sub; double t1; double t2; double t3; double t4; double t5; double t6; double t7;
s1=(v_max^2*h+v_max*a_max^2)/(2*a_max*h); s2=(a_max^3)/(h^2);
fprintf('s1*2= %f \\n',s1*2); fprintf('s2*2= %f \\n',s2*2); if(s>=s1*2)
t1=a_max/h;
t2=(v_max-a_max^2/h)/a_max; t3=t1;
t5=t3; t6=t2; t7=t1;
t4=(s-s1*2)/v_max; disp('6_scurve:');
fprintf('t1= %f \\n',t1); fprintf('t2= %f \\n',t2); fprintf('t3= %f \\n',t3); fprintf('t4= %f \\n',t4); fprintf('t5= %f \\n',t5); fprintf('t6= %f \\n',t6); fprintf('t7= %f \\n',t7);
%t0=0:0.001:(t1+t2+t3+t4+t5+t6+t7);
plot(0,0); hold on;
xlabel('t(s)'); ylabel('v(m/s)');
for t=0:0.001:(t1+t2+t3+t4+t5+t6+t7+1) if(t<=t1)
v=h*t^2/2; plot(t,v)
elseif(t>t1 && t<=(t1+t2) ) v=h*t1^2/2+a_max*(t-t1); plot(t,v);
elseif(t>(t1+t2) && t<=(t1+t2+t3) ) v=v_max-h*(t1+t2+t3-t)^2/2; plot(t,v);
elseif(t>(t1+t2+t3) && t<=(t1+t2+t3+t4) ) v=v_max; plot(t,v);
elseif(t>(t1+t2+t3+t4) && t<=(t1+t2+t3+t4+t5) ) v=v_max-h*(t-(t1+t2+t3+t4) )^2/2; plot(t,v);
elseif(t>(t1+t2+t3+t4+t5) && t<=(t1+t2+t3+t4+t5+t6) ) v=v_max-h*t5^2/2-a_max*(t-(t1+t2+t3+t4+t5)); plot(t,v);
elseif(t>(t1+t2+t3+t4+t5+t6) && t<=(t1+t2+t3+t4+t5+t6+t7) )
v=h*((t1+t2+t3+t4+t5+t6+t7)-t)^2/2; plot(t,v); else v=0;
plot(t,v); end end
elseif(s>=s2*2 && s v_max_sub=(-a_max^2+sqrt(a_max^4+4*h^2*s*a_max))/(2*h); t1=a_max/h; t2=(v_max_sub-a_max^2/h)/a_max; t3=t1; t5=t3; t6=t2; t7=t1; t4=0; disp('6_scurve:'); fprintf('a_max= %f\\n',a_max); fprintf('v_max_sub= %f \\n',v_max_sub); fprintf('t1= %f \\n',t1); fprintf('t2= %f \\n',t2); fprintf('t3= %f \\n',t3); fprintf('t4= %f \\n',t4); fprintf('t5= %f \\n',t5); fprintf('t6= %f \\n',t6); fprintf('t7= %f \\n',t7); %t0=0:0.001:(t1+t2+t3+t4+t5+t6+t7); plot(0,0); hold on; xlabel('t(s)'); ylabel('v(m/s)'); for t=0:0.001:(t1+t2+t3+t4+t5+t6+t7+1) if(t<=t1) v=h*t^2/2; plot(t,v) elseif(t>t1 && t<=(t1+t2) ) v=h*t1^2/2+a_max*(t-t1); plot(t,v); elseif(t>(t1+t2) && t<=(t1+t2+t3) ) v=v_max_sub-h*(t1+t2+t3-t)^2/2; plot(t,v); %elseif(t>(t1+t2+t3) && t<=(t1+t2+t3+t4) ) % v=v_max; % plot(t,v); elseif(t>(t1+t2+t3+t4) && t<=(t1+t2+t3+t4+t5) ) v=v_max_sub-h*(t-(t1+t2+t3+t4) )^2/2; plot(t,v); elseif(t>(t1+t2+t3+t4+t5) && t<=(t1+t2+t3+t4+t5+t6) ) v=v_max_sub-h*t5^2/2-a_max*(t-(t1+t2+t3+t4+t5)); plot(t,v); elseif(t>(t1+t2+t3+t4+t5+t6) && t<=(t1+t2+t3+t4+t5+t6+t7) ) v=h*((t1+t2+t3+t4+t5+t6+t7)-t)^2/2; plot(t,v); else v=0; plot(t,v); end end else if(s a_max_sub=(s*h^2/2)^(1/3) t1=a_max_sub/h; t2= 0; t3=t1; t5=t3; t6=0; t7=t1; t4=0; v_max_sub=h*t1^2; disp('4_scurve:'); fprintf('a_max_sub= %f \\n',a_max_sub); fprintf('v_max_sub= %f \\n',v_max_sub); fprintf('t1= %f \\n',t1); fprintf('t2= %f \\n',t2); fprintf('t3= %f \\n',t3); fprintf('t4= %f \\n',t4); fprintf('t5= %f \\n',t5); fprintf('t6= %f \\n',t6); fprintf('t7= %f \\n',t7); %t0=0:0.001:(t1+t2+t3+t4+t5+t6+t7); plot(0,0); hold on; xlabel('t(s)'); ylabel('v(m/s)'); for t=0:0.001:(t1+t2+t3+t4+t5+t6+t7+1) if(t<=t1) v=h*t^2/2; plot(t,v) %elseif(t>t1 && t<=(t1+t2) ) % v=h*t1^2/2+a_max*(t-t1); % plot(t,v); elseif(t>(t1+t2) && t<=(t1+t2+t3) ) v=v_max_sub-h*(t1+t2+t3-t)^2/2; plot(t,v); %elseif(t>(t1+t2+t3) && t<=(t1+t2+t3+t4) ) % v=v_max; % plot(t,v); elseif(t>(t1+t2+t3+t4) && t<=(t1+t2+t3+t4+t5) ) v=v_max_sub-h*(t-(t1+t2+t3+t4) )^2/2; plot(t,v); %elseif(t>(t1+t2+t3+t4+t5) && t<=(t1+t2+t3+t4+t5+t6) ) % v=v_max_sub-h*t5^2/2-a_max*(t-(t1+t2+t3+t4+t5)); % plot(t,v); elseif(t>(t1+t2+t3+t4+t5+t6) && t<=(t1+t2+t3+t4+t5+t6+t7) ) v=h*((t1+t2+t3+t4+t5+t6+t7)-t)^2/2; plot(t,v); else v=0; plot(t,v); end end end end 七段式仿真结果： 六段式： 百度搜索“77cn”或“免费范文网”即可找到本站免费阅读全部范文。收藏本站方便下次阅读，免费范文网，提供经典小说综合文库基于目标行程的S型速度曲线规划在线全文阅读。
相关推荐：