兩輪自平衡小車智能控制研究中期檢查報(bào)告書

1、項(xiàng)目編號(hào)ky2014-131學(xué)科分類號(hào)（二級(jí)） 510.10 云南師范大學(xué)大學(xué)生科研訓(xùn)練基金項(xiàng)目中期檢查報(bào)告書項(xiàng)目名稱: 兩輪自平衡小車智能控制研究 項(xiàng)目類型: 一般項(xiàng)目 主 持 人: 趙慶波 起止年月： 2014年12月 結(jié)題日期： 2015年12月 聯(lián)系電話:云南師范大學(xué)教務(wù)處填 表 說(shuō) 明一、填寫項(xiàng)目中期檢查報(bào)告書前，請(qǐng)先查閱云南師范大學(xué)大學(xué)生科研訓(xùn)練基金管理辦法和相關(guān)通知。二、項(xiàng)目中期檢查報(bào)告書各項(xiàng)內(nèi)容，必須實(shí)事求是，表達(dá)要明確嚴(yán)謹(jǐn)，并要求用打印。對(duì)于填寫不合要求、內(nèi)容含糊不清、字跡潦草者，不予受理。三、項(xiàng)目類型：選填重點(diǎn)項(xiàng)目或一般項(xiàng)目。四、項(xiàng)目類別：選填自然

2、科學(xué)或社會(huì)科學(xué)。五、封面的項(xiàng)目編號(hào)與申請(qǐng)書上的編號(hào)一致。六、打印格式：（一）紙張為A4大小，雙面打印；（二）文中小標(biāo)題：五號(hào)、黑體；（三）欄內(nèi)正文：五號(hào)、宋體。七、填寫中期檢查報(bào)告書一式二份（至少含一份原件），其中一份提交教務(wù)處實(shí)踐教學(xué)科，另一份留學(xué)院存檔。課題名稱兩輪自平衡小車智能控制研究項(xiàng)目類別自然科學(xué)項(xiàng)目編號(hào)ky2014-131填表時(shí)間2015年5月20日主 持 人趙慶波主要成員趙慶波、余坦藏、聶浩南一、項(xiàng)目進(jìn)展及取得的成果情況（一）項(xiàng)目進(jìn)展成果情況本研究從2014年10月份就開始了計(jì)劃時(shí)間范圍內(nèi)的相關(guān)學(xué)習(xí)及制作，到目前為止已經(jīng)學(xué)習(xí)了STC12C5A60S2單片機(jī)，并且能夠熟練的掌握運(yùn)用

3、相關(guān)的應(yīng)用軟件，同時(shí)對(duì)雙環(huán)PID控制及卡爾曼濾波、涉及的各種傳感器、AD轉(zhuǎn)換等知識(shí)有了深入的理解，硬件電路也制作完成了，程序設(shè)計(jì)編寫實(shí)現(xiàn)了初步的完成。1. 在指導(dǎo)老師的指導(dǎo)下針對(duì)該項(xiàng)目的相關(guān)文獻(xiàn)查閱和資料的收集與學(xué)習(xí)；時(shí)間:2014年 10月至2015年1月?？蠢蠋熃o的相關(guān)資料，并利用學(xué)校的圖書館資源查閱文獻(xiàn)及相關(guān)材料，與老師同學(xué)討論學(xué)習(xí)相關(guān)知識(shí)。通過(guò)這一階段的學(xué)習(xí)對(duì)STC12C5A60S2單片的基礎(chǔ)知識(shí)有了一定的掌握，單片機(jī)技術(shù)是軟件和硬件結(jié)合的技術(shù)，學(xué)習(xí)了的一些關(guān)于單片機(jī)開發(fā)的軟件，還學(xué)習(xí)了電機(jī)驅(qū)動(dòng)模塊、陀螺儀加速計(jì)、HC-06藍(lán)牙模塊等大量的單片機(jī)拓展功能實(shí)驗(yàn)器材的功能結(jié)構(gòu)。對(duì)雙環(huán)PID

4、控制及卡爾曼濾波算法都有了全面的掌握。2.在指導(dǎo)老師的指導(dǎo)下開始著手項(xiàng)目的研發(fā)和學(xué)習(xí)；(1).根據(jù)指導(dǎo)老師的指導(dǎo)及指引，進(jìn)行兩輪自平衡智能控制小車的硬件制作；時(shí)間：2015年3月 至 2015年4月。 內(nèi)容及取得相應(yīng)的成果：硬件結(jié)構(gòu)基本完成,能夠?qū)崿F(xiàn)直立行走。(2) 根據(jù)指導(dǎo)老師的指導(dǎo)及指引，對(duì)兩輪自平衡智能控制小車的程序設(shè)計(jì)編寫；時(shí)間：2015年4月 至 2015年5月。內(nèi)容及取得相應(yīng)的成果：可是實(shí)現(xiàn)一部分的功能,但程序設(shè)計(jì)還不成熟,系統(tǒng)運(yùn)行不穩(wěn)定,仍需改進(jìn)。 (二）階段性成果情況 通過(guò)以上比較深入的理論學(xué)習(xí)，以及在我們著手項(xiàng)目的研發(fā)，經(jīng)過(guò)幾個(gè)月的不斷改良，我們初步實(shí)現(xiàn)了兩輪自平衡智能控制小

5、車的直立行走。長(zhǎng)時(shí)間認(rèn)真的理論知識(shí)學(xué)習(xí)為后期研發(fā)制作打下了堅(jiān)實(shí)的理論基礎(chǔ),兩輪自平衡智能控制小車硬件結(jié)構(gòu)都已經(jīng)完成,且性能良好,程序設(shè)計(jì)編寫也有了初步的實(shí)現(xiàn),但由于程序設(shè)計(jì)編寫的時(shí)間較短,所以還不夠成熟,智能控制兩輪小車不能穩(wěn)定運(yùn)行,從中還有許多問(wèn)題,我們會(huì)在后期的研究中完善,具體成果如下：1 兩輪自平衡智能控制小車的硬件部分（1） 速度檢測(cè)模塊設(shè)計(jì)兩輪自平衡小車的原理是利用地面對(duì)車輪的摩擦力抵消車受到的重力，在本系統(tǒng)的控制環(huán)節(jié)中有兩路閉環(huán)控制，即傾角閉環(huán)控制以及速度閉環(huán)控制。為實(shí)現(xiàn)速度的閉環(huán)控制，必須加入速度檢測(cè)裝置實(shí)現(xiàn)速度閉環(huán)控制中的反饋環(huán)節(jié)。本系統(tǒng)測(cè)速模塊采用OMRON(歐姆龍)公司50

6、0線增量式旋轉(zhuǎn)編碼器。編碼器內(nèi)部為一個(gè)中心有軸的光電碼盤，其上有環(huá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相脈沖以代表零位參考位。由于編碼器采用集電極開路輸出，輸出波形為矩形波，因此編碼器外圍電路較為簡(jiǎn)單。需要在信號(hào)輸出端接入一個(gè)上拉電阻，即可將信號(hào)提供給單片機(jī)采集數(shù)據(jù)。（2） 驅(qū)動(dòng)電路采用功率三極管作為功率放大器的輸出控制直流電機(jī)。線性型驅(qū)動(dòng)的電路結(jié)構(gòu)和原理簡(jiǎn)單，加速能力強(qiáng)，采用由達(dá)林頓管組成的H型橋式電路，用單片機(jī)控制達(dá)林頓

7、管使之工作在占空比可調(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，L298N是SGS公司的產(chǎn)品，內(nèi)部包含4通道邏輯驅(qū)動(dòng)電路。是一種二相和四相電機(jī)的專用驅(qū)動(dòng)器，即內(nèi)含二個(gè)H橋的高電壓大電流雙全橋式驅(qū)動(dòng)器，接收標(biāo)準(zhǔn)TTL邏輯電平信號(hào)，可驅(qū)動(dòng)46V、2A以下的電機(jī)。 由于電力電子器件只工作在開關(guān)狀態(tài)，主電路損耗較小，裝置效率較高。根據(jù)以上考慮，以及本設(shè)計(jì)中受控電機(jī)的容量和直流電機(jī)轉(zhuǎn)速的發(fā)展方向，本設(shè)計(jì)采用

8、了H型單極型可逆PWM變換器件LM298N進(jìn)行電機(jī)調(diào)速。具體原理圖如下：圖1 電機(jī)驅(qū)動(dòng)電路原理圖（3） CPU微控制器CPU微控制器采用STC12C5A60S2單片機(jī)，微控制器作為整個(gè)系統(tǒng)的核心部分，主要負(fù)責(zé)對(duì)傳感器采集的信號(hào)進(jìn)行實(shí)時(shí)處理，但其計(jì)算量較大，普通的單片機(jī)不能滿足使用要求。本系統(tǒng)選用了高速STC12C5A60S2單片機(jī)作為微處理器，具有豐富的I/O接口、有雙串口。其功耗低、超強(qiáng)抗干擾能力、指令代碼完全兼容傳統(tǒng)單片機(jī)8051，能夠滿足系統(tǒng)的設(shè)計(jì)需求，在單片機(jī)上增加復(fù)位電路、時(shí)鐘電路等外圍電路構(gòu)成單片機(jī)最小系統(tǒng)，讓單片機(jī)工作起來(lái)，然后通過(guò)編寫程序?qū)纹瑱C(jī)進(jìn)行控制，進(jìn)而單片機(jī)對(duì)傳感器采集

9、信號(hào)進(jìn)行處理。在程序設(shè)計(jì)編寫方面十分容易，且成本較低，也可以很好的實(shí)現(xiàn)本設(shè)計(jì)。具體STC12C5A60S2單片機(jī)原理圖如下：圖2 STC12C5A60S2單片機(jī)原理圖（4） 陀螺儀傳感器模塊陀螺儀可以用來(lái)測(cè)量物體的旋轉(zhuǎn)角速度。本設(shè)計(jì)選用村田公司出品的ENC-03系列的加速度傳感器。它利用了旋轉(zhuǎn)坐標(biāo)系中的物體會(huì)受到科里奧利力的原理，在器件中利用壓電陶瓷做成振動(dòng)單元。當(dāng)旋轉(zhuǎn)器件時(shí)會(huì)改變振動(dòng)頻率從而反映出物體旋轉(zhuǎn)的角速度。在車模上安裝角速度傳感器，可以測(cè)量車模傾斜角速度，再對(duì)傾斜角速度進(jìn)行積分就得到了車模的傾角。由于陀螺儀輸出的是車模的角速度，不會(huì)受到車體運(yùn)動(dòng)的影響，因此該信號(hào)中噪聲很小。車模的角度

10、又是通過(guò)對(duì)角速度積分而得，這可進(jìn)一步平滑信號(hào)，從而使得角度信號(hào)更加穩(wěn)定。因此車?？刂扑枰慕嵌群徒撬俣瓤梢允褂猛勇輧x所得到的信號(hào)。由于從陀螺儀角速度獲得角度信息，需要經(jīng)過(guò)積分運(yùn)算。如果角速度信號(hào)存在微小的偏差和漂移，經(jīng)過(guò)積分運(yùn)算之后，變化形成積累誤差。這個(gè)誤差會(huì)隨著時(shí)間延長(zhǎng)逐步增加，最終導(dǎo)致電路飽和，無(wú)法形成正確的角度信號(hào)。為了消除積分漂移，我們引入姿態(tài)測(cè)量的方法。硬件兩輪自平衡小車實(shí)物圖如下圖： 圖3 兩輪自平衡小車實(shí)物圖 2 程序的設(shè)計(jì)與編寫在自平衡小車未做控制時(shí)，不論其車身向前或向后傾斜，兩輪都處于靜止?fàn)顟B(tài)，這時(shí)其車身前后擺動(dòng)與其車輪轉(zhuǎn)動(dòng)是相互獨(dú)立的；當(dāng)其開始控制時(shí)，其車身的狀態(tài)變化使

11、小車有靜止、前進(jìn)(前傾)、后退(后仰)三種運(yùn)動(dòng)的方式，在正確的控制策略下，小車能夠保持自身的平衡。這三種運(yùn)動(dòng)方式與控制策略如圖所示 （1）前傾 （2）靜止 （3）后仰圖4 小車三種運(yùn)動(dòng)方式（1）前傾狀態(tài)：即車身重心靠前，車身會(huì)向前傾斜，則驅(qū)動(dòng)車輪向前滾動(dòng)，以保持小車平衡。（2）靜止?fàn)顟B(tài)：即車身重心位于電機(jī)軸心線的正上方，則小車將保持動(dòng)態(tài)平衡靜止?fàn)顟B(tài)，不需要做任何控制。（3）后仰狀態(tài)：即車身重心靠后，車身會(huì)向后傾斜，則驅(qū)動(dòng)車輪向后滾動(dòng)，以保持小車平衡。因此，兩輪自平衡小車平衡控制的基本思想是：當(dāng)測(cè)量?jī)A斜角度的傳感器檢測(cè)到車體產(chǎn)生傾斜時(shí)，控制系統(tǒng)會(huì)根據(jù)測(cè)得的傾角產(chǎn)生一個(gè)相應(yīng)的力矩，通過(guò)控制電機(jī)驅(qū)動(dòng)

12、兩個(gè)車輪朝車身要倒下的方向運(yùn)動(dòng)，以保持小車自身的動(dòng)態(tài)平衡。主要程序清單:#include <reg51.h>#include <math.h>#include <stdio.h> #include <string.h>#include<intrins.h>#include<STC12C5A60S2.h>#define SMPLRT_DIV 0x19#define CONFIG 0x1A#define GYRO_CONFIG 0x1B#define ACCEL_CONFIG 0x1C#define ACCEL_XOUT_H

13、0x3B#define ACCEL_XOUT_L 0x3C#define ACCEL_YOUT_H 0x3D#define ACCEL_YOUT_L 0x3E#define ACCEL_ZOUT_H 0x3F#define ACCEL_ZOUT_L 0x40#define TEMP_OUT_H 0x41#define TEMP_OUT_L 0x42#define GYRO_XOUT_H 0x43#define GYRO_XOUT_L 0x44#define GYRO_YOUT_H 0x45#define GYRO_YOUT_L 0x46#define GYRO_ZOUT_H 0x47#defi

14、ne GYRO_ZOUT_L 0x48#define PWR_MGMT_1 0x6B#define WHO_AM_I 0x75#define SlaveAddress 0xD0typedef unsigned char uchar;typedef unsigned short ushort;typedef unsigned int uint;sbit in1=P11;sbit in2=P12;sbit in3=P15;sbit in4=P16;sbit SCL=P27;sbit SDA=P26;void delay(unsigned int k);void InitMPU6050();void

15、 Delay5us();void I2C_Start();void I2C_Stop();void I2C_SendACK(bit ack);bit I2C_RecvACK();void I2C_SendByte(uchar dat);uchar I2C_RecvByte();void I2C_ReadPage();void I2C_WritePage();int GetData(uchar REG_Address);int PWM(int a,int b);void PWM_calculate( void);int temp;uchar Single_ReadI2C(uchar REG_Ad

16、dress);float Adjust_up1,Adjust_up2;unsigned int PWM_Duty1,PWM_Duty2;/*角度參數(shù)*float Gyro_y; /Y軸陀螺儀數(shù)據(jù)暫存float Angle_gy; /由角速度計(jì)算的傾斜角度f(wàn)loat Accel_x; /X軸加速度值暫存float Angle_ax; /由加速度計(jì)算的傾斜角度f(wàn)loat Angle; /小車最終傾斜角度f(wàn)loat Angle1;uchar value; /角度正負(fù)極性標(biāo)記/*卡爾曼參數(shù)*float code Q_angle=0.001; float code Q_gyro=0.003;float

17、code R_angle=0.5;float code dt=0.01; /dt為kalman濾波器采樣時(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 ; void Single_WriteI2C(uchar REG_Address,uchar REG_data);/*/ 卡爾曼濾波/* /在程序中利用Angl

18、e+=(Gyro - Q_bias) * dt計(jì)算出陀螺儀積分出的角度，其中Q_bias是陀螺儀偏差。/此時(shí)利用陀螺儀積分求出的Angle相當(dāng)于系統(tǒng)的估計(jì)值，得到系統(tǒng)的觀測(cè)方程；而加速度計(jì)檢測(cè)的角/度Accel相當(dāng)于系統(tǒng)中的測(cè)量值，得到系統(tǒng)狀態(tài)方程。/程序中Q_angle和Q_gyro分別表示系統(tǒng)對(duì)加速度計(jì)及陀螺儀的信任度。/根據(jù)Pdot = A*P + P*A' + Q_angle計(jì)算出先驗(yàn)估計(jì)協(xié)方差的微分，/用于將當(dāng)前估計(jì)值進(jìn)行線性化處理。其中A為雅克比矩陣。 /隨后計(jì)算系統(tǒng)預(yù)測(cè)角度的協(xié)方差矩陣P。計(jì)算估計(jì)值A(chǔ)ccel與預(yù)測(cè)值A(chǔ)ngle間的誤差A(yù)ngle_err。/計(jì)算卡爾曼增益K

19、_0,K_1，K_0用于最優(yōu)估計(jì)值，K_1用于計(jì)算最優(yōu)估計(jì)值的偏差并更新協(xié)方差矩陣P。/通過(guò)卡爾曼增益計(jì)算出最優(yōu)估計(jì)值A(chǔ)ngle及預(yù)測(cè)值偏差Q_bias，此時(shí)得到最優(yōu)角度值A(chǔ)ngle及角速度值。/Kalman濾波，20MHz的處理時(shí)間約0.77ms； void Kalman_Filter(float Accel,float Gyro) Angle+=(Gyro-Q_bias) * dt; /先驗(yàn)估計(jì)Pdot0=Q_angle - PP01 - PP10; / Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分Pdot1=- PP11;Pdot2=- PP11; Pdot3=Q_gyro; PP00 += Pdot

20、0 * 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 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP01;PP00 -= K_0 * t_0; /后驗(yàn)估計(jì)誤差協(xié)方差 PP

21、01 -= 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ì))的微分=角速度/*/*卡爾曼融合*/*Angle_Calcu(void) /-加速度-/范圍為2g時(shí)，換算關(guān)系：16384 LSB/g /角度較小時(shí)，x=sinx得到角度（弧度）, deg = rad*180/3.14 /因?yàn)閤>=sinx,故乘以1.3適當(dāng)放大 Accel_x = G

22、etData(ACCEL_XOUT_H); /讀取X軸加速度if(Accel_x>0) value=1;else value=0; 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 = -(Gyro_y + 30)/16.4; /去除零點(diǎn)偏移，計(jì)算角

23、速度值,負(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)周期10ms Angle = Angle + (Angle_ax-Angle)*0.5 + Gyro_y)*0.01); int PWM(int a,int b)CCAP0H=(655

24、36-a)/256; CCAP0L=(65536-a)%256;CCAPM0=0x42; /PCA模塊0工作在8位PWM模式CCAP1H=(65536-b)/256; CCAP1L=(65536-b)%256;CCAPM1=0x42;CR=1; /啟動(dòng)PCA定時(shí)器void PWM_calculate( void)/ Adjust_up1 =Gyro_y*2;Adjust_up1=0;Adjust_up2 = Angle*200;if( Angle>0) /加速度向前 in1=0; in2=1; in3=0; in4=1; if(Gyro_y>0) PWM_Duty1 =(int)A

25、djust_up2 -(int)Adjust_up1; if(Gyro_y<=0) PWM_Duty1 =(int)Adjust_up1 + (int)Adjust_up2; else if(Angle<0)/加速度向后 in1=1; in2=0; in3=1; in4=0; if(Gyro_y<=0) PWM_Duty1 =0 - (int)Adjust_up2 - (int)Adjust_up1; if(Gyro_y>0) PWM_Duty1 =0 - (int)Adjust_up2 + (int)Adjust_up1; void delay(unsigned in

26、t k)unsigned int i,j;for(i=0;i<k;i+) for(j=0;j<121;j+);void I2C_Start() SDA = 1; SCL = 1; Delay5us(); SDA = 0; Delay5us(); SCL = 0;void I2C_SendACK(bit ack) SDA = ack; SCL = 1; Delay5us(); SCL = 0; Delay5us();bit I2C_RecvACK() SCL = 1; Delay5us(); CY = SDA; SCL = 0; Delay5us(); return CY;void

27、I2C_SendByte(uchar dat) uchar i; for (i=0; i<8; i+) dat <<= 1; SDA = CY; SCL = 1; Delay5us(); SCL = 0; Delay5us(); I2C_RecvACK();uchar I2C_RecvByte() uchar i; uchar dat = 0; SDA = 1; for (i=0; i<8; i+) dat <<= 1; SCL = 1; Delay5us(); dat |= SDA; SCL = 0; Delay5us(); return dat;void

28、 Single_WriteI2C(uchar REG_Address,uchar REG_data) I2C_Start(); I2C_SendByte(SlaveAddress); I2C_SendByte(REG_Address); I2C_SendByte(REG_data); I2C_Stop();uchar Single_ReadI2C(uchar REG_Address)uchar REG_data;I2C_Start();I2C_SendByte(SlaveAddress);I2C_SendByte(REG_Address);I2C_Start();I2C_SendByte(SlaveAddress+1);REG_data=I2C_RecvByte();I2C_