✅作者简介：热爱科研的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
⛄ 运行结果
⛄ 参考文献
