T型速度时间最优曲线MATLAB代码

%% 梯形速度曲线 在vM和a约束下，到达pd,vd时间最快
clear;clc;
%T型
p0=1;   v0=0;    Vm=3;
pd=0;   vd=0;    Am=10; t0=0;
%三角型
% p0=0;   v0=1;    Vm=30;
% pd=1;   vd=2;    Am=10; t0=0;
%纯加速
% p0=0;   v0=1;     Vm=3;
% pd=1;   vd=3;    Am=3; t0=0;
%纯减速
% p0=0;   v0=3;    Vm=3;
% pd=1;   vd=0;    Am=4; t0=0;
%初末速度限幅
if abs(v0)>abs(Vm)
   v0=abs(Vm)*abs(v0)/v0;
end
if abs(vd)>abs(Vm)
   vd=abs(Vm)*abs(vd)/vd;
end
dx = pd - p0;
%速度、加速度方向确定
if dx > 0
    vM = abs(Vm);  aM=abs(Am); 
else
    vM = -abs(Vm); aM=-abs(Am); 
end
%T型速度转折点计算：T0加速时间、T1匀速时间、T2减速时间、td总时间
T0=abs((vM-v0)/aM); P1 = p0+v0*T0+0.5*aM*T0^2;
T2=abs((vM-vd)/aM); P2 = vM*T2 - 0.5*aM*T2^2;
T1=(pd-P1-P2)/vM;
td=T1+T0+T2;
i=1;
% 判断速度曲线规划属于哪一种情况
if T1>0  
    %达到最大速度，梯形
    v1=vM;v2=vM;
    a0=aM;a1=0;a2=-aM;
    fprintf('三段：\n td=%f\n',td);
else 
% 未达到最大速度，速度曲线为三角形或纯加速或纯减速
%方程组解加速时间T0 、减速时间T2, a、b、c为一元二次方程系数[由位置和速度等式联立方程]
    a=aM;
    b=2*v0;
    c=vd*(v0-vd)/aM+0.5*(v0-vd)^2/aM+p0-pd;
    delt=b^2-4*a*c;
    if delt>=0                 
        r1=(-b+sqrt(delt))/(2*a);%加速时间T0的两个解
        r2=(-b-sqrt(delt))/(2*a);
        x1 = max(r1,r2);
        x2 = min(r1,r2);
        if (x1<0 && x2<0) %加速时间T0不存在，纯减速
           T2=2*dx/(vd+v0);%dx=0.5(v0+vd)td
           v1=vd;v2=vd;
           aM =-abs(vd-v0)/T2;
           a0=aM;a1=0;a2=0;
           T0=T2;T1=0;T2=0;
           fprintf('减速：\n');
           fprintf('重新设定加速度 aM=%f\n  td=%f\n',abs(aM),T0);
        else              %存在加速阶段（加速或者加速减速）
            T0=x1; T2=(v0-vd)/aM+T0;%加速减速v0+aM*T0 = v', vd =v' -aM*T2
            T1=0;
            vp = v0 + aM*T0;
            v1 = vp;v2 = vp;
            a0 =aM;a1=0;a2=-aM;
            if T2<0      %只有加速
                T0=2*(pd-p0)/(vd+v0);
                T2=0;T1=0;
                aM = (vd-v0)/T0;
                v1 = vd; v2 = vd;
                a0=aM;a1=0;a2=0;
                fprintf('加速：\n');
                fprintf('重新设定加速度 aM=%f\n  td=%f\n',abs(aM),T0);
            end
        end
    else
        fprintf('delt<0 error');
    end
end
td = T0 +T1 +T2;  t1 =T0; t2 =T0+T1;t3=td;
v3 =vd;a3 = 0;    
p1=p0+0.5*T0*(v0+v1); p2=p1+T1*vM;p3=pd;


%细化曲线
Ap=[a0, a1, a2, a3];
Tp=[t0, t1, t2, t3];
Vp=[v0, v1, v2, v3];
Pp=[p0, p1, p2, p3];
disp([Tp;Pp;Vp;Ap].');
figure(1); clf;
subplot(3,1,1);plot(Tp,Ap,'ko'); hold on; grid on;
subplot(3,1,2);plot(Tp,Vp,'bo'); hold on; grid on;
subplot(3,1,3);plot(Tp,Pp,'ro'); hold on; grid on;

N=1000; T=linspace(Tp(1),Tp(end),N);
J=zeros(1,N); A=J+a0; V=J+v0; P=J+p0; 
for it=2:N-1
    t=T(it);
    ip=find(t<Tp); k=ip(1);
    t=t-Tp(k-1);    
    A(it) = Ap(k-1);
    V(it) = Vp(k-1) + Ap(k-1)*t;
    P(it) = Pp(k-1) + Vp(k-1)*t + 0.5*Ap(k-1)*t*t; 
end
A(end)=Ap(end);V(end)=Vp(end);P(end)=Pp(end);
subplot(3,1,1);plot(T,A,'k-'); 
subplot(3,1,2);plot(T,V,'b-'); 
subplot(3,1,3);plot(T,P,'r-');