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

原始速度函數

設初始速度爲$v_0$，最大加速度爲$a_m$，加速時間爲$t_1$，濾波時間爲$t_2$，於是有
$\begin{aligned} &v(t)=v_0+a_mt,&& t\in[0,\ t_1] \\ &f(t)=\dfrac{1}{t_2},&& 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{1}{t_2},\qquad\qquad\qquad\quad\ t\in[0,\ t_2] \end{aligned}$
令$V(t)=v(x)\ast f(x)=\int^{\infty}_{-\infty}v(x)\cdot f(t-x)dx$，則被積函數的非零定義域爲
$\begin{aligned} \begin{cases} 0<x<t_1+2t_2 \\ 0<t-x<t_2 \end{cases} \Longrightarrow \begin{cases} 0<x<t_1+2t_2 \\ t-t_2<x<t \end{cases} \end{aligned}$

推導濾波速度公式

若 $t_1\geqslant t_2$

$V(t)$的被積函數的非零定義域如下圖所示

當$t<0$時
$\begin{aligned} V(t)\equiv0. \end{aligned}$
當$0\leqslant t\leqslant t_2$時
$\begin{aligned} V(t)=\int^t_0v_0\cdot\dfrac{1}{t_2}dx=\dfrac{v_0t}{t_2}. \end{aligned}$
當$t_2\leqslant t\leqslant 2t_2$時
$\begin{aligned} V(t)&=\int^{t_2}_{t-t_2}v_0\cdot\dfrac{1}{t_2}dx +\int^t_{t_2}[v_0+a_m(x-t_2)]\cdot\dfrac{1}{t_2}dx \\ &=\dfrac{v_0}{t_2}(2t_2-t) +\dfrac{v_0-a_mt_2}{t_2}(t-t_2) +\dfrac{a_m}{2t_2}(t^2-t_2^2) \\ &=\dfrac{a_m}{2t_2}t^2-a_mt+v_0+\dfrac{1}{2}a_mt_2. \end{aligned}$
當$2t_2\leqslant t\leqslant t_1+t_2$時
$\begin{aligned} V(t)&=\int^t_{t-t_2}[v_0+a_m(x-t_2)]\cdot\dfrac{1}{t_2}dx \\ &=\dfrac{v_0-a_mt_2}{t_2}[t-(t-t_2)] +\dfrac{1}{2t_2}a_m[t^2-(t-t_2)^2] \\ &=v_0-a_mt_2+a_mt-\dfrac{1}{2}a_mt_2 \\ &=a_mt+v_0-\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_1+t_2}_{t-t_2}[v_0+a_m(x-t_2)]\cdot\dfrac{1}{t_2}dx +\int^t_{t_1+t_2}v_m\cdot\dfrac{1}{t_2}dx \\ &=\dfrac{v_0-a_mt_2}{t_2}(t_1+2t_2-t) +\dfrac{a_m}{2t_2}[(t_1+t_2)^2-(t-t_2)^2] +\dfrac{v_m}{t_2}(t-t_1-t_2) \\ &=\dfrac{v_0-a_mt_2}{t_2}(t_1+2t_2-t) +\dfrac{a_m}{2t_2}(t_1^2+2t_1t_2-t^2+2t_2t) +\dfrac{v_m}{t_2}(t-t_1-t_2) \\ &=-\dfrac{a_m}{2t_2}t^2 +\dfrac{a_m(t_1+2t_2)}{t_2}t +v_0 -\dfrac{a_m(t_1^2+2t_1t_2+4t_2^2)}{2t_2}. \end{aligned}$
當$t_1+2t_2\leqslant t\leqslant t_1+3t_2$時
$\begin{aligned} V(t)=\int^{t_1+2t_2}_{t-t_2}v_m\cdot\dfrac{1}{t_2}dx =\dfrac{v_m}{t_2}(t_1+3t_2-t). \end{aligned}$
當$t>t_1+3t_2$時
$\begin{aligned} V(t)\equiv0. \end{aligned}$

若 $t_1>t_2$

$V(t)$的被積函數的非零定義域如下圖所示

當$t<0$時
$\begin{aligned} V(t)\equiv0. \end{aligned}$
當$0\leqslant t\leqslant t_2$時
$\begin{aligned} V(t)=\int^t_0v_0\cdot\dfrac{1}{t_2}dx=\dfrac{v_0t}{t_2}. \end{aligned}$
當$t_2\leqslant t\leqslant t_1+t_2$時
$\begin{aligned} V(t)&=\int^{t_2}_{t-t_2}v_0\cdot\dfrac{1}{t_2}dx +\int^t_{t_2}[v_0+a_m(x-t_2)]\cdot\dfrac{1}{t_2}dx \\ &=\dfrac{v_0}{t_2}(2t_2-t) +\dfrac{v_0-a_mt_2}{t_2}(t-t_2) +\dfrac{a_m}{2t_2}(t^2-t_2^2) \\ &=\dfrac{a_m}{2t_2}t^2-a_mt+v_0+\dfrac{1}{2}a_mt_2. \end{aligned}$
當$t_1+t_2\leqslant t\leqslant 2t_2$時
$\begin{aligned} V(t)&=\int^{t_2}_{t-t_2}v_0\cdot\dfrac{1}{t_2}dx +\int^{t_1+t_2}_{t_2}[v_0+a_m(x-t_2)]\cdot\dfrac{1}{t_2}dx +\int^t_{t_1+t_2}v_m\cdot\dfrac{1}{t_2}dx \\ &=\dfrac{v_0}{t_2}(2t_2-t) +\dfrac{v_0-a_mt_2}{t_2}t_1 +\dfrac{a_m}{2t_2}[(t_1+t_2)^2-t_2^2] +\dfrac{v_m}{t_2}(t-t_1-t_2) \\ &=\dfrac{a_mt_1}{t_2}t +v_0 -\dfrac{a_mt_1^2+2a_mt_1t_2}{2t_2} \\ &=\dfrac{a_mt_1}{t_2}t +v_0 -\dfrac{a_mt_1(t_1+2t_2)}{2t_2}. \end{aligned}$
當$2t_2\leqslant t\leqslant t_1+2t_2$時
$\begin{aligned} V(t)&=\int^{t_1+t_2}_{t-t_2}[v_0+a_m(x-t_2)]\cdot\dfrac{1}{t_2}dx +\int^t_{t_1+t_2}v_m\cdot\dfrac{1}{t_2}dx \\ &=\dfrac{v_0-a_mt_2}{t_2}(t_1+2t_2-t) +\dfrac{a_m}{2t_2}[(t_1+t_2)^2-(t-t_2)^2] +\dfrac{v_m}{t_2}(t-t_1-t_2) \\ &=\dfrac{v_0-a_mt_2}{t_2}(t_1+2t_2-t) +\dfrac{a_m}{2t_2}(t_1^2+2t_1t_2-t^2+2t_2t) +\dfrac{v_m}{t_2}(t-t_1-t_2) \\ &=-\dfrac{a_m}{2t_2}t^2 +\dfrac{a_m(t_1+2t_2)}{t_2}t +v_0 -\dfrac{a_m(t_1^2+2t_1t_2+4t_2^2)}{2t_2}. \end{aligned}$
當$t_1+2t_2\leqslant t\leqslant t_1+3t_2$時
$\begin{aligned} V(t)=\int^{t_1+2t_2}_{t-t_2}v_m\cdot\dfrac{1}{t_2}dx =\dfrac{v_m}{t_2}(t_1+3t_2-t). \end{aligned}$
當$t>t_1+3t_2$時
$\begin{aligned} V(t)\equiv0. \end{aligned}$

濾波速度公式

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

當$t_1\geqslant t_2$時
$\begin{aligned} V(t)=\begin{cases} \dfrac{a_m}{2t_2}t^2-a_mt+v_0+\dfrac{1}{2}a_mt_2, &若\ t_2\leqslant t\leqslant 2t_2 \\ a_mt+v_0-\dfrac{3}{2}a_mt_2, &若\ 2t_2\leqslant t\leqslant t_1+t_2 \\ -\dfrac{a_m}{2t_2}t^2+\dfrac{a_m(t_1+2t_2)}{t_2}t+v_0-\dfrac{a_m(t_1^2+2t_1t_2+4t_2^2)}{2t_2}, &若\ 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}{2t_2}t^2-a_mt+v_0+\dfrac{1}{2}a_mt_2, &若\ t_2\leqslant t\leqslant t_1+t_2 \\ \dfrac{a_mt_1}{t_2}t+v_0-\dfrac{a_mt_1(t_1+2t_2)}{2t_2}, &若\ t_1+t_2\leqslant t\leqslant 2t_2 \\ -\dfrac{a_m}{2t_2}t^2+\dfrac{a_m(t_1+2t_2)}{t_2}t+v_0-\dfrac{a_m(t_1^2+2t_1t_2+4t_2^2)}{2t_2}, &若\ 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}{t_2}t-a_m, &若\ t_2\leqslant t\leqslant 2t_2 \\ a_m, &若\ 2t_2\leqslant t\leqslant t_1+t_2 \\ -\dfrac{a_m}{t_2}t+\dfrac{a_m(t_1+2t_2)}{t_2}, &若\ 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}{t_2}t-a_m, &若\ t_2\leqslant t\leqslant t_1+t_2 \\ \dfrac{a_mt_1}{t_2}, &若\ t_1+t_2\leqslant t\leqslant 2t_2 \\ -\dfrac{a_m}{t_2}t+\dfrac{a_m(t_1+2t_2)}{t_2}, &若\ 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}{t_2}, &若\ t_2\leqslant t\leqslant 2t_2 \\ 0, &若\ 2t_2< t< t_1+t_2 \\ -\dfrac{a_m}{t_2}, &若\ 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}{t_2}, &若\ t_2\leqslant t\leqslant t_1+t_2 \\ 0, &若\ t_1+t_2< t< 2t_2 \\ -\dfrac{a_m}{t_2}, &若\ 2t_2\leqslant t\leqslant t_1+2t_2 \end{cases} \end{aligned}$
此即爲加速度梯形算法的加加速度公式，若已知系統最大加加速度爲$J_m$，則可以令$\dfrac{a_m}{t_2}=J_m$，於是有$t_2=\dfrac{a_m}{J_m}$。

距離公式

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

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