加速度S形算法：濾波方式下的公式推導

原始速度函數

設初始速度爲$v_0$，最大加速度爲$a_m$，加速時間爲$t_1$，濾波時間爲$t_2$，於是有
$\begin{aligned} &v(t)=v_0+a_mt, && t\in[0,\ t_1] \\ &f(t)=\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}t), && t\in[0,\ t_2] \\ \end{aligned}$

構造濾波速度函數

令$v_m=v_0+a_mt_1$，
$\begin{aligned} &v(t)=\begin{cases} v_0, & t\in[0,\ t_2] \\ v_0+a_m(t-t_2), & t\in[t_2,\ t_1+t_2] \\ v_m, & t\in[t_1+t_2,\ t_1+2t_2] \\ \end{cases} \\ &f(t)=\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}t),\qquad\quad t\in[0,t_2] \end{aligned}$
令$V(t)=v(x)*f(x)=\int^\infty_{-\infty}v(t-x)\cdot f(x)dx$，則被積函數的非零定義域爲
$\begin{aligned} \begin{cases} 0\leqslant t-x\leqslant t_1+2t_2 \\ 0\leqslant x\leqslant t_2 \\ \end{cases} \quad\Longrightarrow \begin{cases} t-(t_1+2t_2)\leqslant x\leqslant t \\ 0\leqslant x\leqslant t_2 \\ \end{cases} \end{aligned}$

推導濾波速度公式

根據$t_1$和$t_2$的大小，分別推導濾波速度公式$V(t)$。

若 $t_1\geqslant t_2$

當 $t<0$ 時
$V(t)\equiv 0.$
當 $0\leqslant t\leqslant t_2$ 時
$V(t)=\int^t_0v_0\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx=-\dfrac{v_0}{2}\cos(\dfrac{\pi}{t_2}x)\Bigg|^t_0=\dfrac{v_0}{2}-\dfrac{v_0}{2}\cos(\dfrac{\pi}{t_2}t).$

當 $t_2\leqslant t\leqslant 2t_2$ 時
$\begin{aligned} V(t)=&\int_{t-t_2}^{t_2}v_0\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx+\int^{t-t_2}_0[v_0+a_m(t-x-t_2)]\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&-\dfrac{v_0}{2}\cos(\dfrac{\pi}{t_2}x)\Bigg|^{t_2}_{t-t_2} \\ &+\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos(\dfrac{\pi}{t_2}x)+\dfrac{a_m}{2}x\cos(\dfrac{\pi}{t_2}x)-\dfrac{a_m}{2}\cdot\dfrac{t_2}{\pi}\sin(\dfrac{\pi}{t_2}x)\right]\Bigg|_0^{t-t_2} \\ =&\dfrac{v_0}{2}+\dfrac{v_0}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right) +\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right)\right. \\ &\left. +\dfrac{a_m}{2}(t-t_2)\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right) -\dfrac{a_m}{2}\cdot\dfrac{t_2}{\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right) \right] \\ &-\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\right] \\ =&v_0+\dfrac{a_m}{2}(t-t_2)-\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right). \end{aligned}$
當 $2t_2\leqslant t\leqslant t_1+t_2$ 時
$\begin{aligned} V(t)=&\int_0^{t_2}[v_0+a_m(t-x-t_2)]\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos(\dfrac{\pi}{t_2}x)+\dfrac{a_m}{2}x\cos(\dfrac{\pi}{t_2}x)-\dfrac{a_m}{2}\cdot\dfrac{t_2}{\pi}\sin(\dfrac{\pi}{t_2}x)\right]\Bigg|^{t_2}_0 \\ =&\dfrac{1}{2}(v_0+a_mt-a_mt_2)-\dfrac{a_m}{2}t_2-[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)] \\ =&v_0+a_mt-\dfrac{3}{2}a_mt_2. \end{aligned}$
當 $t_1+t_2\leqslant t\leqslant t_1+2t_2$ 時
$\begin{aligned} V(t)=&\int_{t-t_1-t_2}^{t_2}[v_0+a_m(t-x-t_2)]\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx +\int_0^{t-t_1-t_2}v_m\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos(\dfrac{\pi}{t_2}x) +\dfrac{a_m}{2}x\cos(\dfrac{\pi}{t_2}x)-\dfrac{a_m}{2}\cdot\dfrac{t_2}{\pi}\sin(\dfrac{\pi}{t_2}x)\right]\Bigg|^{t_2}_{t-t_1-t_2} \\ &-\dfrac{v_m}{2}\cos(\dfrac{\pi}{t_2}x)\Bigg|_0^{t-t_1-t_2} \\ =&\dfrac{1}{2}(v_0+a_mt-a_mt_2)-\dfrac{a_m}{2}t_2 -\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right) \right. \\ &\left. +\dfrac{a_m}{2}(t-t_1-t_2)\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right) -\dfrac{a_m}{2}\cdot\dfrac{t_2}{\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right)\right] \\ &-\dfrac{v_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right)+\dfrac{v_m}{2} \\ =&\dfrac{1}{2}(v_0+v_m)+\dfrac{1}{2}a_mt-a_mt_2+\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right). \end{aligned}$
當 $t_1+2t_2\leqslant t\leqslant t_1+3t_2$ 時
$\begin{aligned} V(t)=&\int_{t-t_1-2t_2}^{t_2}v_m\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&-\dfrac{v_m}{2}\cos(\dfrac{\pi}{t_2}x)\Bigg|_{t-t_1-2t_2}^{t_2} \\ =&\dfrac{v_m}{2}+\dfrac{v_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-2t_2)\right). \end{aligned}$
當 $t>t_1+3t_2$ 時
$V(t)\equiv 0.$

若 $t_1<t_2$

當 $t<0$ 時
$V(t)\equiv 0.$
當 $0\leqslant t\leqslant t_2$ 時
$V(t)=\int_0^tv_0\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx=\dfrac{v_0}{2}-\dfrac{v_0}{2}\cos(\dfrac{\pi}{t_2}t).$
當 $t_2\leqslant t\leqslant t_1+t_2$ 時
$\begin{aligned} V(t)=&\int_{t-t_2}^{t_2}v_0\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx +\int_0^{t-t_2}[v_0+a_m(t-x-t_2)]\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&v_0+\dfrac{a_m}{2}(t-t_2)-\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right). \end{aligned}$
當 $t_1+t_2\leqslant t\leqslant 2t_2$ 時
$\begin{aligned} V(t)=&\int_{t-t_2}^{t_2}v_0\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx +\int_{t-t_1-t_2}^{t-t_2}[v_0+a_m(t-x-t_2)]\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ &+\int_0^{t-t_1-t_2}v_m\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&-\dfrac{v_0}{2}\cos(\dfrac{\pi}{t_2}x)\Bigg|^{t_2}_{t-t_2} \\ &+\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos(\dfrac{\pi}{t_2}x) +\dfrac{a_m}{2}x\cos(\dfrac{\pi}{t_2}x) -\dfrac{a_m}{2}\cdot\dfrac{t_2}{\pi}\sin(\dfrac{\pi}{t_2}x)\right]\Bigg|^{t-t_2}_{t-t_1-t_2} \\ &-\dfrac{v_m}{2}\cos(\dfrac{\pi}{t_2}x)\Bigg|_0^{t-t_1-t_2} \\ =&\dfrac{v_0}{2}+\dfrac{v_0}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right) +\left[ -\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right)\right. \\ &\left.+\dfrac{a_m}{2}(t-t_2)\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right) -\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right) \right] \\ &-\left[-\dfrac{1}{2}(v_0+a_mt-a_mt_2)\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right) \right. \\ &\left.+\dfrac{a_m}{2}(t-t_1-t_2)\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right) -\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right) \right] \\ &-\dfrac{v_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right)+\dfrac{v_m}{2} \\ =&\dfrac{1}{2}(v_0+v_m)-\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right) +\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right). \end{aligned}$
當 $2t_2\leqslant t\leqslant t_1+2t_2$ 時
$\begin{aligned} V(t)=&\int_{t-t_1-t_2}^{t_2}[v_0+a_m(t-x-t_2)]\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx +\int_0^{t-t_1-t_2}v_m\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx \\ =&\dfrac{1}{2}(v_0+v_m)+\dfrac{1}{2}a_mt-a_mt_2+\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right). \end{aligned}$
當 $t_1+2t_2\leqslant t\leqslant t_1+3t_2$ 時
$V(t)=\int_{t-t_1-2t_2}^{t_2}v_m\cdot\dfrac{\pi}{2t_2}\sin(\dfrac{\pi}{t_2}x)dx =\dfrac{v_m}{2}+\dfrac{v_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-2t_2)\right).$
當 $t>t_1+3t_2$ 時
$V(t)\equiv 0.$

濾波速度公式

取 $t_2\leqslant t\leqslant t_1+2t_2$ 時的濾波速度函數作爲濾波速度公式，則有

當 $t_1\geqslant t_2$ 時
$\begin{aligned} V(t)=\begin{cases} v_0+\dfrac{a_m}{2}(t-t_2)-\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right), & 若\ t_2\leqslant t\leqslant 2t_2 \\ v_0+a_mt-\dfrac{3}{2}a_mt_2, & 若\ 2t_2\leqslant t\leqslant t_1+t_2 \\ \dfrac{1}{2}(v_0+v_m)+\dfrac{1}{2}a_mt-a_mt_2+\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ t_1+t_2\leqslant t\leqslant t_1+2t_2 \end{cases} \end{aligned}$
當 $t_1 < t_2$ 時
$\begin{aligned} V(t)=\begin{cases} v_0+\dfrac{a_m}{2}(t-t_2)-\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right), & 若\ t_2\leqslant t\leqslant t_1+t_2 \\ \dfrac{1}{2}(v_0+v_m)-\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right) +\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ t_1+t_2\leqslant t\leqslant 2t_2 \\ \dfrac{1}{2}(v_0+v_m)+\dfrac{1}{2}a_mt-a_mt_2+\dfrac{a_mt_2}{2\pi}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ 2t_2\leqslant t\leqslant t_1+2t_2 \end{cases} \end{aligned}$

濾波速度公式的導數及積分

加速度公式

濾波速度公式的一階導數，如下

當 $t_1\geqslant t_2$ 時
$\begin{aligned} V'(t)=\begin{cases} \dfrac{a_m}{2}-\dfrac{a_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right), & 若\ t_2\leqslant t\leqslant 2t_2 \\ a_m, & 若\ 2t_2\leqslant t\leqslant t_1+t_2 \\ \dfrac{a_m}{2}+\dfrac{a_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ t_1+t_2\leqslant t\leqslant t_1+2t_2 \\ \end{cases} \end{aligned}$
當 $t_1<t_2$ 時
$\begin{aligned} V'(t)=\begin{cases} \dfrac{a_m}{2}-\dfrac{a_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right), & 若\ t_2\leqslant t\leqslant t_1+t_2 \\ -\dfrac{a_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_2)\right) +\dfrac{a_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ t_1+t_2\leqslant t\leqslant 2t_2 \\ \dfrac{a_m}{2}+\dfrac{a_m}{2}\cos\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ 2t_2\leqslant t\leqslant t_1+2t_2 \\ \end{cases} \end{aligned}$

加加速度公式

濾波速度公式的二階導數，如下

當 $t_1\geqslant t_2$ 時
$\begin{aligned} V''(t)=\begin{cases} \dfrac{a_m\pi}{2t_2}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right), & 若\ t_2\leqslant t\leqslant 2t_2 \\ 0, & 若\ 2t_2<t<t_1+t_2 \\ -\dfrac{a_m\pi}{2t_2}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ t_1+t_2\leqslant t\leqslant t_1+2t_2 \\ \end{cases} \end{aligned}$
當 $t_1<t_2$ 時
$\begin{aligned} V''(t)=\begin{cases} \dfrac{a_m\pi}{2t_2}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right), & 若\ t_2\leqslant t\leqslant t_1+t_2 \\ \dfrac{a_m\pi}{2t_2}\sin\left(\dfrac{\pi}{t_2}(t-t_2)\right) -\dfrac{a_m\pi}{2t_2}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ t_1+t_2<t<2t_2 \\ -\dfrac{a_m\pi}{2t_2}\sin\left(\dfrac{\pi}{t_2}(t-t_1-t_2)\right), & 若\ 2t_2\leqslant t\leqslant t_1+2t_2 \\ \end{cases} \end{aligned}$
此即爲加速度S形算法的加加速度公式，若已知系統最大加加速度爲$J_m$，則可以令 $\dfrac{a_m\pi}{2t_2}=J_m$，於是有 $t_2=\dfrac{a_m\pi}{2J_m}$。

距離公式

根據加速度公式的對稱性，按照幾何意義積分，可以得到運動距離的公式爲
$s=\dfrac{1}{2}(v_0+v_m)(t_1+t_2)$
其中 $v_m=v_0+a_mt_1\quad \Leftrightarrow\quad t_1=\dfrac{v_m-v_0}{a_m},\ t_2=\dfrac{a_m\pi}{2J_m}$。

以上就是濾波方式下加速度S形算法的連續表達式形式。