基于目标行程的S型速度曲线规划

基于目标行程的S型速度曲线规划

默认：加加速度为一个常数，当然是可正可负的；

最大加速度受限；

最大速度受限；

说明：在参数确定的条件下，根据行程的远近可以将S型速度曲线划分为三类：七段式、六段式、四段式

MATLAB仿真代码如下：

%SÐÍËÙ¶ÈÇúÏßµÄÉú³É£¬7¶Îʽ£¬6¶ÎʽÒÔ¼°4¶Îʽ¡£

%ÊäÈë²ÎÊý˵Ã÷£¬Ëĸö²ÎÊý·Ö±ðÊǼӼÓËٶȣ¬×î´ó¼ÓËٶȣ¬×î´óËٶȣ¬Â·³Ì%ÇÒÓÐĬÈϵÄÌõ¼þv_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)