2021年7月4日 星期日

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

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


上圖的迴圈線迷宮,不管使用左手或是右手搜尋法則,都無法找到線迷宮的終點,更不用說是最短路徑了。若是輪型機器人上有編碼器,或是利用運動時間來計算行走距離時,就可以引用「電腦鼠走迷宮」的方法來找線迷宮的終點與最短路徑。

下圖是一個簡單的 5x5 電腦鼠迷宮的例子。右手邊是對應的線迷宮地圖,而左手邊是電腦鼠迷宮中習慣用的距離導向洪水演算法。當我們知道終點與起點的座標,以及路徑的組成時,洪水由終點出發,每經過一格,距離加 1,不管有沒有轉彎,直到洪水值出現在起點。此時,我們就可以從起點開始,藉由洪水數值,由大而小,反推回到終點,找到一條最短路徑。

接下來就剩下兩件事 1) 搜尋迷宮終點,以及 2) 建構地圖與路徑。線迷宮鼠(Line maze)與電腦鼠(Micromouse)走迷宮不同的是,線迷宮鼠的競賽並不知道終點座標的位置。因此,對於線迷宮鼠的迷宮,我們需要在走行的過程中,根據運動距離,1) 更新路口的座標(車頭為北方),以及 2) 確認路口的型態(參考下圖),並將這一些資料記錄在「地圖」資料中。當發現以左手或是右手法則搜尋終點時,當下的路口,所有的路徑都已經探索過時,就利用現有的地圖資料,找到鄰近尚未探索過的路徑後,再繼續以左手或是右手法則搜尋終點。這樣的做法,一直持續到抵達終點為止。

當我們以左手法則,在這一個簡單的迴圈迷宮搜尋終點,走到第 28 個路口時,發現這一個路口的路徑已經都探索過了,因此就可以利用儲存的地圖資料,右轉直走找到最近的第 5 個路口,還有一個標示為白色,尚未探索過的路徑。

回到這一條路徑後,繼續以左手法則探索,但到了第 23 個路口時,又發現這一個路口的路徑已經都探索過了,因此再利用儲存的地圖資料,直走左轉左轉,找到最近的第 18 個路口尚未探索過的路徑。接著在第 6 個路口時,又發現這一個路口的路徑已經都探索過了。再利用儲存的地圖資料,迴轉、直走、左轉,找到最近的第 17 個路口尚未探索過的路徑,當左轉後,就直接找到終點了。

沒有編碼器,也不計算距離...

但若是輪型機器人沒有編碼器,也無法計算距離時,有辦法避開這一些迴圈嗎?

還找不到一個好方法,待續...

部分解法

對於以下帶有迴圈的線迷宮(取自張育豪老師臉書照片),單用左手或是右手搜尋法則,都無法找到線迷宮的終點。如果我們加入以下的規則,

「當碰到連續的四次右轉(R)或是左轉(L) (即使中間穿插著直走(S)時),改變搜尋法則。」

以左手法則出發,就會有以下的結果。似乎有解,但還是無法對抗本文之前類似電腦鼠迷宮的例子,殘念...




2021年7月3日 星期六

樹狀線迷宮的搜尋與最佳路徑

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

這一類的「樹狀」線迷宮,我們可以藉由既定的 (https://www.pololu.com/file/0J195/line-maze-algorithm.pdf) 左手優先(左轉路徑優先、中間次之,右轉最後),或者是右手優先法則(右轉路徑優先、中間次之,左轉最後),讓輪型機器人從起點出發,紀錄每一個路口執行的動作(如左轉為’L’,直走為’S’,右轉為’R’),最後可以找到迷宮的終點。

以上圖為例,在每一個路口採用左手優先(左轉路徑優先、中間次之,右轉最後)的終點搜尋方法,出發時一定是直走,當第一次碰到路口時,選擇左轉,所以圖中會有「1L」的文字標示。依此類推,當左轉後第二次碰到路口時,還是選擇左轉,所以會有「2L」的文字標示。第三次碰到路口時,因為是死路,所以迴轉後,紀錄是「3B」的文字標示。最終找到終點時,以文字「28E」做為結尾。因此,每一個路口的動作記錄下來後,由起點到終點的路徑結果就是「LLBLSBLBLSLLLBLSBLLBLSLLBRLE」。


但上圖「樹狀」線迷宮中,由左手優先法則所記錄下來,由起點到終點的路徑,並非最簡單、最短的路徑,因為中間有太多不必要的迴轉(B)動作。因此,針對迴轉(B)動作,可以找出以下幾個簡化的規則,去除掉該路徑上的迴轉(B)動作,從而簡化出一個由起點到終點的「最短路徑」。

第一個規則是當碰到「左卜」路口,而且左轉的路徑為死路時,左手優先法則所得到的動作會是「LBL」,此時可以簡化為直走「S」,不需要左轉。
第二個規則是當碰到「左彎」路口,而且左轉的路徑為死路時,左手優先法則所得到的動作會是「LBR」,此時的路口,就可以簡化為迴轉「B」,不需要左轉。
第三個規則是當碰到「T字」路口,而且左轉的路徑為死路時,左手優先法則所得到的動作會是「LBS」,此時的路口,就可以簡化為右轉「R」,不需要左轉。


第四個規則是當碰到「右卜」路口,而且直走的路徑為死路時,左手優先搜尋法則所得到的動作會是「SBL」,此時的路口,就可以簡化為右轉「R」,不需要直走。
第五個規則是當碰到「右彎」路口,而且右轉的路徑為死路時,左手優先搜尋法則所得到的動作會是「RBL」,此時的路口,就可以簡化為迴轉「B」,不需要右轉。
第六個規則是在路徑簡化時才會碰到,相當於路口僅有直走路徑,而且直走後的路徑為死路,左手優先搜尋法則所得到的動作會是「SBS」,此時的路口,就可以簡化為迴轉「B」,不需要直走。


或許有人會有疑問,難道只有這六個規則嗎?的確是的,因為在路口時, BitRacer 能夠採取的動作只有「直走(S)」、「左轉(L)」、「右轉(R)」,以及「迴轉(B)」四種動作。如果要針對「迴轉(B)」的前一個動作,以及後一個動作,這三個動作組合來簡化,那麼只會有包含在六個規則中的「SBS」、「SBL」、「LBS」、「LBL」、「LBR」、「RBL」,以及沒有在規則中的「SBR」、「RBS」、「RBR」三種。只是沒有在規則中的這三種,在左手優先搜尋法則中,是不可能會出現的組合。

接下來利用 MakeCode 的環境來實現這一個做法。

步驟一:將「樹狀」線迷宮的範例路徑「LLBLSBLBLSLLLBLSBLLBLSLLBRLE」,記錄到「地圖資料」的文字變數中。並以按鈕B啟動本實作對應的主程式。完成主程式的條件有二個,1) 所有包含迴轉(B)的動作組合,皆已被六個路徑簡化規則取代,2) 路徑中不再包含迴轉(B)的動作。因此,主程式中以「地圖資料」的文字變數中包含文字 ”B” 作為執行迴圈的依據,然後再以「替換地圖資料」函式,來執行六個路徑簡化規則。


步驟二:完成「替換地圖資料」函式。這一個函式有「地圖」、「簡化路口」,以及「替換資料」三個輸入參數,還有一個傳遞結果的「Path」輸出參數。其中「地圖」變數,是用來傳遞現有可能需要簡化的紀錄路徑,「簡化路口」是用來傳遞簡化規則中的三個動作組合,而「替換資料」則代表用來取代「簡化路口」的動作資料。

函式一開始先將當「地圖」這一個輸入參數指定給「Path」變數,並且以「Path」變數中含有「簡化路口」的情況,作為持續執行迴圈的依據。

如果「Path」變數中含有「簡化路口」,那麼「簡化index」變數,用來記錄「簡化路口」第一次出現在「Path」變數中的最小索引值。舉例來說,如果「Path = ‘LLBLSBLBLSLLLBLSBLLBLSLLBRLE’」,而且「簡化路口 = ‘LBL’」,那麼「簡化index」這一個變數就會是 1,因為「Path」文字變數中第一次出現的 LBL,是從第2個字元開始,但是對於陣列(文字變數相當於字元陣列)而言,第2個字元的陣列索引值是 1。

接下來就是要將「Path」變數拆開成三部分,第一部分是「簡化路口」變數內容之前的內容,也就是陣列索引值由0開始,長度是「簡化index」數值的大小,將它指定給「TempPath1」變數。第二部分是「簡化路口」變數的內容,第三部分是「Path」變數去除第一與第二部分的內容,也就是陣列索引值由「簡化index」數值加上「簡化路口」變數的長度開始,長度是原有「Path」變數的長度減去「簡化index」數值,再減去「簡化路口」變數的長度,並且將這第三部分指定給「TempPath2」變數。

最後再將「Path」簡化成「TempPath1」、「替換資料」,以及「TempPath2」三個變數的組合。並重複以上的步驟,直到「Path」變數中不再含有「簡化路口」的內容。



思維與挑戰
在「替換地圖資料」函式中,用了不少程式碼,在「Path」變數中找出含有「簡化路口」規則的文字,並且將它替換成對應的文字,但有沒有更簡單的方法呢? 

在 MakeCode 的環境中,其實也可以利用 JavaScript 來寫程式。在 JavaScript 中,針對文字變數,有提供一個取代(replace)的方法,正好是我們所需要的功能。

以下圖為例,「Route」變數為 ”LLBLSE”, 「Best_route」變數用來儲存「Route」變數中,將 ”LBL” 文字替換成 “S” 的結果。在按鈕A按下後的程式內容中,有一個灰色的積木方塊,代表它是在JavaScript 的編寫環境下完成的。我們也可以利用MakeCode 環境中的偵錯模式,來檢查執行的結果是否如我們所預期喔。



用功的同學,你可以用這一個方法來重寫這一個實作的程式嗎?


2019年1月29日 星期二

電腦鼠的速度回授控制器設計 Velocity feedback controller of micromouse

這一篇是要配合鑑別出電腦鼠的系統特性時(直走或旋轉的動態),說明如何設計「速度回授控制器」的文章。

假設電腦鼠的系統特性如下,輸入是 PWM 數值,輸出是直線或角速度:

\( G(s) = \frac{K_m}{\tau_ms+1} \)

其中 $s$ 代表拉氏轉換的變數。

我們的位置控制器架構如圖。


因此這一個位置控制的命令到輸出之間的轉移函數可以寫成

\(  \frac{P_{com}(s)}{P(s)} = \frac{K_mK_p}{\tau s^2 + (1+K_mK_v)s + K_mK_p} = \frac{K_mK_p/\tau}{s^2 + s(1+K_mK_v)/\tau + K_mK_p/\tau} \)

這是一個標準的二階系統,因此可以透過 $s^2 + 2\zeta\omega_n + \omega_n^2$ 中選擇阻尼比 $\zeta$,以及自然頻率 $\omega_n$,來調整系統的反應,還有對應的增益 $K_p$ 與 $K_v$。

\( K_p = \frac{\tau\omega_n^2}{K_m},  K_v = \frac{2\zeta\omega_n\tau - 1}{K_m} \)



2019年1月24日 星期四

關於電腦鼠角速度命令曲線平滑化的討論 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} \)



2018年12月12日 星期三

2018年11月27日 星期二

自走車線軌跡的曲率偵測 Curvature calculation in Robotrace contests

假設自走車的直線速度是 $v_C$ 而且被控制為定值,角速度是 $\omega_C$,重心位置與前方紅外線感測器用來估測線軌跡誤差控制中心點的距離為 $L$。 那麼控制中心點的速度 $v_{LC}$ 的大小,以及它與自走車直線速度方向的夾角為 $\phi$,可以用下列的方程式算出來 \[ v^2_{LC} = v^2_C + (\omega_C L)^2 \] \[ \phi = \tan^{-1}\left(\dfrac{\omega_C L}{v_C}\right) \]
假設我們開始觀測的時間為 $t_0$,而且自走車的直線速度 $v_C$ 在 $t_0$ 到 $t$ 時間內的角度變化為 $\theta(t)-\theta(t_0)$。其中 \[ \theta(t) - \theta(t_0) = \int^t_{t_0} \omega_C(\tau) d\tau \] 
那麼控制中心點速度 $v_{LC}$ 在 $t_0$ 到 $t$ 時間內的角度變化 $\alpha$ 就是 \[ \alpha = \theta(t)-\theta(t_0) + \tan^{-1}\left(\dfrac{\omega_C(t) L}{v_C}\right) - \tan^{-1}\left(\dfrac{\omega_C(t_0) L}{v_C}\right) \]
如果控制中心點能夠一直沿著線軌跡沒有誤差的移動,那麼在時間 $t$ 時線軌跡的曲率半徑 $r$ 就可以利用下列方程式來估測 \[ r = \dfrac{l}{\alpha} \] \[ l = \int^t_{t_0} v_{LC}(\tau) d\tau \]
 另一個估測的方法是 \[ r = \dfrac{v_{LC}}{\dot{\alpha}} \] \[ \dot{\alpha} = \dfrac{d\alpha}{dt} = \omega_C + \dfrac{Lv_C}{v^2_C+(\omega_C L)^2} \dfrac{d\omega_C}{dt} \]
 這是因為 \[ \dfrac{d\tan^{-1}(x)}{dx} = \dfrac{1}{1+x^2} \] 但是當控制中心點無法一直沿著線軌跡沒有誤差的移動時,該怎麼修正呢? 目前的想法是 \[ r_t = r - \delta x \] 其中 $r_t$ 是修正後的估測半徑,$\delta x$ 是控制中心點與線軌跡間的誤差大小。

2017年12月24日 星期日

PD 控制的數位化設計

我們希望針對電腦鼠 PD 控制器的「數位化」做一些討論。 在 Peter 的文章 (Characterising the drive system on the micromouse) 中提到,電腦鼠的直走或旋轉運動,可以利用實驗資料找出對應的動態系統,還有對應的參數 $\tau_m$ 和 $K_m$ \[ G(s) = \frac{K_m}{\tau_ms + 1} \] 這一篇文章要討論的是 PD 控制器,以及它數位化後的結果。Peter 也有一篇類似的文章 Designing the motor controller 。整體的系統架構是 \[ e(s) -> K_p+K_ds -> \frac{K_m}{\tau_ms + 1} \] 其中 $e(s)$ 代表命令與位置輸出之間的誤差。 $G(s)$ 的狀態空間表示式是 \[ \dot{x}(t) = Ax(t) + Bu(t), \\ y(t) = Cx(t) \\ A = \begin{bmatrix} 0 & 1 \\ 0 & \frac{1}{\tau_m} \end{bmatrix} , B = \begin{bmatrix} 0 \\ 1 \end{bmatrix} , C = \begin{bmatrix} \frac{K_m}{\tau_m} & 0 \end{bmatrix} \] $G(s)$ 狀態空間表示式以取樣時間 $\Delta$ 數位化後的結果 $G(z)$ 是 \[ x[n+1] = Fx[n] + Gu[n], \\ y[n] = Hx[n] \\ F = e^{A\Delta}, B = , C = \begin{bmatrix} \frac{K_m}{\tau_m} & 0 \end{bmatrix} \]

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

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