加速度梯形算法：滤波方式下的速度规划

已知线段长度$s$，起点速度$v_0$，利用加速度梯形算法（滤波方式）计算能达到的最大终点速度和最小终点速度。其中，系统最大速度为$v_m$，系统最大加速度为$a_m$，系统最大加加速度为$J_m$，插补周期为$T$。

计算能达到的最大终点速度$v'_m$

（1）假设实际运动的最大加速度为$a'_m$，则
$\begin{aligned} &n=\dfrac{v'_m-v_0}{a'_mT}, \\ &L=\dfrac{a'_m}{J_mT}. \end{aligned}$
其中，$n$为直线加速度加速的整周期数，$L$为速度滤波器的长度。

实际运动的距离$s$：$s=\dfrac{1}{2}(v_0+v'_m)(n+L)T=\dfrac{1}{2}(v_0+v'_m) \left(\dfrac{v'_m-v_0}{a'_m}+\dfrac{a'_m}{J_m}\right)$。

若使运动时间最短，令$\dfrac{v'_m-v_0}{a'_m}=\dfrac{a'_m}{J_m}$，$a'_m=\sqrt{J_m(v'_m-v_0)}$

于是有
$\begin{aligned} &s=\dfrac{1}{2}(v_0+v'_m)\cdot 2\cdot\dfrac{v'_m-v_0}{a'_m}=\dfrac{(v'_m+v_0)(v'_m-v_0)}{a'_m}, \\ &(v'_m+v_0)(v'_m-v_0)=a'_ms=\sqrt{J_m(v'_m-v_0)}\cdot s, \\ &(v'_m+v_0)\cdot \sqrt{v'_m-v_0}=\sqrt{J_m}\cdot s, \\ &(v'_m+v_0)^2(v'_m-v_0)=J_ms^2, \\ &v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3-J_ms^2=0. \end{aligned}$
（2）求解一元三次方程：$v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3-J_ms^2=0$

得到实际的最大终点速度$v'_m(v'_m>v_0)$。

（3）检验$a'_m=\dfrac{v^{'2}_m-v_0^2}{s}$是否满足系统最大加速度的要求。

若$a'_m$不大于系统最大加速度，则$min(v'_m,\ v_m)$即为能达到的最大终点速度。

（4）若$a'_m$大于系统最大加速度，则取$a_m$为实际运动的最大加速度，重新计算能达到的最大终点速度。
$\begin{aligned} &L=\dfrac{a_m}{J_mT},\quad n=\dfrac{v'_m-v_0}{a_mT}, \\ &s=\dfrac{1}{2}(v_0+v'_m)(L+n)T=\dfrac{1}{2}(v_0+v'_m)\left(\dfrac{a_m}{J_m} +\dfrac{v'_m-v_0}{a_m}\right), \\ &J_mv^{'2}_m+a_m^2v'_m+a_m^2v_0-J_mv_0^2-2a_mJ_ms=0. \end{aligned}$
由此求得最大终点速度$v'_m$，并且要求$v'_m>v_0$。若不满足要求，则令$v'_m=v_0$。

计算能达到的最小终点速度$v'_m$

（1）首先判断能否减速到零，如果能减速到零，则$v'_m=0$。

（2）如果不能减速到零，则假设实际运动的最大加速度为$a'_m$，于是有
$\begin{aligned} &n=\dfrac{v_0-v'_m}{a'_mT}, \\ &L=\dfrac{a'_m}{J_mT}. \end{aligned}$
其中，$n$为直线加速度减速的整周期数，$L$为速度滤波器的长度。

实际运动的距离$s$：$s=\dfrac{1}{2}(v_0+v'_m)(n+L)T=\dfrac{1}{2}(v_0+v'_m)\left(\dfrac{v_0-v'_m}{a'_m} +\dfrac{a'_m}{J_m}\right)$。

若使运动时间最短，令$\dfrac{v_0-v'_m}{a'_m}=\dfrac{a'_m}{J_m}$，$a'_m=\sqrt{J_m(v_0-v'_m)}$

于是有
$\begin{aligned} &s=\dfrac{1}{2}(v_0+v'_m)\cdot 2\cdot\dfrac{v_0-v'_m}{a'_m}=\dfrac{(v_0+v'_m)(v_0-v'_m)}{a'_m}, \\ &(v_0+v'_m)(v_0-v'_m)=a'_ms=\sqrt{J_m(v_0-v'_m)}\cdot s, \\ &(v_0+v'_m)\cdot \sqrt{v_0-v'_m}=\sqrt{J_m}\cdot s, \\ &(v_0+v'_m)^2(v_0-v'_m)=J_ms^2, \\ &v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3+J_ms^2=0. \end{aligned}$
（3）求解一元三次方程：$v^{'3}_m+v_0v^{'2}_m-v_0^2v'_m-v_0^3+J_ms^2=0$

得到实际的最小终点速度$v'_m(v'_m>0$并且$v'_m<v_0)$。

（4）检验$a'_m=\dfrac{v_0^2-v^{'2}_m}{s}$是否满足系统最大加速度的要求。

若$a'_m$不大于系统最大加速度，则$v'_m$即为能达到的最小终点速度。

（5）若$a'_m$大于系统最大加速度，则取$a_m$为实际运动的最大加速度，重新计算能达到的最小终点速度。
$\begin{aligned} &L=\dfrac{a_m}{J_mT},\quad n=\dfrac{v_0-v'_m}{a_mT}, \\ &s=\dfrac{1}{2}(v_0+v'_m)(L+n)T=\dfrac{1}{2}(v_0+v'_m)\left(\dfrac{a_m}{J_m} +\dfrac{v_0-v'_m}{a_m}\right), \\ &J_mv^{'2}_m-a_m^2v'_m-a_m^2v_0-J_mv_0^2+2a_mJ_ms=0. \end{aligned}$
由此求得最小终点速度$v'_m$，并且要求$v'_m>0$并且$v'_m<v_0$。若不满足要求，则令$v'_m=v_0$。

插补

已知起始速度$v_s$，终止速度$v_e$，系统最大速度为$v_m$，系统最大加速度为$a_m$，系统最大加加速度为$J_m$。

粗插补计算能加速到的最大速度$v_m'(v_m'\leqslant v_m)$，正向最大加速度$a_m'$，反向最大加速度$a_m''$。
$\begin{aligned} &a_m'=\sqrt{J_m(v_m'-v_s)},\quad a_m''=\sqrt{J_m(v_m'-v_e)}, \\ &a_m'=min(a_m',\ a_m),\quad a_m''=min(a_m'',\ a_m). \\ \end{aligned}$
正向运动距离：$s'=\dfrac{1}{2}(v_s+v_m')(L'+n')T$，其中$L'=\dfrac{a_m'}{J_mT}$，$n'=\dfrac{v_m'-v_s}{a_m'T}$。

反向运动距离：$s''=\dfrac{1}{2}(v_e+v_m')(L''+n'')T$，其中$L''=\dfrac{a_m''}{J_mT}$，$n''=\dfrac{v_m'-v_e}{a_m''T}$。