【正文】 Pulse。 // void GetMotorPulse(void) //采集脈沖信號 5ms { nLeftPulse=PACNT。 } 電機(jī) 控制程序： //電機(jī)計(jì)算 MotorControl float g_fLeftMotorOut=0, g_fRightMotorOut=0。 } void SpeedControlOutput(void) //5ms { float fValue。 if(g_fSpeedControlIntegral SPEED_CONTROL_OUT_MAX) //判斷積分是否達(dá)到設(shè)定最大最小值 g_fSpeedControlIntegral = SPEED_CONTROL_OUT_MAX。 fP = error * sudup。 //fP = fDelta * SPEED_CONTROL_P。 d_error=errorpre_error。 return。 //電機(jī)速度平均值 g_nLeftMotorPulseSigma = g_nRightMotorPulseSigma = 0。 float fI。 float g_fSpeedControlOutOld=0, g_fSpeedControlOutNew=0,g_fSpeedControlIntegral=0。 if(fValue ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX。 if(g_nAngleControlFlag == 0) { g_fAngleControlOut = 0。 fDeltaValue = (g_fGravityAngle g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT。 void AngleCalculate(void) //周期為 5ms { double fDeltaValue。 } if(cout==3500){ CAR_SPEED_Set1=CAR_SPEED_Set。(cout3000)) { CAR_SPEED_Set1=CAR_SPEED_Set*3/5。amp。 } else if((cout1500)amp。 PITTF_PTF0=1。 } else if (jieshou==17|jieshou==69|jieshou==34|jieshou==68) { nMidPosition=。 } else if(jieshou==51) { CAR_SPEED_Set1+=。 } PITTF_PTF2=1。 //根據(jù)曝光時(shí)間計(jì)算 20ms 周期內(nèi)的曝光點(diǎn) integration_piont = 20 IntegrationTime。 if(Input_Num=500) 成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 20 { Input_Num=0。 //等待站立 CheckCarStand()。 //VOLTAGE_LEFT ,VOLTAGE_RIGHT g_fCarAngle OutData[3] = g_fSpeedControlIntegral。 OutData[0] =g_fGravityAngle 。 //加速度 LCD_Print(52,6,中 )。 Dis_Num(70,4, DIRECTION_CONTROL_P_DEFAULT,5)。 // Dis_Num(96,2, blake,5)。 // LCD_Print(0,2,速 I)。 Dis_Num(22,0, ANGLE_CONTROL_P_DEFAULT ,5)。 uart_putstr(Usart Is Working!)。 DDRE=0XFF。 unsigned char *pixel_pt。 } void main(void) { int zhongxian。ims。 ReadEeprom(debug,0x0800,0x0c)。 //結(jié)構(gòu)體定義變量 CarControlStop()。 //脈沖捕捉 PT0 UART_Init()。 //配置 64M 時(shí)鐘 ADC_Init()。 //通道 0 輸入 捕捉中斷允許 TSCR1=0x80。 } /********************脈沖采集 PT0 口 **********************************/ void ECT0_Init(void) { TSCR2=0x00。 while(!(CRGFLG_LOCK==1))。 POSTDIV=0x00。 } //**************************** 設(shè)置總路 線頻率*********************************/ void SetBusCLK_64M(void) { CLKSEL=0X00。 = GYROSCOPE_ANGLE_RATIO_DEFAULT。 = DIRECTION_CONTROL_P_DEFAULT。 = SPEED_CONTROL_P_DEFAULT。 = GRAVITY_MAX_DEFAULT。跌倒判斷可以通過小車傾角是否超過了一定范圍進(jìn)行確定，或者通過安裝在小車前后防撞支架上的微動開關(guān)來判定。這兩類任務(wù)可以通過全局變量實(shí)現(xiàn)相互的通訊。為了和電機(jī)驅(qū)動不產(chǎn)生干擾，電源模塊位于主控板的最下面。電源芯片 LM2940 具有波紋小、電路結(jié)構(gòu)簡單的優(yōu)點(diǎn)，但是效率較低、功耗大。由于小車使用雙電機(jī)，所以我們要使用 4 片 BTS7960 芯片構(gòu)成兩個(gè)全橋分別控制兩個(gè)電機(jī)。 MMA7361LC 三軸加速度傾角傳感器接口電路如圖 34 所示。 圖 32 XS128單片機(jī)最小系統(tǒng) 該單片機(jī)是飛思卡爾公司的 16 位 HCS12 系列單片機(jī)，是 HCS12 系列的增強(qiáng)型產(chǎn)品，基于 S12 的 CPU 內(nèi)核，可達(dá)到 25MHz 的 HCS12 的 2— 5 倍性能， S12X 系列增加了 172 條額外指令，可以執(zhí)行 32 位計(jì)算，總線頻率最高可以達(dá)到 40MHz，改進(jìn)了中斷處理能力， S12X 系列的 CPU 采用復(fù)雜指令 CISC 架構(gòu)，集成了中斷控制器，有豐富的尋址 方式，中斷有 7 個(gè)優(yōu)先級并且內(nèi)核支持優(yōu)先級的調(diào)度，最多可有 117 個(gè)中斷源， S12X可訪問最多 8M 的全部存儲空間。 圖 31 兩輪自平衡小車的整體電路框圖 電路劃分為以下幾個(gè)模塊： （ 1）單片機(jī)最小系統(tǒng)：包括 DSC 處理器，程序下載調(diào)試接口等； （ 2）藍(lán)牙模塊：接收手機(jī)藍(lán)牙控制信號；在這里用藍(lán)牙模塊充當(dāng)無線串口用，由手機(jī)藍(lán)牙作為藍(lán)牙母機(jī)，通過手機(jī)上位機(jī)軟件發(fā)送一系列的 ASCII 數(shù)據(jù)給藍(lán)牙模塊，藍(lán)牙模 塊再以串口發(fā)送給單片機(jī)，單片機(jī)接收到數(shù)據(jù)后通過一定的數(shù)據(jù)處理，然后得到方向的信息，控制小車加減速或者轉(zhuǎn)彎等一些列動作。利用控制單片機(jī)的計(jì)數(shù)器測量在固定時(shí)間間隔內(nèi)速度脈沖信號的個(gè)數(shù)來反映電機(jī)的轉(zhuǎn)速。在重力 的作用下，小車就會沿著傾斜的方向加速前進(jìn)，控制小車的速度就只需通過控制小車的傾角來實(shí)現(xiàn)了。 在小車上安裝陀螺儀，可測出小車的傾斜角速度，將角速度信號進(jìn)行積分可得到小車的傾角。 圖 23 MMA7361LC 三軸加速度傳感器 加速度傳感器安裝在小車上，為了減少移動帶來的干擾，加速度傳感器安裝的高度越低越好，但是仍然不能夠徹底消除小車運(yùn)動的影響。只要保證 k1g、 k20 在條件下，可以使得車模像單擺一樣維持在直立狀態(tài)。因此需要另外增加控制阻尼力。這樣倒立擺所受到的回復(fù)力為： 式中，由于 θ 很小，所以進(jìn)行了線性化。如圖 21 所示。這要兩條件：一是托木棒的手可以移動：二是眼睛可觀察木棒的傾斜度和其傾斜趨勢。三個(gè)分解后的任務(wù)各自獨(dú)立地進(jìn)行控制，但是最終都是對同一個(gè)控制對象（小車的電機(jī)）進(jìn)行控制，因此它們之間存在著耦合。我們將小車電機(jī)虛擬地分解成兩個(gè)不同功能的驅(qū)動電機(jī)，這兩個(gè)電機(jī)同軸相連，但是它們卻分別控制著小車的直立平衡和左右方向。小車行走控制任務(wù)分解成下面三個(gè)部分的控制任務(wù) : (1) 控制小車的平衡：要保持小車的直立平衡狀態(tài)只需要通過控制兩個(gè)電機(jī)的正反向運(yùn)動。 中國科技大學(xué)研制出了兩輪自平衡代步電動車 Free Mover，它是一種左右兩輪并行結(jié)構(gòu)的具有自平衡能力的電動車，在車體內(nèi)嵌入 CPU，采集平衡狀態(tài)速度和加速度傳感器的數(shù)據(jù)，根據(jù)系統(tǒng)數(shù)學(xué)模型添加控制算法，計(jì)算輸出脈寬調(diào)制信號（ PulseWidth Modulation， PWM），來控制兩個(gè)伺服電機(jī)的轉(zhuǎn)矩，使車體保持平衡，并能夠根據(jù)人體重心的偏移，自動前進(jìn)、后退及轉(zhuǎn)彎等 [9]。當(dāng)這條軸往前傾斜時(shí)， Segway 身內(nèi)的內(nèi)置電機(jī)會產(chǎn)往前成都學(xué)院學(xué)士學(xué)位論文（設(shè)計(jì)） 4 的力量，一方面平衡人與車往前傾倒的扭矩，一方面產(chǎn)生讓車輛前進(jìn)的加速度，相反的，當(dāng)陀螺儀發(fā)現(xiàn)駕駛者的重心往后傾時(shí)，也會產(chǎn)生向后的力量達(dá)到平衡效果。其運(yùn)動特點(diǎn)類似于蕩秋千時(shí)秋千的運(yùn)動，總是自然的向鉛直的、平衡位置回歸。 Segway 的誕生源起 于美國大型醫(yī)療器材生產(chǎn)商強(qiáng)生公司 (Johnsonamp。 圖 Legway 機(jī)器人 美國 SegwayLLC 公司開發(fā)的 Segway 兩輪平臺 電動車，它不僅能在駕駛者平穩(wěn)站立時(shí)，保持動態(tài)平衡前進(jìn)，而且在駕駛者身體傾斜時(shí)也能保持平衡，可以在各種天氣環(huán)境下使用，其外觀如圖 中左圖所示 [6]。操作者還可以通過無線裝置遠(yuǎn)程遙控 Joe 的運(yùn)動速度和運(yùn)動方向，能在惡劣的條件下運(yùn)行并且保持平衡 [4]。通過對兩輪自平衡系統(tǒng)的改造，可快速方便的應(yīng)用到眾多環(huán)境中去，如承載、運(yùn)輸、代步等 。正因?yàn)橛辛诉@些優(yōu)點(diǎn)，兩輪自平衡機(jī)器人經(jīng)過了一定的改造就可以制作成為戰(zhàn)場機(jī)器人或“保姆”機(jī)器人，具有廣闊的應(yīng)用前景。 兩輪自平衡機(jī)器人是一個(gè)集動態(tài)決策和規(guī)劃、環(huán)境感知、行為控制盒執(zhí)行等多種功能于一體的綜合復(fù)雜系統(tǒng)，其關(guān)鍵是解決在完成自平衡的同時(shí)，還能夠適應(yīng)各種環(huán)境下的控制任務(wù)。兩輪自平衡小車式一個(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)注 [2]。在小車制作完成后，各個(gè)模塊之間能夠正常并且協(xié)調(diào)的工作，小車可以只無人干預(yù)的條件下實(shí)現(xiàn)自主平衡，運(yùn)用手機(jī)藍(lán)牙可以控制小車的前進(jìn)、后退、左右轉(zhuǎn)動等各個(gè)動作。 本 科 畢 業(yè) 設(shè) 計(jì) 題 目 兩輪自平衡小車研究 學(xué) 院 電子信息工程學(xué)院 專 業(yè) 自動化 學(xué)生姓名 劉長根 學(xué) 號 202020311332 年級 2020級 指導(dǎo)教師 羅浚溢 職稱 博士 年 月 日 兩輪自平衡小車研究 專 業(yè)：自動化 學(xué) 號： 202020311332 學(xué) 生：劉長根 指導(dǎo)教師：羅浚溢 摘要： 現(xiàn)在兩輪自平衡小車的研究在全世界得到很大的關(guān)注。系統(tǒng)采用飛思卡爾公司的 DSC 16位處理器XS128單片機(jī)為核心控制處理器，完成傳感器信號的處理，濾波算法的實(shí)現(xiàn)和車身控制等一些任務(wù)。雙輪自平衡機(jī)器人概念就是在這樣的背 景下提出來的。 選題的目的和意義 兩輪自平衡機(jī)器人兩輪共軸、獨(dú)立驅(qū)動、車身中心位于車輪軸上方，通過運(yùn)動保持平衡，可以直立運(yùn)動，因?yàn)樘貏e的結(jié)構(gòu)，它對于地形的變化有著很強(qiáng)的適應(yīng)能力，有著靈活的運(yùn)動性能，能夠在比較復(fù)雜的環(huán)境里面工作，和傳統(tǒng)的輪式移動機(jī)器人相比較，兩輪自平衡機(jī)器人有著以下的幾個(gè)優(yōu)點(diǎn)：（ 1）能夠?qū)崿F(xiàn)在原地回轉(zhuǎn)和任意半徑的轉(zhuǎn)向，有著更加靈活易變的移動軌跡，很好地彌補(bǔ)了傳統(tǒng)多輪布局的缺點(diǎn)；（ 2）具有占地面積小的優(yōu)點(diǎn)，能夠在場地面積小或者要求靈活運(yùn)輸?shù)膱龊仙线\(yùn)用；（