2.1 梯形加减速算法
简介
本文将详细介绍梯形加减速算法的原理和代码实现。梯形加减速算法是一种常用的运动规划算法，用于实现平滑的加速和减速过程，以达到稳定运动的目的。这种轨迹分为三个部分。梯形加减速规划时，一般只对路径长度L进行规划，不管路径的具体形状，在插补的模块同时根据路径几何规律和加减速规律，计算路径上的插补点。
梯形加减速第一部分，加速度恒定，速度是时间的线性函数，位移是时间的抛物线函数；第二部分，加速度为0，速度恒定，位移是时间的线性函数；第三部分，加速度为恒定的负值，速度线性减小，位移是时间的二次多项式，如下图所示。
梯形加减速算法的原理
给定的参数为位移
L
L
L, 起步速度
v
s
v_s
vs, 匀速速度
v
c
v_c
vc ，末速度为
v
e
v_e
ve, 加速段加速度为
a
=
a
m
a
x
a=a_{max}
a=amax, 减速段加速度为
d
=
−
d
m
a
x
d=-d_{max}
d=−dmax 。梯形加减速的速度计算公式为
v
(
t
)
=
{
v
s
+
a
t
,
0
≤
t
<
t
1
v
c
,
t
1
≤
t
<
t
1
+
t
2
v
c
+
d
t
,
t
1
+
t
2
≤
t
<
t
1
+
t
2
+
t
3
\begin{equation} v(t)= \begin{cases} v_s+at,& 0\le t<t_1\\ v_c,& t_1\le t<t_1+t_2\\ v_c+dt, & t_1+t_2 \le t< t_1+t_2+t_3\\ \end{cases} \end{equation}
v(t)=⎩
⎨
⎧vs+at,vc,vc+dt,0≤t<t1t1≤t<t1+t2t1+t2≤t<t1+t2+t3
梯形加减速的位移计算公式为
q
(
t
)
=
{
v
s
t
+
1
2
a
t
2
,
0
≤
t
<
t
1
L
a
+
v
c
(
t
−
t
1
)
,
t
1
≤
t
<
t
2
L
a
+
L
v
+
v
c
(
t
−
t
1
−
t
2
)
+
1
2
d
(
t
−
t
1
−
t
2
)
2
,
t
1
+
t
2
≤
t
<
t
1
+
t
2
+
t
3
\begin{equation} q(t)= \begin{cases} v_st+\frac{1}{2}at^2,&0\leq t < t_1\\ L_a+v_c(t-t_1),&t_1\leq t< t_2\\ L_a+L_v+v_c(t-t_1-t_2)+\frac{1}{2}d(t-t_1-t_2)^2,& t_1+t_2\le t<t_1+t_2+t_3\\ \end{cases} \end{equation}
q(t)=⎩
⎨
⎧vst+21at2,La+vc(t−t1),La+Lv+vc(t−t1−t2)+21d(t−t1−t2)2,0≤t<t1t1≤t<t2t1+t2≤t<t1+t2+t3
其中，
L
a
=
v
s
t
1
+
1
2
a
t
1
2
,
L
v
=
v
c
t
2
L_a=v_st_1+\frac{1}{2}at_1^2,L_v=v_ct_2
La=vst1+21at12,Lv=vct2。
为了确定梯形速度曲线，需要指定其中部分参数，其他参数根据轨迹几何关系计算出来。对于一台机器要运动的位移，时间是千变万化的，但是其允许的最大速度，加速度，启动速度一般受到机械部件的限制，是确定的了。因此，实际应用中一般是指定梯形轨迹的位移 L L L，初始速度 v s v_s vs，最大速度 v m v_m vm，终止速度 v e v_e ve，加速度 a a a，减速度 d d d，然后计算出对应的加速时间 t 1 t_1 t1,匀速时间 t 2 t_2 t2和减速时间 t 3 t_3 t3。
用户给定的起始速度，终止速度，加速度，减速度，最大速度，位移参数，不一定都能满足，若给定的参数的轨迹不存在，那么需要修改速度参数，保证必须满足位移条件（在实际的数控系统中常常是这么做），常常修改的是边界速度（常常是末速度），同时保证修改后的速度不会超出用户指定边界的速度。因此，可以按照以下步骤计算时间最优的梯形速度曲线
采用试探法，假设没有匀速段，只有加速度段和减速段，运动中达到的最大速度为
v
f
v_f
vf，则有
L
=
L
a
+
L
d
=
v
f
2
−
v
s
2
2
a
+
v
e
2
−
v
f
2
2
d
L =L_a+L_d=\frac{v_f^2-v_s^2}{2a}+\frac{v_e^2-v_f^2}{2d}
L=La+Ld=2avf2−vs2+2dve2−vf2
这是可以解的
v
f
v_f
vf如下
v
f
=
a
v
e
2
−
d
v
s
2
−
2
a
d
L
a
−
d
v_f=\sqrt{\frac{av_e^2-dv_s^2-2adL}{a-d}}
vf=a−dave2−dvs2−2adL
需要根据
v
f
v_f
vf与匀速的最大速度
v
c
v_c
vc的关系进行分类讨论
-
若 v f > v c v_f>v_c vf>vc，有匀速段。说明位移足够长，可以加速到指定的匀速速度，并且能减速到给定的末速度。
（1）此时对应的时间可以计算出来
{ t 1 = v c − v s a , t 2 = L − v c 2 − v s 2 2 a − v e 2 − v c 2 2 d v c , t 3 = v e − v c d \begin{equation} \begin{cases} t_1=\frac{v_c-v_s}{a},\\ t_2=\frac{L-\frac{v_c^2-v_s^2}{2a}-\frac{v_e^2-v_c^2}{2d}}{v_c},\\ t_3=\frac{v_e-v_c}{d} \end{cases} \end{equation} ⎩ ⎨ ⎧t1=avc−vs,t2=vcL−2avc2−vs2−2dve2−vc2,t3=dve−vc -
若 v f ≤ v c v_f\le v_c vf≤vc,没有匀速段。说明位移较短，无法加速到期望的匀速速度就要开始减速了。此时还需要分3种情况讨论：
（2）若 v s < v f < v e v_s<v_f < v_e vs<vf<ve，则无法到达给定的末速度，只有加速度段，且末速度必须降低，才能保证位移满足条件。这时末速度为
v e = v s 2 + 2 a L v_e=\sqrt{v_s^2+2aL} ve=vs2+2aL
各段时间为
{ t 1 = v e − v s a , t 2 = 0 , t 3 = 0 \begin{equation} \begin{cases} t_1=\frac{v_e-v_s}{a},\\ t_2=0,\\ t_3=0 \end{cases} \end{equation} ⎩ ⎨ ⎧t1=ave−vs,t2=0,t3=0
(3)若 v e < v f < v s v_e<v_f<v_s ve<vf<vs,则无法达到给定的末速度，只有减速段，这时的末速度修正为
v e = v s 2 + 2 d L v_e=\sqrt {v_s^2+2dL} ve=vs2+2dL
各段时间为
{ t 1 = 0 , t 2 = 0 , t 3 = v e − v s d \begin{equation} \begin{cases} t_1=0,\\ t_2=0,\\ t_3=\frac{v_e-v_s}{d} \end{cases} \end{equation} ⎩ ⎨ ⎧t1=0,t2=0,t3=dve−vs
(4)若 v s < v f 且 v f > v s v_s<v_f 且v_f>v_s vs<vf且vf>vs,则存在加速度段和减速段，各段时间为
{ t 1 = v c − v s a , t 2 = 0 , t 3 = v e − v c d \begin{cases} t_1=\frac{v_c-v_s}{a},\\ t_2=0,\\ t_3=\frac{v_e-v_c}{d} \end{cases} ⎩ ⎨ ⎧t1=avc−vs,t2=0,t3=dve−vc
求出梯形加减速的三段曲线对应的时间后，就可以按照公式(1)和(2)计算对应的位移和速度。
示例演示
梯形加减速算法的原理给出了梯形加减速4种情况的公式推导，这里给出这4种情况的示例和源码（代码附件在文章最后）。
（1）有加速度段、匀速段和减速段。
给定参数为L =10, vs = 5, vmax= 50, ve = 10, acc = 500, dec = 400，速度曲线如下。
该情况下的公式也可以处理起步速度与匀速速度相同，或末速度与起步速度相同，且位移较大的情况。
例如，给定参数为L = 10, vs = 50, vmax= 50, ve = 20, amax= 500, dmax= 400，这是加速段时间为0,，只有匀速段段和减速段。
例如，给定参数为L = 10, vs = 10, vmax= 50, ve = 50, amax= 500, dmax= 400，这是加速段时间为0,，只有加速段和匀速段。
（2）只有加速段
给定参数为L = 1, vs = 10, vmax= 50, ve = 45, amax= 500, dmax= 400,速度曲线如下。
（3）只有减速段。
给定参数为L = 1, vs = 10, vmax= 50, ve = 45, amax= 500, dmax= 400,速度曲线如下。
（4）有加速段和减速段。
给定参数为L = 5, vs = 10, vmax= 50, ve = 20, amax= 500, dmax= 400,速度曲线如下。
总结
梯形加减速算法可以实现平滑的加速和减速过程，从而达到稳定的运动控制效果。
优点：
- 平滑稳定：梯形加减速算法能够实现平滑的加速和减速过程，减少了机械系统的冲击和震动，提高了运动控制的稳定性。
- 精确控制：梯形加减速算法可以精确控制运动过程中的加速度和减速度，从而保证了机械系统的运动精度。
- 简单易用：梯形加减速算法的实现比较简单，代码量相对较小，易于理解和维护。
缺点：
- 不适用于高速运动：梯形加减速算法在高速运动时，加速和减速时间会变得很短，导致机械系统的冲击和震动加剧，难以实现平滑的运动控制。
- 不适用于复杂运动：梯形加减速算法只适用于直线运动或简单曲线运动，对于复杂的路径规划，需要使用更加复杂的运动规划算法。
- 限制运动时间：梯形加减速算法在计算速度曲线时，需要提前确定总运动时间，难以适应需要灵活调整运动时间的应用场景。
综上所述，梯形加减速算法在简单的运动控制场景中具有优越性，但在高速运动和复杂运动控制中存在一定的局限性。需要根据具体应用场景选择适合的运动规划算法。
代码附件
这里给出了以上原理和示例的所有实现代码。
MATLAB测试绘图脚本：文章来源：https://www.toymoban.com/news/detail-685010.html
clc
clear
close("all")
set(gcf,'color','white');%白色
%读取C语言写入文件的数据
data=load("tpProfile.txt");
ti=data(:,1);
Li=data(:,2);
vi=data(:,3);
ai=data(:,4);
%绘制位移曲线
subplot(3,1,1);
plot(ti,Li,'-r.');
xlabel('time(s)')
ylabel('position')
%绘制速度曲线
subplot(3,1,2);
plot(ti,vi,'-g.');
xlabel('time(s)')
ylabel('velocity')
%绘制加速度曲线
subplot(3,1,3);
plot(ti,ai,'-b.');
xlabel('time(s)')
ylabel('acceleration')
三个关键函数文章来源地址https://www.toymoban.com/news/detail-685010.html
function tp = trapProfile(vs, vmax, ve, amax,dmax, L)
acc=amax;
dec=-dmax;
%起步速度和终止速度不能大于匀速速度
if (vs > vmax)
vs=vmax;
elseif (ve > vmax)
ve = vmax;
end
%4种情形
vf = sqrt((-2.0 * acc * dec * L - vs * vs * dec + ve * ve * acc) / (acc - dec));
if (vf > vmax)%有匀速段
vc = vmax;
t1 = (vc - vs) / acc;
t3 = (ve - vc) / dec;
L1 = vs * t1 + 0.5 * acc * t1 * t1;
L3 = vc * t3 + 0.5 * dec * t3 * t3;
L2 = L - L1 - L3;
t2 = L2 / vc;
t = t1 + t2 + t3;
else%没有匀速段
if (vs < vf && vf < ve)%只有加速段
ve = sqrt(vs * vs + 2 * acc * L);
vc = ve;
t1 = (ve - vs) / acc;
t2 = 0;
t3 = 0;
L1 = vs * t1 + 0.5 * acc * t1 * t1;
L2 = 0;
L3 = 0;
t = t1;
elseif (ve < vf && vf < vs)%只有减速段
ve = sqrt(vs * vs + 2 * dec * L);
vc = vs;
t1 = 0;
t2 = 0;
t3 = (ve - vs) / dec;
L1 = 0;
L2 = 0;
L3 = vc * t3 + 0.5 * dec * t3 * t3;
t = t3;
else%存在加速段和减速段
vc = vf;
t1 = (vc - vs) / acc;
t2 = 0;
t3 = (ve - vc) / dec;
L1 = vs * t1 + 0.5 * acc * t1 * t1;
L2 = 0;
L3 = vc * t3 + 0.5 * dec * t3 * t3;
t = t1 + t3;
end
end
tp.vs=vs;
tp.vc=vc;
tp.ve=ve;
tp.acc=amax;
tp.dec=-dmax;
tp.t1=t1;
tp.t2=t2;
tp.t3=t3;
tp.L1=L1;
tp.L2=L2;
tp.L3=L3;
tp.L=L;
tp.t=t;
end
function Lt= trapProfilePos(tp,t)
if (tp.t1>0 && t < tp.t1)
Lt = tp.vs * t + 0.5 * tp.acc * t * t;
elseif (tp.t2>0.0 && t < tp.t1 + tp.t2)
Lt = tp.L1 + tp.vc * (t - tp.t1);
else
tmp = t - tp.t1 - tp.t2;
Lt = tp.L1 + tp.L2 + tp.vc * tmp + 0.5 * tp.dec * tmp * tmp;
end
end
function vt= trapProfileVel(tp, t)
if(t<0.0)
vt=0.0;
elseif (tp.t1>0 && t < tp.t1)
vt = tp.vs + tp.acc * t;
elseif (tp.t2> 0.0 &&t < tp.t1 + tp.t2)
vt = tp.vc;
else
vt = tp.vc + tp.dec * (t - tp.t1 - tp.t2);
end
end
function acc= trapProfileAcc(tp,t)
if (t < 0.0)
acc= 0.0;
elseif (tp.t1>0 && t < tp.t1)
acc=tp.acc;
elseif (tp.t2>0 && t < tp.t1 + tp.t2)
acc=0.0;
else %if(tp.t3>0 && t <= tp.t)
acc=tp.dec;
end
end
到了这里，关于2.1 梯形加减速算法的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
