即時速度控制之空間性軌跡追蹤法(Real-time Velocity Control - SPTM,

經由上一小節的討論，我們知道空間性軌跡追蹤法(ε-ControllerorC_ε)是一種 運作在純量_ε 上的非線性控制器；它可以將二維的軌跡追蹤問題轉換成一個相對 於_ε的純量調整問題，因而有效地降低機器人平台進行軌跡追蹤的難度。

但是，經過實際的運用之後，我們發現此空間性軌跡追蹤法的控制架構中是 有可以改善的地方的。圖 25 中的 MSP(MakeSetPoint)演算法負責將ε-Controller所 產生的慣性速度指令轉換成機器人平台的輪子轉速指令，但這必須以輪子不發生 之空間性軌跡追蹤法」(Real-time Velocity Control - SPTM, RVC-SPTM)。

RVC-SPTM 與原本的空間性軌跡追蹤法(SPTM)比較起來，最大的改良之處在 於RVC-SPTM 中使用離散型極值三角搜尋法(DTS-ESA)的概念取代原本 SPTM 中 的MSP 演算法；我們稱之為 Modified-MSP(MMSP)演算法。

如圖 27 所示，MMSP 演算法是使用 DTS-ESA 來進行機器人平台的即時速度 控制；利用光流影像所偵測到平台的即時移動速度，然後迴授至 MMSP 演算法 中的DTS-ESA 進行控制，使得該機器人平台的實際移動速度能與C_ε所指定的慣 性速度指令一致；進而增加空間性軌跡追蹤法的精準度及實用性。

圖 27 RVC-SPTM 的控制架構圖

(5) 在「MMSP」階段，MMSP 會調整平台的速度，以使平台的實際速度趨近於C_ε 所計算出的目標速度。在此階段，為了維持軌跡追蹤的效率，我們將會限制MMSP 的調整次數；若 MMSP 在限定的次數之內尚未將實際速度調整至目標速度，此 時將會結束此階段的動作，直接回到C_ψ 階段。若 MMSP 在限定的次數之內將實 際速度調整至目標速度，此時則會進入下一階段「Move」進行定速移動。

(6) 在「Move」階段，因為平台的實際移動速度等同於目標速度，因此平台只需 進行定速運動；在持續了一段設定的時間之後，系統會判斷是否已經到達使用者 定義的軌跡終止點；若已到達終點，系統則進入下一個階段「Next Path」；若未 到達終點，系統則會回到C_ψ 階段。

(7) 在「Next Path」階段，因為平台已經到達某一段路徑的終止點，此時系統會 判斷使用者是否有指定下一段新的移動路徑，如果有指定新的路徑，系統則會回 到「Start」階段以執行新的軌跡追蹤任務，如果沒有指定的話，系統則會進入最 後階段「Stop」。

(8) 在「Stop」階段，因為平台已經到達軌跡終止點，因此平台會停止運動而停 留在該點。

圖 28 RVC-SPTM 的控制流程圖

2.2.5 利用 RVC-SPTM 進行碰撞偵測及動態路徑規劃

圖 29 碰撞偵測_1

圖 30 碰撞偵測_2

圖 31 碰撞偵測_3

因此，我們在程式中設計了一個用來計算平均速度大小及方向的副程式；當

(a) (b)

(c) (d)

(e) (f)

圖 32 動態路徑規劃_1

關於在避障路徑上再次碰撞到障礙物的議題，如果運動平台碰撞到如圖 33(b) 所示，是屬於寬扁型的障礙物時，此時系統所規劃的新的避障路徑將會有所不 同。圖 33(a)到(d)與圖 32(a)到(d)所描述的事情是相同的，只是其中的障礙物大 小形狀有所不同。如圖 33(e)所示，當平台沿著避障路徑前進時，將會再次與該 障礙物發生碰撞(注意，此時發生碰撞的地方是屬於避障路徑中的第 4 部分)。此 時，如圖 33(f)中橙色線段所示，系統將會再規劃一條避開障礙物的新路徑，如 此一來，該機器人平台將可以順利繞過該障礙物，並回到原先要求的路徑上，繼

因此，我們所設計的動態路徑規劃方法可以經由多次與障礙物發生碰撞而動 態地更改其避障路徑，以增加該避障功能的執行效率及成功率。

另外，圖 34 為加入碰撞偵測及動態路徑規劃方法後的 RVC-SPTM 新控制流 程，其中VM 代表速度量測(Velocity Measure)方法。

(a) (b)

(c) (d)

(e) (f)

圖 33 動態路徑規劃_2

圖 34 含碰撞偵測及動態路徑規劃之 RVC-SPTM 控制流程圖

第三章 光流影像偵測定位法

3.1 光流(Optical Flow)的定義

所謂的光流(Optical Flow)是指，對於兩張以上的連續影像中之物體，預測其 運動方向及大小。光流是透過比較兩張影像之間的影像強度而獲得，通常該兩張 影像之間必須擁有許多相同的內容，以保證該方法可以正常運作。

假設將攝影機裝設在某運動物體上，攝影機朝著地面進行拍攝，而該物體正 在地面上進行移動；當運動物體往某個方面移動某個距離時，攝影機所拍攝到的 地面影像則會有相對的變動，其移動的方向相反、距離成正比關係(視裝置狀況 決定)。因此，可以利用「光流」方法來偵測影像中物體的運動，因而推算出該 運動物體的真實運動狀況。

之所以會以光流做為計算運動位移的工具，主要是因為光流方法比較不容易 受到外界環境因素的影響，且不需要事先了解追蹤的物體與當時的環境狀況，因 此比較適合用於未知的環境來進行運動位移偵測。

圖 35 為一個光流向量場的示意圖，其中物體分別往正 X 軸和正 Y 軸方向各 移動了一個像素(pixel)的距離[11]。

圖 35 光流向量場

3.2 光流影像偵測定位法之原理[11]

為了得到該平台的速度和角速度值，我們可以將上述式子改寫成矩陣向量的 小平方誤差法(least square error method)來求得此方程式的最佳近似解，也就是此 平台的速度與角速度值(Vr x, ，Vr y, ，ω_r)；在得到平台速度與角速度之後，我們便

圖 37 光流軌跡視窗

目前方向

移動軌跡

第四章 全方位運動平台

4.1 簡介

全方位運動平台使用特殊結構的輪子，一般稱為全向輪(omni-directional wheel or omni-wheel)；它可以將地面作用於輪子上的摩擦力之中平行於輪軸的部 分，藉由滾子的轉動而分散掉；因而，造就了全向輪平台所擁有的獨特運動模式。

4.2 運動學分析

本平台使用三顆全向輪，如圖 39 所示，三輪之間各夾 120 度角，為這個移 動平台最大的一個特點，這種構造使得這個平台得以呈現較一般輪式平台更多樣 化的運動模式，在此將詳細探討這種構造的運動學理論[14-17]。

圖 39 全向輪平台簡圖

圖 40 全向輪簡圖

在圖 39 及圖 40 中，各項符號的意義表示如下：[14-17]

在x y z 座標上，各輪子之軸心端點的速度等於平台中心點的速度加上平台自

4.3 運動模式

(c) (d)

(e)

圖 41 運動模式示意圖

第五章 軟硬體設計與實現

5.1 系統架構

圖 42 為整個系統的架構圖，主要分成兩個部分：個人電腦端(PC)以及海星 號(Starfish)，而兩者之間則是透過無線網路機制來進行訊息的溝通傳遞。接下來 的兩個小節將分別針對兩者(PC and Starfish)進行詳細的介紹。

Optical-flow Localization Algorithm

Real-time Velocity Control Spatial Path Tracking Method

Wireless

Motor Controller Sensor Controller Power Supply

Motor * 3 Optical flow sensor * 2

Wireless LAN (IEEE 802.11b/g) TCP/IP

Battery

Starfish PC

圖 42 完整系統架構

5.2 全方向運動平台-海星號(Starfish)之架構

Point(無線網路基地台且包含 Switch Hub)、2 組嵌入式網路系統板(Embedded Ethernet) 、 1 組 馬 達 控 制 板 (Motor Controller) 、 1 組 感 測 器 控 制 板 (Sensor

5.2.1 電路功能介紹

底下將逐一介紹海星號上面各個電路架構的詳細功能，可參照圖 42 之完整 系統架構圖。

(1)嵌入式網路(Embedded Internet)板，如圖 44 所示；它可以將網路訊息封包解 開，並將資料內容從RS-232 port 傳送出去；或是將從 RS-232 port 所收到的訊息

(2)馬達控制(Motor Control)板，如圖 45 所示；它會根據 RS-232 port 所接收到的 控制指令內容去調整每一顆馬達裝置的PWM 控制訊號。

(3)感測器控制(Sensor Control)板，如圖 46 所示；它會解讀光流影像偵測器輸出 的Quadrature waveform 訊號，將之轉換成累積位移資訊，並且將此資訊內容從 RS-232 port 輸出。

(4)將 Futaba S9402 伺服機改裝後便成為全方向平台所使用的馬達裝置，如圖 47 所示。原本伺服機只能控制輸出角度，但經由將內部的可變電阻移除換上兩顆固 定電阻且磨除齒輪上的防過轉閂後，便可依照PWM 訊號來控制馬達轉速。

(5)Access Point 以及 Switch Hub 係使用 ASUS 出產的 WL-530g，如圖 48 所示，

它的最大傳輸率為54Mbps，符合 IEEE802.11g 的規格。

圖 44 嵌入式網路板

圖 45 馬達控制板

圖 46 感測器控制板

圖 47 馬達裝置

圖 48 AP(包含 Switch Hub)

(6)電源模組(Power module)採用三個 Step-down 穩壓模組，相較於一般常用的 78 系列降壓IC，Step-down 穩壓模組能更有效率地轉換能量以減少損失。電池(Battery) 使用三顆 7.4V 鋰聚合電池，比起鎳鎘、鎳氫電池，它的重量較輕、體積較小而 且容量較大。

(a) (b)

圖 49 降壓模組與電池組

(7)在這裡我們使用逸盛公司出產的迷你雷射滑鼠作為光流影像感測器(Optical flow sensor)；這個感測器擁有 1600 cpi (counts per inch)的解析度，並且相較於普 通的光學滑鼠，雷射滑鼠有幾項優點：1. 解析度較高 2.可在玻璃等透明物質上使 用3.不需與地面接觸。滑鼠改裝後將如圖 50 所示。

圖 50 光流感測器

(8)利用這個感測器所提供的「Quadrature Mode Output Waveform」，我們便可以得 知該感測器在X 軸和 Y 軸上面的移動資訊。以下列出 Quadrature Mode 的相關資 訊圖表：

圖 51 Quadrature Mode Output state

圖 52 Quadrature Mode Output waveform

最後，組裝完成後的嵌入式網路全方向運動平台內部電路如圖 53 所示，從 底部拍攝的樣子如圖 54 所示。

圖 53 內部電路圖

圖 54 底部照片

5.3 主機端架構

對照圖 42 中的 PC 端部分，PC 端程式中的 Real-time Velocity Control Spatial Path Tracking Method 部分是用來實現第二章所介紹的即時速度控制空間性軌跡 追蹤法、第四章所介紹的全方向運動平台演算法，而 Optical-flow Localization Algorithm 部分則是用來實現第三章所介紹的光流影像定位演算法。由於該機器

程式中的Optical flow Localization Algorithm 負責從網路接收兩個光流感測器 所量測到的平台之單位時間位移資訊，經過運算後，便可以將運動平台的位移距 離、旋轉角度、移動速度以及旋轉角速度等即時資訊傳達給程式中的 Real-time Velocity Control Spatial Path Tracking Method(RVC-SPTM)。而 RVC-SPTM 則會計 算出接下來平台的移動速度小大及方向，並透過無線網路傳輸該控制指令以控制 此全方向運動平台。

圖 55 Visual Basic 所建立之使用者圖形介面

圖 56 利用 DDE 將資料傳至 MATLAB 進行記錄

圖 57 利用 MATLAB 所繪製的速度及指向圖

圖 58 利用 MATLAB 所繪製的誤差及軌跡圖

第六章 實驗結果與討論

實驗2：

運動平台指向Y 軸，往正 X 軸移動 1 公尺(100cm)。

圖 60 光流定位軌跡圖(左)及平台移動示意圖(右)-實驗 2 表 2 光流定位誤差表-實驗 2

誤差

X 軸(cm) Y 軸(cm) 角度(degree)

3.19 1.00 -1

實驗3：

運動平台指向Y 軸，往 25.6 度方向移動 1 公尺(往負 X 軸移動 43.3cm，往正 Y 軸移動 90.3cm)。

圖 61 光流定位軌跡圖(左)及平台移動示意圖(右)-實驗 3 表 3 光流定位誤差表-實驗 3

誤差

X 軸(cm) Y 軸(cm) 角度(degree)

2.23 2.06 -4

實驗4： 方誤差求解法(least square error method)所產生的計算誤差…等，都會對光流影像 定位法造成一定程度上的影響。其中，光流感測器的裝置誤差可以使用精密的雷

實驗4： 方誤差求解法(least square error method)所產生的計算誤差…等，都會對光流影像 定位法造成一定程度上的影響。其中，光流感測器的裝置誤差可以使用精密的雷

在文檔中 使用光流影像感測的全方向運動平台之空間軌跡追蹤系統設計 (頁 34-0)