無校正視覺伺服系統之影像Jacobian矩陣估測研究

國 立 交 通 大 學

電機與控制工程研究所

碩 士 論 文

無校正視覺伺服系統之

影像 Jacobian 矩陣估測研究

The Estimation of Image Jacobian Matrix

for Image-based Uncalibrated Visual Servoing

研 究 生： 許 育 綸

指導教授： 胡 竹 生 博士

無校正視覺伺服系統之

影像 Jacobian 矩陣估測研究

The Estimation of Image Jacobian Matrix

for Image-based Uncalibrated Visual Servoing

研 究 生：許 育 綸

Student：Yu-Lun Hsu

指導教授：胡 竹 生 博士

Advisor：Jwu-Sheng Hu

國立交通大學

電機與控制工程學系

碩 士 論 文

A Thesis

Submitted to Institute of Electrical and Control Engineering

College of Electrical Engineering and Computer Science

National Chiao Tung University

in partial Fulfillment of the Requirements

for the Degree of Master

in

Electrical and Control Engineering

June 2009

Hsinchu, Taiwan, Republic of China

無校正視覺伺服系統之

影像 Jacobian 矩陣估測研究

研究生：許 育 綸

指導教授：胡 竹 生 博士

國立交通大學電機與控制工程研究所碩士班

摘要

摘要

摘要

摘要

本文建立一個以影像為基礎的視覺回授系統，此系統使用不需要校正的

相機，估測並更新影像 Jacobian 矩陣，進而控制機器手臂到達所需的位置。

本文將視覺回授系統分成兩大部分。第一部份是影像特徵的擷取，我們把

電腦視覺的平均位移法，應用在特徵擷取上，以提高特徵追蹤的準確率。

平均位移法是以空間中的色彩分佈為特徵，並且以相似函數來鎖定追蹤的

對象，在複雜的環境，也能即時準確的擷取特徵。第二部分是視覺回授的

控制，我們利用簡單的演算法，估測相機與機器手臂末端的關係。相較於

其他文獻，此方法易於實現。

在模擬的部分，本文透過仿真的相機與機器手臂，可以有效的模擬。而

實作的部份，本文僅使用一台電腦，來完成所有的軟體開發，包括對機器

手臂的傳輸與雙相機的處理，以解決全域與區域的 Jacobian 問題。最後，

在實驗的部份，我們選擇使用球具的球類運動，加入了運動偵測的演算法，

展示機器手臂連續擊球的任務，並將結果加以分析與討論。

The Estimation of Image Jacobian Matrix

for Image-based Uncalibrated Visual Servoing

Student： Yu-Lun Hsu

Advisor： Prof. Jwu-Sheng Hu

Institute of Electrical and Control Engineering

Abstract

This thesis proposes an image-based visual servo system, which can estimate and update Image Jacobian Matrix to control a robot arm without camera calibration. There are two parts in the visual servo system. The first one is image feature extraction. We apply the Mean-Shift algorithm in order to improve the performance of feature tracking. Mean-Shift algorithm, which takes the color distribution as a model, is based on the similarity measure function to decide a tracking candidate. This algorithm works well in a real-time and complicated environment. The second part is the control of a manipulator to track visual objects. An easy-to-implement algorithm is proposed to find the relationship between the camera and the manipulator. It can be carried out easily as compared with other methods.

We use the camera and the manipulator emulator to simulate the system effectively. In the implementation, we use only one computer to develop all the software, including the communication to the manipulator and the processing of two cameras' images to solve the local and the global Image Jacobian matrices. Ball-hitting experiments such as the juggling task are presented to analyze the real-time performance of the proposed algorithms.

誌謝

誌謝

誌謝

誌謝

本文能順利完成，首先要感謝我的指導教授－胡竹生教授、口試委員 楊谷洋教授與蔡文祥教授的提醒，其中楊教授的鼓勵與蔡教授的親切讓我 終生難忘。胡教授是我在大學時專題的指導教授，教授的博學多聞、態度 熱誠等等，開拓了我的視野，激發了我的潛能，在這裡真的要好好謝謝他。 接著，我要感謝嵌入式系統實驗室的好伙伴們，例如幫我校正英文的 嘟嘟、助教合作愉快的 judo、跟我搶著用 DV 的肉鬆、與我同一屆畢業的 文棋、營隊高手的 gum、常跟我練習英文的活多福、屢屢幫我突破瓶頸的 阿吉、一起去深圳比賽的興哥、帶我們拿第一名的永融、幫忙維修網站的 大師兄、數學超強的學姊鏗元、讓我沉住氣的唐哥、已經畢業還常被我們 諮詢的 papa 與 hcy、從總務退休還要被我煩的俊宇與宗敏、傳承電影給我 的劉大、幫實驗室打點很多事的淑伶、常陪我聊天的小蔡、用功的塞門、 很靦腆的阿 him、超像馬拉桑的沛錡、低調的聖翔、復出的阿法等等。 此外，也要感謝與我同名的黃育綸教授，與她的實驗室那群陪我玩樂 的同學們，阿彬、道哥、雕兄、宗勳、小狼、小叮噹，還有那些還與我有 聯絡的朋友們，與那些默默關心我的的朋友們，真誠的感謝你們。最後， 我要感謝扶養我的老爸、教育我的老媽、一同長大的老哥老弟、每年團圓 一次的家人們與在天堂的奶奶、每一位喜歡過我、愛過我的人們。 在交大的六年，讓我從搖滾青年，蛻變成宅宅工程師。我曾經對未來 迷惘、對社會失望，而年輕只是一種過程，最終我們還是必須面對這世界。 除了念書，抽空回家、聚餐約會、辦營隊、玩樂團、弄社團、搞梅竹等等， 這之間夾雜的友情、愛情與稍縱即逝的親情，讓我學著如何應對關於成長 這回事。如果這真的是一種過程，我要感謝至今陪我兩年的雷子曦與她的 家人，他們讓我重新相信了一些事情。如果沒有她就沒有現在的我，或是 說沒有她的話，我無法想像現在的我是什麼樣子。

目錄

目錄

目錄

目錄

摘要 摘要 摘要 摘要...i Abstract...ii 誌謝 誌謝 誌謝 誌謝... iii 目錄 目錄 目錄 目錄...iv 表目錄 表目錄 表目錄 表目錄...v 圖目錄 圖目錄 圖目錄 圖目錄...vi 一 一 一 一、、、、緒論緒論緒論緒論...1 1.1 1.1 1.1 1.1 研究背景動機研究背景動機研究背景動機研究背景動機 ...1 1.2 1.2 1.2 1.2 相關架構分類相關架構分類相關架構分類相關架構分類 ...1 1.3 1.3 1.3 1.3 文獻回顧探討文獻回顧探討文獻回顧探討文獻回顧探討 ...4 1.4 1.4 1.4 1.4 論文成果貢獻論文成果貢獻論文成果貢獻論文成果貢獻 ...6 1.5 1.5 1.5 1.5 章節概要章節概要章節概要章節概要 ...6 二 二 二 二、、、、相機系統相機系統相機系統相機系統 ...7 2.1 2.1 2.1 2.1 系統簡介系統簡介系統簡介系統簡介 ...7 2.2 2.2 2.2 2.2 相機模型相機模型相機模型相機模型 ...7 2.3 2.3 2.3 2.3 相機參數相機參數相機參數相機參數 ...9 2.4 2.4 2.4 2.4 其他模型其他模型其他模型其他模型 ...10 2.5 2.5 2.5 2.5 相機校正相機校正相機校正相機校正 ... 11 三 三 三 三、、、、特徵擷取特徵擷取特徵擷取特徵擷取 ...15 3.1 3.1 3.1 3.1 系統簡介系統簡介系統簡介系統簡介 ...15 3.2 3.2 3.2 3.2 方法分析方法分析方法分析方法分析 ...16 3.3 3.3 3.3 3.3 特徵追蹤特徵追蹤特徵追蹤特徵追蹤 ...18 3.4 3.4 3.4 3.4 實驗模擬實驗模擬實驗模擬實驗模擬 ...22 四 四 四 四、、、、視覺回授視覺回授視覺回授視覺回授 ...26 4.1 4.1 4.1 4.1 系統簡介系統簡介系統簡介系統簡介 ...27 4.2 4.2 4.2 4.2 方法分析方法分析方法分析方法分析 ...27 4.3 4.3 4.3 4.3 影像回授影像回授影像回授影像回授 ...31 4.4 4.4 4.4 4.4 實驗模擬實驗模擬實驗模擬實驗模擬 ...37 五 五 五 五、、、、模擬實作模擬實作模擬實作模擬實作 ...42 5.1 5.1 5.1 5.1 任務介紹任務介紹任務介紹任務介紹 ...43 5.2 5.2 5.2 5.2 模擬簡介模擬簡介模擬簡介模擬簡介 ...44 5.3 5.3 5.3 5.3 模擬結果模擬結果模擬結果模擬結果 ...47 5.4 5.4 5.4 5.4 實作簡介實作簡介實作簡介實作簡介 ...50 5.5 5.5 5.5 5.5 實作結果實作結果實作結果實作結果 ...57 六 六 六 六、、、、結論結論結論結論...65 七 七 七 七、、、、相關文獻相關文獻相關文獻相關文獻 ...66

表目錄 表目錄 表目錄 表目錄 表格 1.相機校正後實際的內部參數表...14 表格 2.平均位移法常用的 kernel 函數表...19 表格 3.Logitech Quick-cam 規格...52 表格 4.Mitsubishi RV-M2 手臂規格...53 表格 5.Mitsubishi RV-M2 控制器規格...53 表格 6.Visual C++與 MFC 版本對照表...55 表格 7.球棒與球的打擊實驗表...63 表格 8.圖形處理的時間測試表...64

圖目錄 圖目錄 圖目錄 圖目錄 圖 1.位置基礎視覺觀察運動架構圖...2 圖 2.位置基礎視覺伺服控制架構圖...2 圖 3.影像基礎視覺觀察運動架構圖...3 圖 4.影像基礎視覺伺服控制架構圖...3 圖 5.相機幾何座標與透鏡失真圖...8 圖 6.針孔相機模型圖解...10 圖 7.相機校正實驗圖解...12 圖 8.實際校正步驟圖...13 圖 9.校正前後比較圖...14 圖 10.影像空間濾波器圖解...17 圖 11.機率分布的函數比較圖...19 圖 12.平均位移演算法流程圖...21 圖 13.主要成分分析圖...22 圖 14.平均演算法的追蹤結果示意圖...23 圖 15.平均位移法的誤差圖與遞迴次數圖...24

圖 16.SONY EVI-D70 PTZ 攝影機與 EVERFOCUS 彩色 TFT LCD 螢幕....25

圖 17.平均位移法的即時追蹤結果圖...25 圖 18.視覺回授示意圖...28 圖 19.遞迴影像 Jacobian 矩陣即時估測法流程圖...35 圖 20.排除異常值之遞迴即時估測法流程圖...37 圖 21.遞迴即時估測法模擬示意圖...38 圖 22.遞迴即時估測法模擬誤差圖...39 圖 23.排除異常值之遞迴即時估測法模擬示意圖...40 圖 24.排除異常值之遞迴即時估測法模擬誤差圖...40 圖 25.視覺回授硬體軟體分配架構圖...42 圖 26.MATLAB 機器手臂工具箱原始使用介面圖示...45 圖 27.MATLAB 機器手臂工具箱新增顯示函數圖示...45 圖 28.MATLAB 世界座標系與相機座標系模擬圖...46 圖 29.MATLAB 模擬程式流程圖...47 圖 30.相機影像誤差造成的位置誤差模擬圖...49 圖 31.手臂位置誤差造成的位置誤差模擬圖...50 圖 32.影像基礎視覺觀察運動實作架構圖...51 圖 33.實作使用的相機系統...52 圖 34.實作使用的機器手臂...54 圖 35.機器手臂的工作範圍...54 圖 36.機器手臂五軸示意圖...54 圖 37.MFC 與 OpenCV 實際程式流程圖...56

圖 38.棒球與手套的位置判斷實驗...58 圖 39.球拍與球的位置判斷實驗...59 圖 40.運動偵測程式流程圖...60 圖 41.球棒與球的打擊實驗前半段...61 圖 42.球棒與球的打擊實驗後半段...62 圖 43.球棒與球的打擊實驗狀況一...62 圖 44.球棒與球的打擊實驗狀況二...63

一

一

一

一、

、

、

、緒論

緒論

緒論

緒論

1.1

1.1

1.1

1.1 研究背景

研究背景

研究背景動機

研究背景

動機

動機

動機

在眾多的機器人感測器之中，視覺感測有許多優點。第一、機器人視覺感測 相似於人類的視覺感測[1]，比起其他感官，人類的視覺行為較容易撰寫成演算 法。第二、視覺感測不需要直接的觸碰環境[1]，也能有相對較佳的精確度。 機器人的視覺研究類似於電腦視覺，能作到追蹤、偵測、定位等工作[2]， 若是機器人利用視覺感測的資訊來控制機器手臂，進而影響感測的環境，就可以 稱之為視覺回授(visual feedback)[1][3]。此概念由 Shirai 與 Inoue[4]所提出，是 為了增加機器人動作的準確度，整合了機器手臂控制與視覺感測，這方面的研究 包含控制理論、機器人運動學、動力學、相機校正、高速影像處理與即時運算。 簡單來說，機器人不再只是被動觀察環境，而是可以主動改變觀察的內容，甚至 可以改變觀察角度。對於機器人來說，如何善用視覺感測，是相當重要的議題。 基於上述的需求與應用，過去已經有不少此方面的研究成果。由於這些研究 是屬於跨領域的範圍，因此在各領域下，也有更多更深入的探討，而整體的研究 必須在各領域發展到一定的程度才能突破。幸運的是，近年來機器人技術與電腦 的升級，有助於此項研究的推進[1]。過去關鍵的問題，譬如視覺感測器、計算 量等，已經能夠從軟硬體的方面改善，以往機器人比較難做到的任務也能完成。 在這些知識基底下，我們能夠改善視覺回授的表現，來解決更多的課題與任務。

1.2

1.2

1.2

1.2

相關

相關

相關架構分類

相關

架構分類

架構分類

架構分類

Shirai 與 Inoue[4]在 1973 年整合機器手臂控制與視覺感測，描述視覺回授 的概念，也就是先觀察再移動(looking then moving)，如此一來結果將由兩者各自 的準確度來決定。在 1979 年，Hill 與 Park[5]提出視覺伺服(visual servo)的觀念， 建立新的系統。隔年 Sanderson 與 Weiss[6]整理分析出四種系統架構，依照兩個 問題來分類，第一、機器手臂是否有各關節角度的回授。第二、得到視覺感測器

的資訊後，是否直接使用影像特徵來定位(image-based)，或是算出空間中的位置 來定位(position-based)。以下我們依序介紹這四種系統架構[1][3][7]：

1.位置基礎視覺觀察運動架構(Dynamic position-based look-and-move structure) 此架構以位置為基準，先利用理論算出各關節角度，再由控制器移動手臂， 最後用視覺感測器來擷取影像特徵，估算出手臂的位置。

圖 1.位置基礎視覺觀察運動架構圖

2.位置基礎視覺伺服控制架構(PBVS, Position-based visual servo structure)

此架構以位置為基準，先利用理論算出各關節的角度之後，直接控制手臂， 最後用視覺感測器來擷取影像特徵，估算出手臂的位置。

圖 2.位置基礎視覺伺服控制架構圖

3.影像基礎視覺觀察運動架構(Dynamic image-based look-and-move structure) 此架構以影像為基準，先利用理論算出各關節角度，再由控制器移動手臂，

最後用視覺感測器來擷取影像特徵，比對影像特徵的變化。

圖 3.影像基礎視覺觀察運動架構圖

4.影像基礎視覺伺服控制架構(IBVS, Image-based visual servo structure)

此架構以影像為基準，先利用理論算出各關節的角度之後，直接控制手臂， 最後用視覺感測器來擷取影像特徵，比對影像特徵的變化。 圖 4.影像基礎視覺伺服控制架構圖 依照不同的軟硬體設計，可以選擇不同的系統架構，也有不同的優缺點。以 位置為基礎的話，優點是卡式座標系的轉換比較容易，而缺點是必須校正相機的 參數，對於校正參數的誤差也非常敏感[1]。相機校正的參數可以分成外部參數 (external parameters)與內部參數(intrinsic parameters)，外部參數包含相機的位置與 角度，內部參數包含焦距、透鏡失真、影像中心、影像傾斜，這方面的研究目前 已經相當的完整，譬如 R. Y. Tsai [8] 的校正技術或是 Zhang [9] 的校正模型。以

影像為基礎的話，優點是校正參數的誤差比較不影響結果，缺點是特徵擷取變得 非常重要，這裡可以參考 Jang 與 Bien[10]的一般影像特徵擷取的方法。 除了上述的分類方法，根據視覺感測器的架設位置，可以分成 eye-in-hand 與 eye-to-hand 兩種[1][3]。eye-in-hand，顧名思義是將感測器裝在機器手臂上， 故手臂與感測器的相對位置是已知的。而 eye-to-hand 是將感測器裝在工作環境 中，感測器與手臂移動是各自獨立的。如何搭配視覺感測器的架設，來選擇適當 的系統架構，可以參考 Hutchinson et al.[1]的指導文獻與 Corke[7]的回顧文獻。

理論上，因為採用以位置為基礎的架構需要相機校正，所以通常將視覺感測 器固定，而不論感測器是否移動，都可以選用以影像為基礎的架構。然而，無論 感測器是藉由機器手臂移動[11]，或是本身有轉動機制來改變觀察的內容[12]， 皆可稱之為主動式視覺(active vision)，部份文獻將這個領域獨立出來討論，本文 不將重點放在這個部份，這邊不再深入撰寫，更多的細節可參考[11-13]。

1.3

1.3

1.3

1.3

文獻回顧探討

文獻回顧探討

文獻回顧探討

文獻回顧探討

引用[1][3][7]的定義，我們利用系統架構，將文獻分門別類，可發現大部分 是採用以位置為基礎的系統，再搭配上相機校正的技術[8][9]，此種方法優點是 系統簡單容易實現，缺點是對機器人來說，使用雙眼立體視覺重建環境時，校正 誤差將嚴重影響結果[1]。若是以影像為基礎，雖然不必將影像換算回卡式座標 系，但必須將影像移動與物體移動的關連性，利用其他方式來表示。在 1987 年， Sanderson et al.[14]提出了 Image Jacobian 的概念。此方法在 Papanikolopoulos 與 Khosla[15][16]與 Espiau et al.[17]的文獻中雖改用不同的名詞，但基本觀念是相同 的。接著，許多文獻開始研究決定 Jacobian 的方法，譬如區域線性[17]、估測法 [18-20]、預測法[21]、實驗法[22]等等，各種方法有各自的使用的條件，其中[18][19] 不仔細考慮相機與機器人的模型，而是觀察靜態的目標來估測，相反的，[23][24] 是依照其模型來觀察動態的目標。最後，Piepmeier et al.[25][26]結合以上的優點，

在不根據模型的情況下，利用靜態相機，追蹤動態目標的 visual servoing。基本 上，靜態的 eye-to-hand 能完成大範圍的任務，而精確度比 eye-in-hand 低。

在 1994 年，Yoshimi et al.[11] 提出不需要校正的主動式視覺伺服概念(Active, Uncalibrated Visual Servoing)，之後 N. Hollinghurst et al.[27]加入立體視覺系統， Hager et al.[28]則是修正了估計誤差。除此之外，Chaumette et al.[29]、Jang 與 Bien[10]、Papanikolopoulos 與 Khosla[15][16]延續 eye-in-hand 的高解析度優點， 然而 eye-in-hand 的系統容易被兩種狀況限制住，一、相機或機器人模型。二、 需要靜態的目標。在[31]，Asada 與 Hosoda 擴展到移動的目標。最後，[32]在 不根據模型的狀況下，解決不需要靜態的參考目標，來完成 visual servoing 的系 統。理論上，動態的 eye-in-hand 雖然有比較好的解析度，但缺乏全域的資訊。 在實驗的評估方面，Wijesoma et al.[33]展示了兩個自由度的實現，Chan et al.[34]則是作到 peg-in-hole 的動作，在 1997 年，Jagersand 在[35]有深入的整理， 朝向更多自由度、更通用的演算法來推進。事實上，以往的研究，偏向解決區域 性的問題，在 2007 年，Jagersand 在[36][37]將區域推廣到全域，使用 K-Nearest Neighborhood (K-NN)演算法來處理。另外，也可以利用兩個相機來解決，譬如 在[19]的實驗展示，還有[38][39]所提出的系統架構，這方面的研究不屬於雙相機 的立體視覺，而是綜合 eye-in-hand 與 eye-to-hand 的合作[40]，結合兩種架構的 優點，混合全域與區域的資訊，增加了任務的可執行度與準確度。 以上整理了以影像為基礎的架構，探討重點在研究 Jacobian 估測法的文獻。 不論在 eye-to-hand 或 eye-in-hand，都已經可以作到追蹤動態目標，而不用事先 知道模型的資訊，而相關理論的證明、穩定度的討論、控制器的選用、實作等等 也有足夠的文獻可以支持[37]。至於全域 Jacobian 的問題，除了上述的全域演算 法之外，也可以改變硬體架構，譬如用雙相機合作來解決。值得注意的是，上述 的機器手臂系統，其輸入可能是手臂末端的位置，或是各關節角度[32]，故如何 針對各種硬體，撰寫適合的軟體，是此研究的關鍵，也是文獻最值得參考的地方。 除此之外，系統估測的演算法已經十分繁多，而且針對任務的種類，將系統

特化的研究也趨於完善，而對於一般性的任務，卻較少有詳細的探討。這些方法 無論使用伺服控制架構，或是觀察運動架構，基本骨架都是估測出影像特徵移動 與物體移動的關連，故影像處理與任務也較簡單，像是 peg-in-hole、pick-and-place 等等。實際上，利用改善估測法與擷取有效的特徵，來確保低運算量與高強健度， 才是最根本的問題。多數的文獻將資源分配在後端，前端只有簡單的影像處理。 在這邊我們可以活用電腦視覺研究的知識基底[41-44]，發揮在影像處理上，這樣 即可在更複雜的環境，用機器手臂執行更艱難的任務，並且達到即時的反應。

1.

1.

1.

1.4

44

4

論文成果

論文成果

論文成果貢獻

論文成果

貢獻

貢獻

貢獻

本文利用 Image Jacobian 矩陣，估測無校正的相機與機器手臂末端的關係， 達成視覺回授，同時也利用雙相機合作，解決全域與區域的問題。在演算法部分， 相機影像的特徵擷取可以在複雜的環境下，達到即時追蹤，而視覺回授可以透過 RS-232 的傳輸，從電腦端即時命令機器手臂的動作。在模擬的部份，本文介紹 仿真的機器手臂軟體，達到有效的模擬。而實作時，我們只用一台電腦完成所有 的軟體開發，滿足即時的運算。最後在實驗的部份，我們選擇了使用球具的球類 運動來展示，像是追球、擊球、挑球等等，完成整個視覺回授的系統研究。

1.

1.

1.

1.5

55

5

章節概要

章節概要

章節概要

章節概要

本文可以分成四大部分：第一章的概括介紹、第二章至第四章的系統分析、 第五章的系統實現、第六章的結論。其中第二章到第四章是個別分析系統的內部 架構，而第五章是討論整個架構如何模擬與實作。這兩大部份的介紹如下： 第二章：系統硬體的模型，我們省略機器手臂模型，只分析相機模型。 第三章：特徵擷取的推導與模擬，包含演算法討論與其他方法的比較。 第四章：視覺回授的推導與模擬，包含演算法討論與其他方法的分析。 第五章：系統的模擬實作，選擇相關硬體後，用軟體實現演算法來完成實驗。

二

二

二

二、

、

、

、相機系統

相機系統

相機系統

相機系統

本文強調的是以影像為基礎的架構，而以位置為基礎的研究可以參考[1]。 為了方便描述，我們將整個系統切割成各個子系統，接著分析每個子系統，最後 再將它們組合起來，探討整體的架構。依照研究領域與前面分類的系統方塊圖， 我們可以將整個系統略分成四大塊，分別是機器手臂系統、相機系統、特徵擷取 系統、視覺回授系統，其中前兩者以硬體為主，後兩者是以軟體為主。由於每篇 文獻都有各自不同的機器手臂系統，因此我們先不限定系統是採用伺服控制架構 (visual servo structure)或觀察運動架構(dynamic look-and-move structure)，讓整個 討論更加的通用，接著我們依照順序介紹各個子系統，並且將它們方程式化。

2

22

2.

..

.1

11

1

系統簡介

系統簡介

系統簡介

系統簡介

相機系統的輸入是觀察的內容，輸出是數位的影像。一般來說，CCD 內部 的光電轉換工作晶片，將光強度訊號轉成類比訊號後，經由影像擷取卡或者 A/D 卡，將類比影像資訊轉換成數位資訊。不同的相機有不同的晶片，其輸出的格式 也略有不同，處理方式也有所不同[3]。相同的是，整個相機成像可以分成四個 步驟，由這些步驟定義了相機的外部與內部參數，此為 R. Y. Tsai 在 [8] 提出的 模型，而 Zhang [9] 將此模型以另一種方式表示，定義了同樣的參數。以下我們 採用最常見的針孔模型(pinhole model)，依序介紹兩種相機模型。

2

22

2.

..

.2

22

2

相機

相機

相機模型

相機

模型

模型

模型

R. Y. Tsai 的模型是由四種步驟構成，下圖是前三步驟的相機幾何模型。 1.由世界座標系轉換成相機座標系 在 3D 的世界座標系當中，(x_w,y_w,z_w)是質點P的座標值。而在 3D 的相機 座標系當中，( , , )x y z 也是質點P的座標值。依照幾何座標系轉換，我們可以將 (x_w,y_w,z_w)與( , , )x y z 表示成下列的關係，其中R是旋轉矩陣，T是平移矩陣。

11 12 13 21 22 23 31 32 33 ,where and w x w y w z x x r r r T y y r r r T z z r r r T          ₌  _{+ =}  ₌                          R T R T (1) 圖 5.相機幾何座標與透鏡失真圖 2.由相機座標系轉換成理想影像座標系 本步驟用針孔模型，以透視投影(perspective projection)來成像，如上圖所示， (X Y_u, _u)是理想影像座標系的座標，其中O₁為影像平面的中心，O₁與O的距離 f

稱之為焦距(effective focal length)，( , , )x y z 與(X Y_u, _u)的關係如下：

, u u x y X f Y f z z = = (2) 3.由理想影像座標系轉換成透鏡失真座標系 上圖我們可以發現相機實際成像的位置(X Y_d, _d)與理想狀況(X Y_u, _u)不同，此 現象被稱為透鏡失真，其中失真係數可以用κ₁與κ₂表示，它們的關係如下： 2 4 1 2 2 2 2 4 1 2 ( ) and ( ) d u x d u y x d d d y d X X D Y Y D D X r r r X Y D Y r r κ κ κ κ = −   _{= −}   = +  _{= +}  _{= +}  (3)

由[8]，我們可以知道透鏡失真分成radial與tangential兩種，然而在實驗中 我們發現，只需要考慮κ₁的radial失真，其他項影響甚小，可以忽略不計。 4.由透鏡失真座標系轉換成實際影像座標系 最後，(X Y_d, _d)經由運算後，可以轉換成實際的影像pixel (X_f,Y_f)，其中 (C C_x, _y)是中心影像pixel，而s_x是比例參數，其他則是可以參閱文件的設計參數。 1 1 ' ,where ' f x x d x cx x x f y d y fx X s d X C N d d Y d Y C N − −  = +  ₌  _{= +}  (4)

2

22

2.

..

.3

33

3

相機

相機

相機參數

相機

參數

參數

參數

上面四個步驟，定義了六個外部參數，五個內部參數，合計十一個校正參數。 其中T矩陣有三個參數，R矩陣雖然有九個元素，但可以用Euler angles yaw θ 、 pitch φ 、tilt ψ 來簡化成三個參數，其中的關係如下：

cos cos sin cos sin

sin cos cos sin cos cos cos sin sin sin cos sin sin sin cos sin cos cos sin sin sin cos cos cos

ψ θ ψ θ θ ψ φ ψ θ φ ψ φ ψ θ φ θ φ ψ φ ψ θ φ ψ φ ψ θ φ θ φ −     = −_ + + _  + − +    R (5) 而五個內部參數，分別是上述的 f、κ 、₁ s_x、C_x、C_y，其中 f 為focal length， 1

κ 為失真係數，s_x是scale factor，(C C_x, _y)是中心影像的pixel值。方便起見，將

各個步驟代入後，求得最後(X_f,Y_f)的與最初(x_w,y_w,z_w)的關係： 1 1 2 1 2 3 1 7 8 9 4 5 6 2 1 7 8 9 ' ' ' w w w x x x f x x f w w w Z w w w y y f y f w w w Z r x r y r z T s d X s d X r f r x r y r z T r x r y r z T d Y d Y r f r x r y r z T κ κ − − + + +  _{+ =}  _{+ + +}   _{+ + +}  _{+ =}  _{+ + +}  (6) 上式右側外部參數，左側為內部參數。往後我們校正這些參數的時候，可以 用已知的(x_w,y_w,z_w)與相機得到的(X_f,Y_f)，來估算出這些參數的值。

2

22

2.

..

.4

44

4

其他

其他

其他模型

其他

模型

模型

模型

對以位置為基礎的架構來說，要將機器手臂的末端移動到想要的位置，校正 這些參數是絕對必要的，因為校正誤差對系統影響非常大，所以校正結果越準確 越好。以影像為基礎的話，因為是將機器手臂末端移動到想要的影像位置，所以 誤差影響較少。實作上，R. Y. Tsai [8] 的模型雖然觀念容易想像，比較好理解， 但實際操作比較煩瑣，方程式較多，要寫成演算法較複雜。幸運的，Zhang [9] 將此模型重新寫成如下的矩陣形式，對於我們用軟體來校正有相當大的助力。 圖 6.針孔相機模型圖解

上圖座標定義與之前相同，O為光學中心(optical center)，O₁為主點(principal point)，f 為焦距(focal length)。空間中一點M 投影至影像平面I 為 m，其中M 為

( , , )x y z ，m 為( , )u v ，其關係可以用齊次座標系(homogeneous coordinate)表示成： Let , , 1 0 0 0 0 0 0 0 0 1 0 1 fX _fX M _Z M m m fY fY Z Z X f Y m f PM Z   _{ }     _{ } =_{ } =  =_{ }     _{ }          _{  }  _{  } ⇒ =_{   }=  _{  }     ɶ _ɶ ɶ ɶ (7) 上式假設影像平面I 的原點是在O₁，而實際上原點卻在(p p_x, _y)，故改寫成：

0 0 0 0 0 0 1 0 1 x x x y y y X fX _{fX Zp f p} p Y Z m m fY Zp f p PM fY Z p Z Z    ₊  _{ +    }   _{     } = ⇒ =_ + _{ }= _{  }=  ₊  _{     }         _{ } ɶ ɶ ₍₈₎

為了方便，我們寫成定義了相機校正矩陣K (camera calibration matrix)：

[

]

0 0 0 1 x y f p f p m M γ     =_{ }⇒ =     K ɶ K I | O ɶ (9) 其中γ 是歪斜參數(skew factor)，是考慮到若影像兩軸發生不平行的狀況。 最後我們考慮比例參數( ,u u_{x y})的問題，即可定義出相機的內部參數矩陣為：

[

]

0 0 0 0 1 0 0 1 x x x x y y y y fu p p fu p p m M γ α γ α         =_{  }= _⇒ =         K ɶ K I | O ɶ ₍₁₀₎ 接著我們只要乘上旋轉矩陣R與平移矩陣T，即完成了模型的定義：

[

|

]

mɶ = K R T ɶM ₍₁₁₎ 上面的矩陣，也定義了六個外部參數，五個內部參數，合計十一個校正參數。 其中T與R與上述相同，而(p p_x, _y)即是(C C_x, _y)，而 f 與s_x的參數合併成α 與_x y α ，而新參數γ 是R. Y. Tsai沒考慮到的參數。至於失真係數κ ，₁ Zhang在[9]的 作法是將得到的影像資訊作運算，利用最大相似函數，來估測κ 與₁ κ 的值。 ₂

2

22

2.

..

.5

5

5

5 相機校正

相機校正

相機校正

相機校正

相機校正，是為了求出相機的外部參數與內部參數。在[8]，R. Y. Tsai提出 了四項觀察來校正，而Zhang在 [9] 也提出類似的限制條件，應用於校正技術 當中。由於校正相機需要空間的特徵點，因此通常是選用已知特徵的圖形，譬如 下圖黑白格狀的圖形，利用軟體抓出該特徵點在影像上的位置，就可以開始校正。

圖 7.相機校正實驗圖解 R. Y. Tsai的作法是使用將校正分成兩步，第一步是利用相機的文件查出硬體 設計參數N_cx、N_fx、d_x，算出d_x，接著依照 [8] 的指導先估算出(C C_x, _y)與s_x， 有了這些資訊即可由(X_f,Y_f)換算成(X Y_d, _d)。因為我們已知圖形的特徵，所以 我們可以估算出相對的κ ，用₁ (X Y_d, _d)來求(X Y_u, _u)。然後先上下相除，把焦距 f 約分掉，如此一來我們得到許多的特徵點(x_w,y_w,z_w)與其相對應的(X Y_u, _u)，這邊 可以使用最佳化的理論來估算出外部參數r r 與₁...₉ T矩陣，而r r 即是₁...₉ R矩陣的 元素。最後再回去求其他的參數，其相關的推導與證明可以參考[8]的敘述，Zhang 的作法是利用已知的特徵點(x_w,y_w,z_w)與影像上相對應的(X_f,Y_f)，用最佳化的 方法，一次估算內部與外部參數，最後用最小平方法解決失真的問題[9]。 以上兩種校正技術，都已經成功的被軟體實現，現今的研究已經進展到如何 用最少的特徵點來校正。對於以影像為基礎的話，原則上是不需要任何校正的， 然而在一些文獻提到，事實上必須考量各自的系統狀況來決定，假設相機的失真 過於嚴重，還是建議略為校正參數，才能增加系統穩定度[11]。除此之外，對於 eye-in-hand的系統，因為相機會跟隨著機器手臂移動，校正外部參數是沒有意義

的，所以一般系統不需要精確的校正，有需要的話則校正內部參數即可。 目前相機校正已經被模組化。Jean-Yves在2008年公佈了MATLAB的相機 校正工具箱，適用於MATLAB 5.0、6.0、7.0，在Windows、Unix、Linux等作業 系統皆可使用。其原理是用各種角度的圖形，再結合R. Y. Tsai與Zhang的校正 演算法，估算出相機的參數，以下我們介紹此工具箱的使用方法，如下圖： 1.將相機所拍攝的靜態圖形建立成資料庫 一般我們存取的檔案格式為TIF，圖形數量建議為20張至25張。 2.輸入拍攝時參考圖形的幾何特徵 譬如我們使用黑白格狀的圖形，故輸入格狀的邊長與大小。 3.讀取資料庫後手動選取圖形特徵點 如下圖所示，我們依序點選黑白格狀的特徵點，誤差容忍範圍在5 pixel內。 此步驟是為了避免軟體自動偵測特徵的錯誤，縮小之後校正的誤差。 圖 8.實際校正步驟圖

4.將所有圖形的特徵點選完成後自動算出所有參數值 在點選的過程中，若是失真過於嚴重，可以先手動猜測失真係數，方便點選 特徵。此步驟完成後，軟體將顯示出校正後的所有參數與誤差範圍。 我們按照上面的步驟，用20張圖形來校正內部的參數，將校正後的結果用 下表列出，並且用此參數來校正回原來的圖形，來比較校正前後的差別。下圖是 校正比較圖，用肉眼就可以發現失真的狀況明顯的改善了，如箭頭所示。 圖 9.校正前後比較圖 表格 1.相機校正後實際的內部參數表

Parameter Calibration results after optimization Uncertainties

Focal Length [661.67001 662.82858] [± 1.17913 1.26567] Principal Point [306.09590 240.78987] [± 2.38443 2.17481]

Skew Factor [0.00000] [± 0.00000] Distortion Coefficient [-0.26425] [± 0.00934]

Note: The numerical errors are approximately three times the standard deviations.

了解相機模型之後，對於控制系統的設計也會有所幫助。以位置為主的控制 架構，必須以繁瑣的相機校正為基礎，由外部參數的相機位置來反推物體位置， 而校正誤差嚴重影響其結果。以影像為主的話，則不需要以上冗長的步驟。本文 為了比較兩種架構的優劣，特地介紹相機模型與校正，以供其他論文參考。

三

三

三

三、

、

、

、特徵擷取

特徵擷取

特徵擷取

特徵擷取

觀察的環境內容通過相機系統後，將類比的訊號轉換成數位影像，然而得到 資料不等同於得到資訊，必須再經過資料處理等手續，整理過後，才能得到真正 的資訊。雖然相機系統的演進，現今的相機每秒鐘至少能輸出數十張數位影像， 甚至還有更高速的相機能捕捉更多的資料，但是與其增加資料量，讓機器人看得 更細，不如改善資料處理的演算法，讓機器人看得更精。即使這樣在單位時間內 還要考慮演算法的時間，透過特徵擷取也能更有效率的分析資料。 特徵擷取是屬於前處理(preprocessing)，能降低輸入資料的維度，克服維度 災難(the curse of dimensionality)，同時能減少資料處理的數量，避免資料累積。 對於之後的系統處理來說，特徵擷取提供了更有意義的輸入，也讓它變得更快且 較少花費，更能補償前面擷取特徵時所耗費的資源，增進整個系統的表現。

擷取特徵有相當多種方法，很多文獻採用設定特徵的門檻(threshold)，大於 此門檻的資料表示擁有某資訊，小於此門檻的資料則沒有某資訊。多維度的門檻 我們稱之為decision boundary，譬如線分割或是平面切割。其它方法像是Principal Components Analysis(PCA)等等。值得一提的是，如何選取適當的特徵，是重要 的問題。在選擇的時候，必須考慮演算法的假設與所需時間等等，才能事半功倍。

3

33

3.1

.1

.1 系統簡介

.1

系統簡介

系統簡介

系統簡介

在電腦視覺的文獻中，影像特徵的定義是從影像測量出來的任何結構關係， 譬如像是影像的輪廓或是物體的邊角。典型的影像特徵是物體特徵投影到影像上 的對應[1]。在這種定義下，一個好的影像特徵點，必須是從任何角度投影下， 都是不容易混淆的，像是[17]提到的contrived pattern。而Jang et al.定義影像特徵 的參數，是適用於單張或多張的影像中，都能夠運算出來的實數[7]。藉由這些 參數，我們即可應用到以影像為基礎架構的控制，包含在影像上的影像平面座標 點[15-17]、直線、橢圓[17]等等結果。簡單來說，假設影像特徵維度為k，我們

可以定義影像特徵的參數向量為 f =

[

f₁ ... f_k

]

T  ，其中 f_i是實數，又可以寫成

[

1 ...

]

T k f = f f ∈ ⊆F Rk  ，其中F為影像特徵的參數空間[1]。此子系統的輸入 是數位影像，輸出是參數向量 f  。如果將機器手臂末端的移動空間，定義為T的 話，則整個系統的對應關係，就可以寫成T→F，而通常 _⊆ 3 T R 。

3.2

3.2

3.2

3.2

方法分析

方法分析

方法分析

方法分析

影像特徵選取的方法，通常是依照後端的設計需求來決定參數。部分的文獻 簡化了這個問題，採用簡單的物體與特定的工作環境，部份則是使用特定任務的 線索，譬如Allen[23]利用移動偵測來定位物體是否被抓住，或是利用鮮豔顏色 來完成撿拾水果的系統，更多例子可以參考回顧文獻[7]。然而，擷取整張影像 的特徵需要高速的運算，而並非整張影像的所有像素，都是我們有興趣的。如果 我們能針對特徵出現機率較高的區域進行搜索，勢必能減少運算的時間。此概念 稱之為window-based法[1]，優點是運算簡單、適用性廣、不需要其他硬體實現， 缺點是必須知道初始的區域位置。對以影像為基礎的架構來說，我們能有技巧的 控制機器手臂從起始位置開始移動，這部份的問題可以在實驗中克服。 window-based分為兩種。在非限定的工作環境裡，可以從影像中清晰的劇烈 對比來建立特徵，像是輪廓或邊角等等，可以判斷物體在影像中的位置，這種叫 feature-based法，屬於影像處理的應用[1]。而如果利用區域內的資訊，像是色彩 等等，來建立特徵，稱為area-based法，屬於電腦視覺的應用[1]。在這邊我們將 引入影像追蹤的觀念，在時間花費與表現成果的折衷下，應用在適合我們的系統 上。在其他領域中，有更多關於物體辨識的文獻，譬如類神經網路、建立資料庫 等等。可惜這些系統太過於龐大，不適合應用在現階段的系統當中。 1.feature-based法 此方法是影像的空間濾波器處理，以濾波器覆蓋的中心為基準點，鄰近的點 為運算範圍，將它們的關係求出後，填回基準點。這裡我們介紹的是微分濾波器，

如下圖的Roberts operator、Prewitt operator、Sobel operator，在變化比較劇烈的 地方，通過濾波器之後，會有較大的斜率，這些點對應到的是物體的輪廓或邊角， 可以作為特徵。若是覆蓋區域太大，或是要對整張圖形作濾波，非常耗費時間。 在[1]提出了 window-based法，強健性卻不高，必須再觀察其他的特徵，來增加 辨識度[1]。對我們來說，為了提升穩定度與強健性，必須選取更有效率的特徵。 圖 10.影像空間濾波器圖解 2.area-based法 此方法適用於區域內的圖形識別比對，首先將初始區域運算出其特徵，接著 以此特徵為參考，以初始區域為中心，往鄰近的所有區域搜尋，直到找到發現與 此特徵最相似為止，而下次的搜尋，則是以最相似的此點為起點開始。事實上， 由於搜尋的區域越大，越耗費時間，這表示如果物體在兩張影像內移動太遠，在 犧牲運算時間下，還是可以找到的，因此實作時，必須在這兩者取得平衡[1]。 以上兩種方法都需要對特徵作起始化。feature-based的方法是先求出輪廓或 邊角特徵後，接著尋找附近最相似的關係。此方法優點是十分簡單，缺點是不夠 強健。area-based的方法是先得到區域內的色彩分布，接著也是尋找附近最相似 的關係。此方法的優點是比較通用，缺點是在光影變化下容易失常。在文獻回顧 後，我們發現過去的研究都將重點放在Jacobian的估測，之前的影像只作簡單的 處理。若是影像這邊處理品質不佳，之後的估測當然會不準確，故只要改善特徵 的擷取系統，即可明顯的增加系統強健性，改善以往的瓶頸。在電腦視覺的研究 中，提供了很多資源來解決這方面的問題，我們可以引進影像追蹤的技術，應用 在以影像為基礎的架構上。讓整個的系統更加平衡，也更佳的完善。

3.

3.

3.

3.3

33

3

特徵

特徵

特徵追蹤

特徵

追蹤

追蹤

追蹤

影像的物體追蹤技術已經發展多年，依照其方式可以分成非常多種類，像是 骨架的分析、模型的建立等，由於影像追蹤擁有相當多種不同的技術，因此我們 這邊不詳細的舉例，而是找出最適合我們的系統應用的即時技術。在2003年，

Comaniciu et al.[41]利用kernel函數計算權重，將平均位移法成功的實現在物體 追蹤上。平均位移法(mean shift)與上述的area-based法相似，其中的技巧就是將 區域內的色彩用分布圖表示，而搜索的範圍也是用分布圖表示，兩者用相似函數 比較之後，就可以觀察出物體移動的位移。我們將一開始作為參考的起始區域叫 目標模型(target model)，搜索的結果叫目標候選(target candidate)，它們的分布圖 用機率分布函數(probability density function)來表示，稱為q與p。不失一般性， 我們令目標模型的中心點為0，而目標候選的中心點為y，我們把分布圖看作是 成切割後m bin− 的向量。m 越小，計算越慢，而結果越準確，是可調整的參數。 1,..., 1 1,..., 1 { } 1 ( ) { ( )} 1 m u u m u u m u u m u u q q q p y p y p = = = =  = =    _{= =} 

∑

∑

    (12) 接下來計算bin的值。假設

{x }



*_{i i=1...n}是目標模型標準化過後pixel的位置，而 標準化是為了取norm。

{x }

_{i 1...} h i= n



是目標候選標準化的pixel位置。我們利用kernel 函數k x( ) :[0, )∞ → R來表示權重，而函數 2

{

}

( ) : 1... b x R → m 則表示在位置為x的

pixel所屬於bin的編號，δ 是Kronecker delta function。經由這些定義，我們寫出： 2 * * i i _* 2 1 i 1 2 2 1 1 1 ( x ) [ (x ) ] ( x ) 1 ( ) ( ) [ ( ) ] ( ) h h n u _n i i n i u h i h n i _i i q C k b u C k y x p y C k b x u C h y x k h δ δ = = = =  _{= − =}     ₋ = − =  −  

∑

∑

∑

∑

         (13) 其中C與C_h是為了滿足總和為1的係數，h為用kernel函數的頻寬。kernel 函數是convex的單調遞減函數，越遠離中心的權重越低，這與現實追蹤的觀念

相同，背景的權重必須小於中心的權重。這裡我們列出兩個常用的kernel函數：

表格 2.平均位移法常用的 kernel 函數表

Name Definition Sketch with d = 2

Gaussian Function 2 / 2

1

1

exp(-

),

1

(2 )

2

( )

0,

.

d N

x

if x

k

x

otherwise

π



_<



= 









Epanechnikov Function 2 1 ( 2)(1 ), 1 2 ( ) 0, . d E d x if x C k x otherwise  _{+ − <}  =      

d : dimension of space (in our 2D image case, d = ) 2

C : the volume of the unit d-Dimension sphere (in our 2D image case, _d C_d = ) π

我們用下圖的Bhattacharyya係數ρ 來定義相似函數d y( ) 。考慮兩個維度 m 的單位向量( q₁,..., qm)T與( 1( ),..., ( )) T m p y p y 。我們將單位化的元素，相乘後 開根號相加，如果越相似，則ρ 越大。注意 ρ 是y的函數，可以寫成下列的關係： 1 ( ) [ ( ), ] ( ) ( ) 1 [ ( ), ] m u u u y p y q p y q d y p y q ρ ρ ρ =  _{≡ =}    _{= −} 

∑

     (14) 圖 11.機率分布的函數比較圖

接著我們用泰勒展開式，對y₀展開，其中y₀是上一個frame的目標中心點。 0 ₀ 1 0 1 1 0 1 1 0 [ ( ), ] ( ) 1 ( ) [ ( ) ( )] ... 2 ( ) 1 1 ( ) ( ) 2 2 ( ) m u u u m m u u u u u u y y u u u _{y y} m m u u u u u u u p y q p y q p y q q p y p y p y q q p y q p y p y ρ = = ₌ = = = = = = + ⋅ ⋅ − + ≈ +

∑

∑

∑

∑

∑

 _          (15) 我們將後項省略，將之前的定義代入上式，可以得到下式： 2 0 1 1 1 0 2 0 1 1 1 0 1 1 [ ( ), ] ( ) ( ) [ ( ) ] 2 2 ( ) 1 ( ) ( ) 2 2 [ ( ) ] ( ) h h n m m i u u u h i u u i u n m h i u u i u i m u i i u u y x q p y q p y q C k b x u h p y C y x p y q w k h q where w b x u p y ρ δ δ = = = = = = − ≈ + − − = + = −

∑

∑ ∑

∑

∑

∑

           (16) 接著我們要對ρ 求最大值，於是對 ρ 求微分為零的關係式如下： 2 2 1 2 2 1 ( ) [ ( ), ] '( ) 2 ( ) '( ) 0 2 h h n h i i i i n h i i i i C y x y x p y q w k h h C y x y x w k h h ρ = = − − ∇ = ⋅ − = − =

∑

∑

         (17) 令

g x

( )

= −

k x

'( )

，我們可以整理出下列的方程式： 2 2 1 2 2 2 1 1 2 1 2 1 [ ( ), ] ( ) ( ) 2 ( ) ( ) 0 2 ( ) ( ) h h h h h n h i i i i n n h i i i i i i i n _i i i i n _i i i C y x p y q x y w g h h C y x y x x w g y w g h h h y x x w g h y y x w g h ρ = = = = = − ∇ = −  _{− −}  =  − =     − ⇒ = −

∑

∑

∑

∑

∑

                 (18) 此方法稱為平均位移法，y即是相似區域的位移量。我們將方程式寫成遞迴 演算法，若是位移小於我們設定的遞迴參數ε，則遞迴結束。下圖是流程圖：

new

y



0

ˆy

0 1,...,

ˆ

{

p

_u

(

y

)}

_{u= m} 1...

{ }

h i i n

w

₌ 1 0

ˆ

−

ˆ

<

ε

y

y

1

ˆy

0 1

ˆ

←

ˆ

y

y

1,...,

ˆ

{ }

q

_{u u= m} 1 0

ˆ

[ ( )

]

ˆ ˆ

( )

m u i i u u

q

w

δ

b

u

ρ

=

=

∑

x

−

y

2 0 1 1 2 0 1

ˆ

(

)

ˆ

ˆ

(

)

h h n _i i i i n _i i i

x w g

h

w g

h

= =

−

=

−

∑

∑

y

x

y

y

x

圖 12.平均位移演算法流程圖 上述的演算法，被稱為傳統的平均位移法，此方法是利用色彩分布圖(color histogram)當作目標，而找出與此目標最相似的位移。比起原始的特徵擷取法， 此方法優點是雖然增加運算時間，但強健度卻改善非常多。在硬體上，對於目前 個人電腦來說，已經能夠即時的反應。在軟體上，如果遞迴次數太多，表示可能 找不到目標，我們可以技巧的先結束這次的遞迴，下次的遞迴再繼續即可。而此

方法缺點是為了減少運算量，只採用目標的色彩資訊，捨棄了目標的空間資訊。

S. Birchfield 與 S. Rangarajan[42]提出了空間分布圖的概念(spatial histogram, or spatiogram)，也就是多計算出區域中的mean vector與covariance matrix，當作是 目標。除此之外，C. Yang et al.[43]改寫了方程式，定義了新的相似函數與縮放的 參數，來解決追蹤區域縮放(scaling)的問題。而追蹤區域轉向(orientation)的問題， 可以用下圖的PCA技術來克服[44]，讓平均位移法能適用於一般的狀況。 圖 13.主要成分分析圖

3.4

3.4

3.4

3.4

實驗

實驗

實驗模擬

實驗

模擬

模擬

模擬

以上我們引進了物體追蹤的觀念，將系統數位影像的輸入，先轉換成追蹤的 目標模型(target model)，再定義目標候選(target candidate)，算出與目標最相似的 位移。此位移可以視為追蹤的目標中心，即可以當作是系統的輸出。以往的系統 僅用簡單的背景與單純的物體，依照狀況設定門檻，將輸入處理成二元的影像， 再找尋影像的中心後輸出，把節省下來的資源分配到後端。這樣的優點是可以用 較繁瑣的估測法增加準確度，缺點是工作環境遭受限制。一旦影像處理出了錯， 所付出的代價極大。故我們將資源重新分配，只要前端的影像計算準確，後端的 系統就可以改用較低運算量的估測，也可以在未知的環境，還能有不錯的表現。 Largest Principal Component Smallest Principal Component 1

v



2

v



Principal Component Vector

電腦視覺在物體追蹤的研究有非常多文獻可以引用，只要資源許可，[42-44] 的方法都可以應用，端看工作環境與工作任務的複雜程度。平均位移法的好處是 耗費的時間較少，資源佔用不多，而表現卻是比較好的演算法，不僅在個人電腦 上，用解析度較差的視訊都能夠實現，連在嵌入式系統，用CCD(charge-coupled device)的影像輸入也能即時處理，以下是我們的模擬與實驗的結果。 1.MATLAB模擬

本模擬是在個人電腦完成，CPU規格為AMD Athlon(tm) 64 Processor 3000+ 1.81 GHz，RAM為1.87GB，OS是Microsoft Windows XP Professional Version 2002 SP3，軟體是MATLAB version 7.0.4.365(R14) SP2。我們先用DV預錄一段格式 為AVI的影片，再用MATLAB讀取後，代入演算法。最後將運算後的結果存成 JPG檔圖形，來觀察整體的表現。下圖是以杯子為追蹤目標的結果，我們擷取 frame編號為1、50、100、150、200、250、300、350、400的圖形。追蹤的結果 用黃色框框表示，可以發現平均位移法在較複雜的背景也能適用[44]。 圖 14.平均演算法的追蹤結果示意圖

圖 15.平均位移法的誤差圖與遞迴次數圖 上左圖是平均位移法的誤差圖，橫軸是frame的編號，縱軸是追蹤的中心與 實際中心的誤差距離。我們發現在編號為71與310的地方誤差比較大，原因是 因為杯子的翻轉，使得色彩分布圖差異較大，所以略有誤差，而之後的追蹤結果 又馬上回到可接受的範圍。上右圖是遞迴次數圖，橫軸是frame的編號，縱軸是 遞迴的次數，可以發現在編號為71的地方遞迴數較多，我們在程式中設定遞迴 次數超過五十次，就跳出這次的遞迴，以便在下次的遞迴再繼續尋找。 2.系統實驗

為了測試即時的追蹤狀況，我們使用德州儀器(TI, Texas Instrument)所發行的

DM6446達文西平台，利用其中的RISC CPU ARM926來實現。此平台作業系統 是MONTAVISTA Linux V4.0，具有影像處理的功能，可以將影像的輸入暫存， 方便我們程式讀取。開發時我們使用arm_v5t_le-gcc作為轉換編譯器，將程式碼 先在PC編譯，確定無誤後，再轉譯存入到平台。在PC的Linux上，我們安裝

MONTAVISTA Linux V4.0 System Tools，內含相關系統開發套件，包括了編譯器 與平台的Linux kernel與file system的重製工具。簡單來說，我們整個開發過程 是在PC完成，只是將結果放入平台上。使用嵌入式系統的優點，是能在最少的 資源下測試演算法的結果，而且周邊硬體齊全。我們用下圖攝影機輸入影像後， 在平台上代入演算法，最後輸出到螢幕。下下圖是以杯子為目標的結果，以綠色 框框表示。取樣時間為一秒[45]，可以發現平均演算法在即時的互動也能支援。

圖 16.SONY EVI-D70 PTZ 攝影機與 EVERFOCUS 彩色 TFT LCD 螢幕 圖 17.平均位移法的即時追蹤結果圖 在這個階段中，我們引用了平均位移法，來提升特徵擷取的效能，與過去的 子系統相比，我們更能準確的得到物體中心的位置，而且在複雜的環境與移動的 狀況下，也能有穩健的表現。其他技術還有立體視覺可以應用，不過該方法需要 雙相機，而且受限於雙眼的距離，並不適合。最後，本文建議設計時，必須考慮 硬體的負荷。若是在此階段耗費太多資源，在下個子系統中，將嚴格受到考驗。

四

四

四

四、

、

、

、視覺回授

視覺回授

視覺回授

視覺回授

數位影像通過特徵擷取系統之後，將未經處理的資料(raw data)轉換成所需的 資訊。在我們的系統內，此資訊被定義為特徵，是為了得知物體在影像中的位置。 而在其他研究領域中，特徵被廣泛定義為輪廓、邊角、色彩等容易被識別的特殊 內容。以位置為基礎架構的缺點，除了之前提到的對於校正誤差比較敏感，還有 計算的問題。使用影像特徵來定位的優點是不需要校正，不需要算出空間中相機 與物體的相對位置，缺點是不論選用伺服控制，或是觀察運動架構，系統設計都 比較複雜。下面我們先從文獻回顧開始，分析與探討兩種控制系統的架構。 在1980年，Sanderson 與 Weiss[6]介紹了系統架構分類法，其中分辨方法 是依照手臂得到的指令種類決定。更仔細的說，影像特徵與我們想要的特徵彼此 相減，可以算出特徵誤差，此誤差經過控制法則來驅動機器手臂。如果這個法則 是直接用特徵誤差來得到角度輸出，分配給各個關節的話，我們稱之為伺服控制 架構。如果這個法則是用特徵誤差判斷如何移動，而讓手臂自行計算各個關節， 這我們稱為觀察運動架構。在定義上[1]，視覺伺服(visual servo)一般已經被定義 為用任何種類的視覺回授(visual feedback)來控制機器手臂，而定義伺服控制架構 為direct visual servo structure，觀察運動架構為dynamic look-and-move structure。

通常，選擇伺服控制架構的原因是考慮到機器手臂的輸入比較簡單，而且此 方法可以運用到相同自由度下的任何機器手臂，也就是不限定模型、比較一般的 方法，而運算的速度由此演算法來決定。然而，選擇觀察運動系統的原因有三種， 一、此系統有內部回授，二、目前手臂都能接受此種移動指令。三、避免kinematic singularities等等問題，因為能接受這種指令的手臂，通常會有最佳化過的inverse kinematics，將末端位置換算回關節角度，所以運算的速度是由該演算法決定。 對於現今的發展來說，不論是角度的輸入或是端點的輸入都很常見。總而言之， 輸入角度移動量的方法比較直接、通用，而輸入端點移動量的方法必須考慮手臂 是否接受，其它文獻較少討論兩種架構的優劣，本文建議依照硬體規格選擇即可。

4

44

4.1

.1

.1

.1

系統

系統

系統簡介

系統

簡介

簡介

簡介

視覺回授的子系統分成兩部分，前面是軟體演算法的計算，後面是機器手臂 的移動。基本上硬體的作業系統、通訊協定、輸入介面為不可更動的固定規格， 我們只能依照不同的硬體來設計不同的演算法，而分成伺服控制與觀察運動兩種 架構。兩者輸入都是特徵誤差，而前者的輸出是各關節角度，後者的輸出是位移。 以理論的角度來看，由於兩者的輸出是類似的，只有變數的維度不同，因此我們 在分析方法時，可以不限定輸出變數的維度與內容，完成不失一般性的推導。 在前面的文獻回顧探討中，提到了估測Jacobian的方法，Hosoda與Asada[18]

是用exponentially weighted recursive least-squares (RLS) 來更新方程式，Jagersand [19]是用nonlinear least-squares optimization method與Broyden estimation，而兩者 是觀察靜態目標，Piepmeier et al.[25][26]則是觀察動態的目標，完成了靜態相機 視覺回授，推導了收斂的性質，而不考慮精確的相機校正模型與機器手臂模型。 以上是靜態相機的討論，而動態相機方面，Asada 與 Hosoda[31]延伸了他們的 方法，以靜態參考點來觀察動態目標，而Piepmeier[32]在不根據模型的狀況下， 解決了靜態參考點的問題。2007年，Farahmand et al.[36]試圖解決全域性的問題， 在方程式中不限定變數的維度，可能是各關節的角度或是位移。以下我們與[36] 相同，綜合上述兩種架構，配合eye-in-hand與eye-to-hand，以求推導的一般性。

4

44

4.2

.2

.2

.2

方法

方法

方法分析

方法

分析

分析

分析

關於視覺回授系統，我們參考[1]的定義，是影像特徵誤差 e 的函數。不論是 eye-in-hand與eye-to-hand，機器手臂的移動造成了影像特徵的移動，此移動結果 與我們想要的影像特徵 f_d相減，得到特徵誤差 e ，我們的目的是讓誤差 e 為零。 我們可以想像成嬰兒的學習，當他們看到有興趣的物體，會試圖抓取。由於他們 不太能控制自己的動作，將會發現手的位置與物體的位置不同，因此觀察到這個 現象後，修正幾次即可抓到物體。我們把眼睛看到的物體，稱為想要的影像位置

特徵 f_d，與眼睛看到手的位置相減，得到特徵誤差 e ，再去修正這個誤差。 為了完成這個目的，我們必須將影像的變化，對應到機器手臂的變化，這個 相對應的概念，稱之為Jacobian矩陣。很多文獻探討著如何決定Jacobian，各自 有各自的名稱。在這邊我們統一使用Jagersand[35]的變數名稱，與Farahmand et al.[36]的變數定義，我們定義關節角度(joint angles)或位移(displacements)為變數

x ∈ n R  ，而影像特徵(feature points)為y ∈ m R  。當變數x改變的時候，機器手臂也 改變其位置，此時由影像回授，擷取改變的特徵y，此函數關係可以寫成： ( ) y= f x (19)

因為函數 f 與手臂的運動學模型(kinematic model)與相機模型(camera model)

有關，所以是非線性函數，Jagersand稱之為visual-motor function[35]。如果起始 狀態為x₀與y₀，我們想得到的最終狀態是y*。簡單來說，我們必須想辦法得到 k x ∆ ，而使得 f (x +₀

∑

∆x ) = y*_k  。下圖是簡單的示意圖，其中 1 f− 是 f 的反函數。 圖 18.視覺回授示意圖 傳統上的藉由校正，我們可以得到兩種函數，第一是手臂運動學g，第二是 相機校正h，故我們可以把 f 看成g與h的合成函數，可以寫成： ( ) ( ( )) y= f x =h g x (20) 由於此為非線性函數，我們用分段線性(piecewise linear)的方法來分析，在 某個區間k時假設可微分，因此我們可以用一階微分的模型，將y= f x( ) 寫成： ( ) ( _k) ( _k)( _k) f x ≈ f x +J x x−x (21)

此模型在x_k附近是有效的，而J矩陣是Jacobian矩陣，Jagersand稱此矩陣為

visual-motor Jacobian[35]，最初稱為feature sensitivity matrix[14]，如下式定義：

1 1 1 1 2 2 2 2 1 1 2 1 1 2 ( ,..., ) ( ..., ) n m n n m m m n y y y x x x y y y y y x x x x x y y y x x x ∂ ∂ ∂   _{∂ ∂ ∂}    ∂ ∂ ∂   ∂ _{∂ ∂ ∂ } = _{=  } ∂ _{ } _{∂ ∂ ∂}    ∂ ∂ ∂    J … ⋮ ⋱ (22) 簡單來說，f 是 n → m R R 的函數，此函數的偏微分是m n× 的Jacobian矩陣， 意義是將手臂移動變化，對應到影像的移動變化。我們把方程式改寫成其他形式： 1 k k y ₊ = ∆ +J x y ⇒ ∆ = ∆y J x (23) 我們知道Jacobian矩陣J之後，必須想辦法得到 −1 J ，而 −1 J 的意義是將影像 的移動變化，對應到手臂移動變化。有了 −1 J ，才能利用視覺回授，來控制手臂。 1 y x − y ∆ = ⇒ ∆ = J  J ∆ (24) 為了得知 −1 J ，我們考慮以下兩種情況，第一種是當J為rank deficient時， 因為 −1

J 不存在，所以用奇異值分解(SVD, singular value decomposition)來分解J： ×   _×   _×  T U : m m J = UΣV Σ : m n V : n n (25) Σ是m n× 的對角矩陣(diagonal matrix)，假設 T J J的特徵值為λ₁ ≥ ≥... λ_r >0 而且λ_r+1= =... λ_n =0，其中r是 T J J的rank值，則Σ矩陣的定義如下列方程式： 1 1 ( ,..., _n) ( ,..., _n) diag λ λ diag σ σ × = = ∈ m n Σ R (26) 由於λ_r+1= =... λ_n =0，因此我們可以改寫上式，得到Σ矩陣的定義如下式： 1 ( ,..., _r) diag σ σ × = ∈ n n Σ R (27)

而V={ ,..., }v₁ v_n 矩陣則是n n× 的正交矩陣(orthogonal matrix)，v_i的定義為： T i i i v =λv J J  (28) 最後，U={ ,...,u₁ u_m}是m m× 的正交矩陣(orthogonal matrix)，u_i的定義為： T i i i u =λu JJ   (29) 由於λ_r+1= =... λ_n =0，因此我們可以改寫U矩陣的定義，得到簡化的下式： ×   _×   _×  T U : m n J = UΣV Σ : n n V : n n (30)

當J為rank deficient時，我們可以用J的pseudo-inverse，J+來計算 −1

J ： + ₌ + T J VΣ U (31) 如此即可得到其對應關係。更多細節可以參考[1]與Farahmand et al.的[36]。 x + y ∆ = J ∆ (32) 第二種是當J為full rank時，可分成：m=n、m>n、m<n三種來討論： 1.m=n Feddema[39]使用的方法，當m=n且J為可逆時，則 −1 J 存在且 1 x − y ∆ = J ∆。 2.m>n

Jang[10]使用的方法，此為over-determined的狀況，當J為滿秩(full rank)，

也就是 1 ( ) rank J− =n的話，可用J的pseudo-inverse，J+來計算，其關係如下： 1 ( T ) T _{x y} + ₌ − _{⇒ ∆ =} +_∆ J J J J  J  (33) 3.m<n 此為under-determined 的狀況，我們可用J的pseudo-inverse，J+來計算： 1 ( ) T T + ₌ − J J JJ (34) 此狀況較少發生，原因是 f 為 n → m R R 的函數，m<n的意義是特徵空間的 維度，比機器手臂空間的維度還要少，這樣方程式將會變成under-determined的 狀況，所得到的解是近似解，誤差很大。一般來說我們會避免這種狀況發生。

以上我們討論了各種估測Jacobian的方法。由上面的推導我們得知，怎樣的 手臂變化∆x，會對應到怎樣的影像變化∆y，而在 −1 J 的幫忙下，我們可以知道 怎樣的影像變化∆y，會得到怎樣的手臂變化∆x。在計算出這些關係之後，我們 接著要送訊號u給機器手臂，在[1]的教導下，目前大部分文獻的訊號設計如下： 1 ( _d ) u =ΚJ− f − f    (35) 其中Κ是增益矩陣，在[36]也有類似的應用。更多內容可以參考[14][15]。

4.3

4.3

4.3

4.3 影像回授

影像回授

影像回授

影像回授

以上我們推導了通用狀況Jacobian矩陣的方程式，方程式中不限制輸入維度 n R 與輸出維度Rm的情況下，我們所得到的 f 與 1 f− ，可以適用於伺服控制架構

(direct visual servo structure)與觀察運動架構(dynamic look-and-move structure)，而 這兩種架構相異的地方有二，第一、前者輸出是各關節角度，一般的機器手臂有 六個自由度可以定位，屬於 6 R 空間。後者的輸出是相對的位移，通常是三度空 間，屬於 3 R 空間。第二、由於前者是將六軸關節，先對應到三度空間，接著由 三度空間對應到影像，因此我們更嚴謹的定義，將這裡的Jacobian矩陣稱為合成

Jacobian (composite Jacobian)矩陣，是由運動Jacobian (kinematic Jacobian)與影像

Jacobian (image Jacobian)組成，而後者是直接從三度空間對應到影像，我們稱之 為影像Jacobian (image Jacobian)，故上一節所推導的通用Jacobian矩陣，對伺服 控制架構是合成Jacobian的推導，對觀察運動架構是影像Jacobian的推導。 本文採用的是後者，也就是觀察運動架構，來估測影像Jacobian矩陣。主要 有兩個原因，第一、在輸入影像特徵的誤差後，前者必須花費大量計算時間算出 各軸角度，而後者所計算的矩陣維度與運算量遠小於前者。第二、前者算出各軸 關節角度後，由於還要多花時間找出最小移動量的解，因此這方面的研究主要是 用模擬完成，實作比較難達到即時的反應。相較之下，後者算出是三度空間座標 值，因為目前的機器手臂皆有最佳化的演算法，可以算出最少的關節移動量，來

抵達所需要的座標點，所以實作上比較容易。在前面的文獻回顧也可以發現，大 部分的文獻是以估測影像Jacobian矩陣為主，原因也是大致如上列所述。 以下我們介紹本文決定影像Jacobian矩陣的方法，在這邊我們採用的是即時 估測法。此方法靈感於來自[1]的例子，而採用Feddema[39]的m=n與J為full rank的假設，我們將其方程式加以簡化後[30]，完成即時估測，最後加以實現。 在這邊我們考慮單眼相機(monocular camera)視覺回授的狀況。因為未校正相機並 不知道其確切位置的座標與角度，所以為了符合常理，假設在執行任務時，物體 與機器手臂不會超出影像的感測範圍。接著，我們定義手臂的輸入指令值，都在 指令的定義域(command domain)內，此定義的原因是並非整個三度空間機器手臂 皆可以抵達，故此定義域可以限制指令不會超出手臂伸展空間與相機範圍。寫成 數學式的話，我們將輸入指令稱為p t( )，投影到影像的平面為s t( )，可以寫成： ( ) ( ) ( ) ( ) ( ) ( ) b b x t p t y t u t s t v t    =           =    _{ }    (36) 兩者皆是時間的變數，我們對其微分，可以得到影像Jacobian矩陣的關係式： (t)= (t) (t) where (t)= b b b b u u x y s p v v x y ∂ ∂   _{∂ ∂}    ∂ ∂   _{∂ ∂}    J J _ɺ _ɺ (37) 我們將指令p t( )輸入到手臂，內容為世界座標系的

[

x_b y_b Z

]

T，其中Z為 任意值。由於採用觀察運動架構，因此手臂有內建最佳化的反向運動學(inverse kinematics)演算法，將三度空間座標值換算出最少的關節移動量後，驅動末端到 此座標，再投影到相機座標系的

[

x_c y_c z_c

]

T，兩者的座標值轉換的關係為： c b c b c x x y y z Z      ₌  ₊             R T (38)