文稿說(shuō)明分析

20165人，兩輪自平衡機(jī)器積小，運(yùn)動(dòng)靈活，能夠在比較狹窄、需要大轉(zhuǎn)角的場(chǎng)合中運(yùn)本設(shè)計(jì)以兩輪車模為研究平臺(tái)，以公司32位微處理器MK60FX512VLQ15為控制。通過(guò)加速度傳感器和陀螺儀，設(shè)計(jì)濾波算法，實(shí)現(xiàn)了傾角的測(cè)量；通過(guò)設(shè)計(jì)MOS電機(jī)驅(qū)動(dòng)電路，使用單片機(jī)輸出，實(shí)現(xiàn)了電機(jī)速度控制；通過(guò)編寫控制算法，實(shí)現(xiàn)了兩輪車模的自平衡功能；通過(guò)編測(cè)量電機(jī)轉(zhuǎn)速，對(duì)速度進(jìn)行閉環(huán)控制，實(shí)現(xiàn)了兩輪自平衡機(jī)器人的速度控制功能。最終通過(guò)無(wú)線，使小車能夠在自平衡單片機(jī)，PID，傾角測(cè)量，MOSSelf-balancingrobotbasedonPIDAlongwiththefurtherroboticsresearch,androbotswaswidelyusedintheindustrial Things.Inapracticalapplication,the paredwiththeWheeledrobotwhichhasseveralwheels,self-balancingtwo-wheelrobothastheadvantagesofflexiblemovementbecauseofit’ssmallsize,it’ssuitableinthemorenarrowuseoftheenvironment.Thiskindofrobothastwowheelsofcoaxial,tokeepbalancebywayofmovement.Therobotcanachievethebasicmovementfunction,itcanmoveforwardandbackward,turnleftandright,aswellasstillstanding.Duetoit’sspecialstructure,itcanadapttodifferentterrainenvironment.Forthereasongivenabove,researchesontheTwo-WheeledSelf-BalancingRobotwouldbesignificant.asthecontrollingkernel.Themainworks plishedinthepaperinclude:(1)Designingafilteralgorithm,achievingAnglemeasurementbasedonInertialMeasurementUnit-controlthroughtechnology.(3)Designingcontrolalgorithm,achievingself-balancing.(4)Measuringmotorspeed,achievingspeedcontrolundertheconditionofself-balancingthroughClosed-LoopControl.Finally,realizingfreemovementbyusingaremote:Microcontroller,PID,Measurementofangle，Drivingcircuitof緒 研究目的和意 國(guó)內(nèi)外發(fā)展現(xiàn) 設(shè)計(jì)內(nèi) 總體設(shè)計(jì)方 設(shè)計(jì)思 硬件設(shè)計(jì)方 機(jī)械結(jié)構(gòu)設(shè)計(jì)方 硬件電路設(shè) 單片機(jī)最小系統(tǒng)電 加速度傳感器模 陀螺儀模 電機(jī)驅(qū)動(dòng)模 電源管理模 軟件程序設(shè) 系統(tǒng)流程圖 程序初始化 PID控制算法 PID控制算法介 PID控制算法參數(shù)整 傾角估計(jì)算法 互補(bǔ)濾波 卡爾曼濾波 直立控制程序設(shè) 速度控制程序設(shè) 方向控制程序設(shè) 開發(fā)工具及調(diào)試結(jié) 開發(fā)工具介 MDK介 SerialDigitalScope介 調(diào)試結(jié) PID算法控制電機(jī)調(diào)試結(jié) 互補(bǔ)濾波法調(diào)試結(jié) 卡爾曼濾波法調(diào)試結(jié) 總結(jié)與展 附錄A主控板原理圖與PCB 附錄B電機(jī)驅(qū)動(dòng)原理圖與PCB 附錄C主要源程 參考文 致 研究目的和意兩輪自平衡機(jī)器積小，運(yùn)動(dòng)靈活，能夠在比較狹窄、需要大轉(zhuǎn)角的場(chǎng)合中運(yùn)動(dòng)[1]。國(guó)內(nèi)外發(fā)展現(xiàn)兩輪自平衡機(jī)器人的構(gòu)想來(lái)源于電氣通信的喀左教授在1986年一種自2002年，工業(yè)大學(xué)的FelixGrasser等人設(shè)計(jì)出可的兩輪自平衡機(jī)器人，運(yùn)動(dòng)速度可達(dá)到1.5m/s。兩輪共軸，每個(gè)有獨(dú)立的驅(qū)動(dòng)，能實(shí)現(xiàn)速度控制、2002年，的賽格威公司設(shè)計(jì)了第一輛載人電動(dòng)平衡車Segway，設(shè)計(jì)時(shí)速可20km/h，并且成功推向市場(chǎng)，但是價(jià)格昂貴，高達(dá)5000[3]2004設(shè)計(jì)了一款兩輪自平衡電動(dòng)代步車，可實(shí)現(xiàn)領(lǐng)半徑轉(zhuǎn)彎。使用簡(jiǎn)單，用戶經(jīng)過(guò)10分鐘簡(jiǎn)單的練習(xí)就可以駕駛，正常工作時(shí)，使用者通過(guò)身體前傾和后仰來(lái)控制加速和，通過(guò)手柄，可實(shí)現(xiàn)轉(zhuǎn)彎，時(shí)速可達(dá)10km/h,30km。設(shè)計(jì)內(nèi)使用ARMCortex-M4內(nèi)核的FreescaleK60單片機(jī)為主控制器，對(duì)加速度計(jì)和陀螺儀的數(shù)據(jù)進(jìn)行融合，得到車身傾角最優(yōu)估計(jì)，設(shè)計(jì)MOS電機(jī)驅(qū)動(dòng)電路，編寫PID控制算法控制車輪，達(dá)到自主直立的目的。利用編構(gòu)成速度反饋回路，使用PID控制算法設(shè)計(jì)思2.1硬件設(shè)計(jì)方鍵電路蜂鳴器電路其中電源管理電路分為3.3V電源管理電路和5V電源管理電路，5V管理電路使用LM2940三端線性穩(wěn)壓器，輸入7.2V電池電壓，輸出5V電壓。3.3V管理電路使用LM1117三端線性穩(wěn)壓器，輸入接LM2940的5V電壓，輸出3.3V電壓?？紤]到本系統(tǒng)中器件、傳感器較多，因此5V管理電路和3.3V管理電路均使用兩個(gè)。微控制器接口主要包括：OLED接口、藍(lán)牙接口、MMA7361傳感器接口、L3G4200D傳感器接口、編正交接口2個(gè)、四通道 接口、接口，以及預(yù)留IO，方便調(diào)試圖2.2機(jī)械結(jié)構(gòu)設(shè)計(jì)方的角度安裝在輪軸，使用熱熔膠加以固定。圖2.3單片機(jī)最小系統(tǒng)電K60(ARMCortex-M4耗、混合信號(hào)微控制器，Cortex-M4內(nèi)核帶DSP指令，性能可達(dá)1.25DMIPS/MHz(部分本系統(tǒng)使用MK60FX512VLQ15單片機(jī)為控制，該單片機(jī)主頻高達(dá)150MHz，內(nèi)部512KBFlash、144KBSRAM，可以輕松應(yīng)對(duì)大多數(shù)的嵌入式應(yīng)用場(chǎng)合。片內(nèi)集成了4個(gè)可配置分辨率的高速16位模數(shù)轉(zhuǎn)換器（ADC）、兩個(gè)12位數(shù)模轉(zhuǎn)換器（ADC）、4個(gè)可輸出的高速比較器、6個(gè)支持IrDA的UART、3個(gè)硬件IIC總線接口和3個(gè)硬SPI總線接口、32通道DMA控制器以及大量的多功能定時(shí)器，豐富的片內(nèi)外設(shè)資源，不式，運(yùn)行功耗低至350uA/MHz,靜態(tài)功耗低至5.6uA，在高性能應(yīng)用場(chǎng)合也可有效降低系[8]LQFP144圖3.1主 加速度傳感器模MMA7361是公司（原飛思卡爾）的微型電容式加速度傳感器，其功耗低，體2.2-3.6V400uA，3uAMMA7361的引腳圖如下圖3-1所示，引腳排列如下表3.1所示引12引12X3Y4Z5地6789g-3.2MMA7361陀螺儀模L3G4200D16具有三種可選分辨率（250/500/2000dps），集成高通、低通濾波器，用戶可帶2.4-3.6V，通過(guò)IIC/SPI接口輸出數(shù)據(jù)。3.3L3G4200D電機(jī)驅(qū)動(dòng)模采用集成電機(jī)驅(qū)動(dòng)使用集成電機(jī)驅(qū)動(dòng)設(shè)計(jì)（常用的電機(jī)驅(qū)動(dòng)有BTS7970、L298N等），電路簡(jiǎn)單、可靠性高，但是驅(qū)動(dòng)的性能有限。由于本系統(tǒng)車模的電機(jī)的內(nèi)阻小于10毫歐，而集成驅(qū)動(dòng)內(nèi)部的每個(gè)場(chǎng)效應(yīng)管導(dǎo)通電阻在120毫歐以上，這樣，電機(jī)驅(qū)動(dòng)回路總電阻就太大了，驅(qū)動(dòng)電路的輸出功率和發(fā)熱量都很大，電機(jī)性能得不到充分發(fā)揮。采用N溝道場(chǎng)效應(yīng)管和柵極驅(qū)動(dòng)設(shè)計(jì)，分立的N溝道場(chǎng)效應(yīng)管的導(dǎo)通電阻很低，一般小于10毫歐，大大減小了電機(jī)驅(qū)動(dòng)回路總電阻。另外的柵極驅(qū)動(dòng)可以提高場(chǎng)效應(yīng)管的開關(guān)速度，使控制方式的調(diào)制頻率提高，從而減少電樞電流脈動(dòng)[9]。，經(jīng)過(guò)多次試驗(yàn)與調(diào)試，我們最終選用大電路電機(jī)半橋驅(qū)動(dòng)IR2184，它可以驅(qū)動(dòng)高端和兩個(gè)N溝道場(chǎng)效應(yīng)管能提供較大的柵極驅(qū)動(dòng)電流并具有硬件死區(qū)、硬件防同臂導(dǎo)通等功能使用兩片IR2184型半橋驅(qū)動(dòng)可以組成完整的直流電機(jī)H橋式驅(qū)動(dòng)電路。其性能優(yōu)異、價(jià)格便宜，所以我們選擇它進(jìn)行設(shè)計(jì)10]。電源管理模（7805LM2940LM1117和開關(guān)（LM2596LM2941較低，因此應(yīng)該選擇開關(guān)型穩(wěn)壓電源[1]。LM2940:LM2940-5.05V，1A，在輸出電流為1A時(shí)，最小輸入輸出電壓差為0.8V，最大輸入電壓為26V。工作溫度-LM1117：LM1117是一個(gè)低壓差線性三端穩(wěn)壓5個(gè)固定電壓輸出的型號(hào)：1.8V、2.5V、2.85V、3.3V和5V。當(dāng)其在輸出電壓3.3V，輸出電流為800mA時(shí)輸入輸出電壓差1.2V。LM1117±1%，輸出端需要一個(gè)至少10uF的鉭電解電容來(lái)改善瞬態(tài)響應(yīng)和穩(wěn)定具有TO-263TO-220SOT-223和TO-2523.3VLM1117OLED12]。系統(tǒng)流程圖4.1程序初始化LEDLED轉(zhuǎn)換器進(jìn)行，MK60單片機(jī)內(nèi)部集成4個(gè)16位ADC轉(zhuǎn)換器，通道多達(dá)數(shù)十個(gè)，分辨 ADC_QuickInit(ADC_ACCEL_Z,kADC_SingleDiff10or11);//單端10OLED_P8x16Str(0,2,"oWorld!");初始化：本系統(tǒng)通過(guò)控制電機(jī)轉(zhuǎn)動(dòng)，頻率10Khz，初始化程序如下:UARTDMA初始化：UART即通用異步收發(fā)器（UniversalAsynchronous便進(jìn)行調(diào)試和觀測(cè)數(shù)據(jù)。DMA指直接器技術(shù)（DirectMemoryAccess），GPIO_CallbackInstall(KEY_GPIO,GPIOA_ISR);//按鍵中斷回調(diào)函數(shù)Cortex-能，即estedVectoredInterruptController的縮寫。通過(guò)NVIC控制器，可NVIC_EncodePriority(NVIC_PriorityGroup_2,0,0));PID控制算法PID圖4.2PIDPID控制器的微分方程U(t)Kp[e(t)1te(t)dtTde(t)r(tc(t

Ti

PID

D(s)=U(s)

1Ti*

Td*S]

數(shù)字式PIDU(n)Up(n)Ui(n)Ud(n)

Up(n)

nUi(n)KpTn

Ud(n)Kp*Td[e(n)e(nT

Ti

P PI PD PID

U(n)Up(n)U(n)Up(n)Ui(n)U(n)Up(n)Ud(n)U(n)Up(n)Ui()Ud(n)PIDPIDU(n)Kp{e(n) ne(i)Td[e(n)e(nTT

PIDU(n)=U(n)-U(n-=Kp[e(n)e(n1)]KpTe(n)KpTd[e(n)2e(n1)e(n= PIDPID的參數(shù)整定是PID控制算法設(shè)計(jì)的內(nèi)容，根據(jù)系統(tǒng)的特性確定PID控制器的比例系數(shù)Kp、積分時(shí)間Ti和微分時(shí)間Td。PID的參數(shù)整定方法有很多，概括起來(lái)有PID參數(shù)的工程整定法，主要有臨界比例法、反應(yīng)曲和衰減法。三種方法各有PIDPIDPIDKpPIDTi=0、Td=0，PID60%~70%，0Kp，Kp輸出振蕩，記下此時(shí)的比例系數(shù)Kp，設(shè)定比例系數(shù)Kp為當(dāng)前記錄值得60%~70%。KpTi，Ti，直到系統(tǒng)出現(xiàn)振蕩，然后再逐漸加大Ti，直到系統(tǒng)振蕩。記下此時(shí)的積分時(shí)間常數(shù)Ti，PID控制器的Ti150%~180%。TdKpTi30%PID4.3①arctan(X/Z②arcsin(X/③arccos(Z/INS[23]。K1dt，直到滿意為止。其中，K1K1[24]float // voidYijie_Lvbo(floataccel_pro,floatgyro_pro){angle=K*accel_pro+(1-K)*(angle+gyro_pro*}卡爾曼濾波法源于匈牙利數(shù)學(xué)家RudolfEmilKalman的《ANewApproachtoLinearFilteringandPredictionProblems》（線性濾波與預(yù)測(cè)問(wèn)題的新方法）?？?jī)A角的情況下，設(shè)定目標(biāo)平衡角度，使用PID算法進(jìn)行閉環(huán)控制。因?yàn)檐圀w只會(huì)在一轉(zhuǎn)動(dòng)，抵消車體在該維度上的傾斜，就可以完成平衡控制[27]2.2.14.4L，mx(t)。如4.5a(t)以及外x(t)之間的運(yùn)動(dòng)方程。4.5

g g a(t)cos[

在角度很小時(shí)，運(yùn)動(dòng)方程簡(jiǎn)化

g(t)a(t)

dt

2.2.34.64.7void{angle_Accel=(A_Z-AZ_ZERO);angle_Accel=asin(angle_Accel);Gyro_Now=(T_Y-TY_OFFSET)* elseif(FilterSwitch==1) elseif(FilterSwitch==2) elseif(FilterSwitch==3) {}{}

elseif(AngleControlOut_D<-500)AngleControlOut_D=-500;if(AngleControlOut>800)AngleControlOut=800;}改變車模直立控制的目標(biāo)角度，直立控制PD算法中，有一個(gè)目標(biāo)傾角，當(dāng)目標(biāo)傾角不是真正的平衡位置傾角時(shí)，車模會(huì)一直運(yùn)動(dòng)。通過(guò)編測(cè)量轉(zhuǎn)速，形成PID很難維持平衡，在直立控制的作用下，車模就會(huì)運(yùn)行。通過(guò)編測(cè)量轉(zhuǎn)速，形成PID4.8void{if(SpeedControlOut>300)SpeedControlOut=300;//速度環(huán)輸出限幅elseif(SpeedControlOut<-300)SpeedControlOut=-300;}PD具備自主識(shí)別路徑的功能，因此PD算法的輸入?yún)?shù)使用完成。為了改善轉(zhuǎn)向的PDe(t)-e(t-1)，X4.9voidDirection_Calculate(int16_t{if(DIR.error_sum>2000)DIR.error_sum=2000;elseif(DIR.error_sum<-2000)DIR.error_sum=-2000;DirectionControlOut=DIR.P*DIR.error++DIR.D*(T_X-if(DirectionControlOut>300)DirectionControlOut=300;elseif(DirectionControlOut<-300)DirectionControlOut=-}開發(fā)工具介MDKMDKKeilARMCortex-MCortex-R4ARM7ARM9、調(diào)試，支持C/C++語(yǔ)言和匯編語(yǔ)言，并且可以聯(lián)合編寫，編譯效率高，具有多種優(yōu)化等級(jí)，用于開發(fā)大多數(shù)嵌入式軟件非常方便。MDKARMC/C++Cortex-M0/M3/M4、Cortex-R4、ARM7ARM9KeilRTXTCP/IPUSBUSBGUIMDKCMSIS(CortexSerialDigitalScopeSerialDigitalScope數(shù)據(jù)分析功能，支持最大波特率115200。協(xié)議簡(jiǎn)單可靠，具備CRC校驗(yàn)功能，可以很方調(diào)試結(jié)PID為了學(xué)習(xí)和驗(yàn)證PID算法，設(shè)計(jì)了位置式PID算法在電機(jī)空載情況下進(jìn)試，PID圖5.1PID算法方波效圖5.2PID算法鋸齒波效圖5.3PID算法正弦波效互補(bǔ)濾波主要調(diào)節(jié)的是對(duì)于加速度計(jì)的K,K越大，則速度越快，但濾波效果越差。根據(jù)需要，選擇適合系統(tǒng)的K系數(shù)。下面兩幅圖分別為K=0.01,和K=0.05時(shí)當(dāng)K=0.01時(shí)，濾波效果更明顯，平滑度很高。當(dāng)K=0.05時(shí) 效果好，平滑度差5.4（K=0.015.55.6人，兩輪自平衡機(jī)器積小，運(yùn)動(dòng)靈活，能夠在比較狹窄、需要大轉(zhuǎn)角的場(chǎng)合中運(yùn)本設(shè)計(jì)以32位微控制器MK60FX512VLQ15為，自行設(shè)計(jì)了主控電路板和電機(jī)驅(qū)動(dòng)電路板，以兩輪車模為研究平臺(tái)，編寫CPIDOLED藍(lán)牙可以將系統(tǒng)同外部設(shè)備如電腦等連接起來(lái)可進(jìn)行無(wú)線控制和調(diào)試通過(guò)可以控制自平衡機(jī)器人運(yùn)動(dòng)如果加入環(huán)境識(shí)別系統(tǒng)如頭CCD紅外對(duì)難達(dá)到較高水平。2）PIDPIDPIDPIDA主控板原理圖與PCBB電機(jī)驅(qū)動(dòng)原理圖與PCBCint{uint8_tdirswitchtemp,spswitchtemp;{ {spswitchtemp=SpeedControlSwitch;}}}void{staticuint16_tFindZeroIndex=0;{}{systime_speed=0;Sp_Smooth=0.05;//}{{{}}}}void{/*******DMP用到了printf*********///**********LED初始化，用作系統(tǒng)運(yùn)行指示***********///********OLED初始化**************///**************模擬加速度計(jì)陀螺儀初始化 ////**************IIC及L3G4200D\HMC5883初始化******************//{} /****DMP數(shù)據(jù)輸出中斷*/ //DMP輸出輸出中斷/****************初始化*****************//**************FTM正交初始化/***********初始化位脈沖-方向型編入//**********************按鍵中端配置//*****************PIT定時(shí)中斷初始化*****************//*******************NVIC配置****************/ //中斷優(yōu)先級(jí)分成2組NVIC_SetPriority(PIT0_IRQn,NVIC_EncodePriority(NVIC_PriorityGroup_2,1,NVIC_SetPriority(PORTE_IRQn,NVIC_EncodePriority(NVIC_PriorityGroup_2,2,OLED_P8x16Str(0,2,"o}[1],,,等.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與未來(lái)[J].機(jī)器人,2002,黃永志,.兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J].機(jī)器人,1:40-梁棟,尹曉紅,王夢(mèng)晴.移動(dòng)機(jī)器人研究現(xiàn)狀及發(fā)展趨勢(shì)[J].科技信息2014(9):33-.兩輪自平衡機(jī)器人控制系統(tǒng)研究與設(shè)計(jì)[D].哈爾濱工業(yè)大學(xué),.兩輪自平衡小車的控制技術(shù)研究[D].西安電子科技大學(xué),PathakK,FranchJ,AgrawalSK.Velocityandpositioncontrolofawheeledinvertedpendulumbypartialfeedbacklinearization[J].IEEETransactionsonRobotics,2005,賴義漢,王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計(jì)[J].學(xué)報(bào)：自然科學(xué)版,2014,01期(1):53-57.K60Sub-FamilyReferenceManual. .,杜楊,,等.基于場(chǎng)效應(yīng)管的直流電機(jī)驅(qū)動(dòng)控制電路設(shè)計(jì)[J].電子設(shè)計(jì)工程,2008(2):3-6.卓晴.學(xué)做智能車[M].航空航天大學(xué),.嵌入式硬件系統(tǒng)設(shè)計(jì)與開發(fā)實(shí)例詳解[M].電子工業(yè),徐亮,阮江軍,甘艷,等.去耦電容在PCB板設(shè)計(jì)中的應(yīng)用[J].電測(cè)與儀表2002,楊東軒.ARMCortex-M4筆記[M].航空航天大學(xué),ARMCortex-M4DevicesGenericUserGuide.劉金琨.先進(jìn)PID控 仿真(第3版)[M].電子工業(yè),歷風(fēng)滿.數(shù)字PID控制算法的研究[J].遼寧大學(xué)學(xué)報(bào):自然科學(xué)版,2005,GudeJJ,Kahoraho ysisandPerformanceComparisonofPIDandFractionalControllers[J].Dynamics&ControlofProcessSystems,YoshikazuNishikawaNobuoSannomiyaTokujiOhta,等Amethodforauto-tuningofPIDcontrolparameters[J]