以 ARM Cortex 為核心之智慧型賽格威系統設計 Intelligent Segway control system design using an

(1)

中華大學碩士論文

以 ARM Cortex 為核心之智慧型賽格威系統設計 Intelligent Segway control system design using an

ARM Cortex microcontroller

系所：電機工程學系碩士班學號姓名：M09901037 鍾淳安

指導教授：謝焸家博士許駿飛博士

中華民國 101 年 7 月

(2)

i

摘要

賽格威系統是一種兩輪倒單擺機器人，其具有活動性高以及體機小之優點，

本論文在賽格威之機體架構使用了樂高積木所組成，並利用了樂高公司所推出的直流伺服馬達與Hitechnic公司所推出的陀螺儀感測元件，在控制卡方面，使用新唐公司所推出的ARM Cortex M0為主要控制核心，再配合自己設計的I/O介面卡，

其包含了A/D轉換模組，光學解碼器模組以及PWM模組。由於賽格威系統為一非線性且欠驅動的非穩定系統，本文結合模糊控制與滑動模式控制兩者之優點，提出了解耦合模糊滑動模式控制系統，分別運用在單輪賽格威與雙輪之賽格威系統上，最後，本文經由一些實驗結果顯示出所提出的解耦合模糊滑動模式控制器可以有效地平衡單輪與雙輪賽格威系統。

關鍵字：賽格威系統、倒單擺、平衡控制、模糊控制、滑動模式控制

(3)

ii

Abstract

Segway system is a nonlinear and under-actuated control problem. This study uses Lego blocks to setup the Segway system with Lego’s DC servomotor and Hitechnic’s Gyro sensor. Moreover, a microcontroller-based servocard using a ARM Cortex M0 microcontroller is designed. In the controller design, a decoupled fuzzy sliding mode controller (DFSMC) which combines the advantages of fuzzy control and sliding-mode control. Finally, the proposed DFSMC system is applied to a Single drive and double drive segway systems, From the experimental results, it shows that the developed DFSMC system can achieve favorable balance control performance.

Keywords: Segway, inverted pendulum, fuzzy control, sliding-mode control, balance control

(4)

iii

誌謝

首先我要感謝我的父母，給我了這個機會能夠讀研究所，在經濟上的資助以及精神上的支持，使我能專注的在課業的研究與學習。

在研究所學習過程中首先要感謝的人是我的指導老師許駿飛教授，本論文能順利完成都要感謝許老師的指導與教誨，無論是在論文題目的方向、研究過程中的指導解惑，使得論文可以順利完成。在實驗室中與老師每日的相處看到老師認真的態度且亦師亦友的和學生相處，使我所獲良多，許駿飛老師耐心的教導中不只學習到了專業的技術也學習到了做人處事的道理。在學習的過程中也要感謝與我共同學習的碩士班同學以及一位博士班的邱學長，在修課與研究學習的過程中共同的互相討論以及成長，願再研究所身邊的所有的人在之後能夠持續的連絡保持這份有情。

(5)

iv

表目錄

表 2.1 專案程式執行過 ... 12 表 2.2 常見姿態角度量測元件比較[27] ... 18 表 3.1 單輪賽格威模糊滑動規則庫 ... 27

(8)

vii

圖目錄

圖 1.1 (a)一維的倒單擺[1] ... 2

圖 1.1 (b)二維的倒單擺[1] ... 2

圖 1.1 (d)小車式倒單擺[2] ... 2

圖 1.1 (c)三維的倒單擺[1] ... 2

圖 1.1(e)兩輪賽格威[3] ... 2

圖 1.1(f)獨輪賽格威[4] ... 2

圖 1.2 雙輪賽格威示意圖 ... 3

圖 1.3 模糊控制系統架構圖 ... 5

圖 2.1 Numicro Cortex-m0[23] ... 9

圖 2.2 Keil μvision ide 開發環境[22] ... 10

圖 2.3 專案程式流程圖 ... 11

圖 2.4 樂高馬達[24] ... 13

圖 2.5 樂高傳輸線接頭[24] ... 13

圖 2.6 光學編碼器做正轉運動時量測訊號 ... 14

圖 2.7 光學編碼器做反轉運動時量測訊 ... 14

圖 2.8 HCTL-2032 解碼 IC[25] ... 15

圖 2.9 LB1836 接腳圖[26] ... 16

圖 2.10 LB1836 邏輯示意圖[26] ... 16

圖 2.11 控制馬達原理圖 ... 17

圖 2.12 樂高陀螺儀[28] ... 18

圖 2.13 陀螺儀之實驗結果圖 ... 19

圖 2.14 卡曼濾波器工作原理[30] ... 20

圖 3.1 單輪賽格威示意圖 ... 24

圖 3.2 單輪賽格威控制系統方塊圖 ... 26

(9)

viii

圖 3.3 機身狀態圖 ... 26

圖 3.4 單輪賽格威平衡控制器歸屬函數 ... 27

圖 3.5 自製之單輪賽格威 ... 29

圖 3.6 使用狀態迴授控制單輪賽格威站立不動 ... 31

圖 3.7 使用 DFSMC 控制單輪賽格威站立不動 ... 30

圖 3.8 使用狀態迴授控制單輪賽格威前進退後 ... 33

圖 3.9 使用 DFSMC 控制單輪賽格威前進退後 ... 32

圖 3.10 使用狀態迴授控制單輪賽格威克服障礙物 ... 35

圖 3.11 使用 DFSMC 控制單輪賽格威克服障礙物 ... 34

圖 4.1 雙輪賽格威示意圖 ... 40

圖 4.2 雙輪賽格威控制系統方塊圖 ... 42

圖 4.3 自製雙輪賽格威 ... 44

圖 4.4 使用狀態迴授控制雙輪賽格威站立不動 ... 46

圖 4.5 使用 DFSMC 控制雙輪賽格威站立不動 ... 45

圖 4.6 使用狀態迴授控制雙輪賽格威前進退後 ... 48

圖 4.7 使用 DFSMC 控制雙輪賽格威前進退後 ... 47

圖 4.8 使用狀態迴授控制雙輪賽格威克服障礙物 ... 50

圖 4.9 使用 DFSMC 控制雙輪賽格威克服障礙物 ... 49

(10)

1

第一章緒論

1.1 研究背景目的

倒單擺的研究可以追溯到 20 世紀 50 年代，當時，美國麻省理工學院的控制學專家為研究火箭和導彈自導過程中的姿態控制問題而設計了倒單擺系統，其主要用於測試各種火箭和導彈姿態控制策略。依照倒單擺控制維度來分析，最早期的倒單擺是只有一維的倒單擺，如圖 1.1(a)[1]所示，擺杆在馬達的推動下可沿直線導軌運動，緊接著還有如圖 1.1(b)的二維倒單擺[1]與如圖 1.1(c)的三維倒單擺 [1]陸續出現，後來移動式倒單擺出現了，如圖 1.1(d)的小車式倒單擺[2]和全方位式倒單擺，發展直到現在的兩輪賽格威如圖 1.1(e)所示[3]和獨輪賽格威如圖 1.1(f) 所示[4]。

近年來，在自我平衡機器人出現以前，倒單擺這類自我平衡控制系統做為科學實驗儀器在自動控制理論和方法的研究與實驗中扮演著重要角色，目前種多研究學者透過對兩輪機器人的物理實驗，來驗證機器人學及其相關學科的原理及方法。智慧型機器人已經成為世界各國爭先發展的科技產業，智慧型機器人產業具有龐大的市場也能促進各產業與高科技領域的發展，因此世界各個國家也在智慧型機器人這個新興產業上投資相當多的人力和金錢[5-7]，其中有一產品稱為智慧型賽格威機器人是一種兩輪倒單擺平衡機器人，該機器人是一種新型的科學儀器，

可用於機器人學及其相關學科的科學研究和科學實驗，尤其因為體積小且操作容易，因此一直有眾多專家學者投入心力研究。

兩輪賽格威的運動平衡控制問題涉及兩個方面：一是平衡，即為姿態平衡控制的問題，另一是運動，即為運動軌跡控制的問題。兩輪賽格威的姿態平衡問題類似於倒單擺的平衡問題而且兩輪機器人是一種典型的欠驅動非完整系統。如圖 1.2 所示兩輪機器人有 4 個自由度(2 個平面支撐運動自由度，2 個姿態角運動自

(11)

2

由度) 其中，為姿態傾斜角，與分別為左輪與右輪之馬達轉動角度，

=( + /2 為行進速度，為左輪速度，為右輪速度，為左輪驅動力矩，為右輪驅動力矩[8-10]，然而，兩輪賽格威通常有 3 個量需要控制其中為航向角，但卻只有 2 個可以施加控制量。

圖 1.1 (d)小車式倒單擺[2]

圖 1.1(e)兩輪賽格威[3]

擺

圖 1.1(f)獨輪賽格威[4]

擺圖 1.1 (a)一維的倒單擺[1]

擺

圖 1.1 (b)二維的倒單擺[1]

擺

圖 1.1 (c)三維的倒單擺[1]

擺

(12)

3

(1) 姿態平衡控制基本原理

兩輪賽格威的姿態可以透過控制左輪或右輪的運動速度和運動方向。當兩輪機器人姿態傾斜時，可通過左右馬達產生控制力矩，以和來控制左輪和右輪的運動速度和運動方向，控制左輪和右輪向傾斜方向或傾斜角度運動，使姿態回復直立平衡狀態。

(2) 運動軌跡控制的基本原理

兩輪賽格威的運動軌跡控制問題，其實只是行進速度和行進方向的控制問題。

行進速度 =(

+

/2 是左輪速度 和右輪速度的平均值，可以透過左右輪馬達產生的控制力矩和來調節。行進方向需要透過左輪和右輪的差速來調節，

對左輪和右輪施加不同的作用力矩，以產生不同的運動速度，來實現對兩輪機器人航向的控制。

圖 1.2 雙輪賽格威示意圖

(13)

4

1.2 模糊系統

常用於在現時生活上常出現的一些模糊現象進而發展的一個方法，常用於一些智慧型家電例如：冷氣機、冰箱、電梯等等，包括影像處理、機器人、決策分析、以及軟體工程上都可以看到模糊理論蹤跡[11-14]。模糊理論是由美國著名的學者加利福尼亞大學教授 Zadeh 於 1965 年首先提出,它以模糊數學為基礎,用語言規則表示方法和先進的電腦技術,由模糊推理進行決策的一種高級控制策略。在日常生活中,人們的思維中有許多模糊的概念,如大、小、冷、熱等,都沒有明確的內涵和外延,只能用模糊集合來描述。人們常用的經驗規則都是用模糊條件語句表達,例如,當我們擰開水閥往水桶里註水時,有這樣的經驗:桶里沒水或水較少時, 應開大水閥;桶里水較多時,應將水閥關小些;當水桶里水快滿時,則應把閥門關得很小;而水桶里水滿時應迅速關掉水閥。其中,“較少”、“較多”、“小一些”、“很小”

等,這些表示水位和控制閥門動作的概念都具有模糊性。即有經驗的操作人員的控制規則具有相當的模糊性。

利用模糊理論具有模擬人的思維方式之優點，眾多專家提出了模擬控制系統，

其基本結構如圖 1.3 所示，其中模糊控制系統裡有四個主要部份不包含受控體，

分別有規則庫、模糊化、推論過程與解模糊化。整個模糊系統的程序簡單說明：

(1)首先把測量所得到的值送入模糊控制系統中；(2)在經過模糊化函數，把輸入的數值與所調出的適當參數轉換成一模糊數(3)推理過程在將模糊化所得到的模糊數在配合規則庫，來得到一個模糊型態的控制輸出(4)最後經過解模糊化，將推理過程所得到的模糊型態的控制輸出，解出一明確值來提供受控體所使用。

滑動模式控制使用於強健控制器的一種方法，最初於1957年由蘇聯學者 Aizerman所發表出滑動表面之觀念，則完整的控制架構於1977年由Utkin所提出。

這幾年來滑動模式控制系統類似之理論已大眾應用在各種不同的控制系統中。滑動模式控制其內部可分為兩個階段，分別為滑動模式以及迫近模式。其中滑動模

(14)

5

式為系統的函數軌跡在進入滑動表面後在不脫離此空間情況下，並且朝系統平衡點移動；迫近模式為系統的滑動函數軌跡在有限時間內能夠快速的達到滑動表面上，並且使滑動函數位於零處，所以滑動模式控制具有穩定性高、強健性好、有效的控制暫態響應等優點。

模糊控制的特色比較偏重於人類之經驗和對問題特性的掌握程度，而不主張使用繁雜的數學分析以及數學模式來解決問題，因此在面對系統具有不確定性以及時變性還有非線性等複雜狀態時一樣也能提供傳統控之制器無法達到的效果，

但控制器中的模糊規則為專家的經驗及操控法則，時常需要花費掉許多的時間撰寫龐大規則庫，必須經過實驗後才能讓人得到一個滿意的系統響應。一些專家學者提出模糊滑動模式控制器[15-17]之研究與應用，其結合了模糊控制和滑動模式控制兩者之優點，利用模糊規則對於稱性的現象提出了滑動表面的觀念，可使控制系統中的模糊規則數量大幅的減少，使傳統模糊控制器在應用上所遇到的記憶體不足之問題即可解決。

圖 1.3 模糊控制系統架構圖

(15)

6

1.3 論文架構

賽格威系統為一非線性系統可使用在平地或是有斜坡的地形上，為了研究賽格威平衡控制之設計方式，本論文規劃了以下五個章節如下：

第一章緒論：

起初先介紹雙輪賽格威的由來，接著說明其控制方式與困難處，再來介紹模糊控制的歷史與優點。

第二章控制介面卡之硬體設計：

一開始先介紹本論文所使控制核心 Cortex-M0 和開發環境，接著說明如何用光學編碼器來計算角度和如何用 PWM 加上馬達控制晶片來控制直流馬達，並介紹如何用陀螺儀結合卡曼濾波器來得到最理想的機身傾斜角度。

第三章單輪賽格威平衡系統設計:

本論文先考慮單輪賽格威控制問題，並且提出了解耦合模糊滑動模式控制器 (DFSMC)設計方法，介紹單輪賽格威的平衡系統如何利用 DFSMC 控制方式使得機身平衡，接著經由實際實驗結果比較傳統的狀態迴授控制與所提出 DFSMC 控制之可行性，探討兩者控制方式的效能。

第四章雙輪賽格威平衡系統設計：

本論文先考慮雙輪賽格威控制問題，除了提出 DFSMC 控制方法，還增加考慮左右兩輪差之補償設計，介紹雙輪賽格威的平衡系統到如何利用 DFSMC 控制方式在加上差速使原理使得機身平衡，並接著經由實際實驗結果比較傳統的狀態迴授控制與 DFSMC 控制之可行性，探討兩者控制方式的效能。

第五章結論：

整理前四章所提到的作一個總結，接著介紹此方法還有用到哪些研究。

(16)

7

第二章控制介面卡之介紹

輪型倒單擺之運動平衡控制問題是一個極具挑戰性的研究課題，本論文分別考慮單輪與雙輪賽格威系統，首先利用樂高積木來設計單/雙賽格威系統之主體，

並利用 ARM Cortex-M0 微處理器進行演算法實現工作。在樂高積木與 ARM Cortex-M0 處理器之間，本論文進一步設計所需之控制介面卡，內包含光學解碼器、馬達驅動 IC 以及 AD/DA 轉換器等，其各仔細介紹如下：

2.1 Cortex-M0 微處理器

ARM 的設計是 Acorn 電腦公司於 1983 年開始的開發計畫。這個團隊由 Roger Wilson 和 Steve Furber 帶領，著手開發一種新架構，類似進階的 MOS Technology 6502 處理器。團隊在 1985 年時開發出 ARM1 Sample 版，而首顆"真正"的產能型 ARM2 於次年量產。ARM2 具有 32 位的資料匯流排、26 位的定址空間，並提供 64 Mbyte 的定址範圍與 16 個 32-bit 的暫存器。這些暫存器其中有一顆做為程式計數器，其前面 6 bits 和後面 2 bits 用來儲存處理器狀態標記。ARM2 可能是全世界最簡單實用的 32 位微處理器，其之所以精簡的原因在於它不含微碼，它沒有包含任何的高速快取。這個精簡的特色使它只需消耗很少的電能，卻能發揮比 Intel 80286 更好的效能。後繼的處理器 ARM3 更備有 4KB 的高速快取，使它能發揮更佳的效能[18-20]。

後來蘋果電腦開始與 Acorn 合作開發新版的 ARM 核心，由於這專案非常重要，Acorn 甚至於 1990 年將設計團隊另組成一間名為安謀國際科技 Advanced RISC Machines Ltd.的新公司。由於其母公司 ARM Holdings plc 於 1998 年的倫敦交易市場和 NASDAQ 掛牌上市，使得 Advanced RISC Machines 成了 ARM Ltd 旗下擁有的產品。接著開發出 ARM6，首版的式樣在 1991 年釋出，然後蘋果電

(17)

8

腦使用 ARM6 架構的 ARM 610 來當作他們 Apple Newton PDA 的基礎。在 1994 年，Acorn 使用 ARM 610 做為他們 Risc PC 電腦內的 CPU。ARM 的經營模式在於出售其智慧財產權核 IP core，授權廠家依照設計製作出建構於此核的微控制器和中央處理器。最成功的實作案例屬 ARM7TDMI，幾乎賣出了數億套內建微控制器的裝置。

ARM 架構過去稱作進階精簡指令集機器，更早稱作 Acorn RISC Machine 是一個 32 位元元精簡指令集中央處理器架構，其廣泛地使用在許多嵌入式系統設計。由於節能的特點，ARM 處理器非常適用於移動通訊領域，符合其主要設計目標為低耗電的特徵。在今日，ARM 家族占了所有 32 位元嵌入式處理器 75%

的比例，使它成為占全世界最多數的 32 位元架構之一。ARM 處理器可以在很多消費性電子產品上看到，從可攜式裝置 PDA、行動電話、多媒體播放軟體、掌上型電玩，和電腦到電腦周邊設備硬碟、桌上型路由器甚至在飛彈的彈載電腦等軍用設施中都有他的存在。在此家族中衍伸的重要產品還包括 Marvell 的 XScale 架構和德州儀器的 OMAP 系列。

本論文使用核心為新唐公司所出的 NuMicro 家族產品其採用 LQFP48/64/100 封裝[23]，同一封裝提供不同程式空間大小，當程式空間不敷使用或程式功能要升級時，客戶能在不更動系統硬體設計情況下即可升級，整個家族產品主要架構為 ARM Cortex-M0。Cortex-M0 為 ARM 公司最新的 32 位處理器内核並且比傳統 8 位元更加便宜的價格。因應廣大客戶需求，新唐科技用 8 位元控制器價位，

提供 32 位效能 NuMicro 家族，以創造產品價值。NuMicro 家族其核心執行速度高達 50MHz，相當於 45DMIPS 每秒執行 4 千五百萬條指令；內建 32 位乘法器，

巢狀中斷控制結構 NVIC，雙通道 APB 設計，並配置周邊直接記憶體存取 PDMA；

內建 32K 到 128K 快閃記憶體及 16K Bytes SRAM，整合高效能周邊能力與特性，

使系統效能發揮到極致。通訊部份，NuMicro 家族整合高速 UART、SPI、I2C、

USB 2.0 FS；類比周邊部份，NuMicro 家族提供 8 通道高速 12 位元類比數位轉

(18)

9

換器 ADC、2 通道比較器、低壓偵測功能，另外還有脈寬調變 PWM、捕獲與比較功能。此外，LIN、IrDA、USB 2.0 FS Device、PS2，4 組 32 位元時鐘 Timer、

RTC、看門狗 Watch Dog Timer，內部晶振、內部復位等，亦內置在 NuMicro 家族裡。關鍵特性部份，除了 Cortex-M0 核心與周邊 IP 特性外，新唐科技獨特的高抗干擾、抗雜訊、工業/車用溫度規格設計，更能滿足客戶對於高品質、高性能通用微控制器的要求。本論文選用 NuMicro NUC120LE3AN 為主要運算核心，

如圖 2.1 所示。

2.2 Keil C 開發環境介紹

開發工具環境方面，新唐科技 NuMicro 家族 32 位元控制器已獲得 ARM 微控制器開發套件 MDK-ARM 的支援，MDK-ARM 結合了 Keil RealView 編譯工具以及Keil μVision IDE 開發環境如圖 2.2 所示[22]。另一國際大廠 IAR，EWARM 亦提供完備的整合發展方案，包括一項目管理員、編輯器、編譯工具，以及 C-SPY 除錯器等。此外，新唐科技亦提供功能齊全的軟硬體開發評估系統，如 SWD 硬體橋接器、開發評估學習板、軟體庫、單板燒錄器及支援檔，以説明客戶在最短時間內開發完成產品且快速切入市場，搶得先機。客戶尚可利用新唐科技提供的專屬 ISP 或 ICP 功能，透過線上或離線燒錄，對系統直接做線上更新，增加生產的靈活度和便利性。

圖 2.1 NuMicro Cortex-M0[24]

(19)

10

在開發工具方面，新唐提供簡單、易於使用、功能齊全的軟硬體開發系統，

支援 NuMicro Cortex-M0 全系列產品的開發。Keil 公司所出的 μVision4 是一種整合型的軟體能夠使用 C 語言和組合語言，方便的是將專案的管理、原始程式撰寫、編譯、Ｄebug 和模擬都整合在一起，而且內不包含了很多 ARM 的系統周邊設定環境及模擬。專案用的方式來管理，使得操作變得很有效率以及更簡單。由上到下可分為五層：專案→目標模組→程式群組→程式檔案→包括檔。Keil 的 μVision4 包含了建立以及偵錯兩種操作模式，如下[23]：

（１）建立操作模式：

負責程式的編寫，編譯以及專案的管理，在將程式編譯和連結後，則會產生可執行的檔案。即可將執行檔送到偵錯在進行除錯的工作。

（２）偵錯操作模式：

是一種軟/硬體偵錯器，負責這程式的模擬器以及偵錯。

Keil 主要特點如下：

（１）符合 ANSIC 標準，所產生的程式碼容量少，所以執行速度會比較快。

（２）在 C 語言部分它提供了標準的資料型態，包含 1-bits、8-bits、16-bits、32-bits 整數以及 32-bits 的浮點數型態。

圖 2.2 Keil μVision IDE 開發環境[22]

(20)

11

（３）提供了使用 C 語言所編寫的 ARM 中斷服務函數。

（４）提供周邊設備軟硬體模擬器以及核心軟硬體模擬器的功能。

（５）對於浮點型運算以及長整型數運算有較好的效率。

（６）具有非常多功能的函數庫，其中大多數為可再進入函數。

（７）可接受 Atmel、NXP 以及 Intel 等各大家公司的 ARM 核心晶片。

在新建專案程式部分，內部有幾個常用的檔案，這些檔案除了主程式外，其於的檔案都在資料夾 Driver 內部以設定延時為例，如下圖 2.3 與表 2.1 所示：

程式執行的順序如下[23]：

（１）重製時會進入 0x0000_0000 位置，執行開機啟動程式(Startup_NUC1xx.s) 及核心程式(Core_cm0.c)，進行前置的重置動作，包含：異常中斷的定義、

堆疊初始化還有記憶體管理，最後才會進入主程式 main()。

（２）在主程式一開始會進入(System_NUC1xx.c)，進行系統初始化來設定時脈執行完畢後再返回主程式 main()。

（３）在主程式內部會碰到 DrvSYS_Delay(500)，在進入輸出入程式(Drvsys.c) 內執行延時函數 DrvSYS_Delay(uint32_t us)，執行完畢在主程式內重覆執行。

圖 2.3 專案程式流程圖

(21)

12

2.3 直流馬達

此論文之機構所使用的馬達採用樂高 NXT 直流馬達如圖 2.4 所示[24]，為了達到馬達減速的效果其內部有八組齒輪而齒輪比總比值為 1:48，內部也包含了一組光學編碼器，能夠回授訊號給控制器來得知馬達轉動的角度，這才可以使得控制端能夠精確地控制馬達轉動之方向以及前進、後退或停止。NXT 直流馬達的的傳輸連接線是使用特定規格，共有 6 支接腳如圖 2.5 所示當白色接腳為 9V 且黑色接腳為 0V，則馬達正轉；當白色接腳為 0V 且黑色接腳為 9V，則馬達反轉。

紅色接腳為接地接腳。綠色接腳為 5V 提供電源給馬達內部的編碼器使用。黃色接腳與接腳藍色則是馬達轉動時內部光學編碼器所輸出的訊號。在編碼器內部有 2 組光電晶體讓 2 組晶體排列方式呈現相位差 90 度，便可產生 2 組 AB 向方波藉此則可以算出馬達方向與速度，假如 A 方波領先 B 方波則馬達正轉，反之 A 方波落後 B 方波則馬達反轉。

(Startup_NUC1xx.s) 主程式輸出入程式(Drvsys.c) (Core_cm0.c) #include "DrvSYS.h"

main()

{ /*系統初始化*/

SystemInit();

while(1)//重複執行 {

DrvSYS_Delay(500);

} }

#include "DrvSYS.h"

DrvSYS_Delay(uint32_t us) {

設定 Delay }

異常中斷向量記憶體管理進入 main()

(System_NUC1xx.c) SystemInit();

{

設定時脈 }

表 2.1 專案程式執行過程示意圖

(22)

13

2.3.1 光學編碼器與光學解碼器

光學編碼器是一種很常見的感測傳導器，適合用於馬達轉動時感測轉動量。

而在樂高公司推出的馬達中為了在機構上簡單化和方便性內部也包含了一組角度感應器，來得到馬達定位的功能。光學編碼器可分為增量式編碼器以及絕對式編碼器兩種，兩者最主要差別在於輸出之訊號不同，增量式編碼器輸出有 AB 相兩種不同相位的方波；絕對式編碼器輸出為特定數位碼，只要在編碼器解析角度範圍內之任一角度都會輸出一數位碼出來。

樂高馬達內部的角度感測器是增量式光學編碼器，增量式光學編碼器內部其圖 2.4 樂高馬達[24]

圖 2.5 樂高傳輸線接頭[24]

(23)

14

實就是一個發光二極體與一個光電晶體而在這兩者之間插入一個轉盤，在轉盤上設計了很多間距相同的洞為了讓光電晶體接收到發光二極體產生高低電位訊號，

藉由此方波訊號改變的數量來推算馬達轉動的角度，也可以用此方波訊號的高低電位變化的頻率來推算馬達轉動的速度，但是卻無法得知馬達轉動的方向性。根據以上的問題，樂高公司也在光學編碼器內部多設計了第二組的光電晶體，並且設計讓兩組光電晶體擺放位置相差 90 度，編碼器內部轉盤共有 12 個透光孔，馬達的輸出齒輪比為 1:48 再加上馬達對編碼器齒輪比為 10:32。所以輸出輪轉動一圈等於光學編碼器轉動 15 圈(48*10/32=15)，則光學偵測到 180 個狹縫(15*12)，

剛好一個變化量就等於 1 度。

光學編碼器量測時，是利用圓轉盤的正/反轉運動來分辨的。在做法上利用二組光電晶體接收到發光二極體的光源所產生的兩組訊號來辨別，並將兩者光電晶體的排列相位相差成 90 度且將原來的孔距定義成 360 度。當圓盤轉動時就會產生 90 度相位差的二個脈波訊號，其二個脈波訊號之超前或落後與正反轉有關，

當 A 脈波訊號超前 B 脈波訊號表示光學編碼器正轉，如圖 2.6 所示；當 A 脈波訊號落後 B 脈波訊號表示光學編碼器反轉，如圖 2.7 所示，並且可觀察以下二圖的 A/B 兩脈波訊號之變化頻率來判斷光學編碼器轉動之速度。

圖 2.6 光學編碼器做正轉運動時量測訊號

圖 2.7 光學編碼器做反轉運動時量測訊號

(24)

15

本論文所使用的光學解碼器為 HCTL2032 其接腳如圖 2.8 所示[25]，在我們所設計界面板內有兩組馬達有回授功能，所以我們選擇此光學解碼器可以一次計算兩組光學編碼器。在設定解碼 IC 時，計數資料採用低位元模式(LSB)所以 SEL1 和 SEL2 分別固定給高態訊號與低態訊號，在計數方面我們採用四倍頻方式所以 EN1 與 EN2 要分別固定給高態訊號與低態訊號。而每組光學編碼器各會輸出兩個方波訊號，共有四種訊號 CHAx、CHBx 和 CHAy、CHBy 輸入光學解碼器在經過此 IC 內部的邏輯運算出 8Bits 的數位訊號 D0~D7，在藉由 X/Y 接腳輸入的高態訊號或低態訊號來選擇其中一顆馬達所解碼出來的數位訊號，最後在送到 Cortex-M0 裡來做角度上的計算。在角度運算方面使用絕對角度的運算方式，也就是不論角度轉到哪起始點都為零度角，所以在設計界面板時把解碼 IC 的 RSTX 與 RESTY 兩支接腳接出來，在介面卡一開始送電時做歸零的動作。

2.3.2 馬達驅動晶片

此論文所採用馬達驅動 IC 為 LB1836，之所以採用此 IC 是因為工作電壓都在於樂高原廠馬達許可範圍內而且一次可以驅動兩顆馬達運作其提供了四種操作模式：正轉、反轉、煞車、停止，也多設計了內建過電流保護電路以及過熱保護電路，LB1836 接腳圖如圖 2.9 所示，以及邏輯示意圖如圖 2.10 所示[26]，假如當 IN1 給高電位時而且 IN2 給低電位時則馬達正轉；反之 IN1 給高電位時而

圖 2.8 HCTL-2032 解碼 IC[25]

(25)

16

且 IN2 給低電位時則馬達反轉；假如當 IN1 和 IN2 同時給高電位則馬達煞車；

當 IN1 和 IN2 同時給低電位則馬達停止但是可會滑動。

因為控制馬達只能給用單一種電壓，所以我們給予馬達訊號採用 PWM(脈波寬度調變)的方式控制馬達電源開關的時間來達到控制馬達的轉速。大部分 PWM 頻率都會在 1KHz 以上，假如給予 30%頻率週期來說等於是提供電源 0.3 毫秒，

接著停止提供電源 0.7 毫秒。因為 1KHz 的頻率相當於會有 1000 個週期變化，

所以即使是給予 10%的工作週期給馬達的運作看起來也不會斷斷續續的。而取樣週期為

T

_S

 T

_ON

 T

_OFF，責任週期為

s ON

T

d  T

，其中 d 越大時，代表馬達兩端電

壓差也越大，此時馬達轉速將越快，並且把 PWM 訊號送到 LB1836 來達到控制馬達正反轉以及轉速的目的。一顆 LB1836 可分別控制兩顆馬達所以必須輸入四組 PWM，分別為 IN1、IN2、IN3 以及 IN4，我們就以 IN1 和 IN2 來看如圖 2.11

圖 2.10 LB1836 邏輯示意圖[26]

圖 2.9 LB1836 接腳圖[26]

(26)

17

所示。

2.4 姿態角度量測元件

運動系統的姿態測量問題，是一個很重要的問題。假如沒有姿態訊息，船舶就無法控制航向，飛機就無法操作俯仰，機器人就無法保持直立的姿態和平衡。

運動系統的姿態測量，一般需要慣性測量原件是一種能夠檢測重力方向或角變化的傳導器，可檢測轉軸不固定或無固定轉軸物體的角位移或腳速度，包括陀螺儀、

加速規、傾角儀等。但是每個原件都有本身的有缺點如表 2.2 所示[27]。

加速規用來測量與慣性有關的加速度，包括旋轉、重力和線加速度。加速規可透過三角函速運算能得到傾斜角度。因此可用於可用於測量運動系統的俯仰姿態和橫滾姿態，並提供準確的傾角信息，角度估計值誤差小，精度高。當加速規垂直於俯仰軸安裝時，通過反正切函數運算可做為傾角儀使用，並對俯仰姿勢進

圖 2.11 控制馬達原理圖

(27)

18

行 360 度的全方位測量。對測量數據進行積分，可得到線速度，二次積分可得到線位移但積分產生的漂移誤差將隨時間累積而增長，最後可能導致積分數據誤差過大而無法使用。加速規的動態特性較好於傾角儀，但仍然不夠理想，動態響應速度較慢。傾角儀是一種傾角傳感器，用於測量與重力方向相關的姿態角，包括俯仰角和橫滾角。傾角儀的靜態性能好，精度高，一些高靈敏度電子傾角儀的精度甚至可達 0.0001 度。然而，傾角儀的動態性能較差，響應速度慢，輸出頻率低，實時性較差。經過分析的結果顯示，傾角儀存在相位滯後。實際上，就動態特性而言，傾角類似一個一階低通濾波器。

2.4.1 陀螺儀

陀螺儀是一種很重要的慣性測量原件，在運動系統姿態測量中發揮著重要作用。陀螺儀，又稱陀螺傳感器，是檢測轉動而產生角位移或角速度的傳感器，即使沒有安裝在轉動軸上，也能檢測角位移或轉動速度。因此，陀螺儀被廣泛應用於飛機、導彈、衛星、機器人上等運動系統中。本論文選用 Hitechnic 公司與樂高配合所推出的陀螺儀感測元件，如圖 2.12 所示[28]。

測量原件測量值優點缺點

陀螺儀角速度動態響應好，頻帶寬，更新頻率快。

外在溫度影響大，靜態時容易飄動。

加速規加速度靜態性能好，精度高，更新頻率快。

會累積漂移誤差，動態響應慢。

傾角儀角度靜態性能好，精度高，無累積誤差。

動態響應速度慢。

圖 2.12 樂高陀螺儀[28]

表 2.2 常見姿態角度量測元件比較[27]

(28)

19

所使用之陀螺儀本身輸出的數值型態為類比訊號，電壓值從 0V~5V 之間，

在經過 Cortex-M0 內部的 A/D 轉換成數位訊號 0~1024 之間的數值。但是初始值角度 0 度時陀螺儀所輸出的訊號並非 0V，所以在求角度時必須把讀到的值減去基準值也就轉動的速度，實驗結果圖如下圖 2.13 所示。當陀螺儀輸出值低於基準值較少時表示陀螺儀反轉慢、相對的當陀螺儀輸出高於基準值較少時表示正轉慢、當陀螺儀低於基準值較多時表示反轉快、當陀螺儀高於基準值較高時表示正轉快。

由以上敘述我們可以大概知道了陀螺儀的運動方式，再來就是如何得到精準傾斜角度，本論文在求機體傾斜角度使用梯形法，也就是以每個單位時間所讀到的值減去基準值在來求積分，而在時間採樣頻率部分我們使用 1msec 時間長度，

其公式(2.1)如下：

陀螺儀角度＝（之前的速度＋現在的速度）＊取樣時間間隔

(2.1)

圖 2.13 陀螺儀之實驗結果圖

(29)

20

2.4.2 Kalman 濾波器設計

由於所使用樂高陀螺儀之量測輸出為一類比訊號，讀訊號時十分容易受到外來干擾所影響，常用克服雜訊干擾之方法有：均值濾波、Wiener 濾波、Kalman 濾波、互補濾波等，而本論文使用 Kalman 濾波器是一種有效的濾波器，通過一串含有雜訊的測量數據，估計線性動態系統內部狀態。Kalman 濾波器輸出的估計值與原始測量數據相比，更接近真實的值，因為加取平均值無論與預測值相比還是與測量值相比，都具有更好關於不定性因素的估計[29,30]。如下圖 2.14 所示，預測與修正交替地運行，形成了 Kalman 濾波器的遞迴循環過程。

(1)更新時間預測過程：

1. 假設目前的狀態為，依據系統模型使用上一狀態來預測出目前狀態，為上一狀態結果而為目前狀態估計值，為目前狀態的控制量，假如沒控制量則為 0 如下式(2.2)：

(2.2)

(1)推算前一狀態變量

(2)推算前一誤差協方差

(1)計算卡曼增益

(2)由觀測變量更新估計

(3)更新誤差協方差

更新時間(預測) 更新測量(修正)

與為初始估計

圖 2.14 卡曼濾波器工作原理[30]

(30)

21

2. 依據以上公式系統結果以更新，可是對應於的劦方差還沒更新，為對應的估計的誤差協方差且為對應的估計的誤差協方差且為的轉置矩陣而為過程雜訊的協方差如下式(2.3)：

(2.3) (2)更新測量修正過程：

1. 在更新測量過程中，首先必須先求出卡曼增益， 為卡曼增益，H 為測 量系統的參數，為測量雜訊的協方差如下式(2.4)：

(2.4) 2. 當有了估計值在結合量測量 ，就能夠得到現在的狀態時間 k 之最優

化估計值如下式(2.5)：

(2.5) 3. 目前為止，k 狀態下最優化的估計值 已經得到。但是要讓卡曼濾波器不

斷的執行下去直到系統的過程結束，還必須要更新的協方差如下式 (2.6)：

(2.6) 在使用 Kalman 濾波器必須讓 A、B、、H、、給定初始值，經由一須嘗試，

本論文設定 A=1、 =1、 =0、 =1、 =0.01、 =0.001，在加以上式子重覆的疊代出更接近實際值的可以獲得較佳數值，如此我們再將濾波後數值來做對時間的積分求得最正確的角度。

圖 2.1 NuMicro Cortex-M0[23]

(31)

22

第三章單輪賽格威平衡系統設計

3.1 單輪賽格威系統動態方程式

在本章節考慮單輪賽格威系平衡統控制器之設計過程中，受控系統的動態方程式是非常重要的，將有助於我們進行控制器設計分析。本文在實做上，我們使用樂高所推出的直流伺服馬達來提供整個單輪賽格威系統之動力，其馬達內部利用很多齒輪來得到較高的扭力。整個直流馬達運動方程式可表示如下[31]

(3.1)

其中為馬達瞬間轉動的慣量，為馬達轉動時的阻抗，為馬達轉子位置，與

為電力轉矩且可定義為

(3.2) 在上式中為轉矩之常數與為轉矩之電流。同時，直流馬達的電力方程式可表示如下

(3.3)

其中為電樞的電阻，為反電動勢係數，為電樞之電感與為操作電壓。

因為電樞的電感值為一很小的常數，為了方便後續推導工作，我們在此將其忽略不計。將式子(3.1)與(3.2)分別帶入(3.3)後，直流馬達的動態方程式可表示如下 (3.4)

其中也就是所送給馬達的電壓值。

本章節所考慮的單輪賽格威平衡系統如圖 3.1 所示，其是一個單輸入多輸出的架構，在單一輸入的情況下必須同時控制機身的角度與車輪的位置，其中為

(32)

23

車輪所轉動的角度、為機身所傾斜的角度、為輪軸到機構重心的距離、為 輪胎半徑、m 與 M 分別為輪胎與機構之重量、 與為分別為輪胎與機體之瞬間轉動慣量，再藉由拉格朗日法。輪賽格威平衡運動方程式可表示如下[32]

(3.5) 其中表示轉動慣量之矩陣、表示為向心力與科氏力之矩陣、表示為重力向量、

為增益向量。經由一些推倒後，上述之矩陣可獲得如下

(3.6)

(3.7)

(3.8)

(3.9)

其中為直流伺服馬達與機身的摩差力與為輪胎與地面之摩差力。綜合以上公

式我們可以將單輪賽格威系統看待成與二個子系統，其中子系統包含

（）和子系統 B 包括（），並且假設子系統 A 設定為主要控制目標系統 且子系統 B 設定為次要控制目標系統，即所考慮之單輪賽格威動態方程式可被 化簡為

子系統 A：

子系統 B： (3.10)

其中狀態變數設為與、、與為動態非線性函數。在實際系統中，上述的、、與的參數往往無法正

確的獲得，最大原因在於因為系統放置在不同平面上，摩擦力大小都會不同，

(33)

24

即大小在實做上十分困難獲得明確資訊，因此根據以上結論在於單輪賽格威這類的即時非線性控制問題上，依舊使用傳統的控制設計方式是難以實現。

M、

m、

圖 3.1 單輪賽格威示意圖

(34)

25

3.2 DFSMC 控制器設計

本章節所考慮的單輪賽格威平衡系統控制問題即是典型的非完整和欠驅動的非平衡系統，在控制單輪賽格威平衡時，以簡單方式來說當單輪賽格威系統向前傾時，控制平衡系統為了保持平衡將會輸出使馬達正轉向前走，便可以將向前傾的角度修正回來，同理地若當車身向後仰時，控制平衡系統為了保持平衡則會輸出使馬達反轉向後走，便可以將向後仰的角度修正回來，但是單輪賽格威系統之位置已經不在原處了。

使用傳統的控制技術進行設計時，往往需要明確得知受控系統之動態方程式，

但是單輪賽格威這種即時平衡控制之動態方程式都很難獲得。為了解決這個問題，

本論文使用解耦合模糊滑動模式控制系統，其結合了模糊滑動模式控制器與解耦 合計巧，如圖 3.2 所示，分別考慮(3.10)之子系統 A 與子系統 B 可分別獲得滑動 表面與滑動表面如下：

(3.11) (3.12) 其中與均為正常數。

假如只使用模糊滑動模式控制時，利用之資訊進行控制器設計時，只能確保系統達到指定位置，並無法確表系統平衡，同理，利用資訊進行控制器設計時，只能確保系統平衡，但無法確定系統是否有達到指定位置。針對此問題，本章節所提出之 DFSMC 是將傳統模糊滑動模式控制系統的輸入變數與變化量組合成一個滑動表面，使整個單輪賽格威系統在起始點時有令人滿意的瞬間響應。我們把解耦合滑動表面定義如下

(3.13)

其中為正的常數，由(3.13)可得知解耦合滑動表面 即包含之子系統 A 與子系

(35)

26

統 B 之資訊。如此，利用 來進行控制器設計，將有機會可以同時地控制系統能達到指定位置且能確保系統平衡。

在控制賽格威系統時首先必須先了解賽格威系統的運動決策。一開始必須先讓賽格威系統保持直站狀態才能開始控制平衡，在控制平衡時可歸類成三種行動方式並且施以決策分別如下述，如圖 3.3 所示。

(1) 直立：假設車身的重心在左右兩輪的正中心，且機身靜止不動時，則左右輪 將不轉動，平衡控制器輸出 u 為零。

(2) 前傾：假設車身之重心向前傾，則左右輪將正轉，且平衡控制器將依照機身 傾斜之角度給予適當的正 u 值，使機身向前走到達直立狀態。

(3) 後傾：假設車身之重心向後傾，則左右輪將反轉，且平衡控制器將依照機身 傾斜之角度給予適當的負 u 值，使機身向後走到達直立狀態。

圖 3.2 單輪賽格威控制系統方塊圖

u

圖 3.3 機身狀態圖

>0 <0

(36)

27

DFSMC 平衡控制器的控制目的是為了解耦合滑動表面至零點。在這種情況下，滑動面的變數和將會同時收斂到零點，接著其內部兩個子系統（）和

（）也將會同時收斂至零點，其第 i 條模糊規則表示如下。

Rule i：If

is

，then u is (3.14) 在於模糊滑動模式控制器之前半部輸出的使用的是三角形歸屬函數，而後半部 輸出變數 u 是單值形的歸屬函數。模糊滑動表面 之歸屬函數集合為負大(NB)、

負小(NS)、零(Z0)、正小(PS)、正大(PB)，控制輸出 u 之歸屬函數集合為負大(NB)、

負小(NS)、零(Z0)、正小(PS)、正大(PB)。在單輪賽格威中，前半部之輸入變數為，其論域的歸屬函數規劃成七個不同程度的語句變數，如圖 3.4 所示。

根據單輪賽格威訂定平衡控制器控制法則如表3.1所示，經由觀察模糊控制之規則庫，可以發現其規則具有對稱性。在這裡以控制表其中一條規則來做說明。

假設負很大，此時必須使控制器的輸出大小為-3300，能讓馬達快速往傾倒的 反方向旋轉，使得單輪賽格威機器人平衡。輸出控制u利用重心解模糊化法來解 模糊化，得到馬達控制器輸出u，如下式所示

(3.15) 其中 為第i條模糊規則馬達輸出值與為第i條模糊規則之歸屬程度。

NB NM NS Z0 PS PM PB

u

-3300 -2000 -1000 0 1000 2000 3300 圖 3.4 單輪賽格威平衡控制器歸屬函數

表 3.1 單輪賽格威模糊滑動規則庫

(37)

28

3.3 實驗結果

本小節針對本章所介紹的DFSMC控制方法應用在單輪賽格威上實際系統如圖3.5所示，我們利用ARM Cortex M0來實現所設計之控制法則，為了驗證DFSMC 之效能，我們選擇狀態迴授控制來進行比較，本論文所使用之狀態迴授控制其控制方法如下

u=

＋＋＋ (3.16) 其中、、與為控制器增益常數，在求出 u 並轉換成 PWM 之後直接丟給馬達來做單輪賽格威之平衡控制在，本文中、、與經由嘗試法不斷地調整出一組比較合適的參數。同時，本章節規劃了三種不同實驗項目來驗證 DFSMC 之可行性，分別實驗項目一為平衡控制，實驗項目二為移動控制與實驗項目三為克服障礙物控制，其實驗結果與分析比較如下：

實驗一：單輪賽格威之平衡控制

首先我們先利用狀態迴授控制如圖 3.6 所示與 DFSMC 控制如圖 3.7 所示進行設計實驗一控制，本實驗是針對車身平衡控制的穩定度進行測試，並且比較兩者優異性。一開始為了使操作者能夠有時間把機身放直，所以在抓完起始點角度時設計延遲 2 秒，並且可從圖 3.6(a)與圖 3.7(a)可觀察出來，使用狀態迴授控制方法比 DFSMC 控制方法機身晃動來的大，也明顯的反應在機身的位置上圖 3.6(b) 與圖 3.7(b)使用 DFSMC 控制方法在控制機身平衡時比狀態迴授控制方法來的平順。當機身有效的控制在平衡的範圍內時，相對的給馬達之控制量也不會變化很大如圖 3.6(c)與圖 3.7(c)。

實驗二：單輪賽格威前進退後

使用狀態迴授控制如圖 3.8 所示與 DFSMC 控制如圖 3.9 所示來控制單輪賽格威平衡，在藉由改變單輪賽格威所讀到之位置來達到移動之目的，並且在比較兩種控制方法之優異性。我們利用改變機體起始位置來達到機體移動之目的，一開始設定延遲 2 秒接著保持平衡 6 秒，使機身穩定之後再向前行走 4 秒，接著在

(38)

29

向後回原點 4 秒。並且可從圖 3.8(a)與圖 3.9(a)觀察出來，使用 DFSMC 控制方法前進後退時比狀態迴授控制方法機身擺動幅度更來的小，但是在圖 3.8(b)與圖 3.9(b)單輪賽格威移動，狀態迴授控制方法卻比 DFSMC 控制方法距離擺動幅度來的小，這是因為差速的問題。綜合參考圖 3.8(c)與 3.9(c)馬達之控制量可發現使用使用 DFSMC 控制方法比狀態迴授控制方法給馬達之控制量更來的小與少，

這表是使用 DFSMC 控制方法確實比狀態迴授控制方法更有效達到控制平衡之目的。

實驗三：單輪賽格威克服障礙物

使用狀態迴授控制如圖 3.10 所示與 DFSMC 控制如圖 3.11 所示，來控制單輪賽格威前進退後在行走軌道上加入障礙物，並且觀察兩種控制方法之差異性。

利用實驗二使機體前進退後時並且在前方擺設 5mm 高度的壓克力板，並且觀察兩種控制方式之優異性，從圖 3.10(a)與 3.11(a)可看出在克服壓克力板時，DFSMC 控制方法比狀態迴授控制方法機身擺動幅度依舊較小，接著觀察機身位置從圖 3.10(b)與 3.11(b)在控制平衡時差速問題還是存在，並且發現使用 DFSMC 控制方法現從障礙物下來時超過滿多位置，但是在克服障礙物時比狀態迴授控制來的更有效率。

圖 3.5 自製之單輪賽格威

(39)

30 12^o 2sec

0^o

body pitch angle

(a)

1m 2sec 0m

body position

(b)

8V 2sec 0V

control input of motor

(c)

圖 3.6 使用狀態迴授控制單輪賽格威站立不動 (a)賽格威系統傾斜角之響應圖

(b)賽格威系統位移之響應圖

(c)馬達控制量之響應圖延遲 2 秒讓操作者放好受測系統

(40)

31 12^o 2sec

0^o

(a)

1m 2sec 0m

body position

(b)

8V 2sec 0V

(c)

control input of motor

圖 3.7 使用 DFSMC 控制單輪賽格威站立不動 (a)賽格威系統傾斜角之響應圖

(c)馬達控制量之響應圖延遲 2 秒讓操作者放好受測系統

(41)

32 12^o 2sec

0^o

(a)

2sec 8V

0V

control input of motor

(c)

2sec 1m

0m

body position

(b)

(a)賽格威系統傾斜角之響應圖

(c)馬達控制量之響應圖

圖 3.8 使用狀態迴授控制單輪賽格威前進退後 8 秒後機體開始向前行走

12 秒後機體開始向後行走

16 秒後機體開始向原點行走行走時使機身傾斜角速度

延遲 2 秒讓操作者放好受測系統

(42)

33 12^o 2sec

0^o

(a)

2sec 1m

0m

body position

(b)

2sec 8V

0V

(c)

control input of motor (a)賽格威系統傾斜角之響應圖

圖 3.9 使用 DFSMC 控制單輪賽格威前進退後 8 秒後機體開始向前行走

16 秒後機體開始向原點行走行走時使機身傾斜角速度

延遲 2 秒讓操作者放好受測系統

(43)

34 12^o 2sec

0^o

(a)

1m 2sec 0m

body position

(b)

2sec 8V

0V

control input of motor

(c)

圖 3.10 使用狀態迴授控制單輪賽格威克服障礙物碰到障礙物時

從障礙物上下來過頭前進時碰到障礙物使機身傾斜角速度

從障礙物下來過頭使機身傾斜角速度延遲 2 秒讓操作者放好受測系統

(44)

35 12^o 2sec

0^o

(a)

2sec 1m

0m

body position

(b)

8V 2sec 0V

(c)

control input of motor (a)賽格威系統傾斜角之響應圖

圖 3.11 使用 DFSMC 控制單輪賽格威克服障礙物碰到障礙物時

從障礙物上下來過頭延遲 2 秒讓操作者放好受測系統

(45)

36

3.4 討論

單輪賽格威為一典型欠驅動非線性平衡控制問題，本章我們分別利用 DFSMC 控制與狀態迴授控制兩種方法進行即時控制，針對單輪賽格威系統之實驗結果可發現所提出的 DFSMC 控制可以獲得較佳之結果，尤其在當單輪賽格威系統遭遇到較大系統不確定性時，DFSMC 控制之強健性優點可以更明白地展現出來。因此，本章節所設計之 DFSMC 平衡控制器十分適合用於單輪賽格威系統控制問題上，並且可以同時地有效控制單輪賽格威系統之傾斜角度與位移距離達到平衡目的。

(46)

37

第四章雙輪賽格威平衡系統設計

4.1 雙輪賽格威系統介紹

雙輪賽格威系統具有特殊的動力學特性，其數學模型的研究和動力學特性的分析是機器人學的重要研究內容。控制雙輪賽格威平衡之前必須先了解其運動的模式，而雙輪賽格威平衡系統是一種非完整和欠驅動的非平穩系統，如圖 4.1 所示，為機體之傾斜角度、、分別為左車輪與右車輪所轉動的角度，為輪軸到機構重心的距離，為輪胎半徑，與分別為輪胎與機構之重量，與為分別為輪胎與機體之瞬間轉動慣量。利用拉格朗日方程式推導出雙輪賽格威系統的運動方程式，其中為平移之動能如式(4.1)、為轉動之動能如式(4.2)、為位能如式(4.3)公式表示如下[32]

(4.1)

(4.2)

(4.3) 其中( 、、分別為雙輪賽格威的左輪、右輪以及重心在俯視座標之位置，、、為左輪與右輪以及重心在側視座標之位置、

為萬有引力常數，藉由拉格朗日法以 L 表式示可如下式(4.4)[32]：

(4.4)

經由一些計算化簡後，雙輪賽格威平衡運動方程式可表式如下

(4.5)

(47)

38

其中表示轉動慣量之矩陣、表示為向心力與科氏力之矩陣、表示為重力向量而為增益向量。各關係矩陣表示如下

(4.6)

(4.7)

(4.8)

(4.9)

上式中為直流伺服馬達與機身的摩差力、為輪胎與地面之摩差力、為轉矩之常數、為反電動勢係數、為電樞的電阻。綜合以上公式我們可以把雙輪賽格威看待成與二個子系統，其中子系統 A 包含（）和子系統 B 包含

（），假設子系統 A 設定為主要且子系統 B 設定為次要，並且把狀態變數以及平衡控制器輸出定為如下：

(4.10) (4.11) 因此，我們可以整理出雙輪賽格之威狀態之方程式為

(4.12)

(4.13)

(48)

39

(4.14)

其中、、、、、、、，進一步化簡可得到雙輪賽格威之運動方程式可被化簡為

子系統 A：

(4.15)

子系統 B：

(4.16)

其中 = 與 = ， 其中與 ，而且 u 為平衡控制器之輸出和 為左右輪轉速差補償器之輸出。如此可再進一步化簡得到如下公式

子系統 A：

(4.17)

子系統 B：

(4.18) 其中與，但是 ﹐ ﹐ 與在實做上參數無法正確的獲得，例如系統放置在不同平面上控制摩擦力都會不同，根據以上結論在於雙輪賽格威使用傳統的控制是難以實現。

(49)

40

M、

m、

圖 4.1 雙輪賽格威示意圖

(50)

41

4.2 雙輪賽格威控制法則設計

在控制雙輪賽格威平衡時，必須先了解其動作原理，簡單來說當雙輪賽格威向前傾時，平衡控制器為了要保持平衡，則會輸出左輪與右輪兩個控制量，使兩顆輪胎正轉向前走，並且將會把向前傾的角度修正回來；同理地若當車身向後仰時，平衡控制器為了要保持平衡，則會輸出左輪與右輪兩個控制量，便可以將向後仰的角度修正回來，由此可知設計賽格威平衡控制系統時不需要知道那些繁雜的動力學方程式。

單輪賽格威與雙輪賽格威最大不同的地方，在於平衡控制計所輸出馬達控制量的各數，單輪賽格威只要輸出一馬達控制量但可以同時控制兩顆在同轉軸上的輪子，雙輪賽格威則要輸出兩個控制量分別控制兩顆輪子，但雙輪賽格威的左輪與右輪的馬達雖然相同，可是當給相同控制量時兩顆馬達並不會轉動相同角度，

所以必須設計雙輪同步控制器。在雙輪賽格威平衡設計使用 DFSMC 控制系統，

大多數的模糊控制系統以與變化量以兩個輸入變數來決定控制器之輸出，所以必須建立一個以與變化量為輸入之二維規則庫，但 DFSMC 控制系統是將變數與變化量組合成一個滑動表面，即可將二維的模糊規則庫簡化為一維的模糊滑動模式規則庫，使整個雙輪賽格威系統在起始點時有令人滿意的瞬間響應。如第三章一般，我們把解耦合滑動表面定議成下式

(4.19)

其中為正的常數，而且 為子系統 A 之滑動表面與 為子系統 B 之滑動表面，

分別可表示為下式

(4.20)

(4.21)

(51)

42

其中為陀螺儀透過 A/D 轉換所讀出之數位訊號、為陀螺儀透過 A/D 轉換在經過卡曼濾波器後做積分求得的角度，但是與並不同於單輪賽格威，其表式方式如下式

(4.22)

(4.23)

其中為右輪光學編碼器所讀到的值、為左輪光學編碼器所讀到的值，而後半 部輸出變數 u 與單輪賽格威方式相同。

圖 4.2 雙輪賽格威控制系統方塊圖

(52)

43

4.3 實驗結果

本小節實驗使用DFSMC控制與狀態迴授控制來驗證雙輪賽格威之可行性，

實際上我們使用ARM Cortex M0為控制核心來實現所設計之控制法則如圖4.3，

為了驗證DFSMC控制之效能，我們使用狀態迴授來進行比較，並且觀察陀螺儀所讀到的值、機身所移動之距離與給兩顆馬達之變化量。在於狀態迴授控制本論文使用比較簡易之方式，其控制方法如下式

(4.24) (4.25) 經過 DFSMC 控制器或狀態迴授控制器處理後在求出 u 並轉換成 PWM 之後直接丟給馬達來做雙輪賽格威之平衡控制，其中、、、為單輪賽格威所得到

之狀態倍率並且都為正常數，為經過差動控制器所求左輪與右輪之補，以上

數值是經過長時間的實驗所試出之最好的參數。如同第三章之規劃，本章之實驗分為三個項目：

實驗一：雙輪賽格威之平衡控制：

利用態迴授控制如圖 4.4 所示與狀 DFSMC 控制如圖 4.5 所示進行設計實驗一控制，本實驗是針對雙輪賽格威平衡控制的穩定度進行測試，並且比較兩者優異性。一開始抓完起始點角度時設計延遲 2 秒為了好讓操作者放穩機體，起出可從圖 4.4(a)與圖 4.5(a)可觀察出來，表示態迴授控控制方法比 DFSMC 控制方法機身晃動頻率較大，並且觀察機身的位置圖 4.4(b)與圖 4.5(b)使用態迴授控控制方法在控制機身平衡時雖然看起來比較平順，但根據機身傾斜角度可以判斷是因為機身一直在控制範圍內斷擺動，DFSMC 控制方法可以有效的把控制機身平穩。

當機身有效的控制在平衡的範圍內時，相對的給馬達之控制量也不會變化很大如圖 4.4(c,d)與圖 4.5(c,d)。

(53)

44

實驗二：雙輪賽格威前進退後：

使用狀態迴授控制如圖4.6所示與DFSMC控制如圖4.7所示來控制雙輪賽格威平衡，在藉由遞增或遞減機體所讀到之位置來達到移動之目的，並且在比較兩種控制方法之優異性。利用遞增或遞減機體起始位置來達到機體移動之目的，一開始設定延遲2秒接著保持平衡6秒，使機身穩定之後再向前行走4秒，接著在向後回原點4秒。並且可從圖4.6(a)與圖4.7(a)觀察出來，使用DFSMC控制方法前進後退時比態迴授控控制方法機身擺動幅度更來的小，圖4.6(b)與圖4.7(b)機體位置移動，使用DFSMC控制方法比使用態迴授控控制方法可以行走得更平順。綜合參考圖4.6(c,d)與4.7(c,d)馬達之控制量可發現使用使用DFSMC控制方法比態迴授控控制方法能夠更有效的達到控制行走目的。

實驗三：雙輪賽格威克服障礙物：

使用狀態迴授控制如圖4.8所示與DFSMC控制如圖4.9所示，來控制雙輪賽格威前進退後在行走軌道上加入障礙物，並且觀察兩種控制方法之差異性。本實驗使用實驗二方法並在前方擺置5mm高度的壓克力板，在觀察兩種控制方式之優異性，從圖4.8(a)與4.9(a)可看出在克服壓克力板時，DFSMC控制方法比態迴授控控制方法機身擺動幅度較小，觀察機身位置從圖4.8(b)與4.9(b)使用DFSMC控制方法機身位置移動更來得平順。

圖 4.3 自製雙輪賽格威

(54)

45 12^o 2sec

0^o

(a)

8V 2sec 0V

(c)

control input of right motor

(d)

control input of left motor

0V

8V

(c)右馬達控制量之響應圖

(d)左馬達控制量之響應圖

圖 4.4 使用狀態迴授控制雙輪賽格威站立不動 (a)賽格威系統傾斜角之響應圖

延遲 2 秒好讓操作者放好受測系統

(55)

46

12

^o

2sec 0

^o

body pitch angle

(a)

2sec 8V

0V

(c)

control input of right motor

(d)

control input of left motor 0V

8V

圖 4.5 使用 DFSMC 控制雙輪賽格威站立不動

(b)賽格威系統位移之響應圖離度

(56)

47 12^o 2sec

0^o

(a)

1m 2sec 0m

body position

(b)

2sec 8V

0V

(c)

(d)

0V

8V

圖 4.6 使用狀態迴授控制雙輪賽格威前進退後

行走時使機身傾斜角速度

8 秒後機體開始向前行走

16 秒後機體開始向原點行走延遲 2 秒好讓操作者放好受測系統

(57)

48 12^o 2sec

0^o

(a)

1m 2sec 0m

body position

(b)

2sec 8V

0V

(c)

8V 2sec 0V

(d)

圖 4.7 使用 DFSMC 控制雙輪賽格威前進退後

行走時使機身傾斜角速度

8 秒後機體開始向前行走

16 秒後機體開始向原點行走

(58)

49 12^o 2sec

0^o

(a)

1m 2sec 0m

body position

(b)

8V 2sec 0V

(c)

8V 2sec 0V

(d)

(b)賽格威系統位移之響應圖離度

圖 4.8 使用狀態迴授控制雙輪賽格威克服障礙物

前進時碰到障礙物使機身傾斜角速度

從障礙物下來過頭使機身傾斜角速度

前進時碰到障礙物

從障礙物上下來過頭延遲 2 秒好讓操作者放好受測系統

(59)

50 12^o 2sec

0^o

(a)

2sec 1m

0m

body position

(b)

2sec 8V

0V

(c)

8V 2sec 0V

(d)

圖 4.9 使用 DFSMC 控制雙輪賽格威克服障礙物

前進時碰到障礙物

從障礙物上下來過頭延遲 2 秒好讓操作者放好受測系統

(60)

51

4.4 討論

為了驗證 DFSMC 控制方法應用在雙輪賽格威能上能有較佳之結果，本章節同時應用迴授控制方式在雙輪賽格威上來做兩者之比較，並且從以上的實驗結果圖發現使用 DFSMC 控制方法的確有效的達到控制平衡目的，使用 DFSMC 控制方法時機體為了控制平衡所晃動的頻率明顯的小於使用迴授控制方法，尤其是在雙輪賽格威系統碰到較大系統不確定性時，DFSMC 控制之強健性優點更能明顯的展現出來。我們藉由加入左右輪同步控制器，來改善因為左右輪誤差所導致整體機身位置的偏移，也有明顯的改善。所以本章節所使用的 DFSMC 平衡控制器非常適合用於雙輪賽格威這類的即時控制系統上。

以 ARM Cortex 為核心之智慧型賽格威系統設計 Intelligent Segway control system design using an

中 華 大 學 碩 士 論 文