國 立 雲 林 科 技 大 學

電 機 工 程 系 碩 士 班

碩 士 論 文

地雷偵測機器人之系統開發與研究

Development and research of a landmine-detection robot

研 究 生 曾耀輝

指導教授吳佳儒 博士

中華民國 九十八 年 六 月

地雷偵測機器人之系統開發與研究

Development and research of a landmine-detection robot

研 究 生 曾耀輝 Yao-Huei Tseng 指導教授吳佳儒 Chia-Ju Wu

國立雲林科技大學

電機工程系碩士班

碩士論文

A Thesis

Submitted to Institute of Electrical Engineering

National Yunlin University of Science amp Technology in Partial Fulfillment of the Requirements

for the Degree of Master of Science

in Electrical Engineering

June 2009 Douliu Yunlin Taiwan Republic of China

中華民國九十八年 6 月

i

地雷偵測機器人之系統開發與研究

學生曾耀輝 指導教授吳佳儒 博士

國立雲林科技大學電機工程系碩士班

摘 要

本論文之目的乃在於設計並製作一部價格低廉能在崎嶇地形運動定位精

確且能以全自動方式或半自動方式工作的履帶驅動式地雷偵測機器人使其能

進行地雷偵測的工作

此履帶機器人以微處理器 dsPIC 30F4011 為核心控制直流無刷馬達驅動器及

金屬探測器使馬達能往復運動並接受金屬探測器之回傳訊號同時能整合以上

訊號並透過無線 RS232 模組回傳到 PC 端而於 PC 端則利用 Borland C++ Builder

所撰寫之控制介面手動或半自動控制履帶機器人之運動狀態並且接收及處理安

裝在機器人上之 GPS 模組回傳字串以解析成可供程式判讀或儲存的經緯度座標

系統

ii

Development and research of a landmine-detection robot

Student Yao-Huei Tseng Advisor Professor Chia-Ju Wu

Institute of Electrical Engineering National Yunlin University of Science amp Technology

ABSTRACT

The main purpose of this thesis is the development and research of a

landmine-detection robot For the robot to be able to work in an area that is possibly

buried with landmines terrain adaptive configuration of the robot is considered and a

tracked vehicle mechanism is used Moreover the robot is designed such that it can be

operated in a semi-automatic manner Meanwhile after the robot detected the buried

landmines how to localize the positions of the landmines accurately is also studied

The designed robot uses a microprocessor dsPIC 30F4011 as the core to control the

brushless DC motor drives and the metal detector simultaneously The control signals

can be transmitted through a wireless RS232 module to the PC client In the PC client

the Borland C++ Builder is used to write the softwares to control the movement of the

robot Meanwhile a GPS module is mounted on the robot to locate the position of robot

which is done by sending back a string to interpret the latitude and the longitude

coordinates of the robot system

iii

誌 謝

時光匆匆歲月如梭轉眼之間將近兩年首先感謝指導教授 吳佳儒教授

兩年來在處事與學業上的諄諄教誨使得學生在研究所生涯中獲益良多其次

感謝口試委員蘇國嵐老師柯嘉南老師賴岦俊老師在口試及論文上的指正與許

多建議研究所就讀期間承所上蔡樹川老師蘇仲鵬老師王偉修老師何前

程老師於課業上的解惑在此呈上最深的敬意

感謝建安學長毓儀學姐有民學長勝文學長國祥學長瑞乾學長以及

建富學長在生活與研究上均給予相當大的幫助並且能適時的鼓勵與經驗分享

讓我受益很多

在論文研究和撰寫期間感謝實驗室的俊雄同學世賢鵬栩永昕學弟

們在生活上及知識上的相互切磋讓我們一起經歷過了實驗的重重考驗另外

也要感謝電機系控制組的同學對我的關心與鼓勵在此兩年之間感謝國科會計畫

(NSC96-2628-E-224-007-MY2) 的支持

最後深深感謝恩師 吳佳儒教授諸多的支持與耐心等待還有我深愛的

家人你們無微不至的關懷與照顧讓我能全心全意致力於論文的研究謝謝你

們謹以此論文與你們分享

iv

目 錄

中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi

英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipii

誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii

目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv

表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipviii

圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipix

第一章 緒論 1

11 研究目的與動機 1

12 文獻回顧 3

13 論文架構 6

第二章 系統架構 7

21 前言 7

22 履帶車機構 8

23 直流無刷伺服馬達與減速機 9

24 履帶馬達驅動器 11

25 直流馬達控制 12

251 可變電阻控制 12

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

地雷偵測機器人之系統開發與研究

Development and research of a landmine-detection robot

研 究 生 曾耀輝 Yao-Huei Tseng 指導教授吳佳儒 Chia-Ju Wu

國立雲林科技大學

電機工程系碩士班

碩士論文

A Thesis

Submitted to Institute of Electrical Engineering

National Yunlin University of Science amp Technology in Partial Fulfillment of the Requirements

for the Degree of Master of Science

in Electrical Engineering

June 2009 Douliu Yunlin Taiwan Republic of China

中華民國九十八年 6 月

i

地雷偵測機器人之系統開發與研究

學生曾耀輝 指導教授吳佳儒 博士

國立雲林科技大學電機工程系碩士班

摘 要

本論文之目的乃在於設計並製作一部價格低廉能在崎嶇地形運動定位精

確且能以全自動方式或半自動方式工作的履帶驅動式地雷偵測機器人使其能

進行地雷偵測的工作

此履帶機器人以微處理器 dsPIC 30F4011 為核心控制直流無刷馬達驅動器及

金屬探測器使馬達能往復運動並接受金屬探測器之回傳訊號同時能整合以上

訊號並透過無線 RS232 模組回傳到 PC 端而於 PC 端則利用 Borland C++ Builder

所撰寫之控制介面手動或半自動控制履帶機器人之運動狀態並且接收及處理安

裝在機器人上之 GPS 模組回傳字串以解析成可供程式判讀或儲存的經緯度座標

系統

ii

Development and research of a landmine-detection robot

Student Yao-Huei Tseng Advisor Professor Chia-Ju Wu

Institute of Electrical Engineering National Yunlin University of Science amp Technology

ABSTRACT

The main purpose of this thesis is the development and research of a

landmine-detection robot For the robot to be able to work in an area that is possibly

buried with landmines terrain adaptive configuration of the robot is considered and a

tracked vehicle mechanism is used Moreover the robot is designed such that it can be

operated in a semi-automatic manner Meanwhile after the robot detected the buried

landmines how to localize the positions of the landmines accurately is also studied

The designed robot uses a microprocessor dsPIC 30F4011 as the core to control the

brushless DC motor drives and the metal detector simultaneously The control signals

can be transmitted through a wireless RS232 module to the PC client In the PC client

the Borland C++ Builder is used to write the softwares to control the movement of the

robot Meanwhile a GPS module is mounted on the robot to locate the position of robot

which is done by sending back a string to interpret the latitude and the longitude

coordinates of the robot system

iii

誌 謝

時光匆匆歲月如梭轉眼之間將近兩年首先感謝指導教授 吳佳儒教授

兩年來在處事與學業上的諄諄教誨使得學生在研究所生涯中獲益良多其次

感謝口試委員蘇國嵐老師柯嘉南老師賴岦俊老師在口試及論文上的指正與許

多建議研究所就讀期間承所上蔡樹川老師蘇仲鵬老師王偉修老師何前

程老師於課業上的解惑在此呈上最深的敬意

感謝建安學長毓儀學姐有民學長勝文學長國祥學長瑞乾學長以及

建富學長在生活與研究上均給予相當大的幫助並且能適時的鼓勵與經驗分享

讓我受益很多

在論文研究和撰寫期間感謝實驗室的俊雄同學世賢鵬栩永昕學弟

們在生活上及知識上的相互切磋讓我們一起經歷過了實驗的重重考驗另外

也要感謝電機系控制組的同學對我的關心與鼓勵在此兩年之間感謝國科會計畫

(NSC96-2628-E-224-007-MY2) 的支持

最後深深感謝恩師 吳佳儒教授諸多的支持與耐心等待還有我深愛的

家人你們無微不至的關懷與照顧讓我能全心全意致力於論文的研究謝謝你

們謹以此論文與你們分享

iv

目 錄

中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi

英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipii

誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii

目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv

表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipviii

圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipix

第一章 緒論 1

11 研究目的與動機 1

12 文獻回顧 3

13 論文架構 6

第二章 系統架構 7

21 前言 7

22 履帶車機構 8

23 直流無刷伺服馬達與減速機 9

24 履帶馬達驅動器 11

25 直流馬達控制 12

251 可變電阻控制 12

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

i

地雷偵測機器人之系統開發與研究

學生曾耀輝 指導教授吳佳儒 博士

國立雲林科技大學電機工程系碩士班

摘 要

本論文之目的乃在於設計並製作一部價格低廉能在崎嶇地形運動定位精

確且能以全自動方式或半自動方式工作的履帶驅動式地雷偵測機器人使其能

進行地雷偵測的工作

此履帶機器人以微處理器 dsPIC 30F4011 為核心控制直流無刷馬達驅動器及

金屬探測器使馬達能往復運動並接受金屬探測器之回傳訊號同時能整合以上

訊號並透過無線 RS232 模組回傳到 PC 端而於 PC 端則利用 Borland C++ Builder

所撰寫之控制介面手動或半自動控制履帶機器人之運動狀態並且接收及處理安

裝在機器人上之 GPS 模組回傳字串以解析成可供程式判讀或儲存的經緯度座標

系統

ii

Development and research of a landmine-detection robot

Student Yao-Huei Tseng Advisor Professor Chia-Ju Wu

Institute of Electrical Engineering National Yunlin University of Science amp Technology

ABSTRACT

The main purpose of this thesis is the development and research of a

landmine-detection robot For the robot to be able to work in an area that is possibly

buried with landmines terrain adaptive configuration of the robot is considered and a

tracked vehicle mechanism is used Moreover the robot is designed such that it can be

operated in a semi-automatic manner Meanwhile after the robot detected the buried

landmines how to localize the positions of the landmines accurately is also studied

The designed robot uses a microprocessor dsPIC 30F4011 as the core to control the

brushless DC motor drives and the metal detector simultaneously The control signals

can be transmitted through a wireless RS232 module to the PC client In the PC client

the Borland C++ Builder is used to write the softwares to control the movement of the

robot Meanwhile a GPS module is mounted on the robot to locate the position of robot

which is done by sending back a string to interpret the latitude and the longitude

coordinates of the robot system

iii

誌 謝

時光匆匆歲月如梭轉眼之間將近兩年首先感謝指導教授 吳佳儒教授

兩年來在處事與學業上的諄諄教誨使得學生在研究所生涯中獲益良多其次

感謝口試委員蘇國嵐老師柯嘉南老師賴岦俊老師在口試及論文上的指正與許

多建議研究所就讀期間承所上蔡樹川老師蘇仲鵬老師王偉修老師何前

程老師於課業上的解惑在此呈上最深的敬意

感謝建安學長毓儀學姐有民學長勝文學長國祥學長瑞乾學長以及

建富學長在生活與研究上均給予相當大的幫助並且能適時的鼓勵與經驗分享

讓我受益很多

在論文研究和撰寫期間感謝實驗室的俊雄同學世賢鵬栩永昕學弟

們在生活上及知識上的相互切磋讓我們一起經歷過了實驗的重重考驗另外

也要感謝電機系控制組的同學對我的關心與鼓勵在此兩年之間感謝國科會計畫

(NSC96-2628-E-224-007-MY2) 的支持

最後深深感謝恩師 吳佳儒教授諸多的支持與耐心等待還有我深愛的

家人你們無微不至的關懷與照顧讓我能全心全意致力於論文的研究謝謝你

們謹以此論文與你們分享

iv

目 錄

中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi

英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipii

誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii

目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv

表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipviii

圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipix

第一章 緒論 1

11 研究目的與動機 1

12 文獻回顧 3

13 論文架構 6

第二章 系統架構 7

21 前言 7

22 履帶車機構 8

23 直流無刷伺服馬達與減速機 9

24 履帶馬達驅動器 11

25 直流馬達控制 12

251 可變電阻控制 12

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

ii

Development and research of a landmine-detection robot

Student Yao-Huei Tseng Advisor Professor Chia-Ju Wu

Institute of Electrical Engineering National Yunlin University of Science amp Technology

ABSTRACT

The main purpose of this thesis is the development and research of a

landmine-detection robot For the robot to be able to work in an area that is possibly

buried with landmines terrain adaptive configuration of the robot is considered and a

tracked vehicle mechanism is used Moreover the robot is designed such that it can be

operated in a semi-automatic manner Meanwhile after the robot detected the buried

landmines how to localize the positions of the landmines accurately is also studied

The designed robot uses a microprocessor dsPIC 30F4011 as the core to control the

brushless DC motor drives and the metal detector simultaneously The control signals

can be transmitted through a wireless RS232 module to the PC client In the PC client

the Borland C++ Builder is used to write the softwares to control the movement of the

robot Meanwhile a GPS module is mounted on the robot to locate the position of robot

which is done by sending back a string to interpret the latitude and the longitude

coordinates of the robot system

iii

誌 謝

時光匆匆歲月如梭轉眼之間將近兩年首先感謝指導教授 吳佳儒教授

兩年來在處事與學業上的諄諄教誨使得學生在研究所生涯中獲益良多其次

感謝口試委員蘇國嵐老師柯嘉南老師賴岦俊老師在口試及論文上的指正與許

多建議研究所就讀期間承所上蔡樹川老師蘇仲鵬老師王偉修老師何前

程老師於課業上的解惑在此呈上最深的敬意

感謝建安學長毓儀學姐有民學長勝文學長國祥學長瑞乾學長以及

建富學長在生活與研究上均給予相當大的幫助並且能適時的鼓勵與經驗分享

讓我受益很多

在論文研究和撰寫期間感謝實驗室的俊雄同學世賢鵬栩永昕學弟

們在生活上及知識上的相互切磋讓我們一起經歷過了實驗的重重考驗另外

也要感謝電機系控制組的同學對我的關心與鼓勵在此兩年之間感謝國科會計畫

(NSC96-2628-E-224-007-MY2) 的支持

最後深深感謝恩師 吳佳儒教授諸多的支持與耐心等待還有我深愛的

家人你們無微不至的關懷與照顧讓我能全心全意致力於論文的研究謝謝你

們謹以此論文與你們分享

iv

目 錄

中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi

英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipii

誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii

目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv

表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipviii

圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipix

第一章 緒論 1

11 研究目的與動機 1

12 文獻回顧 3

13 論文架構 6

第二章 系統架構 7

21 前言 7

22 履帶車機構 8

23 直流無刷伺服馬達與減速機 9

24 履帶馬達驅動器 11

25 直流馬達控制 12

251 可變電阻控制 12

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

iii

誌 謝

時光匆匆歲月如梭轉眼之間將近兩年首先感謝指導教授 吳佳儒教授

兩年來在處事與學業上的諄諄教誨使得學生在研究所生涯中獲益良多其次

感謝口試委員蘇國嵐老師柯嘉南老師賴岦俊老師在口試及論文上的指正與許

多建議研究所就讀期間承所上蔡樹川老師蘇仲鵬老師王偉修老師何前

程老師於課業上的解惑在此呈上最深的敬意

感謝建安學長毓儀學姐有民學長勝文學長國祥學長瑞乾學長以及

建富學長在生活與研究上均給予相當大的幫助並且能適時的鼓勵與經驗分享

讓我受益很多

在論文研究和撰寫期間感謝實驗室的俊雄同學世賢鵬栩永昕學弟

們在生活上及知識上的相互切磋讓我們一起經歷過了實驗的重重考驗另外

也要感謝電機系控制組的同學對我的關心與鼓勵在此兩年之間感謝國科會計畫

(NSC96-2628-E-224-007-MY2) 的支持

最後深深感謝恩師 吳佳儒教授諸多的支持與耐心等待還有我深愛的

家人你們無微不至的關懷與照顧讓我能全心全意致力於論文的研究謝謝你

們謹以此論文與你們分享

iv

目 錄

中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi

英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipii

誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii

目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv

表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipviii

圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipix

第一章 緒論 1

11 研究目的與動機 1

12 文獻回顧 3

13 論文架構 6

第二章 系統架構 7

21 前言 7

22 履帶車機構 8

23 直流無刷伺服馬達與減速機 9

24 履帶馬達驅動器 11

25 直流馬達控制 12

251 可變電阻控制 12

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

iv

目 錄

中文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipi

英文摘要helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipii

誌謝helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiii

目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipiv

表目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipviii

圖目錄helliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphelliphellipix

第一章 緒論 1

11 研究目的與動機 1

12 文獻回顧 3

13 論文架構 6

第二章 系統架構 7

21 前言 7

22 履帶車機構 8

23 直流無刷伺服馬達與減速機 9

24 履帶馬達驅動器 11

25 直流馬達控制 12

251 可變電阻控制 12

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

v

252 電壓控制 13

253 脈波寬度調變 14

26 金屬探測器轉盤總成 16

261 轉盤驅動 16

262 探測器支撐架 17

263 轉盤馬達驅動 IC 18

27 金屬探測器 20

271 原理 20

272 諸元介紹 20

28 無線傳輸模組 22

29 GPS 定位系統 23

291 衛星部份 23

292 地面監控部份 24

293 接收儀 24

294 NMEA 格式 24

295 GPS位置接收器 26

第三章 控制電路與程式設計 28

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

vi

31 微處理器 28

311 數位輸出入埠 31

312 10位元類比數位轉換器(ADC) 32

313 計時器 34

314 通用非同步傳輸介面 35

315 馬達控制脈波寬度調變(PWM)模組 37

32 控制電路設計 39

321 主控制電路 39

322 轉盤驅動電路 40

323 控制程式流程圖 42

第四章 實驗結果 43

41 系統整合 43

411 硬體整合 43

412 軟體介面整合 45

42 系統測試 49

421 金屬探測器轉盤測試 49

422 室內測試 50

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

vii

423 室外測試 52

第五章 總結與未來展望 54

參考文獻 56

自傳 61

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

viii

表 目 錄

表 11 部分國家或地區未被清除的地雷數目 2

表 21 馬達規格表 10

表 22 CSBL900 伺服驅動器詳細規格 12

表 23 TA8429H 輸出入真值表 19

表 24 NEMA-0183 內碼定義表 25

表 25 GPRMC 格式內容 26

表 31 主要周邊功能 29

表 32 本論文所使用的輸出入埠簡介 32

表 33 類比訊號轉換模組設定所需的 6 個 16 位元長度暫存器 33

表 34 本論文用到的 UART 函式與用途 36

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

ix

圖 目 錄

圖 11 世界地雷分布圖[1] 2

圖 12 [24]中的第一種地雷偵測機器人 3

圖 13 [25]中的第二種地雷偵測機器人 3

圖 14 [24]中的地雷偵測機器人 4

圖 15 [26]中的地雷偵測機器人 4

圖 16 具有六個步足的掃雷機器人[10] 4

圖 21 履帶型載具設計與履帶型載具外觀 7

圖 22 系統架構圖 8

圖 23 履帶車側視圖 9

圖 24 底盤機構分佈 9

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機 10

圖 26 馬達與驅動器接線圖 11

圖 27 CSBL900 伺服驅動器 11

圖 28 可變電阻與驅動器接線圖 13

圖 29 類比弦波與方波的脈波寬度關係圖 14

圖 210 微處理器與馬達驅動器接線圖 15

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

x

圖 211 輸入脈波示意圖 15

圖 212 掃雷機器人探測區域範圍圖 16

圖 213 探測器支撐架 17

圖 214 全橋電路 18

圖 215 TA8429H 全橋式驅動 IC 外觀圖 18

圖 216 M97 金屬探測器外觀 21

圖 217 無線傳輸模組 SST-2400EXT 外觀圖 22

圖 218 全球定位系統 23

圖 219 GPS 接受器 GPS 12XL 外觀圖 27

圖 31 哈佛式架構中各自獨立的程式與資料匯流排 28

圖 32 DSPIC30F4011 硬體架構方塊圖 30

圖 33 類比數位轉換器硬體方塊圖 33

圖 34 UART 通訊模組的硬體架構圖 35

圖 35 PWM 模組硬體架構方塊圖 38

圖 36 主控制電路完成圖 39

圖 37 主控制電路元件分布圖與長寬標示 40

圖 38 轉盤驅動電路完成圖 41

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

xi

圖 39 轉盤驅動電路元件分布圖與長寬標示 41

圖 310 控制電路主程式流程圖 42

圖 41 探測地雷機器人第一層各部元件 43

圖 42 探測地雷機器人第二層各部元件 44

圖 43 BCB 程式介面 45

圖 44 BCB 程式介面介紹 46

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能 47

圖 46 GPS 介面介紹-GPS 座標值 47

圖 47 將經緯度座標存在純文字檔中 48

圖 48 GPS 介面介紹-GPS 字串值 48

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右 49

圖 49-2 影片第 4 秒探測器在最右側 49

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退 50

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者 50

圖 410-3 前進控制 51

圖 410-4 左轉控制 51

圖 410-5 右轉控制 51

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

xii

圖 411-1 開始點 52

圖 411-2 前進 52

圖 411-3 左回轉 53

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈 53

圖 51 脈波命令式伺服系統(半閉迴路) [43] 54

圖 52 電壓命令式伺服系統(全閉迴路) [43] 55

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

1

第一章 緒論

11 研究目的與動機

過去的幾十年中在中南半島波斯灣巴爾幹半島非洲等地都有各種戰

爭的發生當戰爭發生時交戰雙方為了造成對方的傷亡經常佈有各種地雷

而由於這些地雷在戰爭結束後並沒有立即被清除因此常有平民百姓因不慎觸雷

而傷亡的情況發生而除了人員的重大傷亡外也會對當地的經濟造成非常大的

影響圖 11 所示為目前世界地雷的分布圖[1]另外根據一些網路資料統計表

11 所示為目前的一些國家目前尚未被清除的地雷數目

由於地雷在世界各地所造成的重大影響已有很多官方或非官方的組織積極

加入掃除地雷的行動但是埋設一枚地雷只需 3 塊美元清除卻需花 100 至 1000

塊美元而埋設一枚地雷只需 5 分鐘但清除卻需 2 至 3 個小時加上要掃除的

地雷數目實在太多因此乃有各種掃雷機器人的出現希望機器人能以全自動或

和操作人員配合的半自動方式以較高的效率和較安全的方式來從事這項危險的

工作不過一般來說這些掃雷機器人由於要把偵測地雷和清除地雷這兩項工作

同時完成因此體積都相當龐大造價也十分高昂實在非一般人員或機構所能

負擔也因此並沒有被廣泛地使用有鑑於此本論文的主要目的是想設計並製

作一部價格低廉能在崎嶇地形運動定位精確且能以全自動方式或半自動方

式工作的地雷偵測機器人使其能進行地雷偵測的工作雖然這種機器人並不具

有清除地雷的能力但只要地雷能被精確地定位出來一般的平民百姓即可免除

誤觸地雷的危險而在經費或能力許可的情況下日後再對這些地雷進行清除

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

2

圖 11 世界地雷分布圖[1]

表11部分國家或地區未被清除的地雷數目

國家地區 未被清除地雷數目 國家地區 未被清除地雷數目

安哥拉 一千萬至一千五百萬 科威特 五百萬至一千萬

阿富汗 九百萬至一千萬 拉丁美洲 三百萬至一千萬

埃及 兩千兩百萬 莫三比克 兩百萬

柬埔寨 八百萬至一千萬 前南斯拉夫 六百萬

斯里蘭卡 三百五十萬 索馬尼亞 一百萬

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

3

12 文獻回顧

有關掃雷或地雷偵測機器人的研究在國外有許多專家學者對於掃雷機器人

已經有相當多且深入的研究[2-26]至於在外觀方面其中一些掃雷機器人的外觀

被設計成具有兩個輪子[2-3]而有些則具有四個輪子[422]或四隻腳[5-925]有些

具有六個輪子[1026]或六隻腳[1123]甚至有些具有八隻腳[1213]下頁圖 12 至

圖 16 則為部分機器人的外觀圖

圖 12 [24]中的第一種地雷偵測機器人

圖 13 [25]中的第二種地雷偵測機器人

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

4

圖 14 [24]中的地雷偵測機器人

圖 15 [26]中的地雷偵測機器人

圖 16 具有六個步足的掃雷機器人[10]

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

5

在以上文獻中有作者設計一個具有金屬探測器及紅外線感測器的掃雷機器

人可用於偵測地雷並可於行進間避免車體碰撞到其他障礙物[23]在[4]中

作者設計一個具有超音波感測器紅外線感測器和接近式感測器的多功能微型機

器人在[5-9]中作者設計掃雷機器人TITAN-VIII和TITAN-XI此機器人共

具有 4 個步足可在移動過程中使用金屬探測器來檢查地面是否有埋藏地雷並

可利用機械手臂前端所裝設的機械夾將地雷摘除在[13]中作者使用金屬探測器

紅外線加上雷達製造出八腳的掃雷機器人具有低成本的特性在[14]中作者

討論如何使機器人在未知而複雜的地形中正常的行進不因地形的變化而受到

限制

在[1519]中作者利用在雷區所建立的地雷分佈圖為機器人設計一條最佳路

徑以提高掃雷的效率另外也有專家學者[1618]討論更有效的除雷方式其方式

是以多部機器人分別搭載不同類型的感測器透過相互的輔助達到更好的掃雷效

果在[17]中作者討論四腳掃雷機器人在除雷過程中失去一隻腳的情況下如

何能以剩餘的三隻腳安全的離開地雷區

在[20]中作者設計具有攝影機與掃雷功能的機械手臂讓操作人員可以在遠

距離端進行掃雷的工作在[21]中作者設計具有紅外線感測器的群組式機器人

利用機器人做地毯式的搜索可節省大量時間

在[22]中作者設計一具特殊機構的四輪機器人使機器人可做大範圍搜索

並有一 3 自由度的手臂可供移除目標物在[23]中作者製造一六足掃雷機器人

COMET-Ⅲ其架構設計使機器人易於在一些不平順的地形變化較大的未知環境

中操作在[24]中作者將所謂的rdquoArea Reductionrdquo的觀念實現在輪型足型航空

掃雷機器人上使機器人在分析未知地形與避障上更為便利

在[25]中作者製造一四足的機器人該機器人上有多種不同的感測器可同時

來偵測地雷以降低單一感測器的偵測錯誤率在[26]中 作者設計一具有金屬探

測器的六輪機器人並使用攝影機分析地表環境是否有異與周遭地表環境以增

加地雷偵測準確率

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

6

13 論文架構

本論文研究架構分為履帶載具機構研製探測器轉盤總成金屬探測器訊

號擷取伺服馬達驅動GPS 接收儀經緯度座標回傳微處理器控制等以求發

展出一套價格低廉操作簡便之遠距探測機器人各章節內容描述如下

第一章 說明研究目的與動機並探討外國文獻中掃雷機器人發展概況

第二章 履帶載具的系統規劃及簡述各部元件諸元與 GPS 系統

第三章 微處理器架構與電路板設計並探討 PC 端程式架構

第四章 實驗結果

第五章 本論文的總結與建議

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

7

第二章 系統架構

21 前言

本作品設計圖如下圖 21 所示其主要功能為能靈活應對各種地面譬如光滑的

地磚甚至到泥濘的花園因此在載具設計上是採用履帶驅動整個平台

圖 21 履帶型載具設計與履帶型載具外觀

履帶驅動的優點為有良好的爬坡性能和不錯的越野能力但效率比較低履

帶系統的受力面積比輪形系統更大履帶系統所受的摩擦力均勻分佈在履帶上

而輪形系統的摩擦力只是集中在輪胎與地面的接觸面上就抓地力而言它們是

一樣的但在轉彎或者爬坡時履帶系統所受的摩擦力分佈不會像輪形系統那樣

發生劇變所以就表現出更好的操控性履帶系統也是屬於兩輪差速驅動系統的

延伸應用履帶與地面的接觸點會產生側滑(skid steering)現象因此在作航行位移

計算(dead reckoning)時會產生較大的誤差本作品由於考量到在泥土地上之運動

需要扭力較大的馬達因此在選用直流無刷馬達時另外搭配了減速機以求更大

的扭力輸出以應付惡劣之地形地貌利用其一定的體積下提供較大的扭力輸出的

特性來解決機器人在惡劣地形移動時馬達扭力不足的問題本作品為了能達成

準確定位也選用了伺服級之馬達驅動器在室外應用時期能搭配安裝在載具

上之全球衛星導航系統接收器(GPS)

下圖22為設計構想系統架構圖有分為硬體及軟體部分首先談到硬體方面

主要有以微處理器為主的核心的控制電路伺服級馬達驅動器轉盤馬達控制電

路無線傳輸模組以及 GPS 位置接收器等硬體架構則軟體部分是利用 Borland C++ Builder(BCB)所撰寫的監控介面

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

8

無線模組(載具端) RS-232

無線模組(載具端) RS-232

PC(BCB)

控制訊號

PWM 履帶馬達伺服驅動器

轉盤馬達控制電路

履帶馬達

轉盤馬達

RS-232 無線模組(GPS端)

無線電控制訊號

載具MCU『dsPIC

30F4011』

GPS位置接收器

RS-232 無線模組(GPS端)

無線電位置訊號

圖 22 系統架構圖

如上圖 22 所示當使用者在個人電腦下的監控介面 (BCB)下達控制指令後

其訊號透過 RS-232 無線傳輸模組傳輸到 dsPIC 微處理器上再由 dsPIC 內部的程

式來做處理之後把控制訊號傳給馬達驅動器以及轉盤馬達控制電路於是就能控

制馬達正反轉與轉盤馬達等動作在載具動作的過程中利用加裝在載具身上的

GPS 接收器能即時的把載具目前的位置回報給 PC 端再由程式擷取及處理

NMEA 字串轉化為能加以處理應用的經緯度及速度資料再把所接收到的資料

存為資料庫以備日後所用進而達到位置回報的用途

載具系統主要分為履帶車機構直流無刷伺服馬達與減速機履帶馬達驅動

器金屬探測器轉盤機構與金屬探測器此為掃雷機器人之主要硬體受控部分

接下來將再個小節針對各部份元件一一介紹

22 履帶車機構

基於前面章節所述在控制性操控性以及室外環境的應用權衡之下選擇

了履帶式底盤做為車身車身結構如下圖 23 以及下頁圖 24 所示後部為馬達及

其減速機馬達的動力通過傳動軸傳遞到後面的減速機再傳到兩側的主動輪上

撥動履帶推動車輛前進主動輪在後誘導輪在前每側有一個負重輪配合

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

9

減速機提供的高扭力以求克服各種型態的地形以及路面

圖 23 履帶車側視圖

圖 24 底盤機構分佈

23 直流無刷伺服馬達與減速機

本 作 品 上 的 馬 達 採 用 台 灣 東 元 公 司 (TECO) 的 直 流 無 刷 伺 服 馬 達

DBT56-80C1AE外觀如下圖 25 所示下表 21 為此馬達的規格圖 26 為馬達

與驅動器接線圖

減速機 履帶馬達

轉盤馬達

主動輪 負重輪誘導輪

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

10

圖 25 直流無刷伺服馬達 DBT56-80C1AE 與減速機

表21 馬達規格表

符 號 單 位 DBT56-80C1AE

電 壓 V Volts 24

無負載轉速 SNL RPM 3700

連續運轉扭力 TC kg-cm 285

連續運轉速度 SC RPM 2740

連續運轉電流 IC Amp 55

連續運轉馬力 Pout W 80

峰值運轉扭力 Tp kg-cm 108

峰值運轉電流 Ip Amp 185

持續電壓 KE Vradsec 006

繞線阻抗 R Ω 08

轉子慣量 JM g-cm2 330

重量 W Kg 11

周圍溫度 T -10～+60 Co

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

11

圖 26 馬達與驅動器接線圖

24 履帶馬達驅動器

本作品所用之馬達驅動器為擎翔實業有限公司(CSIM)之產品其外觀如 下圖 27 所示而詳細規格如下表 22 所示[27]

圖 27 CSBL900 伺服驅動器

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

12

表22 CSBL900 伺服驅動器詳細規格

25 直流馬達控制

馬達驅動器 CSBL900 支援各種控制方式而各種方法皆有不同特點以下各

小節將針對本論文使用之驅動器與直流無刷馬達討論並介紹數種控制方式[28]

251 可 變 電 阻 控 制

如圖 28 所示將一 10KΩ可變電阻與接頭 CN1 中之接腳 4125 連接

即可隨著調整電阻值控制電流來達成轉速之控制這種方式雖然簡單卻有需

要手動調整可變電阻之問題無法跟上時代潮流透過 PC 或微處理器達成自走或

半自走之目的並且利用可變電阻控制電流也有增加功率消耗的疑慮故而作

廢不用

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

13

圖 28 可變電阻與驅動器接線圖

252 電 壓 控 制

由於電阻控制會有不利控制以及功率消耗等問題因此改由嘗試改變馬達端

電壓來控制轉速控制電壓可使用市售的類比數位轉換器(ADC)電路透過與 PC端連接下達指令改變端電壓達成可程式化控制的目的接線圖如下圖 29 所示

另外由於市售的 ADC 輸出大部分被限制在直流 10V 以下也就是說馬達的控制

電壓被限制住了如此不利於需要高扭力之履帶車系統故須另尋其他更加適當

的控制方式

圖 29 類比數位轉換器與驅動器接線圖

ADC

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

14

253 脈 波 寬 度 調 變

脈波寬度調變（Pulse Width ModulationPWM）其應用方式是以將類比信

號脈波調變成一個週期固定脈波寬度(工作週期 Duty Cycle)變化不同的新方波

而且新的方波是隨原始的類比信號波形大小而呈比例變化方式來傳送如下圖 29所示之類比弦波與方波的脈波寬度關係圖這是一般在無線通訊穩壓影音

放大器等應用上來提高效能及抑制雜訊的調變方式

圖 29 類比弦波與方波的脈波寬度關係圖

而在馬達控制上可謂反其道而行透過調整方波的 Duty Cycle進而改變電晶

體的 ONOFF 切換時間間隔從而使供給馬達的電流發生變化達成速度快慢之

控制

為了達成使用微處理器驅動馬達的目的須由微處理器 IO 接腳提供合適的脈

波輸出以求達成速度以及正反轉的控制透過馬達驅動器之接頭 CN1(如圖 27所示)CN1 與微處理器之間的接線關係圖如下圖 210 所示由微處理器控制機板

提供+5V 給接腳 PLS+與 DIR+並輸出 PWM 訊號給接腳 PLS-以控制速度另外

接腳 DIR-與微處理器數位輸出埠連接以利用程式來規劃與控制馬達之正反轉

如此達成馬達速度與旋轉方向之控制輸入脈波示意圖如下圖 211 所示

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

15

圖 210 微處理器與馬達驅動器接線圖

圖 211 輸入脈波示意圖

Pulse

Dir

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

16

26 金屬探測器轉盤總成

金屬探測器轉盤機構目的在於支撐並固定金屬探測器並擴大掃雷機器人車

前的探測範圍使其偵搜範圍為車前約 50 度角的扇型範圍整體結構主要由轉盤

與支撐架構成

圖 212 地雷探測機器人探測區域範圍圖

261 轉 盤 驅 動

轉盤的運動與利用極限開關限制的車前探測範圍如上圖 212 所示轉盤控制

動作為一自動往復機構當轉盤機構觸碰到兩側的極限開關時微處理器數位輸

入埠接收到開關狀態變化由微處理器執行邏輯判斷決定正轉或反轉並透過數

位輸出埠連接至馬達驅動 IC下達正反轉指令使其達成轉盤往復之運動進而

擴大金屬探測器在機器人前方之探測範圍

距離車前 45cm

22cm

探測範圍 約 50 度角

極限開關

極限開關

轉盤馬達

傳動軸

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

17

262 探 測 器 支 撐 架

為求穩定並支撐金屬探測器之位置需要另行使用支撐架加以固定探測器

以避免探測器受到外在因素干擾造成誤判此外為了適應不同路面或環境的可

能因素需將支撐架設計成能夠加以手動調整高低與角度成品如下圖 213 所示

圖 213 探測器支撐架

區塊 A連接轉盤與支撐架並加以固定以求穩定探測器

區塊 B連接探測器與支撐架並可使用共八顆螺絲調整探測器高度與角度

區塊 A 區塊 A

區塊 B

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

26

以推

達

速切

要求

全橋

3 轉 盤 馬

馬達驅動電

推動直流馬達

除了高電流

切換效果此

透過控制全

求而為求易

橋式驅動 IC

馬 達 驅 動

電路單純

達此時就

流輸出外

此時全橋電

全橋電路中

易於開發與使

如下圖 21

圖 2

動 IC

純靠微處理器

就有必要透過

還要能夠方

電路即派上用

圖 2

中的電晶體切

使用簡便本

15 所示[29]

215 TA842

18

器數位輸出

過馬達驅動

方便執行馬

用場如下

214 全橋電

切換即可

本論文選用

]

9H 全橋式驅

出埠所提供

動 IC 提供之

馬達正反轉

下圖 214 所示

電路

可達成轉盤

用了 TOSHI

驅動 IC 外觀

M

供之低電流輸

之高電流輸出

轉狀態高電位

示

盤馬達正轉或

IBA 公司出

觀圖

輸出變化並

出來驅動直

位與低電位

或是反轉的

出品的 TA84

並不足

直流馬

位的快

的動作

429H

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

驅動

都給

電位

透過技術文

動 IC 的驅動

給低電位會

位即可使馬達

文件所附之

動訊號IN1

會使得 IC 停

達正反轉

之輸出入真值

和 IN2 都給

停止送電並

達成轉盤控

表23 TA8

19

值表 23可

給高電位

並達成慣性

控制目的

8429H 輸出

可以方便使

會控制馬達

性煞車的目

出入真值表

使用者規劃微

達強制煞車

目的IN1I

微處理器傳

車鎖死IN1

IN2 分別給

傳送給

IN2

給不同

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

20

27 金屬探測器

271 原 理

一段通過電流的導線會在其周圍產生磁場由法拉第定理瞭解到若通過一

線圈內之磁通隨著時間產生變化時則該線圈將產生一感應電勢其大小與線圈

之匝數及磁通之變化率成正比若把線圈的兩端短路或經一阻抗連接則線圈中

感應電壓所引起的電流(Induce Current)依據楞次定律將產生一磁場反抗外加磁場

的變化如果時變的磁場垂直通過一平面導體可視同很多同心圓所組成的平面

導體與時變的磁場磁交鏈同心圓導體感應出形同漩渦式的電流所以稱此電流

為渦電流(Eddy Current)此渦電流會與線圈產生互感的作用當待測物與線圈之

距離變化時互感的程度也會跟著改變而造成線圈電感值的改變換句話說

即為金屬與線圈距離變化時產生電流變化並利用外加一電流計來判讀電流變

化大小給使用者透過電流的大小變化來判讀金屬物體的大小遠近此為近代

金屬探測器之原理[30]

272 諸 元 介 紹

本論文選用的金屬探測器為 Fisher 公司出品的 M97 探測器目的專為尋找掩

埋或鋪設的閥門或人孔蓋或任何其他隱藏的金屬物體它也能對含有鋁銅和

鉛的目標產生反應以下為諸元介紹[31]

操作頻率45KHz 靈敏度02mv RMS靈敏調整 121 輸出顯示(1)指針式電表1mA0~100 刻度

(2)內藏式擴音器16 歐姆阻抗 電源9Vtimes2 只

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

21

電池使用時間25~35 小時 電源消耗量138mA 探測線圈直徑 8 英吋(2032 公分) 儀器重量15Kg(8 英吋) 探測深度約 40 吋(102 公分) 儀器長度38~50 吋(96~127 公分)可調整

圖 216 M97 金屬探測器外觀

透過 M97 資料文件以及上圖 216 之外觀可以發現 M97 可由指針式電流計

顯示金屬物體之遠近與大小因此本論文選擇由電流計拉出兩條線路並把此

線路連接至微處理器 dsPIC 的數位輸入接腳利用數位類比轉換器將電流計的

刻度顯示轉換成數位訊號以供 PC 端判讀並利用

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

22

28 無線傳輸模組

個人電腦與機器人上所使用的無線傳輸模組皆採用傑程科技公司所生產的

SST-2400EXT 無線電通訊數據機它可提供單點對單點(point-to-point)單點對多

點(point-to-multiple)多點對多點(multiple-to-multiple)的無線傳輸SST-2400EXT

採用直接序向展頻 (Direct Sequence Spread Spectrum DSSS)及 RF 技術在

2400~24835MHz ISM頻率下操作不需特定的通訊格式具高保密性及高穩定性

SST-2400EXT 外觀如圖 217 所示其內部包含一顆 CPU一顆展頻晶片及射頻模

組(RF module)可藉由內部電路板上的跳線設定全雙工半雙工同步非同步

及使用頻道等[3233]

圖 217 無線傳輸模組 SST-2400EXT 外觀圖

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

23

29 GPS 定位系統

全 球 定 位 系 統 （ NAVSTARGPS NAVigation Satellite Timing And RangingGlobal Positioning System簡稱 GPS）原是美國國防部為了軍事定時定

位與導航的目的所發展希望以衛星導航為基礎的技術可構成主要的無線電導航系

統[34]全球定位系統架構可分為三個部份太空部份控制部份及使用者部份

如下圖 218 所示

衛星24+3 衛星

週期11時58分高度20200公里

監控系統時間同步

預估衛星軌道數據傳送

衛星狀況監測

接收儀接收虛擬距離與相位信號

接收衛星座標定位計算

GPS接收儀

圖 218 全球定位系統

291 衛 星 部 份

GPS 系統之太空部份針對運行的衛星本體而言目前係由 27 顆衛星組成其

中 24 顆為操作衛星3 顆為備用衛星三個備用衛星的功能主要在於作為當主衛

星失效時之備用及加強衛星之幾何分佈在平時這些備用衛星也可用於定位

故為主動預備(active spare)方式而每顆衛星上面都有一個頻率穩定的原子鐘產

生1023MHz的穩定基頻用以組成CA碼(頻率1023MHz)及P碼((頻率1023MHz)並調制在L1載波(頻率1541023MHz波長為19cm)及L2載波(頻率1201023MHz波長為 24cm)上L1 及 L2 皆調制為 50BPS(Bit Per Second)的衛星訊息兩者組成

為無線電雙頻訊號持續向地面發射

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

24

292 地 面 監 控 部 份

GPS 之地面系統是於 1985 年 9 月完成整個系統包括一個主控站3 個地面

天線及 5 個監理站每個監裡站均擁有數個 GPS 雙頻接收器標準原子鐘感應

器及資料處理機每個監測站每天 24 小時不停地連續追蹤觀測每一個衛星並

將每 15 秒之虛擬距離觀測量觀測所得氣象資料及電離層資料聯合起來求解得

到每 15 分鐘一組之均勻化數據然後將數據在送至主控站

對於 GPS 導航定位而言GPS 衛星是一動態已知點它是依據衛星傳送的星

曆計算而得所謂衛星星曆是一系列描述衛星運動及其軌道的參數每顆 GPS 衛

星所傳送的星曆皆由 GPS 的地面監控系統提供GPS 衛星進入軌道運行之後

衛星的健康狀況即衛星上的各種設備是否正常運作以及衛星是否依據預定軌

道運行皆都需要由地面設備進行監測和控制此外地面監控系統還有一個重

要工作保持各顆衛星處於同一時間標準即 GPS 時間系統因此地面監控系統監

測各顆衛星的時間且計算得它們的有關改正數進而由導航訊息傳送給用戶

以確保處於 GPS 系統正常

293 接 收 儀

接收儀部份所指的是能夠接收 GPS 衛星訊號及資料處理之接受儀由於 GPS的用途相當廣泛使用者部份可依目的之不同而有不同功能精度的接收儀及應

用對象而有所不同的特性如依用途性質而言GPS 信號分為民用的標準定位服

務 (SPS Standard Positioning Service)和軍規的精確定位服務 (PPS Precise Positioning Service)兩類由於 SPS 無須任何授權即可任意使用故在民用訊號中人

為地加入誤差 以降低其精確度使其最終定位精確度大概在 100 公尺左右軍規

的精度在 10 公尺以下2000 年以後美國政府決定取消對民用訊號的干擾因此

現在民用 GPS 也可以達到 10 公尺左右的定位精度[35]

294 NMEA 格 式

GPS 接收儀都具備有美國國家海洋電子學會 (National Marine Electronic Association簡稱 NMEA)所制定的標準規格其訂定了所有航海電子儀器間的通

訊標準包含了傳輸資料的格式以及傳輸資料的通訊協定[36]NMEA規格有 01800182及 0183 等三種 NEMA-0183 是在 0180 和 0182 格式上增加了 GPS 接收儀輸

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

25

出的內容而完成的在電子傳輸的實體界面格式簡言之 NEMA-0183 包含了

NEMA-0180 及 NEMA-0182 所定的 RS-232 界面格式

NMEA-0813 協定的格式是以傳輸傳輸(句子)的方式每個句子以($)字元作為

開頭第二第三做為傳輸格式的識別碼第四五六字元為傳輸句子的名稱

不同的句子名稱其後面皆代表不同訊息內容內容以逗號做為分隔ltCRgtltLFgt作為句子結尾

本論文以 GPS 接收儀提供的 NMEA-0183 協定其句子的格式為 GPRMC表

24 為 GPRMC 的格式內容及代表意義

表24 NEMA-0183 內碼定義表

GGA GPS 位置時間方位資訊

GLL 獲得位置經緯度(LatitudeLongitude)以及時間資訊

GSA GPS接收的模式以及各衛星的頻道資訊以及DOP值

GSV GPS 接收器有接收到訊號衛星 ID 列表PRN

RMC 建議最小特定的 GNSS 資料

VTG 追蹤行進軌跡的資訊以及行進速度的記錄

本論文選擇取得 GPRMC 碼做為做經緯度解碼經由程式解出其值做為機器

人位置判讀而實際所得的經緯度所代表為何呢如 llllllll 代表為緯度之中的度

分萬分之一以 2341672N 為例解得實際為代表北緯 23 度 41672 分若想

要換算成實際單位需要乘上 18 公里式子如下[34]

)(60)(1 分度 =deg

公里1081 =deg 公里811 =

而 公尺811000 =

所以 2341672N 中的小數點第三位代表每多 1距離加 18 公尺而一般 GPS接收儀的由於受到 AS 效應影響都有的十五公尺左右的誤差

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

26

表25 GPRMC格式內容 RMC 接收時間狀態經緯度方位速度接收日期

$GPRMChhmmssssAllllllllayyyyyyyyyBxxxxxxxxxxxxahhltCRgtltLFgt

DDDDHHHHHHHMGGGGGMxxJJJJ訊 息 名 稱 單 位 說 明

$GPRMC 封包標題

hhmmss 時分秒 UTC 格林威治標準時間

A(V) AGPS 接收儀正常V接收儀暖機中

llllllll 位置緯度（Latitude）

a 北緯或南緯(N or S)

yyyyyyyyy 位置經度（Longitude）

B 東或西經（E or W）

xx 節(海哩) 速度

xx 行進方向

xxxxxx 日期日月西元

xx 磁場

hh Checksum

【CR】【LF】 封包結束

295 GPS 位 置 接 收 器

安裝於載具上之 GPS 接受器選用 GARMIN 科技開發之戶外活動專用 GPS

12XL其外觀如下圖 219 所示選用此戶外接受器主要著眼點在於以下幾點

串列通訊內建 RS232 通訊埠方便資料傳送到 PC 端的 BCB 程式主控端

以利後續座標資料解析與分類儲存

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

防水能力

螢幕顯示

根據 GAR

級(水下

使用之防

誤動作或

透過螢幕

標準來說

螢幕即時

煩而能

圖

RMIN 的資

1 公尺30

防潑水等級已

或不動作的情

幕顯示可以

說至少需聯絡

時顯示衛星數

能加快找出問

219 GPS 接

27

資料該接收

0 分鐘內防

已綽綽有餘

情況

以即時知道目

絡到 3 顆衛

數量可以

問題的癥結

接受器 GPS

收器具備可

防水)而如

餘以利開

目前所能通

衛星才能收

以避免開發

結

S 12XL 外觀

可達 IEC 52

如考慮到戶外

開發中避免一

通訊的衛星數

收到可靠的座

發中花費時間

觀圖

29 IPX7 防

外防水或是

一些因水氣

數量以 N

座標資訊

間故障排除

防水等

是居家

氣發生

MEA

有了

除的麻

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

28

第三章 控制電路與程式設計

31 微處理器

為求控制程式簡單易懂易於傳承在微處理器的選用上需選擇支援 C 語

言之處理器另外為求簡化電路板元件數量須能支援數位類比轉換器(ADC)與脈波寬度調變(PWM)等功能基於以上訴求本論文選擇 Microchip 公司出品的

dsPIC 30F4011 微處理器其特色如下[373839]

使用哈佛式資料匯流排架構允許不同位元大小的數據資料(16 位元)與程式指

令(24 位元)分別有不同的匯流排傳輸到數學邏輯處理單元(Arithmetic amp Logic Unit ALU)此種架構可以提高微處理器執行效率如下圖 31

圖 31 哈佛式架構中各自獨立的程式與資料匯流排

dsPIC 微處理器中的資料空間可達 64K 位元組其數據資料記憶體對於大多數

程式指令碼而言可視為一個完整的線性定址空間有別於市面其他將資料記

憶體切割成兩個區塊的 DSP 處理器dsPIC 不執行數位訊號處理時整個記憶

體將被視為單一的資料記憶體

dsPIC 控制器的數位訊號處理引擎具備有一個高速運算的 17 位元乘法器一

40 個位元的數學邏輯處理器兩個 40 位元的飽和累加器以及一個 40 位元的

多位元移位器這個多位元移位器可以在單一指令執行週期內將一個 40 位

元的數值向右移位達 15 位元或者向左移位達 16 位元

CPU

Program Memory 24-bit 16-bit

Data Memory

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

29

dsPIC 數位訊號控制器具備有一個向量式的中斷架構每一個中斷來源都有它

自己的向量定義並且可以被動態的指定為 7 種優先順序每一個中斷狀態的

進入與返回所需的延遲都是相同且固定的

具備彈性的模式來處理電能的節省電磁干擾的降低以及錯誤狀況的處理具

有多種震盪器操作模式與故障檢測並有其他安全特性像是直流低電壓偵測

異常電壓重置監視計時器(Watchdog Timer)重置以及數個不可遮罩的中斷

具有眾多的周邊功能可供使用者選擇主要的功能請參考表 31並在後續章

節詳細介紹本論文有使用到之重要元件

表31 主要周邊功能

數位輸出入埠 10 位元類比數位轉換器(ADC)

計時器 通用非同步傳輸介面(UART)

輸入捕捉(Input Capture) SPI

輸出比較(Output Compare)與 PWM I2C

馬達控制波寬調變 資料轉換介面(DCI)

定位(光學)編碼器介面(QEI) 控制器區域網路(CAN)

彈性的定址模式與為 C 編譯器最佳化的指令集架構

48K 位元組的內建快閃記憶體程式空間(16K 指令字元)

2K 位元組的內建資料暫存器

6 個 PWM 輸出通道3 個工作週期(Duty Cycle)產生器

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

30

圖 32 dsPIC30F4011 硬體架構方塊圖

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

31

311 數 位 輸 出 入 埠

所有輸出入埠腳位都有 3 個暫存器直接和這些腳位操作連結由另外 1 個暫

存器決定該腳位是數位或類比[373839]

1 資料方向暫存器(TRISx)

決定這個腳位是一個輸入或是輸出當相對應的資料方向位元是rdquo1rdquo的話這

個腳位的狀態會被設定為rdquo輸入例如

TRISB = 0xffff 定義所有 PROTB 為輸入

2 拴鎖暫存器(LATx)

寫入該腳位的輸出值為高電位或是低電位例如

LATB = 0xffff 所有 PROTB 腳位輸出高電位 LATBbits = 0 PROTB 的第 5 腳位位元將輸出低電位

3 輸出入埠暫存器(PORTx)

讀取並轉存輸入值的狀態視必要性可以存為一個自定變數例如

STATE=PORTE PORTE 所有腳位狀態轉存到變數 STATE

4 類比腳位設定暫存器(ADPCFG)

由於所有的 PROTB 都可以選擇最為類比輸入或是數位輸出入所以有必要在

這個暫存器中決定 PROTB 的用途例如

ADPCFG = 0xffff 開啟 PROTB 作為數位輸出入埠的功能

下頁表 32 為本論文有使用到的輸出入埠與使用簡介

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

32

表32 本論文所使用的輸出入埠簡介 腳位 功能

RE0PWM1L 輸出PWM訊號給左馬達驅動器 RE1PWM1H 輸出PWM訊號給右馬達驅動器

RB0 控制固態繼電器(SSR)以作為馬達驅動器的總開關 RB1 輸出高或低電位至左馬達驅動器控制左馬達正反轉 RB2 輸出高或低電位至右馬達驅動器控制右馬達正反轉 RB6 輸出高或低電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RB7 輸出低或高電位至轉盤馬達驅動電路控制轉盤馬達正反轉 RE2 連接左極限開關判斷轉盤旋轉狀態將狀態回傳微處理器 RE3 連接右極限開關判斷轉盤旋轉狀態將狀態回傳微處理器

RB3AN3 連接金屬探測器電流計回傳電流變化給微處理器以供ADC處理

312 10 位 元 類 比 數 位 轉 換 器 (ADC)

dsPIC30F4011 控制器內建有 1 組 10 位元高速類比數位訊號轉換器可以用

來將類比輸入訊號轉換成一個 10 位元長度的數位訊號這個模組是建立在連續近

似暫存器的硬體架構上而且可以提供最高達每秒 500K 次的取樣頻率此微處理

器的類比訊號轉換模組中共有 9 個類比輸入通道這些通道在硬體上以多工的方

式被安置在 4 組取樣與保持(Sample amp Hold)放大器的輸入端如下圖 33 所示

取樣與保持模組的輸出將會被輸入到類比數位訊號轉換器來產生相對應的 10

位元長度數位訊號類比訊號的參考電壓可以使用軟體設定為控制器的供應電壓

或者是在參考電壓腳位上的電壓值類比訊號轉換器還有一個特點是能在處理器

休眠時繼續操作本控制器中的類比訊號轉換器硬體和操作上取樣與轉換的硬

體控制為分開處理這樣的設計使得本控制器可以選擇多種的類比訊號轉換方式

但是在使用與程式撰寫上也增加了許多必要的功能設定與操作程序而這些設

定與程序都必須要透過相關的控制暫存器來完成如下表 33 所示

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

33

圖 33 類比數位轉換器硬體方塊圖

表33 類比訊號轉換模組設定所需的6個16位元長度暫存器

AD控制暫存器1 (ADCON1)

AD控制暫存器2 (ADCON2)

AD控制暫存器3 (ADCON3)

AD輸入選擇暫存器 (ADCHS)

AD腳位設定暫存器 (ADPCFG)

AD輸入掃描選擇暫存器 (ADCSSL)

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

34

313 計 時 器

dsPIC30F4011 控制器內建了 5 個 16 位元的計時器計數器它們的編號依序

為 Timer1Timer2Timer3Timer4 以及 Timer5每一個計時器計數器模組都是

一個 16 位元的計時器計數器並包含了下列可讀寫的暫存器

TMRx16 位元的計時計數暫存器

PRx連接計數器的 16 位元週期暫存器

TxCON連接計時器的 16 位元控制暫存器

每一個計時器模組同時也有下列的相關位元作為中斷控制

TxIE中斷致能控制位元

TxIF中斷旗標狀態位元

TxIPlt20gt中斷優先層次控制位元

本論文使用計數器的目的在於控制類比數位轉換器(ADC)於每 250 毫秒

(millisecond)啟動一次 ADC 副程式來偵測並轉換金屬探測器電流計的電流變化值

為達成以上程式設計目的在微處理器上規劃使用 Timer2 來計數每 1ms 計數一

次由 Timer2 中斷副程式計算是否達到 250 次達到後即開啟 ADC 取樣並轉換

電流變化計時器中斷如下段介紹當 Timer2 的 16 位元計時器計數內容符合週

期暫存器 PR2 的內容時而且中斷功能也開啟T2IF 中斷旗標將會被設定為 1產

生一個中斷一旦中斷發生T2IF 位元必須要用軟體指令來清除計時器中斷旗

標 T2IF 位元是位於中斷控制暫存器 IFS0 裡面例如

IFS0bitsT2IF = 0 清除中斷旗標

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

31

TRA傳輸

傳輸

應的

收的

資料

式對

4 通 用 非

通 用 非 同

ANSMITTE輸的資料存入

輸程序都不需

的中斷旗標或

的方面微處

料並存入暫存

對所接收到的

通用非同步

全雙工8

奇數偶數

非 同 步 傳

同 步 傳 輸

R(UART) M入到特定的

需要應用程

或位元設定

處理器的硬

存器中時

的資料做後

圖

步傳輸接收

8 或者 9 位元

數或無謂員

傳 輸 介 面

輸 介 面 (UNMODULE)

的暫存器中

程式的介入

定提供應用程

硬體將會自動

控制器將會

後續處理下

圖 34 UART

收模組的主要

元資料通訊

員檢查選項(

35

面

NIVERSAL)在 dsPIC30控制器中

而且當硬

程式作為傳

動的處理接

會設定相對

下圖 34 為

通訊模組的

要功能包括

訊

(供 8 位元傳

L ASYNC0F4011 的使

中的硬體將

硬體完成傳

傳輸狀態的

接收資料的

對應的中斷

UART 模組

的硬體架構

括

傳輸使用)

CHRONOU使用上應用

將自動地處理

傳輸的程序後

的檢查同樣

的前端作業

斷旗標或位元

組的硬體架

構圖

S RECEI用程式只需

理後續的

後也會有

樣的在資

當接收到

元觸發應

架構

IVER 需要將

的資料

有相對

資料接

到完整

應用程

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

36

1 或 2 個停止位元

完全整合的鮑率產生器並附有 16 位元預除器

在 30MHz 指令執行速率下鮑率可選擇由 38bps 至 1875Mbps 間的範圍

4 個字元大小的資料傳輸緩衝器

4 個字元大小的資料接收緩衝器

位元檢查資料格及緩衝器超越(Overrun)錯誤偵測

支援 9 位元傳輸格式下的位址偵測中斷

獨立的傳輸與接收中斷

可作為程式檢測的資料回傳模式

為了使用 UART 傳輸模組在主電路板須配置一個相容於 MAX232 規格的

RS-232 收發器藉以提升資料匯流排上的電壓位準同時須配置標準的 DB9 傳輸

線接頭可用於連接無線 RS232 模組或是與 PC 作 COM 傳輸埠連接由於 dsPIC控制器有兩個 UART 傳輸模組為了未來方便擴充其他周邊感測器在設計上是

預留了 2 組 DB9 接頭

在鮑率產生器的調整上須遵循 Data sheet 給予的公式並寫入暫存器 UxBRG來決定該 UART 模組的傳輸速度寫入暫存器的數值公式如下

UxBRG = ( (控制器指令執行頻率設計鮑率)16) ndash 1

所以假設電路板震盪器使用 8MHz設計鮑率為 9600bps則 UxBRG = 103下表 34 列出了本論文有使用到的 UART 函式庫與用途

表34 本論文用到的UART函式與用途 OpenUART1() 開啟與設定UART模組 ConfigIntUART1() 設定UART資料接收與發送中斷 DataRdyUART1() 檢查UART資料接收狀態 ReadUART1() 接收單一位元組 CloseUART1() 關閉UART模組

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

37

315 馬 達 控 制 脈 波 寬 度 調 變 (PWM)模 組

dsPIC30F4011 數位訊號控制器的馬達控制脈波寬度調變(PWM)模組有下列的

功能與特性

6 個 PWM 輸出入腳位以及 3 個工作週期(Duty Cycle)產生器

16 位元的解析度

即時 PWM 頻率切換

邊緣對齊與中央對齊輸出模式

單一脈衝產生模式

中央對齊模式下的非對稱更新中斷支援

電氣交換馬達操作的輸出強制修改控制

安排其他周邊事件發生的特殊事件比較器

錯誤偵測腳位可驅使每一個 PWM 輸出到定義的狀態

馬達控制 PWM 模組包含三個工作週期產生器編號由 1 到 3這個馬達控制

PWM 模組擁有 6 個 PWM 輸出腳位編號分別為 PWM1HPWM1L

PWM2HPWM2LPWM3HPWM3L這六個輸出入腳位被編成高低編號的三對

以標註 HL 來區分對應對於互補式的負載定義為低(low)的 PWM 腳位永遠

都是相對於高(high)輸出腳位的互補狀態

本論文在 PWM 模組的初始化設定上有開啟 PWM1L 與 PWM1H關閉其餘

PWM 輸出並設定為數位輸出入埠如表 32 中的 RE2PWM2LRE3PWM2H其他

主要設定為工作週期固定為 50關閉 PWM1LH 兩腳位的互補模式成為獨

立輸出最後並將 PWM 模組設定為邊緣對齊訊號的 FREE 計時器模式在速度控

制方面會在副程式中利用特殊暫存器 PTCON 的數值設定達成履帶載具的三段

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

38

變速控制PWM 模組方塊圖如下圖 35 所示

圖 35 PWM 模組硬體架構方塊圖

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

39

32 控制電路設計

本控制電路設計目標在於盡可能簡化電路元件的使用並且將大部分功能

模組化或者是交由微處理器自行運算處理以求簡化電路設計基於以上理由以

及各章節的研究與討論以此目的規劃所需使用到的功能與周邊得到主控制電

路以及轉盤驅動電路

321 主 控 制 電 路

圖 36 主控制電路完成圖

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

40

圖 37 主控制電路元件分布圖與長寬標示

本電路設計參考 MPLAB C30 實驗版之接線圖[38]並針對研究需求增刪所需

元件成品如圖 36 所示由圖 37 所示主電路板以 dsPIC30F4011 為核心加

上一顆 HIN232 IC 統一調整電壓位準並分別輸出給 2 組 DB9 接頭定為 UART1與 UART2以對應核心處理器之兩組 UART 使用

搭配 7805 穩壓 IC 供應穩定的 5 伏特電源給予處理器並安置一組 LED 與開

關作為電源導通控制與標示加上一按鍵開關配合電路設計連接至腳位 MCLR以備重置微處理器之用並由一顆 8MHz 的石英震盪器作為微處理器指令時脈來

源

322 轉 盤 驅 動 電 路

轉盤驅動電路由全橋式驅動 IC 與 OP 放大器組成主要為驅動直流馬達帶動

金屬轉盤預留 OP 放大器以驅動 RC 伺服馬達詳見圖 38 和圖 39

12cm

95cm

微處理器

dsPIC

HIN

232

UA

RT1

UART2

7805 穩壓 IC

Reset

總開關

8MHz OSC

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

41

圖 38 轉盤驅動電路完成圖

圖 39 轉盤驅動電路元件分布圖與長寬標示

104cm

77cm 鋰電池

TA8429H

7808 穩壓 IC

開關

OP 放大器

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

42

323 控 制 程 式 流 程 圖

圖 310 控制電路主程式流程圖

Main Start

Set IO

Initialize

Timer2

count=250

ADC( )

While(1)

Is Limit1

Pressed

Is Limit2

Pressed

Is UART

Ready

Motor( )

Rotary Table CW

Rotary Table CCW

YES

NO

YES

YES

YES

NO

NO

NO

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

43

第四章 實驗結果

41 系統整合

411 硬 體 整 合

將上述章節中提到的各硬體元件與系統程式結合後完成測試探測地雷機器

人的系統整合第一層外觀圖如圖 41 所示

圖 41 探測地雷機器人第一層各部元件

區塊 B

區塊 D

區塊 E

區塊 C 區塊 C

區塊 B

區塊 A

區塊 F

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

44

以下為圖 41 各區域功能簡介

區塊 A M97 金屬探測器與其控制面板負責探測車體前方金屬物體並回傳

電流訊號變化給控制電路板以供 ADC 訊號轉換

區塊 B左右極限開關負責限制金屬探測器左右擺盪幅度當有碰觸開關

事件發生時由控制電路偵測變化並控制轉盤反方向轉動

區塊 C左右伺服馬達驅動器負責接收控制電路板的 PWM 脈波與控制脈

波進而驅動直流無刷馬達控制機器人運動路徑

區塊 D控制電路板分為主控制電路與轉盤驅動電路負責接受 PC 端命令

發出控制脈波執行邏輯判斷與數位類比信號轉換等

區塊 E限電流 10 安培的固態繼電器(Solid State Relay)當作伺服馬達驅動器

總開關可由控制電路發出訊號開啟或關閉並作為過電流保護器

以防電池供給過大瞬間電流避免驅動器燒毀

區塊 F由湯淺公司出品的 12 伏特7 安培小時的鉛酸電池串連 2 顆供給 24V給伺服馬達驅動器使用

圖 42 探測地雷機器人第二層各部元件

區塊 H

區塊 G

A

B

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

45

由於機器人的平面空間不足所以另外加上一層平台作為空間擴充如圖 42所示以下為圖 42 各區域功能簡介

區塊 G全球衛星定位系統(GPS)收發模組負責接收 NMEA 格式資料透過

PC 端處理並切割字串資料

區塊 H無線 RS232 模組由兩個模組分開負責不同訊號模組 A 負責傳送載

具系統狀態與接收 PC 端控制訊號模組 B 負責接收 GPS 模組的資料

並回傳給 PC 端處理

412 軟 體 介 面 整 合

以下測試會使用 Borland C++ Builder(BCB)所自行設計撰寫之程式介面

[404142]控制機器人運動接收控制程式狀態訊號接收 GPS 字串與獲取感測

器數值下圖 43 為控制程式介面圖 44 到圖 48 為控制介面功能介紹

圖 43 BCB 程式介面

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

46

4121 運動控制與金屬探測

圖 44 BCB 程式介面介紹

區塊 A負責開啟關閉 RS232 通訊開啟關閉伺服馬達驅動器重置微處

理器關閉程式介面在 Memo 元件回報系統狀態

區塊 B負責接收控制電路回傳的金屬探測器訊號強度強度範圍為 0~130並使用綠色 Shape 元件與靜態文字顯示車前有無金屬物體參數設定

為訊號強度超過 50 即視為危險當超過 50 時Shape 元件會改變成

紅色出現警告訊息並且把前進按鈕除能(disabled)實際狀況如下圖

45 所示

區塊 C當微處理器接收到控制訊號並回傳在此區塊的 Memo 元件供使用

者確認已收到控制指令

區塊 D負責機器人運動控制可控制基本的前進後退左回轉和右回轉等

動作

區塊 E負責速度控制並回報目前速度三段速度控制由低至高為 1 檔~3 檔

區塊 A

區塊 B

區塊 C 區塊 D

區塊 E

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

47

圖 45 警告使用者前方有金屬物體並且將前進按鈕除能

4122 GPS 介面

圖 46 GPS 介面介紹-GPS 座標值

區塊 A顯示當前經緯度座標位置格式為度分表示如上圖 2341672N 代表

北緯 23 度 41672 分

區塊 B負責儲存目前的座標每按下按鈕一次後會將目前座標存在指定的純

前進按鈕除能 訊號強度超過 50

警告使用者

區塊 A

區塊 B

區塊 C

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

48

文字檔(副檔名txt)一次以利日後查詢如下圖 47 所示

圖 47 將經緯度座標存在純文字檔中

區塊 C當按下區塊 B 的按鈕時經緯度會同步記錄在 C 區的表格中供使用

者即時查詢已存了哪些點

圖 48 GPS 介面介紹-GPS 字串值

區塊 D顯示 GPS 接收儀機碼由於機碼為一連串 NMEA 格式字串難以用

人工閱讀發式辨識因此在設計上用分頁模式將區塊 D 加以隱藏有

需要時再開啟此分頁閱覽

區塊 D

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

49

42 系統測試

421 金 屬 探 測 器 轉 盤 測 試

下接圖 49-149-2測試轉盤運動狀況驗證機構能限制探測器在車前往返

運動

圖 49-1 影片第 1 秒探測器在最左側 影片第 3 秒探測器在中間偏右

圖 49-2 影片第 4 秒探測器在最右側

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

50

422 室 內 測 試

室內測試主要目的在驗證運動控制與控制程式的保護機制能確實在偵測到

金屬物體時警告使用者測試地點為電機系館走廊偵測標的為走廊埋設管線

以下測試如圖 410-1 至 410-5 所示

圖 410-1 偵測到隱藏管線控制程式提醒使用者避開圖為使用者控制後退

圖 410-2 發現第二處隱藏管線控制程式發出紅燈信號提醒使用者

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

51

圖 410-3 前進控制

圖 410-4 左轉控制

圖 410-5 右轉控制

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

52

423 室 外 測 試

室外測試主要是驗證履帶機構是否能克服崎嶇不平的路面測試場地為電機

系館頂樓實驗結果差強人意即使是最低速在左右回轉時仍然會有無法帶動

的情形如下圖 411-1 至 411-4 所示

圖 411-1 開始點

圖 411-2 前進

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

53

圖 411-3 左回轉

圖 411-4 左回轉四分之一圈後卡住無法迴轉半圈

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

54

第五章 總結與未來展望

本論文主要研究探測地雷機器人控制GPS 座標回傳的軟硬體設計讓機器

人可經由PC端的控制透過金屬感測器在履帶車前左右探測當遇到金屬物體時

能夠停下車體並回傳座標至 PC 端紀錄以利日後移除該金屬物經過實驗與整合

驗證證明系統可由一組微處理器控制能夠減少元件數量與設計成本並保持設

計彈性

基於使用 GPS 後發現一些相關問題諸如第一次定位時間太長有可能超

過 10 分鐘最小精確度為 5 公尺對於這種小尺寸的履帶型機器人不甚理想會

受天候影響會受周圍建築物密度影響精確度在在顯示無法只依賴一種定位系

統也許可以考慮一般市售房車整合陀螺儀與 GPS 的作法甚至使用接收手機

基地台封包訊號的 AGPS 來加快定位的速度都是可以參考的走向

未來可以改善的方向如下

1 在 Encoder 回授的部分原始架構為使用如圖 51 之半閉迴路架構缺點是

無法掌握確實掌握回授值未來會考慮使用如圖 52 之全閉迴路系統方便

做路徑規劃與更精確的位置定位模式

圖 51 脈波命令式伺服系統(半閉迴路) [43]

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

55

圖 52 電壓命令式伺服系統(全閉迴路) [43]

2 在定位系統導入座標與地圖能夠針對所在地區座標與車體本身座標做比對

以利自動行走

3 導入攝影機針對影像辨識避障能夠有進一步的發展

4 在 PC 端加入資料庫功能能記錄金屬物的經緯度座標以及發現時間可以做

為地圖顯示的輔助功能

5 研究如何增強扭力以利機器人室外移動

6 由於金屬探測器原理為針對平面導體所引發的磁場磁交鏈的渦電流變化來判

斷金屬物體遠近與大小如遇到崎嶇不平地面探測效果將打折扣未來考慮

在轉盤總成機構加上防傾斜架構當機器人遇到不平坦地面時可以靠此結構

補償傾斜角度並且保磁探測器與地面水平以利準確探測

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

56

參考文獻

[1] Landmine Monitor網頁資料httpwwwicblorglm

[2] J D Nicoud and M K Habib ldquoThe Pemex-B autonomous demining robot Perception and navigation strategiesrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 419-424 1995

[3] J D Nicoud and P Machler ldquoRobots for anti-personnel mine searchrdquo Control Eng Practice vol 4 pp 493-498 1996

[4] Y Tojo P Debenest E F Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025-2030 2004

[5] K Kato and S Hirose ldquoProposition and basic experiments of shape feedback master-slave arm-on the 1application for the demining robotsrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 3 pp 2334-2339 2000

[6] K Kato and S Hirose ldquoProposition of the humanitarian demining system by the quadruped walking robot-adaptability for various tasks using the foot-end-effecter changing mechanismrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 769-774 2000

[7] K Kato and S Hirose ldquoDevelopment of the Quadruped Walking Robot TITAN-IX rdquo Proceedings of the IECON vol 1 pp 40-45 2000

[8] S Suganuma M Ogata K Takita and S Hirose ldquoDevelopment of detachable tele-operation Gripper for the walking robotrdquo Proceedings of the IEEERSJ Intl Conference on Intelligent Robots and Systems vol 3 pp 3390-3395 2003

[9] K Arikawa and S Hirose ldquoDevelopment of quadruped walking robot

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

57

TITAN-VIIIrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 1 pp 208-214 1996

[10] httpchinarobotblogchinacomblogcategory83706html

[11] ldquoLandmine detection research pushes forward despite challengesrdquo Proceedings of the IEEE International Conference on Intelligent Systems vol 17 pp 4-7 2002

[12] httpbbsmoninetcomtwboardtopiccgiforum=11amptopic=8375ampshow=150

[13] L Marques M Rachkov and A T Almeida ldquoMobile pneumatic robot for deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 4 pp 3508-3513 2002

[14] S X Yang C Luo and Q H M Meng ldquoArea-covering operation of a cleaning robot in a dynamic environment with unforeseen obstaclesrdquo Proceedings of the IEEE International Symposium on Computational Intelligence in Robotics and Automation vol 2 pp 1034-1039 2003

[15] Y Zhang M Schervish E U Acar and H Choset ldquoProbabilistic methods for robotic landmine searchrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 3 pp 769-774 2001

[16] R Cassinis G Bianco A Cavagnini and P Ransenigo ldquoStrategies for navigation of robot swarms to be used in landmines detectionrdquo Proceedings of the Third European Workshop on Advanced Mobile Robots pp211-218 1999

[17] Y J Lee and S Hirose ldquoThree-legged walking for fault tolerant locomotion of a quadruped robot with demining missionrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 973-978 2000

[18] G M Univ ldquoGeo-location of detected landmines via mobile robots equipped with

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

58

multiple sensorsrdquo Proceedings of the IECON vol 3 pp 1972-1977 2002

[19] S C Wong and B A Macdonald ldquoA topological coverage algorithm for mobile robotsrdquo Proceedings of the IEEERSJ International Conference on Intelligent Robots and Systems vol 2 pp 1685-1690 2003

[20] V Kumar and F Sahin ldquoCognitive maps in swarm robots for the mine detection applicationrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 4 pp 3364-3369 2003

[21] E Chapman and F Sahin ldquoApplication of swarm intelligence to the mine detection problemrdquo Proceedings of the IEEE International Conference on Systems Man and Cybernetics vol 6 pp 5429- 5434 2004

[22] Y Tojo P Debenest EF Fukushima and S Hirose ldquoRobotic system for humanitarian deminingrdquo Proceedings of the IEEE International Conference on Robotics and Automation vol 2 pp 2025 - 2030 2004

[23] K Nonami and Y Ikedo ldquoWalking control of COMET-III using discrete time preview sliding mode controllerrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems vol 4 pp 3219 - 3225 2004

[24] P Santana J Barata H Cruz A Mestre J Lisboa and L Flores ldquoA multi-robot system for landmine detectionrdquo Proceedings of the IEEE International Conference on Emerging Technologies and Factory Automation vol 1 pp 721-728 2005

[25] S Larionova L Marques and AT de Almeida ldquoFeatures selection for sensor fusion in a demining robotrdquo Proceedings of the IEEE International Conference on Robotics and Automation pp 3175-3180 2005

[26] J Coronado-Vergara G Avina-Cervantes M Devy and C Parra ldquoTowards landmine detection using artificial visionrdquo Proceedings of the IEEE International Conference on Intelligent Robots and Systems pp 659-664 2005

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

59

[27] CSBL900 直流無刷伺服馬達驅動器使用說明書

[28] 辰白編譯1995DC 伺服馬達應用電路技術初版建興出版社

[29] TA8429H full bridge driver IC datasheet

[30] 蘇琨祥孫振峰王仁宏「渦電流地雷檢測器之設計及磁場模擬分析」

中華民國第十三屆國防科技學術研討會pp 683~690

[31] M97 metal detector datasheet

[32] Wireless Radio Modem SST-2400 Userrsquos Manual

[33] 宋國祥2008兩足攀爬機器人之研究與製作國立雲林科技大學碩士論

文

[34] 呂紹銓2003具位置回報功能之無人自走車的研製與控制國立雲林科技

大學碩士論文

[35] B D Nordwall殷書平2002GPS 系統對民用和軍用用戶的重大升級控

制工程(北京)

[36] Kevin rdquoNMEA 標 準 格 式 rdquo 網 頁 資 料

httpblogxuitenetcchkevinmyBlog21596174

[37] dsPIC30F40114012 Data Sheet Microchip Inc2005

[38] 曾百由2005dsPIC 數位訊號控制器原理與應用-MPLAB C30 開發實務

初版宏友圖書

[39] dsPIC30F Peripheral Module Training ndash RTC Taiwan

[40] 洪維恩2007C 語言教學手冊第四版旗標

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

60

[41] Time 研究室2003C++ Builder 6 完全攻略初版金禾資訊

[42] 范逸之江文賢陳立元等編著2002C++ Builder 與 RS-232 串列通訊控

制初版文魁資訊股份有限公司

[43] 擎翔實業網頁資料httpwwwcsimcomtwteach12tech12htm

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf

61

自傳

曾耀輝台灣省台北縣人民國七十年十二月二十六日生

民國九十六年甄試進入國立雲林科技大學電機工程研究所控制組就讀迄今

學經歷

民國 86 年 9 月 進入國立台北工專五專部 電機科

民國 91 年 9 月 進入國立雲林科技大學二技部 電機工程系

民國 93 年 10 月 投身軍旅 擔任陸軍義務役戰車士官 服役 1 年 3 個月

民國 96 年 9 月 進入國立雲林科技大學 電機研究所 控制組

• 探雷機器人篇前部分_定稿_pdf
• 曾耀輝論文行使同意書pdf
• 曾耀輝論文審定書pdf
• 探雷機器人內文_定稿_0724pdf