一种梯形速度规划算法代码与仿真

下面介绍的时一种较为常用的梯形速度规划，梯形速度规划因为存在理论的匀速段而且曲线较为简单受控，往往使用起来较为方便。但该速度规划的加速度没有连续，相对于S型速度规划来说冲击力较大，因此在高精度、少冲击、高要求的速度规划里面用的比较少。但此处仅作为简单介绍梯形曲线如何编写和仿真。

因此，时间取整的问题本处省略了，多轴同步本处也省略了。这些问题其实也很简单，由读者思考。

function trapezoid( angle, maxSpeed, maxAcc,ts )
%UNTITLED 此处显示有关此函数的摘要
%   此处显示详细说明
% anlge 转动的角度
% speed 转动的速度
% acc 转动的加速度
% ts 插补周期
    Motor.a(1)=maxAcc;
    Motor.a(2)=0;
    Motor.a(3)=-maxAcc;
    Motor.a(4)=-maxAcc;

    Motor.v(1)=0;
    Motor.v(4)=0;
    Motor.pos(1)=0;
    Motor.t(1)=0;
    Motor.t(2)= maxSpeed / maxAcc;
    if(angle>maxSpeed*Motor.t(2))
        %有匀速段
        Motor.t(3)=(angle-maxSpeed*Motor.t(2))/maxSpeed+Motor.t(2);
        Motor.t(4)=Motor.t(3)+Motor.t(2);
        Motor.v(2)=maxSpeed;
        Motor.v(3)=maxSpeed;
        Motor.pos(2)=0.5*maxSpeed*Motor.t(2);
        Motor.pos(3)=Motor.pos(2)+maxSpeed*(Motor.t(3)-Motor.t(2));
        Motor.pos(4)=2*Motor.pos(2)+maxSpeed*(Motor.t(3)-Motor.t(2));   
    else
        %无匀速段
        Motor.t(2)=sqrt(angle/maxAcc);
        Motor.t(3)=Motor.t(2);
        Motor.t(4)=2*Motor.t(3);
        Motor.v(2)=maxAcc*Motor.t(2);
        Motor.v(3)=Motor.v(2);
        Motor.pos(2)=0.5* Motor.v(2)*Motor.t(2);
        Motor.pos(3)=Motor.pos(2);
        Motor.pos(4)=2*Motor.pos(2);
    end
    
    for time=0:ts:Motor.t(4)
        for i=1:4
                if(time <= Motor.t(i+1))
                    if(time>=Motor.t(i)&&time<=(Motor.t(i+1)+ts/2))
                        tt=time-Motor.t(i);
                        acc=Motor.a(i);
                        vel=Motor.v(i)+Motor.a(i)*tt;
                        pos=Motor.pos(i)+Motor.v(i)*tt+0.5*Motor.a(i)*tt*tt;
                        subplot(3,1,1);
                        scatter(time,acc,'.')
                        hold on;
                        subplot(3,1,2);
                        scatter(time,vel,'.');
                        hold on;
                        subplot(3,1,3);
                        scatter(time,pos,'.');
                        hold on;
                        break;
                    end
                end
        end
    end

end

图一：执行函数  trapezoid(70,300,3000,0.001)结果

图二：执行函数trapezoid(10,300,3000,0.001)结果。

分析，上述两个仿真实例，正好验证了if的两个分支。