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

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