國
立
交
通
大
學
電機與控制工程學系
碩
士
論
文
全向性移動平台之精密運動控制設計
Precision Motion Control Design on the Omnidirectional
Mobile Robot
研 究 生：莊孝麟
指導教授：徐保羅 博士
全向性移動平台之精密運動控制設計
Precision Motion Control Design on the Omnidirectional Mobile Robot
研 究 生：莊孝麟 Student：Xiao-Lin Chuang 指導教授：徐保羅 教授 Advisor：Dr. Pau-Lo Hsu
國 立 交 通 大 學 電機與控制工程學系
碩 士 論 文
A Thesis
Submitted to Department of Electrical and Control Engineering
College of Electrical and Computer Engineering
National Chiao Tung University
in partial Fulfillment of the Requirements
for the Degree of
Master
in
Electrical and Control Engineering
December 2006
全向性移動平台之精密運動控制設計
研究生：莊孝麟 指導教授：徐保羅 博士
國立交通大學電機與控制工程學系
摘要
近幾年，智慧型機器人逐漸成為研究的熱門話題，本論文將藉由 inverse kinematics，實現全向性移動平台多軸同動的運動控制技術，並探 討不同運動路徑之下追跡能力，以及加減速對於改善打滑之影響。 每一種感測元件都有其缺點，有時無法反應出平台運動的真實行為， 於是結合多種感測器，相互截長補短已是現今許多研究之方法。本論文利 用感測融合概念，分析輪式編碼器與電子羅盤之優缺點，並建立動態調整 回授機制，在兼顧穩定性與準確性之下，改善平台運動方位誤差約 50%。 近來 RFID 的應用創意百出，在機器人導航領域中，此技術被拿來作 為絕對定位之參考，本論文提出以平台分段式移動策略並結合 RFID 功率 調變，建立全向輪平台之絕對定位技術，並改善絕對定位誤差從 11 cm 降 到約 3 cm。Precision Motion Control Design on the Omnidirectional
Mobile Robot
Student：Xiao-Lin Chuang Advisor：Dr. Pau-Lo Hsu Department of Electrical and Control Engineering
National Chiao-Tung University
ABSTRACT
In recent years, the intelligent robot attracts a lots of research attentions. By using the inverse kinematics, the realization of multi-axis motion control of the omnidirectional mobile robot is achieved in this thesis. Besides, the tracking accuracy in various paths and velocity planning have been discussed.
Since each sensor is limited to certain functions on capabilities, different types of sensors can be fused to overcome the limitations of individual sensors. This thesis uses the sensor fusion concept to integrate both the wheel encoder and the compass, and uses the sensor fusion to built a dynamic regulating feedback scheme to improve 50% of heading error for the robot.
Moreover, the RFID technology was also developed for absolute localization in robot navigation. In this thesis, we propose a localization strategy which combines both the multi-stage movement strategy with the power modulation. Furthermore, the proposed strategy has been successfully applied to the omnidirectional mobile robot to improve its motion accuracy from 11 cm to 3 cm.
誌 謝
首先要感謝指導教授徐保羅 博士在兩年研究生活中的耐心指導，當研究遇 到問題與挫折時，能與他一起尋求突破之道，並且讓我有充分的發展空間，使我 能順利的完成碩士學位，在此表達我最真誠的敬意與感謝。另外感謝口試委員王 伯群 博士、宋開泰 博士、葉賜旭 博士對於本論文的指正，並給予寶貴的建議， 使得本論文更加完善。 感謝鎮洲學長在研究上的建議與生活上的心得分享，感謝一同參與計畫的 琮政學長、元銘同學容忍我對於研究上的無知與堅持，感謝昭琳學長、俊賢學長 在工作與生活上的寶貴經驗，景文學長對於程式撰寫與生活趣事的分享；感謝尚 玲學姊研究上的開導、建良學長指導做人處世的本分、煒生學長教導研究應有的 態度；感謝興漢同學帶來的歡笑與鼓勵、宗翰同學抬槓式的生活交談、瑞原同學 適時的幫助、元銘同學在課業、研究、生活上的砥礪與扶持；感謝學弟宗勝、雨 坤、林億、文諒、醇偉、建龍、也強、旭志陪伴我的研究生活。特別對琮政學長、 元銘同學、也強學弟在研究與實驗上的大力幫忙，獻上誠摯地感謝。感謝這兩年 實驗室所有夥伴帶來的回憶：烤肉、唱歌、聚餐、出遊、籃球隊、羽球隊、環校 路跑、慶生會、煮湯圓…等。感謝工研院的機械所同仁對於計畫的贊助與指導。 感謝一路走來始終陪伴、鼓勵我的摯友明瑾。 最後，將此論文獻給我最敬愛的父親 莊泮森先生、母親 陳琴女士，以及 所有關心我的家人與朋友，因為有你們在身旁的支持與關懷，使我能夠心無旁騖 地完成學業，並且順利地克服我人生中的每一個重大考驗。 願 平安 健康 成就感2006.12.08
目
錄
中文摘要
i
英文摘要
ii
誌謝
iii
目錄
iv
表目錄
vii
圖目錄
viii
第一章 緒論 ...1
1.1
研究動機與目的
... 1
1.2
研究背景與發展概況
... 2
1.3
問題陳述
... 5
1.4
研究方法與步驟
... 6
1.5
論文架構
... 8
第二章 平台運動模型與系統架構...9
2.1
平台運動模型
... 9
2.2
硬體感測架構
... 12
2.2.1 eZdsp
TMF2812 DSK之功能介紹... 13
2.2.2 DAC介面 ... 14
2.2.3 Encoder介面 ... 16
2.2.4 Compass介面... 19
2.2.5 RFID介面 ... 28
2.3
程式控制架構
... 31
2.3.1 命令產生流程... 31
2.3.2 平台控制迴路... 34
2.3.3 各軸控制迴路... 35
第三章 全向平台之多軸運動控制...36
3.1
各軸PID控制器之實驗結果... 36
3.1.1 各軸特性分析... 36
3.1.2 設計各軸PID控制器 ... 39
3.2
平台控制器之實驗結果
... 42
3.2.1 平台控制器設計... 42
3.2.2 直線路徑 ... 45
3.2.3 直線加自旋路徑... 47
3.2.4 S曲線路徑 ... 50
3.2.5 S曲線加自旋路徑 ... 53
3.2.6 實驗結果討論... 56
3.3
速度命令規劃對於運動慣性之影響
... 56
第四章 感測融合與RFID定位 ...59
4.1
感測融合機制
... 59
4.1.1 動態感測融合... 59
4.1.2 實驗結果 ... 60
4.2
RFID定位... 65
4.2.1 RFID中斷程式 ... 65
4.2.2 RFID策略定位方法 ... 66
4.2.3 無策略定位時的實驗結果... 71
4.2.4 分段式移動策略定位時的實驗結果... 72
4.2.5 結合功率調變與分段策略時的實驗結果... 73
第五章 結論與未來展望...75
5.1
結論
... 75
5.2
未來展望
... 76
參考文獻………77
表
目 錄
表 2-1 平台原地旋轉之compass平均誤差與標準差 ... 24 表 2-2 SPI 時脈規劃選擇... 27 表 2-3 RFID常用頻段... 28 表 2-4 RFID reader規格... 30 表 3-1 無載時各軸正轉之IAE與平均誤差百分比 ... 38 表 3-2 無載時各軸反轉之IAE與平均誤差百分比 ... 38 表 3-3 Kp=1.1 時調整Ki對於加減速之平均速度IAE ... 41 表 3-4 Kp=1.1、Ki=0.1 時調整Kd對於加減速之平均速度IAE ... 41 表 3-5 Kp=1.1、Ki=0.2 時調整Kd對於加減速之平均速度IAE ... 41 表 3-6 四種路徑之實驗參數... 43 表 3-7 平台位置控制器Kp、Kd參數調整 ... 45 表 3-8 直線路徑時平台控制器位置IAE比較 ... 47 表 3-9 直線加自旋路徑時平台控制器位置IAE比較 ... 50 表 3-10 S曲線路徑時平台控制器位置IAE比較 ... 53 表 3-11 S曲線加自旋路徑時平台控制器位置IAE比較 ... 56 表 3-14 速度規劃對於位置與方位之影響... 58 表 4-1 不同的感測角度方式時之位置IAE比較 ... 61 表 4-2 感測融合對於位置與方位之影響... 62 表 4-3 RFID中斷內switch函式... 66 表 4-4 絕對定位偵測路徑之狀態表... 69 表 4-5 各角度量測RFID reader 讀取有效距離 ... 72 表 4-6 分段式移動策略定位實驗結果... 73 表 4-7 結合功率調變與分段式移動策略定位實驗結果 ... 74圖
目 錄
圖 1-1 全方向性輪子[4] ... 2 圖 1-2 RFID reader有效偵測範圍(單位:cm)[18] ... 6 圖 2-1 四輪全向性移動平台座標與符號 ... 9 圖 2-2 硬體系統架構示意圖... 12 圖 2-3 平台實體圖... 13 圖 2-4 DAC 7625 內部功能方塊圖 ... 15 圖 2-5 單軸DAC介面電路... 16 圖 2-6 輪式編碼器原理圖... 16 圖 2-7 QEP訊號示意圖... 17 圖 2-8 雙軸encoder介面電路... 18 圖 2-9 本文使用全向輪[22]... 18 圖 2-10 二維的電子羅盤[23]... 19 圖 2-11 各角度感測之磁通密度[24]... 20 圖 2-12 compass校正流程圖... 21 圖 2-13 compass之靜態分析... 23 圖 2-14 平台直線行走之compass回授情形 ... 23 圖 2-15 平台原地旋轉之compass回授誤差情形 ... 24 圖 2-16 電子羅盤頻譜圖... 25 圖 2-17 比較compass有無LPF... 25 圖 2-18 改善後compass穩定性與encoder比較 ... 26 圖 2-19 V2Xe與F2812 介面電路 ... 26 圖 2-20 SPI master-slave連接圖... 27 圖 2-21 V2Xe與F2812 通訊時序圖 ... 28 圖 2-22 RFID原理示意圖... 29 圖 2-23 Magstripe通訊格式... 30 圖 2-24 RFID reader模組與F2812 介面電路 ... 31 圖 2-25 主程式流程大綱... 31 圖 2-26 命令產生流程... 32圖 2-27 梯形與S形速度規劃示意圖... 34 圖 2-28 平台控制迴路... 34 圖 2-29 各軸控制迴路... 35 圖 3-1 無載時四軸正轉之步階響應圖... 36 圖 3-2 無載時四軸反轉之步階響應圖... 37 圖 3-3 無載時四軸正反轉速度追跡特性(顯示於同一圖中) ... 37 圖 3-4 Kp=1.1 時調整Ki對於各軸之響應 ... 40 圖 3-5 各軸PID調整時之特性說明... 41 圖 3-6 設計PID參數較好的速度追跡情形 ... 42 圖 3-7 直線路徑之理想位置變化... 42 圖 3-8 直線與直線加自旋路徑之理想位置變化 ... 43 圖 3-9 S曲線路徑之理想位置變化... 43 圖 3-10 S曲線加自旋路徑之理想位置變化 ... 44 圖 3-11 位置控制器Kp過小或過大時追跡情形 ... 44 圖 3-12 直線路徑之各軸理想旋轉方式... 46 圖 3-13 平台位置P控制器之直線路徑追跡(encoder) ... 46 圖 3-14 平台位置PD控制器之直線路徑追跡(encoder) ... 47 圖 3-15 直線加自旋路徑之各軸理想旋轉方式 ... 48 圖 3-16 平台位置P控制器之直線加自旋路徑追跡(encoder) ... 48 圖 3-17 平台位置PD控制器之直線加自旋路徑追跡(encoder) ... 49 圖 3-18 S曲線路徑之各軸理想旋轉方式... 50 圖 3-19 平台位置P控制器之S曲線路徑追跡(encoder) ... 51 圖 3-20 平台位置PD控制器之S曲線路徑追跡(encoder) ... 52 圖 3-21 S曲線加自旋路徑之各軸理想旋轉方式 ... 54 圖 3-22 平台位置P控制器之S曲線加自旋路徑追跡(encoder) ... 54 圖 3-23 平台位置PD控制器之S曲線加自旋路徑追跡(encoder) ... 55 圖 3-27 步階速度規劃時Xw、Yw響應... 57 圖 3-28 梯形速度規劃時Xw、Yw響應... 57 圖 3-29 S形速度規劃時Xw、Yw響應... 58 圖 4-1 角度的信任權重比... 60 圖 4-2 Type 1 之S曲線加自旋路徑角度追跡(μencoder=100%) ... 63
圖 4-3 Type 2 之S曲線加自旋路徑角度追跡(μcompass=100%) ... 63 圖 4-4 Type 3 之S曲線加自旋路徑角度追跡(Fusion) ... 64 圖 4-5 說明Type 3 感測融合之穩態時誤差 ... 64 圖 4-6 RFID 中斷流程圖... 65 圖 4-7 分段定位示意圖... 66 圖 4-8 RFID tag實際有效區示意圖... 67 圖 4-9 絕對定位偵測路徑示意圖... 68 圖 4-10 本實驗用之RFID reader讀取有效範圍 ... 71 圖 4-11 分段式移動策略定位實驗圖-1... 72 圖 4-12 分段式移動策略定位實驗圖-2... 73 圖 4-13 結合功率調變與分段式移動策略定位實驗圖 ... 74
第一章 緒論
11.1 研究動機與目的
機器人導航(navigation)的研究領域是從 1950 年代開始，發展至今，機 器人的移動形式向來是主要研究領域之一，因應各類環境的運用需求，發 展了相對的移動自由度(Degree Of Free，DOF)機器人，舉凡足型、輪型、 軌跡型……等不同類型的機器人。 在輪型機器人(亦稱移動平台)中，全向輪(omni-directional wheel)平台因 為具有 3DOF，機動性最高，故現今很多研究機器人的平台均以此結構為 基礎，然而全向輪因為有側向輪，使得與地面打滑(slip)現象所造成的相對 誤差(relative error)將比傳統的差動式輪子更為顯著。 另外運動過程中，終究有累積誤差(accumulated error)問題，這使得平 台無法長時間使用，所以在機器人導航領域中，也有絕對位置(absolute position)定位之研究，其中以標地物(landmark)方式定位的研究已有幾十年 歷史，近幾年，原先規劃使用於貨物、人員管制的無線射頻辨識系統 (Radio Frequency IDentification，RFID )，被引進作為定位的一種方法，其 優點是：便於安裝擺放，標籤(tag)不需電源供應，且每個 tag 均有唯一識 別碼(unique identification)，但對於定位而言也有其缺點：收訊範圍不固 定，隨著 tag 與讀取器(reader)對應角度、天線設計、傳輸訊號方式、電磁 技術、功率大小、周圍環境的不同，也將影響定位精確性。 本論文需建立全向平台所需之運動模型(kinematics model)，使平台能 達到任意方向、路徑移動，並發展所需之控制法則，加強其運動精確性， 降低相對定位之誤差。並且研究如何將 RFID 定位之精確性提升的機制與 修正法則，使平台能在長時間使用下，均維持一定準確性。1.2 研究背景與發展概況
1、全向性機構研究與發展 近年來，移動平台不只是個熱門的研究題目，而且已廣泛被應用，在 工業界，舉凡工廠廠房、倉庫、建築工地，更被推廣應用在我們生活的居 家環境中。這些平台不僅協助人們搬運重物、完成困難繁瑣的工作還可以 做巡邏或清掃房子等工作。隨著技術的發展，使得平台能擁有更多更強的 功能，工作於不同的環境。 一種全方向性的移動平台概念因此被提出 [1]，它提供更好的移動性， 使得平台更具機動性。全方向性移動平台顧名思義為一種可以任何方向移 動的平台，一直到現在，有多種款式的全向平台被人們所提出，如：足型 機械人(legged robots)、球型輪移動平台(ball wheel robots)[2]、爬蟲型移動 平台(crawler mechanisms)[3]等。其中足型機械人其機械結構及控制方法非 常的複雜，且它的移動速度遠遠慢於輪型移動平台；球型輪移動平台受限 於輪子與地板的摩擦力過小，使得平台的定位困難，進一步影響控制結 果，這類的平台只適合工作於少數的適合地板；而爬蟲型平台可以克服一 些階梯式的地形，但是使用太軟的輪子卻使他們移動速度不快。 圖 1-1 全方向性輪子 [4]一種新的全向平台被提出 [5]，它採用全方向性輪子，如 圖 1-1，這種 輪子同時擁有橫向及縱向相互垂直的輪子，可以透過輪子間合力分力的關 係，決定任何移動方向，比起差動輪移動平台，全向輪移動平台有更好的 移動性，其優點為：(1)改變平台方向時，輪軸不需移動。(2)可在原地旋轉 平台，不需旋轉半徑。(3)可同時自旋與曲線運動。此類的移動平台，已被 廣泛的討論[6-11]。 目前常見的全向性移動平台，大多以三軸到五軸的平台為主，在不同 的考量下，這三類平台各有本身的優缺點 [12]，三軸在以 60 度為倍數的移 動角度上，可以達到較好的移動效率，四軸則是在90 度為倍數的角度上， 能獲得遠大於其他兩者的效果，至於五軸的效能，則是在各種角度效能都 相當平均，此外，在負重能力上，軸數越多負重能力就相對的越好，本論 文中是採用四軸全向平台，對於直線路徑，移動速率與節省消耗功率都有 所幫助。 2、移動平台自我定位研究與發展 在機器人自動導航研究領域中，大致可分為四類 : 地圖建制(map building)、路徑規劃(path planning)、自我定位(self-localization)以及防障碰 撞(obstacle avoidance)。 其中自我定位又可分成未知環境或已知環境之下的定位問題，本論文 所探討的是在已知環境之下如何定位。在此條件之下可再細分為相對位置 (relative position)與絕對位置(absolute position)之研究。
(1)相對位置之研究背景
自主式移動平台，必須透過感測器(sensor)告知外界資訊，才能有效定 位，一般輪型機器人測量距離均使用輪式編碼器(wheel encoder)，而最常被 使用的為遞增型光學編碼器(incremental optical encoder)，然而encoder由於
各種因素造成輪子與地面接觸不完全或打滑而產生累積誤差，在此研究領 域中，大部分均把平台系統之動態特性或打滑現象忽略，而在此系統外做 閉迴路控制，C.C. Tsai et al.[13]的系統使用三軸全向輪平台，encoder回 授，各軸使用比例積分控制器(PI controller)將動態系統包含在內，驗證了 全向平台在直線與曲線行走上的可行性，其行走速度為 3 cm/s。L. Huang et al.[14]是使用四軸全向輪平台，encoder回授，同樣使用PI控制器，但速 度上快許多(85cm/s)，同樣在直線與圓形行走上有不錯的效果，兩者之直線 行走(100cm)或圓形行走(半徑 2cm[13]與 20cm[14])位置誤差皆在 5cm內。 而在感測方位上，常使用陀螺儀(gyroscope)[15]、加速計(accelerometer) 或電子羅盤(compass)[16]，其中陀螺儀與加速計為慣性導航元件，其特性 為不需外界給予任何的輔助資訊，憑藉本身的感測資訊，即能達到導航目 的。陀螺儀在導引系統所扮演的角色，是感測角度的改變量(即各軸的角速 度)，提供物體的姿態與方位資訊；加速計則是提供系統運動過程中，各個 方向的加速度數據，經由數學運算後，可求得系統之速度以及移動距離。 然而陀螺儀與加速計在積分之後仍會有小幅度的累積誤差的問題，所以在 使用上，還需做定期校正。compass則是純感測方位變化，是屬於絕對方位 定向的儀器，沒有累積誤差問題，但必須預防週遭電磁波干擾，本論文使 用compass作為方位感測元件。 (2)絕對位置之研究背景
對於室內(indoor)環境定位，D. Hahnel et al.[17]使用雷射(laser)與兩組 RFID天線，利用裝於室內RFID tag，建立描述此空間之幾何架構，輔助雷 射感測器做範圍搜尋，估測機器人的位置則是配合RFID結合Monte Carlo演
算法。實驗結果定位誤差約1m內。Weiguo Lin et al. [18-19]使用兩種方式
定位，首先利用四個ID tag為一組定位裝置，利用機器人偵測到四個ID tag
WEB camera與ID tag，將tag與供影像辨識的card平行貼於天花板，RFID讀 取器(reader)裝於機器人上，且朝上偵測，當讀取器讀到識別碼時，利用影 像辨識到card與tag的中間點，做定位校正，實驗結果定位誤差約為 3cm 內。
1.3 問題陳述
1、全向平台多軸運動控制之誤差問題 誤差的產生有許多的原因，在全向性移動平台上，藉由四軸相互作用 產生的合力分力來形成運動路徑，但各軸為獨立驅動，特性上有所差異， 容易造成運動時的誤差。另外，由於運動慣性的關係，平台在加減速時， 因靜摩擦與動摩擦的不同，使得速度會突然加快或減慢，造成打滑現象與 encoder 錯誤的方位推算(dead-reckoning)，進而影響位置準確性。再者，對 於較多變的運動路徑而言，各軸的運動是多樣的，如何設計一個適於平台 的控制器也將是一研究課題。上述皆為誤差產生的原因之一，本論文的運 動控制研究重點在於： (1)如何實現平台的多軸運動控制技術。 (2)探討因運動慣性所引起的打滑誤差如何減緩。 (3)當平台因打滑產生方位誤差時，如何適時修正。 2、絕對定位RFID之位置誤差問題 利用RFID量測距離時，由於不具方向性，造成定位上的困難，通常需 輔助多個RFID的tag，利用相互之間感應到的時間差、或同時偵測到的ID所 產生的相對關係，來彌補方向性的問題，這在成本、安裝或演算上都較為 麻煩，若能只使用單一個tag就能達成定位，將可減少許多人事上的負擔。[19]中提到使用單一tag結合影像處理技術是可以達成需求的，但缺點在於 光源一旦有變化，將會大幅影響定位的準確性。另外，在偵測訊號方面， 在 [18]實驗中(圖 1-2)顯示RFID讀取器並非理想圓形接收範圍，最大接收 範圍直徑(Dmax)約 110cm，而最小接收範圍直徑(Dmin)約 80cm，計算定位點 最大偏差(ΔD)約 30cm，可表示成： ΔD = Dmax - Dmin ( 1-1 ) 此現象普遍存在於 RFID 系統上，若能減少 ΔD，對於 RFID 定位之精 確性，就有提升的效果。本論文的 RFID 絕對定位研究重點在於如何僅單 一 RFID 標籤作為定位點，即可完成絕對位置定位，並減少 ΔD 來提升絕 對定位之準確性。 圖 1-2 RFID reader有效偵測範圍(單位:cm)[18]
1.4 研究方法與步驟
1、全向平台之多軸運動控制(1) 首先需建立硬體電路，將數位訊號處理器(Digital Signal Processor， DSP)與驅動器(driver)、encoder連接，完成基本的閉迴路(closed-loop) 控制系統。藉由encoder回授訊號，推算出各軸之間的轉速與位移量， 並導出馬達轉速與驅動器控制訊號間的關係式，做為閉迴路控制時之
(2) 針對各軸之間的差異性，先在無載的情況下研究各軸對於命令的追跡 (tracking)能力，並分析其誤差與特性，進一步於有載時利用前述之結 論，設計各軸皆可適用之控制器，以降低因軸與軸間不匹配所產生的 誤差問題。 (3) 僅依各軸獨立控制並無法得知平台產生的方位、距離誤差，為此先導 出平台運動模型，將軸與平台關係相連，再設計平台控制器，並比較 不同路徑下的誤差追跡能力。 (4) 運動慣性而造成的誤差方面，首先以步階(unit step)式速度命令進行實 驗，而此種的加速是呈現脈衝(impulse)形式，最容易造成平台瞬間打 滑。理想的運動速度規劃應是具有加減速的，以提升對運動路徑的追 跡能力，而最常使用的是梯形(trapezoid)速度規劃，其為等加速度運 動，將可改善步階式的缺點，然而此加速度是片段常數而非連續曲 線，在加速度不連續處可能有抖動的現象，故將試著使用S形速度規 劃，其為變加速度，可減少梯形速度規劃不連續處的缺失。本論文將 從上述三種速度的規劃方式，觀察其加減速時之誤差改善情形。 (5) 當平台打滑時，將造成encoder錯誤的方位推算，本論文將藉由感測融 合(sensor fusion)方式，建立動態調整機制，輔助修正方位推算的結 果。 2、RFID絕對定位之誤差 RFID主要因為訊號範圍非理想造成定位誤差，在HOLTEK公司RFID使 用手冊 [20]中表示，RFID reader功率大小與有效感測距離成正比，此也間
接指出定位最大偏差(ΔD)也將縮小，我們將利用此一特性作絕對定位提升 精度的方法。先利用compass所回授之角度值，修正encoder的誤差並計算 平台之絕對方向，在tag周圍策略性移動修正累積誤差，並配合調變reader 有效接收範圍提升絕對定位之精確性。
1.5 論文架構
本論文共分五章，第一章旨在簡述研究的動機、目的、背景與發展， 並陳述所要研究的問題、方法、步驟。第二章介紹全向性移動平台的定義 與運動模型推導，並說明整體系統架構，包含使用的硬體元件規格、連接 方式、感測元件之原理、特性，以及應用於此平台之程式架構。第三章描 述全向平台的多軸運動控制實驗結果，分析各軸之間特性差異與設計合適 的軸控制器、平台控制器，並探討多種運動路徑之下，控制器追跡能力， 與速度規劃對於運動慣性所產生的誤差有何影響。第四章則是感測融合與 RFID 定位，利用動態融合機制改善路徑追跡的效果。RFID 定位以分段式 移動策略、功率調變等方式，作為絕對定位之方法，並量測其成效。最後 第五章中會對本論文做一個結論，並提出未來可再改進發展之方向。第二章 平台運動模型與系統架構
2 為了便於分析平台特性與設計控制技術，在此先定義全向性移動平台 其座標系統、符號，並推導其運動模型。而在得到運動模型之後，進一步 發展平台所需之控制法則，使平台能達到任意平面方向、角度移動。另 外，本章也就實驗之硬體系統架構、軟體控制方式作通盤性的介紹。2.1 平台運動模型
圖 2-1 四輪全向性移動平台座標與符號 1、全向性移動平台座標系統與符號定義 平台建模定義符號如下：XW、YW、OW：世界座標(world coordinate)的X軸、Y軸與原點
XM、YM、OM：平台座標(mobile coordinate)的X軸、Y軸與原點
W2：平台座標第二象限的全向輪(wheel B) W3：平台座標第三象限的全向輪(wheel C) W4：平台座標第四象限的全向輪(wheel D) +D1 ~ +D4：定義W1 ~W4之正轉方向 1 α ~α ：X4 M與W1 ~W4之間的角度 F：平台移動方向 θ：平台移動方向與平台X軸的夾角 φ ：XM與XW的夾角，平台逆時針旋轉為正 L：輪子與平台中心距離 r：全向輪半徑 1 ~ 4 ω ω ：全向輪W1~W4的旋轉角速度 2、建立全向性移動平台運動模型 平台的運動模型定義如 圖 2-1，各輪軸與平台座標固定差α ~1 α 的角度4 關係，可表示成下面( 2-1 )通式：
[
sin - cos]
M i=1,2,3,4i i i M X r L Y ω α α φ • • • ⎡ ⎤ ⎢ ⎥ = − ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ( 2-1 ) 而 座 標 之 間 關 係 則 透 過 座 標 轉 換 矩 陣(coordinate transformation matrix)CT： W M T M W X X C Y Y • • • • ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ( 2-2 ) 其中 cos sin sin cos T C φ φ φ φ ⎛ ⎞ = ⎜− ⎟ ⎝ ⎠ ( 2-3 )
由(2-1 ~ 2-3)式可整合成： 1 1 2 2 3 3 4 4 ( ) W W v r X v r De Y v r v r ω ω φ ω φ ω • • • ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢≡ ⎥= ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ( 2-4 ) 其中De( )φ 稱為解耦合(decoupled)矩陣 1 1
(sin cos ) (sin cos ) L
2 2
1 1
( sin cos ) (sin cos ) L
2 2
( )
1 1
( sin cos ) ( sin cos ) L
2 2
1 1
(sin cos ) ( sin cos ) L
2 2 De φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ ⎡ + − − ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ − + + − ⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ − − − + − ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ − − − − ⎥ ⎢ ⎥ ⎣ ⎦ ( 2-5 ) 從各軸反推世界座標可寫成： 1 2 1 3 4 ( ) W W v X v Y De v v φ φ • • − • ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦ ( 2-6 ) 其中 1( ) De− φ 稱為逆運動(inverse kinematics)矩陣 1 2 2 2 2
(sin cos ) ( sin cos ) ( sin cos ) (sin cos )
4 4 4 4
2 2 2 2
( ) (sin cos ) (sin cos ) ( sin cos ) ( sin cos )
4 4 4 4 1 1 1 4 4 4 De L L L φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ φ − + − + − − − = − + − + − − − − − 1 4L ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ − ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ( 2-7 )
2.2 硬體感測架構
近年來由於超大型積體電路(VLSI)技術的不斷演進，使得DSP的運算 速度及周邊處理功能，均有大幅的改善，在各種應用場合如行動通訊、資 訊家電、馬達驅動等領域都廣泛的使用。因此本論文採用由德州儀器 (Texas Instrument，TI)研發的TMS320 F2812 DSP(簡稱F2812)作為控制核 心，下 圖 2-2 即為硬體系統架構示意圖，圖 2-3 為平台實體圖。在 2.2.1 節先介紹有關F2812 的功能，2.2.2~2.2.5 則是說明周邊電路設計與感測原 理。 圖 2-2 硬體系統架構示意圖圖 2-3 平台實體圖 2.2.1 eZdspTM F2812 DSK 之功能介紹 應用DSP 於運動控制系統，主要就是希望充分利用 DSP 強大的數學運 算能力，而以最少的硬體周邊元件來實現馬達控制所需的功能。德州儀器 公司所生產的 DSP 中，’C2000 系列的硬體發展平台是專為控制應用最佳 化而設計的，其中本論文所使用的 F2812 晶片是此系列中目前最高等級的 控制器，其特點在於擁有150 MHz(6.67 ns cycle time)的快速處理能力，比 TI 早期出產的 DSP ‘C240 快 7 倍，比傳統 8051 單晶片快約 150 倍。在核 心部分，算數邏輯單元(ALU)、累積器(ACC)均採用 32 位元定點運算，為 了提升數位訊號運算效能，以硬體方式實現乘法器、乘積位移器，可在一 個指令週期(instruction cycle)內完成乘加運算。在整數計算方面，為減少數 值計算所衍生如溢位(overflow)等問題，也採用硬體式的輸出倍率位移器來
提高軟體執行的精確度。
記憶體容量方面，F2812(on chip)主要擁有 128K*16 Flash EEPROM、
兩組4K*16 Single-Access RAM(SARAM)、一組 8K*16 SARAM，並採用哈
佛匯流排(Harvard bus)架構。較舊型的 DSP 大部分將程式、資料、I/O 記憶 體獨自分離(定址位址重複)，但 F2812 卻走向單獨的記憶體空間，裡頭包 含上述三種記憶體，使用更為有彈性，也可減少不同記憶體之間般動資料 的指令集。
Spectrum Digital 公司則利用 F2812 晶片結合自行開發之周邊模組，發 展出eZdspTM F2812 初學板(DSP Started Kit，DSK)，其週邊主要有 16 個通
道的 12 位元類比數位轉換器(ADC)，串列傳輸支援了四種常用的型式：
SPI (Serial Peripheral Interface) 、 SCIs (Two Serial Communications Interface) 、 eCAN (Enhanced Controller Area Network) 、 McBSP (Multi-channel Buffered Serial Port)，最高可支援 56 個 GPIO(General-Purpose I/O)。
因 DSK 板是專用於開發初期的系統，欲達到可控制全向性移動平台的
目標，必須外加數位類比轉換器(Digital-Analog Converter，DAC)、擷取 encoder 訊號、compass 通訊以及 RFID 通訊等介面。
2.2.2 DAC 介面 本論文採用Burr-Brown公司出產專用於馬達控制用途之DAC 7625，內 部功能方塊圖如 圖 2-4 所示，主要特色如下： ¾ 可選擇單極性(unipolar)與雙極性(bipolar)運作模式。 ¾ 穩定時間約為 10 us。 ¾ 12 位元的解析度。 ¾ 數位資料並列輸入。
¾ 支援 4 通道(channel)且同步輸出。
圖 2-4 DAC 7625 內部功能方塊圖
利用選擇通道接腳 A1、A0，依序將數位值由 DB11~DB0 寫入，四軸
的數位值會存放於各自的 input register 中，待 LDAC 控制接腳變為 Low
時，才一次同時輸出，並轉換為類比訊號於電壓輸出端(Vout)：
( )
4096
REFH REFL value
out REFL V V D V =V + − ( 2-8 ) 其中VREFH與VREFL各代表參考電壓的上下限值，設為+2.5V與 0V， Dvalue則是輸入的數位值。由於驅動器操作在0V~+5V之間，而DAC 7625 的 VREFH僅能達到+2.5V(亦為輸出電壓上限)，為此，Vout必須增加兩倍放大電 路之後，才能匹配驅動器之需求，單軸DAC介面電路連接如 圖 2-5 所示。
圖 2-5 單軸 DAC 介面電路 2.2.3 Encoder 介面 1、Encoder 原理 圖 2-6 左側為一般遞增型 [21]輪式編碼器，其特色為將圓等分成數千 甚至上萬黑白間隔，當輪子轉動時，利用光在相異顏色下折射效果不同或 有無穿透等原理，造成電子訊號反覆ON-OFF而產生方波，此又稱為方形 編碼器脈波( Quadrature Encoder Pulse，QEP )。
圖 2-6 輪式編碼器原理圖
11、10)，除了可提升四倍的解析度之外，也可藉由四種邏輯訊號出現的先 後順序得知輪軸轉動方向(Direction)。 圖 2-7 QEP 訊號示意圖 2、Encoder 介面電路 如 圖 2-2 所示，encoder會將A、B相訊號傳給負責QEP計數的HCTL 2032(簡稱 2032)，藉由已知encoder一轉能產生的QEP個數(EPulse)，以及 2032 計數值(K)，可推算出輪軸轉動角度ωangle (rad.)，關係式為： 2 angle Pulse K E π ω = ( 2-9 ) 2032 主要特色如下： ¾ 最高 33 MHz 的時脈頻率。 ¾ 支援雙軸(Dual Axis)同時計數。 ¾ 支援 32 位元上下計數。 ¾ 高雜訊免疫性：施密特(Schmitt)觸發器、數位雜訊濾波器。 ¾ 8 位元並列式計數值輸出。 本論文平台為四軸運動控制，故需要兩組 2032 作為輪軸轉動之計數 器。又因輸出為5V訊號，必須轉換成F2812 可接受之 3.3V，故銜接位準轉 換器(transceiver) LVTH 245，圖 2-8 為雙軸encoder介面電路。
圖 2-8 雙軸 encoder 介面電路 3、Encoder 應用方式 由於使用的encoder解析度為 1000 pulse/cycle，配合A、B相位可提升解 析度為4000 pulse/cycle，全向輪則如 圖 2-9，輪半徑(r)為 4 cm，圓周長約 為 25.13 cm ， 如 此 可 求 得 輪 子 轉 動 時 cm 與pulse 之 間 的 關 係 約 為 1 ： 159.155，藉由此對照關係，方便以常用之單位cm來下達路徑控制命令。 圖 2-9 本文使用全向輪 [22] 回授 encoder 訊號時，速度迴路之取樣週期為 10 ms，位置迴路則為 100 ms，利用各軸 2032 所累積之計數值作為平台位置之參考，每 10 ms 的 差分計數值作為各輪速度之依據。
2.2.4 Compass 介面 1、Compass 原理 Compass係利用感應磁場(地磁北極)大小與方向，進而推算出角度變 化，如下 圖 2-10 為本文所使用之二維電子羅盤，方位主要是倚靠正交線 圈(X、Y-sensor)上的電磁效應所產生的磁通密度(Tesla，T)而定。 圖 2-10 二維的電子羅盤 [23] 由 於 感 應 來 源 為 地 磁 場 ， 所 以 在 不 同 地 方 的 磁 偏 角(Magnetic Declination)、地磁密度均有所差異，電子羅盤必須經過校正(calibration)程 序之後才能正確使用。圖 2-11 為compass旋轉一圈所偵測到的磁通密度， 座標軸單位為一計數值(counts)，並與取樣週期有二的乘冪關係 [23]，本文 之計數值與磁通密度換算關係為： 4 counts T μ = ( 2-10 ) 未校正時(圖 2-11(a))，因為地磁與compass感應磁場的分量會引起磁通 密度改變，故呈現橢圓形且圓心偏移(offset)。
(a) without calibration (b) with calibration 圖 2-11 各角度感測之磁通密度 [24]
最常使用的校正方法是將compass等速旋轉 1~2 圈後，可得X、Y軸的
最大與最小計數值( Xmax、Ymax、Xmin 、Ymin)，並經由下列步驟校正， 圖
2-12 為此步驟之流程圖。
compass 偵測到未校正過的 Xraw、Yraw計數值，利用下列四式求得未校
正前圓心偏移量(Xoffset、Yoffset)以及橢圓形的長短軸(Xrange、Yrange)。
max min 2 offset X X X = + ( 2-11 ) max min 2 offset Y Y Y = + ( 2-12 ) max min range X = X −X ( 2-13 ) max min range Y =Y −Y ( 2-14 ) 去除偏移量，再以長軸為準，將兩軸正規化。
value raw offset
X = X −X ( 2-15 )
value raw offset
Y =Y −Y ( 2-16 )
range
value value range range
range
X
Y Y when X Y
Y
range
value value range range
range Y X X when X Y X = ⋅ < ( 2-18 ) 已知XY 正規化值，代入三角反函數，求相對角度值 Angle。 1 180
(deg.) tan ( value )
value Y Angle X π − = ( 2-19 ) 利用XY 的正負號決定絕對角度值(heading)。
Xvalue Yvalue Heading
+ + Angle - + 180 - Angle - - 180 + Angle + - 360 - Angel 下圖 declination 即為是否考量磁偏角的問題，若使用者只需使用地磁 北極，則可略過此一步驟，但若需使用地理北極時，只需將算出之 heading
加上磁偏角即可，各地磁偏角可藉由 NGDC (National Geophysical Data
Center)網站查詢。
校正之後，會如 圖 2-11(b)呈現圓形且圓心位於座標原點，稱為磁圓 (Magnetic circle)。compass的量測除了受地磁與磁偏角影響外，很容易受周 圍電磁波影響，如平台系統所產生之電流、週遭電器用品、鐵製物品等。 這些電磁干擾會產生雜訊，不定向的附加於磁圓之上，易造成誤判，故通 常會加入濾波器(filter)等機制降低干擾，提升compass訊號的可靠度。 2、Compass 特性分析 完成初始化校正之後，藉由標準差(standard deviation，σ )測試所使用 的 compass 特性，σ 為描述一組數據自平均分散開的變異性(variability)， 分析結果將做為絕對方位定向之參考。標準差計算方式如(2-20)、(2-21)所 示： 1 1 N mean k k N μ μ = =
∑
( 2-20 ) 2 1 1 ( ) N k mean k N σ μ μ = =∑
− ( 2-21 ) 其中， k μ ： 第 k 筆取樣到的值 mean μ ： 總取樣數的平均值 N ： 總取樣次數 σ ： 標準差值 (1)Compass 靜態分析 將compass定於平台上並朝一固定方向連續取樣 3000 筆角度值，取樣 頻 率 為 25 Hz ， 如 下 圖 2-13 藉 由 標 準 差 的 統 計 方 式 可 求 得μmean為 92.5607D、σ 為0.3061D。圖 2-13 compass 之靜態分析 (2)Compass 動態分析 藉由平台直線行走與原地旋轉兩種路徑測試compass之動態情形，第一 種路徑量測角度的一致性，圖 2-14 為平台直線行走 100 cm、15 cm/s等速 度運動下的角度回授，首先以一秒時間量測其靜態誤差，在動態起步時， 由 於 採 步 階 式 運 動 ， 導 致 瞬 間 角 度 陡 變 。 其 動 態 時 平 均 角 度μmean 為 182.2482D、σ 為0.7387D。 圖 2-14 平台直線行走之 compass 回授情形 第二種測試路徑為原地旋轉 360 度、20 deg/s等角速度運動，圖 2-15 為各個角度的動態回授誤差，經過校正後的compass同心圓對稱於理想
(ideal)值，如 表 2-1，整體的角度平均誤差μmean為−0.00493
D、σ 為1.6098D。
圖 2-15 平台原地旋轉之 compass 回授誤差情形 表 2-1 平台原地旋轉之 compass 平均誤差與標準差
No. Turns 1 2 3 4 5 Ave.
mean μ -0.00453° -0.00757° 0.000419° -0.00651° -0.00649° -0.00493° σ 1.7114° 1.7073° 1.5628° 1.5653° 1.5023° 1.6098° 3、Compass 應用方式 綜合特性分析之結果，使用compass訊號於平台運動時，若經濾波器或 感測融合等處理方式，其回授資訊將有更可靠的參考價值。如 圖 2-16 為 平台動態特性的頻譜分析結果，首先設計一截止頻率於 0.3Hz之低通濾波 器(Low-Pass Filter，LPF)，如 圖 2-17 改善其穩定性(因靜態時可靠性極
高，故低通濾波器初始值將直接引用靜態量測值)，但其與encoder訊號相比 (圖 2-18)仍有其缺失，故再利用動態調整的方式(詳見 4.1 節)融合encoder 與compass之資訊，提高回授準確性。
圖 2-16 電子羅盤頻譜圖
圖 2-18 改善後 compass 穩定性與 encoder 比較
4、Compass 介面電路
本論文所使用之compass 為 PNI 公司出產的 V2Xe，主要特色如下：
¾ 0.01D的高解析度。 ¾ 2DRMS 的高精確度。 ¾ 多重感測功能(方向與磁場大小)。 ¾ 大範圍的磁場量測能力(±1100 Tμ )。 ¾ 採非揮發記憶(可儲存校正值與功能設定)。 ¾ SPI 傳輸介面。 由於V2Xe與F2812 均有SPI傳輸介面，如 圖 2-19 可透過簡單的連接方 式即可完成資料的傳遞。 圖 2-19 V2Xe 與 F2812 介面電路
SPI是一種全雙工(full duplex)同步(synchronous)串列資料的傳輸協定， 裝置間為主僕式(master-slave)關係，通常會有兩條資料傳輸線，兩條訊號 控 制 線 ； 資 料 傳 輸 線 包 含MOSI(Master data Out / Slave data In) 、 MISO(Master data In / Slave data Out)，而訊號控制線包含由Master發出的同
步時脈(CLK)、slave選擇線(SS)。如 圖 2-20 為少了SS的SPI連接圖，主僕
雙方各自有一移位暫存器，會依序從最高有效位元(MSB)送出位元訊號， 同時從最低有效位元(LSB)接收位元訊號，master端負責發送同步訊號，故 還須一個時脈產生裝置(SPI clock generator)。
圖 2-20 SPI master-slave 連接圖
通訊協定只須連接完成，並保證主僕雙方的時脈取樣(sample)時機相同即 可，取樣時機的選擇如 表 2-2 所示，V2Xe採用Rising edge with delay格式， 實際與F2812 通訊之時序如 圖 2-21 所示。
圖 2-21 V2Xe 與 F2812 通訊時序圖 2.2.5 RFID 介面 1、RFID 原理[25-26] RFID是一種非接觸的自動識別技術，包含reader、tag和天線(antenna) 等部分，reader發送RF時所使用的頻率稱為RFID系統的工作頻率，由於也 是屬於無線電的應用範疇，不能干擾到其他領域應用的正常工作，因此 ISM(Industrial Science Medical)通常採用局部的無線頻段，而RFID也將使用 ISM頻段，基本分成如 表 2-3。
表 2-3 RFID 常用頻段
KHz，在此頻段下，reader與tag之間主要是利用電磁耦合技術達成識別動 作，如 圖 2-22 為RFID工作原理示意圖，圖(a)若天線為圓形設計時，一般 可感應範圍(reaction range)近似圓球狀，虛線箭頭為天線的磁場強度(H)變 化，磁場強度會隨天線與tag線圈(coil)的垂直距離(z)增加而減弱，tag需足 夠的磁場強度使其產生感應電壓造成反磁場，進而完成系統耦合。而讀取 範圍的條件，即reader發出能夠驅使tag產生反磁場的最小磁場強度(Hmin)， 如圖(b)，有效讀取之垂直距離約 11 cm。 (a) (b) 圖 2-22 RFID 原理示意圖 2、RFID 介面電路 低頻段的RFID 系統，具有以下特點： ¾ 較強的穿透能力(水、金屬、動物等導體材料)。 ¾ 近接(proximity)型的有效感應距離(通常約十幾公分)。 ¾ 與更高頻段相比，指向性弱，感應範圍是以面而非點為主。 ¾ 低訊號雜訊比(S/N)，易受干擾。 受限於感應距離，本論文將reader放置在平台下方離地約 4 cm處，而 tag則平放於地面，雖然指向性弱，但球體狀的感應範圍更適合本論文在搜 尋定位點上之研究。表 2-4 為採用的reader規格，由於易受干擾，所以在
輸出上選擇檢誤能力較佳的Magstripe格式(如 圖 2-23)，配合奇同位(odd parity)檢誤與水平冗長檢誤(Longitudinal Redundancy Check，LRC)，能找出 並訂正一位元的錯誤。 表 2-4 RFID reader 規格 圖 2-23 Magstripe 通訊格式 由於之後的實驗需調整reader功率來達成絕對定位之研究，故在reader 的電源部分多加功率調整電路，如 圖 2-24，其中電壓輸出(Vout)可利用下 式求得。藉由設計多組開關(NPN9013)來達到不同的Requ值，進而完成 reader功率調整之目的。 1 1.25( equ 1) out R V R = + ( 2-22 )
圖 2-24 RFID reader 模組與 F2812 介面電路
2.3 程式控制架構
如下 圖 2-25，主程式可分為三個區段：命令產生、平台控制迴路、各 軸 控 制 迴 路 ； 而 每 個 區 段 內 又 可 分 成 數 個 區 塊 式 的 程 式 片 段 ， 如 圖 2-26、圖 2-28、圖 2-29。 圖 2-25 主程式流程大綱 2.3.1 命令產生流程 在命令產生區段(圖 2-26)中，可大致分成參數設定(parameter)、檢查輸 入的參數是否超過限制(check limit)、規劃路徑與運動速度(velocity & path schedule)、最後產生位置的命令(position generator)。圖 2-26 命令產生流程 參數設定可分成移動路徑的選擇與平台速度規劃的調整；以本論文為 例，路徑的選擇可以是任意直線、自旋、S 曲線等，此時必須決定起點與 終點的直角座標參數、自旋的角度、曲線的半徑等參數；而在速度規劃方 面則要決定使用步階、梯形還是 S 形的速規方法，並決定平台最高運動速 度、等加速時間、變加速時間等。 由於全向性移動平台在運動時並非輪子轉動方向即平台行進方向，這 與一般差動式平台有所不同，故須計算各軸是否均能承擔其輸入的參數要 求，此部分即由 check limit 負責，若有單軸產生無法負荷的警告時，平台 自動告知使用者，要求修正參數。 速度規劃的用意在於讓平台運動軌跡更為平滑(smooth)，除了減少對於 機構的傷害之外，也能減少因運動慣性所產生的打滑現象。如下 圖 2-27 為梯形與S形的速度規劃示意圖。 1、梯形速度規劃 梯形速規的已知條件為：平均速度(Vave)、平均角速度(φave • )、等加速時 間(Ta)、距離(d)與角位移(φ )，設 1 3 a T = T且加減速時間相等，可求出總時間 (T)、最大速度(Vmax)、最大角速度(φmax • )： d ave T V = or ave T φ φ• = ( 2-23 )
max a d T - T V = and max a T - T φ φ• = ( 2-24 ) 三段速度(或角速度)可各別表示成： max 0~ ( ) a t T a t V t V T = = ( 2-25 ) max ~( ) ( ) a a t T T T V t =V = − ( 2-26 ) max ~ ( ) ( ) a t T T T a T t V t V T = − − = ( 2-27 ) 2、S 形速度規劃 S 形速規的已知條件與梯形相似，僅多了變加速度的時間(Ts)參數，設 1 2 s a T = T 、 1 3 a T = T且加減速時間相等之下： 2 0~ ( ) s t T V t =Kt = ( 2-28 ) 2 max ~ ( ) ( ) s a a t T T V t =V −K t T− = ( 2-29 ) max ~ ( ) a a t T T T V t =V = − ( 2-30 ) 2 max ~ ( ) ( ) a s a t T T T T V t =V −K T − −T t = − − ( 2-31 ) 2 ~ ( ) ( ) s t T T T V t =K T−t = − ( 2-32 ) 其中在此條件下 2max2 s V K T = 。最後再將規劃出的速度積分成位置，作為平 台之位置命令，而路徑的給定方式則在3.2 節說明。
圖 2-27 梯形與 S 形速度規劃示意圖 2.3.2 平台控制迴路 平台控制迴路主要分成平台控制器(platform controller)、平台運動模型 的解耦合(decoupled)與逆轉換(inverse)、感測融合四部分。 平台控制器定義於其輸入與輸出僅與XW 、YW、φ 有關，換言之，可以 是平台的位置或速度控制器，而 圖 2-28 是以位置控制表示其迴路。運動 模型部分即把2.1 節所建立之運算矩陣編於其中。感測融合部分於 4.1 節介 紹。 圖 2-28 平台控制迴路
2.3.3 各軸控制迴路 如 圖 2-29 為各軸控制架構，本論文之各軸僅作速度迴路，而位置迴 路則整合到平台控制器實現，在軸控制器(axis controller)之後還需經過轉速 與電壓關係的轉換，以及將類比電壓轉換成對應之數位值輸出。軸控制器 的設計方式與結果則在3.1 節說明。 圖 2-29 各軸控制迴路
第三章 全向平台之多軸運動控制
3 本章描述相對定位研究的實驗結果，分析各軸之間特性差異與設計合 適的軸控制器、平台控制器，比較在不同運動下之路徑追跡效果，以及不 同速度規劃下對於運動慣性所產生的誤差有何影響，並探討利用上述幾種 方式，對於提升平台最終位置正確性的效果。3.1 各軸 PID 控制器之實驗結果
3.1.1 各軸特性分析 本論文希望以單一組PID控制器套用於四軸，簡化平台多軸控制器設計 的複雜度，故先分析各軸的特性，首先利用步階命令測試其時間響應的性 能，如下 圖 3-1、圖 3-2 為無載時正(positive)反(negative)轉的速度特性，在 上升時間(rise time)、安定時間(settling time)、超越量(overshoot)上都有極為 相似之特性，而穩態誤差(steady-state error)則相差約±1 ~ 2 pulse左右。圖 3-2 無載時四軸反轉之步階響應圖
另外，給定各軸一梯形速度命令(最高速度為 35 pulse/10ms)來觀察其各 軸的追跡能力，如下 圖 3-3，加速時間(acceleration time，A)、等速水平時 間(horizontal time，H)、減速時間(deceleration time，D)各佔整體運動時間的 三分之一。評量標準則是利用速度的絕對誤差累積值(Integral of Absolute Error，IAE)與速度IAE平均誤差百分比，如( 3-1 )、( 3-2 )式。
1 N k k IAE e = =
∑
( 3-1 )IAE 平均誤差 (%) k ave k=A,B,C,D-axis
ave IAE IAE IAE − = ， ( 3-2 ) 表 3-1 與 表 3-2 說明在正反轉上，各軸之間的速度IAE與平均速度 IAE差距皆在百分之五以內，顯示各軸對於相同條件之下所產生的特性是 類似的。綜合正反轉的比較結果，各軸之間在同方向運動特性上較為相 近，而在正反轉間差異較大，這使得設計PID控制器時，需考量到正反轉 可能有不同的最佳PID參數。 表 3-1 無載時各軸正轉之 IAE 與平均誤差百分比
Ave. for 4-Axis Stage A-Axis B-Axis C-Axis D-Axis
Max%-Min% (for A~D-Axis) IAE 465.0061 471.684 459.9955 469.8861 466.642924
A
Compare with Ave. (%) -0.35 1.08 -1.42 0.69 2.5047 IAE 206.0091 207.4695 200.5861 199.2991 203.340955 H
Compare with Ave. (%) 1.31 2.03 -1.35 -1.98 4.0180 IAE 288.141 283.5807 293.4371 294.2555 289.8535625 D
Compare with Ave. (%) -0.59 -2.16 1.23 1.51 3.6828
表 3-2 無載時各軸反轉之 IAE 與平均誤差百分比
Ave. for 4-Axis Stage A-Axis B-Axis C-Axis D-Axis
Max%-Min% (for A~D-Axis) IAE 459.9134 471.9955 463.8314 481.2524 469.248164
A
Compare with Ave. (%) -1.98 0.58 -1.15 2.55 4.5474 IAE 215.9682 206.3883 213.7027 214.4018 212.61525 H
Compare with Ave. (%) 1.57 -2.92 0.51 0.84 4.5057 IAE 297.878 299.7045 304.5647 306.5752 302.1805745 D
3.1.2 設計各軸 PID 控制器 當有載時(平台放於地面時)，各軸之間將會相互干擾，為求減少影響， 命令大小應相等，且由於正反轉之間特性有所差異，故將選取正反轉皆適 用的 PID 參數，本論文將平台直線(沿著平台座標+X 軸方向)運動作為設計 PID 參數時的路徑，因為此路徑將造成 A、B 軸正轉，C、D 軸反轉，可同 時觀察 PID 參數對於正反轉之間差異性，且此時四軸命令大小均相等，加
減速與等速時間各佔 2.22 sec，命令的最高速度為 25 pulse/10ms。其 PID
調整實驗過程如下： (1) Kp值已先選定於 1.1。在無平台回授控制，僅各軸獨立速度控制之 下調整Ki參數，結果如下 圖 3-4 與 表 3-3。在沒有Ki情況之下(圖 3-4(a))，各取樣週期所產生的誤差無法獲得累積，而加入Ki之後(圖 3-4(b)(c))，對於安定時間與穩態誤差獲得大幅度改善。 (2) 隨著Ki值持續增加，終將導致系統不穩定(圖 3-4(d))。由上述實驗 綜合出Kp=1.1、Ki=0.1~0.2 時，將會有較好的控制性能(表 3-3)，此 時若加入Kd參數，將有可能改善穩定性(stability)[27]。 (3) 在Kp固定，Ki=0.1、0.2 之下加入Kd值，實驗結果如下 表 3-4、表 3-5 藉由此實驗可導出下列幾項小結： 9 當 Kp、Kd 固定時，適度的增加 Ki，可減少加減速時的 IAE，但等速時的 IAE 則相對上升。 9 當Kp、Ki固定時，過大的Kd將造成IAE不減反增，尤其在 低速時更為明顯，如 圖 3-5(a)為例，給定各軸一低速(9 pulse / 10 ms)時，四軸均有不穩定情形。
9 當 Kp、Ki 固定時，適度的增加 Kd，可降低等速時的 IAE，但對於加減速時並無明顯助益。 9 當 Kp、Kd 固定時，過大的 Ki 並不會再減少加減速時的 IAE，反而微幅增加等速時的 IAE，降低了穩定性。 9 相同的PID參數，在正反轉上也會有不同的特性產生，如 圖 3-5(b)，給定四軸最高速度(25 pulse/10 ms)，反轉的C、D兩 軸對於此組PID參數有較好的適應能力，而正轉的A、B兩軸 較差，使得IAE值大幅上升。 9 Kp=1.1、Ki=0.2、Kd=2 此組參數雖在等速時的表現上較 Ki=0.1 時差(圖 3-6)，但在加減速上卻有較大幅度的改善， 故最後選擇作為各軸之PID參數。 (a) Ki=0 (b) Ki=0.1 (c) Ki=0.2 (d) Ki=0.3 圖 3-4 Kp=1.1 時調整 Ki 對於各軸之響應
表 3-3 Kp=1.1 時調整 Ki 對於加減速之平均速度 IAE Kp=1.1，Ave. for 4-Axis
Stage Ki=0 Ki=0.1 Ki=0.2 Ki=0.3
A(IAE) 1695.092 387.1163 258.8151 776.3100
H(IAE) 2977.960 224.0698 339.7849 2811.925
D(IAE) 1547.033 229.4812 278.1178 4464.106
表 3-4 Kp=1.1、Ki=0.1 時調整 Kd 對於加減速之平均速度 IAE Kp=1.1，Ki=0.1，Ave. for 4-Axis
Stage Kd=1 Kd=2 Kd=3 Kd=4 Kd=5
A(IAE) 391.5673 385.1985 390.6178 575.5784 639.125 H(IAE) 190.4445 180.3549 181.2836 195.3015 225.891 D(IAE) 219.8135 220.2887 234.2109 260.5617 324.961
表 3-5 Kp=1.1、Ki=0.2 時調整 Kd 對於加減速之平均速度 IAE Kp=1.1，Ki=0.2，Ave. for 4-Axis
Stage Kd=1 Kd=2 Kd=3 Kd=4 Kd=5 A(IAE) 230.2724 228.4599 224.0147 237.8333 615.7546 H(IAE) 230.0525 195.8021 200.0165 201.7127 241.8558 D(IAE) 196.4506 188.1394 205.7107 220.8236 396.5221 (a) (b) 圖 3-5 各軸 PID 調整時之特性說明
圖 3-6 設計 PID 參數較好的速度追跡情形
3.2 平台控制器之實驗結果
3.2.1 平台控制器設計 平台控制器係指運動命令在分於四軸前，對於Xw、Yw、 φ 的運算調 整。本論文藉由S曲線路徑(表 3-6、圖 3-10)作為設計平台的P、PD位置控 制器時之參考依據，並利用四種路徑(如 圖 3-7~圖 3-10)做為相對定位之 運動控制成效評比。平台初始位置皆設為(XW YW ) (0 0 0)φ = ，各軸控制器的 PID值定於 1.1、0.2、2，並於步階式平台命令之下實驗。 圖 3-7 直線路徑之理想位置變化圖 3-8 直線與直線加自旋路徑之理想位置變化
表 3-6 四種路徑之實驗參數
Path Behavior description
straight distance 200 cm
Straight Line
The robot's velocity 15 cm/sec
straight distance 100 cm
The robot's velocity 10 cm/sec Self-rolling angle 225 degree Straight Line
& Self-rolling
Angular velocity 22.5 degree/sec
radius of curvature 20 cm
The robot's velocity 54 degree/sec S-curvilinear
Self-rolling angle 0 degree
radius of curvature 20 cm
The robot's velocity 54 degree/sec Self-rolling angle 60 degree S-curvilinear
& Self-rolling
Angular velocity 9 degree/sec
圖 3-10 S 曲線加自旋路徑之理想位置變化 設計結果如下 表 3-7，使用位置IAE作為調整參數的評量標準，當只 有Kp時，如 圖 3-11，較小的值會拉長安定時間，造成無法追上命令，而 過大的值則會使得斜率( W W Y X + + )等於零或無限大處的超越量增加，加入Kd之 後可以再減少位置IAE值，最後控制參數決定： P 控制器時：KP(XW,YW, ) {0.5,0.5,0.5}φ = 。 PD 控制器時：KP(XW,YW, ) {0.5,0.5,0.5}φ = 、Kd(XW,YW, ) {0.3,0.3,0.3}φ = 。 (a) Kp=0.2 (b) Kp=0.9 圖 3-11 位置控制器 Kp 過小或過大時追跡情形
表 3-7 平台位置控制器Kp、Kd參數調整
IAE (cm) or (deg.) IAE (cm) or (deg.) when K =0.5 p
p
K X-Axis Y-Axis φ K d X-Axis Y-Axis φ
0.1 593.084 452.447 2.003 0.1 8.738 9.519 2.682 0.2 237.742 223.084 3.271 0.2 8.147 7.633 3.146 0.3 106.965 110.694 3.739 0.3 6.998 8.061 3.4 0.4 43.979 44.633 5.621 0.4 10.276 11.793 3.282 0.5 11.464 17.255 5.639 0.5 11.951 14.75 2.164 0.6 31.064 29.779 5.596 0.6 12.319 17.258 3.378 0.7 48.275 46.146 6.353 0.7 15.501 20.87 2.753 0.8 61.28 60.863 6.078 0.8 16.329 24.57 3.432 0.9 72.972 70.708 7.089 0.9 19.423 27.527 3.635 1 99.19 89.33 8.585 1 20.313 31.09 3.925 3.2.2 直線路徑 由 於 使 用 步 階 式 速 度 命 令 ， 故 在 直 線 路 徑 的 命 令 給 定 方 式 為 (XW YW ) n (200 0 0) N φ = ，N 表示總運動時間，n 的間隔時距取決於命令取樣 週期；當 n>N 時，命令固定於(XW YW ) (200 0 0)φ = ，而對於任意從原點出 發的直線距離(d)，最終位置可寫成極座標參數型式： (XW YW φ) ( *cos( ) = d θ d*sin( ) 0)θ ( 3-3 ) 各軸之間旋轉方式如 圖 3-12，其中A、B軸重疊，C、D軸重疊。直線 路徑之P、PD控制器追跡情形如 圖 3-13、圖 3-14。表 3-8 是位置IAE的比 較，針對直線運動方式而言，PD雖在Xw、Yw較佳，但φ 的誤差卻比P大一 些。
圖 3-12 直線路徑之各軸理想旋轉方式
(a) Xw&Yw位置變化 (b)角位置變化
(c) Xw&Yw速度變化 (d)角速度變化
(a) Xw&Yw位置變化 (b)角位置變化 (c) Xw&Yw速度變化 (d)角速度變化 圖 3-14 平台位置 PD 控制器之直線路徑追跡(encoder) 表 3-8 直線路徑時平台控制器位置 IAE 比較 Xw (cm) Yw (cm) φ (deg.) Straight Line P PD P PD P PD Mean 3.958 3.619 2.923 2.429 1.919 2.128 Position IAE Max 4.601 3.939 3.311 2.727 1.978 2.261 3.2.3 直線加自旋路徑 直 線 加 自 旋 多 了 角 度 上 的 變 化 ， 路 徑 的 產 生 方 式 為 (XW YW ) n (100 0 225) N φ = ，定義ϕ 為角位移總量，對於此路徑任意從原點出 發時，最終位置可以表示成極座標參數型式： (XW YW φ) ( *cos( ) = d θ d*sin( ) θ ϕ) ( 3-4 )
各軸之間旋轉方式如 圖 3-15，由於四軸是設計成對稱的關係，故任兩 軸的運動方式均會與另外兩軸相對稱，此對稱值為一非零值則表示平台有 角速度自旋運動，若為等角速度則對稱值固定，反之則對稱值隨角速度而 變化。直線加自旋路徑之P、PD控制器追跡情形如 圖 3-16、圖 3-17，從 兩圖的(a)可發現平台在旋轉到 90 度倍數時，均有較大且規律的追跡誤差 產生。表 3-9 是位置IAE的比較，已經可以看出PD小幅優於P控制器。 圖 3-15 直線加自旋路徑之各軸理想旋轉方式 (a) Xw&Yw位置變化 (b)角位置變化 圖 3-16 平台位置 P 控制器之直線加自旋路徑追跡(encoder)
(c) Xw&Yw速度變化 (d)角速度變化
圖 3-16 平台位置P控制器之直線加自旋路徑追跡(encoder)(續)
(a) Xw&Yw位置變化 (b)角位置變化
(c) Xw&Yw速度變化 (d)角速度變化
表 3-9 直線加自旋路徑時平台控制器位置 IAE 比較
Xw (cm) Yw (cm) φ (deg.)
Straight & Rolling
P PD P PD P PD Mean 17.942 15.199 10.543 8.707 28.123 22.584 Position IAE Max 19.589 15.810 11.574 10.470 30.495 24.633 3.2.4 S 曲線路徑 S 曲線路徑為另外一種二維變動的命令，路徑的產生方式表示成極座 標參數型式： ( ) [( cos ) ( sin ) 0] ,0 < ( ) [( cos( ) 3 ) ( sin ) 0] , 2 W W W W X Y R R R X Y R R R φ θ θ θ π φ θ π θ π θ π = − + − ≤ = − − + − ≤ < ( 3-5 ) 2n N π θ = ( 3-6 ) 其中R為S路徑的曲率半徑，N表示總運動時間，n的間隔時距取決於命 令取樣週期；當n>N時，θ固定於2π 。各軸之間旋轉方式如 圖 3-18，此四 軸轉速對稱於零，表示平台無角速度自旋運動，並依照四軸的合力方向移 動。S曲線路徑之P、PD控制器追跡情形如 圖 3-19、圖 3-20。表 3-10 是 位置IAE的比較，由於平台控制器是以此路徑作為設計之參考，故此時PD 追跡誤差優於P控制器。 圖 3-18 S 曲線路徑之各軸理想旋轉方式
(a) Xw&Yw位置變化
(b) Xw&Yw速度變化
(c)角位置變化 (d)角速度變化 圖 3-19 平台位置 P 控制器之 S 曲線路徑追跡(encoder)
(a) Xw&Yw位置變化
(b) Xw&Yw速度變化
(c)角位置變化 (d)角速度變化 圖 3-20 平台位置 PD 控制器之 S 曲線路徑追跡(encoder)
表 3-10 S 曲線路徑時平台控制器位置 IAE 比較 Xw (cm) Yw (cm) φ (deg.) S-curvilinear P PD P PD P PD Mean 14.309 7.842 18.288 8.855 4.949 3.549 Position IAE Max 15.423 8.583 21.768 9.364 5.639 4.000 3.2.5 S 曲線加自旋路徑 S 曲線加自旋路徑是三維均有變化的路徑模式，路徑的產生方式與 S 曲線相似，表示成極座標參數型式： ( ) [( cos ) ( sin ) ] ,0 < ( ) [( cos( ) 3 ) ( sin ) ] , 2 W W W W n X Y R R R N n X Y R R R N ϕ φ θ θ θ π ϕ φ θ π θ π θ π = − + − ≤ = − − + − ≤ < ( 3-7 ) 2n N π θ = ( 3-8 ) 其中ϕ 為角位移總量，R為S路徑的曲率半徑，N表示總運動時間，n的 間隔時距取決於命令取樣週期；當n>N時，θ固定於2π ，φ 固定於ϕ 。各軸 之間旋轉方式如 圖 3-21，此四軸轉速對稱於非零值，表示平台有角速度 自旋運動，並依照四軸的合力方向移動。S曲線加自旋路徑之P、PD控制器 追跡情形如 圖 3-22、圖 3-22。表 3-11 是位置IAE的比較，可明顯觀察到 在較為複雜的運動路徑時，不論P、PD控制器，追跡誤差皆大幅增加，主 要因為三維(XW、YW、φ)控制器之間並無協調之機制，而增加平台運動時 控制上的相互干擾，在此路徑下PD追跡誤差仍優於P控制器。
圖 3-21 S 曲線加自旋路徑之各軸理想旋轉方式
(a) Xw&Yw位置變化
(b) Xw&Yw速度變化
(c)角位置變化 (d)角速度變化 圖 3-22 平台位置P控制器之S曲線加自旋路徑追跡(encoder)(續)
(a) Xw&Yw位置變化
(b) Xw&Yw速度變化
(c)角位置變化 (d)角速度變化 圖 3-23 平台位置PD控制器之S曲線加自旋路徑追跡(encoder)(續)
表 3-11 S 曲線加自旋路徑時平台控制器位置 IAE 比較
Xw (cm) Yw (cm) φ (deg.)
S-curvilinear & Rolling
P PD P PD P PD Mean 37.628 19.571 57.592 39.220 69.440 52.400 Position IAE Max 41.479 20.741 64.671 41.926 77.437 55.263 3.2.6 實驗結果討論 綜合上述四種路徑的實驗結果，將 PD 與 P 控制器相比，加入 Kd值能 即時的修正誤差，縮短安定時間，並且在一維到三維運動均有較好的表 現，故選擇 PD 做為接下來速度命令規劃、感測融合等實驗的平台控制 器。
3.3 速度命令規劃對於運動慣性之影響
藉由3.2 節PD控制器、200 cm的直線路徑，以及 2.3.1 節所述之速度規 劃方式，觀察其改善效果，如 圖 3-24~圖 3-26 為步階、梯形、S形的Xw、 Yw響應，表 3-12 為每種速規五次之實驗結果，首先在最終位置方面，因 encoder與馬達同動，且為平台控制器之回授，故在打滑時，其數據無法明 顯看出差異性，但實際量測上，梯形與S形均優於步階式，改善了因脈衝式加速度產生的位置偏差。而在最終角度方面，使用compass量測，發現步階 情況下的變異性最大，主要是平台起步與止步時的抖動所造成，梯形與S形 在角度方面的穩定性也優於步階。本實驗中，由於硬體上的限制，在速度 規劃時，已設限等加速度之最大值，故使得梯形在加速度不連續處可能抖 動的情形並不明顯。 圖 3-24 步階速度規劃時 Xw、Yw響應 圖 3-25 梯形速度規劃時 Xw、Yw響應
圖 3-26 S 形速度規劃時 Xw、Yw響應
表 3-12 速度規劃對於位置與方位之影響
Unit-step Trapezoid S-curve Straight Line
Encoder Measured Encoder Measured Encoder Measured Mean 0.012 -9.940 0.004 -7.860 0.010 -8.400 |Max| 0.030 10.500 0.020 8.500 0.010 8.900 Final Position error (cm) σ 0.010 0.589 0.012 0.441 0.000 0.529 Mean 0.001 -1.423 -0.004 1.401 0.012 1.269 |Max| 0.043 2.699 0.029 1.521 0.046 1.905 Final Heading error (deg.) σ 0.035 0.828 0.015 0.110 0.032 0.502
第四章 感測融合與 RFID 定位
4 本章利用動態感測融合機制，以compass輔助改善encoder回授可靠性， 並以一個打滑的運動路徑實驗融合機制的成效。最後，絕對方位的判定在持 續運動過程中是必要的，藉由RFID訊息定位，以分段式移動策略、功率調變 方式，作為絕對定位之方法，並量測其成效。4.1 感測融合機制
4.1.1 動態感測融合 感測融合部分是將encoder 與 compass 獲得的方位資訊做一整合。在此 之前，compass 藉由設計一階濾波器處理訊號： ( ) 0.9274 ( 1) 0.07263 ( ) y n = y n− + x n ( 4-1 ) 將 encoder 與 compass 偵測到的角度值，利用其各自優勢截長補短： encoder 有較好的穩定性，但隨著時間的累積或打滑的產生而準確度下降； compass 有較好的準確性，但不時出現高頻雜訊而影響穩定性。 如下 圖 4-1 為感測融合的信任權重比，μ代表角度的相信程度，是依 據前一取樣週期之角度命令與compass的差距而決定，定義成：error pre command compass
φ ≡φ − −φ ( 4-2 )
並令兩信任程度函數：μencoder、μcompass。
在φerror較小時，由於encoder 穩定性較佳，故μencoder值較高；當φerror較大
![圖 2-21 V2Xe 與 F2812 通訊時序圖 2.2.5 RFID 介面 1、RFID 原理[25-26] RFID是一種非接觸的自動識別技術，包含reader、tag和天線(antenna) 等部分，reader發送RF時所使用的頻率稱為RFID系統的工作頻率，由於也 是屬於無線電的應用範疇，不能干擾到其他領域應用的正常工作，因此 ISM(Industrial Science Medical)通常採用局部的無線頻段，而RFID也將使用 ISM頻段，基本分成如 表 2-3。](https://thumb-ap.123doks.com/thumbv2/9libinfo/8215161.170245/40.892.146.774.106.388/技術包含和天等部發送RF時所使用頻率稱為不能用的正常工作因此.webp)
