控制設計

本論文採用視覺伺服為 PBVS 架構，藉由計算出空間中實際的座標關係來對 機器人做控制，避免在 IBVS 架構中，雖然影像平面上的參考點已經到達所需的 位置，但是實際上機器人卻尚未移動到所需的位置，這種情況的發生。對於整個 移動式機械臂來說，攝影機所獲取的資訊是所有控制器的控制命令來源，包括頭 部、手臂、平台。從第三章我們得知，如何由對應的特徵點算出 3D 座標，接著 由 4.1 節跟 4.2 節得知機器人跟目標物之間的關係，來決定機器人的移動，導引 平台前進，並控制手臂抓取目標物。

與一般 PBVS 架構的不同地方在於本論文針對欲抓取的目標物並不是先建 立其模型，而是建立一張目標物的影像於資料庫中，作為我們一開始在第二章所 提到特徵點比對的依據，藉由整個目標物辨識的過程接續著第三章的座標估測，

可以算出所有對應點的 3D 座標。至於要如何從這群座標中挑選出抓取所需要的 參考點，我們引入了[28]所提到中心點(Center point)的概念，運用 Homography 可以框出在左右攝影機影像平面中目標物的位置，取這兩個框的中心點，當作我 們抓取所需的參考點，由這組對應的中心點所得到的 3D 座標，來當作機器人跟 目標物位置關係的依據，藉由 encorder 回傳手臂位置跟攝影機量測目標物的位置 之間的誤差是否已經小於某一個誤差值，來判斷是否完成抓取的任務。

控制器法則(control law)裡的行為模式如圖 4.19 所示，由於機器人並非一開 始就位於機械手臂的工作範圍之內，目標物也可能會移動，所以針對這些情況在 影像伺服的過程中設計了不同的模式，機器人根據目前辨識的結果選擇最適當的 模式來操作，增加機器人的智慧性。

圖 4.19 移動式機械臂工作模式流程圖

整體的系統架構，如圖 4.20 所示。將輸入的影像做特徵點擷取之後得到兩 群特徵點

[

^{xil yil}

]

^T以及

[

^{xir yir}

]

^{T，接著和資料庫特徵點}

[

^{xid yid}

]

^{T做第一次的特}

徵比對，得到兩組比對成功的特徵點

[

^{xil yil xid yid}

]

^T以及

[

^{xir yir xid yid}

]

^T，分別計算這兩組的特徵點的平面轉換矩陣，運用 RANSAC 將第一次比對錯誤的特徵點剔除，得到比對成功率更高的一組特徵點

T

il il id id

x y x y

 

  以及

T

ir ir id id

x y x y

 

  ，由這組特徵點算出平面轉換矩陣

之後，就能夠將目標物在左右攝影機影像平面的位置框出來，並以此區域的中心

[ ]

^T

標物的中心點在空間中的三維座標

[

^{xd yd zd}

]

^T，將這個座標加上目標物的三軸 旋轉角就可以組成一個完整的目標物 6D 姿態

[

^{xd yd zd φ θ ϕd d d}

]

^T，此外

由目標物座標可以算出平台的位置^_x_pd y_pd z_pd^_^T，作為平台移動的控制命 令。在控制迴路中，將手臂末端位置及平台位置的誤差輸入，轉換成修正誤差所 需給的命令，手臂方面是θ θ₁~ ₆^{，移動平台是}

[

^{xd yd zd}

]

^T，控制器法則(control law)裡的功能在於判斷機器人目前的狀態，選擇機器人要完成抓取任務目前所需 的行為模式，例如:要繼續移動平台，還是尋找目標物，手臂是否開始執行抓取 的動作等等。當移動抓取完成時，所有的位置都跟我們的估測結果一致，誤差達 到最小，任務完成。

第 第 第

第五 五 五 五章 章 章 章實驗結果 實驗結果 實驗結果 實驗結果

本章節的主要目的在驗證第二章及第三章介紹之特徵點擷取方法及座標估 測方法的確可使機器人辨識出目標物在空間中的位置，並且整合第四章所設計的 控制架構，達成移動機器人抓取功能的實現。

5.1 實驗用機器人構造 實驗用機器人構造 實驗用機器人構造 實驗用機器人構造

圖 5.1 為本實驗所使用之機器人，機器人的運算核心為工業電腦，攝影機在 擷取影像之後，辨識目標物及下達控制命令都是由電腦處理。機器人最上方為立 體攝

Stereo camera Two DOF robotic head

Six DOF robotic arm

IPC

Omni-directional

robotic platform

圖 5.2 Bumblebee2 立體攝影機

影機，實驗所使用的攝影機為 Point Grey Research, Inc.所生產的 Bumblebee2 立體 攝影機，他是透過 IEEE-1394 的傳輸介面跟電腦進行連結，左右眼的間距為 12cm。

攝影機裝置在實驗室自行設計的二自由度機械頭部上面，其下是機械手臂本 體，構造如 4.1.1 節所介紹，最下面為全向式移動平台，構造如 4.2.1 節所介紹。

機器人的硬體架構圖 5.3 如所示，以工業電腦(IPC)為運算核心，透過 IEEE-1394 跟立體攝影機傳輸擷取到的影像，而移動平台方面，則是透過 RS-232 傳輸，在機械手臂及頭部馬達的部分，則是使用 CANbus 傳輸控制命令。

在做實驗之前，我們需要得到攝影機參數，表 5.1 是原廠所附的攝影機參數 列表，而表 5.2 則是使用 Matlab Toolbox 所得到的攝影機參數列表。在本實驗中，

我們所需要用到的參數只有焦距(Focal Lengths)跟鏡心距離(Baseline)，這兩個參 數為計算目標物中心點之三維座標所需要的數值，而其他數值則是列出來讓讀者 參考。

表 5.1 Bumblebee2 立體攝影機之規格[29]

Specification BB2-03S2

Image Sensor Type Sony® 1/3” progressive scan CCD ICX424 (648x488 max pixels) 7.4µm square pixels

Baseline 12 cm

Focal Lengths 2.5mm with 97° HFOV or 3.8mm with 66° HFOV or 6mm with 43° HFOV

A/D Converter 12-bit analog-to-digital converter

White Balance Automatic / Manual (Color model) Frame Rates 48 FPS

Interfaces 6-pin IEEE-1394a for camera control and video data transmission

表 5.2 使用 Matlab Toolbox 得到的左攝影機內部參數 Intrinsic parameters of left camera

Focal Length [ 412.40757 417.62677 ]_±[ 9.67967 9.58434 ] Principal point [ 198.34122 108.10353 ]±[ 10.94986 11.51069 ] Skew [ 0.00000 ]±[ 0.00000 ]

=> angle of pixel axes = 90.00000±0.00000 degrees Distortion [ 0.26032 -0.26006 -0.02099 0.03091 0.00000 ]

±[ 0.07935 0.17035 0.01609 0.01570 0.00000 ] 表 5.3 使用 Matlab Toolbox 得到的右攝影機內部參數 Intrinsic parameters of right camera

Focal Length [ 418.08286 419.27400 ]±[ 9.92160 9.51373 ] Extrinsic parameters (position of right camera wrt left camera)

Rotation vector [ 0.04255 0.07063 0.00280 ]

(a) (b) 圖 5.4 (a)資料庫物件及(b)其特徵點位置

座標估測公式，求出目標物在空間中的座標，重複上述步驟，不斷的更新，當辨 識的結果穩定，小於我們所設定的閥值時，進入到抓取的模式，移動機械臂到目 標物前方，並做抓取路徑規劃，手爪沿路徑前進之後，闔上夾爪，完成任務。

第一個實驗的情境是機器人已經位於工作範圍內，此時藉由攝影機擷取目前 影像，算出目標物的三維座標，控制機械手臂抓取目標物。圖 5.5 所顯示的是攝 影機所觀察到目標物在影像平面的位置，透過本論文設計的目標物辨識方法，能 夠準確的框出目標物的位置。我們將啟動程式到抓取成功過程中，所計算出的目 標物中心點紀錄下來，若是有前後兩筆資料變動太大時，系統會將其視為無效的 資料，由圖 5.6 可以看出當機器人在固定位置時，所量測出來的中心點幾乎保持 不變，顯現出本論文所設計的目標物辨識方法，在實際運作時，有優良的表現。

由於量測出來的中心點有穩定的結果，圖 5.7 中，估測出來的座標呈穩定的結 果，沒有太大的變動。將上面所量測到穩定的座標位置，當作機械手臂的控制器 輸入，移動手爪到目標物前方，將目標物抓起，過程中並透過即時的估測，修正 手爪應該移動到的位置。圖 5.8 可以看出手臂抓取過程的軌跡，紅色為起點到預 抓取點間的路徑，而藍色則為最後抓取路徑的規劃。

移動到預抓取點(a)-(d)，(d)-(e)則是將手爪移動到讓目標物位在夾爪中心的地 方，最後將夾爪闔上舉起目標物。

圖 5.5 平台靜止時，攝影機所擷取的畫面及目標物位置的計算結果

圖 5.6 量測出之目標物在影像平面上的中心點

圖 5.8 抓取過程中手臂各軸軌跡圖

(a) (b) (c)

5.3 平台移動抓取實驗 平台移動抓取實驗 平台移動抓取實驗 平台移動抓取實驗

在本實驗中，我們一樣在機器人前方放置一個瓶子，作為抓取的目標物，而 這次瓶子放置在離機器人約 93 公分處，離地高度為 105 公分，這次的實驗主要 目的在於驗證，當機器人判斷目標物所在位置超出機械手臂工作範圍時，他會控 制移動平台向前移動到機械手臂的工作範圍之內，再進行最後的抓取任務。

將整個實驗過程中，從開始到完成抓取動作，所計算出目標物的中心點在影 像平面上的位置記錄下來。在起點時攝影機擷取到的畫面如圖 5.10，此時計算 出目標物的中心點位置如圖 5.11，是在整個影像平面上半部的位置。由於此時 算出機器人跟目標物的距離超出手臂工作範圍，於是計算出要移動的距離之後，

控制平台往前移動。到達工作範圍後，攝影機所截取到的畫面如圖 5.12，此時 紀錄的中心點位置如圖 5.13，中心點在影像平面上的位置向下移動。由於攝影 機傾斜的角度固定，機器人在移動的過程中，中心點在影像平面上的位置也有所 變化，目標物在影像平面上的位置隨著距離的減少，y 值有所變動，而 x 值則變 化不大，顯示出機器人是往正前方移動。在座標量測方面，隨著機器人的移動，

目標物在影像平面上的位置有所變動，大致分為兩個區間，分別是在起點時，及 到達工作範圍之後，所以在圖 5.14 中數據有一個跳動，就是對應到當時所計算 出的目標物中心點位置。圖 5.15 則是手臂移動的軌跡。

圖 5.11 攝影機在起點所量測出之目標物在影像平面上的中心點

圖 5.12 移動抓取實驗中，到達工作範圍時攝影機所擷取的畫面及目標物位置

圖 5.13 攝影機在工作範圍所量測出之目標物在影像平面上的中心點

圖 5.14 攝影機量測到的目標物座標位置

圖 5.15 抓取過程中手臂各軸軌跡圖

實際機器人移動抓取時的情形則如圖 5.16 (a)-(i)所示，(a)-(d)為機器人移動 的過程，在機器人判讀尚未到達手臂的工作範圍之前，控制器會持續的下命令給 平台，使其移動，透過影像資訊的輸入，再即時的計算是否到達工作範圍，到達 後平台才會停止。根據估測出來的目標物位置，來判斷機器人移動的距離，在本 實驗中，目標物所對應的適當工作範圍大致在深度為 600mm 的距離之內，一開 始攝影機估測出距離大於 600mm 所以根據目前位置到工作範圍之間的距離，下

(a) (b) (c)

(d) (e) (f)

(g) (h) (i)

其移動的軌跡紀錄如圖 5.17 所示，由於我們將世界座標原點定在機器人本 體，所以畫出來的移動軌跡圖，為一個相對的座標關係。由距離紀錄可以得知，

機器人在出發點所量測到的深度是 932mm，於是機器人應該向前移動的距離應 該是 332mm，其中移動的誤差可以根據攝影機計算的結果再作調整，根據移動

機器人在出發點所量測到的深度是 932mm，於是機器人應該向前移動的距離應 該是 332mm，其中移動的誤差可以根據攝影機計算的結果再作調整，根據移動

在文檔中 基於立體視覺之移動式機械臂影像伺服設計 (頁 69-0)