兩輪滑板車物體追隨控制之設計與實現

全文

(1)國立臺灣師範大學電機工程學系碩士論文指導教授：呂藝光博士王偉彥博士兩輪滑板車物體追隨控制之設計與實現 Design and Implementation of Object Following Control of Two-Wheeled Self-Balancing Robots. 研究生：李冠東撰中華民國一百零五年七月.

(2) 兩輪滑板車物體追隨控制之設計與實現學生：李冠東. 指導教授：呂藝光博士王偉彥博士. 學生：楊智琨. 指導教授. 國立臺灣師範大學電機工程學系碩士班. 摘. 要. 本論文主要研究目的為兩輪自平衡滑板車物體追隨控制之設計與實現。兩輪自平衡滑板車之主要架構為馬達、驅動器、感測器與控制器；感測器部分包含一個三軸加速度計、陀螺儀及雷射測距儀。因兩輪自平衡滑板車具有高度非線性與時變之特性，故本論文利用混合多種控制器來完成兩輪滑板車之物體追隨控制，其中混合的控制器包括比例積分微分控制器 (Proportional-Integral-Derivative controller, PID)及模糊控制器(Fuzzy Controller) 。最後，藉由電腦模擬與實驗結果來驗證整個系統的性能。. 關鍵字：模糊控制、PID 控制、非線性系統、兩輪滑板車. I.

(3) Design and Implementation of Object Following Control of Two-Wheeled Self-Balancing Robots. student:Kuan-Tung Lee. Advisors: Yih-Guang Leu and Wei-Yen Wang. Department of Electrical Engineering National Taiwan Normal University. ABSTRACT The main goal of the thesis is the design and practical investigation of object following control of a two-wheeled self-balancing robot. The body structure of the two-wheeled self-balancing mobile robot includes the motors and drivers, sensor equipment and controllers. The sensor equipment includes a three-axis accelerometer, a gyroscope and a laser rangefinder. Because the two-wheeled self-balancing mobile robot possesses the highly non-linear and time-varying characteristics, the proportional–integral–derivative controller (PID controller) and fuzzy controller are used such that the robot achieves the object following control. Finally, some simulation and experimental results are given to verify the performance of the overall system.. Keywords：Fuzzy control, PID control, nonlinear Systems, two-wheeled mobile robots.. II.

(4) 致謝本研究得以順利完成，首先，要感謝指導教授呂藝光博士與王偉彥博士，兩位教授不僅在專業領域上給我許多建議及資源協助，讓我在學習的過程中能心無旁騖的嘗試一次次的實驗，同時亦感謝口試委員吳政郎教授，在口試時提出寶貴的意見，讓我的論文品質可以更加精進。. 回想起這兩年來的一切，所有的人事物都使我的研究所生活如此充實，接著感謝我的同學黃仕華、林宇恆、李承諺、陳冠安、呂承益，還有實驗室的學弟妹徐銘偉、林則宇、曹顥瀚、施君達、連孟哲、楊采玲及張儀如，謝謝你們在我研究所生涯的這段時間，給予我莫大的協助與陪伴，讓我感受到支持與鼓勵。. 最後，我要感謝我的家人。尤其是父母在我需要的時候提供協助，深深感謝你們宜路上給予的關心與鼓勵，讓我專心的在求學過程中完成學業，展開人生的新頁。在此，願將成長之喜悅與他們共享，並貢獻所學於社會。. III.

(5) 目錄摘. 要 .................................................................................................................... I. ABSTRACT ................................................................................................................... II 致謝 ...............................................................................................................................III 目錄 .............................................................................................................................. IV 表目錄 .......................................................................................................................... VI 圖目錄 ......................................................................................................................... VII 第一章. 緒論 ...............................................................................................................1. 1.1 研究背景與動機 ..............................................................................................1 1.2 研究目的 ..........................................................................................................3 1.3 研究方法 ...........................................................................................................3 1.4 研究架構 ..........................................................................................................4 第二章. 兩輪滑板車之平台設計 ...............................................................................5. 2.1 兩輪滑板車之車體機構 ..................................................................................5 2.2 系統電源 ...........................................................................................................6 2.3 系統架構 ...........................................................................................................6 2.4 載具感測裝置 ...................................................................................................7 2.5 數位訊號濾波 .................................................................................................13 2.7 藍芽傳輸模組 HC-05 .....................................................................................17 2.8 霍爾感測器 3144-402 ....................................................................................18 2.9 軟體架構 .........................................................................................................19 第三章. 兩輪滑板車之物體追隨控制 .....................................................................21 IV.

(6) 3.1 比例積分微分 ................................................................................................21 3.2 模糊控制系統 .................................................................................................23 3.2.1 模糊化 ..................................................................................................24 3.2.2 模糊推論引擎 .....................................................................................26 3.2.3 模糊規則庫 ..........................................................................................26 3.2.4 解模糊化 ..............................................................................................27 3.3 兩輪滑板車動態方程式 .................................................................................28 3.4 模擬結果 ........................................................................................................30 第四章. 實驗與討論 .................................................................................................44. 4.1 實驗結果 .........................................................................................................44 第五章. 結論與未來展望 .........................................................................................71. 5.1 結論 .................................................................................................................71 5.2 未來展望 .........................................................................................................71 參考文獻 .......................................................................................................................72 自傳 ...............................................................................................................................74 學術成就 .......................................................................................................................74. V.

(7) 表目錄表 2-1 ADXL335 規格表 ................................................................................................8 表 2-2 IDG500 規格表 ....................................................................................................9 表 2-2 URG-04LX 規格表 ............................................................................................10 表 2-3 命令格式 ............................................................................................................ 11 表 2-4 S-G 濾波係數 ....................................................................................................15 表 3-1 模糊規則庫 ........................................................................................................27 表 3-2 動態方程變數表 ................................................................................................28 表 3-3 模糊規則庫 ........................................................................................................34 表 3-4 模糊規則庫 ........................................................................................................39 表 4-1 模糊-PID 串聯式控制架構之模糊規則庫 .......................................................47 表 4-2 新增規則之模糊規則庫 ....................................................................................49 表 4-3 規則數與收斂情況比較 ....................................................................................52 表 4-4 模糊-PID 雙迴路控制架構之模糊規則庫 ......................................................57 表 4-5 控制器比較表 ....................................................................................................70. VI.

(8) 圖目錄圖 2-1 兩輪滑板車鳥瞰圖與實體電路圖 ......................................................................5 圖 2-2 系統電源 ..............................................................................................................6 圖 2-3 控制系統架構 ......................................................................................................7 圖 2-4 ADXL335 .............................................................................................................7 圖 2-5 IDG500 .................................................................................................................8 圖 2-6 URG04-LX ...........................................................................................................9 圖 2-7 MAX232 方塊圖 ............................................................................................... 11 圖 2-8 回傳資料格式 ....................................................................................................12 圖 2-9 資料編碼範例 ....................................................................................................13 圖 2-10 資料解碼範例 ..................................................................................................13 圖 2-11 驗證碼範例 ......................................................................................................13 圖 2-12 均值濾波波型圖 ..............................................................................................14 圖 2-13 S-G 濾波波型圖 ..............................................................................................15 圖 2-14 互補濾波器示意圖 H ......................................................................................16 圖 2-15 互補濾波波型圖 ..............................................................................................17 圖 2-16 藍芽模組 HC-05 ..............................................................................................17 圖 2-17 霍爾感測器 3144-402 .....................................................................................18 圖 2-18 增量型編碼器原理 ..........................................................................................18 VII.

(9) 圖 2-19 控制器流程圖 ..................................................................................................19 圖 2-20 單晶片與雷射測距儀之架構 ..........................................................................20 圖 2-21 測距儀解碼流程圖 ..........................................................................................20 圖 3-1 PID 系統方塊 ....................................................................................................22 圖 3-2(a)輸入參考訊號(b)輸出訊號(c)與參考訊號間誤差(d)誤差之微分 ..............23 圖 3-3 模糊邏輯系統方塊圖 ........................................................................................24 圖 3-4 三角形模糊化 ....................................................................................................25 圖 3-5 高斯函數模糊化 ................................................................................................25 圖 3-6 單值模糊化 ........................................................................................................26 圖 3-7 兩輪滑板車示意圖 ............................................................................................29 圖 3-8 使用 PID 控制器之車身傾斜角度 ...................................................................31 圖 3-9 模糊-PID 雙迴路控制架構 ...............................................................................32 圖 3-10 輸入間隔歸屬函數 ..........................................................................................32 圖 3-11 輸入速度歸屬函數 ..........................................................................................33 圖 3-12 輸出 PWM 歸屬函數 ......................................................................................33 圖 3-13 模糊-PID 雙迴路架構之車身傾角模擬 .........................................................34 圖 3-14 模糊-PID 雙迴路架構之追隨模擬 .................................................................35 圖 3-15 模糊-PID 雙迴路架構之車身傾角收斂模擬 .................................................36 圖 3-16 模糊-PID 雙迴路架構之物體追隨模擬 .........................................................36 VIII.

(10) 圖 3-17 模糊-PID 串聯式控制架構 .............................................................................37 圖 3-18 輸入間隔歸屬函數 ..........................................................................................37 圖 3-19 輸入速度歸屬函數 ..........................................................................................38 圖 3-20 輸出後一級 PID 之參考訊號 .........................................................................38 圖 3-21 模糊-PID 串聯式控制架構之車身傾角收斂模擬 .........................................39 圖 3-22 模糊-PID 串聯式控制架構之追隨收斂模擬 .................................................40 圖 3-23 雙 PID 迴路控制架構 .....................................................................................40 圖 3-24 雙 PID 迴路控制架構之車身傾角收斂模擬 .................................................41 圖 3-25 雙 PID 迴路控制架構之追隨收斂模擬 .........................................................41 圖 3-26 串聯式 PID 控制架構 .....................................................................................42 圖 3-27 串聯式 PID 控制架構之車身傾角收斂模擬 .................................................42 圖 3-28 串聯式 PID 控制架構之位置收斂模擬 .........................................................43 圖 4-1 模糊-PID 串聯式控制架構 ...............................................................................44 圖 4-2 輸入間隔歸屬函數 ............................................................................................45 圖 4-3 輸入速度歸屬函數 ............................................................................................46 圖 4-4 輸出後一級 PID 之參考訊號歸屬函數 ...........................................................46 圖 4-5 模糊-PID 串聯式控制架構之車身傾角 ...........................................................47 圖 4-6 模糊-PID 串聯式控制架構之車身位置 ...........................................................48 圖 4-7 輸入速度歸屬函數之修改 ................................................................................49 IX.

(11) 圖 4-8 模糊-PID 串聯式控制架構之車身傾角 ...........................................................50 圖 4-9 模糊-PID 串聯式控制架構之車身位置 ...........................................................50 圖 4-10 加入人為外部干擾之車身傾角 ......................................................................51 圖 4-11 加入人為外部干擾之車身位置 ......................................................................51 圖 4-12 輸入位置歸屬函數之修改 ..............................................................................52 圖 4-13 模糊-PID 串聯式控制架構之車身傾角 .........................................................53 圖 4-14 模糊-PID 串聯式控制架構之車身位置 .........................................................53 圖 4-15 加入人為外部干擾之車身傾角 ......................................................................54 圖 4-16 加入人為外部干擾之車身位置 ......................................................................54 圖 4-17 模糊-PID 串聯式控制架構實驗過程 .............................................................55 圖 4-18 模糊-PID 雙迴路控制架構 .............................................................................56 圖 4-19 輸出 PWM 歸屬函數 ......................................................................................56 圖 4-20 模糊-PID 雙迴路控制架構之車身傾角 .........................................................57 圖 4-21 模糊-PID 雙迴路控制架構之車身位置 .........................................................58 圖 4-22 加入人為外部干擾之車身傾角 ......................................................................58 圖 4-23 加入人為外部干擾之車身位置 ......................................................................59 圖 4-24 模糊-PID 雙迴路控制架構實驗過程 .............................................................60 圖 4-25 串聯式 PID 控制架構 .....................................................................................61 圖 4-26 串聯式 PID 控制架構之車身傾角 .................................................................61 X.

(12) 圖 2-27 串聯式 PID 控制架構之車身位置 .................................................................62 圖 4-28 串聯式 PID 控制架構之車身傾角 .................................................................63 圖 4-29 串聯式 PID 控制架構之車身位置 .................................................................63 圖 4-30 加入人為外部干擾之車身傾角 ......................................................................64 圖 4-31 加入人為外部干擾之車身位置 ......................................................................64 圖 4-32 串聯式 PID 控制架構實驗過程 .....................................................................65 圖 4-33 PID 雙迴路控制架構 ......................................................................................66 圖 4-34 PID 雙迴路控制架構之車身傾角 ..................................................................66 圖 4-35 PID 雙迴路控制架構之車身位置 ..................................................................67 圖 4-36 加入人為外部干擾之車身傾角 ......................................................................67 圖 4-37 加入人為外部干擾之車身位置 ......................................................................68 圖 4-38 PID 雙迴路控制架構實驗過程 ......................................................................69. XI.

(13) 第一章. 緒論. 1.1 研究背景與動機隨著科技發達及個人交通工具的普及，馬路上充斥著汽車、機車，而汽車、機車所排放的廢氣更直接影響到了人們的健康，輕者身體不適重者致癌。使得國內外許多團體都在研發取代石化燃料的方案，而目前市場上面主要的替代能源車輛有混合動力汽車及純電動汽車等，各類型的車輛都有其優缺點，如混合動力車因擁有多個能量來源，相較於純電動汽車要來的有續航力，而純電動汽車與其他能源類型比較下，有低碳排放量、低噪音、低汙染、高能量轉換率等優點，將能降低使用高污染的石化能源，並彌補其他能源轉換效率不佳等缺點，因此許多國家紛紛投入電動車的相關研究。. 其中最具代表性的是美國發明家 Dean Kamen 在 2001 年所發明的兩輪平衡載具 Segway[1]，Segway 的原理是建立在倒單擺原理上，以車身安裝的五顆陀螺儀通過內部的微處理器來計算車身目前所改變的傾斜狀況，依照狀況驅動馬達轉動進而達到車身平衡之效果，在有駕駛人站立於車體的情況下，可以利用人體重心的前後移動，讓車體傾斜角度產生改變，使得車體有前進與後退兩種模式，且隨車體傾斜角度越大，速度也相對的越快，搭配利用差速輪原理，便可以做到左右轉等靈活運動。在現今已經被廣泛應用在生活中，除了交通警察或其他公務所需之街頭巡邏，也因中國公司「Ninebot」收購「Segway」後，在兩者互相合作發揮長處，再推出小米九號平衡車[2]後不只體積變更輕、重量變小，價格也慢慢親民而成為個人代步工具出現在街頭巷弄之中。. 1.

(14) 平衡車在市場上與其他電動車在未來的競爭優勢在於體積小且重量輕可以方便攜帶，即使在不使用的情況下也能由一名成人輕鬆提起行走，所以平衡車可以帶進室內，不用冒著電動車電池被偷竊的風險停放於室外，這優點將可以實現在室內空間之間的交通，如辦公室與辦公室間或室內作業區的移動，這是電動車較不能克服的難題，而平衡車則能近乎完美的做到移動至所有步行所可以到達的地方，甚至是樓層間的移動。. 如同電動車未來發展的重要趨勢議題-無人自動駕駛，平衡車也有許多學者提出追隨控制系統之相關文獻研究[3-9]，相較於戶外自動駕駛擁有各種天氣地形、未知的動態障礙物等因素的影響及考量，室內空間的移動路線較為單純，但考慮的不只是平衡車的車體角度及角速度，也必須考慮位移與速度之間的關係，這使被控系統更加的複雜、控制難度更高，因此使得本論文更想投入此研究，進而完善比較各控制器的優缺點變成為本論文的動機。. 2.

(15) 1.2 研究目的本論文具體研究目的為利用不同的控制策略，做到物體追隨控制，由於兩輪滑板車屬於倒單擺系統，因此擁有非線性與不穩定的特性，需要外加控制器才能使其平衡受控，所以本研究選擇此系統做為驗證控制方法的受控系統。經由模擬後加以實現於兩輪滑板車物體追隨控制上，並將各控制器之間的響應做比較。其中安裝在載體上的感測器所量測到的資訊，經由單晶片整合後由藍芽串列傳出資料讓平板電腦接收，為了做到能遠端收集監控的平台，搭配其開發之手機應用程式，與網頁伺服器進行通訊上傳載具資訊，最後經由網頁呈現，應用這一系統便能將兩輪滑板車與網頁即時顯示做串聯，使得未來使用者能更方便在室內與其他設備做靈活運用，更進一步提升使用意願。. 1.3 研究方法兩輪滑板車為本論文的受控平台，首先，論文裡會使用雷射測距儀、三軸加速度計與陀螺儀回傳前方距離、載體角度與角加速度，並使用數位濾波器過濾雜訊得到較平滑的訊號作為控制器的輸入，再經由比例積分微分控制與模糊控制做結合，用不同的組合做實驗，使兩輪滑板車可以在依照回傳的數值算出合適的控制力，在保持車身不倒下的狀態，依雷射測距儀量測的前方距離做快速的物體追隨控制，經由實驗過程調整各項參數，並對實驗結果優劣做比較。. 在本論文實驗中，先由手機以藍芽的方式與兩輪滑板車 UART 接口上的藍芽模組做連線，並傳送指定與物體的間隔給兩輪滑板車，在移動過程中，載具的各項資料會經由藍芽傳輸傳送至手機，透過手機應用程式將載體所量測到的資訊經網路上傳至伺服器，並經由網頁即時呈現，以圖像或文字表示當前載體傾斜角度及距離物體的誤差，讓使用者能以簡單的跨平台操作設定載具與物體的間隔。 3.

(16) 1.4 研究架構本論文共分為六個章節，各章內容分述如下: 第一章. 緒論: 敘述本論文之研究背景及動機、研究方法、論文架構。. 第二章. 兩輪滑板車之硬體架構與設計: 首先會介紹兩輪平衡車的構造，再個別介紹各個硬體及感測系統的運作原理和規格。. 第三章. PID 及 FUZZY 控制器: 依序介紹 PID、FUZZY 控制器，並互相結合設計，最後以模擬再對其收斂性作證明。. 第四章. 實驗結果與結論：將第三章所提到的控制方法的實驗結果作分析、比較，並且實驗證明控制方法，最後架設伺服器以便跨平台監控。. 第五章. 結論與未來展望. 4.

(17) 第二章. 兩輪滑板車之平台設計. 本論文所使用的兩輪滑板車為本實驗室以簡單且價格低廉的方式自行設計，使用控制核心為 Megawin 單晶片 82G516AE，並使用串列將程式燒錄至微控制器中，依照感測器回傳數值，經由所設計之控制器運算調整 PWM(Pulse Width Modulation)的責任週期，以此控制馬達轉速，如此一來就能使兩輪滑板車做到前進或後退，進而做到本實驗希望做到之動作，到達設定的間隔。本章節中將逐一介紹兩輪滑板車的所有硬體設備、電路及工作原理，包括車體機構、系統電源電路、系統軟體架構及各感測器模組。. 2.1 兩輪滑板車之車體機構兩輪滑板車是以可載人載物為目的的載具，所以載具上方必須有合適的空間放置各式物品，且必須有堅固的車體乘載重量，故車身分作上層木板與下層底部等部分，上板用實心木質材料製成如圖 2-1 左圖所示，而車身底部中央則放置所有硬體電路，包括兩顆鉛酸電池用於系統電源，左邊放置馬達驅動模組、降壓模組、控制器電路及各感測器如圖 2-1 右圖所示。. 圖 2-1 兩輪滑板車鳥瞰圖與實體電路圖. 5.

(18) 2.2 系統電源由於載具主要的電源供應來源為兩顆 12 伏特,12 安時的鉛酸電池所串聯而成，電池電壓電流會隨著剩餘電量發生改變，為了能供應穩定的電源，並保護電路不被燒毀，必須先進行穩壓，本論文以 LM338t 穩壓，有了穩定的電壓源後，由於其他模組的工作電壓為 3.3 伏特和 5 伏特兩種，故使用三種降壓 IC 分別降壓到 5 伏特與 3 伏特提供給各個感測器及微控制器穩定電源，接著使用 LM2596 進行，調整上方可變電阻降壓至 5 伏特給 82G516 及兩組馬達驅動模組，另外再將 5 伏特經由 LM317 降壓至 3.3 伏特給三軸加速度計與陀螺儀使用，由於雷射測距儀對電源要求高、耗電量大，故另外使用 LM2596 將 24 伏特降至 9 伏特，再由 L7805 降壓至 5 伏特供應給雷射測距儀。鉛酸電池24V. 3.3V. 陀螺儀. 三軸加速度計. 驅動器. 5V. 雷射測距儀. 控制器 82G516. 直流馬達. 圖 2-2 系統電源. 2.3 系統架構如下圖 2-3 所示，這是兩輪滑板車的系統架構示意圖，微控制器 82G516 使用內建 10bit 類比數位轉換功能，以取樣頻率 100Hz 從三軸加速度計及陀螺儀取類比的輸出電壓訊號，並轉換成數位訊號，經過 S-G 濾波器 (Savitzky-Golayfiltering)及互補濾波器(Complementaryfiltering)進行軟體濾波並且得到車體的傾斜角度，透過所設計的控制器即時運算出 PWM 的責任週期輸出至左右輪的馬達驅動模組，驅動兩輪移動到追隨物體。. 6.

(19) 加速度計 A/D. 陀螺儀. 濾波器. 雷射測距儀. 控制器. 單晶片. 串列通訊. 馬達驅動器 (右). 直流馬達 (右). 馬達驅動器 (左). 直流馬達 (左). 圖 2-3 控制系統架構. 2.4 載具感測裝置 2.4.1 三軸加速度計 ADXL335. 圖 2-4 ADXL335 實體如圖 2-4 所示，加速度計原理為極板受到應力的作用，如傾斜或加速度，進而改變電容、電阻值，並產生量測電壓，其感測器規格如表 2-1 所示，由於輸出訊號為類比電壓訊號，故必須使用類比數位轉換將數值轉換給微控制器使用，在本實驗中主要是透過量測重力加速度，進一步計算當前載體傾斜角度，可以用式 2-1 計算出角度。 7.

(20) 𝑎𝑛𝑔𝑙𝑒𝑥𝑦𝑧 = (𝑉𝑥𝑦𝑧 − 𝑉0𝐺𝑥𝑦𝑧 ) ∗. 9.81 𝑉1𝑔𝑥𝑦𝑧 − 𝑉0𝐺𝑥𝑦𝑧. (2-1). 表 2-1ADXL335 規格表項目. 規格. 單位. 工作電壓. 1.8~3.6. V. 工作電流. 350. μA. 量測範圍. ±3G. g. 靈敏度. 300. mV/g. 2.4.2 陀螺儀 IDG500. 圖 2-5 IDG500 實體如圖 2-5 所示，陀螺儀主要是當受到外來力矩的影響時，可以量測載具的角速度，進而得知載具的姿態變化，其感測器規格如表 2-2 所示，由於輸出訊號也為類比電壓訊號，所以需要透過類比數位轉換將電壓數值轉換給微控制器使用，電路提供類比數位轉換器的電壓為 5 伏特，這使轉換器能將 0 伏特~5 伏特表示為 0~1024 之間的數值，解析度約為 4.9 毫伏，擁有如此高的解析度將幫助實驗達到更好的控制效果。. 8.

(21) 表 2-2IDG500 規格表項目. 規格. 單位. 工作電壓. 3. V. 工作電流. 7. mA. 1 倍輸出. ±500. ∘/s. 4.5 倍輸出. ±110. 1 倍輸出. 2. 4.5 倍輸出. 9.1. 量測範圍. 靈敏度. mV/∘/s. 2.4.3 雷射測距儀 URG-04LX. 圖 2-6 URG04-LX 實體如圖 2-6 所示，雷射測距儀 URG-04LX 是 HOKUYO 公司的產品，其規格最大掃描弧度為 240 度，最大掃描半徑為 4 公尺，每個掃描點相距 0.36 度且解析度為 1 毫米如表 2-2 所示。. 9.

(22) 表 2-2 URG-04LX 規格表項目. 規格. 單位. 供應電源. 5±5%. Vdc. 消耗電流. 0.5~0.8. A. 量測距離. 60~4095. mm. 準確度. 30~1000±10. mm. 1000~4000±1% 距離解析度. 1. mm. 掃描速度. 0.1. s. 角度解析度. 0.36. deg. 傳輸方式. USB Version 2.0 FS mode (12000). kbps. RS-232C (19.2). URG-04LX 的傳輸方式有 RS-232 與 USB2.0 兩種，為了讓雷射測距儀與單晶片做通訊傳輸，所以採用 RS232 傳輸，鮑率使用 19200bps，但由於雷射測距儀的傳輸方式 RS232 與單晶片的 TTL 兩者間的電壓準位不同，故在感測器與單晶片間使用一介面電路，此電路由 MAX232 所組成，能在感測器與單晶片之間做到 RS232 與 TTL 的電位轉換，其電路圖如圖 2-7。. 10.

(23) VCC. C4 - + 10. VCC IC1. C1 - + 10. C1 - +. 10 C1 + 10. VCC 16. 1 C1+ 2 V+. GND 15. 3 C1-. T1OUT 14. 4 C2+. R1IN 13. 5 C2-. R1OUT 12. 6 V-. T1IN 11. 7 T2OUT. T2IN 10. R2OUT 9. 8 R2IN. MAX232. 圖 2-7MAX232 方塊圖在使用雷射測距儀時，需要先從單晶片串列傳送一組 MDMS 控制指令給雷射測距儀，在收到指令後雷射測距儀會依照指令自動量測，指令格式如表 2-3 所示，符號皆為 ASCII 碼，其格式定義如下說明。表 2-3 命令格式 M D. Starting Step. Number of Scans 1.. End Step. Cluster Count. String Characters. LF. Scan Interval. 指令種類:依照需求選擇指令種類，MD 指令能將距離編碼成三個字元，MS 則將距離編碼成兩個字元，因為 MD 指令有三個字元可以編碼，能將量測的範圍比 MS 指令長，在本論文中皆使用 MD 指令去做量測。. 2.. 起、終點:在這一次指令中的掃描起點和終點，如表 2-3 所示，因為雷射測距儀有 120 度的範圍無法量測，起始點從 0044 開始掃描，當要從起始點往後掃描 120 度，則將起點設定為 0044，終點設定為 0384。 11.

(24) 3.. 群集數:可以從數個取樣點中取一個最小值，其值介於 0 到 99 之間。. 4.. 掃描間隔:用於設定掃描時，每一個掃描點的間隔。. 5.. 掃描次數:此設定為雷射測距儀從起點到終點掃描的次數，若要持續不斷的量測距離，則將此設定設為 00，直到收到 QT 停止指令，關閉感測器為止。. 當雷射測距儀接收正確的指令時開始自動量測，每量測一個掃描點就會回傳一筆 3 個 ASCII 所組成的資料，依照回傳的資料量決定回傳的格式，如圖 2-8 所示，當掃描資料小於 64bytes 時，則依照圖 2-8 的格式去抓取雷射測距儀所量測到的資料。 M D. Starting Step. Remaining Scans 9. 9 b LF. Data. Sum. End Step LF. Cluster Count Starting Characters. Time Stamp LF. Sum. LF. LF. LF. 圖 2-8 回傳資料格式雷射測距儀的通訊協定使用 SCIP2.0，為了使單晶片與感測器的傳輸時間減少，在量測資料傳輸前會依照 SCIP2.0 的格式先進行編碼，編碼範例如圖 2-9 所示，在量測到資料後會先轉換為二進制，再以 6bit 為一組分開，每一組各別加上十六進制 30H 後，傳出相對應到的 ASCII 碼，所以單晶片在收到編碼後的資料時，也必須先進行解碼才能做處理，當收到感測器編碼過後的資料時，先將收到的資料各別轉為相對應的 ASCIII 碼，再自每一組 ASCII 各自減掉 30H，之後轉換為二進制的表示方式，最後將這 18bit 的二進制依序排列為一組就是感測器所量測到的數值，其解碼範例如圖 2-10 所示，資料最後有一驗證碼可供比對，驗證碼是將編碼後的資料以十六進制相加取後 6bit，加上 30H 所對應到的 ASCII 碼，驗證碼範例如圖 2-11 所示，其顯示為英文字母 o。. 12.

(25) 5432mm  1010100111000 2  Separation 0000012. 010100 2 111000 2.  Hexadecima l Equivalent 1H. 14H. . 38H. add 30H. 31H. 44H. . 68H. ASCII Equivalent. 1. D. H. 圖 2-9 資料編碼範例 1Dh  1. D. h.  Hexadecima l Equivalent 31H 44H. 68H.  Subtract 30H 1H 14H 38H  Binary 0000012. Equivalent. 010100 2 111000 2  M erge 000001010100111000  Decimal. Equivalent. 5432. 圖 2-10 資料解碼範例 Hokuyo  48H  6FH  6BH  75H  79H  6FH  27FH  10011111112 Sum  1111112  3FH  30H  6FH. 圖 2-11 驗證碼範例. 2.5 數位訊號濾波 2.5.1 均值濾波器(Mean Filtering) 由於感測器本身容易受到外在如溫度、電磁、人為等因素，使得訊號會混入許多干擾，進而產生測量上的誤差，因此在本實驗中類比訊號的取樣會先經過簡單的訊號平均化法，先透過累加數值後除以累加次數，如圖 2-12 所示，這樣便能有效的過濾掉環境雜訊，達到抑制誤差的效果。 13.

(26) 圖 2-12 均值濾波波型圖 2.5.2S-G 濾波器(Savitzky-Golayfiltering) S-G 濾波器[10]最初 Savitzky 和 Golay 在 1964 年提出，是一種特別的低通濾波器，常被用來做訊號平滑及濾除雜訊，利用最小二乘的多項式進行最佳訊號估測，濾波參數則對應表 2-4 中的加權係數[19]，如式 2-2。 𝑥𝑆𝐺 = −2𝑥𝑡 +3𝑥𝑡−1 +6𝑥𝑡−2 +7𝑥𝑡−3 +6𝑥𝑡−4 +3𝑥𝑡−5 −2𝑥𝑡−6. 14. (2-2).

(27) 圖 2-13 S-G 濾波波型圖表 2-4S-G 濾波係數 (資料來源:[19]) Degree Window size. 2/3(quadratic/cubic) 5. 7. 4/5(quartic/quintic) 9. -4. 7. -21. -3. 9 15. -2. 14. 5. -55. -2. -3. 3. 39. -30. 30. -1. 12. 6. 54. 75. 135. 0. 17. 7. 59. 131. 179. 1. 12. 6. 54. 75. 135. 2. -3. 3. 39. -30. 30. 15.

(28) 3. -2. 14. 4 Normalisation. 5. -21 35. 21. 231. -55 15. 231. 429. 2.5.3 互補濾波器(Complementary filtering) 互補濾波器[11, 12]原理是把兩種雜訊特性不同的訊號，透過合適的比例相加做相互補償，以圖 2-14 為例，有兩個感測器同時量測一個物理量，但各自擁有不同的頻率雜訊，感測器 1 的輸出訊號  acc 為欲量測訊號 S 加上 N1，感測器 2 的輸出訊號  gyro 為欲量測訊號 S 加上 N2，N1 為一高頻雜訊，N2 為一低頻雜訊，將  acc 經過一低通濾波器之訊號與將  gyro 經過一高通濾波器之訊號做固定比例互補，如式 2-3: θ = (1 − a) ∗ (𝜃𝑔𝑦𝑟𝑜 + 𝜃) + a ∗ 𝜃𝑎𝑐𝑐. (2-3). 如此一來既可以改善高頻的干擾，也可消除低頻的誤差，且容易實現，降低控制器處理資料的時間，如圖 2-15，實際應用證實可以有效的抑制感測器誤差。. 圖 2-14 互補濾波器示意圖 H (來源:[13]) 16.

(29) 圖 2-15 互補濾波波型圖. 2.7 藍芽傳輸模組 HC-05. 圖 2-16 藍芽模組 HC-05 如圖 2-16 所示，由於藍芽模組預設鮑率為 9600bps，為使整個系統的鮑率一致，再使用前需先進入 AT 模式修改鮑率，更改過之後將不需再做修改，便可以使用藍芽傳送兩輪滑板車的狀態資訊回平板電腦。. 17.

(30) 2.8 霍爾感測器 3144-402. 圖 2-17 霍爾感測器 3144-402 CW. OFF ON. A Signal. B Signal. D CCW. 圖 2-18 增量型編碼器原理如圖 2-17 所示，霍爾元件會感測磁場變化，進而產生輸出脈波，本實驗平台因驅動器及馬達有許多製程上誤差，導致兩輪在接收相同 PWM 訊號會產生不同轉速，將兩霍爾感測器貼近馬達轉軸並產生相位差 90∘的兩脈波，如圖 2-18 經由脈波的變化可以測得兩輪旋轉數，達成一增量型編碼器的功能。 18.

(31) 2.9 軟體架構以下是兩輪滑板車與解碼單晶片的程式流程圖，圖 2-19 中一開始程式會先進行初始化，初始化過程中會先等待四秒讓系統電源穩定且感測器暖機後，進行十次計算角度的副程式判斷異常，之後繼續讀取三軸加速度計與陀螺儀，其分別使用系統核心單晶片內建 A/D Converter 取類比電壓資訊，陀螺儀的數值經由均值濾波處理訊號，加速度的數值經由 S-G 濾波處理高頻雜訊，將兩感測器量測之訊號經由互補濾波器按照比例整合計算車體角度，與距離資訊一起交由控制器計算合適的責任週期，來控制車體的角度與和物體間的間隔，最後將角度資訊加入測距儀命令格式中，經由串列傳輸控制測距儀掃描之角度，以便量測更新前方水平方向之距離。開始. 初始化. 互補濾波. 讀取感測器. 控制器. 均值濾波. 直流馬達. S-G濾波. 串列傳輸. 圖 2-19 控制器流程圖. 19.

(32) 雷射測距儀. Tx Rx. Tx. Rx 藍芽模組. 控制器. Tx Rx2 解碼單晶片. Rx1. Rx. Tx2. 圖 2-20 單晶片與雷射測距儀之架構如上圖 2-20 所示控制器將命令格式後，雷射測距儀會傳距離資訊給解碼單晶片其流程如下圖 2-21，首先會抓取回傳資訊的特徵，判斷測量狀態正常後將抓取編碼過之距離資訊並與驗證碼進行驗證做第二次異常判斷，若判斷正常便將資訊進行解碼，並整合經由藍芽模組設定的物體間隔傳回控制器進行距離資訊的更新。. 串列中斷開始. 判斷特徵. 抓取資料. 解碼驗證. 傳出資料. 等待中斷圖 2-21 測距儀解碼流程圖 20.

(33) 第三章. 兩輪滑板車之物體追隨控制. 3.1 比例積分微分在工業界和控制領域的實際應用中，傳統比例積分微分 (Proportional-Integral-Derivative controller, PID)控制器的發展應用十分純熟，是目前最常被業界廣為使用的控制方法，PID 控制器會將回授的狀態與參考值做相減比較，然後將誤差當作控制器的輸入值重新計算新的控制力，其目的是將系統誤差收斂為零，由於 PID 的各個單元輸出效果相當的明確，可以根據實驗結果與經驗法則去調整各單元參數，讓系統做到更快的收斂達到穩定。. 比例單元 P(proportional)、積分單元 I(integral)、微分單元 D(derivative)這三個控制單元為組成 PID 控制器的主要單元。比例控制器是常見且容易的控制方法，能將參考訊號與目前狀態之間的誤差乘上一個增益當作輸出的控制力，由於輸出會因誤差變大而上升，可以加快系統反應時間，但同時會使系統容易產生震盪降低系統穩定度，且從控制力的組成來看，當誤差等於零將完全不會有控制力;此時積分控制器能將過去的誤差不斷的累積，如同參考值與實際狀態之間所圍出來的面積，並乘上一個積分常數，當比例控制器達到穩態時，積分控制器能做到消除穩態誤差的效果，但這會影響系統的暫態響應;微分控制器能從誤差的微分，也就是判斷誤差變化的斜率進而領前做出反應，目的是為了抑制控制力的超調量，也就是增加系統的阻尼效應。其 PID 控制器的系統方塊圖如圖 3-1 所示，e 為參考訊號γ與系統狀態 y 間的誤差，數學表示式如下式 3-1. t. u (t )  k p e(t )  ki  e( )d  k d 0. 21. de(t ) dt. (3-1).

(34) 對式 3-1 做拉式轉換後，可以得到 PID 的傳遞函數方程式 3-2。其中 k p 為比例增益， TI 為時間積分常數， TD 是時間微分常數。. D( s ) .   U ( s) 1  k p 1   TD s  E ( s)  TI s . (3-2). 圖 3-1 PID 系統方塊 (來源:[4]) 由圖 3-2 所示各單元輸出控制力，PID 控制器會依照誤差變化，三個控制單元所輸出之控制力也隨之改變，依照受控系統的特性搭配調整三個控制單元的參數，便可以達到預期的控制性能。. 22.

(35) (a). (b). (c). (d). 圖 3-2(a)輸入參考訊號(b)輸出訊號(c)與參考訊號間誤差(d)誤差之微分. 3.2 模糊控制系統模糊的概念在日常生活中經常被人們使用，例如現在時間五點多、騎車慢點等，都是由一個模糊的概念去描述一個數值，每一個人對這一個描述的判斷也都不盡相同，1965 年美國柏克萊大學教授 L.A. Zadeh 發表了「Fuzzy Sets」的論文，這造就了之後模糊邏輯的理論，在 1974 年 E.H. Mamdani 首次將 Fuzzy 理論應用於蒸汽引擎的控制實驗。當在設計一個複雜且龐大的控制系統時，會花費許多時間在建構數學模型，模糊理論能簡化系統的複雜度，模糊邏輯系統架構如圖 3-3 23.

(36) 所示，將變量經過模糊化後，利用模糊規則描述模糊輸入與模糊輸出間的關係，經由解模糊將模糊數值轉變成明確的輸出，這些特點使模糊理論之應用已涵蓋各個領域，如影像變式處理、自動控制系統等，因此能以一個直覺且不需要受控體完整模型的方式建構一個符合期望的非線性控制器。. 模糊化機構. 模糊推論引擎. 解模糊化機構. 模糊規則庫圖 3-3 模糊邏輯系統方塊圖 3.2.1 模糊化當要將感測器測量到的數值輸入進模糊規則控制前，必須先轉換成模糊規則中所使用的模糊變數，這個動作稱為模糊化(Fuzzifier)。首先將輸入變數映射到每一個模糊集合的歸屬函數(Membership Function)，並計算輸入變數對模糊集合的歸屬度(Grade of Membership)，這可以知道輸入變數在此模糊集合佔有多少相關程度，一般常用的模糊集合表示法有以下三種，本研究輸入使用三角形模糊化，輸出使用單值模糊化: 1.. 三角形模糊化(Triangular fuzzifier).  x  a   1, if x   a  1, a   A1 ( x)   a  x   1, if x   a, a  1 0 ,otherwise . 24. (3-3).

(37) 圖 3-4 三角形模糊化 2.. 高斯函數模糊化(Gaussian fuzzifier). A2 x   ae  xb  / 2c 2. 2. (3-4). 圖 3-5 高斯函數模糊化 3.. 單值模糊化(singleton fuzzifier). 1, x  a A3   0,otherwise. 25. (3-5).

(38) 圖 3-6 單值模糊化 3.2.2 模糊推論引擎模糊規則庫與模糊推論引擎(Fuzzy Inference Engine)是模糊控制中的核心，模糊推論以類似人類的思考方式，從各個模糊化後的變數中計算出每一條規則的輸出，常使用的推論引擎有廣義肯定前提式(Generalized Modus Ponens, GMP)、廣義否定後論式(Generalized Modus Tollens, GMT)，在推論過程中所使用的方法大致分為四種為最大乘積合成(Max product operation)、最大-最小合成(Max-min operation)、最大激烈積合成(Max drastic product operation)、最大邊界積合成(Max bounded product operation)，本研究使用最大-最小合成其表示式如下式 3-6: min(𝑟(𝑥1 ), 𝑟(𝑥2 )). (3-6). 3.2.3 模糊規則庫模糊規則庫以許多 if-then 的描述，依照設計的專家經驗建構出輸入及輸出間的關係。常見的三種模糊規則形式有語意式模糊規則(Mamdani Fuzzy Rule)、函數式模糊規則(Sugengo Fuzzy Rule)、Tsukamoto 式模糊規則(Tsukamoto Fuzzy Rule)。以兩個輸入變數做例子設計一個語意式模糊規則，規則形式如式 3-7 所示:. 26.

(39) If x1 is A1 and x 2 is A2 then. u. (3-7). is y1. 一條規則被分為前件部與後件部， A1 、 A 2 是模糊集合的前件部， y1 為後件部， x1 、 x 2 為輸入訊號， u 為輸出訊號，將其代入倒單擺的應用，則規則會成為「如果角度誤差很大，且角速度誤差很小，則輸出控制力很大」。模糊規則庫的規則數則與輸入對應到的模糊集合數有關，一般一個變數所對應的模糊集合數量為七個，以倒單擺的例子設計規則庫則如表 3-1 所示: 表 3-1 模糊規則庫. . NB. NS. ZO. PS. PB. NS. NS. NS. PS. PS. PB. ZO. NB. NS. ZO. PS. PB. PS. NB. NS. NS. PS. PS. . 3.2.4 解模糊化經過模糊控制系統計算出一個輸出的模糊集合後，最後必須將模糊集合轉換成一個最適合此集合的數值當作此模糊系統的輸出控制力，這過程稱為解模糊化，其中較具代表性的解模糊化方式有重心解模糊化法 (Center of Gravity Defuzzification)、中心平均值解模糊化法(Center Average Defuzzification)、加權平均解模糊化法(Weighted Average Defuzzification)、高度解模糊化法(Height Method Defuzzification) 、最大值平均解模糊化法 (Midpoint of Maxima Defuzzification) 、最大面積和之中心解模糊化法 (Center of Largest Area Defuzzification)，本研究使用重心解模糊化法，𝜇∗ 為解模糊化後之重心值，𝜇𝑖 為輸出集合的隸屬函數，n為規則數，𝑟為第 i 個規則權重，其公式如式 3-8:. 27.

(40) ∑𝑛𝑖=1 𝜇𝑖 𝑟(𝜇𝑖 ) μ = 𝑛 ∑𝑖=1 𝑟(𝜇𝑖 ) ∗. (3-8). 3.3 兩輪滑板車動態方程式兩輪滑板車的之動態方程[14]可以從牛頓第二定律分析，而控制原理是以倒單擺為基礎，如圖 3-7 所示，當滑板的傾角誤差大於零時，控制器則提供一個向前的控制力;但若滑板傾角誤差小於零時，控制器則向後提供一個控制力來維持滑板的平衡，其考慮到的變數如表 3-2 所示。. 表 3-2 動態方程變數表符號. 單位. 物理意義. 符號. 單位. 物理意義. g. m s2. 重力加速度. Iw. kgm2. 輪轉動慣量. . rad. 滑板傾角. Fp. N. 輪轉之正向力. . rad. 輪轉動角度. Fa. N. 輪轉之摩擦力. Tm. Nm. 輪產生之扭力. Ff. N. 輪對地面摩擦力. mp. kg. 質量. Fnw. N. 輪對地面正向力. mw. kg. 輪質量. kt. Nm A. 馬達扭力常數. m. 輪半徑. u. V. 控制輸入. Vs rad. 馬達反電勢. rm. ohm. 馬達內阻. rw kv. 28.

(41) 圖 3-7 兩輪滑板車示意圖 (資料參考來源:[15]).  . .   rw m p rp2  2 2   rm m p rw  mw rw    m p rp rm.  0     g  rw2 2    2 2        rw    0    2     rw   rw g     2 rw 2. .  0    2k t k v   rw        2k t k v    rw    0 0         2 rw 2    2 rw 2 u     0 0 0 1         2k t k v   rw      2k t k v    rw           2 rw 2 0 0   2 2     rw 1 0. (3-9). 0. 29. (3-10).

(42) 由於 x  rw ，將此動態方程改寫為式 3-11.    1        0  x   0     x  0. 0. 0. 1. 0. 0. rw. 0. 0. 0      0   0      rw  . (3-11). 本論文以此動態模型模擬並尋找控制器參數，證明能使兩輪滑板車的狀態收斂至穩定，接著以模擬的參數代入至微控制器來測試其控制器的結果。. 3.4 模擬結果本研究希望兩輪滑板車能在可擺動的角度範圍內，進行移動到定點的動作。但由於雷射測距儀的測量頻率比控制器的執行頻率要小的多，所以將三軸加速度計與陀螺儀量測出的角度誤差值 e 與角速度誤差值 e 固定使用 PID 控制器去快速維持載體角度，另將由雷射測距儀所量測到的距離與微分所得到的速度則分別使用 PID 控制器及模糊控制器得到適當的 PWM 輸出控制力來改變兩輪滑板車的角度與間隔，最後將兩種控制架構與兩種控制器組合作比較。首先使用 MATLAB 模擬兩輪滑板車在四種方法的控制器下的控制結果。在進行移動追隨模擬之前，必須擁有一個可以維持移動車平衡的控制器，以 MATLAB 模擬初始車體角度為向後傾斜 0.3 徑度(約 17 度)，PID 控制器所選用的參數為: k p =5、 k i =6、 k d =0.9，而車身角度與時間的關係如圖 3-8 所示。. 30.

(43) 圖 3-8 使用 PID 控制器之車身傾斜角度. 3.4.1 模糊控制器本實驗先使用模糊控制器去模擬兩輪滑板車之物理特性在各個情況下所應該對應到的輸出大小，首先將輸入變數對應的模糊集合以三角形模糊化表示，輸出則使用模糊單值簡化計算的複雜度，其模糊規則庫核心建構由 Mamdani 模糊模型並進行最大-最小合成法計算出模糊輸出值，最後將計算出的模糊輸出值經由重心解模糊化法轉變成明確的值給馬達。. 31.

(44) A. 模糊-PID 雙迴路控制架構 r(t). +. +. Fuzzy. +. -. x Two-Wheeled Mobile Robots θ. PID. 圖 3-9 模糊-PID 雙迴路控制架構如圖 3-10 所示，將輸入的距離誤差訊號採用三角型模糊化劃分為五個部分，速度則如圖 3-11 所示劃分為三個部分，其組合分別代表兩輪滑板車在行進過程中所會遇到的情況，如圖 3-12 所示，模糊輸出則採用五個單值輸出，分別代表不同的責任週期，如表 3-3 所示，模糊規則為 15 條，經由模擬所調整的經驗累積最終將成為專家經驗去進行實作。. 圖 3-10 輸入間隔歸屬函數. 32.

(45) 圖 3-11 輸入速度歸屬函數. 圖 3-12 輸出 PWM 歸屬函數. 33.

(46) 表 3-3 模糊規則庫. x. NB. NS. ZO. PS. PB. NB. NB. NS. NS. ZO. PS. ZO. NS. NS. ZO. PS. PS. PB. NS. ZO. PS. PS. PB. x. 模擬條件在初始角度設定為 0.3 徑度(約 17 度)，位移參考訊號為 3 公尺，在模擬的過程中可以得知在進行追隨控制時，在雙迴路的控制架構下，維持車體平衡的積分控制器會與其他用於追隨的控制器互相牴觸，進而將控制力抵銷，如下圖 3-13、圖 3-14。. 圖 3-13 模糊-PID 雙迴路架構之車身傾角模擬. 34.

(47) 圖 3-14 模糊-PID 雙迴路架構之追隨模擬因為在倒單擺的架構中，沒辦法在維持車體平衡的狀態下同時使車體移動，故在之後的雙迴路架構中，都將把積分控制器給移除，只使用 PD 控制維持車體平衡，在移除積分控制後，PID 參數為: k p =4.8、 k d =0.9，車體的位移與角度的變化如圖 3-15、圖 3-16 所示，模擬結果顯示角度約在 35 秒時收斂為 0 度，車體位置則在約 30 秒時到達定點。. 35.

(48) 圖 3-15 模糊-PID 雙迴路架構之車身傾角收斂模擬. 圖 3-16 模糊-PID 雙迴路架構之物體追隨模擬 36.

(49) B. 模糊-PID 串聯式控制架構 r(t). +. +. Fuzzy -. PID +. Two-Wheeled x Mobile θ Robots. 圖 3-17 模糊-PID 串聯式控制架構如圖 3-18 所示，一樣將輸入的距離誤差訊號採用三角型模糊化劃分為五個部分，但由於這一級控制輸出會成為後一級的參考訊號，故在劃分上會比前一個架構要來的謹慎，而速度則如圖 3-19 所示劃分為三個部分，模糊輸出則採用五個單值輸出，如圖 3-20 所示分別代表不同的角度追隨參考訊號，如表 3-4 所示，模糊規則為 15 條，所使用的 PID 參數為: k p =4、 k i =2、 k d =0.9。. 圖 3-18 輸入間隔歸屬函數 37.

(50) 圖 3-19 輸入速度歸屬函數. 圖 3-20 輸出後一級 PID 之參考訊號 38.

(51) 表 3-4 模糊規則庫. x. NB. NS. ZO. PS. PB. NB. NB. ZO. ZO. ZO. PS. ZO. NB. NS. ZO. PS. PB. PB. NS. ZO. ZO. ZO. PB. x. 圖 3-21 模糊-PID 串聯式控制架構之車身傾角收斂模擬. 39.

(52) 圖 3-22 模糊-PID 串聯式控制架構之追隨收斂模擬經由模擬結果如圖 3-21、圖 3-22 所示，串聯式架構的角度約在 38 秒時收斂為 0 度，車體位置則在約 40 秒時到達定點。 3.4.2PID 控制器 A. 雙 PID 迴路控制架構 r(t). +. +. PID1. x Two-Wheeled Mobile Robots θ. +. -. PID2. 圖 3-23 雙 PID 迴路控制架構由於機構的關係，車體的角度範圍約在  60 ~ 60 ，使得角度誤差及控制的範圍能有一定的上限，而距離則隨著感測器的規格擁有很高的測量範圍，因此在模擬及實作的過程中會基於收斂速度、追隨精度與安全的考量，以下 PID 設計會為 40.

(53) 距離誤差設定一個上限，此模擬的距離誤差上限為兩公尺，如圖 3-24、圖 3-25 所示，模擬參數為: k p1 =1.6、 k i1 =0.2、 k d 1 =0.6; k p 2 =5、 k i 2 =0、 k d 2 =2.9。. 圖 3-24 雙 PID 迴路控制架構之車身傾角收斂模擬. 圖 3-25 雙 PID 迴路控制架構之追隨收斂模擬 41.

(54) 經由模擬結果所示，雙 PID 迴路架構的角度約在 40 秒時收斂為 0 度，車體位置則在約 40 秒時到達定點。. B. 串聯式 PID 控制架構 +. r(t) +. PID1 -. PID2 +. x Two-Wheeled θ Mobile Robots. 圖 3-26 串聯式 PID 控制架構此架構 PID1 所輸出的訊號為 PID2 的參考訊號，而 PID2 則會輸出責任週期改變車體角度，故在此架構下設定距離誤差上限為兩公尺，如圖 3-27、圖 3-28 所示，模擬參數為: k p1 =0.3、 k i1 =0、 k d 1 =0.01; k p 2 =5、 k i 2 =3、 k d 2 =1。. 圖 3-27 串聯式 PID 控制架構之車身傾角收斂模擬 42.

(55) 圖 3-28 串聯式 PID 控制架構之位置收斂模擬經由模擬結果所示，串聯式 PID 架構的角度約在 50 秒時收斂為 0 度，車體位置則在約 50 秒時到達定點。. 43.

(56) 第四章. 實驗與討論. 本章節中，主要是介紹兩輪滑板車的實驗結果，其中包含各個控制器追蹤與追隨動態物體之實驗，先將前章節模擬過的控制器與架構分別實現於實驗平台上，比較實驗結果與模擬間的差距，並加以微調參數，而後比較不同的控制器與架構之間的收斂時間與穩態誤差。. 4.1 實驗結果由於實際測試後得知載具在移動過程中，即使控制器以不再輸出 PWM 訊號，兩輪滑板車也會因慣性作用而持續往前滑行，而這個現象在模型中是沒有如此明顯的，所以在實作一組之後控制器，PID 控制器會將其中微分控制參數加大，而模糊控制器則會再將速度的歸屬函數從三個分割成五個。 A. 模糊-PID 串聯式控制架構 r(t). +. +. Fuzzy -. PID +. Two-Wheeled x Mobile θ Robots. 圖 4-1 模糊-PID 串聯式控制架構 . 範例一首先依照模擬結果先決定其距離誤差、速度與輸出單值的歸屬函數如圖 4-2、. 圖 4-3、圖 4-4 所示，並建立一個模糊規則庫如表 4-1 所示並進行微調，其規則數為 15 條規則，PID 參數為: k p =4.8、k d =0.9、k i =6 初始位置為距離離牆 5.5 公尺，以藍芽模組設定追隨間隔為距離離牆兩公尺處，滑板車約在 10 秒後追隨至指定間隔，與此同時角度達到穩定如圖 4-5、圖 4-6 所示，而此控制器在移動速度較高時，因關於速度的歸屬函數較少，沒有對速度方面做更多判斷，使滑板車無法 44.

(57) 及時停止移動而產生較大的震盪。. 圖 4-2 輸入間隔歸屬函數. 45.

(58) 圖 4-3 輸入速度歸屬函數. 圖 4-4 輸出後一級 PID 之參考訊號歸屬函數 46.

(59) 表 4-1 模糊-PID 串聯式控制架構之模糊規則庫. x. NB. NS. ZO. PS. PB. NB. NS. NB. NS. NS. ZO. ZO. NS. NS. ZO. PS. PS. PB. ZO. PS. PS. PB. PS. x. 圖 4-5 模糊-PID 串聯式控制架構之車身傾角. 47.

(60) 圖 4-6 模糊-PID 串聯式控制架構之車身位置. . 範例二因此將速度的歸屬函數從三個分割成五個，速度的歸屬函數如圖 4-7 所示，. 規則也因此多了十條如表 4-2，新增規則後在其他條件不變的情況下，如圖 4-9 所示兩輪滑板車在約 7 秒時到達目標位置，而後角度約在 9 秒時到達穩定，收斂速度明顯提升且擁有較小的超調量，另外如圖 4-11 所示，在 4 秒加入一個人為的干擾，約在 15 秒進入穩態誤差。. 48.

(61) 圖 4-7 輸入速度歸屬函數之修改. 表 4-2 新增規則之模糊規則庫. x. NB. NS. ZO. PS. PB. NB. NS. NB. NS. NS. ZO. NS. NS. PS. PS. PS. PS. ZO. NS. NS. ZO. PS. PS. PS. NS. NS. NS. NS. PS. PB. ZO. PS. PS. PB. PS. x. 49.

(62) 圖 4-8 模糊-PID 串聯式控制架構之車身傾角. 圖 4-9 模糊-PID 串聯式控制架構之車身位置. 50.

(63) 圖 4-10 加入人為外部干擾之車身傾角. 圖 4-11 加入人為外部干擾之車身位置. 51.

(64) 表 4-3 規則數與收斂情況比較 15 條規則. 25 條規則. 收斂速度(s). 10. 7. 超調量(cm). -50. 6.5. 穩態誤差(cm). -12. 7. . 範例三由於車體的擺動與感測器的雜訊會影響量測的數值，進而使滑板車的角度有. 上下的震盪，所以在歸屬函數的形狀上做些修正如圖 4-12，並加 PID 中的積分控制器參數改為 3，如圖 4-14 所示，約在 13 秒時收斂，穩態誤差為-13 公分，如圖 4-16 所示，在 21 秒與 34 秒加入人為干擾最終都能收斂至穩定。. 圖 4-12 輸入位置歸屬函數之修改. 52.

(65) 圖 4-13 模糊-PID 串聯式控制架構之車身傾角. 圖 4-14 模糊-PID 串聯式控制架構之車身位置. 53.


(67) 實驗截圖. 動作說明尚未以平板輸入間隔資訊前，滑板車維持穩定平衡不動。. 以平板輸入間隔資訊後第 0 秒，滑板車改變傾斜角度。. 第五秒接近設定間隔距離，車身維持平衡，車體持續移動。. 第八秒車體移動超出設定間隔距離，車身改變角度向後移動。. 第十二秒車體穩定停在設定間隔距離。. 圖 4-17 模糊-PID 串聯式控制架構實驗過程 55.

(68) B. 模糊-PID 雙迴路控制架構 r(t). +. +. Fuzzy. -. +. x Two-Wheeled Mobile Robots θ. PID. 圖 4-18 模糊-PID 雙迴路控制架構. 圖 4-18 中 PID 參數為: k p =4.8、 k i =0、 k d =0.9，如圖 4-19 所示，滑板車在約 14 秒時穩定，穩態誤差為-24 公分，如圖 4-23 所示，在 12 秒到達追隨位置後，在 22 秒加入一人為干擾，最終在 32 秒回到穩定. 圖 4-19 輸出 PWM 歸屬函數 56.

(69) 表 4-4 模糊-PID 雙迴路控制架構之模糊規則庫. x. NB. NS. ZO. PS. PB. NB. NB. NB. NB. NS. ZO. NS. NS. ZO. PS. PB. PB. ZO. NS. NS. ZO. PS. PS. PS. NB. NB. NS. ZO. PS. PB. ZO. PS. PB. PB. PB. x. 圖 4-20 模糊-PID 雙迴路控制架構之車身傾角. 57.

(70) 圖 4-21 模糊-PID 雙迴路控制架構之車身位置. 圖 4-22 加入人為外部干擾之車身傾角 58.

(71) 圖 4-23 加入人為外部干擾之車身位置. 59.

(72) 實驗截圖. 動作說明尚未以平板輸入間隔資訊前，滑板車維持穩定平衡不動。. 以平板輸入間隔資訊後第 0 秒，滑板車改變傾斜角度. 第二秒維持車身傾斜向前移動。. 第五秒接近設定間隔距離，車身維持平衡，車體持續移動。. 第十秒車體穩定停在設定間隔距離。. 圖 4-24 模糊-PID 雙迴路控制架構實驗過程 60.

(73) C. 串聯式 PID 控制架構 +. r(t) +. PID1. PID2 +. -. x Two-Wheeled θ Mobile Robots. 圖 4-25 串聯式 PID 控制架構. . 範例一上圖 4-25 所示，PID1 參數為: k p1 =0.0017、 k i1 =0、 k d 1 =0.03，PID2 參數為: k p 2. =5、 k i 2 =6、 k d 2 =0.9，如圖 4-27 滑板車約在 18 秒到達追隨位置並穩定，與之同時角度也收斂完成。. 圖 4-26 串聯式 PID 控制架構之車身傾角 61.

(74) 圖 2-27 串聯式 PID 控制架構之車身位置. . 範例二. 由上圖可得知須將微分控制參數修改提高以改善超調量，所以將 PID1 參數為: k p1 =0.0017、k i1 =0、 k d 1 =0.07，PID2 參數為: k p 2 =5、k i 2 =6、 k d 2 =0.9，如圖 4-29 所示，滑板車約在 7 秒達到預定間隔並維持穩定，穩態誤差為 23 公分，如圖 4-31 所示，當滑板車於 10 秒穩定後，在 21 秒給予人為干擾，9 秒後再次回復穩定。. 62.

(75) 圖 4-28 串聯式 PID 控制架構之車身傾角. 圖 4-29 串聯式 PID 控制架構之車身位置. 63.


(77) 實驗截圖. 動作說明尚未以平板輸入間隔資訊前，滑板車維持穩定平衡不動。. 以平板輸入間隔資訊後第 4 秒，滑板車維持傾斜角度，車體向前移動。. 第六秒超出設定間隔距離，車身改變角度向後移動。. 第八秒接近設定間隔距離，車身維持平衡，車體持續移動。. 第十秒車體穩定停在設定間隔距離。. 圖 4-32 串聯式 PID 控制架構實驗過程 65.

(78) D. PID 雙迴路控制架構 r(t). +. +. PID1. x Two-Wheeled Mobile Robots θ. +. -. PID2. 圖 4-33 PID 雙迴路控制架構. 圖 4-33 中 PID1 參數為: k p1 =0.6、 k d 1 =40，PID2 參數為: k p 2 =4、 k d 2 =0.9，如圖 4-35 所示滑板車約在 11 秒時達到預定間隔並穩定，穩態誤差為 50 公分，如圖 4-37 所示，滑板車在約 9 秒穩定後，在第 14 秒給予一人為的干擾，在 9 秒後回復穩定。. 圖 4-34 PID 雙迴路控制架構之車身傾角. 66.

(79) 圖 4-35 PID 雙迴路控制架構之車身位置. 圖 4-36 加入人為外部干擾之車身傾角. 67.

(80) 圖 4-37 加入人為外部干擾之車身位置. 68.

(81) 實驗截圖. 動作說明尚未以平板輸入間隔資訊前，滑板車維持穩定平衡不動。. 以平板輸入間隔資訊後第 0 秒，滑板車改變傾斜角度。. 第六秒接近設定間隔距離，車身角度隨距離接近而縮小，車體持續移動。. 第八秒接近設定間隔距離，車身維持平衡，車體持續移動。. 第十秒車體穩定停在設定間隔距離。. 圖 4-38PID 雙迴路控制架構實驗過程 69.

(82) 表 4-5 控制器比較表模糊-PID 雙迴. 模糊-PID 串聯. PID 雙迴路控. 串聯式 PID 控. 路控制架構. 式控制架構. 制架構. 制架構. -13. -24. 50. -23. 10. 14.4. 11. 8. 穩態誤差 (cm) 收斂時間 (sec). 70.

(83) 第五章. 結論與未來展望. 5.1 結論本論文將不同控制架構及方法搭配系統核心單晶片、紅外線測距儀、三軸加速度計及陀螺儀將兩輪車之物體追隨實現於兩輪滑板車上，由於本實驗平台為倒單擺系統，在控制領域上被廣泛應用於驗證其控制器的效能，以此更能突顯其控制器之優缺點，而所選擇的控制方法屬於被業界大量使用的，與控制架構互相搭配研究其特性被能因應不同情況作出選擇。由實驗結果可以得知，雙迴路架構可直覺式的設計，製作與實現上也較容易，但由於系統輸入是多個控制器輸出之結合，故輸出則會相互影響，導致控制之穩定性容易不如預期，而串聯式設計架構則因系統的輸入控制力單一，不管是精度，收斂速度及穩定性都優於雙迴路架構，但因後一級之參考訊號為前一級之輸出，在設計與實作上較不這麼直覺及容易，經由實驗和模擬證明本論文所設計之控制器能夠有效的控制兩輪滑板車的姿態與位置。. 5.2 未來展望由於研究平台為自行研究設計，許多方向是未來可以發展及改進的地方。 1. 透過區域地圖建置，本研究可發展至區域平面路線的巡邏與導航。 2. 自適應控制器能更快搜尋到良好的控制參數，並適用於不同的動態模型系統，如載人載物時重量動態模型的改變，抑或是當載具設備擁有不對稱的誤差，為了不喪失控制效果，自適應控制器也是未來必發展的議題。 3. 由於目前使用雷射測距儀的掃描速度及範圍能應用於室內，但若是要其他更嚴苛的動作則須改用其他感測器作替代。. 71.

(84) 參考文獻 [1] [2] [3]. [4]. [5]. [6]. [7]. [8]. [9]. [10]. [11] [12] [13] [14] [15] [16]. SEGWAY，http://www.segway.com/ 小米網，http://www.mi.com/tw/ C. Ye and J. Borenstein, "Obstacle avoidance for the segway robotic mobility platform," in ANS 10th Int. Conf. on Robotics and Remote Systems for Hazardous Environments, 2004, pp. 107-114. C.-H. Huang, W.-J. Wang, and C.-H. Chiu, "Design and implementation of fuzzy control on a two-wheel inverted pendulum," IEEE Transactions on Industrial Electronics, vol. 58, pp. 2988-3001, 2011. T. H. S. Li, C. Shih-Jie, and T. Wei, "Fuzzy target tracking control of autonomous mobile robots by using infrared sensors," IEEE Transactions on Fuzzy Systems, vol. 12, pp. 491-501, 2004. W. Li-Xin, "Stable adaptive fuzzy controllers with application to inverted pendulum tracking," IEEE Transactions on Systems, Man, and Cybernetics, Part B (Cybernetics), vol. 26, pp. 677-691, 1996. S. J. Mou and F. Liu, "Trajectory tracking control of the planar inverted pendulum," in 2015 IEEE Advanced Information Technology, Electronic and Automation Control Conference (IAEAC), 2015, pp. 364-368. W. Jia-Jun, "Position and speed tracking control of inverted pendulum based on double PID controllers," in Control Conference (CCC), 2015 34th Chinese, 2015, pp. 4197-4201. W. Jia-Jun, "Stabilization and tracking control of X-Z inverted pendulum based on PID controllers," in Control Conference (CCC), 2015 34th Chinese, 2015, pp. 4202-4207. A. Savitzky and M. J. Golay, "Smoothing and differentiation of data by simplified least squares procedures," Analytical chemistry, vol. 36, pp. 1627-1639, 1964. Piter-Jan，http://www.pieter-jan.com/node/11 W. T. Higgins, "A comparison of complementary and Kalman filtering," IEEE Transactions on Aerospace and Electronic Systems, vol. 11, pp. 321-325, 1975. Rapot，http://rapot2014.blogspot.tw/ 汪志宇，兩輪移動車模糊控制，臺灣師範大學碩士論文， 2012。溫彥侯，兩輪式自我平衡車之控制器設計與實現，臺灣師範大學碩士論文， 2013。孫仕勳，於能量螞蟻演算法之路徑規劃與其在雲端平台運算的實現，臺灣師範大學碩士論文， 2015。 72.

(85) [17] [18] [19]. 黃良吉，GPS 與感測器整合於三維地面車輛定位之應用，臺灣科技大學碩士論文， 2005。高嘉良，移動機器人之階層模糊邏輯控制，臺灣師範大學碩士論文， 2012。 P. A. Gorry, "General least-squares smoothing and differentiation by the convolution (Savitzky-Golay) method," Analytical Chemistry, vol. 62, pp. 570-573, 1990.. 73.

(86) 自傳本人李冠東，出生於台北，在一個幸福又溫暖的家庭中長大。年幼時因父母工作的關係，曾頻繁地在不同地方居住，直到學齡後慢慢定居於三重，在求學過程中一路從育才國小、厚德國小、三和國中、格致中學、虎尾科技大學，之後到師範大學就讀，過程中隨著學校中各個老師的啟發與教導，讓我對學習新知擁有一定的欲望，在大學後充分學習各個領域，增加知識的廣度，並在研究所加深對專業領域的深度，讓我隨著年齡與學歷的增長的同時，也充實了我的知識與內在。在這求學的路途中所遇到的每一個人，也提供了我許多專業領域外的智慧，希望未來可以不愧對所有栽培我的人，將所學習的經驗與憑藉自己的能力用於幫助更多人，為社會的進步盡一份力。. 學術成就 [1]. K.-T. Lee, S.-H. Huang, S.-H. Sun, \and Y.-G. Leu, "Realization of an Energy-Based Ant Colony Optimization Algorithm for Path Planning," in IOS Press, 2015.. 74.

(87)