加速度梯形算法：濾波方式下的速度規劃

已知線段長度$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}$。