【声明】实验中用到梯形速度规划相关知识，发现关于梯形速度规划的这篇知乎文章资料很好，特此做的归纳笔记，原文中的公式稍有错误，本人已在下面博文中更正，打算整理一个经典点到点轨迹规划方法的专栏。如有侵权，请告知删除！参考文章原文链接奉上。

1、简介

梯形速度曲线轨迹规划算法是工业控制领域应用最为广泛的速度控制策略之一，标准形式的梯形速度规划包含匀加速、匀速、匀减速三个阶段，在变速过程中，加速度始终是人为设定的固定值。

首先定义变量：
q0q_0q0：初始位置； t0t_0t0：初始时间； v0v_0v0：初始速度；
q1q_1q1：结束位置； t1t_1t1：结束时间； v1v_1v1：结束速度；
aaa_aaa：最大加速度；
ada_dad：最大减速度；
vmaxv_{max}vmax：用户输入最大速度；
vvv_vvv：实际最大速度；
TaT_aTa：匀加速段时长；
TdT_dTd：匀减速段时长；

匀加速段： 加速度为恒定的正直，速度随时间线性增加，位移是时间的抛物线函数；
匀速段： 加速度为零，速度恒定，位移是时间的线性函数；
匀减速段： 加速度为恒定的负值，速度随时间线性减小，位移是关于时间的二次多项式。

2、通用梯形速度曲线算法

在实际的机电系统中，其执行部件（一般是电机）允许的最大速度、最大加速度、启动速度、停车速度等会受到自身的物理条件约束，不可以人为改变。因此，实际点到点轨迹规划任务中，用户一般指定起始点位置、速度、最大速度、最大加速度、终止点位置、速度来计算梯形速度轨迹。

因此，通用梯形速度曲线为已知初始速度，终止速度，加速度，减速度，最大速度，位移。要确定轨迹，关键是计算出加速度段，匀速段，减速度段对应的时间 TaT_aTa，TvT_vTv，TdT_dTd，然后就可以计算任意时刻t∈[0,Ta+Tv+Td]t\in[0,T_a+T_v+T_d]t∈[0,Ta+Tv+Td] 对应的位移，速度，加速度。

用户给定的起始速度，终止速度，加速度，减速度，最大速度，位移参数，不一定都能满足，若给定的参数对应的的轨迹不存在，那么需要修改最大速度参数，优先满足位移条件（在实际的数控系统中常常是这么做)，常常修改的是最大速度。因此，可以按照以下步骤计算梯形速度曲线：

（1）首先，根据 hhh，v0v_0v0，v1v_1v1，aaa_aaa，ada_dad，其中 h=q1−q0h=q_1-q_0h=q1−q0，计算能够达到的最大速度，要达到最大速度，只有加速度段和减速度段，没有匀速段，因此有：
h≥ha+hd=vf2−v022aa+v12−vf22adh \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
即，
vf=2aaadh−aav12+adv02ad−aav_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）然后，根据用户指定的最大速度 vmaxv_{max}vmax 和 vfv_fvf 判断梯形速度曲线有无匀速段

若 vf<vmaxv_f < v_{max}vf<vmax，说明不用到达最大速度即可完成起点到终点的规划，则令 vv=vfv_v=v_fvv=vf，此时速度波形没有匀速段，只有匀加速和匀减速。
若 vf≥vmaxv_f \geq v_{max}vf≥vmax，说明需要匀速段的参与才能走完全部位移。但是又不允许超过最大速度，因此将匀速段速度设置为 vv=vmaxv_v=v_{max}vv=vmax。

（3）计算加速段、匀速段、减速段的时间和位移
Ta=vv−v0aa，La=v0Ta+12aaTa2T_a = \frac{v_v-v_0}{a_a}，L_a = v_0 T_a+\frac{1}{2}a_a T_a^2Ta=aavv−v0，La=v0Ta+21aaTa2
Tv=h−vv2−v022aa−v12−vv22advv，Lv=vvTvT_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_vTv=vvh−2aavv2−v02−2adv12−vv2，Lv=vvTv
Td=v1−vvad，Ld=vvTd+12adTd2T_d = \frac{v_1-v_v}{a_d}，L_d = v_vT_d + \frac{1}{2}a_dT_d^2Td=adv1−vv，Ld=vvTd+21adTd2
（4）当 t0=0t_0=0t0=0 时，三个阶段的位置、速度、加速度时间关系方程分别为：
{q(t)=q0+v0t+12aat2q˙(t)=v0+aatq¨(t)=aa,t∈[0,Ta)\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)=q0+La+vv(t−Ta)q˙(t)=vvq¨(t)=0，t∈[Ta,Ta+Tv)\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)=q0+La+Lv+12ad(t−Ta−Tv)2q˙(t)=vv+ad(t−Ta−Tv)q¨(t)=ad，t∈[Ta+Tv,Ta+Tv+Td)\begin{cases} q(t)=q_0+L_a+ L_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+21ad(t−Ta−Tv)2q˙(t)=vv+ad(t−Ta−Tv)q¨(t)=ad，t∈[Ta+Tv,Ta+Tv+Td)

当初始时刻 t0≠0t_0 \neq 0t0=0 时，将 ttt 换成 t−t0t-t_0t−t0，上式同样适用。

3、应用案例

t0=0,q0=0,q1=2π,v0=0,v1=0,vmax=0.01,aa=0.0001,ad=−0.0001t_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.0001t0=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;
elseVv=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+Tdtime(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;endif(t>=Ta &&t<Ta+Tv)q(j)=q0+La+Vv*(t-Ta);dq(j)=Vv;ddq(j)=0;endif(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

点到点轨迹规划【1】——梯形速度曲线规划相关推荐

电机速度曲线规划1：梯形速度曲线设计与实现
电机驱动是很常见的应用,在很多系统中我们都会碰到需要改变电机的速度以实现相应的控制功能,这就涉及到电机速度曲线规划的问题.在这篇中我们就来简单讨论一下电机的梯形曲线规划的问题. 1.基本原理 ...
机器人关节空间轨迹规划--S型速度规划
关节空间 VS 操作空间关节空间与操作空间轨迹规划流程图如下(上标$i$和$f$分别代表起始位置initial和目标位置final): 在关节空间内进行轨迹规划有如下优点: 在线运算量更小,即无需进 ...
电机速度曲线规划2：S形速度曲线设计与实现
电机驱动是很常见的应用,在很多系统中我们都会碰到需要改变电机的速度以实现相应的控制功能,这就涉及到电机速度曲线规划的问题.在这篇中我们就来简单讨论一下电机的S型曲线规划的问题. 1.基本原理 ...
电机速度曲线规划_S曲线
导读:电机驱动是很常见的应用,在很多系统中都会碰到需要改变电机的速度以实现相应的控制功能,这就涉及到电机速度曲线规划的问题. 本文仅代表记录自己的学习理解,如有错误,欢迎指正.想要S型速度曲线规划的仿 ...
无人驾驶汽车系统入门——基于Frenet优化轨迹的无人车动作规划方法
作者简介:申泽邦(Adam Shan),兰州大学在读硕士研究生,主攻无人驾驶,深度学习: 动作规划动作在无人车规划模块的最底层,它负责根据当前配置和目标配置生成一序列的动作.本文介绍一种基于Frene ...
无人驾驶汽车系统入门（二十一）——基于Frenet优化轨迹的无人车动作规划方法
动作规划动作在无人车规划模块的最底层,它负责根据当前配置和目标配置生成一序列的动作,我们前面讨论的三次样条插值实际上只是一个简单的路径,而非我们最终能够执行的轨迹,本文介绍一种基于Frenet坐标系的 ...
1学习笔记之知乎话题：多轴插补为什么普遍使用梯形速度曲线？
机械臂-轨迹规划-速度规划-学习笔记学习对象:知乎话题:多轴插补为什么普遍使用梯形速度曲线? ICRA2014 Joonyoung PPT 了解一下运动控制器从工业角度讲:算法与硬件平台的关系韩 ...
二.路径规划---二维路径规划实车实现---gmapping+amcl+map_server+move_base
专栏系列文章如下: 一.路径规划---二维路径规划仿真实现-gmapping+amcl+map_server+move_base_goldqiu的博客-CSDN博客本次实验是利用gmapping只使 ...
一.路径规划---二维路径规划仿真实现-gmapping+amcl+map_server+move_base
参考http://www.autolabor.com.cn/book/ROSTutorials/chapter1.html 1.首先实现仿真平台在rviz和gazebo的实现: 编写rviz环境下的x ...

点到点轨迹规划【1】——梯形速度曲线规划

1、简介

2、通用梯形速度曲线算法

3、应用案例

点到点轨迹规划【1】——梯形速度曲线规划相关推荐

最新文章

热门文章