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} \]

2017年10月1日 星期日

加速規信號的零點校正

這一個問題緣起於我們希望對四旋翼直升機上的加速度信號做「零點校正」。

假設從 MPU6500 取得的加速度信號分別是 $x_i$, $y_i$, 以及 $z_i$,這三個軸未知零點的數值分別是 $x_0$, $y_0$, 以及 $z_0$,而且重力加速度 $g$ 不變。

如果我們量測三次,得到 $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, $(x_3, y_3, z_3)$ 三組數據,那麼我們就可以有以下三個關係式
\[ (x_1-x_0)^2+(y_1-y_0)^2+(z_1-z_0)^2 = g^2, \] \[ (x_2-x_0)^2+(y_2-y_0)^2+(z_2-z_0)^2 = g^2, \] \[ (x_3-x_0)^2+(y_3-y_0)^2+(z_3-z_0)^2 = g^2. \]
第一式減去第二式可以得到
\[ x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 = 2(x_1-x_2)x_0 + 2(y_1-y_2)y_0 + 2(z_1-z_2)z_0 \]
第一式減去第三式可以得到
\[ x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 = 2(x_1-x_3)x_0 + 2(y_1-y_3)y_0 + 2(z_1-z_3)z_0 \]
第二式減去第三式可以得到
\[ x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2 = 2(x_2-x_3)x_0 + 2(y_2-y_3)y_0 + 2(z_2-z_3)z_0 \]

寫成矩陣的形式
\[
\left[
\begin{array}
 .2(x_1-x_2) & 2(x_1-x_3) & 2(x_2-x_3) \\
 2(y_1-y_2) & 2(y_1-y_3) & 2(y_2-y_3) \\
 2(z_1-z_2) & 2(z_1-z_3) & 2(z_2-z_3)
\end{array}
\right]
\left[
\begin{array}
 .x_0 \\
 y_0 \\
 z_0
\end{array}
\right] =
\left[
\begin{array}
 .x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 \\
 x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 \\
 x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2
\end{array}
\right]
\]

因此,這三個軸未知零點的數值 $x_0$, $y_0$, 以及 $z_0$,就可以利用以下的公式求出來,當然前提是反矩陣存在。
\[
\left[
\begin{array}
 .x_0 \\
 y_0 \\
 z_0
\end{array}
\right] =
\left[
\begin{array}
 .2(x_1-x_2) & 2(x_1-x_3) & 2(x_2-x_3) \\
 2(y_1-y_2) & 2(y_1-y_3) & 2(y_2-y_3) \\
 2(z_1-z_2) & 2(z_1-z_3) & 2(z_2-z_3)
\end{array}
\right]^{-1}
\left[
\begin{array}
 .x_1^2 + y_1^2 + z_1^2 - x_2^2 - y_2^2 - z_2^2 \\
 x_1^2 + y_1^2 + z_1^2 - x_3^2 - y_3^2 - z_3^2 \\
 x_2^2 + y_2^2 + z_2^2 - x_3^2 - y_3^2 - z_3^2
\end{array}
\right]
\]

只是從 MPU6500 取得的加速度信號通常很髒,因此 $(x_1, y_1, z_1)$, $(x_2, y_2, z_2)$, $(x_3, y_3, z_3)$ 這三組數據,究竟應該是濾波後的結果,還是不需要呢?另外,反矩陣的計算其實對寫程式而言不是很友善,因此如何利用其他的演算法(如 Recursive Least Square的方法)來實現,也是值得討論。


2017年4月21日 星期五

一階的數位 IIR 低通濾波器

這是一篇網路上可以參考的文章。

First Order Digital Filters - An Audio Cookbook

這一個數位濾波器的數學式是以下的樣子 (直流增益值為 1) \[ y_n = ay_{n-1} + (1-a)u_n \] 其中 $y_n$、$y_{n-1}$ 與 $u_n$ 分別代表濾波器現在的輸出值、濾波器過去一個取樣時間的輸出值,和現在的原始信號。

 $a$ 這一個係數定義成 \[ a = e^{-\pi f/F_n} \] 
其中 $F_n$ 是取樣頻率 $F_s$ 的一半,$f$ 代表設定的頻寬。

畫出對應的頻率響應圖,的確一如預期。

Matlab 的程式碼

% constants
Fs=20000;       % sampling rate
t=1/Fs;         % sampling interval
Fn=Fs/2;        % Nyquist frequency
numPts=2^13; % number of points of analysis
%
n = [0.002 0.01 0.05 0.1 0.2];
a = exp(-pi*n*Fn/Fn);
%
% First order IIR filter
% y(n) = a*y(n-1) + (1-a)*u(n)
% find frequency response
%
h1 = figure(1);
set(h1,'color','white');
for i = 1:5
    [h,w] = freqz([1-a(i) 0],[1 -a(i)], numPts);
    % calculate dB values, for power quantities
    y(:,i) = 20*log10(abs(h));
end;
semilogx(w*10000/pi, y,'-.','LineWidth',2);
ax3 = gca;
set(ax3,'fontsize',14,'linewidth',1.0);
grid;
xlabel('frequency in Hz','fontsize',16);
ylabel('Gain in dB, 20log_{10}(Gain)','fontsize',16);
title('Frequency Responses, y_n = ay_{n-1} + (1-a)u_n, a=exp(-\pif/F_n), F_n=10000Hz','fontsize',16); 
axis([2 10^4 -45 5])
l1=legend('f = 20Hz','f = 100','f = 500','f = 1000','f = 2000','Location','SouthWest')
set(l1,'fontsize',16);


但為何如此呢?

我知道了!其實上述的想法,並非是一個公式,而是近似如此(工程師觀點)!

為了證明這樣的觀點,我自己針對特定的頻率值 $\pi f/F_n$ 把頻率響應在認為是3dB頻寬與實際值做了一下比較

\[ \frac{Y(z)}{U(z)} = \frac{1-e^{-\pi f/F_n}}{z-e^{-\pi f/F_n}} \]

其中 $z = e^{-j\Omega}$ ,而且 $\Omega = e^{-j\pi f/F_n}$。

結果如下:



MATLAB 程式

h2 = figure(2);
set(h2,'color','white');
aa = (0.02:0.02:0.4)*pi;
bb= 1-exp(-aa);
cc=exp(j*aa)-exp(-aa);
dd = abs(bb)./abs(cc);
plot(aa,dd,'--o','LineWidth',2);grid;
ax2 = gca;
set(ax2,'fontsize',14,'linewidth',1.0);
axis([0.02*pi 0.4*pi 0.4 0.8]);
xlabel('Values of \pif/F_n','fontsize',16);
title('Ratio of abs(1-exp(\pif/F_n))/abs(exp(-j\pif/F_n))-exp(-\pif/F_n))','fontsize',16); 


當 $f/F_n$ 在 0.4 以下,也就是 $\pi f/F_n$ 在 1.26 以下時,這一個數位低通濾波器關於增益的頻率響應,在 $\pi f/F_n$ 的位置上的確是在 3dB (0.707) 左右的位置。

由上圖可以看出,這樣的想法,在 $\pi f/F_n$ 越來越大時,會越不準!只是因為是低通濾波器的關係,以工程師的觀點來看,應該還是可以接受的!

但究竟是如何看出這樣的關係呢?

想的出來的話,鐵定功力大增!

很吃力的一篇技術文章!

2017年4月10日 星期一

數位信號處理教學心得 part I - IIR低通濾波器設計

這是一個實際處理數位信號的例子,我覺得應該有助於整合上課學習到的概念。

左上角的藍色波形,是學生錄製的人聲 DO。
右上角是原始聲音的頻譜分析,以及 6 階 butterworth 低通濾波器的增益頻譜圖形。
左下角是將原始信號經過 6 階 butterworth 低通濾波器後的波形,振幅有些縮小了。
右下角是濾波過後信號的頻譜分析,配合右上角的的圖形,希望學生可以很容易理解低通濾波器針對信號頻譜的相乘作用。(因為對現性非時變系統而言,輸出信號是由輸入信號與系統的脈衝響應做摺積而得,也可以看成是輸入信號的頻譜與系統轉移函數的頻譜相乘)

以下是 MATLAB 的原始程式碼,其中也包含如何在微控制器中去實現對應的低通濾波器,只是程式中並沒有考慮整數運算的限制,全都是浮點數運算。

clear;
clc;
%
Fs = 44100;
y1 = audioread('d974323027.wav');
u = y1(1:44101,1);
n1=1:max(size(u));
tt=n1/Fs;
yy = [tt' u];
  
[b,a]=butter(6,0.03);
sys = tf(b, a, 1/Fs);

bs = max(size(b));
sb=zeros(size(u));
for k = bs:max(size(u))
    sbk = sb(k-1:-1:k-6);
    uk = u(k:-1:k-6);
    sb(k) = -a(2:7)*sbk+b*uk;
end

sz = max(size(sb));
u2 = u(15001:25000);
sb2= sb(15001:25000);
fu = fft(u2);
fb = fft(sb2);
df = 0:Fs/max(size(u2)):(max(size(u2))-1)*Fs/max(size(u2));
dfb= 0:Fs/max(size(sb2)):(max(size(sb2))-1)*Fs/max(size(sb2));
[mag, phase]=bode(sys, df*2*pi);
mag2 = squeeze(mag);

h = figure(1);
set(h,'color','white');
n2=1:max(size(sb));
subplot(221);plot(n1,u,'r');axis([4000 14000 -0.8 0.8]);
ax1 = gca;
set(ax1,'fontsize',14,'linewidth',1.0);
title('Original sound signal','fontsize',16);
xlabel('points','fontsize',16);
%
subplot(223);plot(sb,'b');axis([4000 14000 -0.8 0.8]);
ax3 = gca;
set(ax3,'fontsize',14,'linewidth',1.0);
title('Filtered sound signal','fontsize',16);
xlabel('points','fontsize',16);
%
subplot(222);
[ax,h1,h2]=plotyy(df(1:400),abs(fu(1:400)),df(1:400),mag2(1:400));
grid;
axis(ax(1),[0 1500 0 1200]);
axis(ax(2),[0 1500 0 1.2]);
set(ax(1),'fontsize',14,'linewidth',1.0);
h1.Color = 'blue';
h1.LineWidth = 1;
h2.LineWidth = 1;
set(ax(2),'fontsize',14,'linewidth',1.0);
title('Spectrum of the original sound signal and filter','fontsize',16);
xlabel('Hz','fontsize',16);
ylabel(ax(2),'Gain','fontsize',16);
%
subplot(224);
p4 = plot(dfb,abs(fb),'b');
axis([0 1500 0 1200]);
set(p4,'linewidth',1);
ax4 = gca;
set(ax4,'fontsize',14,'linewidth',1.0);
grid;
title('Spectrum of the filtered sound signal','fontsize',16);
xlabel('Hz','fontsize',16);


2016年7月31日 星期日

相位超前補償器 Phase Lead controller design for DC motors

假設直流有刷馬達由輸入電壓(V)到位置輸出(pulses)的轉移函數如下
Assume the following dynamic function stands for transfer function of DC brush motor input voltage (V) to the position output (pulses)

\[ G(s) = \frac{K_m}{s(1+\tau_m s)}, \]

其中 $k_m$ 是穩態增益,$\tau_m$ 是時間常數。

假設相位超前補償器的形式如下:
The form of phase lead controller $C(s)$ is

\[ C(s) = \frac{1+T_zs}{1+\alpha T_z s} \]

我們的目標是利用 $\alpha$ 和 $T_z$ 這兩個參數來調整閉迴路控制系統的迴路增益(Loop Gain) $C(s)G(s)$ 波德圖的零交越點 $\omega_c$ 頻率,以及相位邊限 $\phi_c$ (phase margin)。

設計參數是零交越點 $\omega_c$ 頻率,以及相位邊限 $\phi_c$ (phase margin)。因此先確認相位超前補償器在 $\omega_c$ 頻率處,可以提供多少相位

\[ \phi_m(\omega) = \angle C(j\omega) = tan^{-1}(\omega T_z) - tan^{-1}(\alpha\omega T_z)\]

其中 $\phi_m(\omega)$ 的最大值發生在微分等於 0 的頻率,也就是我們要的零交越點頻率 $\omega_c$。因此

\[ \frac{d\phi_m(\omega)}{d\omega} = \frac{T_z}{1+\omega^2T_z^2} - \frac{\alpha T_z}{1+\alpha^2\omega^2T_z^2} \]

也就是

\[ \frac{T_z}{1+\omega_c^2T_z^2} = \frac{\alpha T_z}{1+\alpha^2\omega_c^2T_z^2} \Rightarrow \alpha^2\omega_c^2T_z^2 - \alpha (1+\omega_c^2T_z^2) + 1 = 0 \Rightarrow (\alpha\omega_c^2T_z^2 - 1)(\alpha - 1) = 0 \]

由於 $\alpha \ne 1$ (否則$C(s)=1$),所以

\[ \alpha = \frac{1}{\omega_c^2T_z^2} \Rightarrow \omega_c = \frac{1}{\sqrt{\alpha}T_z} \]

這時提供的最大相位是

\[ \phi_{max} = \max_{\omega} \phi_m = \phi_m(\omega_c) = tan^{-1}(\frac{1}{\sqrt{\alpha}}) - tan^{-1}(\sqrt{\alpha}) \]

2015年11月12日 星期四

利用車頭角速度來分類賽道的曲率半徑

剛完成利用車頭角速度來分類賽道曲率半徑的韌體程式。實在有些辛苦,因為我熟悉的模擬環境是 MATLAB,可是 MATLAB 程式語言的語法和韌體程式的 C 語言有些不同,最明顯的是 if-then-else 的結束,MATLAB 程式語言用的是 end,而 C 語言則是 },MATLAB 程式語言中陣列索引的起始值是 1,而 C 語言則是 0。因此程式的轉換得花一些時間。
韌體程式的狀態機圖

MATLAB 程式與檔案

2014年11月28日 星期五

另一個使用加速計與低解析度編碼器的位置估測法則 A method for estimating position with accelerometer and low resolution encoders


以下的方法,可以和 Kojima 先生的做法比較 The following method could be compared with the one proposed by Kojima san.

假設位置資料是 $x_1$,速度資料是 $x_2$,加速度資料是 $a$。那麼以狀態空間的方式來描述的話,就會是以下形式 Assume that $x_1$, $x_2$, and $a$ stand for position, velocity, and acceleration, respectively.  The following state equation describes their relationship.

$\left[ \begin{matrix} \dot{x_1} \\ \dot{x_2} \end{matrix} \right]=\left[ \begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix} \right]\left[ \begin{matrix} x_1 \\ x_2 \end{matrix} \right] + \left[ \begin{matrix} 0 \\ 1 \end{matrix} \right] a$

如果位置資料的估測值是 $\hat{x}_1$,速度資料的估測值是 $\hat{x}_2$。那麼以下的狀態空間動態方程式,就可以用來估測位置資料,並且具備較高的解析度。這是利用 Luenberger observer 的概念來做的。Assume that $\hat{x}_1$, and $\hat{x}_2$, stand for position, and velocity estimations, respectively.  The following dynamic equation derived from the idea of state observer could be used to estimate the position and velocity signals with better resolution and accuracy.

$\frac{d}{dt} \left[ \begin{matrix} \hat{x}_1 \\ \hat{x}_2 \end{matrix} \right]=\left[ \begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix} \right]\left[ \begin{matrix} \hat{x}_1 \\ \hat{x}_2 \end{matrix} \right] + \left[ \begin{matrix} 0 \\ 1 \end{matrix} \right] a + \left[ \begin{matrix} g_1 \\ g_2 \end{matrix} \right] (x_1 - \hat{x}_1)$

$g_1$ 與 $g_2$ 的設計可以用 $g_1=2\zeta \omega_n$,$g_2=\omega_n^2$,來設計,$\zeta$ 是 damping ratio ,$\omega_n$ 可以看成頻寬,$\zeta$ 可以選擇 [0.7, 1] 的範圍。Gains of $g_1$ and $g_2$ can be adjusted by using second order systems.

以下就是假設編碼器的解析度是 16 pulses/r 的模擬結果。其中加速度的資料來自加速規,而且我加了一些雜訊已接近真實情況。目前看起來還不錯。接下來就是數位化的工作。The following SIMULINK behavior model assumes that the resolution of a encoder is 16 pulses/r, and acceleration signal is added some white noise.


當我把編碼器輸出信號的單位弄錯時,下圖是錯誤的模擬結果,速度信號不對。The following incorrect results come from the wrong setting of the unit of position signals.  The estimated velocity signal does not match the true one.

修正過後的結果,可以看出位置與速度都正確的跟上了。The followings are correct results.

放大來看,可以看出位置信號的解析度的確增加了,加速規雜訊的影響也不大。

整個估測器的 SIMULINK 行為模型。

產生模擬信號的SIMULINK 行為模型。

離散時間的模擬看起來也可以了,取樣時間設定為 $T_s$ = 1ms, Discrete time simulations with $T_s$ = 1ms seems OK now.



2014年11月11日 星期二

一個電腦鼠運與自走車運動控制的新方法 A new method for the motion control of micromouse and robotracers

為了解決因編碼器解析度帶來差分的雜訊問題,我做了一個二階的位置與速度估測器,並且用這一些估測值來控制電腦鼠運與自走車的位置與速度。
A second order filter is devised in this article to estimate the position and velocity of wheeled mobile robots such that the noise effect arose from difference methods can be lessened.




我在 MPLAB 中利用以下的副程式實現了這樣一個演算法 The algorithm is implemented in the MPLAB environment

//--------- Functions -----------------------------------------
// Function Name : EST_FILTER
// Description   : 2nd order filter for vc and w
// input              : struct ESTIMATOR_S est_ps, or est_ths
// output            : struct ESTIMATOR_S est_ps, or est_ths
//-------------------------------------------------------------
struct ESTIMATOR_S EST_FILTER(struct ESTIMATOR_S states, long ref)
{
//long er_estp2;

states.yv.n = states.yv.n1 + states.erv.n1;
states.yp.n = states.yp.n1 + states.yv.n1;
states.er = ref - states.yp.n;
states.erv.n = (states.er>>2) - states.yv.n;
states.erv.n1 = states.erv.n;
states.yv.n1 = states.yv.n;
states.yp.n1 = states.yp.n;

return (states);
}

struct ESTIMATOR_S { union POSITION yp; struct FILTER_S yv; struct FILTER_S erv; long er; };

struct FILTER_S { int n; int n1; };

union POSITION { struct { long n; // in pulses, 81 pulses ~ 1mm, Q26.5 long n1; }; struct { unsigned int Ln; unsigned int Hn; unsigned int Ln1; unsigned int Hn1; }; } ;

其中 ref 代表真實的編碼器位置信號,states.yp.n 與 states.yp.n1 分別表示現在與過去一個取樣時間的編碼器位置信號估測值,states.yv.n 與 states.yv.n1 分別表示現在與過去一個取樣時間的編碼器速度信號估測值,states.er 是編碼器位置信號真實值與估測值之間的誤差。

states.erv.n = (states.er>>2) - states.yv.n;

上述方程式中的 states.erv.n 是使用編碼器位置信號真實值與估測值之間的誤差乘以 P 增益 0.25,加上編碼器速度信號估測值乘以 D 增益 1的結果,然後再積分得到編碼器速度信號估測值。

states.yv.n = states.yv.n1 + states.erv.n1; $\leftarrow y_v[n] = y_v[n-1] + erv[n-1]*T_s$

真正執行程式時,為了提高解析度,我將編碼器位置信號真實值放大 32 倍。

est_psR = EST_FILTER(est_psR, (out_pR.n<<5));
est_psL = EST_FILTER(est_psL, (out_pL.n<<5));

做位置控制的程式段

// find feedback variables
POS_con.pQEI = ((est_psR.yp.n + est_psL.yp.n)>>1);
ANG_con.pQEI = (est_psR.yp.n - est_psL.yp.n);

// find controlled errors
POS_con.perr = POS_con.pcom - POS_con.pQEI;
ANG_con.perr = ANG_con.pcom - ANG_con.pQEI;

// find motor command ->  $K_pe_p - K_v v_c^e$, and $K_p\theta_p - K_v \omega_c^e$
POS_PWM = (POS_con.perr>>1) - 9*((est_psL.yv.n+est_psR.yv.n)>>1);
ANG_PWM = (ANG_con.perr>>2) - 4*(est_psR.yv.n-est_psL.yv.n);

// find right and left motor commands
R_PWM = POS_PWM + ANG_PWM;
if (R_PWM>PWM_100) R_PWM = PWM_100;
else if (R_PWM<-PWM_100) R_PWM = -PWM_100;
//
L_PWM = POS_PWM - ANG_PWM;
if (L_PWM>PWM_100) L_PWM = PWM_100;
else if (L_PWM<-PWM_100) L_PWM = -PWM_100;

實驗結果 Experimental results for center and angular velocity




2014年10月29日 星期三

二次曲線內差法 Second order interpolation algorithm

假設我們有以下三組資料 assume that we have the following 3 pairs of data,
$(x_1-\Delta, y_0)$, $(x_1, y_1)$, and $(x_1+\Delta, y_2)$

利用一個一元二次方程式來表示通過這三個點的曲線,參數為 $a, b, c$
A second order equation is used to represent a curve that passes these 3 points

$y_0 = a(x_1-\Delta)^2 + b(x_1-\Delta) + c$
$y_1 = ax_1^2 + bx_1 + c$
$y_2 = a(x_1+\Delta)^2 + b(x_1+\Delta) + c$

以下討論如何利用這三個點以及內差法,來找出當 $x=x_1+\alpha\Delta$ 的 y 值
The following discussion is used to predict what the $y$ value is, when $x=x_1+\alpha\Delta$

由於 Since

$y_0 = a(x_1-\Delta)^2 + b(x_1-\Delta) + c = y_1 - 2a\Delta x_1 + a\Delta^2 - b\Delta$
$y_2 = a(x_1+\Delta)^2 + b(x_1+\Delta) + c = y_1 + 2a\Delta x_1 + a\Delta^2 + b\Delta$

因此 Therefore

$y = a(x_1+\alpha\Delta)^2 + b(x_1+\alpha\Delta) + c = y_1 + \alpha(y_2-y_1) - \alpha(1-\alpha)a\Delta^2$

至於變數 $a$,可以利用以下關係求出來
and the variable $a$ can be found by using the following equation

$a = \frac{y_0+y_2-2y_1}{2\Delta^2}$

只是計算時,可以考慮使用 $a\Delta^2$,
It would be easier in calculation if we use the following equation

$a\Delta^2 = 0.5(y_0+y_2-2y_1)$

也就是說

$y = y_1 + \alpha(y_2-y_1) - 0.5\alpha(1-\alpha)(y_0+y_2-2y_1)$

其中的 $y_1 + \alpha(y_2-y_1)$,就是線性內差的結果,而 $- 0.5\alpha(1-\alpha)(y_0+y_2-2y_1)$ 可以看成是二次內差法針對一次內差法的修正項。

但也有可能我們必須求出當 $x=x_1-\Delta + \alpha\Delta$ 的 y 值,也就是自變數 $x$ 的值落在建表自變數的第一個與第二個之間的數值 It may be possible that we have to find the interpolation value that corresponds to $x=x_1-\Delta + \alpha\Delta$.  This would occur when the $x$ value lies in the interval between the first and the second $x$ value of the table.

$y = a(x_1-\Delta+\alpha\Delta)^2 + b(x_1-\Delta+\alpha\Delta) + c = y_0 + (1-\alpha)(y_1-y_0) - \alpha(1-\alpha)a\Delta^2$

可以看出修正項是一樣的。


2014年8月14日 星期四

陀螺儀與編碼器濾波後信號的比較 (Comparisons of gyro and filtered encoder signals)

爲了能夠得到比較乾淨的「車頭角速度」估測,我需要一個乾淨的「車中心角速度」。
To get a clean estimate of 'head angular velocity' (The place where my Beetle robotracer is controlled to follow the line), I need a clean body angular velocity.

基於使用 PD 控制來調整循跡時的角速度,我試了兩個方法,一個是用編碼器濾波後的兩輪速度信號相減 (右輪減左輪),另一個是使用陀螺儀。
Based on PD control algorithm to adjust the angular velocity command to follow the line, I tried two methods to obtain the body angular velocity.  The first one is to use filtered encoder signals (right wheel velocity - left wheel velocity), and the other one is to use gyro signals.

可惜這兩個信號都不夠乾淨,以下是實驗結果。
It is a pity that both methods are not good enough, and the experimental results are shown in the following figure.



恐怕我得試試「基於誤差大小的變動比例增益」控制方法來循跡了。
I am afraid that I have to use gain scheduled proportional control based on the line following error to reach my goal.


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

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