基于Matlab模拟5段S形多轴同步规划

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

​5段S型加减速算法（5segments S-curve velocity profile），是由T型加速度曲线(Trapezoidal motion profile)演化而来(此外，还有多项式规划（Polynomial Profile），如三次多项式、五次多项式、七次多项式等。虽然五次以上多项式曲线可以让速度加速度都连续变化，但速度曲线没有匀速运动时间，不利于对加减速过程进行优化。)，通过在速度曲线的加速段与匀速段之间增加抛物线(parabolic)或者三角函数(triangular)过度，是的加速度平滑(smooth)过渡，从而解决了加区间两端速度不连续(discontinuous)的问题。

⛄ 部分代码

function [T,VMAX,AMAX] = Pretraj(ini_theta,tar_theta,v,a)本程序旨在同步规划之前分别对每个轴单独进行轨迹规划

%T,VMAX,AMAX代表每个轴的规划时间、运行最大速度、运行最大加速度

[~,num] = size(ini_theta);  % 判段轴的个数

T = zeros(1,num);

VMAX = zeros(1,num);

AMAX = zeros(1,num);

% 每个轴依次规划

for i = 1:num

    %----------------------清除每个轴的p，pd，pdd数据----------------------%

    if i>1

        clear p pd pdd;

    end

    %--------------------------计算规划所需的参数--------------------------%

    p0 = ini_theta(1,i);

    p1 = tar_theta(1,i);

    vs = v(1,i);

    A  = a(1,i);

    h = p1 - p0;

    %-------------------------判断是否存在h=0的情况-----------------------%

    if  h ==  0

        T(1,i) = 0; VMAX(1,i) = 0; AMAX(1,i) = 0;

    else

        %计算加减速段的时间和位移

        Ta=sqrt(vs/A);

        L1=A*(Ta^3)/6;

        L2=A*(Ta^3)*(5/6);

        %计算整段轨迹的总位移

        Tp=4*Ta+(h-2*L1-2*L2)/vs;

        for t=0:0.01:Tp

            if t<=Ta    %加加速度阶段

                ad=A*t;

                vd=0.5*A*t^2;

                sd=(1/6)*A*t^3;

            elseif t>Ta && t<=2*Ta    %减加速阶段

                ad=-A*(t-2*Ta);

                vd=-0.5*A*(t-2*Ta)^2+A*Ta^2;

                sd=-(1/6)*A*(t-2*Ta)^3+A*Ta^2*t-A*Ta^3;

            elseif t>2*Ta && t<=Tp-2*Ta    %匀速阶段

                ad=0;

                vd=vs;

                sd=A*Ta^2*t-A*Ta^3;

            elseif t>Tp-2*Ta && t<=Tp-Ta   %加减速阶段

                ad=-A*(t-(Tp-2*Ta));

                vd=-0.5*A*(t-Tp+2*Ta)^2+A*Ta^2;

                sd=-(1/6)*A*(t-Tp+2*Ta)^3+A*Ta^2*t-A*Ta^3;

            elseif t>Tp-Ta && t<=Tp     %减加速阶段

                ad=A*(t-Tp);

                vd=0.5*A*(t-Tp)^2;

                sd=(1/6)*A*(t-Tp)^3-2*A*Ta^3+A*Ta^2*Tp;

            end

        end

        T(1,i) = Tp; VMAX(1,i) = vs; AMAX(1,i) = A;

    end

end

end

⛄ 运行结果

⛄ 参考文献

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料