兩輪自平衡車平衡控制系統(tǒng)的研究

兩輪自平衡車平衡控制系統(tǒng)的研究,平衡,控制系統(tǒng),研究,鉆研 ……………………. ………………. ………………… 裝 訂 線 ……………….……. …………. …………. ……… 山 東 農(nóng) 業(yè) 大 學(xué) 畢 業(yè) 論 文 兩輪自平衡車平衡控制系統(tǒng)的研究 院 部 機械與電子工程學(xué)院 專業(yè)班級 電氣工程及其自動化 2 班 屆 次 2015 屆 學(xué)生姓名 王恒 學(xué) 號 20116115 指導(dǎo)教師 婁偉 副教授 二 О 一五年六月六日 i 目 錄 摘要 ..................................................................................................................................................I Abstrat.............................................................................................................................................II 引言 .................................................................................................................................................1 1 緒論 .............................................................................................................................................1 1.1 兩輪自平衡車的研究背景及研究意義 ...............................................................................1 1.1.1 兩輪自平衡車的研究背景 ............................................................................................1 1.1.2 兩輪自平衡車的研究意義 ............................................................................................2 1.2 兩輪自平衡車的研究現(xiàn)狀 ...................................................................................................2 1.2.1 兩輪自平衡車的國外研究現(xiàn)狀 ....................................................................................2 1.2.2 兩輪自平衡車的國內(nèi)研究現(xiàn)狀 ....................................................................................3 1.3 本課題的研究內(nèi)容及目標 ...................................................................................................4 2 自平衡系統(tǒng)的分析 .....................................................................................................................4 2.1 控制目標 ...............................................................................................................................4 2.2 自平衡系統(tǒng)的控制學(xué)原理分析 ...........................................................................................5 2.2.1 自平衡小車平衡原理解析 ............................................................................................5 2.2.2 自平衡小車的運動學(xué)模型分析 ....................................................................................7 2.2.3 自平衡小車的數(shù)學(xué)建模 ................................................................................................9 3 系統(tǒng)硬件組成 ...........................................................................................................................11 3.1 慣性測量單元 .....................................................................................................................11 3.1.1 MPU6050 芯片介紹 ......................................................................................................11 3.1.2 MPU6050 數(shù)據(jù)處理 ......................................................................................................12 3.1.3 數(shù)據(jù)濾波算法 ..............................................................................................................16 3.1.4 MPU6050 驅(qū)動電路 ......................................................................................................20 3.2 主控單元 .............................................................................................................................20 3.3 電源模塊 .............................................................................................................................21 3.3.1 LM2596 芯片介紹 ........................................................................................................21 3.3.2 AMS1117 芯片介紹 ......................................................................................................22 3.4 電機驅(qū)動模塊 .....................................................................................................................23 3.5 編碼器模塊 .........................................................................................................................24 4 系統(tǒng)程序及算法 .......................................................................................................................25 4.1 MPU6050 獲取數(shù)據(jù) ............................................................................................................25 4.1.1 MPU6050 初始化 ..........................................................................................................26 4.1.2 主函數(shù)中的姿態(tài)數(shù)據(jù)處理 ..........................................................................................26 5 兩輪自平衡車平衡控制的總結(jié) ...............................................................................................35 ii 5.1 設(shè)計的主要內(nèi)容 .................................................................................................................35 5.2 設(shè)計的不足 .........................................................................................................................36 參考文獻 .......................................................................................................................................37 致 謝 .......................................................................................................................................38 iii Contents Abstract ...........................................................................................................................................II Introduction .....................................................................................................................................1 1 Exordium ......................................................................................................................................1 1.1 The Background of self-balanced car....................................................................................1 1.1.1 The Background .............................................................................................................1 1.1.2 Research meaning of self-balance car ............................................................................2 1.2 The present situation of research of self-balance ..................................................................2 1.2.1 Internetional situation .....................................................................................................2 1.2.2 The research situation of China ......................................................................................3 1.3 Goal of this research ..............................................................................................................4 2 Analysis of self-balance system ...................................................................................................4 2.1 The purpose of control system...............................................................................................4 2.2 Control Science Analysis.......................................................................................................5 2.2.1 Balance Theory of self-balanced car ..............................................................................5 2.2.2 Kinesiology model analysis............................................................................................7 2.2.3 Mathematical modeling ..................................................................................................9 3 Hardware component..................................................................................................................11 3.1 Inertial measurment uint......................................................................................................11 3.1.1 MPU6050 introduction .................................................................................................11 3.1.2 Date processing of MPU6050.......................................................................................12 3.1.3 Algorithm of data filter.................................................................................................16 3.1.4 Drive circuit of MPU6050............................................................................................20 3.2 Main control unit .................................................................................................................20 3.3 Power unit............................................................................................................................21 3.3.1 Introduction of LM2596 ...............................................................................................21 3.3.2 Introduction of AMS1117 ............................................................................................22 3.4 Motordriver unit ..................................................................................................................23 3.5 Encoder unit.........................................................................................................................24 4 Program and algorithm ...............................................................................................................25 4.1 MPU6050 get data ...............................................................................................................25 4.1.1 Initial of MPU6050.......................................................................................................26 4.1.2 processing gesture data in main functiong ...................................................................26 5 Summarize of balance controling of self-balance car.................................................................35 5.1 Main substance of this project .............................................................................................35 iv 5.2 Blemish of this project.........................................................................................................36 Reference documentation ..............................................................................................................37 Thanks ...........................................................................................................................................38 I 兩輪自平衡車平衡控制系統(tǒng)的研究 王恒 （山東農(nóng)業(yè)大學(xué) 機械與電子工程學(xué)院 泰安 271018） 摘要：自從在 2008 年北京奧運會上的精彩亮相，兩輪自平衡車在中國市場已經(jīng)逐漸為大眾所接受。節(jié) 能環(huán)保、便攜新穎，兩輪自平衡車以其獨特的優(yōu)勢博取人們的青睞。又兩輪自平衡小車以其一級倒立 擺結(jié)構(gòu)成為控制算法驗證實現(xiàn)的獨特平臺。因此，兩輪自平衡小車有較高的研究價值。本設(shè)計的重點 是抓住兩輪自平衡車的基礎(chǔ)即平衡控制進行系統(tǒng)設(shè)計，結(jié)合姿態(tài)解算數(shù)據(jù)融合算法驅(qū)動直流電機以實 現(xiàn)兩輪小車的平衡控制。 關(guān)鍵詞：自平衡 姿態(tài)解算 數(shù)據(jù)濾波 倒立擺 II The research of self-balanced car control system Heng Wang (Mechanical desture analysis; single data filter; inverted pendulum structure 1 引言 新型交通方式越來越吸引人們的注意，北京奧運會上雙輪自平衡車的亮相讓人們眼 前一亮。以電池作能源，清潔無污染，便攜、靈活、新穎等優(yōu)勢使自平衡車迅速博得消 費者的青睞。移動機器人是機器人學(xué)的一個重要分支，對于移動機器人的研究，包括輪 式、腿式、履帶式以及水下式機器人等，可以追溯到 20 世紀 60 年代。移動機器人得到 快速發(fā)展有兩方面原因：一是其應(yīng)用范圍越來越廣泛；二是相關(guān)領(lǐng)域如計算、傳感、控 制及執(zhí)行等技術(shù)的快速發(fā)展。移動機器人尚有不少技術(shù)問題有待解決，因此近幾年對移 動機器人的研究相當活躍。 近年來，隨著移動機器人研究不斷深入、應(yīng)用領(lǐng)域更加廣泛，所面臨的環(huán)境和任務(wù) 也越來越復(fù)雜。機器人經(jīng)常會遇到一些比較狹窄，而且有很多大轉(zhuǎn)角的工作場合，如何 在這樣比較復(fù)雜的環(huán)境中靈活快捷的執(zhí)行任務(wù)，成為人們頗為關(guān)心的一個問題。雙輪自 平衡機器人概念就是在這樣的背景下提出來的。兩輪自平衡小車是一個高度不穩(wěn)定兩輪 機器人，是一種多變量、非線性、強耦合的系統(tǒng)，是檢驗各種控制方法的典型裝置。同 時由于它具有體積小、運動靈活、零轉(zhuǎn)彎半徑等特點，將會在軍用和民用領(lǐng)域有著廣泛 的應(yīng)用前景。 因為它既有理論研究意義又有實用價值，所以兩輪自平衡小車的研究在最近十年引起了 大量機器人技術(shù)實驗室的廣泛關(guān)注。 由于特殊的兩輪結(jié)構(gòu)，自平衡小車本身就是一個不穩(wěn)定系統(tǒng)。如何讓雙輪小車在保 持穩(wěn)定的基礎(chǔ)上實現(xiàn)前后行駛以及自由轉(zhuǎn)向，成為一個技術(shù)上的問題。解決這一問題， 將要用到機械設(shè)計和自動化專業(yè)的知識，涉及電子、機械、運動學(xué)、控制學(xué)等多學(xué)科知 識。 雙輪自平衡車系統(tǒng)可進一步抽象化為一級倒立擺結(jié)構(gòu)。經(jīng)過對一級倒立擺結(jié)構(gòu)的數(shù)學(xué)建 模和運動學(xué)分析，結(jié)合控制學(xué)原理，可以得出使雙輪自平衡車保持動態(tài)穩(wěn)定的幾個條件。 （詳見第二章 自平衡系統(tǒng)的分析） 雙輪自平衡車平衡控制系統(tǒng)以 STM32F103C8T6 芯片作為主控芯片運用姿態(tài)傳感器 MPU6050，結(jié)合數(shù)據(jù)濾波算法進行數(shù)據(jù)融合，建立閉環(huán)控制系統(tǒng)驅(qū)動直流電機。再用編 碼器實時監(jiān)測電機轉(zhuǎn)速以反饋給主控系統(tǒng)，形成一個小閉環(huán)。最終能夠?qū)崿F(xiàn)雙輪自平衡 車的平衡控制。 （詳見第三章 系統(tǒng)硬件組成） 1 緒論 雙輪自平衡車是輪式機器人的一種，兩輪自平衡機器人技術(shù)涉及機械、控制、力學(xué)、 微電子等多個領(lǐng)域，本章從實際應(yīng)用角度出發(fā)，闡述了兩輪自平衡車的實際研究價值及 廣闊的研究前景。 1.1 兩輪自平衡車的研究背景及研究意義 2 1.1.1 兩輪自平衡車的研究背景 在私家車遍及千萬家庭的當今社會，人們在面對長途旅行時可以有多樣化的選擇， 但是中短途旅行所能做出的選擇卻捉襟見肘。自平衡小車以及新穎性、趣味性、便攜性、 實用性，正在吸引越來越多的人的關(guān)注。 兩輪自平衡車以其體積小、靈活度高等優(yōu)勢，進入市場便得到大眾的青睞，眾多公 司中又以 Segway 公司的產(chǎn)品更為大眾所認可。雙輪自平衡車以其眾多優(yōu)勢在面向以觀光 游覽、休閑散心、購物代步為主的短途旅行成為得力的代步工具。 1.1.2 兩輪自平衡車的研究意義 同時，兩輪自平衡小車作為一種新式交通工具，以電力為能源，清潔無污染，便攜 體積小，對于減小工作日里的交通擁堵，減輕環(huán)境污染程度，起到建設(shè)性的作用。 ①兩輪自平衡車作為代步工具，以其便攜、靈活的特點為優(yōu)勢，可以在人員流動量 大且占地面積大的公園、商場、展覽館、游樂場等作為首選代步工具。因此具有廣闊的 市場前景及應(yīng)用價值。 ②兩輪自平衡車以電池為能源清潔環(huán)保無污染，推廣兩輪自平衡小車的使用，尤其 在是以中短途為主的旅行中的使用，能有效的減輕環(huán)境污染問題。 ③兩輪自平衡車系統(tǒng)可以抽象化一級倒立擺，因此系統(tǒng)本身可以作為控制策略及算 法的實施驗證對象，有較高的研究價值與研究意義。 ④兩輪自平衡車的一級倒立擺系統(tǒng)與多旋翼飛行器的運動學(xué)系統(tǒng)相同，因此兩輪自 平衡車的研究可以為日后研究多旋翼飛行器打下基礎(chǔ)。同時兩輪自平衡車系統(tǒng)用到的 IMU（慣性測量單元）可以在多種實用、娛樂器件上應(yīng)用，因此研究兩輪自平衡車系統(tǒng)具 有重要實際價值。 1.2 兩輪自平衡車的研究現(xiàn)狀 1.2.1 兩輪自平衡車的國外研究現(xiàn)狀 兩輪自平衡車的研究始于 1987 年的日本東京電信大學(xué)的山藤雄一教授 [1]。他提出了 類似兩輪自平衡系統(tǒng)的設(shè)計思想。 1995 年美國人 Dean Kamen 與他的研發(fā)公司（DEKA Reasearch and Development Corp）發(fā)明了最早的雙輪平衡機器人，取名 Segway。 7 年后，Segway 正式投入市場。 該產(chǎn)品以動態(tài)穩(wěn)定理論為基礎(chǔ)，通過內(nèi)置慣性測量單元，測量出駕駛者的身體重心及車 體姿態(tài)，由中央微處理器發(fā)出指令驅(qū)動電機調(diào)整車體以達到動態(tài)的實現(xiàn)自平衡的效果。 3 圖 1-1 Segway 自平衡車 同年，瑞士聯(lián)邦工業(yè)大學(xué)研制出了可以對其遙控兩輪自平衡機器人 JOE,其最大運動 速度達到 1.5m/s。 圖 1-2 JOE 2005 年日本的村田制作所在自平衡車系統(tǒng)的研究取得突破。此后，兩輪自平衡車在 控制系統(tǒng)與機械結(jié)構(gòu)等方面都取得了長足的進步。 1.2.2 兩輪自平衡車的國內(nèi)研究現(xiàn)狀 國內(nèi)對自平衡機器人的研究起步較晚，但至今也取得了很大的進步。 2005 年，哈爾濱工業(yè)大學(xué)研制出了采用 DSP 作為控制核心的兩輪自平衡機器人。 圖 1-3 哈工大兩輪自平衡車樣機 中國科學(xué)技術(shù)大學(xué)也研發(fā)制作了自己的兩輪自平衡車，取名 Free Mover。Free Mover 的最高時速能夠達到 10km/h。 由于從出現(xiàn)至今不過短短十年時間，兩輪自平衡車現(xiàn)今仍不能作為一種新型交通方 式。現(xiàn)在市面上只有 Segway 公司能夠生產(chǎn)滿足大眾使用需求的產(chǎn)品。值得指出的是 2008 年的北京奧運會上 Segway 公司的產(chǎn)品得到了露臉的機會——在奧運會上執(zhí)行安保任務(wù)的 4 特警使用的就是 Segway 公司的產(chǎn)品。 圖 1-4 奧運會安保特警使用 Segway 自平衡車 1.3 本課題的研究內(nèi)容及目標 鑒于兩輪自平衡小車設(shè)計制作的綜合性，比較適合本科階段的畢業(yè)設(shè)計論文。與此 同時，市面上的兩輪自平衡車價格高昂，因此設(shè)計一個性能良好的小型樣機是非常有必 要的。 本文提出的兩輪自平衡車的亮點在于電源模塊、控制器模塊、通信模塊、電機驅(qū)動 模塊多采用貼片封裝，能夠最大限度的降低雜生電容和雜生電感對慣性測量單元的干擾。 ①本設(shè)計采用 Cortex M3 內(nèi)核的 STM32F103 系列嵌入型芯片作為主控器，性能穩(wěn)定。 ②慣性測量單元采用 MPU6050 芯片，包含三軸加速度計和三軸陀螺儀，其中加速度 計用以測量車體的角度，陀螺儀用以測量車體的角速度。 ③對信號采集后的濾波和姿態(tài)解算采用了兩種濾波算法作為比較，分別是四元數(shù)法 和卡爾曼濾波算法。 ④電源部分采用開關(guān)型芯片 LM2596 提供直流電機驅(qū)動電源及 AMS1117 提供芯片電 源，采用光耦開關(guān) TLP521 與電機驅(qū)動芯片 L298P 結(jié)合，通過 PID 控制算法控制電機， 最終使兩輪自平衡小車這個動態(tài)不穩(wěn)定系統(tǒng)實現(xiàn)最終的“自平衡” 。 2 自平衡系統(tǒng)的分析 2.1 控制目標 由對自平衡小車的客觀需要可知，兩輪自平衡小車最終要實現(xiàn)的功能有三種，分別 是保持直立、速度變化、和方向控制。其中保持直立即平衡控制是另外兩個功能實現(xiàn)的 5 基礎(chǔ)。我們可以把小車車體（姿態(tài)）作為控制對象，把兩個電機作為執(zhí)行機構(gòu)，把兩個 輪子各自的轉(zhuǎn)速作為控制系統(tǒng)的輸入量。則控制系統(tǒng)又可進一步分為三個子系統(tǒng)： ①平衡控制系統(tǒng)：把小車的傾角作為輸入量，通過控制兩路電機的正反轉(zhuǎn)來實現(xiàn)保 持平衡。 ②速度控制系統(tǒng)：在平衡控制的基礎(chǔ)上，通過調(diào)節(jié)車體的傾角實現(xiàn)對小車速度的控 制，其實質(zhì)仍是通過控制電機轉(zhuǎn)速實現(xiàn)車輪速度的控制。 ③方向控制系統(tǒng)：控制兩個電機的轉(zhuǎn)速使其不同以實現(xiàn)車體的轉(zhuǎn)向。 自平衡小車的平衡控制和方向控制都是通過直接控制小車的兩個電機實現(xiàn)的。在實 際的控制中，平衡控制與方向控制的信號疊加在一起加載到電機上，因此只要電機處于 正常工作的線性狀態(tài)就可以同時完成這兩個功能。而自平衡小車的速度是通過調(diào)節(jié)車體 的傾角實現(xiàn)的，車體的傾角不同則速度不同即小車會有加減速現(xiàn)象，從而實現(xiàn)小車的速 度控制。 由控制理論可知三個分解后的子系統(tǒng)的控制對象相同，因此他們之間存在系統(tǒng)耦合。 為便于分析，在分析一個子系統(tǒng)時，假設(shè)另外兩個控制對象已經(jīng)達到穩(wěn)定狀態(tài)。譬如， 在平衡控制時，假設(shè)速度和方向控制實現(xiàn)平穩(wěn)；同樣，在速度控制時，假設(shè)已經(jīng)可以保 持平衡狀態(tài)。 在三個子系統(tǒng)中以平衡控制系統(tǒng)最為關(guān)鍵。但從車體平衡控制的角度來看，另外兩 個控制成為它的干擾。因而自平衡小車的方向和速度的控制應(yīng)盡可能的保持平滑，以減 少對平衡控制系統(tǒng)的干擾。以小車的速度控制為例，需要通過改變車體傾角的設(shè)定值來 改變車體的實際傾角，達到速度控制的要求。同時，為了不對車體的平衡控制造成干擾， 車體傾角的變化應(yīng)該非常緩慢的進行。三者之間的配合如圖 2-1 所示。 圖 2-1 三層控制之間的配合 2.2 自平衡系統(tǒng)的控制學(xué)原理分析 2.2.1 自平衡小車平衡原理解析 為了對自平衡小車的平衡控制有一個直觀的感覺，首先來回顧我們在日常生活中的 經(jīng)驗。 6 圖 2-2 保持木棒直立的反饋控制 為使木棒在手指保持直立而不倒需要兩個器官——可以靈活移動的手指和觀察傾斜 的眼睛。自平衡小車的控制也需要負反饋機制。 前進 后仰 （a） 后退 糾正后仰 （b） 后退 前傾 （c） 前進 糾正前傾 （d） 圖 2-3 車體姿態(tài)調(diào)整分析圖 為了實現(xiàn)上圖中所示的小車的位姿調(diào)整，需要慣性測量單元對車體傾斜角度進行實 7 時檢測，通過控制車輪的轉(zhuǎn)動抵消傾斜力矩以保持車體平衡 [2]。 2.2.2 自平衡小車的運動學(xué)模型分析 從上一小節(jié)的分析我們可以引出這樣一個問題：車輪如何運行，才能保持車體平衡 穩(wěn)定？為了回答這個問題，可以通過建立車體的運動學(xué)和動力學(xué)數(shù)學(xué)模型，設(shè)計反饋控 制來保證車體的平衡。首先我們通過與單擺模型的對比說明自平衡小車平衡的控制規(guī)律。 理想化的單擺模型如下圖： l θ 圖 2-4 單擺模型 對單擺模型進行受力分析如下圖所示： θ ??sin?? l ??=???? ?????sin?? 圖 2-5 單擺受力分析 由受力分析可知，當物體離開垂直的平衡之后，將會受到重力與懸線的作用合力， 驅(qū)動物體回復(fù)到平衡位置。我們不妨稱這個力為回復(fù)力 。 [3]??= ?????sin?? 在 值很小的情況下，回復(fù)力與偏移角之間可以看作正比關(guān)系，但方向相反。即 ?? 。受到這個回復(fù)力的作用，單擺會作周期性運動。又由于受到空氣的阻尼力，??≈??????? 單擺最終還是會停止在平衡位置。由單擺運動產(chǎn)生的空氣阻尼力與單擺的運動速度成正 比，但方向相反。阻尼系數(shù)越大，單擺會越快停止擺動回復(fù)平衡。 總結(jié)后發(fā)現(xiàn)有兩個讓單擺能夠穩(wěn)定在垂直位置的條件： 1 受到回復(fù)力的作用且回復(fù)力方向與便宜方向相反； 8 2 受到阻尼力的作用且阻尼力方向與運動速度方向相反。 阻尼力在此起到很重要的作用?？梢月?lián)想控制系統(tǒng)中的系統(tǒng)阻尼。同樣，在實際的 單擺系統(tǒng)中，如果沒有空氣的阻尼力，則單擺會在平衡位置左右晃動而無法停止；如果 空氣的阻尼力過?。惐惹纷枘嵯到y(tǒng)） ，則單擺會在平衡位置震蕩；如果空氣的阻尼力過 大（類比過阻尼系統(tǒng)） ，則單擺回復(fù)平衡位置的時間會加長。因此存在一個合適的阻尼系 數(shù)，使單擺能夠回復(fù)至平衡位置且所需時間最短，稱這個阻尼系數(shù)為臨界阻尼系數(shù)。 對單擺模型分析完畢后接下來進一步對一級倒立擺模型進行建模及分析。為了便于 分析，在建模過程中不考慮車輪與地面的滾動摩擦力。 ????sin?? 圖 2-6 一級倒立擺模型受力分析 由對靜止一級倒立擺模型受力分析可知，其回復(fù)力為 [3]：??=????sin?? 可知，倒立擺之所以不能向單擺一樣可以穩(wěn)定在垂直位置，就是因為在其偏移平衡 位置時受到的回復(fù)力與其偏移方向相同而不是相反，因此不滿足穩(wěn)定在垂直位置的條件， 反而會加速偏離垂直位置直至傾倒。 因此，為了滿足回復(fù)力方向與運動方向相反的要求，實際可行的一種方法是驅(qū)動倒 立擺底部的車輪做加速運動，假設(shè)車輪加速度為 a,則由牛頓第一定律及第二定律可知，倒 立擺所受慣性力與重力分量的合力即為其回復(fù)力： ??=????sin???????cos??≈???????????1 ?? 式中，由于 很小，可以進行線性化。且假設(shè)車輪的加速度 a 與偏移角 成正比，?? ?? 。則當 時即可滿足回復(fù)力方向與運動方向相反的要求。??=??1?? ??1>?? 與此同時，對比上述單擺模型可知，為使一級倒立擺模型盡可能塊的回復(fù)平衡位置 還應(yīng)該給系統(tǒng)增加阻尼力。雖然車體與地面及空氣存在摩擦力等阻尼力但是相對較小， 因此需要額外增添加阻尼力。阻尼力與運動速度即角速度成正比，方向相反。因此整理 后，一級倒立擺模型的回復(fù)力大小為： 。??=???????????1???????2?? ' 綜上，車輪所需提供的加速度為： ??=??1??+??2??' 式中， 為車體傾角， 為傾角速度， 、 為比例系數(shù)。只有當 時，倒立擺?? ??‘ ??1 ??2 ??1>?? 才能滿足回復(fù)至平衡位置的條件。 為阻尼系數(shù)，合適的阻尼系數(shù)可以加快倒立擺回復(fù)穩(wěn)??2 定。 9 ?????cos?? ????sin?? 圖 2-7 在車輪上的參照系中車模受力分析 2.2.3 自平衡小車的數(shù)學(xué)建模 在以上的分析中，通過類比倒立擺得到了車體平衡控制的方案。接下來對車體進行 簡單的數(shù)學(xué)建模，通過基本控制理論討論通過閉環(huán)控制實現(xiàn)車體保持平衡的方案。 將車體模型簡化成高度為 L，質(zhì)量為 m 的簡單倒立擺，且置于可以左右移動的車輪 上。假設(shè)外力干擾使車體產(chǎn)生的角加速度為 x(t)，對車體進行受力分析可得自平衡車的運 動方程。 l ?? a(t) 圖 2-8 外力干擾條件下的車體受力 車體運動方程 [3]： (2-1) ????2??(??)????2 =??sin[??(??)]???(??)cos[??(??)]+????(??) 在角度 很小時，運動方程簡化為：?? (2-2) ????2??(??)????2 =????(??)???(??)+????(??) 車體靜止時： (2-3) ????2??(??)????2 =????(??)+????(??) 補充：圓周運動的切向加速度 ????= lim???→0??????? =??????????=???? 式中 為圓周運動的角加速度。?? 10 21Lsg?+ 將式 2-3 經(jīng)拉式變換轉(zhuǎn)化到復(fù)平面，可以得到車體靜止時的系統(tǒng)方程為： (2-4)??2??Θ(??)=??Θ(??)+????(??) 對應(yīng)車體靜止時，系統(tǒng)輸入輸出的傳遞函數(shù)為： (2-5) ??(??)= Θ(??)??(??)= ????2?????= 1??2??? ?? 可知，系統(tǒng)有兩個極點 .，且有一個極點位于右半平面。由奈奎斯特穩(wěn)定 ????= ± ???? 判據(jù)可知，當車體靜止時系統(tǒng)不穩(wěn)定。 由上一小節(jié)對小車的運動學(xué)分析可知，車體能夠回復(fù)至平衡位置的額外條件是車輪 提供加速度 ，即相當于在系統(tǒng)中引入比例和微分負反饋。車體引入比例微??=??1??+??2?? ' 分反饋后的系統(tǒng)為下圖所示： ??Xs ??s? 圖 2-9 加入比例微分反饋后的系統(tǒng)結(jié)構(gòu)圖 [2] 此時系統(tǒng)的傳遞函數(shù)為： ，系統(tǒng)兩個極點位于 ??(??)= Θ(??)??(??)= 1??2+??2 ????+??1????? ，由奈氏判據(jù)可知，若使系統(tǒng)保持穩(wěn)定，需要滿足 ????=???2± ??22?4??(??1???)2?? 。因此可以得出結(jié)論，當 時兩輪小車可以穩(wěn)定。??1>??,??2>0 ??1>??,??2>0 在反饋環(huán)節(jié)中，由 對比 ，可知 是 的微分，因而稱與??(??)= ??1+??2?? ??=??1??+??2?? ' ??‘ ?? 角速度成比例的控制量為微分控制，稱 為微分微分控制參數(shù)；稱與角度成比例的控制量??2 為比例控制，稱 為比例控制參數(shù)。又由對小車的運動學(xué)分析可知，微分控制參數(shù)相當于??1 系統(tǒng)的阻尼力，能起到抑制小車系統(tǒng)震蕩的作用。 在實際編寫程序時將用到式 2-2 來改變驅(qū)動直流電機的 PWM 占空比。 PWM = ??????= ????×??????????+ ????×??????????_?????? L 12ks? 11 式中 PWM 為驅(qū)動電機的 PID 輸出量， 為比例系數(shù)， 為微分系數(shù)。 為車???? ???? ?????????? 體傾角值， 為車體角速度值 [2]。??????????_?????? 本章小結(jié)：從運動學(xué)及控制學(xué)兩方面分析了兩輪小車保持自平衡所學(xué)要的條件，并 且進行了論證。找到了實現(xiàn)車體保持平衡的方案。 3 系統(tǒng)硬件組成 3.1 慣性測量單元 3.1.1 MPU6050 芯片介紹 MPU6050 是由 InevenSense 公司生產(chǎn)的全球首例整合性 6 軸運動處理組件，相較于 多組件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了大量的包裝空間。 MPU6050 內(nèi)部嵌入三軸加速度計、三軸陀螺儀和一個數(shù)字式運動處理硬件加速引擎能為 第三方數(shù)字傳感器提供 I2C 總線接口。MPU6050 輸出 16 位的加速度計數(shù)據(jù)和 16 位的陀 螺儀數(shù)據(jù)。 [6] 圖 3-1 MPU6050 芯片引腳圖 MPU6050 的內(nèi)置數(shù)字式運動處理引擎提供三維運動處理和姿態(tài)檢測算法。直接輸出 四元數(shù)。MPU6050 能在用戶定義的數(shù)據(jù)采集頻率下收集加速度計和陀螺儀的數(shù)據(jù)。 3.1.2 MPU6050 數(shù)據(jù)處理 在處理加速度計和陀螺儀用到的方法都是比較簡單的，這里的簡單并不是不需要任 何基礎(chǔ)知識，只是這些基本知識都是最基本的，比如簡單的三角函數(shù)，數(shù)學(xué)計算，物理 知識。 12 1. 加速度和陀螺儀原理 當然，在開始之前需要知道什么是加速度計，什么是陀螺儀。簡單的說，加速度計 主要是測量物體運動的加速度，陀螺儀主要測量物體轉(zhuǎn)動的角速度。 2. 加速度測量 我們都知道加速度具有合成定理。簡單來說就是重力加速度可以理解成是由 x,y,z 三 個方向的加速度共同作用的結(jié)果。反過來說就是重力加速度可以分解成 x,y,z 三個方向的 加速度。 加速度計可以測量某一時刻 x,y,z 三個方向的加速度值。而兩輪自平衡小車利用加速 度計測出重力加速度在 x,y,z 軸的分量，然后利用各個方向的分量與重力加速度的比值來 計算出小車大致的傾角。其實在自平衡小車上非靜止的時候，加速度計測出的結(jié)果并不 是非常精確。根據(jù)高中物理學(xué)過的知識，物體時刻都會受到地球的萬有引力作用產(chǎn)生一 個向下的重力加速度。而小車在動態(tài)時，受電機的作用一定會有一個前進或者后退方向 的作用力，而加速度計測出的結(jié)果是重力加速度與小車運動加速度合成得到一個總的加 速度在三個方向上的分量。 不過為便于分析，暫時不考慮電機作用產(chǎn)生的運動加速度對測量結(jié)果的影響。下邊 開始分析從加速度得到角度的方法。如下圖，把加速度計平放，分別畫出 x、y、z 軸的方 向。這三個軸就是我們接下來分析所要用到的坐標系。 圖 3-2 空間直角坐標系 把 MPU6050 安裝在自平衡車上時也是這樣的水平安裝在小車底盤上的，假設(shè)兩個車 輪安裝時車軸和 y 軸在一條直線上。那么小車擺動時，參考水平面就是桌面，并且車軸 （y 軸）與桌面始終是平行的，小車擺動和移動過程中 y 軸與桌面的夾角是不會發(fā)生變化 的，一直是 0 度。發(fā)生變化的是 x 軸與桌面的夾角以及 z 軸與桌面的夾角，而且桌面與 x 軸 z 軸夾角變化度數(shù)是一樣的。所以我們只需要計算出 x 軸和 z 軸中任意一個軸的夾角就 可以反映出小車的傾斜的情況了。 13 圖 3-3 以平衡車底盤平面為參考水平面 為了方便分析，由于 y 軸與桌面夾角始終不變，我們從 y 軸的方向俯看下去，那么 這個問題就會簡化成只有 x 軸和 z 軸的二維關(guān)系。假設(shè)某一時刻小車上加速度計 （MPU6050）處于如下狀態(tài)，下圖是我們看到簡化后的模型。 圖 3-4 車體運動傾斜 在這個圖中，y 軸已經(jīng)簡化和坐標系的原點 o 重合在了一起。我們來看看如何計算出 小車的傾斜角，也就是與桌面的夾角 a。上圖 g 是重力加速度， 、 分別是 在 x 軸和???? ???? ?? z 軸的分量。 由于重力加速度是垂直于水平面的，得到： 角 a+角 b=90 度 x 軸與 y 軸是垂直關(guān)系，得到： 角 c+角 b=90 度 于是輕松的就可以得出： 角 a=角 c 根據(jù)力的分解， 、 、 三者構(gòu)成一個長方形，根據(jù)平行四邊形的原理可以得出：?? ???? ???? 角 c=角 d 所以計算出角度 d 就等效于計算出了 x 軸與桌面的夾角 a。前邊已經(jīng)說過 是 在 x???? ?? 軸的分量，那么根據(jù)正弦定理就可以得出： sin??= ?????? 得到這個公式可是還是得不到想要的角度，因為需要計算反正弦，而反正弦在單片 機里不是很好計算。 14 角度的正弦與角度對應(yīng)的弧度成線性關(guān)系。如下圖： 圖 3-5 正弦與弧度的對應(yīng)關(guān)系 這個圖 x 軸是角度，取值范圍是 0~90 度，有三