基於立體視覺之移動式機械臂影像伺服設計

國立交通大學

電控工程研究所

碩士論文

基於立體視覺之移動式機械臂

影像伺服設計

Visual Servo Design of a Mobile Manipulator

Based on Stereo Vision

研 究 生：張哲豪

指導教授： 宋開泰 博士

林清安 博士

基於立體視覺之移動式機械臂影像伺服設計

Visual Servo Design of a Mobile Manipulator

Based on Stereo Vision

研 究 生：張哲豪 Student: Che-Hao Chang

指導教授：宋開泰 博士 Advisor: Dr. Kai-Tai Song

林清安 博士 Dr. Ching-An Lin

國 立 交 通 大 學

電 控 工 程 研 究 所

碩 士 論 文

A Thesis

Submitted to Institute of Electrical Control Engineering College of Electrical and Computer Engineering

National Chiao Tung University in Partial Fulfillment of the Requirements

for the Degree of Master in

Electrical Control Engineering July 2010

Hsinchu, Taiwan, Republic of China

基於立體視覺之移動式機械臂影像伺服設計

學生：張哲豪 指導教授：宋開泰 博士 林清安 博士

國立交通大學 電控工程研究所

摘要

本論文之主旨在設計一移動式機械臂的抓物控制系統。藉由安裝於機器人頭 部之立體攝影機擷取影像資訊，使機器人得以在環境中找尋目標物體，並自主導 引機器手臂至適當的抓取位置，完成抓取之動作。本文採用加速強健特徵點演算 法(Speed up robust feature, SURF)來定義目標物體的特徵點，並且藉由比對當前 畫面中的特徵點來判斷目標物是否存在於影像中。為了強化特徵比對結果並算出 抓物控制所需的參考點，本論文採用隨機取樣篩選演算法(RANdom Sample Consensus, RANSAC)來估測平面轉換矩陣(Homography matrix)以準確的標出目 標物的中心點。本論文並發展出一套座標估測的校正法，提高對於目標物座標估 測之準確度。在影像伺服的控制設計上，針對機械手臂及移動平台設計了以座標 估測結果產生控制命令的控制方法，導引移動式機械臂自主抓物。在本論文中， 以實驗室自行設計的多自由度雙臂機器人搭配全向式移動平台，並裝置立體攝影 機作為影像資訊輸入，驗證整套方法的可行性。經過實驗驗證，此系統可以導引 機器人順利的移動並拿取所設定的目標物。

Visual Servo Design of a Mobile Manipulator

Based on Stereo Vision

Student: Che-Hao Chang Advisor: Dr. Kai-Tai Song Dr. Ching-An Lin

Institute of Electrical Control Engineering National Chiao Tung University

ABSTRACT

The objective of this study is to design a grasping system for a mobile manipulator, such that it can find and grasp a target in the environment. Speed up robust feature (SURF) algorithm is used to define the feature of a target object and to match features between current image and object database to confirm the target. To strengthen the feature matching results and calculate the necessary reference control point, we adopt RANSAC(RANdom Sample Consensus) algorithm to estimate the planar transformation matrix (homography matrix) in order to accurately mark the center of target. A set of coordinate estimation correction method is developed to improve the accuracy of target location estimation. A control design is developed based on coordinate estimation for the mobile manipulator for visual servoing for object grasping. Experiments on an self-constructed mobile manipulator reveal that the proposed method can find and grasp a target object successfully.

誌謝

誌謝

誌謝

誌謝

謹向我的指導教授宋開泰博士致上感謝之意，感謝他兩年來在專業上和生活 上待人處事的指導，以他豐富的學識與經驗，配合理論的應用，使得本論文得以 順利完成。 也要感謝學長孟儒、嘉豪、格豪、學姊巧敏在理論與實驗過程中的指導，以 及感謝同學信毅、宗暘、維峻、傑巽、奕彣在研究過程互相勉勵與打氣，同時要 感謝學弟仕晟、建宏、上畯、碩成、家昌、章宏在生活上與課業中帶來的樂趣。 最後，特別感謝我的父母以及所有家人，由於他們的辛苦栽培與鼓勵，在生 活上給予我細心地關愛與照料，並支持我就讀碩士學位，使得我才得以順利完成 此論文，在此我願以此論文獻給我最感激及最偉大的父母親。

目錄

目錄

目錄

目錄

摘要... i ABSTRACT ... i 誌謝... i 目錄... i 圖目錄... i 第一章 緒論... 1 1.1 研究動機... 1 1.2 相關研究... 1 1.3 問題描述... 5 1.4 系統架構... 6 1.5 章節說明... 7 第二章目標物辨識... 8 2.1 特徵點擷取... 9 2.1.1. SURF 工作原理... 9 2.1.2. 影像特徵點比對... 13 2.2 錯誤比對剔除... 14 2.2.1. Homography ... 14 2.2.2. RANSAC ... 16 2.3 定義中心點... 19 第三章目標物姿態估測... 21 3.1 立體視覺原理... 21 3.2 3D 座標公式校正 ... 23 3.2.1. Z 軸座標值估測 ... 24 3.2.2. X 軸座標值估測 ... 28 3.2.3. Y 軸座標值估測 ... 32 3.3 三軸旋轉角... 36 第四章移動式機械臂之影像伺服控制設計... 39 4.1 手臂控制... 39 4.1.1. 硬體構造... 39 4.1.2. 座標轉換... 39 4.1.3. 手腕逆運動學推導... 44 4.1.4. 從假定腕部點逼近實際腕部點的方法... 46 4.1.5. 抓取動作路徑規劃... 48 4.2 移動平台控制... 49 4.2.1. 硬體構造... 49

4.2.3. 姿態估測 (Odometry) ... 54 4.2.4. 平台之運動控制... 55 4.3 影像伺服控制... 57 4.3.1. 視覺伺服控制架構... 57 4.3.2. 控制設計... 58 第五章實驗結果... 62 5.1 實驗用機器人構造... 62 5.2 平台靜止抓取實驗... 65 5.3 平台移動抓取實驗... 69 第六章結論與未來展望... 74 6.1 結論... 74 6.2 未來展望... 74 參考文獻... 76

圖目錄

圖目錄

圖目錄

圖目錄

圖 1.1 姿態(pose)追蹤的結果[10] ... 3 圖 1.2 物體模型建立的基本流程[11] ... 3 圖 1.3 影像辨識流程[12] ... 4 圖 1.4 目標物辨識的 3D 點群結果[16] ... 5 圖 1.5 Database 在目前影像中的比對結果，藍色的框框表示 2D 的定位結果 [17] ... 6 圖 1.6 系統流程圖... 7 圖 2.1 直接對左右影像做對應點計算的結果... 8 圖 2.2 計算 D 區域像素總和 ... 10 圖 2.3 從左到右分別是在 y 及 xy 方向高斯二階偏微分，以及其中利用 box 濾波器的近似。而這些灰色的區域近似於零[2]。 ... 11 圖 2.4 區域極值計算[1] ... 12 圖 2.5 這種場景清楚的顯示出從 Hessian-based 偵測器所得到特徵的性質[2] ... 12 圖 2.6 一個塗鴉場景的細節顯示出在不同尺度下描述器視窗的大小[2] ... 13 圖 2.7 哈爾小波響應轉換結果統計圖[2] ... 13 圖 2.8 透過轉換矩陣 H 得到資料庫物件在目前影像平面中之位置 ... 16 圖 2.9 左圖為一群分布不均勻的點集合，需要從這裡面決定出一條直線。 右圖為經過 RANSAC 計算後，得到符合條件的直線，不符合的點則被 剔除在外。[22] ... 18 圖 2.10 用 SURF 做特徵比對的結果... 18

圖 2.11 用 RANSAC 做 homography estimation 的結果 ... 18

圖 2.12 實際左影像跟資料庫影像做轉換矩陣計算的結果... 19 圖 2.13 實際右影像跟資料庫影像做轉換矩陣計算的結果... 19 圖 2.14 目標物在影像平面的區域及中心點位置的定義... 20 圖 3.1 立體影像處理的模型[23] ... 21 圖 3.2 攝影機跟世界座標關係的上視圖[23] ... 22 圖 3.3 校正版圖示... 24 圖 3.4 距離為 19 格時(532mm)左圖為左攝影機之影像，右圖為右攝影機之 影像... 24 圖 3.5 Z 座標估測值 ... 26 圖 3.6 校正後 Z 座標誤差值 ... 27 圖 3.7 Xl 座標估測值... 28 圖 3.8 Xr 座標估測值 ... 29

圖 3.10 Yl 座標估測值 ... 33 圖 3.11 Yr 座標估測值 ... 34 圖 3.12 校正後 Y 座標誤差值 ... 36 圖 3.13 目標物的三軸向量與世界座標的旋轉關係... 37 圖 4.1 機械手臂設計圖... 40 圖 4.2 機器人各部位及目標物之間的座標關係... 41 圖 4.3 base 跟 arm 座標軸的關係 ... 42 圖 4.4 機器人頭部跟目標物的座標關係... 43 圖 4.5 移動平台跟目標物的座標關係... 43 圖 4.6 (a)機械手爪設計圖(b)手臂各點代號位置 ... 45 圖 4.7 機械手臂各關節點之間的關係圖... 45 圖 4.8 空間中向量及平面的夾角關係... 46 圖 4.9 給定抓取點 b，假定手腕點 c，對應 c 的抓取點 a，對應 b 的手腕點 c’， 四點之間的相對關係圖。... 47 圖 4.10 逼近抓取點的示意圖，隨著 b’漸漸接近 b，我們可以得到對應 b 的 腕部點 c’ ... 48 圖 4.11 夾取之動作規劃示意圖 ... 49 圖 4.12 全向輪實體圖[24] ... 50 圖 4.13 平台四輪配置圖[24] ... 51 圖 4.14 全向輪轉動示意圖[24] ... 52 圖 4.15 全向輪之幾何關係[24] ... 53 圖 4.16 平台座標旋轉示意圖[24] ... 53 圖 4.17 平台運動模型圖[24] ... 56 圖 4.18 平台控制方塊圖[24] ... 57 圖 4.19 移動式機械臂工作模式流程圖... 59 圖 4.20 整體系統架構圖... 60 圖 5.1 實驗用機器人... 62 圖 5.2 Bumblebee2 立體攝影機 ... 63 圖 5.3 機器人硬體架構圖... 64 圖 5.4 (a)資料庫物件及(b)其特徵點位置 ... 66 圖 5.5 平台靜止時，攝影機所擷取的畫面及目標物位置的計算結果... 67 圖 5.6 量測出之目標物在影像平面上的中心點... 67 圖 5.7 攝影機量測到目標物的座標位置... 67 圖 5.8 抓取過程中手臂各軸軌跡圖... 68 圖 5.9 平台靜止抓取實驗... 68 圖 5.10 移動抓取實驗中，起點時攝影機所擷取的畫面及目標物位置... 69

置... 70 圖 5.13 攝影機在工作範圍所量測出之目標物在影像平面上的中心點... 70 圖 5.14 攝影機量測到的目標物座標位置... 71 圖 5.15 抓取過程中手臂各軸軌跡圖... 71 圖 5.16 平台移動抓取實驗... 72 圖 5.17 平台移動軌跡紀錄... 73

表目錄

表目錄

表目錄

表目錄

表 3.1 不同距離時目標點在影像平面上的座標值... 25 表 3.2 Z 座標未校正前的估測值 ... 26 表 3.3 Z 座標校正後的結果及誤差值 ... 27 表 3.4 Xl 座標未校正前的估測值... 29 表 3.5 Xr 座標未校正前的估測值 ... 30 表 3.6 X 座標校正後的結果及誤差值 ... 31 表 3.7 Yl 座標未校正前的估測值 ... 32 表 3.8 Yr 座標未校正前的估測值 ... 33 表 3.9 Y 座標校正後的結果及誤差值 ... 35 表 3.10 三軸旋轉角實際測試結果... 38 表 5.1 Bumblebee2 立體攝影機之規格[29] ... 63 表 5.2 使用 Matlab Toolbox 得到的左攝影機內部參數 ... 65 表 5.3 使用 Matlab Toolbox 得到的右攝影機內部參數 ... 65 表 5.4 使用 Matlab Toolbox 得到的攝影機外部參數 ... 65

第

第

第

第一

一

一

一章

章

章

章 緒論

緒論

緒論

緒論

1.1

研究動機

研究動機

研究動機

研究動機

由於近年來人類不斷的創新研發，致使科技的發展日益提升，電腦運算能力 大幅提高，可以透過各種不同的感測器以及電腦高速的運算能力來完成複雜的工 作。因此，相較於過去傳統僅在工廠中固定式的機械手臂，現在已經有越來越多 的機器人開始走進人類的生活周遭，並且有許多也已朝商品化的方向發展，例如 吸塵機器人或寵物機器人等等。尤其近年來時常會有智慧型機器人的公開發表， 由這個趨勢來看，機器人與人類的生活之間將會有更密切的關係。 所有科技發展的目的都是為了使人類的生活更加便利，而機器人的研究當然 也不例外，如何讓機器人能夠幫助人類甚至是代替人類做一些繁複瑣碎或是危險 的事，都是所有在這個領域努力的人們所追求的目標。如同人類一般，視覺掌握 了人類大多數的資訊來源，因此一個具有影像系統，機械手臂加上移動功能的機 器人，將會對我們的生活有相當大的幫助。以往的機器人大多是存在於工廠中， 在單純的環境中工作，重複著同樣的動作，但是現今的發展中，機器人與人互動 漸趨重要，因應這個需求，勢必需要一個穩定又安全的移動式抓取系統，才能讓 人機互動時具有安全性的保障。 此研究的主要目的在於實現一台具有手臂的移動式機器人，使其能夠辨識出 指定的物品後達成拿取物體的功能。透過裝置於頭部的攝影機，辨識出一般日常 生活中的用品，藉此估測出目標物體所在的位置，並且能由此位置資訊導引機器 人前進並抓取目標物。

1.2

相關研究

相關研究

相關研究

相關研究

對於建立機器人的自主抓取能力，有許多方面的問題需要解決，例如：偵測 環境的能力、搜尋物體的能力、移動路徑規劃的能力以及抓取物體的能力，有了 這些能力的綜合運用，才能確保機器人在環境中能搜尋到目標物，並且能夠透過

近年來在辨識物體方面最常見的方法為 SIFT[1]和 SURF[2]，許多的方法都 以此為基礎延伸下去。文獻[3-5]都是用立體攝影機擷取影像資訊將目標物周邊的 環境建立起來，知道了整體環境情況之後就能針對抓取最適當的路徑規劃。文獻 [3]除了利用 SIFT 的特徵點資訊之外還加上了幾何的特徵，將物體的邊緣都描繪 出來，如此一來目前影像中的環境資訊就能更清楚的顯示出來，此外作者們還提 出了一個方法能夠根據目前影像來決定下一時刻最好的視角位置。文獻[4]所用 的方法是將所有物體的以方塊的形式描繪出來，再將目標物標色，模擬出機械手 臂抓取的路徑，找出最適當的抓取角度。文獻[5]能夠做到即時的在整個工作區 域當中辨識出目標物並將其替換成三維模型。文獻[6]則是以 SIFT 作為其能夠在 複雜環境中辨識出物體的方法，以三個目的建立其視覺系統，分別是辨識目標 物，追蹤目標物及克服複雜環境，最後透過一個六軸的機械手臂來完成抓取任 務。文獻[7]發展出一套結合反投影直方圖及 SURF 的辨識方法，讓救援機器人 能夠在一個未知的環境中辨識出危險標誌，自我定位，劃出環境地圖讓搜救隊在 急難救助時能對災害現場有一個參考的依據。 為了讓機器人在一個智慧型的居家環境中辨識出目標物，文獻[8]發展出一 套方法，將射頻辨識技術(RFID)加入辨識架構中，目的是讓未來的服務型機器人 能夠達到便宜且強健的目的。為了讓機器人在居家服務更有效率，M. Aiguo, et al 在文章中提出一個方便且有效率的訊息融合系統包含 RFID、立體攝影機、雷射 掃描儀在其中[9]。Changhyun Choi, et al 提出一套可以同時用在單眼視覺跟立體 視覺之中的即時目標物姿態估測跟追蹤方法[10]，他用的是目標物上的自然特 徵，以這些特徵當作追蹤點，攝影機在不同角度的情況下，都能將目標物的姿態 清楚的描繪出來，如圖 1.1 所示。 由 Tomono, et al.提出的研究結果[11]，係利用一個移動式機器人建立整體環 境的物體地圖，其首先藉由雷射掃描整個環境，建立出整體二維環境的地圖，接

圖 1.1 姿態(pose)追蹤的結果[10]

圖 1.2 物體模型建立的基本流程[11]

在抓取的規劃上，Yamazaki, et al.則利用 SFM(Structure from motion)的方式 描繪物體的 3D model[12]，並描述物體每個像素上的 Oriented points，但亦由於 資料量較大，因此透過 voxel model 將資料量降低。並且計算在滿足所設定的條 件下，所有可能的抓取方式，如圖 1.3 所示，並從中找尋最佳的姿態(pose)，利 用 Oriented points 描繪出物體，以及所有可能的抓取方式，並且最後計算出夾爪 最適當的抓取位置跟角度。 除了單一機器人抓取之外， Hirata, et al 提出了一個多機器人合作來拿同一 個物體的運動控制演算法[13]，克服了操作時手臂動作及輪子移動之間的時間延 遲而衍生出的問題，這個問題會使人在操作機械臂拿取物體時產生困難，但是在 這個新的控制方法之下，人們就能操控機器人合作達成任務而不用考慮這些控制 的問題。

圖 1.3 影像辨識流程[12] 在整體的移動式抓取設計方面，文獻[14-17]所提出的方法之中有許多值得參 考的地方。由 Christensen, et al.所提出的研究結果[14]，發展出一隻具備定位、導 航、物體辨認、追蹤、抓取規劃的機器人，此機器人為 eye-in-hand 的構造。擁 有一套演算法可以從特徵擷取，目標物位置及姿態(pose)估測，到最後完成抓取 的功能。由 Lee, et al.提出來的研究結果[15]中，發展出一套任務導向的抓取演算 法，用在規劃對於不同的任務在物體上做適當的抓取。在運作的同時，利用一個 外部攝影機，透過不同角度的影像資訊，可以使機器人同時觀察手及物體來進行 操作。 在攝影機的擺放位置方面，若是能放置於頭部的位置，如[16][17]，就能夠 改善在 eye-in-hand 的構造中，抓取到目標物後，攝影機就失去繼續接受資訊的 功能，無法繼續新的任務。[16]提出的研究結果，發展出一套演算法，可以做到 物體的辨識跟定位，藉由 SIFT 特徵點擷取建立 3D 的點群資訊，如圖 1.4 所示， 描述出物體的 3D pose，利用眼手座標關係，完成抓取的任務，並藉由精準的校 正達到抓取過程中的避障功能。

圖 1.4 目標物辨識的 3D 點群結果[16] 由 Azad, et al.等人提出的研究結果[17]，發展出一套用在抓取的基於立體影像的 物體 6D 定位演算法，應用在人形機器人 ARMAR。由五個步驟完成抓取的任務， 一開始分別用左右眼取特徵點，把每個比對成功的轉換成 3D 座標，再將這些 3D 座標集合相稱的放到 3D 平面上，最後得到目標物的四個角落框出表面，如圖 1.5，並給定他們的世界座標。 近期的研究中，將 RANSAC 及 Homography 加入 3D 姿態估測中也越來越常 見，[18]所提出來的座標估測方法中就有將這兩個方法用在其中，應用在無人遙 控直升機在飛行時的定位，藉由於地面上的參考目標或停機坪，定義無人飛機上 的攝影機座標系，此文獻中所提出的演算法，對於直升機的 3D 位置及朝向的估 測方面都具有很高的品質，可以將結果應用在複雜的控制如自主降落及低空準確 定位和下降等任務。

1.3

問題描述

問題描述

問題描述

問題描述

在一個具有抓取功能的移動式機械手臂的研究中，首先會遇到的問題就是如 何在環境中辨識出欲抓取的目標物。我們期望能在較少的先期資訊之下，不必耗 費很多時間在建立資料庫影像。利用物體本身所具有特徵點的辨識方法上，其優 點在於可以運用簡單的辨識方法達成，以利於機器人在即時的環境下進行反應。 尤其對於辨識環境中 3D 的物體而言，還需考量如何可以盡量達到不受物體 旋轉、視角改變以及遮蔽物的影響，正確的辨識出物體。而最近使用的辨識方法 中以 SIFT 及 SURF 演算法較常見，他們具有抗旋轉、尺度不變的特性，可以針

圖 1.5 Database 在目前影像中的比對結果，藍色的框框表示 2D 的定位結果[17] 對不同的目標先行建立 database，不過在運算量較為龐大且可能出現錯誤比對的 情況，所以需要有其他輔助篩選特徵點的演算法，提高辨識的準確性跟穩定度。 當順利辨識出物體後，其下一步驟便是估測目標物的所在位置，其中深度資 訊決定了機器人在影像伺服過程中需要移動的距離，於是在本論文中採用立體視 覺系統，藉由同一時刻的兩張影像，擷取其中的資訊，期望能提升在目標物位置 估測上的準確度。 在移動式抓取的研究上，必須將機器人本身所有的部件彼此間的座標轉換關 係弄清楚，且在控制上必須有一個穩定的參考點當作控制命令輸入，以利於導引 機器人能夠順利的朝向目標物來移動。如何得到這個穩定的參考點就是本論文中 一個重要的環節，如果能經由一般性的影像辨識方法就能找出這個參考點，而不 用透過許多複雜的演算法，想必對解決問題會有很大的幫助。 如何運用這個參考點為中心，轉換成機器人在移動抓取過程中的控制命令， 整體的影像伺服控制設計就顯得格外重要，且必須考慮到如何做出機器人各種 行為之間的決策，如此一來，就能夠順利的完成移動抓取的任務。

1.4

系統架構

系統架構

系統架構

系統架構

本論文使用實驗室研製的全向式移動平台，上面裝置自行設計的雙臂機械手 臂，以及兩自由度的頭部機構，組合成一具完整的移動式機械臂(Mobile manipulator)。本系統所使用的主要感測器為立體攝影機，系統流程圖如圖 1.6

圖 1.6 系統流程圖 物在空間中的座標位置，作為導引平台移動及控制機械手臂抓取目標物的命令。 藉由不斷的更新目標物的即時位置，以及讀取手臂跟平台所回傳的位置，比較兩 者之間的誤差關係，來做為是否完成任務的判斷依據。

1.5

章節說明

章節說明

章節說明

章節說明

本論文一共分成六章，第一章介紹研究動機與目的，並且概略介紹所設計之 系統架構。第二章說明 SURF 特徵點擷取演算法及剔除錯誤比對的方法。第三章 則是介紹目標物的姿態估測及座標量測的校正。第四章介紹本論文所用之機器人 的影像伺服設計以及各部位控制設計的介紹。第五章為實驗結果，驗證整體設計 上的可行性。第六章為結論與未來展望。

第

第

第

第二

二

二章

二

章

章目標物辨識

章

目標物辨識

目標物辨識

目標物辨識

在本論文中所提出的影像伺服方法之中，最重要的一個環節，就是估測出目 標物的三維座標。估測目標物的三維座標時，資訊來源就是目標物在左右攝影機 影像中成對的特徵點，雖然我們可以直接將取到的左右攝影機影像直接做對應點 的計算(correspondence)但是這樣得到的是整個影像平面上的對應點，在複雜背景 的狀況下，整個影像平面都會是對應點，而單純背景但是存在數個物體時，如圖 2.1 所示，則會在不同物體上出現多組對應點，如何從複雜背景的情況下針對單 一目標物找出我們所需的對應點，就是一個需要解決的問題。 為了解決這個問題，也就是如何找出目標物在左影像平面跟右影像平面之中 的位置，我們將特徵比對設計成一開始將資料庫影像分別跟左右攝影機取到的影 像做特徵比對，這樣就能得到兩組比對成功的特徵點，之後再將這兩組特徵點去 做交叉比對，就可以得到目標物在左右攝影機影像平面上成對的特徵點。 另一方面，在本論文的控制器輸入需要一個在目標物上穩定的參考點，我們 希望這個參考點能在目標物的中心位置，以利於在整個影像伺服控制的過程中， 給予機器人一個準確的控制命令，使其能夠正確的移動到目標物前方，並穩定的 抓取目標物，而不會因為控制命令的不正確，而使機器人無法移動到適當的抓取 位置，導致抓取任務失敗。 雖然說我們可以將上述方法中所得到的成對特徵點取平均，當作控制用的參

考點，但是我們不能保證，這個平均值是位於目標物的中心，因為目標物的特徵 點分布可能在任何位置。於是為了能夠得到穩定的參考點，我們將 homography 加入我們的特徵比對過程之中[19]，在資料庫影像分別跟左右攝影機影像做特徵 比對之後，分別將這些成對的特徵點拿去算平面轉換矩陣，因為這個平面轉換矩 陣包含著縮放及旋轉因子，所以我們可以由這個平面轉換矩陣準確的框出目標物 在左右攝影機影像中的區域，接著再取其四個角點的平均，就可以得到一組穩定 的對應點，也就是目標物的中心點，由這組中心點就能算出控制所需要的穩定參 考點。 本章將說明算出這組對應點的方法。首先，2.1 節介紹特徵點擷取及比對的 方法，2.2 節說明如何得到平面轉換矩陣(Homography matrix)，至於如何求得這 組中心點的三維座標作為控制所需的參考點將在第三章會有詳細的說明。

2.1

特徵點擷取

特徵點擷取

特徵點擷取

特徵點擷取

要從目前攝影機所擷取到的影像中找出我們所要的目標物，需要有一個辨識 物體的演算法。在本論文中，使用的是特徵點擷取。現存的特徵點擷取方法之中， 以 SIFT 跟 SURF 較常見，經常在論文中被引用。由於立體攝影機需要同時處理 兩張影像，於是我們採用了在運算上較為快速的 SURF 來作為本論文的特徵點擷 取演算法。

2.1.1.

SURF 工作原理

工作原理

工作原理

工作原理

加速強健特徵點演算法(Speed Up Robust Feature, SURF)[2]為 Bay et al.所提 出的一種新的尺度不變且抗旋轉的特徵點偵測及描述法。這個方法的設計概念在 於發展出一套重複性、獨特性以及強健性優於現存方案的特徵點擷取演算法，且 能夠有更快的運算速度。在計算過程中，首先採用 Integral image，定義為 0 0 ( , ) ( , ) j y i x i j x y i j ≤ ≤ ∑ = = =

∑∑

I I ，它是讓 SURF 能快速計算的關鍵[20]，概念為累積影像矩 形區域的像素總和，並以此為基礎計算不同區塊代表的特徵值。例如: 如圖 2.2

圖 2.2 計算 D 區域像素總和 接著根據定理 D=4-2-3+1，即可快速算出 D 區域像素總和。本章節將描述如何透 過 SURF 所得到的特徵點資訊來描述目標物體，使機器人能夠從影像中辨識出物 體存在的位置。 以下為 SURF 在運算過程中主要的步驟:

2.1.1.1

Fast-Hessian Detector

SURF 尋找特徵點的方式是基於 Hession 矩陣的行列式，找出二次微分的極 大跟極小值。在 SURF 演算法裡將 Hessian matrix 當作偵測器(Detector)，因為它 在計算時間及精確度上具有很好的表現。給定一個在影像平面上的點 ( , ) X = x y ，則Hessian matrix H x( , )σ 以尺度σ ，在X 中被定義如下: ( , ) ( , ) ( , ) ( , ) ( , ) xx xy xy yy L x L x H x L x L x σ σ σ = _{σ σ}    (2-1) 其中L_xx( , )xσ 、L_xy( , )x σ 及L_yy( , )xσ 是Laplacian of Gaussians(LoG)。 在圖 2.3中的9x9遮罩是σ =1.2高斯二階微分的近似值且代表我們的最低 尺度，也就是最高空間的解。我們將D_xx、Dyy、Dxy標記為我們的近似值。而為 了計算的效率應用，在長方形區域的權重被保持為簡單的形式，但是為了 Hessian’s 行列式 (1.2) (9) 0.912... 0.9 (1.2) (9) xy _F xx _F L D L D = ≃ ，其中 x 是_F Frobenius norm。

圖 2.3 從左到右分別是在 y 及 xy 方向高斯二階偏微分，以及其中利用 box 濾波 器的近似。而這些灰色的區域近似於零[2]。 2 det(H_approx)=D D_{xx yy}−(0.9D_xy) (2-2) 此外，應該要將濾波器的遮罩大小正規化。因此可以保證對任何的濾波器大 小都能有一個常數的 Frobenius norm。經過測試 9x9 的遮罩效果符合在σ =1.2時 的高斯微分，而27x27則符合於σ =3.6時。

2.1.1.2

找

找

找

找出

出

出

出區域極值

區域極值

區域極值

區域極值

算出近似值之後的步驟就是要找出區域極值，參考圖 2.4，檢查中間X位置點周 圍8點與相鄰取樣尺度空間的18個點，也就是綠色點的位置，如果X點是這些 點中的極值，則將X選為關鍵點。

2.1.1.3

計算特徵點主要方向

計算特徵點主要方向

計算特徵點主要方向

計算特徵點主要方向

由於特徵點對四周的描述向量會跟方向有關，所以首先要找到主要的方向， 才能抗旋轉。為了能夠抗旋轉，將所有找到的特徵點確認一個重複性地朝向。要 達成這個目的，首先計算在x跟y方向的哈爾小波響應(Haar-wavelet responses)， 接著將這些特徵點以6倍的 s 畫圓，如圖 2.5所示。 s 為這些特徵點被偵測到的 尺度。

2.1.1.4

計算出

計算出

計算出

計算出 64 個特徵向量

個特徵向量

個特徵向量

個特徵向量

這些圓經過哈爾小波響應可以得到一個方向，接著再利用這個方向切出20 倍σ 為邊長的正方形，如圖 2.6。將每個正方形再切割成4x4個小方塊，為了簡 化，我們將d_x稱為水平方向的哈爾小波響應，d_y稱為垂直方向的哈爾小波響應。

圖 2.4 區域極值計算[1] 每個子區域具有一個四維的描述向量 v 當作它基本強度的結構。將結果的水平、 垂直變化量分別做累加與絕對後再累加，v=(

∑ ∑ ∑

d_x, d_y, d_x,

∑

d_y)，如圖 2.7，便可得到在所有4x4的子區域中維度是64的特徵向量。另一方面，切成3x3 方塊(SURF-36)的較短描述效果就稍差，不過他允許快速的比對，跟其他描述子 比較之下在文獻中依然有相當的被接受度。 圖 2.5這種場景清楚的顯示出從Hessian-based 偵測器所得到特徵的性質[2]

圖 2.6 一個塗鴉場景的細節顯示出在不同尺度下描述器視窗的大小[2] 圖 2.7 哈爾小波響應轉換結果統計圖[2]

2.1.2.

影像特徵點比對

影像特徵點比對

影像特徵點比對

影像特徵點比對

當計算出影像平面中每一個特徵點的特徵描述向量後，便可以透過與資料庫 中所儲存的特徵點資訊進行比對，藉此得以判斷當前影像中是否存在機器人所需 抓取的目標物。

本論文利用最近鄰居演算法（Nearest neighborhood algorithm），如 (2-3)式，

其中Des i_c( )代表當前影像中該特徵描述向量的第 i 個元素，Des i_d( )代表資料庫

影像中該特徵描述向量的第 i 個元素。藉由比較當前影像中所有特徵點描述向量 與資料庫中現存的特徵點描述向量，尋找其兩點距離最短的，則其本身則可能為 相同的特徵點。

1 128 2 2 1 ( _c( ) _d( )) i d Des i Des i =   = −  

∑

 (2-3) 雖然經過 SURF 處理後的特徵向量具有高度的差異，但其亦有可能因為來自 於背景或者是雜訊恰巧相同，因此比對上建議至少有 3 點以上的特徵點相同，以 增加比對的正確性。而且對於一般使用固定門檻值來限制是否為正確結果的效果 將受限於所選定的門檻值，因此根據文獻[1]中所提的建議可以採取比較最近鄰 居與次近鄰居的比值，當比值越接近 1 時，其越有可能是來自於背景或雜訊的干 擾，因此可以視為錯誤的配對。

2.2

錯誤比對剔除

錯誤比對剔除

錯誤比對剔除

錯誤比對剔除

在做完特徵點比對之後，經常會出現一些錯誤比對的特徵點。由於成對的特 徵點在之後 3D 座標計算上扮演著非常重要的角色，所以我們必須提高這些特徵 點的品質，追求更加穩定跟準確的結果，本論文採用 RANSAC 跟 homography[19] 來做特徵點的二次篩選加強最後辨識的結果。

2.2.1.

Homography

Homography 的功用在於找到兩個影像平面中，點跟點之間的對應關係。在 影像平面上，一組對應的特徵點之間，存在著一種線性變換的關係，而對應矩陣 (Homography)定義為其中一個影像平面上的點P_a轉換到另一個影像平面上的點 b P之間的線性轉換。Homography 是由一 3*3 的非奇異矩陣(Non-singular)矩陣所 決定， 因為具有縮放因子ω'的關係，能夠反映出目標物在影像平面上，跟資料 庫影像相比，尺度大小的變化倍率。它具有8個自由度(degree of freedom)，因為 平面上一個點( , )x y 具有兩個自由度，所以決定一個Homography至少需要四點 以上的對應關係。 '_{b ab a} p =H p ，其中 2 , a b p p ∈P ， H為3*3的Homography matrix

11 12 13 21 22 23 31 32 33 H_ab h h h h h h h h h     =_{ }     (2-4) 11 12 13 21 22 23 31 32 33 ' ' ' 1 ' a b b ab a a b h h h x x p H p h h h y y h h h ω ω ω             = =_{   }=_{ }             (2-5) 11 12 13 31 32 33 a a b a a h x h y h x h x h y h + + = + + ， 21 22 23 31 32 33 a a b a a h x h y h y h x h y h + + = + + ，ω'=h x31 a+h y32 a +h33 (2-6) 轉換矩陣H共有 9 個未知數，由於齊次座標轉換有比例相等的關係，於是 假設轉換矩陣H中的某一個未知數固定(如:h₃₃=1)，如此一來待解的參數變成 8 個，而平面上一組對應點可提供 2 個線性獨立的方程式，因此 8 個未知數需要 4 組對應點提供 8 個線性獨立的方程式，而這 4 組對應點是從所有的對應點中任意 選出的，所以要得到一個轉換矩陣至少要 4 組對應點。當求得轉換矩陣後，可算 出每個對應點的誤差，因此符合最小誤差的對應矩陣極為我們所需要的對應矩 陣。我們可以將 (2-6)改寫成(2-7)。 當我們算出轉換矩陣之後，就可以經由(2-8)來驗證特徵點之間的轉換關係跟 這個轉換矩陣的關係是否一致。將資料庫影像中的特徵點代入(2-8)中，可以得到 資料庫中的特徵點對應在目前影像平面上的特徵點位置，這個步驟在 2.2.2 節 RANSAC 演算法的步驟中，是相當重要的一環，我們必須檢查這個對應關係的 誤差來決定所算出來的轉換矩陣是否符合大部分的特徵對應關係。 1 1 1 1 1 1 11 1 1 1 1 1 1 12 2 2 2 2 13 2 2 2 2 2 2 2 2 21 3 3 3 3 2 3 3 3 3 3 3 3 3 4 4 4 4 4 4 4 4 4 4 4 4 1 0 0 0 ' ' 0 1 ' ' 0 0 1 0 0 0 ' ' 0 1 ' ' 0 0 1 0 0 0 ' ' 0 1 ' ' 0 0 1 0 0 0 ' ' 0 1 ' ' 0 0 x x x y x y h x y y x y y h x x x y h x y x y y x y y h x x x y h x y x y y x y y x x x y x y x y y x y y − −    _{− −}     _{− −}    − −    _{− −}    − −    _{− −}    − −     1 1 2 2 2 3 23 3 31 4 32 4 ' ' ' ' ' ' ' ' x y x y x h y h x h y                        ₌                             (2-7)

' / ' 1 b b b b x p p ω y     = =_{ }    

(2-8) 因為 homography 具有對應旋轉及尺度變化的特性，所以即使目標物在不同 距離或是不同角度，我們都可以準確的框出目標物在目前影像平面上的位置。如 圖 2.8 所示，左邊是資料庫影像，首先藉由所擷取到的特徵點，求得資料庫影像 跟目前影像平面之間的轉換矩陣，接著透過轉換矩陣H，我們可以得到目標物 在目前影像平面(image plane)中的位置。

2.2.2.

RANSAC

從 2.2.1 節中得知，要算出平面轉換矩陣，需要四個對應點，雖然從前面的 特徵點擷取及最近鄰居演算法（Nearest neighborhood algorithm）運算過後，可以 得到一群對應點，但是要算出更精確的轉換矩陣，就需要更準確的特徵對應點， 所在這邊我們採用 RANSAC(Random sample consensus)[21]此種隨機取樣方式。 RANSAC 演算法，其主要精神在於從一群分布不均勻或是參雜雜訊的資料中， 隨機挑選數個樣本，算出參考的模型，之後再把其他資料逐次拿進來運算，留住 符合模型的大多數，排除少數不符合者，以符合大眾資料的模型為最後結果。舉 例來說[22]，如圖 2.9 所示，運用此演算法能夠從一群分布不均勻的點集合之中 找出一條直線，符合大多數的點，而少數不符合的點則被剔除在外。

RANSAC 演算法，為一個條件中止式的隨機演算法，當算出來的結果合乎 我們所設定的閥值時，表示此結果已經合乎大部分的特徵對應關係了。 其演算步驟簡述如下: 1. 隨機挑選 4 對以上在第一階段比對成功的對應點，求出目前影像跟資料 庫影像之間的平面轉換矩陣 H。 2. 將所有對應點使用步驟 1 求得的平面轉換矩陣進行位置轉換，將資料庫 影像的對應點代入轉換矩陣得到另一組配對點，之後計算這些經過轉換 的配對點座標與原本目前影像中的配對點座標之間的距離。 3. 統計步驟 2 中所有配對點間的距離，其配對點的距離若小於某個自訂的 臨界距離D_τ，則表示此組配對點符合這個平面轉換矩陣，然後統計這 個平面轉換矩陣的正確配對數量。 重複上述步驟 N 次之後，若某次的正確配對點數量最多，且大於某個自訂 的臨界數量S_τ時，則採用此次的平面轉換矩陣，再將這些配對點拿去求得最終 的平面轉換矩陣。 在本論文中，參雜雜訊的資料樣本就是第一次特徵比對的結果，如圖 2.10 所示，可以看出當中存在在錯誤比對的特徵點，而模型就是轉換矩陣(homography matrix)，將 RANSAC 作為計算轉換矩陣的演算法，如此一來就可以得到對應正 確比例最高的一群特徵點，用 homography 來框出在目前影像平面上的目標物 時，位置也能更加準確，如圖 2.11，剔除錯誤比對特徵點之後，再將剩餘比對 成功的特徵點拿來算轉換矩陣，就可以準確的框出目標物的位置，在分別對左右 攝影機影像目標物的四個角點取平均後，就能得到中心點，在平面轉換矩陣計算 的相當準確的情形下，目標物的範圍可以清楚的被框出來，所以用這個範圍的平 均位置來當作參考點是比較穩定的結果。將這對中心點代入三維座標估測公式， 如此一來就能夠得到穩定的控制參考點，將會對整個移動式抓取的控制上有很大 的助益。

圖 2.9 左圖為一群分布不均勻的點集合，需要從這裡面決定出一條直線。右圖 為經過 RANSAC 計算後，得到符合條件的直線，不符合的點則被剔除在外。[22] 實際的運算結果如圖 2.12 及圖 2.13 所示，我們將立體攝影機所讀取到的左 右兩個影像，分別跟資料庫影像做前述的處理，框出目標物後，分別求得目標物 在左右攝影機影像的中心點，接著將這對中心點代入第三章將會提到的三維座標 估測公式，就可以知道目標物在世界座標中的位置，做為導引機器人往前抓取目 標物的控制命令輸入。 100 200 300 400 500 600 50 100 150 200 圖 2.10 用 SURF 做特徵比對的結果 50 100 150 200

圖 2.12 實際左影像跟資料庫影像做轉換矩陣計算的結果 圖 2.13 實際右影像跟資料庫影像做轉換矩陣計算的結果

2.3

定義

定義

定義

定義中心點

中心點

中心點

中心點

當我們算出 homography 之後，我們可以得到目標物跟目前影像之間的平面 轉換關係。接著我們可以算出目標物在左右影像平面上，四個角點的位置，將這 四個角點連接起來框出目標物在目前影像平面的位置。如圖 2.14 所示，我們將 目標物在左攝影機影像平面的四個角點取平均，定為左中心點(left center)，目標 物在右攝影機影像平面的四個角點取平均，定為右中心點(right center)，將這組 中心點當作控制用的參考點，以這點的三維座標表示目標物的位置，藉此導引機 器人完成抓取任務。

第

第

第

第三

三

三章

三

章

章目標物姿態估測

章

目標物姿態估測

目標物姿態估測

目標物姿態估測

本章節的主要目的在說明如何將第二章所算出的特徵對應點，轉換成在空間 中實際的三維座標，當作控制器輸入的重要資訊來源。

3.1

立體視覺原理

立體視覺原理

立體視覺原理

立體視覺原理

要如何從空間中一組成對的影像中找出互相匹配的特徵配對，也就是在世界 座標中的某個點分別在左右眼影像中成像的位置是立體視覺的形成很重要的一 個關鍵點。這組對應點稱為(Corresponding points)，而對應點在影像中的位置差 稱為視差(Disparity)。圖 3.1 中，世界座標中的一點w=( , , )x y z 分別投影到左影 像平面中的p u vl( , )l l 以及右影像平面中的 p u vr( ,r r)，兩個攝影機之間的鏡心距離 設為B(baseline)，透鏡的焦距為λ，兩個成像點的橫軸誤差為d = −u_l u_r，最後 可由這組對應點求回世界座標[23]。 圖 3.2為圖 3.1投影到XZ平面的示意圖，由其中的幾何關係可以得到，如 何從對應點p u vl( , )l l 和 p u vr( ,r r)推導回世界座標w=( , , )x y z 的公式。 ( , ) l l l p u v ( , ) r r r p u v ( , , ) w= x y z

λλλλ λλλλ

( , )

u v

_{l l}

( ,

u v

r r

)

( , , )

w

=

x y z

圖 3.2 攝影機跟世界座標關係的上視圖[23] 首先由圖 3.2 中相似三角形的關係得到 u x x z z λ = − −λ λ= − (3-1) v y y z z λ = − −λ λ= − (3-2) 接著整理過後得到 x u z λ λ = − (3-3) y v z λ λ = − (3-4) 將(3-3)及(3-4)的 x 及 y 整理到左項，得到 XY 座標的公式 ( ) u x λ z λ = − (3-5) ( ) v y λ z λ = − (3-6)

1 1 ( 1) u x λ z λ = − (3-7) 2 2 ( 2) u x λ z λ = − (3-8) 1 x 、x2之間的座標差為兩攝影機間鏡心距離B(Baseline) 2 1 x = +x B (3-9) 左右攝影機看到的 Z 座標值皆相同 2 1 z = =z z (3-10) 將(3-9)代入(3-8)得到 2 1 ( 2) u x B λ z λ + = − (3-11) 整理之後，就得到 Z 座標的公式 * l r B z u u λ λ = − − (3-12)

3.2

3D 座標公式校正

座標公式校正

座標公式校正

座標公式校正

雖然可以從3.1節中所述的公式求出一組對應點的世界座標，但是從實際計 算的結果來看，仍存在著不小的誤差，假設結果跟實際座標值存在著一個轉換關 係，於是我們需要求出這個轉換關係，來對3D座標公式做出修正，以期待能夠 影像平面上的座標值，得到目標物在空間中實際的三維座標。 為了得到公式值跟實際座標值之間的轉換關係，我們需要一群參考的資料。 在本論文中，採用的方法是針對世界座標中固定的一點跟攝影機的相對距離越來 越遠時，紀錄其投影到左右攝影機影像平面上的座標值，將這些座標值代入公式 中，找出不同距離時，公式值跟實際值之間的關係。求出這個轉換關係後，我們 就可以由空間中任何一點的在左右攝影機影像平面上的二維座標位置，求出其在 空間中的三維座標。 在公式的校正方面，用的是每個方格28mm*28mm的校正板，如圖 3.3所 示。校正板每次往後退一個方格的距離(Z方向)，針對校正板上的同一個點做紀 錄(在本次校正中是針對C(0,27,Z)這個點)，如圖 3.4所示，把C在左右眼影像平

28 mm 28 mm 圖 3.3 校正版圖示 視窗中的目標點)的座標值跟上一個距離相比幾乎沒有變動時，表示到達攝影機 辨識物體的極限距離。把這過程中記錄下來的所有座標對，用來求出二維座標到 三維座標之間的轉換公式。

3.2.1.

Z 軸座標值估測

軸座標值估測

軸座標值估測

軸座標值估測

由於 X、Y、Z 都需要 Z(深度)的估測值來做計算，所以一開始我們先將上面 得到的所有對應點代入(3-12)得到深度的估測值(Z)。從圖 3.5 及表 3.2 可以發 現，Z跟實際距離(Z_a)的關係呈現出一個二元一次直線的關係，因此，只要找出 兩者之間的關係就能從Z得到實際的 Z 座標。 l

p

r

p

表 3.1 不同距離時目標點在影像平面上的座標值

Za(mm)center_lxcenter_lycenter_rxcenter_ryZa(mm) center_lxcenter_lycenter_rxcenter_ry

280.000 493 150 157 150 868.000 389 208 277 208 308.000 479 158 173 158 896.000 387 209 279 209 336.000 466 164 185 164 924.000 385 210 281 210 364.000 457 170 198 170 952.000 383 211 282 211 392.000 449 175 207 175 980.000 382 212 283 212 420.000 443 179 217 179 1008.000 380 212 284 212 448.000 435 183 223 183 1036.000 379 213 286 213 476.000 430 186 230 186 1064.000 378 213 287 213 504.000 425 190 236 190 1092.000 377 213 288 213 532.000 421 193 241 193 1120.000 376 214 289 214 560.000 416 195 245 195 1148.000 375 214 290 214 588.000 412 197 249 197 1176.000 374 215 291 215 616.000 409 199 253 199 1204.000 373 215 292 215 644.000 406 201 257 201 1232.000 372 216 294 216 672.000 404 202 261 202 1260.000 371 216 295 216 700.000 402 203 264 203 1288.000 370 216 296 216 728.000 399 203 267 203 1316.000 370 217 297 217 756.000 397 204 270 204 1344.000 369 217 297 217 784.000 395 205 272 205 1372.000 369 218 298 218 812.000 393 206 274 206 1400.000 368 218 299 218 840.000 390 207 276 207 為了得到這個轉換關係，首先要求出一條逼近Z的直線，於是我們將Z_a跟Z 之間的對應數值，運用最小迴歸曲線的原理，就能夠得到逼近這條直線的方程式。 (3-13)是由 MATLAB 所計算出來的就是Z跟 Z 座標之間的轉換公式。  _ 802.6 1.9701* pose Z = − Z (3-13) 根據圖 3.6可看出，校正後的Z座標值在1公尺的範圍內誤差都在正負 10mm之間，但是隨著距離變遠，對應點的差距越來越不明顯而導致誤差略為上 升，大概在1.5公尺之後，目標點在左右影像平面上的位置幾乎不再變動，沒辦

離增加，誤差也跟著增加，但是結合公式值及本論文所提出的轉換關係尋找方法 之後，Z 座標的估測精確度跟未修正前之結果相比，可以說是有相當顯著的提升。 表 3.2 Z 座標未校正前的估測值 a Z (mm) _Z a Z (mm) _Z 280.000 265.119 868.000 -29.458 308.000 250.679 896.000 -45.823 336.000 236.290 924.000 -63.447 364.000 221.331 952.000 -77.582 392.000 207.908 980.000 -87.480 420.000 193.430 1008.000 -103.102 448.000 178.969 1036.000 -119.731 476.000 164.963 1064.000 -131.427 504.000 150.561 1092.000 -143.648 532.000 137.469 1120.000 -156.430 560.000 122.999 1148.000 -169.815 588.000 108.795 1176.000 -183.844 616.000 95.171 1204.000 -198.567 644.000 80.267 1232.000 -222.066 672.000 66.331 1260.000 -238.762 700.000 53.792 1288.000 -256.361 728.000 37.492 1316.000 -265.523 756.000 22.731 1344.000 -274.938 784.000 10.059 1372.000 -284.619 812.000 -3.466 1400.000 -304.823 840.000 -21.706  Z  Z

表 3.3 Z 座標校正後的結果及誤差值

a

Z (mm) pose_Z pose_Z_error Za (mm) pose_Z pose_Z_error

280.000 280.289 0.289 868.000 860.635 -7.365 308.000 308.737 0.737 896.000 892.876 -3.124 336.000 337.084 1.084 924.000 927.598 3.598 364.000 366.556 2.556 952.000 955.444 3.444 392.000 393.001 1.001 980.000 974.945 -5.055 420.000 421.523 1.523 1008.000 1005.721 -2.279 448.000 450.013 2.013 1036.000 1038.482 2.482 476.000 477.606 1.606 1064.000 1061.524 -2.476 504.000 505.979 1.979 1092.000 1085.600 -6.400 532.000 531.772 -0.228 1120.000 1110.784 -9.216 560.000 560.280 0.280 1148.000 1137.152 -10.848 588.000 588.264 0.264 1176.000 1164.792 -11.208 616.000 615.104 -0.896 1204.000 1193.796 -10.204 644.000 644.466 0.466 1232.000 1240.091 8.091 672.000 671.921 -0.079 1260.000 1272.986 12.986 700.000 696.624 -3.376 1288.000 1307.658 19.658 728.000 728.738 0.738 1316.000 1325.706 9.706 756.000 757.817 1.817 1344.000 1344.256 0.256 784.000 782.783 -1.217 1372.000 1363.329 -8.671 812.000 809.428 -2.572 1400.000 1403.132 3.132 840.000 845.362 5.362 圖 3.6 校正後 Z 座標誤差值

3.2.2.

X 軸座標值

軸座標值

軸座標值估測

軸座標值

估測

估測

估測

在 X 座標方面，一樣要先把對應點代入(3-5)式中，求出各個距離的X ，在 本論文中，攝影機座標系的原點是訂在左右攝影機的中間，加上在本次校正實驗 中是針對 X 座標等於 0 時，所以對於左攝影機來說，實際的 X 座標值Xl_a是 60， 對於右攝影機，實際的 X 座標值Xr_a則是-60，由此可得到左右攝影機的估測值 跟實際 X 座標值之間的誤差。 從左攝影機跟右攝影機的X 值統計圖表可以看出來，隨著距離增加，X 值 的誤差逐漸增加，但是從圖 3.7 及圖 3.8 可以看出來誤差大致上是呈線性的增 加，所以在這邊我們需要找出來，隨著距離的增加我們所要減掉的估測誤差，也 就是誤差方程式。 _X x0

(

λ _Z

)

λ = − 為估測出來的 X 座標， 將 pose_Z 跟算出來的 X 座標誤差 x_error 的所有數值，算出誤差方程式，(3-14)及(3-15)是由 MATLAB 所求出來的 誤差方程式。 _ 1.1557 0.41236 * _ xl error= + pose Z (3-14) _ 4.3684 0.40812 * _ xr error= + pose Z (3-15) _Xl  Xl

表 3.4 Xl 座標未校正前的估測值 Za(mm) Xl Xla Xl_error Za(mm) Xl Xla Xl_error 280.000 176.071 60 116.071 868.000 416.786 60 356.786 308.000 187.843 60 127.843 896.000 430.000 60 370.000 336.000 199.004 60 139.004 924.000 444.231 60 384.231 364.000 211.737 60 151.737 952.000 455.050 60 395.050 392.000 222.645 60 162.645 980.000 463.030 60 403.030 420.000 235.221 60 175.221 1008.000 475.000 60 415.000 448.000 246.226 60 186.226 1036.000 489.032 60 429.032 476.000 258.000 60 198.000 1064.000 498.462 60 438.462 504.000 269.841 60 209.841 1092.000 508.315 60 448.315 532.000 280.667 60 220.667 1120.000 518.621 60 458.621 560.000 291.930 60 231.930 1148.000 529.412 60 469.412 588.000 303.313 60 243.313 1176.000 540.723 60 480.723 616.000 314.615 60 254.615 1204.000 552.593 60 492.593 644.000 326.980 60 266.980 1232.000 572.308 60 512.308 672.000 339.021 60 279.021 1260.000 585.789 60 525.789 700.000 349.565 60 289.565 1288.000 600.000 60 540.000 728.000 362.727 60 302.727 1316.000 608.219 60 548.219 756.000 375.118 60 315.118 1344.000 615.000 60 555.000 784.000 385.366 60 325.366 1372.000 623.662 60 563.662 812.000 396.303 60 336.303 1400.000 640.000 60 580.000 840.000 410.526 60 350.526  Xr _Xr

表 3.5 Xr 座標未校正前的估測值 Za(mm) Xr Xra Xr_Slope Za(mm) Xr Xr a Xr_error 280.000 57.441 -60 0.422 868.000 296.517 -60 356.517 308.000 69.271 -60 0.399 896.000 309.579 -60 369.579 336.000 80.442 -60 0.455 924.000 323.644 -60 383.644 364.000 93.180 -60 0.389 952.000 334.329 -60 394.329 392.000 104.061 -60 0.448 980.000 342.215 -60 402.215 420.000 116.603 -60 0.391 1008.000 354.036 -60 414.036 448.000 127.540 -60 0.418 1036.000 367.905 -60 427.905 476.000 139.249 -60 0.420 1064.000 377.220 -60 437.220 504.000 151.011 -60 0.384 1092.000 386.953 -60 446.953 532.000 161.757 -60 0.399 1120.000 397.133 -60 457.133 560.000 172.922 -60 0.403 1148.000 407.791 -60 467.791 588.000 184.205 -60 0.400 1176.000 418.962 -60 478.962 616.000 195.408 -60 0.438 1204.000 430.684 -60 490.684 644.000 207.659 -60 0.426 1232.000 450.159 -60 510.159 672.000 219.591 -60 0.373 1260.000 463.471 -60 523.471 700.000 230.033 -60 0.465 1288.000 477.502 -60 537.502 728.000 243.057 -60 0.438 1316.000 485.623 -60 545.623 756.000 255.320 -60 0.362 1344.000 492.312 -60 552.312 784.000 265.456 -60 0.386 1372.000 500.870 -60 560.870 812.000 276.271 -60 0.502 1400.000 517.000 -60 577.000 840.000 290.329 -60 0.221 將估測值減去對應的誤差值，就可以得到左右攝影機對應的 X 座標值。 _ _ _

pose x=est x−x error (3-16)

(3-16)式所代表的意義是，隨著距離的不同，在x座標產生的誤差量也不同，

由誤差方程式我們可以算出在不同距離時產生的誤差量，得到此誤差量後，將當 時x座標的估測值減掉誤差就能得到正確的x座標。

表 3.6 X 座標校正後的結果及誤差值

a

Z (mm) pose_X pose_X_error Za (mm) pose_X pose_X_error

280.000 -0.991 -0.991 868.000 0.822 0.822 308.000 -0.861 -0.861 896.000 0.734 0.734 336.000 -1.324 -1.324 924.000 0.637 0.637 364.000 -0.679 -0.679 952.000 -0.034 -0.034 392.000 -0.634 -0.634 980.000 -0.101 -0.101 420.000 0.224 0.224 1008.000 -0.831 -0.831 448.000 -0.492 -0.492 1036.000 -0.320 -0.320 476.000 -0.071 -0.071 1064.000 -0.401 -0.401 504.000 0.091 0.091 1092.000 -0.485 -0.485 532.000 0.296 0.296 1120.000 -0.573 -0.573 560.000 -0.186 -0.186 1148.000 -0.666 -0.666 588.000 -0.333 -0.333 1176.000 -0.764 -0.764 616.000 -0.091 -0.091 1204.000 -0.867 -0.867 644.000 0.172 0.172 1232.000 -0.264 -0.264 672.000 0.895 0.895 1260.000 -0.362 -0.362 700.000 1.254 1.254 1288.000 -0.465 -0.465 728.000 1.172 1.172 1316.000 0.302 0.302 756.000 1.570 1.570 1344.000 -0.574 -0.574 784.000 1.520 1.520 1372.000 0.212 0.212 812.000 1.465 1.465 1400.000 0.117 0.117 840.000 0.864 0.864

我們希望在影像伺服的過程中所觀測到目標物的 X 座標應該是一致的，不 能隨著距離的增加而有所變動，否則將會導致機器人在移動的過程中，無法朝著 目標物前進，而出現左右偏移的情況。由圖 3.9 可看出，X 座標的估測公式經過 校正後，可以讓誤差縮減到將近 5mm 之內，大大增加 X 座標的精確度。

3.2.3.

Y 軸座標值估測

軸座標值估測

軸座標值估測

軸座標值估測

在 Y 座標方面，一樣將對對應點代入(3-6)式中，求出各距離時的Y，在本 實驗中校正給定的 Y 座標Y_a是 27mm，而因為攝影機的配置是左右眼平行，所 以在攝影機平行於地面的情況下，量測出來的 Y 座標應是相同的值。 表 3.7 Yl 座標未校正前的估測值 Za(mm) Yl  Yla Yl_error Za(mm) Yl  Yla Yl_error 280.000 54.777 27 27.777 868.000 222.671 27 195.671 308.000 63.161 27 36.161 896.000 231.932 27 204.932 336.000 71.210 27 44.210 924.000 241.904 27 214.904 364.000 79.905 27 52.905 952.000 250.197 27 223.197 392.000 87.879 27 60.879 980.000 256.408 27 229.408 420.000 96.093 27 69.093 1008.000 264.338 27 237.338 448.000 104.577 27 77.577 1036.000 274.066 27 247.066 476.000 112.530 27 85.530 1064.000 280.031 27 253.031 504.000 121.502 27 94.502 1092.000 286.264 27 259.264 532.000 129.471 27 102.471 1120.000 294.158 27 267.158 560.000 137.569 27 110.569 1148.000 301.016 27 274.016 588.000 145.680 27 118.680 1176.000 309.646 27 282.646 616.000 153.651 27 126.651 1204.000 317.225 27 290.225 644.000 162.368 27 135.368 1232.000 330.854 27 303.854 672.000 169.917 27 142.917 1260.000 339.490 27 312.490 700.000 176.853 27 149.853 1288.000 348.592 27 321.592 728.000 184.776 27 157.776 1316.000 354.966 27 327.966 756.000 192.896 27 165.896 1344.000 359.859 27 332.859 784.000 200.062 27 173.062 1372.000 366.570 27 339.570 812.000 207.709 27 180.709 1400.000 377.117 27 350.117



Yl



Yl

圖 3.10 Yl 座標估測值 表 3.8 Yr 座標未校正前的估測值 a Z (mm) _Yr Yr_a Yr_error Za (mm) Yr Yra Yr_error 280.000 55.151 27 28.151 868.000 222.614 27 195.614 308.000 63.534 27 36.534 896.000 231.842 27 204.842 336.000 71.574 27 44.574 924.000 241.779 27 214.779 364.000 80.259 27 53.259 952.000 250.043 27 223.043 392.000 88.222 27 61.222 980.000 256.233 27 229.233 420.000 96.419 27 69.419 1008.000 264.132 27 237.132 448.000 104.885 27 77.885 1036.000 273.826 27 246.826 476.000 112.818 27 85.818 1064.000 279.767 27 252.767 504.000 121.771 27 94.771 1092.000 285.976 27 258.976 532.000 129.720 27 102.720 1120.000 293.843 27 266.843 560.000 137.795 27 110.795 1148.000 300.675 27 273.675 588.000 145.882 27 118.882 1176.000 309.274 27 282.274 616.000 153.829 27 126.829 1204.000 316.823 27 289.823 644.000 162.520 27 135.520 1232.000 330.403 27 303.403 672.000 170.043 27 143.043 1260.000 339.005 27 312.005 700.000 176.955 27 149.955 1288.000 348.071 27 321.071 728.000 184.848 27 157.848 1316.000 354.424 27 327.424 756.000 192.940 27 165.940 1344.000 359.297 27 332.297 784.000 200.082 27 173.082 1372.000 365.987 27 338.987 812.000 207.703 27 180.703 1400.000 376.492 27 349.492 840.000 217.716 27 190.716

 Yr  Yr 圖 3.11 Yr 座標估測值 從Y的統計圖表可看出，大致上跟距離成正比，誤差隨距離逐漸增加，所以 我們要求出其誤差方程式，針對不同距離減去對應的誤差值，得到正確的 Y 座 標。  y0

(

)

Y λ Z λ = − 為估測出來的 Y 座標，接著算出 pose_Z 跟 Y 座標誤差 y_error 之間的誤差方程式，(3-18)及(3-19)是透過 MATLAB 所求出來的誤差方程式。 _ 50.722 0.28601* _ yl error= − + pose Z (3-18) _ 49.993 0.28507 * _ yr error= − + pose Z (3-19) 將估測值減去對應的誤差值，就可以得到左右攝影機對應的Y座標值。

pose_y=est_y−y error_ (3-20)

(3-20)式所代表的意義是，隨著距離的不同，在y座標產生的誤差量也不 同，由誤差方程式我們可以算出在不同距離時產生的誤差量，得到此誤差量後， 將當時y座標的估測值減掉誤差就能得到正確的y座標。 分別將左影像跟右影像的結果代入得到pose_yl跟pose_yr，將兩個值相加取 平均，就能得到目標點的Y座標值 = +

度是在哪個位置，也就是手臂在抓取時所要移動到的垂直位置，所以 Y 座標必 須要準確，機器人才能確實的抓到目標物。 由校正後的 Y 座標誤差統計圖表可看出，經過以上求出來的誤差方程式， 可以精確的修正 Y 座標的誤差達到 5mm 以內，確實增加量測精確度。 在完成 3.2 節中 3D 座標估測公式的校正後，我們可以由左右影像平面上的 對應點算出相對的 3D 座標，並且可以將誤差縮小到 10mm 之內，對於之後手臂 的控制，平台的控制，甚至是整個移動式機械臂(Mobile Manipulator)所需的控制 命令都有相當大的助益。 表 3.9 Y 座標校正後的結果及誤差值 a

Z (mm) pose_Y error_Y Za (mm) pose_Y error_Y

280.000 24.787 -2.213 868.000 26.754 -0.246 308.000 25.048 -1.952 896.000 26.793 -0.207 336.000 24.999 -2.001 924.000 26.832 -0.168 364.000 25.273 -1.727 952.000 27.160 0.160 392.000 25.691 -1.309 980.000 27.792 0.792 420.000 25.752 -1.248 1008.000 26.919 -0.081 448.000 26.092 -0.908 1036.000 27.275 0.275 476.000 26.156 -0.844 1064.000 26.649 -0.351 504.000 27.017 0.017 1092.000 25.995 -1.005 532.000 27.611 0.611 1120.000 26.685 -0.315 560.000 27.557 0.557 1148.000 26.001 -0.999 588.000 27.666 0.666 1176.000 26.722 -0.278 616.000 27.961 0.961 1204.000 26.005 -0.995 644.000 28.281 1.281 1232.000 26.390 -0.610 672.000 27.977 0.977 1260.000 25.616 -1.384 700.000 27.847 0.847 1288.000 24.801 -2.199 728.000 26.586 -0.414 1316.000 26.011 -0.989 756.000 26.389 -0.611 1344.000 25.597 -1.403 784.000 26.414 -0.586 1372.000 26.851 -0.149 812.000 26.440 -0.560 1400.000 26.011 -0.989 840.000 26.210 -0.790

圖 3.12 校正後 Y 座標誤差值

3.3

三軸旋轉角

三軸旋轉角

三軸旋轉角

三軸旋轉角

一個物體在 3

ℝ 姿態(pose)（位置及朝向）通常是由六自由度表示：三個給位 置，三個給朝向。因此，目標物的姿態(pose)通常被更簡潔地寫為 pose vector

( , , , , , )

p= X Y Z φ θ ϕ Τ，其中 X、Y、Z 代表位置，φ 、 、θ ϕ 是尤拉角(Euler angles)

代表朝向。 在做完3D座標估測後，我們可以利用Homography框出目標物在影像平面 上的位置，如圖 2.12跟圖 2.13，我們可以將這四個角點，當成4組匹配的特徵 點，算出四個角點的3D座標，選定其中三個座標，組成V_x跟V_y，再用V_x跟V_y外 積得到V_z，如圖 3.13所標示，利用這三個向量跟XYZ三軸的夾角，就可以知 道目標物相對於世界座標軸，分別旋轉了幾度如 (3-22)、 (3-23)、 (3-24)。 1 90- cos | || | y y v Z v Z φ ₌ −   i   (3-22) 1 90- cos | || | z z v X v X θ ₌ −   i   (3-23)  

這個設計的目的是在於得到目標物相對於機器人本體的角度變化，因為在更 進一步的抓取規劃上，目標物不見得是直立於桌面，且正面朝著攝影機，他可能 有其他角度的變化，例如:左右傾斜，前後傾斜，正面向左偏轉幾度等情況。若 是我們能夠知道這些狀態，就可以調整我們抓取的姿態，以最適合的方式抓取目 標物[12]。 為了驗證這個三軸角度計算方法的正確性，我們任意選取幾個角度，對每一 筆選取的角度做十次估測取其平均值，觀察其實際估測結果所呈現出的誤差情 況，如表 3.10 所示，當各軸旋轉的角度加大時，雖然是對單一軸旋轉，其他軸 也會被算出轉動的角度，整體來說，各軸在旋轉 30 度之內時，誤差大致上在 10 度之內，但是隨著角度上升，誤差就更大，所以目前來說，這個角度估測方法適 用在小角度變化時的計算，若是出現大角度的旋轉時，需要再輔以其他的演算 法，以便能算出更精確的角度變化。 φ θ ϕ 圖 3.13 目標物的三軸向量與世界座標的旋轉關係

表 3.10 三軸旋轉角實際測試結果

Assigned angle Experiment angle error

X Y Z X Y Z X Y Z 0 0 0 -1.752 -1.771 0.204 1.752 1.771 0.204 -20.000 0.000 0.000 -19.365 -2.159 1.449 0.635 2.159 1.449 30.000 0.000 0.000 30.743 -16.348 -7.143 0.743 16.348 7.143 0.000 20.000 0.000 1.817 21.084 1.007 1.817 1.084 1.007 0.000 -30.000 0.000 7.481 -29.488 -2.536 7.481 0.512 2.536 0.000 0.000 30.000 0.946 -1.273 33.264 0.946 1.273 3.264 0.000 0.000 -20.000 8.475 -9.083 -21.417 8.475 9.083 1.417 0.000 0.000 -40.000 7.929 -10.085 -38.624 7.929 10.085 1.376

第

第

第

第四

四

四章

四

章

章移動式機械臂之

章

移動式機械臂之影像伺服

移動式機械臂之

移動式機械臂之

影像伺服

影像伺服

影像伺服控制

控制

控制

控制設計

設計

設計

設計

經由第二章跟第三章所介紹的方法之後，我們可以得到目標物在空間中的三 維座標。本章節的目的在說明我們如何使用這個估測出來的座標當作控制器的輸 入，導引機器人向前移動並抓取目標物。首先在 4.1 節跟 4.2 節之中，我們分別 介紹機械手臂跟全向式移動平台的控制設計。在 4.3 節中，將會介紹移動式機械 臂整體的系統架構，說明移動式抓取的方法跟設計理念。

4.1

手臂控制

手臂控制

手臂控制

手臂控制

整體的控制透過工業電腦(IPC)，而利用 CANbus 作為傳輸介面，控制各馬 達的運作。手臂的馬達控制器是由 Maxon 公司所生產的 EPOS Positioning

Controller，我們可以用用其內建的指令集，針對我們控制上的需要去撰寫程式。 在本論文中，主要用到的是位置控制，當我們經由逆運動學得到各軸旋轉的角度 後，將角度轉為馬達轉動所需要的資料型態，將這些資訊送給馬達，就可以讓手 臂轉到我們所指定的座標位置，而在手臂從起點移動到終點的過程中，我們可以 藉由讀回編碼器(Encorder)的數值，判斷各軸馬達是否已經轉到我們給定的轉動 量，以便確認手臂是否已經移動至終點。

4.1.1.

硬體構造

硬體構造

硬體構造

硬體構造

本論文所使用的機械手臂是由實驗室所自行設計的雙臂構造。為一具有 6 自由度的機械手臂，其中在肩膀及手肘關節的部分使用的是 Harmonic driver，這 種設計可以讓機械手臂在運作時動作更加的平順。如圖 4.1 所示，單邊肩長 198mm，肩膀到手軸長 285mm，手軸到手腕長 287mm，手掌長度約為 200mm。

4.1.2.

座標轉換

座標轉換

座標轉換

座標轉換

本論文中所使用的實驗平台由數個部分組合起來，包括立體攝影機、機器人 頭部、機械手臂，移動平台，為了整合機器人的各個部分需要知道每個座標間的 轉換關係，以利於整體的控制設計。機器人的座標原點定在雙臂肩關節的連線，

圖 4.1 機械手臂設計圖 跟頭部旋轉軸的交叉點，其中攝影機是主要的資訊來源，所以必須知道攝影機座 標到手臂及平台間的座標轉換。整個移動式機械臂之間的座標關係，如圖 4.2 所示，其中的符號定義如下，C：立體視覺攝影機、B：機械手臂本體中心、E： 機械手臂的手腕點、G：機械手臂的夾爪末端、P：全向式移動平台、O：欲抓取 的目標物體，而A B H 矩陣代表的意義是從 A 座標系轉換到 B 座標系之轉換矩陣。 我們得知了機器人各個部位的座標轉換關係之後，就能夠針對控制的需求，將命 令轉換到適當的座標系，例如:當機器人位於機械手臂的工作範圍之外時，我們 就必須將攝影機所觀測到的座標轉換到移動平台的座標系，給出適當的命令，導 引平台移動到手臂的工作範圍之內。 在圖 4.3 中所表達的是機器人本體的座標系(ver.new0)跟右機械手臂座標系 (ver.0)之間的座標轉換關係，我們希望在手臂控制方面能夠先轉換到一個統一的 座標系，之後再根據逆運動學的需要，將座標系轉換到左手或是右手，以符合手 臂逆運動學所需的座標系。機器人本體的座標系(ver.new0)跟右機械手臂座標系 (ver.0)之間的座標轉換關係之中，在 X 軸跟 Z 軸的座標原點及方向是一致的，不