兩輪自平衡小車(chē)論文.

1、2013年全國(guó)大學(xué)生電子設(shè)計(jì)競(jìng)賽兩輪自平衡小車(chē)設(shè)計(jì)作者：楊魏，黃敏杰，夏俊逸2015.7.17ii本文采用自制的兩輪簡(jiǎn)易小車(chē)作為試驗(yàn)平臺(tái)，以 MEM傳感MPU605（為傳感器的姿態(tài) 感知系統(tǒng)，通過(guò)離散卡爾曼濾波器對(duì)兩種傳感器的數(shù)據(jù)進(jìn)行濾波融合，選用32位單片機(jī)STM32F103R為控制核心處理器，完成對(duì)數(shù)據(jù)的采集處理和車(chē)身控制，采用 PID控制算法 實(shí)現(xiàn)小車(chē)兩輪自平衡。用藍(lán)牙控制前后運(yùn)動(dòng)。實(shí)驗(yàn)結(jié)果驗(yàn)證了該系統(tǒng)的性能滿(mǎn)足設(shè)計(jì)要求。關(guān)鍵詞：兩輪自平衡；姿態(tài)感知；STM32F103RB卡爾曼濾波；PID控制41系統(tǒng)方案11.1 姿態(tài)檢測(cè)模塊的論證與選擇 11.2 電機(jī)驅(qū)動(dòng)模塊的論證與選擇 12系統(tǒng)硬

2、件設(shè)計(jì)12.1 STM32F103RB 單片機(jī)系統(tǒng)22.1.1 STM32F103RB 單片機(jī)介紹22.1.2單片機(jī)最小系統(tǒng)設(shè)計(jì) 32.1.3電源管理模塊設(shè)計(jì) 42.2姿態(tài)檢測(cè)模塊 MPU-605052.2.1 MPU-6050 簡(jiǎn)介52.2.3數(shù)字運(yùn)動(dòng)處理器（DMP 62.3速度檢測(cè)模塊設(shè)計(jì) 72.3.1編碼器介紹72.3.2 編碼器電路設(shè)計(jì)82.4 電機(jī)驅(qū)動(dòng)模塊82.4.1 L298N 簡(jiǎn)介82.4.2 L298N 特點(diǎn)93理論分析與計(jì)算93.1兩輪平衡小車(chē)數(shù)學(xué)模型 93.2 PID 控制器設(shè)計(jì) 103.2.1 PID 控制器原理103.2.2 PID 控制器設(shè)計(jì) 113.2.3 PID

3、程序123.3基于卡爾曼濾波的數(shù)據(jù)融合 134系統(tǒng)軟件設(shè)計(jì) 154.1系統(tǒng)軟件設(shè)計(jì)框架 154.2 資源模塊初始化 154.3兩輪小車(chē)姿態(tài)信息檢測(cè) 165測(cè)試方案與測(cè)試結(jié)果 165.1測(cè)試方案165.1.1硬件連接檢測(cè) 165.1.2小車(chē)功能檢測(cè)165.2 測(cè)試分析與結(jié)論 161系統(tǒng)方案本系統(tǒng)主要由姿態(tài)檢測(cè)模塊、電機(jī)驅(qū)動(dòng)模塊、藍(lán)牙模塊、紅外對(duì)管模塊、電源模塊組成, 下面分別論證這幾個(gè)模塊的選擇。1.1姿態(tài)檢測(cè)模塊的論證與選擇方案一：ENC-03 種普通陀螺儀。方案二：MPU-6050 MPU-60X0是全球首例9軸運(yùn)動(dòng)處理傳感器。它集成了 3軸MEM陀螺 儀，3軸MEMS卩速度計(jì)，以及一個(gè)可擴(kuò)

4、展的數(shù)字運(yùn)動(dòng)處理器DMP（ Digital MotionProcessor），可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過(guò)其I2C或SPI接口輸出一個(gè)9軸的信號(hào)（SPI接口僅在MPU-600C可用）。MPU-60X0 也可以通過(guò)其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。綜合以上兩種種方案，選擇方案二。1.2電機(jī)驅(qū)動(dòng)模塊的論證與選擇方案一：TB6612FNG體積小，但容易壞。方案二：L298N具有信號(hào)指示，轉(zhuǎn)數(shù)可調(diào)，抗干擾能力強(qiáng)，具有過(guò)電壓和過(guò)電流保護(hù)，可單獨(dú)控制兩臺(tái)直流電機(jī)，可單獨(dú)控制一臺(tái)步進(jìn)電機(jī)。綜合以上三種方案，選擇方案二。2系統(tǒng)硬件設(shè)計(jì)本文設(shè)計(jì)的兩輪

5、自平衡小車(chē)采用姿態(tài)傳感器（MPU605）監(jiān)測(cè)車(chē)身所處的俯仰狀態(tài)和狀態(tài)變化率，通過(guò)高速微控制器STM32F103R完成數(shù)據(jù)融合處理，得到平滑而穩(wěn)定車(chē)體姿 態(tài)信息，然后驅(qū)動(dòng)電動(dòng)機(jī)產(chǎn)生前進(jìn)或后退的加速度來(lái)控制車(chē)體保持平衡，紅外線(xiàn)檢測(cè)障礙 物，藍(lán)牙控制前進(jìn)后退。結(jié)構(gòu)框圖如圖。20主控制器STM32F103RBL29SX電機(jī)顋動(dòng)藍(lán)牙紅外線(xiàn)圖1系統(tǒng)設(shè)計(jì)總體結(jié)構(gòu)框圖系統(tǒng)硬件主要由STM32F103R單片機(jī)系統(tǒng)、電源模塊、姿態(tài)檢測(cè)模塊、串口通訊模塊、 電機(jī)驅(qū)動(dòng)模塊等組成。2.1 STM32F103RB單片機(jī)系統(tǒng)本設(shè)計(jì)中控制平臺(tái)采用 ST意法半導(dǎo)體的STM32F103R微控制器，STM32系列基于專(zhuān)為要 求高性

6、能、低成本、低功耗的嵌入式應(yīng)用專(zhuān)門(mén)設(shè)計(jì)的ARM CORTEX-M內(nèi)核，其最高主頻72MHz在滿(mǎn)足平衡控制的調(diào)節(jié)頻率的同時(shí)有更多的CPU資源，工作電壓為5 V。其豐富的內(nèi)置模塊資源可以為姿態(tài)檢測(cè)系統(tǒng)提供 12位精度的A/D轉(zhuǎn)換，同時(shí)使用定時(shí)器可以產(chǎn) 生PWM&出控制電機(jī)轉(zhuǎn)速和轉(zhuǎn)向。2.1.1 STM32F103RB 單片機(jī)介紹STM32F103xB增強(qiáng)型系列使用高性能的 ARM? Cortex?-M3 32位的RISC內(nèi)核，工作頻 率為72MHz內(nèi)置高速存儲(chǔ)器（高達(dá)128K字節(jié)的閃存和20K字節(jié)的SRAM）豐富的增強(qiáng)I/O 端口和聯(lián)接到兩條APB總線(xiàn)的外設(shè)。所有型號(hào)的器件都包含2個(gè)12位的ADC

7、 3個(gè)通用16 位定時(shí)器和1個(gè)PWME時(shí)器，還包含標(biāo)準(zhǔn)和先進(jìn)的通信接口 ：多達(dá) 2個(gè)I2C接口和SPI接 口、3 個(gè) USART接 口、一個(gè) USB接 口和一個(gè) CANS 口。STM32F103xx中等容量增強(qiáng)型系列產(chǎn)品供電電壓為 2.0V至3.6V，包含-40 C至+85 C溫度范圍和-40 C至+105 C的擴(kuò)展溫度范圍。一系列的省電模式保證低功耗應(yīng)用的要 求oSTM32F103x中等容量增強(qiáng)型系列產(chǎn)品提供包括從 36腳至100腳的6種不同封裝形式； 根據(jù)不同的封裝形式，器件中的外設(shè)配置不盡相同。下面給出了該系列產(chǎn)品中所有外設(shè)的 基本介紹。這些豐富的外設(shè)配置，使得 STM32F103X）產(chǎn)

8、品容量增強(qiáng)型系列微控制器適合于 多種應(yīng)用場(chǎng)合：電機(jī)驅(qū)動(dòng)和應(yīng)用控制醫(yī)療和手持設(shè)備 PC游戲外設(shè)和GPS平臺(tái)工業(yè)應(yīng)用：可編程控制器（PLC）、變頻器、打印機(jī)和掃描儀警報(bào)系統(tǒng)、視頻對(duì)講、和暖氣通風(fēng)空調(diào)系統(tǒng)等2.1.2單片機(jī)最小系統(tǒng)設(shè)計(jì)本設(shè)計(jì)采用STM32F103R單片機(jī)為控制器，最小系統(tǒng)原理圖如圖 3-3所示，主要包括 單片機(jī)供電、復(fù)位電路、時(shí)鐘電路以及 BDM接 口電路。系統(tǒng)時(shí)鐘的選擇是在啟動(dòng)時(shí)進(jìn)行，復(fù)位時(shí)內(nèi)部 8MHZ的RC振蕩器被選為默認(rèn)的CPU時(shí) 鐘，隨后可以選擇外部的、具失效監(jiān)控的 416MHZ寸鐘；當(dāng)檢測(cè)到外部時(shí)鐘失效時(shí)，它將 被隔離，系統(tǒng)將自動(dòng)地切換到內(nèi)部的 RC振蕩器，如果使能了中斷

9、，軟件可以接收到相應(yīng) 的中斷。同樣，在需要時(shí)可以采取對(duì) PLL時(shí)鐘完全的中斷管理(如當(dāng)一個(gè)間接使用的外部 振蕩器失效時(shí))。多個(gè)預(yù)分頻器用于配置 AHB的頻率、高速APB(APB2和低速APB(APB1區(qū) 域。AHB和高速APB的最高頻率是72MHz低速APB的最高頻率為36MHzSBfJJM PA；C4 匹pay1亡2丁M1PA42CarPACPAtPA2PA3PA4 PA! PASPATM PAS 41初札2 貯M PA1CI C：jT3 yiz :J3 30044 jfPCUSTM3：FISJMT6圖3-3單片機(jī)最小系統(tǒng)及時(shí)鐘電路原理圖單片機(jī)的外部復(fù)位電路可以使用按鈕和電容構(gòu)成，也可以使用

10、專(zhuān)門(mén)的復(fù)位芯片。考慮到產(chǎn)品經(jīng)濟(jì)型，本設(shè)計(jì)中采用了由電阻電容構(gòu)成的簡(jiǎn)易復(fù)位電路，如圖3-5所示。加電后，由于電容的充電時(shí)間，RESET保持低電平，單片機(jī)復(fù)位；一段時(shí)間后，電容電量充滿(mǎn)，RESET端輸出高電平，此時(shí)單片機(jī)運(yùn)行。手動(dòng)復(fù)位時(shí)，按下手動(dòng)復(fù)位按鈕，RESETS保持低電平,單片機(jī)復(fù)位；釋放手動(dòng)復(fù)位按鈕后，RESETS輸出高電平，單片機(jī)工作。圖3-5復(fù)位及按鍵接口電路2.1.3電源管理模塊設(shè)計(jì)可靠的電源方案是整個(gè)硬件電路穩(wěn)定可靠運(yùn)行的基礎(chǔ)。電源模塊由若干相互獨(dú)立的穩(wěn)壓電路模塊組成。這樣做可以減少各模塊之間的相互干擾。整個(gè)系統(tǒng)需要3種電源： VDD = 2.03.6V: VDD引腳為I/O引腳和

11、內(nèi)部調(diào)壓器供電。 VSSA VDDA = 2.03.6V:為ADC復(fù)位模塊、RC振蕩器和PLL的模擬部分提供供電。 使用ADC時(shí)，VDDAF得小于2.4V。 VDDA和VSSA必須分別連接到 VDDffi VSS VBAT = 1.83.6V:當(dāng)關(guān)閉VDD寸，（通過(guò)內(nèi)部電源切換器）為RTC外部32kHz振蕩器 和后備寄存器供電。電源SL-5V1jJOCJtST SPST* Cl圖3-6電源模塊電路圖2.2姿態(tài)檢測(cè)模塊MPU-6050姿態(tài)檢測(cè)模塊能夠精確并穩(wěn)定地檢測(cè)出當(dāng)前的車(chē)身姿態(tài)，這是本設(shè)計(jì)兩輪自平衡小車(chē)實(shí)現(xiàn)有效控制的關(guān)鍵。本設(shè)計(jì)選用MEMS專(zhuān)感MPU6050作為姿態(tài)檢測(cè)傳感器MPU-605C

12、實(shí)物連接圖2.2.1 MPU-6050 簡(jiǎn)介MPU-60X0是全球首例9軸運(yùn)動(dòng)處理傳感器。它集成了 3軸MEM陀螺儀，3軸MEMS 加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP（ Digital Motion Processor ），可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過(guò)其I2C或SPI接口輸出一個(gè)9軸的信號(hào)（SPI接口僅在 MPU-600C可用）。MPU-60XC也可以通過(guò) 其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。MPU-60X0對(duì)陀螺儀和加速度計(jì)分別用了三個(gè) 16位的ADC將其測(cè)量的模擬量 轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，

13、傳感器的測(cè)量范圍都是用戶(hù) 可控的，陀螺儀可測(cè)范圍為土 250， 500， 1000， 2000 /秒（dps），加速度計(jì)可 測(cè)范圍為土 2， 4， 8， 16g。一個(gè)片上1024字節(jié)的FIFO,有助于降低系統(tǒng)功耗。和所有設(shè)備寄存器之間的通信 采用400kHz的I2C接口或1MHz的SPI接口（ SPI僅MPU-6000可用）。對(duì)于需要高 速傳輸?shù)膽?yīng)用，對(duì)寄存器的讀取和中斷可用 20MHz的SPI。另外，片上還內(nèi)嵌了一個(gè)溫度傳感器和在工作環(huán)境下僅有土 1%變動(dòng)的振蕩器。芯片尺寸 4X 4X 0.9mm采用QFN封裝（無(wú)引線(xiàn)方形封裝），可承受最大 10000g的沖擊，并有可 編程的低通濾波器。關(guān)于

14、電源，MPU-60X0可支持VDD范圍2.5V 5% 3.0V 5% 或3.3V 5% 另外 MPU-6050還有一個(gè)VLOGIC引腳，用來(lái)為I2C輸出提供邏輯電平。VLOGIC電壓可取 1.8 5%或者 VDDMPU6050電路圖2.2.3數(shù)字運(yùn)動(dòng)處理器（DMPDMP就是指MPU605C內(nèi)部集成的處理單元，可以直接運(yùn)算出四元數(shù)和姿態(tài)，而不再需要另 外進(jìn)行數(shù)學(xué)運(yùn)算。DM啲使用大大簡(jiǎn)化了四軸的代碼設(shè)計(jì)。DMP是數(shù)字運(yùn)動(dòng)處理器的縮寫(xiě)，顧名思義mpu6050并不單單是一款傳感器，其內(nèi)部還包含了可以獨(dú)立完成姿態(tài)解算算法的 處理單元。如在設(shè)計(jì)中使用 DMPin ve nse nse官方提供的姿態(tài)解算算法

15、應(yīng)該比像樓主這樣的小白要可靠的多。其次，由DMP實(shí)現(xiàn)姿態(tài)解算算法將單片機(jī)從算法處理的壓力中解放出來(lái)，單片機(jī)所要做的是等待DMP解算完成后產(chǎn)生的外部中斷，在外部中斷里去讀取姿態(tài)解算的結(jié)果。這樣單片機(jī)有大量的時(shí)間來(lái)處理諸 如電機(jī)調(diào)速等其他任務(wù)，提高了系統(tǒng)的實(shí)時(shí)性。DMP從陀螺儀、加速度計(jì)以及外接的傳感器接收并處理數(shù)據(jù)，處理結(jié)果可以從DMP寄存器讀出，或通過(guò)FIFO緩沖。DMPt權(quán)使用MPU勺一個(gè)外部引腳產(chǎn)生中斷。2.3速度檢測(cè)模塊設(shè)計(jì)兩輪自平衡小車(chē)的原理是利用地面對(duì)車(chē)輪的摩擦力抵消車(chē)受到的重力，在本系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。為實(shí)現(xiàn)速度的閉環(huán)控制，必須加入速度檢

16、測(cè)裝置實(shí)現(xiàn)速度閉環(huán)控制中的反饋環(huán)節(jié)。本系統(tǒng)測(cè)速模塊采用OMRO歐姆龍）公司500線(xiàn)增量式旋轉(zhuǎn)編碼器如圖3-112.3.1編碼器介紹編碼器內(nèi)部為一個(gè)中心有軸的光電碼盤(pán)，其上有環(huán)形通、暗的刻線(xiàn)，有光電發(fā)射和接 收器件讀取,獲得四組正弦波信號(hào)組合成 A B C D,每個(gè)正弦波相差90度相位差（相對(duì) 于一個(gè)周波為360度），將C、D信號(hào)反向，疊加在A、B兩相上，可增強(qiáng)穩(wěn)定信號(hào)；另每 轉(zhuǎn)輸出一個(gè)Z相脈沖以代表零位參考位。由于A B兩相相差90度，可通過(guò)比較A相在前還是B相在前，以判別編碼器的正轉(zhuǎn) 與反轉(zhuǎn)，通過(guò)零位脈沖，可獲得編碼器的零位參考位如圖3-12。旋轉(zhuǎn)方向：CCW （從軸側(cè)看：左旋轉(zhuǎn)）A柑日相O

17、N： H；*OFFfLJ ONlHJOFFCL)rrLjr-L一H-1 ,;4T1011945圖3-12編碼器A、B相輸出波形本系統(tǒng)用OMRC旋轉(zhuǎn)編碼器相關(guān)參數(shù)如表3-1所示：表3-1編碼器參數(shù)表項(xiàng)目參數(shù)電源電壓DC 5V12V紋波的峰峰值小于5%消耗電流30mA以 下分辨率500脈沖/轉(zhuǎn)輸出相A B、Z三相輸出狀態(tài)集電極開(kāi)路輸出最高輸出響應(yīng)42kHz輸出上升或下降時(shí)間1us以下起動(dòng)轉(zhuǎn)矩1mN- ml以下軸允許力徑向10N、軸向5N允許最咼轉(zhuǎn)速5000r/mi n質(zhì)量35g2.3.2編碼器電路設(shè)計(jì)由于編碼器采用集電極開(kāi)路輸出，輸出波形為矩形波，因此編碼器外圍電路較為簡(jiǎn)單。 需要在信號(hào)輸出端接入

18、一個(gè)上拉電阻，即可將信號(hào)提供給單片機(jī)采集數(shù)據(jù)。如圖3-13所示，PULSE引腳為編碼器A相，接單片機(jī)的脈沖計(jì)數(shù)口，通過(guò)單片機(jī)的PACN模塊對(duì)輸入脈沖進(jìn)行計(jì)數(shù)從而獲取電機(jī)轉(zhuǎn)速。DIR為編碼器B相輸出，接單片機(jī)I/O口，通過(guò)A、B相位差進(jìn)行軟件鑒相，從而判斷電機(jī)轉(zhuǎn)動(dòng)方向。圖3-13編碼器接口電路2.4電機(jī)驅(qū)動(dòng)模塊電機(jī)的驅(qū)動(dòng)芯片選用L298N作為驅(qū)動(dòng)芯片。2.4.1 L298N 簡(jiǎn)介L(zhǎng)298是SGS公司的產(chǎn)品，內(nèi)部包含4通道邏輯驅(qū)動(dòng)電路，是一種兩相和四相電機(jī)的專(zhuān)用驅(qū)動(dòng)器，即內(nèi)含兩個(gè)H橋的高電壓大電流雙全橋式驅(qū)動(dòng)器，接收標(biāo)準(zhǔn)的TTL邏輯電平信號(hào)，可驅(qū)動(dòng)46V、2A以下的電機(jī)。本設(shè)計(jì)中直流電機(jī)的驅(qū)動(dòng)信號(hào)

19、由STM32F103R提供，信圖6理論分析與計(jì)算2.4.2 L298N 特點(diǎn)基本特點(diǎn)如下：（1）具有信號(hào)指示。（2）轉(zhuǎn)數(shù)可調(diào)。（3）抗干擾能力強(qiáng)。（4）具有過(guò)電壓和過(guò)電流保護(hù)（5）可單獨(dú)控制兩臺(tái)直流電機(jī)（6）可單獨(dú)控制一臺(tái)步進(jìn)電機(jī)（7）PWM脈寬平滑調(diào)速。（8）可實(shí)現(xiàn)正反轉(zhuǎn)。（9）米用光電隔離。3理論分析與計(jì)算3.1兩輪平衡小車(chē)數(shù)學(xué)模型已知自平衡車(chē)高度為I，質(zhì)量為m,將其抽象為一級(jí)倒立擺，并將倒立擺至于可水平移動(dòng)的小車(chē)上。假設(shè)其受外力干擾引起的車(chē)體角加速度為x（t），沿垂直于車(chē)體方向進(jìn)行受力分析如圖2-7，可以得到自平衡車(chē)傾角與車(chē)輪移動(dòng)加速度為a（t）以及外力干擾帶來(lái)的加速度x(t)之間的運(yùn)動(dòng)

20、方程圖2-7外力干擾條件下的小車(chē)受力分析小車(chē)運(yùn)動(dòng)微分方程表達(dá)式如式2-8 :tdt2= gsinfita t cosFtlx t（式 2-8）當(dāng)傾角二很小的時(shí)候，可以進(jìn)行線(xiàn)性化處理：cos(、1、sin(R ：宀，運(yùn)動(dòng)微分方程可簡(jiǎn)化為:d勺tdt2=g t -at lx t（式 2-9）自平衡車(chē)靜止時(shí)a t =0,其運(yùn)動(dòng)微分方程為:d2, t dt2（式 2-10）3.2 PID控制器設(shè)計(jì)3.2.1 PID控制器原理當(dāng)今的自動(dòng)控制技術(shù)都是基于反饋的概念。反饋理論的要素包括三個(gè)部分：測(cè)量、比 較和執(zhí)行。測(cè)量系統(tǒng)需要控制的變量，與期望值相比較，用這個(gè)誤差糾正調(diào)節(jié)控制系統(tǒng)的 響應(yīng)。在工程實(shí)際中，應(yīng)用

21、最為廣泛的調(diào)節(jié)器控制規(guī)律為比例、積分、微分控制，簡(jiǎn)稱(chēng)PID控制，又稱(chēng)PID調(diào)節(jié)。PID控制器問(wèn)世至今已有近70年歷史，以其結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、 工作可靠、調(diào)整方便而成為工業(yè)控制的主要技術(shù)之一。PID控制器由比例單元（P）、積分單元（I ）和微分單元（D）組成。其輸入e （t） 與輸出u （t ）的關(guān)系為：一1 tde（t ）1u（t ）=Kp t-iQt）dtTD（式-Ti 0dt 一2-11）其中Kp為比例系數(shù)；Ti為積分時(shí)間常數(shù)；Td為微分時(shí)間常數(shù)。PID控制器具有原理簡(jiǎn)單、使用方便、適應(yīng)性強(qiáng)、魯棒性強(qiáng)、對(duì)模型依賴(lài)少等特點(diǎn)， 因此使用PID控制器實(shí)現(xiàn)兩輪自平衡車(chē)的控制是完全可行的。3.2.

22、2 PID 控制器設(shè)計(jì)由小車(chē)靜止時(shí)其運(yùn)動(dòng)方程可得到系統(tǒng)輸入輸出傳遞函數(shù):H（s）=妙=匕（式 212）X（s）二 g（式2-12）s _l此時(shí)系統(tǒng)具有兩個(gè)極點(diǎn)：Sp = , g。其中一個(gè)極點(diǎn)位于s平面的右半平面。根據(jù)奈 奎斯特穩(wěn)定判據(jù)可知系統(tǒng)不穩(wěn)定，因此小車(chē)在靜止?fàn)顟B(tài)不能保持平衡0由小車(chē)受力分析可知小車(chē)平衡的條件是提供額外的回復(fù)力及阻尼，其來(lái)源為車(chē)輪與地 面的摩擦力。由式2-7可知，車(chē)輪提供的加速度的大小是根據(jù)角度 二及角速度J的反饋得 出,因此需要在控制系統(tǒng)中引入角度 二及角速度丁構(gòu)成比例（P）微分（D）反饋環(huán)節(jié)，如圖2-8 所示。圖2-8加入比例微分環(huán)節(jié)后的控制系統(tǒng)結(jié)構(gòu)圖加入比例微分反饋后

23、的系統(tǒng)傳遞函數(shù)為：。（S）1H s（式X（s）孑丄匕才匕謳（式I I2-13）此時(shí)，系統(tǒng)的兩個(gè)極點(diǎn)為s = 一k2*Jk；4|（kig）。根據(jù)奈奎斯特穩(wěn)定判據(jù)可知，p2I系統(tǒng)穩(wěn)定需要兩個(gè)極點(diǎn)都位于 s平面的左半平面。要滿(mǎn)足這一點(diǎn)，需要 &g, k20。由此 可得出結(jié)論，但ki g, k20時(shí)，小車(chē)可以保持平衡，這也與上文中小車(chē)受力分析的結(jié)果相 符。在反饋環(huán)節(jié)中，與角度二成比例的控制量稱(chēng)為比例控制；與角速度成比例的控制量稱(chēng) 為微分控制（角速度是角度的微分）。因此上面系數(shù)ki，k2分別稱(chēng)為比例和微分控制參數(shù)。 其中微分參數(shù)相當(dāng)于阻尼力，可以有效抑制自平衡車(chē)振蕩?？刂葡到y(tǒng)的輸出量為電機(jī)控制量，因而小

24、車(chē)平衡控制的PID控制器的輸出方程可寫(xiě)為：OUT_Motor=Kp*Angle+Kd*Angle_dot（式 2-14）式2-14中，OUT_Motor為PID控制輸出量，Angle為反饋傾角值，Angle_dot為反饋 角速度值，Kp和Kd分別為比例系數(shù)及微分系數(shù)。3.2.3 PID 程序#in clude #in clude math.h#i nclude vpwm.hfloat PID_realize（）;void pwm（void）;exter n float Pitch;float SetSpeed=-1.04;定義目標(biāo)值float ActualSpeed=0.0;反饋值float

25、err=0.0;偏差值float err_last=0.0;上次偏差值float Kp=135;float Ki=2.7;float Kd=135;float output=0.0;電壓值控制執(zhí)行器的變量float in tegral=0.0; 積分量float PID_realize()ActualSpeed=Pitch;err=SetSpeed-ActualSpeed;in tegral+=err;output=Kp*err+Kd*(err-err_last)+Ki* in tegral; err_last=err;return output;3.3基于卡爾曼濾波的數(shù)據(jù)融合卡爾曼濾波器是一

26、種高效率的遞歸濾波器（自回歸濾波器），能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)?？柭鼮V波器不僅能估計(jì)信號(hào)的過(guò)去和當(dāng)前狀態(tài)，甚至能估計(jì)將來(lái)的狀態(tài)?？柭鼮V波器解決離散時(shí)間控制過(guò)程的一般方法，首先定義模型線(xiàn)性隨機(jī)微分方程。假設(shè)卡爾曼濾波模型k時(shí)刻真實(shí)狀態(tài)是從（k-1 ）時(shí)刻推算出來(lái)，如下式人二 AXk Buk Wk（式2-15）式2-15中，xk是k時(shí)刻狀態(tài)；A是k-1時(shí)刻狀態(tài)變換模型；B是作用在控制器向量 山上 的輸入控制模型；Wk是過(guò)程噪聲，假設(shè)其均值為零，協(xié)方差矩陣 Qk符合多元正態(tài)分布： Wk N（0,Qk）（式2-16）k時(shí)刻對(duì)應(yīng)真實(shí)狀態(tài)Xk的測(cè)量Zk滿(mǎn)足下式：Zk

27、= HkXk Vk（式2-17）式2-17中Hk是觀測(cè)模型，將真實(shí)控制映射為觀測(cè)空間；Vk為觀測(cè)噪聲，其均值為零,式 2-18)協(xié)方差矩陣Rk符合正態(tài)分布：VkN(O,R)初始狀態(tài)以及每一時(shí)刻的噪聲都認(rèn)為是互相獨(dú)立的卡爾曼濾波器的操作主要包括兩個(gè)階段：預(yù)估與更新。在預(yù)估階段，濾波器根據(jù)上 時(shí)刻狀態(tài)，估算出當(dāng)前時(shí)刻狀態(tài)；在更新階段，濾波器利用當(dāng)前時(shí)刻觀測(cè)值優(yōu)化在預(yù)估階段獲得的測(cè)量值，以獲得一個(gè)更準(zhǔn)確的新估計(jì)值冋1213 卡爾曼濾波器迭代過(guò)程如下：1. 先驗(yàn)狀態(tài)估計(jì)：?k|kAXk_i BUk_i2. 先驗(yàn)估計(jì)誤差協(xié)方差Pk |k_1 二 AkPk_lAQ3.卡爾曼增益KPkk-iHTHPk k_iHT R2-21)4.后驗(yàn)狀態(tài)估計(jì)?k吆|k心夂沖?十)(2-22)5.后驗(yàn)誤差協(xié)方差Pk=(1 - H)Pk k-1(2-23)在上面各式中：A:作用在上的n階矩陣;B:作用在控制向量 山4上的nx 1輸入控制矩陣；H: mx n觀測(cè)模型矩陣，將真實(shí)狀態(tài)空間映射為觀測(cè)空間;Pk|k4 : nx n先驗(yàn)估計(jì)誤差協(xié)方差矩陣；R : n x n后驗(yàn)估計(jì)誤差協(xié)方差矩陣;Q: nx n過(guò)程噪聲協(xié)方差矩陣；R: mx m過(guò)程噪聲協(xié)方差矩陣；I : n階單位矩陣；Kk : nxm矩陣，稱(chēng)之為卡爾曼增益4系統(tǒng)軟件設(shè)計(jì)4.1系統(tǒng)軟件設(shè)計(jì)框架系統(tǒng)軟件設(shè)