分散式救援機器人之即時影像防震與伺服定位

國

立

交

通

大

學

電機與控制工程學系

碩

士

論

文

分散式救援機器人之即時影像防震與伺服定位

Video Stabilization and Visual Servoing on

Distributed Rescue Robots

研 究 生：蕭任斌

分散式救援機器人之即時影像防震與伺服定位

Video Stabilization and Visual Servoing on

Distributed Rescue Robots

研 究 生：蕭任斌

Student：Jen-Pin Hsiao

指導教授：徐保羅 教授

Advisor：Dr. Pau-Lo Hsu

國 立 交 通 大 學

電機與控制工程學系

碩 士 論 文

A Thesis

Submitted to Department of Electrical and 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 and Control Engineering

July 2009

分散式救援機器人之即時影像防震與伺服定位

研究生：蕭任斌 指導教授：徐保羅 博士

國立交通大學電機與控制工程學系

摘要

由於救援機器人常行駛於崎嶇地形，造成機器人上影像晃動，因此發 展了影像防震功能，使監控端可從機器人上收到一穩定的影像。在影像防 震演算流程裡，加入 Kalman filter 估測自主運動向量，機器人即使在移動 的過程中可得到一平穩防震的視訊；為了提升影像防震在崎嶇地形的穩健 性，透過相關係數與變異量的分析，可明顯提升估測向量的精確度；此演 算法已實現在 TI DSP 6437，藉由階層式估測運動向量的機制，降低運算 負載使防震效果更為即時，畫面更新率已由原來的 10 fps 提升至 28 fps。 本研究也將合作搬運目標的功能實現在救援機器人上，使多機器人可 自動趨近目標，並合作性地達成搬運的任務。透過多視角之影像偵測目標 功能，設計融合機制降低感測器造成的誤差以建立精確的目標位置資訊， 整合視覺伺服與虛擬邊界運動控制的機制，使多機器人精確到達目標點， 藉合作機制可自動地完成搬運目標的任務。

Video Stabilization and Visual Servoing on

Distributed Rescue Robots

Student：Jen-Pin Hsiao Advisor：Dr. Pau-Lo Hsu Department of Electrical and Control Engineering

National Chiao-Tung University

ABSTRACT

Rescue robots integrating different sensors and the wireless network are applied to transmit sensing information for remote control. Among various sensors, the video renders the most important and useful information. When rescue robots navigate in a rough terrain, significant vibration of the video occurs unavoidably and a video stabilization system is proposed in this thesis to reduce the disturbances on the visual system of the present rescue robots. The Kalman filter is applied to estimate the motion vector of the robot and the false estimation occurrence can be thus greatly reduced by applying analysis of both correlation and variance of the motion vector estimation. With a hierarchical searching algorithm implemented on the TI DSP 6437, the frame rate can be improved from 10 fps to 28 fps in order to realize the real-time video stabilization.

Furthermore, the vision techniques will be used to help multiple rescue robots accomplish cooperative mission. The cooperative mission of multiple rescue robots has been achieved by applying the developed visual servoing technique. For the target position estimation, triangulation is applied to construct 3D position. By the multiple vision systems, the fusion of the geometrical factors generates accurate target position estimation. The proposed virtual bounded motion control algorithm further leads the robots approaching the target precisely to move the target by the cooperation of multiple rescue robots.

誌 謝

首先要感謝指導教授徐保羅 博士在兩年研究生活中的耐心指導，當研究遇 到問題與挫折時，能夠適時地找到正確出路，使我能順利的完成碩士學位，在此 表達我最真誠的敬意與感謝。另外感謝口試委員蔡清池 博士、宋開泰 博士、葉 賜旭 博士對於本論文的指正，並給予寶貴的建議，使得本論文更加完善。 感謝實驗室的學長、同學與學弟們：博士班謝鎮洲學長、幸琮政學長、賴 建良學長、王安平學長、黃煒生學長，平時給我的意見、指導與幫助，以及實驗 室林也強學長、李建龍學長、劉醇偉學長、黃宗勝學長、蔡雨坤學長、正忠、子 強、煒智、孟哲、哲維、士文、子期、旻慶在學業上的相互切磋與指教，並且時 常分享生活趣事讓實驗室充滿歡笑和活力。特別對琮政學長、建良學長、也強學 長以及正忠、子強、煒智在研究與實驗上的大力幫忙，獻上誠摯地感謝。感謝所 有夥伴這兩年在 Lab 816 給我的美好回憶。 最後要感謝我的所有關心我的父母、哥哥，使我能夠心無旁騖地完成學業， 以及感謝與我同窗兩年的室友嘉鴻、威智，因為有你們在身旁的支持與關懷，並 且順利地克服我人生中的每一個重大考驗。

願 平安 健康 成就感

2009.07.21

目

錄

中文摘要

... i

英文摘要

... ii

誌謝

... iii

目錄

... iv

表目錄

... vii

圖目錄

... viii

第一章 緒論... 1

1.1

研究動機與目的

... 1

1.2

研究背景與發展現況

... 2

1.3

問題陳述

... 4

1.4

研究方法

... 5

1.5

論文架構

... 6

第二章 分散式救援機器人系統架構... 7

2.1

硬體與通訊架構

... 7

2.1.1 CAN 傳輸介面... 8

2.1.2 IEEE 802.11 傳輸介面 ... 9

2.2

DSP

6437

影像處理平台

... 9

2.2.1 EDMA 模組-協助資料搬運... 11

2.2.2 NDK 網路開發套件 ... 12

2.2.3 DSP/BIOS 作業系統 ... 13

2.3

機器人運動控制架構

... 14

2.3.1 運動模型 ... 14

2.3.2 多軸馬達運動控制電路... 15

2.4

感測器

... 16

2.4.1 紅外線攝影機 ... 16

2.4.2 Compass ... 17

第三章 影像防震... 20

3.1

震動向量的估測與補償... 21

3.2

Kalman filter 使畫面平穩化 ... 23

3.3

偵測錯誤估測向量

... 25

3.4

即時嵌入式系統實現

... 28

3.5

小結

... 30

第四章 融合分散視覺系統於位置估測... 31

4.1

影像處理辨識目標物

... 31

4.2

Triangulation 估測目標位置... 35

4.3

融合分散攝影機資訊

... 37

4.4

分析機器人數目對位置估測影響... 42

第五章 整合視覺伺服於多機器人合作搬運... 44

5.1

基於影像視覺伺服

... 45

5.2

基於位置視覺伺服

... 46

5.3

合作搬運任務實現

... 49

5.4

動態目標追蹤

... 51

第六章 結論與未來發展... 53

6.1

結論

... 53

6.2

未來發展

... 54

參考文獻

... 56

表

目 錄

表 3-1 自主移動估測由 Kalman filter 與 low-pass filter 誤差比較 ...24 表 4-1 攝影機參數...35 表 4-2 目標定位誤差比較...41

圖

目 錄

圖 2-1 分散式救援機器人...7 圖 2-2 多機器人硬體與通訊架構...7 圖 2-3 典型的 CAN-bus 架構 ...8 圖 2-4 USBCAN 通訊介面...8 圖 2-5 DSP 6437 開發平台...10 圖 2-6 DSP 6437 硬體方塊圖...10 圖 2-7 利用 EDMA 將影像資料做搬移...11 圖 2-8 區塊同步模式搬運資料...12 圖 2-9 機器人運動模型...14 圖 2-10 多軸馬達運動控制電路板...15 圖 2-11 裝置於機器人之馬達...16 圖 2-12 馬達驅動電路...16 圖 2-13 紅外線攝影機 (a)外觀 (b)黑暗中投光燈補光...16 圖 2-14 黑暗中監控端影像...17 圖 2-15 Compass V2Xe...18 圖 2-16 V2Xe 與 F2812 介面電路 ...18 圖 2-17 量測 Compass 誤差分佈 ...19 圖 3-1 影像穩定演算法流程圖...20 圖 3-2 Block matching 偵測移動向量 ...21 圖 3-3 全域平移運量向量計算...22 圖 3-4 旋轉運動角度計算...22 圖 3-5 Kalman filter 估測自主運動向量 ...24 圖 3-6 Correlation 量測值分佈在不同的情況 (a)正常情況 (b)對比度低 ...25 圖 3-7 (Cimax-Cimean)分佈。上:正確估測向量，下:錯誤估測向量 ...26 圖 3-8 針對平移震動，估測震動向量誤差比較...27 圖 3-9 針對旋轉震動，估測震動向量誤差比較...27 圖 3-10 輸入影像與穩定後輸出，上列為輸入影像，下列為輸出影像...28 圖 3-11 Hierarchical 搜尋法提升向量估測效率 ...29 圖 4-1 影像處理流程...31 圖 4-2 HSV 格式 [27] ...32 圖 4-3 在二值化影像中貼標籤...33 圖 4-4 辨識目標物...34 圖 4-5 影像處理視窗介面...34 圖 4-6 旋轉及移動平面校正物...35

圖 4-7 Triangulation 計算目標位置 ...35 圖 4-8 目標物定位模擬環境...37 圖 4-9 角度因素對誤差的分析 (a)角度 30 度 (b)角度 80 度 ...38 圖 4-10 距離因素對誤差的分析 (a)距離 100 cm (b)角度 300 cm...39 圖 4-11 設計權重...40 圖 4-12 分散式攝影機目標物定位實驗環境...40 圖 4-13 融合機制降低目標定位誤差...41 圖 4-14 模擬環境 (a-d)分別為機器人由 2-5 的數目...42 圖 4-15 機器人數目對位置估測的影響 (point: 誤差平均值，bar: 誤差標準差)43 圖 5-1 設計運動機制於搬運目標任務...44 圖 5-2 基於影像視覺伺服流程...45 圖 5-3 基於影像視覺伺服校正方式...45 圖 5-4 目標位置估測與誤差標準差分析...47 圖 5-5 虛擬邊界位置運動控制流程...47 圖 5-6 虛擬邊界運動控制...48 圖 5-7 搬運合作機制...49 圖 5-8 整合視覺伺服實現機器人合作搬運目標...50 圖 5-9 遠端顯示機器人與目標估測資訊...51 圖 5-10 動態目標追蹤...51 圖 5-11 動態目標位置估測 (a-d)依序為機器人追蹤的過程...52 圖 5-12 遠端監視畫面...52

第一章 緒論

1

1.1 研究動機與目的

台灣位於太平洋地震帶，發生地震的機率非常得高，有鑑於此，最近這幾 年救難機器人技術發展快速，許多種各式各樣的救援機器人被研發出來如[1] [2]，救援機器人必須克服災區的崎嶇地形、並且讓搜救人員能在安全的地方監 控機器人，所以針對機器人之間的通訊、感測系統、運動控制以及機構設計等 相關技術必須要整合在一起。聞名世界的機器人聯盟 RoBoCup [3]，針對此項 還特別舉行 RoBoCupRescue 比賽，其中包括發展災難模擬及機器人自主達成搜 救任務等，其將模擬系統和機器人系統結合在一起，展現在真實的災難現場。 而本研究發展的分散式搜救機器人，將深入探討視覺感測系統在救援機器人上 之應用，並與自主式機器人結合在一起，使機器人合作性地進行搜救任務。 分散式救援機器人整合各種感測資訊，其中影像的資訊最為豐富，災區地 形的崎嶇不平易造成機器人上影像的震動，為了接收穩定之影像資訊，將實現 一影像防震系統，使搜救人員透過監控介面可更清楚地了解災區環境，爾迅速 前往救援。 隨著無線通訊以及微型感測器的發展，無線感測網路的研究越來越受重

視，本研究則著重於網路化分散式視覺系統(distributed vision network)，分散式

攝影機系統可提升對環境中影像資訊接受的效率，由於 CCD 和 CMOS 攝影技 術之成熟，有著體積小、成本低以及更節能的趨勢，多攝影機系統漸實用且普 遍，將此技術應用在救援機器人上，將有助於機器人對環境的探索，本研究將 在機器人上建立分散式攝影機系統，利用此系統透過幾何關係的計算得到環境 中目標物的位置資訊。 機器人的應用中所要執行的任務常並不是單一機器人所可以勝任的，所以 目前學術上對於多機器人合作的研究越來越多，因此本研究在救援機器人上實

現了多機器人搬運的功能，為了實現多機器人合作搬運，必須估算環境中目標 物之位置，並利用視覺伺服技術使多機器人自動地達成合作搬運的任務。

1.2 研究背景與發展現況

影像防震技術在消費性電子產品 DV 攝影機的應用上已被廣泛的探討，常 見的技術為光學影像防震系統，攝影機必需在機構上經過機密的設計，並外加 感測器的搭配，其成本較高，而本研究所發展的為數位影像防震系統，在攝影 機的硬體上不需做任何的更動，可套用至任何攝影機，將攝影機資料透過實現 在處理器上所設計的演算法，便可達到防震的效果。影像穩定系統必須先偵測 移動向量，數位影像防震中對於偵測移動向量常見的方法包括：區域比對演算

之Block matching 方法[4]、以 Phase correlation [5]來決定位移的方法、利用梯度 方程找出移動向量之 Optical flow 方法 [6][7] 等。而 Block matching 使用影像 中多區域比對的方式計算出移動向量，計算負載隨著區域的增加而上升；Phase correlation 計算量高且對於小範圍的震盪不夠精準，不過適合大範圍移動向量的 偵測；而 Optical flow 對雜訊較敏感。本研究使用的是分成五個區域的 Block matching 法，計算較即時且抗雜訊能力較 Optical flow 高。此外，當攝影機在移 動時，影像防震系統必須區別震動以及本身的運動，前者是需被補償，後者則 不需要，否則影像會產生更大的晃動，因此Jin et al. [8]將攝影機裝置在智慧型 車輛上，分析車的運動模型，根據其慣性運動設計低通濾波器估測車子本身運 動對影像產生的移動向量。 隨著攝影機技術的成熟與處理器執行效能的成長，分散視覺系統已廣泛受 到討論，藉由分散視覺系統可計算出環境中目標物的 3D

位置資訊，Barton-Sweeney et al. [11]和 Devarajan 與 Radke [12]利用極線幾何(epipolar geometry)， 對環境中目標做定位，極線幾何法可以強健估測目標位置的準確度，但計算較

視覺系統中使用 Triangulation 方法估算目標位置，計算量較小但對影像感測資 訊的誤差極為敏感。為了降低估測目標位置誤差，Qian et al. [14] 對目標的顏色 特徵設計權重值，藉由多視覺彼此資訊的融合，降低影像資訊誤差造成的影

響。Aarabi [15]利用攝影機視角的特性設計二維空間相似函數(spatial likelihood functions)，減少目標位置估測誤差，越多攝影機時，誤差降低越多，但此方法 沒有考慮到攝影機本身位置的誤差。雖然多視覺系統在監視應用上已被深入探 討，但還未被廣泛地應用在移動式機器人上，本研究將利用分散式機器人的影 像感測資訊可建立一個網路化分散視覺系統，藉由分散視覺系統可計算出環境 中目標物的位置資訊。 為了實現機器人自主性的運動，視覺伺服(visual servoing)已廣泛被應用在 機器人自動導航中，視覺伺服架構可分為兩類，分別是基於影像的視覺伺服

(image-based visual servoing) 與 基 於 位 置 的 視 覺 伺 服 (position-based visual servoing)。在基於位置的視覺伺服架構中，需建立目標物體在卡式座標系中的 位置，因此若用來估測的模型和實際的目標物體有誤差，可能會使系統無法到 達預定的姿態。對基於影像的視覺伺服架構來說，因為直接在影像特徵空間中 做控制，沒有經過姿態估測的過程，因此不需要精確的目標幾何模型，也就沒 有上述的問題，兩種架構都得到深入的探討。Wang et al. [17]使用基於影像的視 覺伺服在移動式機器人上，並利用 Lypunov 方法驗證此運動控制的穩定性。 Chesi et al. [18]利用機器人在移動的過程中透過影像資訊建立更精準的 3D 目標 位置，達成基於位置視覺伺服的自動導航機制。而Kim 與 Oh [19]利用 Fuzzy 的 方式融合基於影像的視覺伺服與基於位置的視覺伺服，由機器人運動模型規劃 機器人的運動控制。 而救援機器人所要執行的自主性任務常需多隻合作才可達成，近年來有許 多文獻專門在研究多機器人合作搬運，[20][21]將多代理人的概念實現在多機器 人上，搭配各機器人行為模式的設計，使多機器人達成合作性的搬運任務。

1.3 問題陳述

1. 影像防震系統的穩健性與即時性： 由於影像防震系統須針對各種環境下的震動，提供一個穩定化的視訊輸 出，考驗著影像防震系統的穩健性，而當機器人在移動時，對畫面產生的移動 向量易超出影像穩定所能補償的範圍，需設計機制偵測機器人本身移動產生的 移動向量。另外，由於數位影像防震處理的演算法常會對處理器造成極大的計 算負載，計算量過大時，畫面更新率便會下降，畫面更新率低於 26 fps (frame per second)時，人眼就會感覺畫面不順暢，因此如何實現一個即時的影像防震 系統是必須克服的問題。 2. 感測資訊誤差造成目標位置估測不精準： 一般分散視覺系統攝影機的位置大部分是固定的，而本研究因為將攝影機 裝置在機器人上，建構了移動式的分散視覺系統，由於影像資訊與電子羅盤以 及機器人定位等各感測資訊都有誤差，造成以分散視覺估測目標位置的誤差極 大，不精確的目標位置使得機器人無法順利地完成自主性的任務，考慮這些誤 差造成因素，如何提高目標位置估測的準確度將是本研究探討的項目之一。 3. 多機器人合作任務： 單隻機器人無法實現合作性的任務，因此為了發揮分散式機器人的優點， 本研究將探討如何利用影像感測資訊，達成合作搬運任務的實現。而因為攝影 機的視角有限，單靠基於影像視覺伺服的運動控制，無法使多機器人執行合作 搬運的任務，本研究將設計如何使用分散視覺系統的目標定位資訊，加上合作 策略使多機器人自動執行搬運任務。

1.4 研究方法

1. 建立即時且穩健之影像防震功能於嵌入式系統：

本研究使用 Kalman filter 估測機器人本身運動對影像產生的移動向量。藉

由兩階的定速模型，運作上不會對演算產生過大的負載，如此當機器人在移動

時，也可得到平穩的視訊。在偵測向量時，當畫面在：(1)對比度差、(2)移動物 存在時，容易偵測出錯誤的震動向量，本研究參考Hsu et al. [9]利用 Correlation 分布的特性設定閥值濾除錯誤向量，並且加入變異量(variance)的分析做錯誤向 量濾除的動作。 為了能實現一個更即時的影像防震系統，必須降低計算震動向量的計算 負載，本研究參考了Hierarchical search [10]演算法，藉由將解析度分層收尋 向量，提升向量估測效率，大量降低計算負載，使得系統可以更即時展現防 震後的影像。 2. 融合分散視覺系統於目標位置估測： 在所建構的移動式分散視覺系統裡，利用Triangulation方法定出目標物在空 間中的位置，考慮機器人本身定位以及電子羅盤角度誤差，攝影機彼此的幾何 關係會直接影響估測目標位置的精確度，針對此特性分析各種幾何關係下的精 確度，並且設計各種情況下的權重值，如此當多機器人在各種幾何關係下，都 可透過融合的機制得到最精確的目標位置。 3. 整合視覺伺服於多機器人合作搬運： 本研究設計一個合作性搬運目標的任務，利用基於影像視覺伺服使機器 人自動趨近目標，並搭配分散視覺系統所估測的目標位置資訊建立基於位置 視覺伺服的運動機制，使機器人確切到達目標點，但由於履帶式機器人運動 過程中常發生打滑，利用定位資訊虛擬一個運動的邊界，使機器人運動過程

即使打滑走偏也能及時做修正，最後設計一個合作策略使機器人可成功地搬 移目標物。

1.5 論文架構

本論文共分六章，第一章旨在簡述研究動機、目的與背景，並陳述所要解 決的問題與研究的方法。第二章介紹分散式救援機器人的通訊以及硬體架構， 硬體架構包括影像處理平台與運動控制電路以及感測器。第三章描述如何在平 台上實現一個即時影像防震系統，利用 Kalman filter 增加機器人在移動時的平 穩性，再藉由所設計的錯誤向量偵測機制增加系統的強健性。第四章利用分散 式攝影機的融合機制，可以精準地估測出環境中目標物的位置。第五章整合第 四章的目標位置估測於視覺伺服運動控制，實現一多機器人合作搬運任務。最 後第六章中對本論文做一個結論，並提出未來可再發展之方向。

第二章 分散式救援機器人系統架構

2

2.1 硬體與通訊架構

為實現分散式救援機器人，發展了三台履帶式機器人，如下圖 2-1。每台 機器人的硬體架構大致分為上層與下層，上層透過 DSP 6437 影像處理板與 PC 執行影像處理與無線網路傳輸；下層透過DSP 2812 負責馬達運動控制。 圖 2-1 分散式救援機器人 Robot 1 DSP 6437 Vision CAN DSP 2812 Location Motion Control Robot 2 Eee PC Vision CAN DSP 2812 Location Motion Control Robot 3 IBM PC Vision CAN DSP 2812 Location Motion Control Wireless IEEE 802.11 圖 2-2 多機器人硬體與通訊架構

通訊架構使用了兩種通訊協定：CAN 與 IEEE 802.11。CAN 負責機器人上 層與下層的資料傳輸；IEEE 802.11 負責機器人彼此間的通訊與影像的傳輸。以 此兩種傳輸協定建立了分散式機器人系統通訊架構，如圖 2-2。

2.1.1 CAN 傳輸介面

CAN 提供可靠快速的連線，網路傳輸速率可達 5 Kbps ~ 1 Mbps，可用於即 時系統(real-time system)，因此極適合用在機器人上層與下層的通訊上。CAN 的接線方式是用 CANH、CANL 兩條訊號線佈線，每一個節點都可以透過此兩 條接線來存取 CAN 上的資訊，在此兩條接線的兩端必須以 120 Ω的終端電阻 做連結，如圖 2-3 所示。由於 1 號與 2 號機器人的上層是 PC-based 處理系統，

必須搭配 USBCAN，讓電腦可以擷取 CAN-bus 上的訊息。本研究採用的 USB

CAN 為周立功單片機發展有限公司開發出來的 USBCANII 智能 CAN 接口卡， 圖 2-4 為 USBCANII 外觀。

圖 2-3 典型的 CAN-bus 架構

2.1.2 IEEE 802.11 傳輸介面

關於 IEEE 802.11 無線網路傳輸部份，DSP 6437 所使用的是開發平台上的 網路模組，晶片為 Micrel KS8001L，藉 TCP/IP 的傳輸協定達成機器人間的通 訊。而 PC-based 的機器人則是利用無線網卡加上程式的開發，達成機器人間透 過 IEEE 802.11 無線網路的資料傳輸架構。IEEE 802.11 最快速度可以到達 54Mbs，實現可移動式的監視系統。802.11 制訂了兩種不同類型的無線區域網 路基本架構，分別是「Ad Hoc Wireless LAN」和「Infrastructure」，在此選擇 Ad Hoc Wireless LAN 的無線網路架構，Ad Hoc 架構能即時架設起無線通信網路， 在這種架構中，通常任二個用戶間都可直接通訊，可以達到點對點傳輸的目 的。

2.2 DSP 6437 影像處理平台

機器人上影像處理所使用的硬體為 TI(德州儀器公司)出產的 DSP 6437，如 圖 2-5，處理速率高達 600MHz，屬於 C6000 系列的產品。TI 依照應用的需 求，區分為三大平台：高效能的C6000 DSP 平台、低功率的 C5000 DSP 平台、 以及最佳化控制的C2000 DSP 平台，由於影像處理以及 Ethernet 網路傳輸需要 極高的運算量，且DSP 6437 整合了許多週邊通訊設備：透過 CAN-bus、RS232 與底層運動控制平台 DSP F2812 做資料的傳輸，以及 Ethernet 使其可與監控端 電腦達成即時的通訊與影像傳輸，因此選用了此平台應用在機器人上。

圖 2-5 DSP 6437 開發平台 圖 2-6 DSP 6437 硬體方塊圖 由圖 2-6 硬體方塊圖中，針對重要的硬體介面做簡單的介紹，如下： „ Video IN： 透過 AV 端子將攝影機資料輸入。 „ CAN、RS232： 分別為 CAN-bus 與串列資料 RS232 傳輸介面。可與 F2812 通訊。 „ ENET RJ45： 10/100 Mbps Ethernet 傳輸介面。本研究利用其介面傳 送影像資料至遠端電腦。 „ Embedded JTAG： 燒錄以及偵錯開發的程式與演算法。

2.2.1 EDMA 模組-協助資料搬運

在影像處理的流程裡，常會使用到大量資料的搬運與複製，例如一個畫面

更新率為 30 fps，大小為 360 x 240 pixel 的彩色影像，每秒鐘就有 7776000 Bytes 的影像資料要處理，如果用 CPU 做這些資料的搬運，便太耗費 CPU 的資 源。因此發展了EDMA (Enhanced Direct Memory Access)，EDMA 在 DSP 上是

一個很重要的週邊，因為它可以協助 CPU 做資料搬運的工作，避免 CPU 的資

源浪費。EDMA 內部具有多個通道，可同時提供多組資料的搬運動作，使得在

大量資料的搬運上效率高又不佔用CPU 資源。

接著介紹本研究所實際應用的例子，由於所接收的影像資料為 YUV422 的

影 像 格 式 ，Y 所代表為明亮度(luminance)，U 和 V 則分別為色度及濃度 (chrominance、chroma)，由於 YUV 的資料是錯綜排列的，必須分別取出 Y、U 與V 的資料，如圖 2-7。 圖 2-7 利用 EDMA 將影像資料做搬移 為了達成此資料搬移，便利用 EDMA 的區塊同步模式(block-sync)，區塊同 步模式便是每次搬運以一個區塊(block)長度為單位，如圖 2-8 所示，每個區塊 由多個矩陣(array)組成，而矩陣是由數個連續資料元(element)組成。藉由在程式 中PaPAM 暫存器設定每個資料的間距與大小，每當完成一個區塊的搬運時，便 會回傳一個TR 觸發訊號，至下個區塊執行同步的搬運。

Array 0 Array 1 Array 2 Array 3

Array 0 Array 1 Array 2 Array 3

Array 0 Array 1 Array 2 Array 3 Block 0 Block 1 Block 2 區塊同步搬運 回傳TR 圖 2-8 區塊同步模式搬運資料 由上圖 2-8 的例子總共完成 3 個區塊的資料搬運，每個區塊內有 4 個矩陣 完成同步搬運，以這樣的觀念便可達成將 YUV422 格式的影像分別取出 Y、U 與 V 的資料。在本研究中除了此應用，另外在許多影像處理的流程中常須要矩 陣資料的搬移與複製都是利用此 EDMA 模組，既不消耗 CPU 資源又可達成大 量資料的搬運，對即時資料處理的系統非常重要。

2.2.2 NDK 網路開發套件

TCP/IP 是 Internet 網路上傳輸資料所必需的協定，這種網路通信模式在 PC 機之間的實現已經完善，但是由於體積、價格等因素限制了應用的範圍。因 此，基於TCP/IP 協定與 Ethernet 的嵌入式系統網路通訊設計成為目前一個熱門 的話題。本系統實現了在以 TI 公司的 DSP 6437 為核心嵌入式系統的 Ethernet 傳輸。利用本系統可輕鬆的實現在局域網中對數位電視傳輸流信號的傳輸、調 度。網路驅動介面採用了 TI 公司的針對 C6000 系列 DSP 推出的 TCP/IP NDK(Network Developer's Kit)網路開發套件來實現。

在 TCP/IP 的模組中定義了兩組傳輸層的協定：TCP 與 UDP。TCP 用以建

立可性賴連結導向的資料傳輸，可確保資料的完整與正確；而UDP 用以產生不

上感測資料與命令的傳輸使用 TCP 協定，確保傳輸的正確性；影像資料的傳輸

因為重視即時性所以使用UDP 的傳輸協定。

已建立 TCP/IP 協定為基礎的網路傳輸在機器人上，為了達到以 IEEE

802.11 為介面的無線傳輸，使用 ASUS 公司的無線傳輸產品 WL-330gE，利用 其轉接器(adapter)功能，可將 Ethernet 介面的傳輸轉為 IEEE 802.11 介面的無線 傳輸，達成機器人彼此之間無線訊號的溝通。

2.2.3 DSP/BIOS 作業系統

在一個嵌入式系統中，軟體的核心就是作業系統，TI DSP提供一套即時作 業系統的開發環境，稱為DSP/BIOS，提供圖形化的系統配置，即時性的訊號分 析，以及API的函式庫，且屬於一個多緒程(multi-thread)的作業系統，讓使用者 快速發展一個嵌入式即時系統。 DSP/BIOS 的 配 置 分 成 六 大 物 件 群 組 ， 分 別 是 系 統 (system) 、 量 測 工 具 (instrumentation)、排程機制(scheduling)、同步機制(synchronization)、輸出輸入 (I/O)、CSL函式庫(chip support library)。在本研究的應用的系統配置中，主要建 構了網路傳輸續程以及影像擷取續程，利用排程機制分配優先權，影像處理優 先權較高執行完成才切換至網路傳輸續程；在資料傳輸的應用上利用了同步機 制中的信號(semaphore)物件，使得CAN與TCP/IP兩種傳輸介面的資料得以同 步；在影像擷取以及CAN傳輸的驅動部份使用CSL函式庫，讓使用者可藉由 CSL函式庫中的函式設計所需的功能運作。因此本研究利用此DSP/BIOS作業系 統設計出符合本研究機器人應用所需的嵌入式即時系統。

2.3 機器人運動控制架構

2.3.1 運動模型

機器人運動模型如圖 2-9 所示，履帶之左右兩軸平行。 θ ω x y X Y v 圖 2-9 機器人運動模型 機器人運動模型參數定義如下： x、y：機器人中心之 X 軸與 Y 軸位置。 θ： 機器人方向與X 軸之間的夾角。 v： 機器人速度。 ω：機器人角速度。 由於機器人在運動時，輪軸的方向是沒有速度的，運動模型必須受到非全向性 (nonholonomic)限制： ycosθ−xsinθ =0 ( 2-1 ) 參考[24]，機器人運動方程式(kinematics equation)如下式 ( 2-2 )，將利用此運動 方程式控制機器人自主性行為。

cos 0 sin 0 0 1 x v y θ θ ω θ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢₌ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ _{⎥ ⎣ ⎦} ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦    ( 2-2 )

2.3.2 多軸馬達運動控制電路

在機器人之運動控制硬體上使用本實驗室所設計的多軸馬達運動控制電路 板，如圖 2-10，功能特色如下： „ 以 TI DSP2812 為核心控制器。 „ 獨立四軸 DAC 輸出介面。 „ 獨立四軸 Encoder 輸入介面。

„ 周邊通訊介面：SCI 兩組、SPI 兩組、CAN 一組。

以此馬達運動控制電路為機器人運動控制核心，將電路體積縮減，並且能 接收周邊感測資料。機器人使用的馬達為祥儀公司出產的IG-32GM 搭配馬達驅 動電路如圖 2-11 與圖 2-12。馬達運動控制電路中的 DAC 輸出介面可輸出 0-12V 的類比電壓至馬達趨動電路，以輸出不同的轉速命令，而 Encoder 輸入介 面可回授馬達的轉速，以此建立的速度迴路上搭配 PI 控制器，達到機器人馬達 的轉速控制。 圖 2-10 多軸馬達運動控制電路板

圖 2-11 裝置於機器人之馬達 圖 2-12 馬達驅動電路

2.4 感測器

2.4.1 紅外線攝影機

在機器人上裝配了紅外線攝影機如圖 2-13，紅外線攝影機是運用人類可見 光與 CCD 可感應光源範圍不同的原理所設計。CCD 是電荷耦合元件(Charge Coupled Device)的簡寫，能將感測到的光轉換成電荷訊號，加以處理，這是目 前以固態影像元件應用在攝影機最尖端的技術。 (a) (b) 圖 2-13 紅外線攝影機 (a)外觀 (b)黑暗中投光燈補光

圖 2-14 黑暗中監控端影像 在黑暗的環境下照度較低，攝影機的 CCD 在這種時候光度不足，必須 運用其他方式補足；紅外線攝影機就是運用紅外燈來補足光源，不過由於紅 外線攝影機是利用不可見光來拍攝，使用彩色攝影機時，由於紅外線的波長 不在可見光譜，且靈敏度低，因此不適合在紅外燈光源下運作，必須運用濾 光片過濾光源，只保留不可見光的波長，此時會以黑白畫面呈現。而把紅光 濾掉，夜晚就無法進行紅外線辨識，因此另外加了紅外線投光燈的裝置如 圖 2-13(b)；有了這紅外光的補助後，不加特別的濾鏡的情況下，在白天， 彩色攝影機得到的成像還是彩色的，而於夜間時，只有紅外光時得到的是黑 白的影像如圖 2-14。

2.4.2 Compass

本研究所使用之 compass 為 PNI 公司出產的 V2Xe，如下圖 2-15，主要特 色如下：

„ 0.01 的高解析度。

„ 大範圍的磁場量測能力( 1100 T± μ )。 „ 採非揮發記憶(可儲存校正值與功能設定)。 „ SPI 傳輸介面。 圖 2-15 Compass V2Xe Compass 傳輸介面： 由於 V2Xe 與 F2812 均有 SPI 傳輸介面，如圖 2-16 可透過簡單的 連接方式即可完成資料的傳遞。 圖 2-16 V2Xe 與 F2812 介面電路 Compass 使用前必須經過校正，本研究所使用的校正方法是將 Compass 等速旋轉1~2 圈後，可得 X、Y 軸的最大與最小計數值，並經模組校正步驟 後，磁通密度會呈現圓形且圓心位於座標原點，稱為磁圓(magnetic circle)。 Compass 的量測除了受地磁與磁偏角影響外，也受周圍電磁波影響，在此分 析了它在不同場地下所量測到的誤差範圍，量測 50 筆數據，圖 2-17 畫出

其中兩筆數據。分析結果此 Compass 平均誤差為 0.16 degrees，誤差標準差 (standard deviation)為 4.39 degrees，此誤差分布將被考慮在第四章所介紹的 計算目標物位置的法則中。

圖 2-17 量測 Compass 誤差分佈

第三章 影像防震

3 為了獲得機器人上之穩定影像，影像防震功能已被發展應用在機器人上， 首先介紹所使用的影像穩定系統演算流程架構。 Stable Stable Video output Video output 圖 3-1 影像穩定演算法流程圖 如上圖 3-1 的演算流程，以下將對各個單元做簡單的介紹： 1. 區域向量估測單元：計算出影像中各個區塊的移動向量。 2. 錯誤向量濾除單元：利用 Correlation 與變異量的分析，濾除錯誤的估 測向量。 3. 全域向量估測單元：計算影像中整體畫面的移動向量。 4. 運動平穩單元：當機器人在運動時，估測本身的運動向量，使處理後 的畫面達到平順的移動。 5. 震動補償單元：將已估測的震動向量做補償，最後輸出一穩定的影 像。 經由以上的演算流程最後輸出一穩定的影像，針對上圖演算流程各單元細 部將在以下做介紹。

3.1 震動向量的估測與補償

此節對於向量的估測與補償方法再做更細部的介紹。 „ 區域運動向量估測單元：

參考Yeh et al. [4]的方法，利用 Block matching 偵測出各區域的移動向量。 如圖 3-2，Block matching 方法計算搜尋區塊 Correlation 值，如下式 (3-1) 與 (3-2)： 1 ( , ) ( , ) [ ( , ) ( , )] i i t t x y S COR m n I x m y n I₋ x y ∈ =

∑

+ + − ( 3-1 ) 正規化： 1 ( , ) max i i i COR m n C COR = − ( 3-2 ) Si區塊大小為20 x 20 pixel 的範圍，i=1~5 分成五個區塊搜尋區域運動向量，將 搜尋區域內取出正規化 Correlation 最大值的方塊與原本方塊之間的向量，便為 區域運動向量。如下公式：

{

}

arg max ( , ),| , i i LMV = C m n − ≤ ≤p m p q n q− ≤ ≤ ( 3-3 ) 其中p=q=15，因此可搜尋的向量為 30 x 30 pixel 的範圍。 圖 3-2 Block matching 偵測移動向量 „ 全域運動向量估測單元： 藉由對區域向量加總作平均推得全域向量。因為畫面的中心的震盪對監控 者是最敏感的，且較不會受旋轉運動的影響而估測出錯誤的平移向量，因此對

於平移向量將中間的權重增為兩倍，最後使用加權平均的方法計算全域運動向 量，如圖 3-3： 圖 3-3 全域平移運量向量計算 對於畫面旋轉運動角度，則是利用餘弦定理計算周圍四個方塊的旋轉角度，如 圖 3-4，取四個角度平均為整個畫面的選轉角度。 圖 3-4 旋轉運動角度計算 „ 震動補償單元： 參考 Chang et al.[6]等人利用已偵測畫面的全域移動向量，實現一個剛性映 射(rigid transformation)的向量空間轉換，此剛性映射公式如下式： ' cos sin ' sin cos x y X X Y Y X Y θ θ δ θ θ δ = − + ⎧ ⎨ _{= + +} ⎩ ( 3-4 )

( , ) is the coordinate of the previous frame ( ', ') is the coordinate of the current frame

, , is the coefficient of rigid transform

x y X Y X Y δ δ θ 其中δ x,δ y 分別代表已估測的平移移動向量，θ 代表轉動角度，影像中每個像 素經此向量空間轉換後，便可得到一穩定的影像。

3.2 Kalman filter 使畫面平穩化

由於機器人在運動的過程中，也會對影像產生移動向量，如果將之視為震 動向量做補償，超出向量的偵測範圍時反而會產生更大的晃動。因此利用 Kalman filter估測自主性運動所產生的移動向量，將自主性向量從要補償的向量 中移除，使得整個畫面跟著Kalman filter輸出的向量移動。所以當機器人在移動 時，在演算法中建立此運動平穩單元，監控者可得到一平穩的移動畫面。 由於所使用的機器人常處於定速的運動模式，因此在Kalman filter的處理中 所使用的運動模型為一個定速移動模型如下式 (3-5)與(3-6)。根據機器人不一定 全在定速下移動，設計 Q (process variance) : 0.001；而根據搜尋範圍的平方， 設計 R (measurement variance ): 225。 ( ) 1 0 1 0 ( 1) ( ) 0 1 0 1 ( 1) [ ] ( ) 0 0 1 0 ( 1) ( ) 0 0 0 1 ( 1) x x y y x x y y x n x n x n x n w v n v n v n v n − ⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎢ ⎥ ⎢ ⎥⎢ ₋ ⎥ ⎢ ⎥₌⎢ ⎥⎢ ⎥₊ ⎢ ⎥ ⎢ ⎥⎢ − ⎥ ⎢ ⎥ ⎢ ⎥⎢ ₋ ⎥ ⎢ ⎥ ⎣ ⎦⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ( 3-5 ) ( ) ( ) 1 0 0 0 ( ) [ ] ( ) 0 1 0 0 ( ) ( ) x mx y my x y x n x n x n v x n v n v n ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ ⎡ _{⎤ ⎢ ⎥} = + ⎢ ⎥ ⎢ _{⎥ ⎢ ⎥} ⎣ ⎦ ⎣ ⎦ _{⎢ ⎥} ⎢ ⎥ ⎣ ⎦ ( 3-6 )

Kalman filter細部演算流程請參考[25]，最後Kalman filter的輸出即為自主移 動的向量。接著將Kalman filter估測所得到的自主移動向量從所要補償的全域運 動向量GMV中扣除，如下式： ( ) ( ) ( ) cmp Klm V n =GMV n −V n ( 3-7 ) Vcmp代表最後需補償的向量，VKlm為Kalman filter估測之自主移動向量。下圖 3-5 是一個模擬的例子，當機器人在移動，並有震動向量產生時，兩個向量對畫面

所產生的向量如圖中的虛線。經由Kalman filter濾出機器人本身的運動向量，如 圖中的實線。將整個畫面的移動向量扣除機器人本身的移動，便可計算出震動 向量，將此震動向量補償後，最後的輸出便為一個平穩的影像。 0 1 2 3 4 5 6 7 8 -10 0 10 20 30 40 50 60 70 80 time (s) M o ti o n ( p ix e l) intentional motion detected motion vibration motion 圖 3-5 Kalman filter 估測自主運動向量

在此並且將Kalman filter與low-pass fiter做比較，設定其low-pass filter截止頻 率為0.2Hz。如上圖機器人移動使畫面往水平方向移動的例子，由下表 3-1的誤 差指標RMSE (Root Mean Square Error)可知：利用low-pass filter會降低對自主移 動向量估測的精確度，使防震的效果變差。這是由於本研究機器人的運動幾乎

處於定速的情況，且Kalman filter參數會自動調整，使自主運動的估測會越來越 趨近一個定速的向量變化，但low pass filter並不行，因此得知Kalman filter極為 適合在本身運動的估測上。

表 3-1 自主移動估測由 Kalman filter 與 low-pass filter 誤差比較 Error Kalman filter Low-pass filter RMSE (pixel)

3.3 偵測錯誤估測向量

當影像畫面(1)對比度差、(2)重複性高、(3)畫面有移動物體時，容易偵測錯 誤的區域移動向量，這是由於在這些情況下經由Correlation量測無法正確的估測 震動向量。Correlation量測在各個情況下的分配如下圖 3-6。 圖 3-6 Correlation 量測值分佈在不同的情況 (a)正常情況 (b)對比度低 (c)畫面有移動物體 畫圈部份：錯誤估測之向量 由於上述情形會造成錯誤向量的估測，因此在演算流程中設計了錯誤向量 濾除單元，將介紹如何利用Correlation量測與變異量的分析，識別出錯誤的區域 向量並且濾除。由上圖 3-6(a)在正常的情況下，Correlation的分佈有一峰值，可 正確的估測移動向量。但影像畫面在圖 3-6(b)(c)的情況下無一明顯峰值，便易

產生錯誤的偵測向量。因此如Hsu et al. [9]所述利用此Correlation特性，做錯誤 向量的濾除。偵測為錯誤向量的條件如下： max mean i i Th C −C <C ( 3-8 ) Th C 值是由擷取 1000 筆已知震動向量的影像數據所建立的資料庫觀察正確向量 與錯誤向量之(C_imax−C_imean)的分佈所決定的。如圖 3-7 由分佈的趨勢分別決定 Th C ＝3500，如此可初步的濾除錯誤向量。 0 1000 2000 3000 4000 5000 6000 7000 8000 9000 10000 0 50 100 150 200 250 Correlation max-mean s am pl es of bl oc k

Correlation distribution in "right estimated" vector

0 1000 2000 3000 4000 5000 6000 7000 8000 9000 10000 0 20 40 60 80 Correlation max-mean s am pl es of bl oc k

Correlation distribution in "wrong estimated" vector

圖 3-7 (C_imax−C_imean)分佈。上:正確估測向量，下:錯誤估測向量 接著，由於錯誤的區域移動向量通常是局部性的，假設誤差成常態分佈， 利用一張影像中所有區域移動向量的變異量計算，濾除差異較大的移動向量。 輸入影像防震系統一連續已知震動向量影像，根據誤差分布設定信賴區間 (confidence interval)，分別針對畫面平移向量與旋轉角度做錯誤向量濾除的動 作，信賴區間的範圍的條件如下式( 3-9 )與( 3-10 )。如此能濾除錯誤的區域估測 向量，避免在之後的震動補償產生更大的晃動。 < < i V i LMV LMV std std_θ θ θ ⎧ ₋ ⎪ ⎨ − ⎪⎩ ( 3-9 ) 2 2 ( ) ( ) , i i V X X std std n θ n θ θ − − =

∑

=

∑

( 3-10 ) Th C

在一對比度差以及畫面中有移動物存在的影像，藉由Matlab模擬產生平移與旋 轉的震盪，比較藉由Correlation與變異量分析濾除錯誤向量後的結果，如下圖 3-8與圖 3-9。 0 0.5 1 1.5 2 2.5 -15 -10 -5 0 5 10 15 time (s) M ot ion ( pi x el )

Origion estimation for translation real translation estimated translation 0 0.5 1 1.5 2 2.5 -15 -10 -5 0 5 10 15 time (s) M ot ion ( pi x el )

Origion estimation for translation real translation estimated translation (a) (b) 圖 3-8 針對平移震動，估測震動向量誤差比較 (a)改善前 (b)藉 Correlation 與變異量分析降低估測誤差 0 0.5 1 1.5 2 2.5 3 -15 -10 -5 0 5 10 15 time (s) R ot at io n (d egr ee) Origion estimation thgb real rotation estimated rotation 0 0.5 1 1.5 2 2.5 3 -15 -10 -5 0 5 10 15 time (s) R o ta ti o n ( d e g re e )

Estimation modified by standard deviation analysis real rotation estimated rotation (a) (b) 圖 3-9 針對旋轉震動，估測震動向量誤差比較 (a)改善前 (b)藉 Correlation 與變異量分析降低估測誤差 由上圖 3-8與圖 3-9發現經濾除錯誤向量後，對於平移或旋轉的震動向量 估測皆更為精準，誤差RMSE下降大約85%，可解決當畫面對比度差估測向量錯 誤的問題。影像穩定化系統之輸入影像與穩定後的輸出影像如下圖 3-10。 RMSE=3.12 RMSE=0.44 RMSE=2.93 RMSE=0.24

圖 3-10 輸入影像與穩定後輸出，上列為輸入影像，下列為輸出影像 上圖 3-10可觀察出，上列為一晃動的輸入影像序列，經穩定補償後，下列 為一個穩定的影像序列，十字的中心點位置可保持不變，證明經由本研究所設 計的影像防震演算法後，可成功得得到一平穩的影像。

3.4 即時嵌入式系統實現

由於動態影像處理很重視即時性，尤其是影像穩定化，如果演算法負載過 重造成畫面更新速率降低，則嚴重降低防震的效果。因此此節將介紹利用 Hierarchical搜尋法提升運動向量估測的效率，如下圖 3-11。將影像分成三層不 同的解析度，先由最小解析度階層3藉由3.1節的方法估測最有可能的9個候選向 量，以此9個候選向量進入階層2用同樣的方法再選出3個候選向量，最後將之傳 入最高解析度的階層1估測出運動向量。由於候選機制的概念，此法可維持估測 向量的精確度，又可明顯提升搜尋效率。

圖 3-11 Hierarchical 搜尋法提升向量估測效率

已將此法實現在 TI DSP 6437影像處理開發版，利用開發板上所建立的視 訊處理系統VPSS (video processing subsystem)擷取影像資訊，在實現上為了提升 矩陣資料的搬移的效率，使用了EDMA模組如2.2.2節所介紹，此週邊模組協助 CPU處理大量的資料搬運，極為有效地降低CPU負載。由於系統含有影像處理 與網路傳輸兩個續程，利用所建構之DSP/BIOS作業系統的排程機制分配優先權 如2.2.3節所介紹，設計出符合本研究機器人應用所需的嵌入式系統。最後再藉 由Hierarchical搜尋法提升演算法效率，畫面更新速率已由原來的10 fps提升至28 fps，使得影像穩定處理更為即時。

3.5 小結

由本章的模擬與實驗結果加以分析，可以得到以下結論：

1. 在所設計之影像穩定系統上，當機器人在移動時，透過徵測自主運動向量 使輸出一平穩的影像，模擬藉由對影像產生已知的震動與自主性移動，比

較自主向量估測誤差的結果，證明Kalman filter明顯優於low-pass filter在自 主向量估測上，使輸出的視訊更為平穩。 2. 透過Correlation與變異量的分析，設計機制濾除錯誤的區域向量，可明顯降 低當影像畫面在(1)對比度差、(2)重複性高、(3)畫面有移動物體等不良條件 下向量估測錯誤的發生，估測誤差降低了85%。 3. 此 演 算 法 已 實 現 在 TI DSP 6437 ， 達 到 省 電 、 體 積 小 等 優 點 ， 且 藉 由 Hierarchical search演算法，階層式估測運動向量的機制，顯著地提升畫面更 新速率由原來的10 fps到現在的28 fps，使影像防震輸出更為即時。 由本研究發展的影像防震系統實現在救援機器人上，使得當救援機器人在 崎嶇地形搜索時，監控端可得到一平穩的視訊。

第四章 融合分散視覺系統於位置估測

4 本章將介紹利用分散機器人對目標物的定位方法，由分散多機器人上 的影像感測建立分散視覺系統，透過多機器人資訊融合的機制，得到更精確 的目標位置。

4.1 影像處理辨識目標物

在每台機器人的影像裝置上，必須自動針對所設定的目標物進行辨識， 而所選擇的目標物為一橙色的目標物，如圖 4-4(a) 所示，必須偵測目標物 在影像中的位置，處理程式流程如圖 4-1，將依序介紹各處理區塊。 圖 4-1 影像處理流程 „ HSV 格式轉換： 人眼並不能分解色彩的組成量，所以RGB或是YUV系統對於人類是難以理 解的，用RGB來表示顏色雖然方便，但是兩個相近的顏色的RGB值卻可能相差 極大。若有一系統能先將顏色分類，然後再控制色彩的細節，那正符合我們人 眼的視覺，依照此需求所設計的色彩系統正是HSV 色彩系統。HSV 色彩系統 將顏色分為色度(hue)、飽和度(saturation)、亮度(value)三個部分來分別作表示， 如圖 4-2。此系統將色度定義在0~360 的角度範圍，而飽和度與亮度的值定義 為0~1 之間，不過原影像系統中的顏色都是用RGB來表示的，參考[26] RGB與 HSV的轉換公式如 (4-1)~(4-3)式。公式中max與min分別代表RGB色彩系統中的 最大值與最小值。

undefined if max min 60 0 , if max=r and G max min 60 360 , if max=R and G<B max min 60 120 , if max=G max min 60 240 , if max=B max min G B B G B H B R R G = ⎧ ⎪ ₋ ⎪ _{× + ≥} − ⎪ ⎪ ₋ ⎪ × + = ⎨ − ⎪ ₋ ⎪ × + − ⎪ ⎪ ₋ × + ⎪ − ⎩ D D D D D D D D ( 4-1 ) 0, if max=0 max min , otherwise max S ⎧ ⎪ =_{⎨ =} ⎪⎩ ( 4-2 ) max V = ( 4-3 ) 圖 4-2 HSV 格式 [27] „ 色彩辨識 以顏色為目標物的特徵，找出目標物在影像中的位置，由影像所接收 到每個像素的 RGB 格式資訊轉換為 HSV 值，由調整 HSV 值範圍，找出目 標物的 HSV 範圍，將符合 HSV 臨界值的像素二值化，所選擇的 HSV 臨界 值為： H： 10 ~ 35D D S： 0.4 ~ 0.7 V： 0.3 ~ 1.0 如圖 4-4(a)為原始影像，(b)為所選取 HSV 範圍所得到之二值化影像。

„ 標籤法 像素與像素之間稱之為連通性，常見的為四鄰近與八鄰近，運用八鄰近的 方式將鄰近像素為 255 值標為同一個編號，如圖 4-3 為標示在一張圖有不同的 區塊，將每個區塊標示為不同的編號。 圖 4-3 在二值化影像中貼標籤 在貼完標籤後，將影像所看到的最大面積分辨出來，並由( 4-4 ) 、( 4-5 )算 出目標物重心

(

x y,

)

，如_{圖 4-4}(c)中的紅色框框即表示所求得的目標物在影像 中的位置。 1 0 1n i i x x n − = =

∑

( 4-4 )

∑

− = = 1 0 1 n i i y n y ( 4-5 ) (a) 原始影像 (b)辨識顏色二值化

(c) 標示目標位置 圖 4-4 辨識目標物 PC-based的影像處理平台上設計了視窗介面，為Borland公司開發的C++ Builder 6 撰寫而成，如圖 4-5，機器人間彼此的通訊以及下節將介紹的目標位 置估測的結果也會顯示在此介面上。 圖 4-5 影像處理視窗介面

4.2 Triangulation 估測目標位置

由於每台機器人上各有一影像感測，建構了分散式視覺系統，藉由此系統

將利用Triangulation計算出目標物的位置資訊。在這之前需先獲得攝影機的參 數，利用[16]提供的“Camera Calibration Tool”，針對已知的模板做各個角度的攝 影，如圖 4-6。此toolbox擷取corner的特徵，由於模板的方塊間的距離已知，由 此計算出攝影機的內部參數。由於本研究使用兩種攝影機，所以計算出兩組參 數，如表 4-1。 Calibration images X Y O

The red crosses should be close to the image corners

50 100 150 200 250 300 50 100 150 200 圖 4-6 旋轉及移動平面校正物 表 4-1 攝影機參數 Parameter IR CCD WebCam

Focal length (pixel) 160.4 244.1 Principal point (pixel) (81.3, 60.7) (84.5, 55.7)

camera 1 camera 2 p1 p2 P O1 O2 圖 4-7 Triangulation 計算目標位置

由上小節所介紹辨識目標的影像處理，目標物在兩影像中的位置為p1和

p2，如圖 4-7。由機器人的定位資訊可得知O1與O2的位置，式子中的T為O2相對

於O1的向量矩陣，而R是由機器人上電子羅盤資訊推得攝影機間的旋轉矩陣， 利用各影像對目標的射線JJKp₁與JJKp₂，便可計算出目標位置P。但實際上受到各感 測器誤差的影響，兩射線由於呈現歪斜線的關係並不會相交，所以在此計算出 兩射線與彼此公垂線的交點，如下式子中的S1與S2，由於S1和S2平均的位置與兩 射線偏移距離的加總是最小的，因此如式子 (4-6)中的P取S1和S2平均為此目標 物位置的估測。

(

)

T T 1 2 1 2 1 1 T 2 2 1 2 ( ) S S 1 P S S 2 a p b p c p p a p b p − + × = = = + = + R R T T R JJK JJK JJK JJK JJK JJK _{( 4-6 )}

4.3 融合分散攝影機資訊

由於上節介紹之Triangulation計算的目標位置有些誤差，將利用分散式攝影 機的特性，透過融合的機制計算出更精確的目標位置。每台機器人上都有一影 像感測資訊，考慮機器人定位以及電子羅盤角度誤差，攝影機彼此間的幾何關 係會直接影響定位的精確度，針對此特性分析各種幾何關係下的精確度，並且 設計各種情況下的權重值，如此當多機器人在各種位置關係時，都可透過融合 的機制得到最精確的目標位置。 本研究考慮機器人的感測資訊設計一個模擬環境，假設機器人定位有20cm 為半徑的誤差範圍，根據2.4.1節實際量測電子羅盤為誤差平均0.16 degrees，標 準差4.39 degrees的誤差，而影像辨識資訊有2 pixel範圍的誤差。誤差分佈都是 以高斯分佈的方式隨機產生，在此模擬環境下目標位置的估測便會產生誤差， 如圖 4-8。 1 2 圖 4-8 目標物定位模擬環境 分析兩因素影響目標位置估測精確度：攝影機彼此間的(1)距離 (2)角度。 假設兩因素彼此是獨立的，測量距離因素時將角度固定，測量角度因素時將距 離固定。分別對角度與距離因素做分析，每個情況各量測100筆數據，如圖 4-9 與圖 4-10。 估測位置 實際位置

-200 -100 0 100 200 300 0 100 200 300 400 500 600 X (cm) Y(cm) -200 -100 0 100 200 300 0 100 200 300 400 500 600 X(cm) Y(cm) (a) (b) 0 20 40 60 80 100 120 140 160 180 200 10 20 30 40 50 60 70 80 90 Angle (degree) erro r(cm ) (c) 圖 4-9 角度因素對誤差的分析 (a)角度 30 度 (b)角度 80 度 (c)各角度分析結果 (point:誤差平均值，bar:誤差標準差)

-200 -100 0 100 200 300 400 500 -100 0 100 200 300 400 500 600 X(cm) Y(cm) -200 -100 0 100 200 300 400 500 -100 0 100 200 300 400 500 600 X(cm) Y(cm) (a) (b) 0 20 40 60 80 100 120 140 50 100 150 200 250 300 350 400 450 500 Distance (cm) error(c m) (c) 圖 4-10 距離因素對誤差的分析 (a)距離 100 cm (b)角度 300 cm (c)各距離分析結果 (point:誤差平均值，bar:誤差標準差) 根據圖 4-9 與圖 4-10 數據中，可依每個情況下誤差的平均值與變異量分 布，定義權重值，使其誤差越小權重越大，如( 4-7 )與( 4-8 )式，所求得的權重 值如圖 4-11，從圖中可知攝影機彼此間距離越小誤差越小，所以Wdist權重最 大；而彼此間夾角的角度 90 度時誤差最小，所以Wang權重最大。且發現圖 4-9 中角度因素變化對誤差的影響較距離因素來的劇烈，因此圖 4-11 中角度因素權

重值的變化也較劇烈。 10 20 30 40 50 60 70 80 90 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

Angle between cameras (degree)

Wei g h t 50 100 150 200 250 300 350 400 450 500 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Distance of cameras(cm) Wei ght (a) 角度因素權重 (b) 距離因素權重 圖 4-11 設計權重 max( ) max( ) 1

2 max( ) min( ) max( ) min( ) ang ang ang ang ang

ang ang ang ang

W μ μ σ σ μ μ σ σ ⎛ − − ⎞ = ⎜_⎜ + ⎟_⎟ − − ⎝ ⎠ ( 4-7 ) max( ) max( ) 1

2 max( ) min( ) max( ) min( ) dist dist dist dist dist

dist dist dist dist

W μ μ σ σ μ μ σ σ ⎛ − − ⎞ = _⎜ + _⎟ − − ⎝ ⎠ ( 4-8 ) 因假設距離與角度因素彼此是獨立的，因此將兩個權重值Wdist與Wang相乘當 最後每個量測值融合的權重，經過加權平均後便為此融合機制的結果如( 4-9 )與 ( 4-10 )式。( 4-10 )式中的i代表每兩台攝影機利用Trianglation計算所得到的估測 位置，實驗中用了三台攝影機因此可得到 3 2 C =3個估測位置，每個估測位置帶入 融合權重便可得到最後的融合結果。 dist ang Weight W= ⋅W _{( 4-9 )} i i i i i Weight P P Weight ⋅ =

∑

∑

( 4-10 ) 圖 4-12 分散式攝影機目標物定位實驗環境

利用所設計的融合機制之權重值，實際測試目標位置估測改善的結果，將 3 台機器人隨意散佈在大小為 600 x 600 cm的實驗環境，並透過影像處理偵測到 目標物，如圖 4-12。經實驗 100 筆的結果如下圖 4-13 以及表 4-2，可發現單 純的平均方法所估測的目標位置誤差是最大的，取平均的意思為將每兩個影像 資訊得到的所有估測值直接作平均，而考慮角度與距離因素融合的結果誤差是 最小的，單只考慮角度因素與距離因素的也都有所改善，不過角度因素的影響 較劇烈。由此可知此融合的機制應用在分散式的機器人上，可明顯提升對目標 位置估測的精確度。 0 20 40 60 80 100 120 140 160 180 200

average weighted average

(distance) weighted average (angle) weighted average (angle+distance) erro r( cm) 圖 4-13 融合機制降低目標定位誤差 (point: 誤差平均值，bar: 誤差標準差) 表 4-2 目標定位誤差比較 estimation error (cm) average weighted average (distance) weighted average (angle) weighted average (angle+distance) μ 110.5 98.2 27.2 22.7 σ 62.4 54.3 16.7 15.8

4.4 分析機器人數目對位置估測影響

上節介紹在 3 台機器人的條件下透過融合機制可明顯改善目標位置估測的 誤差，此節將分析機器人數目增加時，對目標位置估測有何影響。由於每台機 器人上裝置一台攝影機，任 2 台攝影機可對目標建立一個位置估測，所以假設 有n台機器人，便可建立C2n =n₂!個估測值，如果在感測資訊都沒有誤差的情況 下，每個估測值都是精確且相同的，但實際上機器人定位資訊與影像資訊都有 誤差造成每個估測值都有誤差。假設有5 台機器人便可建立 60 個估測值，將這 些資訊透過融合機制，便可降低目標位置估測的誤差。在模擬的環境下如圖 4-14，將機器人隨意分散在大小 600 x 600 cm的場地，比較機器人由 2 至 5 的數 目，圖中分別標示出實際位置、取平均與融合機制所求得的估測位置。每個情 況各量測1000 筆數據，分析結果如圖 4-15。 2 1 2 3 1 (a) (b) 4 2 1 3 3 5 4 1 2 (c) (d) real estimation real fusion average real real fusion average average fusion

0 25 50 75 100 125 150 175 200 225 250 2 3 4 5 robot numbers error(cm) 0 25 50 75 100 125 150 175 200 225 250 2 3 4 5 robot numbers error(cm ) (a) 取平均 (b) 融合機制 圖 4-15 機器人數目對位置估測的影響 (point: 誤差平均值，bar: 誤差標準差) 由圖 4-15 可知機器人數目增加時，可明顯降低對目標位置估測的誤差，另 外從圖 4-15 (a)取平均與(b)融合機制的比較可發現：透過融合機制在 3 台機器 人的數目時便可大幅地降低目標物定位誤差，取平均的方法必須到 4 台機器人 誤差才會趨於收斂，融合機制改善幅度比單只取平均明顯許多，因此本研究最 後使用3 台機器人實現分散視覺對目標位置的建立。

第五章 整合視覺伺服於多機器人合作搬運

5 在分散式多機器人的系統裡，透過設計多機器人的運動機制，使得機器人 利用已計算的目標位置達成合作性的搬運目標。分成兩種運動模式：(1)基於影 像視覺伺服，(2)基於位置視覺伺服。基於影像視覺伺服模式可使多機器人趨近 目標，而當目標位置估測精確度已達所需範圍時，即轉換成基於位置視覺伺服 的運動控制模式，使兩機器人可在左右兩方適當位置的地方達成合作性搬運的 動作。如下圖 5-1。 圖 5-1 設計運動機制於搬運目標任務

5.1 基於影像視覺伺服

基於影像視覺伺服主要是為了讓多機器人可自動地趨近目標，在趨近的過 程中持續更新目標位置的估測，利用第四章介紹的融合方法可在趨近的過程中 使目標位置的估測越來越準確。 運動的機制如圖 5-2，由於所設計的機器人是差動輪，當機器人沒偵測到 目標時，便往之前曾偵測到的方向旋轉；假如偵測到目標且大於10度，則利用 PI控制器將目標修正至影像的中間，如下式 (5-1)與圖 5-3；而角度小於10度則 往前走。利用此方法可以穩定地實現基於影像視覺伺服，使目標在影像的範圍 內並且有效率的趨近目標。 ω=K e k_p⋅ _φ[ ]+K_I⋅

∑

e k_φ[ ] ( 5-1 ) [ ] 10 e k_φ > D e k_φ[ ] 10< D e k_φ[ − >1] 0D e k_φ[ − <1] 0D 圖 5-2 基於影像視覺伺服流程 [ ] e k_φ 圖 5-3 基於影像視覺伺服校正方式

5.2 基於位置視覺伺服

當兩台機器人由左右兩旁合作性的去搬運目標，必須得知精確的目標位 置，由5.1節的方法使兩機器人自動趨近目標，在這樣的過程中可得到逐漸更精 確的目標位置。當誤差到達允許的範圍時，便進入基於位置視覺伺服使機器人 精確地到目標點，並做合作性的搬運。在此將說明如何設計一機制確認目標估 測的誤差已到達允許範圍。藉由變異量與融合機制中權重的分析，設定了兩個 條件來確認目標位置的估測誤差是否收斂： 1. 誤差標準差平均< 30 cm 2. 任一估測目標的權重> 0.811 (根據誤差小於30 cm) 兩機器人在趨近的過程中，目標位置估測值如下圖 5-4的例子，將估測位置資 訊取過去5點的標準差，標準差表示著一組數據的變異性(variability)，如 (5-2) 與 (5-3) 式，取5點的原因是考慮其識別震盪的即時性。接著將標準差平均可顯 示其標準差變化的趨勢，如 (5-4) 式，因為目標估測由一開始的震盪而逐漸收 斂且精確，可發現標準差平均有逐漸下降的趨勢，因此我們設定當誤差標準差 平均< 30 cm，當作一確認收斂的條件，而30 cm是根據搬運機構大小所設定 的。另外由上節的融合機制中權重的分析，考慮機器人彼此角度與距離因素所 成的誤差< 30 cm時的權重值為0.811，以此當作第二確認收斂的條件。 4 5 ( ) 5 n i i n P P Var = − − =

∑

( 5-2 ) 5 5 Std = Var ( 5-3 ) 0 Avg of = n i i Std Std n =

∑

( 5-4 )

0 10 20 30 40 50 60 70 80 90 100 0 20 40 60 80 100 120 140 160 180 200 time(s) (c m ) Std Avg of Std Est. Position Real Position 圖 5-4 目標位置估測與誤差標準差分析 當機器人符合目標估測收斂條件時，則進入虛擬邊界位置運動控制的模 式，運動機制如圖 5-5：

φ

tar D Dbias >15cm 圖 5-5 虛擬邊界位置運動控制流程 使兩機器人可精確地到達目標左右兩處，虛擬邊界運動控制機制是參考 Seo 與 Lee [23]的作法，利用定位資訊虛擬一個運動的邊界，使機器人運動過程即使打

滑走偏也能及時做修正。當機器人接收位置命令時，機器人先轉角度φ，再走 距離 Dtar，使機器人到達所需的位置，在這之中持續的更新影像回授的目標位 置資訊。但由於履帶式機器人極易發生打滑現象而走偏離原來的軌道，因此設 計了一虛擬邊界的機制，示意圖如下圖 5-6，使軌道當偏離超過 15 cm 時，就 進入修正狀態將偏移量修正，15 cm 為考慮搬運機構大小所設定，偏移量的計 算 Dbias是藉由機器人與目標間的角度與距離得知，如(5-5)式，Ptar與 Pr分別代 表目標點與機器人的位置，θ 為機器人之朝向角。如此可解決打滑的問題使兩 機器人精確地到達目標左右兩旁。 tan( ) bias tar r D = P −P ⋅ θ φ− ( 5-5 ) tar D θ φ 圖 5-6 虛擬邊界運動控制

5.3 合作搬運任務實現

當兩機器人走至目標兩端，之後兩機器人即利用合作機制搬運目標。機器 人1號上裝有一載運盒，負責載運目標物，機器人2號裝有一手臂負責將目標推 至機器人1號的載運盒，合作策略如下圖 5-7，機器人1號先往目標前進至可接 到目標的位置，就定位後通知機器人2號使之開始推移目標的動作，當機器人1 號偵測目標已掉入載運盒時，則通知機器人1號與2號回到原點，否則2號繼續推 移，如此以這樣的合作機制便可成功地搬運目標。 圖 5-7 搬運合作機制 整個搬運任務的實現如下圖 5-8，藉由基於影像視覺伺服機器人自動趨近 目標，藉由所設計的機制確認目標位置估測的收斂，便進入基於位置視覺伺服 的運動控制，最後經合作策略搬運目標達成任務。機器人回傳到監控端的人機 介面，人機介面所使用的程式為Borland公司開發的C++ Builder 6撰寫而成，程 式介面如圖 5-9。

(a) 基於影像視覺伺服 (b) 基於位置視覺伺服

(c) 合作性搬運目標物 (d) 將目標物搬回 圖 5-8 整合視覺伺服實現機器人合作搬運目標

(c) 合作性搬運目標物 (d) 將目標物搬回 圖 5-9 遠端顯示機器人與目標估測資訊 (黑:目標物實際位置 紅:目標物估測位置 褐:機器人搬運位置)

5.4 動態目標追蹤

利用5.1 與 5.2 節所介紹的視覺伺服的技術，實現對機器人對動態目標的自 動追蹤，展現此技術的即時性，如圖 5-10 所示，利用第四章所提到融合分散 視覺的機制對動態目標作位置的估測，如圖 5-11，設定當機器人離所估測的目 標位置30 cm 時便停止追蹤。遠端監視畫面藉 802.11 無線傳輸，可監視機器人 偵測目標物的回授資訊，如圖 5-12。機器人自動趨近目標後，從遠端監視畫面 可得到一個更清楚的目標物圖像。 圖 5-10 動態目標追蹤

(a) (b)

(c) (d)

圖 5-11 動態目標位置估測 (a-d)依序為機器人追蹤的過程 (黑:目標物實際位置 紅:目標物估測位置)