雙輪自平衡小車結(jié)題

1、雙輪自平衡小車科研訓(xùn)練總結(jié)報(bào)告項(xiàng)目名稱： 學(xué)生姓名： 學(xué) 院： 指導(dǎo)教師： *學(xué)自動(dòng)化學(xué)院*8系電子信息科學(xué)與技術(shù)專業(yè)科研訓(xùn)練論文2014年12月27日目錄一、研究的目的與意義21.1.項(xiàng)目研究目的與意義21.2.國(guó)內(nèi)外發(fā)展歷史及研究現(xiàn)狀3二、系統(tǒng)設(shè)計(jì)與要求42.1項(xiàng)目研究?jī)?nèi)容42.2設(shè)計(jì)要求42.3方案選取42.3.1主控板模塊42.3.2電機(jī)驅(qū)動(dòng)模塊42.3.3傳感器模塊52.3.4 卡爾曼算法的初值計(jì)算方法及實(shí)際應(yīng)用52.3.5 雙輪自平衡小車的PID控制5三、總體設(shè)計(jì)方案6四、系統(tǒng)硬件設(shè)計(jì)74.1 STC12C5A60S2簡(jiǎn)介74.2 MPU6050簡(jiǎn)介84.3 LM298簡(jiǎn)介104.

2、4硬件平臺(tái)的組合11五、系統(tǒng)軟件設(shè)計(jì)125.1軟件設(shè)計(jì)流程125.2軟件程序如下12六、實(shí)驗(yàn)結(jié)果與分析22七、參與人心得23八、參考文獻(xiàn)24一、 研究的目的與意義1.1.項(xiàng)目研究目的與意義 近年來，兩輪自平衡小車的研究開始在美國(guó)、日本、瑞士等國(guó)得到迅速的發(fā)展。建立了多個(gè)實(shí)驗(yàn)原機(jī)型，提出來眾多解決平衡控制的方案，并對(duì)原機(jī)型的自動(dòng)平衡性能與運(yùn)動(dòng)特性進(jìn)行了驗(yàn)證。通過對(duì)兩輪自平衡系統(tǒng)的改造，可快速方便的應(yīng)用到眾多環(huán)境中去，如承載、運(yùn)輸 ，代步等。這其中蘊(yùn)藏著巨大的商機(jī)，相應(yīng)有些國(guó)外公司現(xiàn)在已經(jīng)推出了商業(yè)化產(chǎn)品，并且已經(jīng)投放到了市場(chǎng)。兩輪自平衡小車兩輪共軸、獨(dú)立驅(qū)動(dòng)、車身重中心位于車輪軸上方，

3、通過運(yùn)動(dòng)保持平衡，可以直立運(yùn)動(dòng)，因?yàn)樘貏e的結(jié)構(gòu)，它對(duì)于地形的變化有很強(qiáng)的適應(yīng)能力，有著良好的運(yùn)動(dòng)性能，能夠在比較復(fù)雜的環(huán)境里面的工作，和傳統(tǒng)的輪式移動(dòng)機(jī)器人相比較，兩輪自平衡小車有著以下的幾個(gè)優(yōu)點(diǎn)：（1）能夠?qū)崿F(xiàn)在原地回轉(zhuǎn)和任意半徑的轉(zhuǎn)向，有更加靈活易變的移動(dòng)軌跡很好地彌補(bǔ)了傳統(tǒng)多輪布局的缺點(diǎn)；（2）具有占地面積小的優(yōu)點(diǎn)，能夠在場(chǎng)地面積很小或者要求靈活運(yùn)輸?shù)膱?chǎng)合上使用；（3）車的結(jié)構(gòu)上有很大的簡(jiǎn)化，可以把車做的更輕更小；（4）有著較小的驅(qū)動(dòng)功率，能夠讓電池長(zhǎng)時(shí)間供電，為環(huán)保型輕車提供了一種新的概念。（5)機(jī)器人的平衡是個(gè)動(dòng)態(tài)過程；機(jī)器人在平衡點(diǎn)附近不停地變化進(jìn)行調(diào)節(jié)以保持平衡；（6)重心的高度

4、對(duì)小車運(yùn)動(dòng)和硬件設(shè)計(jì)的限制小。多輪(三輪或以上)移動(dòng)小車雖然可以穩(wěn)定地平衡，可是重心過高則小車啟動(dòng)或急停時(shí)，有傾倒的危險(xiǎn)。因此重心必須要求很低，設(shè)計(jì)時(shí)總是拉大小車的水平截面積，降低高度。這樣會(huì)造成小車體積變大，質(zhì)量增加，某些功能會(huì)受到限制。兩輪自平衡移動(dòng)小車卻無這方面的約束，重心的高低引起的不平衡已經(jīng)通過動(dòng)態(tài)平衡原理解決。因此重心的高低無嚴(yán)格限制，節(jié)省占地面積，可用在場(chǎng)地面積較小或要求靈活運(yùn)輸?shù)膱?chǎng)合；（7)驅(qū)動(dòng)功率較小，為電池長(zhǎng)時(shí)間供電提供了可能，為環(huán)保輕型車提供了一種新的思路。鑒于這些特點(diǎn)，兩輪自平衡小車有著相當(dāng)廣泛的應(yīng)用前景。 兩輪自平衡小車是一個(gè)集動(dòng)態(tài)決策和規(guī)劃、環(huán)境感知、行為控制和執(zhí)行

5、等多種功能于一體的綜合復(fù)雜系統(tǒng)，其關(guān)鍵是在解決自平衡的同時(shí)，還能夠適應(yīng)在各種環(huán)境下的控制任務(wù)。通過運(yùn)用外速度傳感器、角速度傳感器等，可以實(shí)現(xiàn)小車的平衡自主前進(jìn)。1.2.國(guó)內(nèi)外發(fā)展歷史及研究現(xiàn)狀兩輪自平衡小車自問世以來，迅速成為研究各種控制理論的理想平臺(tái)，具有重大的理論意義，這要?dú)w功于她不穩(wěn)定的動(dòng)態(tài)性能和系統(tǒng)所具有的非線性。早在1 986年，日本電通大學(xué)教授山藤高橋最先提出了兩輪自平衡機(jī)器人的概念，并制造了一個(gè)在導(dǎo)軌上的兩輪倒立擺機(jī)器人。這個(gè)基本的概念就是用數(shù)字處理器來偵測(cè)平衡的改變，然后以平行的雙輪來保持機(jī)器的平穩(wěn)。這款被山藤高橋稱為平行自行車的機(jī)器人開創(chuàng)了兩輪自平衡機(jī)器人研究的先河。 美國(guó)S

6、egway LLC公司開發(fā)的Segway HT兩輪平臺(tái)電動(dòng)車把人們從傳統(tǒng)的“三點(diǎn)平衡”和以低重心、大商穩(wěn)的底盤設(shè)計(jì)來避免傾斜的束縛中解脫出來，通過檢測(cè)車體的角度和角述度用適當(dāng)?shù)幕貜?fù)轉(zhuǎn)矩來避免傾刳摔倒。系統(tǒng)利用5個(gè)慣性比率傳感器(陀螺儀)、2個(gè)傾角侍感器、電機(jī)編碼器和一世光學(xué)腳墊傳感囂把系統(tǒng)的的狀態(tài)提供給了控制器，控制器經(jīng)過運(yùn)算確定輸入給電機(jī)的能量大小。近十年來，兩輪自平衡機(jī)器人引起同外許多研究機(jī)構(gòu)和機(jī)器人愛好者極大關(guān)注，各種基于不同目的、不同設(shè)計(jì)方案和控制策略的自平衡系統(tǒng)相繼而生。在這方面國(guó)外的研究比較超前，研制出了一些具有代表性的機(jī)器人，國(guó)內(nèi)的研究基本上處于理論研究與實(shí)踐的初期，只開發(fā)出了少

7、數(shù)的實(shí)驗(yàn)原型機(jī)。中國(guó)科學(xué)技術(shù)大學(xué)自動(dòng)化系和力學(xué)系多位教授、博導(dǎo)、博士、碩士研究生和本科生聯(lián)合研制完成的科研成果，又名自平衡電動(dòng)代步車，是實(shí)用的兩輪自動(dòng)平衡車(機(jī)器人)，己能實(shí)用化，兩輪自平衡機(jī)器幾是將倒立擺原理移植到移動(dòng)機(jī)器人上的概念。關(guān)于倒立擺的研究多年來國(guó)內(nèi)外已經(jīng)研究非常成熟，其文獻(xiàn)也相當(dāng)之多，然而更重要的是如何將倒立擺有效地應(yīng)用在移動(dòng)機(jī)器人上。以上是國(guó)內(nèi)外兩輪自平衡小車的研究現(xiàn)狀，這些小車都對(duì)本課題的研究提供了很好的指導(dǎo)作用。然而，這些機(jī)器人的有關(guān)資料中并沒有對(duì)機(jī)器人速度控制的詳細(xì)介紹。二、系統(tǒng)設(shè)計(jì)與要求2.1項(xiàng)目研究?jī)?nèi)容1) 兩輪自平衡小車物理實(shí)體的設(shè)計(jì)，各部分之間的硬件連接，搭建。2

8、) 小車角速度傳感器、加速度傳感器對(duì)自身信息的采集，并實(shí)現(xiàn)采集的信息對(duì)單片機(jī)進(jìn)行傳送。 3）單片機(jī)軟件的設(shè)計(jì)，實(shí)現(xiàn)對(duì)小車的信息采集控制。2.2設(shè)計(jì)要求 設(shè)計(jì)一款可以站立的雙輪自平衡小車，一個(gè)集動(dòng)態(tài)決策和規(guī)劃、環(huán)境感知、行為控制和執(zhí)行等多種功能于一體的綜合復(fù)雜系統(tǒng)，其關(guān)鍵是在解決自平衡的同時(shí)，還能夠適應(yīng)在各種環(huán)境下的控制任務(wù)。通過運(yùn)用加速度傳感器、角速度傳感器等，可以實(shí)現(xiàn)小車的平衡站立。2.3方案選取 根據(jù)設(shè)計(jì)要求，確定如下方案：使用STC12C5A60S2單片機(jī)，MPU6050芯片為主要的控制通信器件，加裝一些電源模塊、電源驅(qū)動(dòng)模塊，實(shí)現(xiàn)對(duì)電動(dòng)小車的平衡站立控制。由單片機(jī)根據(jù)所檢測(cè)的各種信息實(shí)

9、現(xiàn)對(duì)小車的智能控制。這種方案能實(shí)現(xiàn)對(duì)自動(dòng)小車的運(yùn)動(dòng)狀態(tài)進(jìn)行實(shí)時(shí)控制，控制靈活、可靠、精度高，可滿足對(duì)系統(tǒng)的各項(xiàng)要求。 雙輪自平衡小車系統(tǒng)主要包括以下模塊：電源模塊、單片機(jī)主控模塊、電機(jī)驅(qū)動(dòng)模塊、傳感器模塊。2.3.1主控板模塊針對(duì)本設(shè)計(jì)特點(diǎn)，多開關(guān)量輸入程序控制系統(tǒng)，需要擅長(zhǎng)處理多開關(guān)量的標(biāo)準(zhǔn)單片機(jī)，而不能用精簡(jiǎn)I/O口和程序存儲(chǔ)器的小體積單片機(jī)，D/A，A/D功能也不必選用。根據(jù)這些分析，我選定了STC12C5A60S2單片機(jī)作為控制器，51系列單片機(jī)具有功能強(qiáng)大的位操作指令，I/O口均可按位尋址，程序空間大，對(duì)于本設(shè)計(jì)很適合，更可貴的是價(jià)格非常低廉。2.3.2電機(jī)驅(qū)動(dòng)模塊 采用功率三極管作

10、為功率放大器的輸出控制直流電機(jī)。線性型驅(qū)動(dòng)的電路結(jié)構(gòu)和原理簡(jiǎn)單，加速能力強(qiáng)，采用由達(dá)林頓管組成的H型橋式電路，用單片機(jī)控制達(dá)林頓管使之工作在占空比可調(diào)的開關(guān)狀態(tài)下，精確調(diào)整電動(dòng)機(jī)轉(zhuǎn)速。這種電路由于工作在管子的飽和截止模式下，效率非常高，H型橋式電路保證了簡(jiǎn)單的實(shí)現(xiàn)轉(zhuǎn)速和方向的控制，電子管的開關(guān)速度很快，穩(wěn)定性也極強(qiáng)，是一種廣泛采用的PWM調(diào)速技術(shù)?，F(xiàn)市面上很多這種芯片，我選了LM298N。由于電力電子器件只工作在開關(guān)狀態(tài)，主電路損耗較小，裝置效率較高。根據(jù)以上考慮，以及本設(shè)計(jì)中受控電機(jī)的容量和直流電機(jī)轉(zhuǎn)速的發(fā)展方向，本設(shè)計(jì)采用了H型單極型可逆PWM變換器件LM298N進(jìn)行電機(jī)調(diào)速。2.3.3傳

11、感器模塊 傳感器模塊采用了MPU6050，MPU-6000（6050）為全球首例整合性6軸運(yùn)動(dòng)處理組件，相較于多組件方案，免除了組合陀螺儀與加速器時(shí)之軸間差的問題，減少了大量的包裝空間。MPU-6000（6050）整合了3軸陀螺儀、3軸加速器，并含可藉由第二個(gè)I2C端口連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數(shù)位運(yùn)動(dòng)處理硬件加速引擎，由主要I2C端口以單一數(shù)據(jù)流的形式，向應(yīng)用端輸出完整的9軸融合演算技術(shù)的運(yùn)動(dòng)處理資料庫(kù)，可處理運(yùn)動(dòng)感測(cè)的復(fù)雜數(shù)據(jù)，降低了運(yùn)動(dòng)處理運(yùn)算對(duì)操作系統(tǒng)的負(fù)荷，并為應(yīng)用開發(fā)提供架構(gòu)化的API。2.3.4 卡爾曼算法的初值計(jì)算方法及實(shí)際應(yīng)用卡爾曼濾波是一種統(tǒng)計(jì)估算算法

12、，通過處理一系列帶有誤差的實(shí)際測(cè)量數(shù)據(jù)而得到的所需要的物理參數(shù)的最佳估計(jì)值。根據(jù)這一基本思想，同樣可以用于處理一系列帶有誤差的預(yù)報(bào)值而得到預(yù)報(bào)值得最佳估計(jì)值。卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。2.3.5 雙輪自平衡小車的PID控制兩輪自平衡小車的核心問題是平衡控制問題和運(yùn)動(dòng)控制問題。兩輪自平衡小車需要始終保持車身直立。PID控制算法是應(yīng)用最為普遍的一種算法，其特點(diǎn)是構(gòu)造簡(jiǎn)單，應(yīng)用有效及具備了許多成熟的穩(wěn)定性分析的方法，有很高的可靠性。針對(duì)兩輪自平衡小車的非線性和不穩(wěn)定性，利用非線性PD控制算法和PID差動(dòng)結(jié)構(gòu)可以

13、實(shí)現(xiàn)小車的平衡控制和運(yùn)動(dòng)控制。三、總體設(shè)計(jì)方案本設(shè)計(jì)采用STC12C5A60S2單片機(jī)作為控制器，通過軟件濾波和自動(dòng)控制理論算法使得小車達(dá)到平衡狀態(tài)。系統(tǒng)的傳感器采用角度傳感和角速度傳感器MPU6050采集小車車身的水平狀態(tài)值和小車的加速度值。并且采用了LM298雙橋大功率集成驅(qū)動(dòng)芯片來驅(qū)動(dòng)電機(jī)。依靠這些可靠的硬件設(shè)計(jì)，使用PID 閉環(huán)控制算法和卡爾曼濾波算法，使得整個(gè)硬件結(jié)構(gòu)和軟件系統(tǒng)能順利匹配。從而使得小車能保持直立自平衡狀態(tài)。 系統(tǒng)框圖如下： 系統(tǒng)框圖四、系統(tǒng)硬件設(shè)計(jì)4.1 STC12C5A60S2簡(jiǎn)介STC12C5A60S2是STC生產(chǎn)的單時(shí)鐘/機(jī)器周期(1T)的單片機(jī)，是高速、低功耗

14、、超強(qiáng)抗干擾的新一代8051單片機(jī)，指令代碼完全兼容傳統(tǒng)8051，但速度快8-12倍。內(nèi)部集成MAX810專用復(fù)位電路，2路PWM，8路高速10位A/D轉(zhuǎn)換，針對(duì)電機(jī)控制，強(qiáng)干擾場(chǎng)合。1、增強(qiáng)型8051CPU，1T(1024G)，單時(shí)鐘/機(jī)器周期2、工作電壓 5.5-3.5V3、1280字節(jié)RAM4、通用I/O口，復(fù)位后為：準(zhǔn)雙向口/弱上拉可設(shè)置成四種模式：準(zhǔn)雙向口/弱上拉，強(qiáng)推挽/強(qiáng)上拉，僅為輸入/高阻，開漏每個(gè)I/O口驅(qū)動(dòng)能力均可達(dá)到20mA，但整個(gè)芯片最大不要超過120mA5、有EEPROM功能6、看門狗7、內(nèi)部集成MAX810專用復(fù)位電路8、外部掉電檢測(cè)電路9、時(shí)鐘源：外部高精度晶體/

15、時(shí)鐘，內(nèi)部R/C振蕩器常溫下內(nèi)部R/C振蕩器頻率為：5.0V單片機(jī)為：1117MHz3.3V 單片機(jī)為：812MHz10、4個(gè)16位定時(shí)器兩個(gè)與傳統(tǒng)8051兼容的定時(shí)器/計(jì)數(shù)器，16位定時(shí)器T0和T111、3個(gè)時(shí)鐘輸出口，可由T0的溢出在P3.4/T0輸出時(shí)鐘，可由T1的溢出在P3.5/T1輸出時(shí)鐘，獨(dú)立波特率發(fā)生器可以在P1.0口輸出時(shí)鐘12、外部中斷I/O口7路，傳統(tǒng)的下降沿中斷或電平觸發(fā)中斷，并新增支持上升沿中斷的PCA模塊，Power Down模式可由外部中斷喚醒，INT0/P3.2，INT1/P3.3，T0/P3.4，T1/P3.5，RxD/P3.0，CCP0/P1.3，CCP0/P

16、1.313、PWM2路14、A/D轉(zhuǎn)換，10位精度ADC，共8路，轉(zhuǎn)換速度可達(dá)250K/S15、通用全雙工異步串行口(UART)16、雙串口，RxD2/P1.2，TxD2/P1.317、封裝：LQFP-48，LQFP-44，PDIP-40，PLCC管腳說明P0.0P0.7 P0：P0口既可以作為輸入/輸出口，也可以作為地址/數(shù)據(jù)復(fù)用總線使用。當(dāng)P0口作為輸入/輸出口時(shí)，P0是一個(gè)8位準(zhǔn)雙向口，內(nèi)部有弱上拉電阻，無需外接上拉電阻。當(dāng)P0作為地址/數(shù)據(jù)復(fù)用總線使用時(shí)，是低8位地址線A0A7，數(shù)據(jù)線D0D7。4.2 MPU6050簡(jiǎn)介MPU-60X0 是全球首例9 軸運(yùn)動(dòng)處理傳感器。它集成了3 軸M

17、EMS 陀螺儀，3 軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP（Digital Motion Processor），可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過其I2C 或SPI 接口輸出一個(gè)9 軸的信號(hào)（SPI 接口僅在MPU-6000 可用）。MPU-60X0 也可以通過其I2C 接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。 MPU-60X0 對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16 位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍都是用戶可控的，陀螺儀可測(cè)范圍為±250，±500，&#

18、177;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)的振蕩器。芯片尺寸4×4×0.9mm，采用QFN 封裝（無引線方形封裝），可承受最大10000g 的沖擊，并

19、有可編程的低通濾波器。 關(guān)于電源，MPU-60X0 可支持VDD 范圍2.5V±5%，3.0V±5%，或3.3V±5%。另外MPU-6050 還有一個(gè)VLOGIC 引腳，用來為I2C 輸出提供邏輯電平。VLOGIC 電壓可取1.8±5%或者VDD。5V/3.3V電源輸入，XDA/XCL用于外接其他IIC接口傳感器，不用。 SDA，SCL連接到單片機(jī)，INT產(chǎn)生中斷信號(hào)，連接至單片機(jī)。AD0一般接地。4.3 LM298簡(jiǎn)介L(zhǎng)298N是SGS公司的產(chǎn)品，內(nèi)部包含4通道邏輯驅(qū)動(dòng)電路。是一種二相和四相電機(jī)的專用驅(qū)動(dòng)器，即內(nèi)含二個(gè)H橋的高電壓大電流雙全橋式驅(qū)動(dòng)器，

20、接收標(biāo)準(zhǔn)TTL邏輯電平信號(hào)，可驅(qū)動(dòng)46V、2A以下的電機(jī)。L298有兩路電源分別為邏輯電源和動(dòng)力電源，上圖中6V為邏輯電源，12V為動(dòng)力電源。J4接入邏輯電源，J6接入動(dòng)力電源，J1與J2分別為單片機(jī)控制兩個(gè)電機(jī)的輸入端，J3與J5分別與兩個(gè)電極的正負(fù)極相連。ENA與ENB直接接入6V邏輯電源也就是說兩個(gè)電機(jī)時(shí)刻都工作在使能狀態(tài)，控制電機(jī)的運(yùn)行狀態(tài)只有通過J1與J2兩個(gè)接口。IN1IN2ENA電機(jī)狀態(tài)XX0停止101順時(shí)針011逆時(shí)針000停止110停止4.4硬件平臺(tái)的組合電源模塊接12V電源，然后通過電源模塊給系統(tǒng)的所有部分供電。MPU6050的SCL 和SDA引腳分別接單片機(jī)的P2.0和P

21、2.1引腳。電機(jī)驅(qū)動(dòng)模塊的INT0，INT1，INT2，INT3分別接單片機(jī)的P2.1、P2.2、P2.5、P2.0引腳。連接后各個(gè)電源線，即完成小車的整體硬件搭建。小車整體布局合理，結(jié)構(gòu)層次清晰，重心穩(wěn)定。五、系統(tǒng)軟件設(shè)計(jì)5.1軟件設(shè)計(jì)流程5.2 軟件程序如下/*/ 兩輪自平衡車最終版控制程序（6軸MPU6050+互補(bǔ)濾波+PWM電機(jī)） / 單片機(jī)STC12C5A60S2 / 晶振：20M/ 日期：2012.11.26 - ？*/#include <REG52.H>#include <math.h> #include <stdio.h> #include

22、<INTRINS.H>typedef unsigned char uchar;typedef unsigned short ushort;typedef unsigned int uint;/*功能模塊頭文件*#include "DELAY.H" /延時(shí)頭文件#include "STC_ISP.H" /程序燒錄頭文件#include "SET_SERIAL.H"/串口頭文件#include "SET_PWM.H"/PWM頭文件#include "MOTOR.H"/電機(jī)控制頭文件#in

23、clude "MPU6050.H"/MPU6050頭文件/*角度參數(shù)*float Gyro_y; /Y軸陀螺儀數(shù)據(jù)暫存float Angle_gy; /由角速度計(jì)算的傾斜角度float Accel_x; /X軸加速度值暫存float Angle_ax; /由加速度計(jì)算的傾斜角度float Angle; /小車最終傾斜角度uchar value; /角度正負(fù)極性標(biāo)記/*PWM參數(shù)*int speed_mr; /右電機(jī)轉(zhuǎn)速int speed_ml; /左電機(jī)轉(zhuǎn)速int PWM_R; /右輪PWM值計(jì)算int PWM_L; /左輪PWM值計(jì)算float PWM; /綜合PWM計(jì)算

24、float PWMI; /PWM積分值/*電機(jī)參數(shù)*float speed_r_l;/電機(jī)轉(zhuǎn)速float speed; /電機(jī)轉(zhuǎn)速濾波float position; /位移/*藍(lán)牙遙控參數(shù)*uchar remote_char;char turn_need;char speed_need;/*/定時(shí)器100Hz數(shù)據(jù)更新中斷/*void Init_Timer1(void)/10毫秒20MHz,100Hz刷新頻率AUXR &= 0xBF;/定時(shí)器時(shí)鐘12T模式TMOD &= 0x0F;/設(shè)置定時(shí)器模式TMOD |= 0x10;/設(shè)置定時(shí)器模式TL1 = 0xE5; /設(shè)置定時(shí)初值TH

25、1 = 0xBE; /設(shè)置定時(shí)初值TF1 = 0; /清除TF1標(biāo)志TR1 = 1; /定時(shí)器1開始計(jì)時(shí)/*/中斷控制初始化/*void Init_Interr(void) EA = 1; /開總中斷 EX0 = 1; /開外部中斷INT0 EX1 = 1; /開外部中斷INT1 IT0 = 1; /下降沿觸發(fā) IT1 = 1; /下降沿觸發(fā)ET1 = 1; /開定時(shí)器1中斷/*卡爾曼參數(shù)*float code Q_angle=0.001; float code Q_gyro=0.003;float code R_angle=0.5;float code dt=0.01; /dt為kalman

26、濾波器采樣時(shí)間;char code C_0 = 1;float xdata Q_bias, Angle_err;float xdata PCt_0, PCt_1, E;float xdata K_0, K_1, t_0, t_1;float xdata Pdot4 =0,0,0,0;float xdata PP22 = 1, 0 , 0, 1 ;/*/ 卡爾曼濾波/*/Kalman濾波，20MHz的處理時(shí)間約0.77ms；void Kalman_Filter(float Accel,float Gyro)Angle+=(Gyro - Q_bias) * dt; /先驗(yàn)估計(jì)Pdot0=Q_ang

27、le - PP01 - PP10; / Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分Pdot1=- PP11;Pdot2=- PP11;Pdot3=Q_gyro;PP00 += Pdot0 * dt; / Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分PP01 += Pdot1 * dt; / =先驗(yàn)估計(jì)誤差協(xié)方差PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;Angle_err = Accel - Angle;/zk-先驗(yàn)估計(jì)PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1

28、 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * PP01;PP00 -= K_0 * t_0; /后驗(yàn)估計(jì)誤差協(xié)方差PP01 -= K_0 * t_1;PP10 -= K_1 * t_0;PP11 -= K_1 * t_1;Angle+= K_0 * Angle_err; /后驗(yàn)估計(jì)Q_bias+= K_1 * Angle_err; /后驗(yàn)估計(jì)Gyro_y = Gyro - Q_bias; /輸出值(后驗(yàn)估計(jì))的微分=角速度/*/ 傾角計(jì)算（卡爾曼融合）/*void Angle_Calcu(void) /-加速度-/范圍為2g時(shí)，換算關(guān)系：16384 LSB/g/角度

29、較小時(shí)，x=sinx得到角度（弧度）, deg = rad*180/3.14/因?yàn)閤>=sinx,故乘以1.3適當(dāng)放大Accel_x = GetData(ACCEL_XOUT_H); /讀取X軸加速度Angle_ax = (Accel_x - 1100) /16384; /去除零點(diǎn)偏移,計(jì)算得到角度（弧度）Angle_ax = Angle_ax*1.2*180/3.14; /弧度轉(zhuǎn)換為度, /-角速度-/范圍為2000deg/s時(shí)，換算關(guān)系：16.4 LSB/(deg/s)Gyro_y = GetData(GYRO_YOUT_H); /靜止時(shí)角速度Y軸輸出為-30左右Gyro_y = -

30、(Gyro_y + 30)/16.4; /去除零點(diǎn)偏移，計(jì)算角速度值,負(fù)號(hào)為方向處理 /Angle_gy = Angle_gy + Gyro_y*0.01; /角速度積分得到傾斜角度./-卡爾曼濾波融合-Kalman_Filter(Angle_ax,Gyro_y); /卡爾曼濾波計(jì)算傾角/*/-互補(bǔ)濾波-/補(bǔ)償原理是取當(dāng)前傾角和加速度獲得傾角差值進(jìn)行放大，然后與 /陀螺儀角速度疊加后再積分，從而使傾角最跟蹤為加速度獲得的角度/0.5為放大倍數(shù)，可調(diào)節(jié)補(bǔ)償度；0.01為系統(tǒng)周期10msAngle = Angle + (Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/

31、/*/電機(jī)轉(zhuǎn)速和位移值計(jì)算/*void Psn_Calcu(void) speed_r_l =(speed_mr + speed_ml)*0.5;speed *= 0.7; /車輪速度濾波speed += speed_r_l*0.3;position += speed; /積分得到位移position += speed_need;if(position<-6000) position = -6000;if(position> 6000) position = 6000; static float code Kp = 9.0; /PID參數(shù)static float code Kd =

32、 2.6; /PID參數(shù)static float code Kpn = 0.01; /PID參數(shù)static float code Ksp = 2.0; /PID參數(shù)/*/電機(jī)PWM值計(jì)算/*void PWM_Calcu(void) if(Angle<-40|Angle>40) /角度過大，關(guān)閉電機(jī) CCAP0H = 0; CCAP1H = 0; return;PWM = Kp*Angle + Kd*Gyro_y; /PID：角速度和角度PWM += Kpn*position + Ksp*speed; /PID：速度和位置PWM_R = PWM + turn_need;PWM_L

33、= PWM - turn_need;PWM_Motor(PWM_L,PWM_R); /*/手機(jī)藍(lán)牙遙控/*void run (void) speed_need = -80; /前進(jìn) /*=*/*/main/*void main() delaynms(500); /上電延時(shí)Init_PWM(); /PWM初始化 Init_Timer0(); /初始化定時(shí)器0，作為PWM時(shí)鐘源Init_Timer1(); /初始化定時(shí)器1Init_Interr(); /中斷初始化Init_Motor(); /電機(jī)控制初始化Init_BRT(); /串口初始化（獨(dú)立波特率）InitMPU6050(); /初始化MP

34、U6050delaynms(500); while(1) run()/*=*/*timer1中斷*void Timer1_Update(void) interrupt 3 TL1 = 0xE5; /設(shè)置定時(shí)初值10MS TH1 = 0xBE; /STC_ISP(); /程序下載 Angle_Calcu(); /傾角計(jì)算 Psn_Calcu(); /電機(jī)位移計(jì)算 PWM_Calcu(); /計(jì)算PWM值 speed_mr = speed_ml = 0; /*右電機(jī)中斷*void INT_L(void) interrupt 0 if(SPDL = 1) speed_ml+; /左電機(jī)前進(jìn) else

35、 speed_ml-; /左電機(jī)后退 LED = LED; /*左電機(jī)中斷*void INT_R(void) interrupt 2 if(SPDR = 1) speed_mr+; /右電機(jī)前進(jìn) else speed_mr-; /右電機(jī)后退 LED = LED; 六、實(shí)驗(yàn)結(jié)果與分析經(jīng)過三周的努力，從開始的選題分組，然后再到開題報(bào)告，電路板焊接，機(jī)構(gòu)設(shè)計(jì)與調(diào)試，硬件電路調(diào)試，軟件電路調(diào)試，再到最后的綜合調(diào)試，我們最終取得了以下實(shí)驗(yàn)結(jié)果1.完成了一個(gè)結(jié)構(gòu)穩(wěn)定的小車硬件平臺(tái)搭建。2.完成了一個(gè)可以控制小車進(jìn)行平衡調(diào)整的。3.設(shè)計(jì)了一款可以基本站立的雙輪自平衡小車。 小車圖片 小車調(diào)試圖片七、參與人心

36、得從這次科研訓(xùn)練中，學(xué)到了很多知識(shí)，不單單加強(qiáng)了自己的動(dòng)手能力，也學(xué)會(huì)了單片機(jī)的應(yīng)用，需要將各個(gè)模塊吃透才能將其運(yùn)用自如，動(dòng)腦是前提，動(dòng)手是硬道理，在動(dòng)手制作過程中總結(jié)經(jīng)驗(yàn)，跟隊(duì)友和老師一起克服各種困難。在此過程中我知道，一個(gè)項(xiàng)目的完成不只是靠一個(gè)人就能成功的，需要團(tuán)隊(duì)的同心協(xié)力，迎難而上，共同克服各種困難。在訓(xùn)練的過程中，在硬件平臺(tái)搭建起來后，發(fā)現(xiàn)小車一直不能平衡的行走，于是乎我和隊(duì)友每天都在研究程序，通過改參數(shù)，發(fā)現(xiàn)還是不能平衡，結(jié)果發(fā)現(xiàn)重心不穩(wěn)，不在小車的中心，于是又重新搭建硬件平臺(tái)，把重心放低，放在小車的中心。經(jīng)過這個(gè)過程，發(fā)現(xiàn)還是不能成功。再后來在老師的指導(dǎo)下，通過調(diào)PID參數(shù)，把小車的速度跟角度調(diào)到合適的位置，最后終于完成了。在完成的那一刻發(fā)現(xiàn)這次項(xiàng)目遠(yuǎn)非想象的那么簡(jiǎn)單，真正通過自己動(dòng)手去做才會(huì)發(fā)現(xiàn)問題和如何堅(jiān)決問題。在這次項(xiàng)目制作過程中我獲得最重的東西是相互學(xué)習(xí)，彌補(bǔ)自己的不足，一起討論中得到不同的意見，從中找到合適的見解，討論一點(diǎn)一滴的知識(shí)并