本文介绍了梯形速度曲线轨迹规划的基本原理，包括加速度、减速率的设定，如何根据用户给定参数计算加减速时间和位移，以及在实际应用中的案例。重点在于解决最大速度限制下的轨迹调整策略。
【声明】实验中用到梯形速度规划相关知识，发现关于梯形速度规划的这篇知乎文章资料很好，特此做的归纳笔记，原文中的公式稍有错误，本人已在下面博文中更正，打算整理一个经典点到点轨迹规划方法的专栏。如有侵权，请告知删除！参考文章原文链接奉上。
1、简介
梯形速度曲线轨迹规划算法是工业控制领域应用最为广泛的速度控制策略之一，标准形式的梯形速度规划包含匀加速、匀速、匀减速三个阶段，在变速过程中，加速度始终是人为设定的固定值。
首先定义变量：
q
0
q_0
q0：初始位置；
t
0
t_0
t0：初始时间；
v
0
v_0
v0：初始速度；
q
1
q_1
q1：结束位置；
t
1
t_1
t1：结束时间；
v
1
v_1
v1：结束速度；
a
a
a_a
aa：最大加速度；
a
d
a_d
ad：最大减速度；
v
m
a
x
v_{max}
vmax：用户输入最大速度；
v
v
v_v
vv：实际最大速度；
T
a
T_a
Ta：匀加速段时长；
T
d
T_d
Td：匀减速段时长；
匀加速段： 加速度为恒定的正直，速度随时间线性增加，位移是时间的抛物线函数；
匀速段： 加速度为零，速度恒定，位移是时间的线性函数；
匀减速段： 加速度为恒定的负值，速度随时间线性减小，位移是关于时间的二次多项式。
2、通用梯形速度曲线算法
在实际的机电系统中，其执行部件（一般是电机）允许的最大速度、最大加速度、启动速度、停车速度等会受到自身的物理条件约束，不可以人为改变。因此，实际点到点轨迹规划任务中，用户一般指定起始点位置、速度、最大速度、最大加速度、终止点位置、速度来计算梯形速度轨迹。
因此，通用梯形速度曲线为已知初始速度，终止速度，加速度，减速度，最大速度，位移。要确定轨迹，关键是计算出加速度段，匀速段，减速度段对应的时间 T a T_a Ta， T v T_v Tv， T d T_d Td，然后就可以计算任意时刻 t ∈ [ 0 , T a + T v + T d ] t\in[0,T_a+T_v+T_d] t∈[0,Ta+Tv+Td] 对应的位移，速度，加速度。
用户给定的起始速度，终止速度，加速度，减速度，最大速度，位移参数，不一定都能满足，若给定的参数对应的的轨迹不存在，那么需要修改最大速度参数，优先满足位移条件（在实际的数控系统中常常是这么做)，常常修改的是最大速度。因此，可以按照以下步骤计算梯形速度曲线：
（1）首先，根据
h
h
h，
v
0
v_0
v0，
v
1
v_1
v1，
a
a
a_a
aa，
a
d
a_d
ad，其中
h
=
q
1
−
q
0
h=q_1-q_0
h=q1−q0，计算能够达到的最大速度，要达到最大速度，只有加速度段和减速度段，没有匀速段，因此有：
h
≥
h
a
+
h
d
=
v
f
2
−
v
0
2
2
a
a
+
v
1
2
−
v
f
2
2
a
d
h \geq h_a + h_d = \frac{v_f^2-v_0^2}{2a_a} + \frac{v_1^2-v_f^2}{2a_d}
h≥ha+hd=2aavf2−v02+2adv12−vf2
即，
v
f
=
2
a
a
a
d
h
−
a
a
v
1
2
+
a
d
v
0
2
a
d
−
a
a
v_f = {\sqrt{\frac{2a_a a_d h - a_a v_1^2 + a_d v_0^2}{a_d-a_a}}}
vf=ad−aa2aaadh−aav12+adv02
（2）然后，根据用户指定的最大速度 v m a x v_{max} vmax 和 v f v_f vf 判断梯形速度曲线有无匀速段
- 若 v f < v m a x v_f < v_{max} vf<vmax，说明不用到达最大速度即可完成起点到终点的规划，则令 v v = v f v_v=v_f vv=vf，此时速度波形没有匀速段，只有匀加速和匀减速。
- 若 v f ≥ v m a x v_f \geq v_{max} vf≥vmax，说明需要匀速段的参与才能走完全部位移。但是又不允许超过最大速度，因此将匀速段速度设置为 v v = v m a x v_v=v_{max} vv=vmax。
（3）计算加速段、匀速段、减速段的时间和位移
T
a
=
v
v
−
v
0
a
a
，
L
a
=
v
0
T
a
+
1
2
a
a
T
a
2
T_a = \frac{v_v-v_0}{a_a}，L_a = v_0 T_a+\frac{1}{2}a_a T_a^2
Ta=aavv−v0，La=v0Ta+21aaTa2
T
v
=
h
−
v
v
2
−
v
0
2
2
a
a
−
v
1
2
−
v
v
2
2
a
d
v
v
，
L
v
=
v
v
T
v
T_v = \frac{h-\frac{v_v^2 - v_0^2}{2a_a}-\frac{v_1^2-v_v^2}{2a_d}}{v_v}，L_v = v_vT_v
Tv=vvh−2aavv2−v02−2adv12−vv2，Lv=vvTv
T
d
=
v
1
−
v
v
a
d
，
L
d
=
v
v
T
d
+
1
2
a
d
T
d
2
T_d = \frac{v_1-v_v}{a_d}，L_d = v_vT_d + \frac{1}{2}a_dT_d^2
Td=adv1−vv，Ld=vvTd+21adTd2
（4）当
t
0
=
0
t_0=0
t0=0 时，三个阶段的位置、速度、加速度时间关系方程分别为：
{
q
(
t
)
=
q
0
+
v
0
t
+
1
2
a
a
t
2
q
˙
(
t
)
=
v
0
+
a
a
t
q
¨
(
t
)
=
a
a
,
t
∈
[
0
,
T
a
)
\begin{cases} q(t)=q_0+v_0t+ \frac{1}{2}a_at^2 \\ {\dot q}(t)=v_0+a_at \\ {\ddot q}(t) = a_a\\ \end{cases},t \in[0,T_a)
⎩
⎨
⎧q(t)=q0+v0t+21aat2q˙(t)=v0+aatq¨(t)=aa,t∈[0,Ta)
{
q
(
t
)
=
q
0
+
L
a
+
v
v
(
t
−
T
a
)
q
˙
(
t
)
=
v
v
q
¨
(
t
)
=
0
，
t
∈
[
T
a
,
T
a
+
T
v
)
\begin{cases} q(t)=q_0+L_a+ v_v(t-T_a) \\ {\dot q}(t)=v_v \\ {\ddot q}(t) = 0 \\ \end{cases}，t \in [T_a,T_a+T_v)
⎩
⎨
⎧q(t)=q0+La+vv(t−Ta)q˙(t)=vvq¨(t)=0，t∈[Ta,Ta+Tv)
{
q
(
t
)
=
q
0
+
L
a
+
L
v
+
V
v
(
t
−
T
a
−
T
v
)
+
1
2
a
d
(
t
−
T
a
−
T
v
)
2
q
˙
(
t
)
=
v
v
+
a
d
(
t
−
T
a
−
T
v
)
q
¨
(
t
)
=
a
d
，
t
∈
[
T
a
+
T
v
,
T
a
+
T
v
+
T
d
)
\begin{cases} q(t)=q_0+L_a+ L_v+V_v(t-T_a-T_v)+\frac{1}{2}a_d(t-T_a-T_v)^2 \\ {\dot q}(t)=v_v+a_d(t-T_a-T_v) \\ {\ddot q}(t) = a_d \\ \end{cases}，t \in [T_a+T_v,T_a+T_v+T_d)
⎩
⎨
⎧q(t)=q0+La+Lv+Vv(t−Ta−Tv)+21ad(t−Ta−Tv)2q˙(t)=vv+ad(t−Ta−Tv)q¨(t)=ad，t∈[Ta+Tv,Ta+Tv+Td)
当初始时刻 t 0 ≠ 0 t_0 \neq 0 t0=0 时，将 t t t 换成 t − t 0 t-t_0 t−t0，上式同样适用。
3、应用案例
t
0
=
0
,
q
0
=
0
,
q
1
=
2
π
,
v
0
=
0
,
v
1
=
0
,
v
m
a
x
=
0.01
,
a
a
=
0.0001
,
a
d
=
−
0.0001
t_0=0, q_0=0, q_1=2\pi, v_0=0, v_1=0, v_{max}=0.01, a_a=0.0001, a_d=-0.0001
t0=0,q0=0,q1=2π,v0=0,v1=0,vmax=0.01,aa=0.0001,ad=−0.0001
目的是得到梯形规划后的角度序列。
clc
clear all
close all
%给定的轨迹参数
t0=0;
q0=0;
q1=2*3.1415926535;
v0=0;
v1=0;
vmax=0.01;
aa=0.0001;
ad=-0.0001;
%%
%设置绘图布局
fig1=subplot(3,1,1);
ylabel('position');
grid on
hold on
fig2=subplot(3,1,2);
ylabel('velocity');
grid on
hold on
fig3=subplot(3,1,3);
ylabel('acceleration');
xlabel('time');
grid on
hold on
h=q1-q0;
%确定最大速度
vf=sqrt((2*aa*ad*h-aa*v1^2+ad*v0^2)/(ad-aa));
if(vf>vmax)
Vv=vmax;
else
Vv=vf;
end
%计算加速和减速段时间
Ta=(Vv-v0)/aa;
La=v0*Ta+1.0/2.0*aa*Ta^2;
Td=(v1-Vv)/ad;
Ld=Vv*Td+1.0/2.0*ad*Td^2;
Tv=(h-(Vv^2-v0^2)/(2.0*aa)-(v1^2-Vv^2)/(2.0*ad))/Vv;
Lv=h-La-Ld;
%计算轨迹的离散点
Ts=1;
j=1;
for t=t0:Ts:t0+Ta+Tv+Td
time(j)=t;
t=t-t0;
if(t>=0&&t<Ta)
q(j)=q0+v0*t+1.0/2.0*aa*t^2;
dq(j)=v0+aa*t;
ddq(j)=aa;
end
if(t>=Ta &&t<Ta+Tv)
q(j)=q0+La+Vv*(t-Ta);
dq(j)=Vv;
ddq(j)=0;
end
if(t>Ta+Tv&&t<Ta+Tv+Td)
q(j)=q0+La+Lv+Vv*(t-Ta-Tv)+1.0/2.0*ad*(t-Ta-Tv)^2;
dq(j)=Vv+ad*(t-Ta-Tv);
ddq(j)=ad;
end
j=j+1;
end
%绘图
c1=plot(fig1,time,q,'-r','LineWidth',1.5);
c2=plot(fig2,time,dq,'-b','LineWidth',1.5);
c3=plot(fig3,time,ddq,'-g','LineWidth',1.5);
% clear q dq ddq
扫一扫
1708
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
