|
文章目录
写在前面学习代码都记录在[个人github](https://github.com/xuuyann/RobotLearningCode)上，欢迎关注~
梯形加减速（与抛物线拟合的线性函数）S型曲线下面是S曲线的算法步骤：S曲线流程图Matlab实现程序
参考
写在前面
学习代码都记录在个人github上，欢迎关注~
前面的几种多项式规划方法机器人学回炉重造（5）：关节空间规划方法——多项式轨迹（三次多项式、五次多项式、抛物线轨迹）都存在没有最大速度和最大加速度限制的问题，当机械系统存在速度和加速度限制范围时，上述方法就不太适合。下面的梯形加减速和S型曲线就不会存在这种问题。
梯形加减速（与抛物线拟合的线性函数）
 梯形曲线在电机控制中经常被用到，因为其速度曲线形状为梯形，因此被称为梯形速度曲线。 一般情况下，用户给定起始速度、终止速度、加速度、减速度、最大速度以及起始时刻和终止时刻的位移参数，需要计算出加速段、匀速段以及减速段对应的时间Ta、Tv、Td，然后再根据位移、速度以及加速度公式计算轨迹。 （1）首先需要计算用户给定参数下所能达到的最大速度。要使能够达到的速度最大，则只有加速段和减速段，没有匀速段。此时h = p1 - p0，有如下公式  根据上述公式，可得到给定参数下所能达到的最大速度：  （2）比较理论最大速度vf与用户限定最大速度vmax之间的大小。当vfvmax时，说明给定的参数可以达到用户限定的最大速度，但是又不能超过限定的最大速度，因此匀速段的最大速度为vv = vmax。 （3）计算加速段、匀速段以及减速段的时间和位移，计算公式如下：  （4）计算各时刻下的位移、速度以及加速度，公式如下： 
%% 与抛物线拟合的线性函数（线性轨迹）
% 梯形加减速，可限制最大速度和最大加速度
% 用户给定起始速度、终止速度、加速度、减速度、最大速度及位移参数
% 该算法需要计算出加速段、匀速段以及减速段对应的时间Ta Tv Td
% t0 = 0, p0 = 5, p1 = 30, v0 = 50, vmax = 150, v1 = 20, aa = 1000
% ad = -1500
t0 = 2;
p0 = 5; p1 = 30;
v0 = 50; vmax = 150; v1 = 20;
aa = 1000; ad = -1500;
h = p1 - p0;
% 可达到的最大速度
vf = sqrt((2.0*aa*ad*h - aa*v1^2 + ad*v0^2) / (ad - aa));
% 确定匀速阶段速度
if (vf < vmax)
vv = vf;
else
vv = vmax;
end
% 计算加速阶段的时间和位移
Ta = (vv - v0) / aa;
La = v0*Ta + (1.0/2.0)*aa*Ta^2;
% 计算匀速阶段的时间和位移
Tv = (h - (vv^2 - v0^2)/(2.0*aa) - (v1^2 - vv^2)/(2.0*ad)) / vv;
Lv = vv*Tv;
% 计算减速阶段的时间和位移
Td = (v1 - vv) / ad;
Ld = vv*Td + (1.0/2.0)*ad*Td^2;
k = 1;
ts = 0.001;
% 计算轨迹的离散点
for t = t0: ts: (t0+Ta+Tv+Td)
time(k) = t;
t = t - t0;
if (t >= 0 && t < Ta)
p(k) = p0 + v0*t + (1.0/2.0)*aa*t^2;
pd(k) = v0 + aa*t;
pdd(k) = aa;
elseif (t >= Ta && t < Ta+Tv)
p(k) = p0 + La + vv*(t - Ta);
pd(k) = vv;
pdd(k) = 0;
elseif (t >= Ta+Tv && t 0)
% 达到最大速度v_max，即存在匀速阶段
vlim = v_max;
T = Ta + Tv + Td;
para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
return;
else
% 达不到最大速度，即匀速阶段Tv=0
% 假设最大加速度和最小加速度均能达到
Tv = 0;
Tj = a_max / j_max;
Tj1 = Tj;
Tj2 = Tj;
delta = (a_max^4/j_max^2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)*(v_0 + v_1));
Ta = ((power(a_max, 2)/j_max) - 2.0*v_0 + sqrt(delta)) / (2.0*a_max);
Td = ((power(a_max, 2)/j_max) - 2.0*v_1 + sqrt(delta)) / (2.0*a_max);
% 对Ta和Td进行讨论
if (Ta < 0 || Td < 0)
if (Ta < 0)
% 没有加速段，只有减速段
Ta = 0; Tj1 = 0;
Td = 2*(q_1 - q_0) / (v_0 + v_1);
Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2) + power(v_1 + v_0, 2)*(v_1 - v_0)))) / (j_max*(v_1 + v_0));
a_lima = 0;
a_limd = -j_max*Tj2;
vlim = v0;
para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
return;
elseif (Td < 0)
% 没有减速段，只有加速段
Td = 0; Tj2 = 0;
Ta = 2*(q_1 - q_0) / (v_0 + v_1);
Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2)) - power(v_1 + v_0, 2)*(v_1 - v_0))) / (j_max*(v_1 + v_0));
a_lima = j_max*Tj1;
a_limd = 0;
vlim = v_0 + a_lima*(Ta - Tj1);
para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
return;
end
elseif (Ta >= 2*Tj && Td >= 2*Tj)
% 加速段和减速段都能达到最大加速度
a_lima = a_max;
a_limd = -a_max;
vlim = v0 + a_lima*(Ta - Tj);
para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
return;
else
% 加速和减速阶段至少有一段不能达到最大加速度
lambda = 0.99; % 系统取0= 2*Tj)
% 加速段和减速段都能达到最大加速度
a_lima = a_max;
a_limd = -a_max;
vlim = v0 + a_lima*(Ta - Tj);
para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
return;
end
end
end
end
end
%% 计算位移
function q = S_position(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
T = Ta + Tv + Td;
% 加速段
if (t >= 0 && t < Tj1)
q = q0 + v0*t + jmax*t^3/6;
elseif (t >= Tj1 && t < Ta - Tj1)
q = q0 + v0*t +(alima/6)*(3*t^2 - 3*Tj1*t + Tj1^2);
elseif (t >= Ta - Tj1 && t < Ta)
q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*((Ta - t)^3/6);
% 匀速段
elseif (t >= Ta && t < Ta + Tv)
q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
% 减速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(power(t - T + Td, 3)/6);
elseif (t >= T - Td + Tj2 && t < T - Tj2)
q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*power(t - T + Td, 2) - 3*Tj2*(t - T + Td) + Tj2^2);
elseif (t >= T - Tj2 && t = 0 && t < Tj1)
qd = v0 + jmax*(t^2/2);
elseif (t >= Tj1 && t < Ta - Tj1)
qd = v0 + alima*(t - Tj1/2);
elseif (t >= Ta - Tj1 && t < Ta)
qd = vlim + jmin*(power(Ta - t, 2)/2);
% 匀速段
elseif (t >= Ta && t < Ta + Tv)
qd = vlim;
% 减速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
qd = vlim - jmax*(power(t - T + Td, 2)/2);
elseif (t >= T - Td + Tj2 && t < T - Tj2)
qd = vlim + alimd*(t - T + Td - Tj2/2);
elseif (t >= T - Tj2 && t = 0 && t < Tj1)
qdd = jmax*t;
elseif (t >= Tj1 && t < Ta - Tj1)
qdd = alima;
elseif (t >= Ta - Tj1 && t < Ta)
qdd = -jmin*(Ta - t);
% 匀速段
elseif (t >= Ta && t < Ta + Tv)
qdd = 0;
% 减速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
qdd = -jmax*(t - T + Td);
elseif (t >= T - Td + Tj2 && t < T - Tj2)
qdd = alimd;
elseif (t >= T - Tj2 && t = 0 && t < Tj1)
qddd = jmax;
elseif (t >= Tj1 && t < Ta - Tj1)
qddd = 0;
elseif (t >= Ta - Tj1 && t < Ta)
qddd = jmin;
% 匀速段
elseif (t >= Ta && t < Ta + Tv)
qddd = 0;
% 减速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
qddd = -jmax;
elseif (t >= T - Td + Tj2 && t < T - Tj2)
qddd = 0;
elseif (t >= T - Tj2 && t |