關於電腦鼠角速度命令曲線平滑化的討論 Discussions of angular speed command profiles for micromouse - PART II

這一篇文章主要是要補足前一篇文章「Kato 與我關於電腦鼠角速度曲線平滑化的討論」和 SIMULINK 行為模型不同之處，因為我忘了為何兩者定義有些不同。

原先的做法是

$\omega_c(t) = \alpha\sin (\beta t),  t \in [0, \pi/(2\beta)]$。

原始餘弦函數若是

$\omega_o(t) = \alpha(1-\cos (\beta t)),  t \in [0, \pi/(2\beta)]$，

其中角加速度的極大值由以下的微分運算可以知道為 $\alpha \beta$

$\frac{d\omega_o(t)}{dt} = \alpha \beta \sin(\beta t),  t \in [0, \pi/(2\beta)]$

把 $\omega_o(t)$ 的微分(角加速度)在 $t=\pi/(2\beta)$ 產生最大值 $\alpha\beta$ 的點，作為「控制點」，將它移動到 $t=\pi/(2\sigma \beta)$ 的點。

因此，為了保持角加速度的極大值為 $\alpha \beta$，就將第一段角速度曲線的緩加速定義成

$\omega_1(t) = \frac{\alpha}{\sigma}(1-\cos (\sigma \beta t)),   t \in \left[ 0, \frac{\pi}{2\beta \sigma} \right]$。

因此第二段角速度曲線 $\omega_2(t)$ 的變化過程，必須符合以下三個條件

1. $\omega_1\left( \frac{\pi}{2\beta \sigma} \right) = \omega_2\left( \frac{\pi}{2\beta \sigma} \right) = \frac{\alpha}{\sigma}$
2. $\omega_2 \left( \frac{\pi}{2\beta} \right) = \alpha$。
3. $\frac{d\omega_1}{dt}\left(\frac{\pi}{2\beta \sigma}\right) = \frac{d\omega_2}{dt}\left(\frac{\pi}{2\beta \sigma}\right)$

假設

$\omega_2(t) = a(b-c\cos(dt+e))$。

此時

$\frac{d\omega_2(t)}{dt} = acd \sin(dt+e)$。

那麼由於 $t=\pi/(2\sigma \beta)$ 時，角加速度必須是最大值，因此

$d\frac{\pi}{2\beta \sigma}+e=\frac{\pi}{2}$。

而且在 $t=\pi/(2\beta)$ 時，必須與原有的弦波角速度命令曲線相同大小，還有角加速度也是0，因此

$d\frac{\pi}{2\beta}+e=\pi$。

這兩個方程式相減，就可以找出 $d$ 的大小

$d \left( \frac{\pi}{2\beta} - \frac{\pi}{2\beta\sigma} \right) = \frac{\pi}{2} \rightarrow d = \frac{\sigma\beta}{\sigma-1}, e = \pi-d\frac{\pi}{2\beta} = \frac{\pi(\sigma-2)}{2(\sigma-1)}$

當 $dt_1+e = \pi/2$ 時，也就是 $t_1=\pi/(2\beta\sigma)$ 時

$\omega_2(t_1) = ab = \omega_1(t_1) = \frac{\alpha}{\sigma}$；

$\frac{d\omega_2}{dt}(t_1) = \frac{d\omega_1}{dt}(t_1) = \alpha\beta = acd\sin(dt_1+e) = acd$

由於 $d$ 已經找出大小了，所以

$acd = \alpha\beta \rightarrow ac = \frac{\alpha\beta}{d} = \frac{(\sigma-1)\alpha}{\sigma}$

當 $dt_2+e = \pi$ 時，也就是 $t_2=\pi/(2\beta)$ 時

$\omega_2(t_2) = a(b+c) = \alpha$；

剩下三個未知數 $a, b, c$，但上面三個方程式卻是相依，因為

$ab = \frac{\alpha}{\sigma}, ac = \frac{(\sigma-1)\alpha}{\sigma} \rightarrow a(b+c) = \alpha$。

因此，令 $c=1$，則

$a = \frac{\alpha(\sigma-1)}{\sigma}, b = \frac{\alpha}{a\sigma} = \frac{1}{\sigma-1}$。

以下就是完整的角速度曲線公式

$f(t) = \left\{ \begin{array}{1,1} \frac{\alpha}{\sigma} [1-\cos (\sigma \beta t)] & t \in \left[0, \frac{\pi}{2\sigma \beta} \right) \\ \frac{(\sigma-1)\alpha}{\sigma} \left[ \frac{1}{\sigma-1}-\cos \left( \frac{\sigma\beta}{\sigma -1}t + \frac{(\sigma-2)\pi}{2(\sigma-1)} \right) \right] & t \in \left[ \frac{\pi}{2\sigma \beta}, \frac{\pi}{2\beta} \right] \end{array} \right.$

因為 $\sin(\sigma\beta t-\pi/2) = -\cos(\sigma\beta t)$，而且

$-\cos \left( \frac{\sigma\beta}{\sigma -1}t + \frac{(\sigma-2)\pi}{2(\sigma-1)} \right) = \sin \left( \frac{\sigma\beta}{\sigma -1}t + \frac{(\sigma-2)\pi}{2(\sigma-1)} - \frac{\pi}{2} \right) = \sin \left( \frac{\sigma\beta}{\sigma -1}t - \frac{\pi}{2(\sigma-1)}  \right) = \sin \left( \frac{\sigma\beta}{\sigma -1} \left( t - \frac{\pi}{2}\frac{1}{\sigma\beta} \right)  \right)$

因此完整的角速度曲線公式，也可以寫成

$f(t) = \left\{ \begin{array}{1,1} \omega_1(t) = \frac{\alpha}{\sigma}\sin(\sigma\beta t-\pi/2) + \frac{\alpha}{\sigma} & t \in \left[0, \frac{\pi}{2\sigma \beta} \right) \\ \omega_2(t) = \frac{(\sigma-1)\alpha}{\sigma}\sin \left( \frac{\sigma\beta}{\sigma -1} \left( t - \frac{\pi}{2}\frac{1}{\sigma\beta} \right)  \right) + \frac{\alpha}{\sigma} & t \in \left[ \frac{\pi}{2\sigma \beta}, \frac{\pi}{2\beta} \right] \end{array} \right.$

接下來要找出上述角速度曲線加減速過程的累積角度，也就是上述曲線的積分。

$\int_0^{t_1} \omega_1(t) \text{d}t = \frac{\alpha}{\beta\sigma^2} (\frac{\pi}{2} - 1), t_1 = \frac{\pi}{2\sigma \beta}$

$\int_{t_1}^{t_2} \omega_2(t) \text{d}t = \frac{\alpha}{\beta} \frac{(\sigma-1)^2}{\sigma^2}  + \frac{\pi}{2\beta} \frac{\sigma-1}{\sigma}  \frac{\alpha}{\sigma},  t_2 = \frac{\pi}{2\beta}$