【正文】 以 ARM CortexM4F內(nèi)核設(shè)計(jì)， 具有 IEEE 1558以太網(wǎng)，全速和高速 USB OnTheGo 帶設(shè)備充電探測(cè)、硬件加密以及防篡改探測(cè)能力，具有豐富的模擬、通信、定時(shí)和控制外設(shè)、單精度浮點(diǎn)單元、 NAND閃存控制器和DRAM控制器，如 High speed analogtodigital converter、 FlexTimers、 Low power timer、 Periodic interrupt timer、 CAN、 Serial peripheral interface、 Interintegrated circuit (I2C)、 UART 等 設(shè)備 ，從 144LQFP封裝 256KB 閃存開始可擴(kuò)展到 256MAPBGA 1MB 內(nèi)存 。 軟件 設(shè)計(jì)分為 PWM輸出 模塊 、 陀螺儀 加速度計(jì)采集模塊、 周期 中斷定時(shí)器模塊、互補(bǔ)濾波算法 及 平衡控制模塊、 速度采集 及 速度控制模塊、 CMOS攝像頭采集 處理 算法及 轉(zhuǎn)向 模塊。 PWM 控制電機(jī)原理：利用 MCU 的 PWM 端口 ，固定 PWM 的周期，通過改變PWM的占空比，從而控制輸出的電壓 。 FTM0MODE amp。 FTM0SC amp。 FTM0CONTROLS[1].CnSC = 0。 FTM0COMBINE amp。 FTM0CONTROLS[3].CnSC = 0。 FTM0COMBINE amp。 FTM0CONTROLS[2].CnV = (((FTM0MOD)*(PWMDuty))/1000)。是微電子通信控制領(lǐng)域廣泛采用的一種總線標(biāo)準(zhǔn)。 通過 MK60FX512VLQ15 MCU 內(nèi)部的 I2C 模塊可以實(shí)現(xiàn) 對(duì) MPU6050的 采集。 I2C_F_REG(I2Cx[i2]) = I2C_F_MULT(mult) | I2C_F_ICR(scl_dev) 。 i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_CONFIG, 0x06)。 GYRO_X = ((int16)i2c_readaddr(I2C_Moudle, ADDRESS, GYRO_XOUT) 8) |(int16)i2c_readaddr(I2C_Moudle, ADDRESS, GYRO_XOUT + 1)。 PITMCR amp。 PITCHANNEL[1].TFLG |= PIT_TFLG_TIF_MASK。 NVIC_Init(PIT1_IRQn, NVIC_PriorityGroup_3, 1, 0)。 case 1:{ }break。 } } 互補(bǔ)濾波算法 及 平衡控制模塊 ： 平衡 理論分析： 對(duì)于基于 視覺兩輪 平衡車， 可以分析為一個(gè)倒立擺系統(tǒng)， 如果 車體 有 向前倒的趨勢(shì)，那么 車體 需要向前加速運(yùn)動(dòng)保持平衡，如果車體有向后倒的趨勢(shì)，那么車體需要向后加速運(yùn)動(dòng)保持平衡。 F = ?mgsinθ ≈ ?mgθ 在 倒立擺系統(tǒng)中： 偏離 一定的位置之后回復(fù)力與位移方向相 同 ，導(dǎo)致繼續(xù)偏離平衡位置，最終倒下。 在 這個(gè) 系統(tǒng) 中，我們需要兩個(gè)條件，第一個(gè)是 我們 可以控制車體前后加速運(yùn)動(dòng)，這個(gè)我們可以控制電機(jī)運(yùn)動(dòng)達(dá)到這個(gè)目的，第二個(gè)條件是我們可以監(jiān)測(cè)車體此時(shí)此刻的姿態(tài) （ 包括角度和角速度） ， 這個(gè)我們可以通過角速度傳感器陀螺儀和 加速度 傳感器加速度計(jì)來 采集 此時(shí)此刻的車體姿態(tài) 。就是，短時(shí)間內(nèi)用陀螺儀比較準(zhǔn) 確，以它 為主；長(zhǎng)時(shí)間用加速度計(jì)比較準(zhǔn)確，這時(shí)候加大它的比重，這就是互補(bǔ)；加速度計(jì)要濾掉高頻信號(hào)，陀螺儀要濾掉低頻信號(hào)，互補(bǔ)濾波器就是根據(jù)傳感器特性不同，通過不同的濾波器（高通或低通，互補(bǔ)的），然后再相加得到整個(gè)頻帶的信號(hào)，例如，加速度計(jì)測(cè)傾角，其動(dòng)態(tài)響應(yīng)較慢，在高頻時(shí)信號(hào)不可用，所以可通過低通抑制高頻；陀螺響應(yīng)快，積分后可測(cè)傾角，不過由于零漂等，在低頻段信號(hào)不好。但是阻隔、減弱的幅度則會(huì)依據(jù)不同的頻率以及不同的濾波程序（目的）而改變。 高通濾波 (highpass filter) 是一種過濾方式，規(guī)則為高頻信號(hào)能正常通過，而低于設(shè)定臨界值的低頻信號(hào)則被阻隔、減弱。 互補(bǔ)濾波 的 Matlab 仿真： % Complement filter Simulation System % angle= * (angle + angle_dot * dt) + * ACCEL_Z。%2ms angle = 0。 angle_dot = GYRO_X。,i,angle_dot,39。)。 ACCEL_Z = sin(i) + (1+(1(1))*rand(1))。 GYRO_X = cos(i) + 5*(1+(1(1))*rand(1))。 angle= * (angle + angle_dot * dt) + * ACCEL_Z。 ACCEL_Z = sin(i) + 10*(1+(1(1))*rand(1))。 Float angle_dot = 0。 //陀螺儀溫漂低通濾波 500次 bias_cf+=gyro_m_cf*。 得到 車體姿態(tài) 穩(wěn)定 的估計(jì)值之后我們就可以用來控制電機(jī)以達(dá)到使車體平衡 的目的，在 通過 姿態(tài)估計(jì)值控制電機(jī)的算法中我們可以使用 經(jīng)典 的 PID 調(diào)節(jié)器 ，即通過角度與零角度的偏差的比例、積分 和 微分控制， 以 達(dá)到快速穩(wěn)定強(qiáng)勁的平衡效果。 // = 比例系數(shù) *角度 Upright_M += Kd_ang * angle_dot。 PWM_R = (int)(moto_start*Upright_M*moto_start)。 速度采集及 速度控制模塊 ： 對(duì)于基于 視覺兩輪 平衡車 系統(tǒng)而言，速度的控制相對(duì)于普通普通系統(tǒng)而言要復(fù)雜很多。 速度采集 ： 對(duì)于速度控制系統(tǒng)，速度采集尤為重要，采集到準(zhǔn)確實(shí)時(shí)的速度信息才是控制速度的關(guān)鍵。如果 A 相超前 B 相，那么電機(jī)的旋轉(zhuǎn)方向被認(rèn)為是正向的。當(dāng)旋轉(zhuǎn)的方向改變時(shí)，這些狀態(tài)的順序與此相反（ 11， 10， 00， 01）。 FTM1MODE |= FTM_MODE_WPDIS_MASK。 FTM1MOD = 65535。 FTM1QDCTRL |= FTM_QDCTRL_QUADMODE_MASK。 FTM1CNT = 0。 Pulse_L = FTM1CNT。 速度控制： 關(guān)于速度測(cè)量與速度采集的速度偏差，我們需要用來控制車體傾角已達(dá)到控制車體速度的目的。 u(t) = ????[??(??) + 1???? ∮??(??)???? +???? ? ????(??)???? ] 式中積分的上下限分別是 0和 t。積分控制可以消除穩(wěn)態(tài)誤差，配合比例控制使用及 PI 控制器，可以使系統(tǒng)穩(wěn)態(tài)無誤差。對(duì)于滯后性比較大的系統(tǒng)可以使用 PD控制器，有利于改善系統(tǒng)的動(dòng)態(tài)特性。 clear。kd=。%離散化 [num, den]=tfdata(Gd, 39。 y_1=0。 for k=1:1:10000 t(k)=k*ts。 e(k)=r(k)y(k)。 c(k)=kp*x(1)+ki*x(2)+kd*x(3)。y_2=y_1。,t,r,39。 單位階躍信號(hào) kp=20。ki=。kd=。 單位正弦信號(hào) kp=80。ki=。 speed_mersure_old = speed_mersure。//更新差值 speed_M_old = speed_M。 } speed_M = Kp_spd*current_error + speed_M_Integral。 攝像頭采集 處理 算法及 轉(zhuǎn)向 模塊： 對(duì)于基于 視覺兩輪 平衡車系統(tǒng)，本論文采用 CMOS攝像頭采集路況信息并加以處理路徑識(shí)別，已達(dá)到視覺導(dǎo)航的目的。 D M A 、 BF 3 0 0 3 U 初始化場(chǎng)同步信號(hào)V S Y N行同步信號(hào)H S Y NYES像素中斷信號(hào)V C L K設(shè)置 D M A 傳輸原地址和目的地址打開行中斷像素中斷觸發(fā) D M A傳輸一個(gè)像素?cái)?shù)據(jù)打開 D M A 開始傳輸特定行數(shù)據(jù)D M A 等待 V C L K 觸發(fā)傳輸數(shù)據(jù)一行數(shù)據(jù)是否傳輸完成一場(chǎng)數(shù)據(jù)是否傳輸完成YESYES等待場(chǎng)同步信號(hào)V S Y NYESNO等待行同步信號(hào)H S Y NNONO一個(gè)像素傳輸完成之后原地址不變目的地址自加關(guān)閉行中斷關(guān)閉 D M ANONO 因此 MCU 在得到 場(chǎng)同步信號(hào) 、行同步信號(hào) 、 像素中斷 信號(hào) 之后所需要做的動(dòng)作總結(jié)如下： /****************************************** 變量*****************************************/ int Line_coll[] = { //采集 特定行 9, 18, 27, 36, 45, 54, 63, 72, 81, 90, 99, 108,117,126,135,144,153,162,171,180, 189,198,207,216,225,234,243,252,261,270, 279,288,297,306,315,324,333,342,351,360, 369,378,387,396,405,414,423,432,441,450, 1000,1000 //1000 表示停止采集 }。 //行采集標(biāo)志 unsigned char Field_Over = 0。 //清除中斷標(biāo)志 DMA_ERQ |= (DMA_ERQ_ERQ0_MASK(4))。 if(*Line_t == V_Cnt) //采集指定行 { Line_coll_flag = 1。 //清中斷標(biāo)志 DMA_Setsource((uint32*)(amp。 //采集行數(shù)回到初始值 V_Cnt=0。 //清除中斷標(biāo)志 DMA_ERQ amp。 轉(zhuǎn)向控制的實(shí)現(xiàn)只需要控制兩個(gè)電機(jī)的差速即可控制轉(zhuǎn)向。 Upright_M = Kp_ang * angle。 PWM_L = (int)(moto_start*Upright_M*moto_start*1 ((speed_M_error*Speed_Cycle)/ SPEED_CYCLE + speed_M_old)*car_start_flag*0 moto_start*((Direction_M_error*Dirction_Cycle)/DIRCTION_CYCLE + Direction_M_old)*0)。 } 人機(jī) 接口及調(diào)試接 口調(diào)試 軟件設(shè)計(jì)： 關(guān)于人機(jī)接口，并不是本系統(tǒng)必須要的功能模塊，但是人機(jī)接口會(huì)使我們的系統(tǒng)具有良好的交互性，并在程序調(diào)試上具有事倍功半的效果。 扎實(shí)的基礎(chǔ)、領(lǐng)悟能力和充分的知識(shí)儲(chǔ)備是順利完成畢業(yè)設(shè)計(jì)的首要條件而且也是對(duì)綜合能力的培養(yǎng)。 參考文獻(xiàn) [1] 邵貝貝．單片機(jī)嵌入式應(yīng)用的在線開發(fā)方法［ M］ .北京：清華大學(xué)出版社， 20xx [2] 劉克成． C 語言程序設(shè)計(jì)［ M］ .北京：中國(guó)鐵道出版社 ,20xx. [3] 王宜懷 ,曹金華．嵌入式系統(tǒng)設(shè)計(jì) — 基于飛思卡 S12X微控制器［ M］ .北京：北京航空航天大學(xué)出版社， 20xx [4] 孫同景． Freescale 9S12十六位單片機(jī)原理及嵌入式開發(fā)技術(shù)［ M］ .北京：機(jī)械工業(yè)出版社， 20xx [5]