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

已知線段長度$s$，起點速度$v_0$，利用加速度S形算法（濾波方式）計算能達到的最大終點速度和最小終點速度。其中，系統最大速度爲$v_m$，系統最大加速度爲$a_m$，系統最大加加速度爲$J_m$，插補週期爲$T$。

計算能達到的最大終點速度$v'_m$

（1）假設實際最大加速度爲$a'_m$，則
$\begin{aligned} &n=\dfrac{v'_m-v_0}{a'_mT}, \\ &L=\dfrac{\pi a'_m}{2J_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{\pi a_m'}{2J_m}\right)$。

若使運動時間最短，令$\dfrac{v_m'-v_0}{a_m'}=\dfrac{\pi a_m'}{2J_m}$，$a_m'=\sqrt{\dfrac{2J_m(v_m'-v_0)}{\pi}}$

於是有
$\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_m's=\sqrt{\dfrac{2J_m(v_m'-v_0)}{\pi}}\cdot s, \\ &(v_m'+v_0)\cdot\sqrt{v_m'-v_0}=\sqrt{\dfrac{2J_m}{\pi}}\cdot s, \\ &(v_m'+v_0)^2(v_m'-v_0)=\dfrac{2J_m}{\pi}s^2, \\ &v_m'^3+v_0v_m'^2-v_0^2v_m'-v_0^3-\dfrac{2J_ms^2}{\pi} = 0. \\ \end{aligned}$
（2）求解一元三次方程：$v_m'^3+v_0v_m'^2-v_0^2v_m'-v_0^3-\dfrac{2J_ms^2}{\pi}=0$

得到實際的最大終點速度$v_m'(v_m'>v_0)$。

（3）檢驗$a_m'=\dfrac{v_m'^2-v_0^2}{s}$是否滿足系統最大加速度的要求。

若$a_m'$不大於系統最大加速度，則$min(v_m',\ v_m)$即爲能達到的最大終點速度。

（4）若$a_m'$大於系統最大加速度，則取$a_m$爲實際加工的最大加速度，重新計算能達到的最大終點速度。
$\begin{aligned} &L=\dfrac{\pi a_m}{2J_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{\pi a_m}{2J_m}+\dfrac{v_m'-v_0}{a_m}\right), \\ &2J_mv_m'^2+\pi a_m^2v_m'+\pi a_m^2v_0-2J_mv_0^2-4a_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_m'T}, \\ &L=\dfrac{\pi a_m'}{2J_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{\pi a_m'}{2J_m}\right)$。

若使運動時間最短，令$\dfrac{v_0-v_m'}{a_m'}=\dfrac{\pi a_m'}{2J_m}$，$a_m'=\sqrt{\dfrac{2J_m(v_0-v_m')}{\pi}}$

於是有
$\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_m's=\sqrt{\dfrac{2J_m(v_0-v_m')}{\pi}}\cdot s, \\ &(v_0+v_m')\cdot\sqrt{v_0-v_m'}=\sqrt{\dfrac{2J_m}{\pi}}\cdot s, \\ &(v_0+v_m')^2(v_0-v_m')=\dfrac{2J_ms^2}{\pi}, \\ &v_m'^3+v_0v_m'^2-v_0^2v_m'-v_0^3+\dfrac{2J_ms^2}{\pi}=0. \\ \end{aligned}$
（3）求解一元三次方程：$v_m'^3+v_0v_m'^2-v_0^2v_m'-v_0^3+\dfrac{2J_ms^2}{\pi}=0$

得到實際的最小終點速度$v_m'$（$v_m'>0$並且$v_m'<v_0$）。

（4）檢驗$a_m'=\dfrac{v_0^2-v_m'^2}{s}$是否滿足系統最大加速度的要求。

若$a_m'$不大於系統最大加速度，則$v_m'$即爲能達到的最小終點速度。

（5）若$a_m'$大於系統最大加速度，則取$a_m$爲實際加工的最大加速度，重新計算能達到的最小終點速度。
$\begin{aligned} &L=\dfrac{\pi a_m}{2J_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{\pi a_m}{2J_m}+\dfrac{v_0-v_m'}{a_m}\right), \\ &2J_mv_m'^2-\pi a_m^2v_m'-\pi a_m^2v_0-2J_mv_0^2+4a_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{\dfrac{2J_m(v_m'-v_s)}{\pi}},\quad a_m''=\sqrt{\dfrac{2J_m(v_m'-v_e)}{\pi}}, \\ &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{\pi a_m'}{2J_mT}$，$n'=\dfrac{v_m'-v_s}{a_m'T}$。

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