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

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

1、简介

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

首先定义变量：
$q_0$：初始位置； $t_0$：初始时间； $v_0$：初始速度；
$q_1$：结束位置； $t_1$：结束时间； $v_1$：结束速度；
$a_a$：最大加速度；
$a_d$：最大减速度；
$v_{max}$：用户输入最大速度；
$v_v$：实际最大速度；
$T_a$：匀加速段时长；
$T_d$：匀减速段时长；

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

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

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

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

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

（1）首先，根据 $h$，$v_0$，$v_1$，$a_a$，$a_d$，其中 $h=q_1-q_0$，计算能够达到的最大速度，要达到最大速度，只有加速度段和减速度段，没有匀速段，因此有：
$$h\ge h_a+h_d=\frac{v_f^2-v_0^2}{2a_a}+2a_d$$