2014年1月3日 星期五

Kato 與我關於電腦鼠角速度曲線平滑化的討論 Discussions of speed command profiles for micromouse to make turns

這一篇文章主要是要討論 Kato 先生關於電腦鼠轉彎過程角加速度曲線的想法

http://blog.livedoor.jp/robolabo/archives/51500936.html

\( \omega = \frac{1}{2} \left(1-\cos \left( \pi \frac{t(2.5-t)}{1.5} \right) \right), t \in [0, 1] \)



取微分得到角加速度

$\frac{d \omega}{dt}=\sin(\pi t(2.5-t)/1.5) \frac{\pi (2.5-2t)}{3}$

但看起來,角加速度的極大值並不容易有解析解可以求得。試試看找出角加速度的微分

\( \frac{d^2 \omega}{dt^2} = \cos \left( \pi \frac{t(2.5-t)}{1.5} \right) \frac{\pi^2(2.5-2t)^2}{4.5} - \frac{2\pi}{3} \sin \left( \pi \frac{t(2.5-t)}{1.5} \right) \)

可以看出即使令角加速度的微分為零,也不容易找出 $t$ 的解,進而找到角加速度的極大值。

找角加速度極大值的原因是希望能限制它的大小,以免造成電腦鼠在轉彎時的滑動。

事實上,Kato 的做法可以用以下的參數化方程式來討論,端看使用者喜歡哪一個

\( \omega(t)=\frac{1}{2} \left( 1-\cos \left(\pi t \frac{\gamma-t}{\gamma-1}\right)  \right),  \gamma \ge 2 \)

以下是與原始餘弦函數與不同的參數 $\gamma$ 值作了比較後的圖形

可以看得出來,$\gamma$ 值越大,曲線越平緩,越接近原始的餘弦函數。

其實這樣的一個做法是靠著調整時間軸 $t$ 由 0 到 1 的變動速度來達成,以平方函數來看 $t(\gamma-t)$,在 $t$ 由 0 到 $\gamma/2$ 的變動斜率 $\gamma-2t$,是由大到小。換句話說,第一段角速度的加速過程,也就是餘弦函數由 $0$ 到 $\pi/2$ 的這一段,會進行得比較快,而第二段角速度的加速過程,也就是餘弦函數由 $pi/2$ 到 $\pi$ 的這一段,則會相對進行得比較慢。但這樣的趨勢,會隨著 $\gamma$ 越大而越不明顯。

以這樣的想法來看,其實並不一定要平方函數,根號函數亦可。例如

\( \omega(t)=\frac{1}{2} \left( 1-\cos \left(\pi t^\gamma\right)  \right),  0.5 < \gamma < 1 \)

以下就是 $\gamma = 0.7, 0.8, 0.9$ 以及 Kato 做法的比較。


我的做法

另一個做法是分段使用餘弦函數,以 $\sigma$ 作為控制變數,$\sigma >1$,用來調整第一段緩加速的時間。

原始餘弦函數若是

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

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

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

把原始餘弦函數在 $t=0.5/\beta$ 的點,以及其對應的角加速度大小作為「控制點」,將它移動到 $t=0.5/(\sigma \beta)$ 的點。因此,為了保持角加速度的極大值為 $0.5\alpha \beta \pi$,就將第一段角速度曲線的緩加速定義成

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

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

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

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

那麼

\( d\frac{0.5}{\beta \sigma}+e=\frac{\pi}{2}, d\frac{1}{\beta}+e=\pi \)。

因此

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

當 $dt_1+e = 0.5\pi$ 時,

\( \omega_2(t_1) = ab = \frac{\alpha}{2\sigma} \);

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

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

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

當 $ct_2+d = \pi$ 時,

\( \omega_2(t_2) = a(b+c) = \alpha \);

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

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

因此,令 $c=1$,則

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

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

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



接下來應該是要找出角速度曲線加減速過程的累積角度,也就是上述曲線的積分。會有一些複雜,目前記錄在簡報擋上,下次再整理上來。

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

\( \int_{t_1}^{t_2} \omega_2(t) \text{d}t = \frac{\alpha}{2\sigma} (t_2-t_1) - \frac{(2\sigma-1)\alpha}{2\sigma} \frac{2\sigma-1}{\sigma\beta\pi} \left. \sin\left( \frac{\sigma\beta\pi}{2\sigma-1}t + \frac{(\sigma-1)\pi}{2\sigma-1} \right) \right|_{t_1}^{t_2} = \)

但我把模擬程式也寫好了。



迴圈線迷宮(looped line maze)的搜尋與路徑簡化

迴圈線迷宮(如下圖),專指一個由直交線段組成的迷宮中,包含「迴圈」的路徑。在每年教育部主辦的「 電腦鼠暨智慧輪型機器人競賽 」中,屬於高中職與大專組的「 線迷宮鼠 」競賽活動。規則請參考以下連結  https://sites.google.com/gm.lhu.edu.tw/20...