【文章內(nèi)容簡介】 [0].CnSC = 0。 FTM0CONTROLS[0].CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK)。 FTM0CONTROLS[1].CnSC = 0。 FTM0CONTROLS[1].CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK)。 FTM0COMBINE amp。= ~FTM_COMBINE_DECAPEN0_MASK。 FTM0COMBINE amp。= ~FTM_COMBINE_COMBINE0_MASK。 FTM0CONTROLS[2].CnSC = 0。 FTM0CONTROLS[2].CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK)。 FTM0CONTROLS[3].CnSC = 0。 FTM0CONTROLS[3].CnSC |= (FTM_CnSC_MSB_MASK|FTM_CnSC_ELSB_MASK)。 FTM0COMBINE amp。= ~FTM_COMBINE_DECAPEN1_MASK。 FTM0COMBINE amp。= ~FTM_COMBINE_COMBINE1_MASK。 FTM0CONTROLS[0].CnV = (((FTM0MOD)*(PWMDuty))/1000)。 FTM0CONTROLS[1].CnV = (((FTM0MOD)*(PWMDuty))/1000)。 FTM0CONTROLS[2].CnV = (((FTM0MOD)*(PWMDuty))/1000)。 FTM0CONTROLS[3].CnV = (((FTM0MOD)*(PWMDuty))/1000)。 陀螺儀 加速度計采集模塊 ： 此視覺 導(dǎo)航平衡車系統(tǒng)采用 MPU6050陀螺儀 加速度計集成芯片 ， MPU6050整合了 3軸陀螺儀、 3軸加速器，并含可藉由第二個 I2C 端口連接其他廠牌之加速器、磁力傳感器、或其他傳感器的數(shù)位運動處理 (DMP: Digital Motion Processor)硬件加速引擎，由主要 I2C 端口以單一數(shù)據(jù)流的形式，向應(yīng)用端輸出完整的 9軸融合演算技術(shù) InvenSense 的運動處理資料庫，可處理運動感測的復(fù)雜數(shù)據(jù)，降低了運動處理運算對操作系統(tǒng)的負荷，并為應(yīng)用開發(fā)提供架構(gòu)化的 API。 I2C（ Inter－ Integrated Circuit）總線是由 PHILIPS 公司開發(fā)的兩線式串行總線，用于連接微控制器及其外圍設(shè)備。是微電子通信控制領(lǐng)域廣泛采用的一種總線標(biāo)準(zhǔn)。它是同步通信的一種特殊形式，具有接口線少，控制方式簡單，器件封裝形式小，通信速率較高等優(yōu)點。 I2C 總線支持任何 IC 生產(chǎn)過程 (CMOS、雙極性）。通過串行數(shù)據(jù)（ SDA）線和串行時鐘 （ SCL）線在連接到總線的器件間傳遞信息。 通過 MK60FX512VLQ15 MCU 內(nèi)部的 I2C 模塊可以實現(xiàn) 對 MPU6050的 采集。具體的配置如下： 配置 I2C： SIM_SCGC4 |= SIM_SCGC4_I2C0_MASK。 PORTB_PCR2 = PORT_PCR_MUX(2)。 PORTB_PCR3 = PORT_PCR_MUX(2)。 I2C_F_REG(I2Cx[i2]) = I2C_F_MULT(mult) | I2C_F_ICR(scl_dev) 。 I2C0_C1 = I2C_C1_IICEN_MASK | I2C_C1_TXAK_MASK。 配置 MPU6050： i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_PWR_MGMT_1, 0x00)。 i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_PWR_MGMT_2, 0x00)。 i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_CONFIG, 0x06)。 i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_GYRO_CONFIG, 0x08)。 i2c_writeaddr(I2C_Moudle, ADDRESS, MPU6050_ACCEL_CONFIG, 0x08)。 讀取 姿態(tài)數(shù)據(jù)： ACCEL_Z = ((int16)i2c_readaddr(I2C_Moudle ,ADDRESS, ACCEL_ZOUT) 8) |(int16)i2c_readaddr(I2C_Moudle, ADDRESS, ACCEL_ZOUT + 1)。 GYRO_X = ((int16)i2c_readaddr(I2C_Moudle, ADDRESS, GYRO_XOUT) 8) |(int16)i2c_readaddr(I2C_Moudle, ADDRESS, GYRO_XOUT + 1)。 周期 中斷定時器模塊 ： 此視覺 導(dǎo)航平衡車系統(tǒng) 需要 運行多任務(wù)，比如速度控制 、 姿態(tài)算法、平衡控制、轉(zhuǎn)向控制、等。因此我們需要簡要的實現(xiàn)任務(wù)切換算法，所以我們需要使用周期中斷定時器實現(xiàn)任務(wù)切換算法。周期中斷定時器配置如下 ： SIMSCGC6 |= SIM_SCGC6_PIT_MASK。 PITMCR amp。= ~PIT_MCR_MDIS_MASK。 PITMCR |= PIT_MCR_FRZ_MASK。 PITCHANNEL[1].LDVAL = (uint32_t)time。 PITCHANNEL[1].TFLG |= PIT_TFLG_TIF_MASK。 PITCHANNEL[1].TCTRL |= PIT_TCTRL_TEN_MASK。 PITCHANNEL[1].TCTRL |= PIT_TCTRL_TIE_MASK。 NVIC_EnableIRQ(PIT1_IRQn)。 NVIC_Init(PIT1_IRQn, NVIC_PriorityGroup_3, 1, 0)。 簡單 的任務(wù)切換程序如下： void PIT0_IRQHandler(void) { PITCHANNEL[0].TFLG |= PIT_TFLG_TIF_MASK。 Stitic unsigned char sys_t = 0。 switch(sys_t ++) { case 0:{ }break。 case 1:{ }break。 case 2:{ }break。 case 3:{ }sys_t = 0。break。 } } 互補濾波算法 及 平衡控制模塊 ： 平衡 理論分析： 對于基于 視覺兩輪 平衡車， 可以分析為一個倒立擺系統(tǒng)， 如果 車體 有 向前倒的趨勢，那么 車體 需要向前加速運動保持平衡，如果車體有向后倒的趨勢，那么車體需要向后加速運動保持平衡。 對 此系統(tǒng) 建立 動力學(xué)數(shù)學(xué)模型 ，可以 看出此系統(tǒng)是一個倒立的重力場系統(tǒng)。 在 重力場中 ： 回復(fù)力 與角度成正比，同時 空氣 中具有阻尼力， 因此 單擺系統(tǒng)可以保持穩(wěn)定 。是一個 負反饋系統(tǒng) 。 F = ?mgsinθ ≈ ?mgθ 在 倒立擺系統(tǒng)中： 偏離 一定的位置之后回復(fù)力與位移方向相 同 ，導(dǎo)致繼續(xù)偏離平衡位置，最終倒下。 是 一個正反饋系統(tǒng)。 因此我們 必須額外提供其他的力（即車輪的加速度） 以 提供回復(fù)力 ， 同時我們還需要提供阻尼力。 F = mgsinθ ? macosθ ≈ mgθ ?????1θ 因此 總結(jié)出，在整個系統(tǒng)中， 角度 即為系統(tǒng)增益，角速度即為系統(tǒng)阻尼，通過對角度和角速度的偏差的比例和微分控制既可以達到平衡。 在 這個 系統(tǒng) 中，我們需要兩個條件，第一個是 我們 可以控制車體前后加速運動，這個我們可以控制電機運動達到這個目的，第二個條件是我們可以監(jiān)測車體此時此刻的姿態(tài) （ 包括角度和角速度） ， 這個我們可以通過角速度傳感器陀螺儀和 加速度 傳感器加速度計來 采集 此時此刻的車體姿態(tài) 。 對于基于 視覺兩輪 平衡車 而言， 我 們通過加速度計和陀螺儀 MPU6050 采集 到原始 姿態(tài) 值 ，但是加速度計對車體的加速度比較 敏感，取瞬時值計算傾角誤差比 較大；而陀螺儀積分得到的角度不受小車加速度的影響，但是隨著時間的增加積分漂移和溫度漂移帶來的誤差比較大。所以這兩個傳感器正好可以彌補相互的缺點。互補濾波就是在短時間內(nèi)采用陀螺儀得到的角度做為最優(yōu)，定時對加速度采樣來的角度進行取平均值來校正陀螺儀的得到的角度。就是，短時間內(nèi)用陀螺儀比較準(zhǔn) 確，以它 為主；長時間用加速度計比較準(zhǔn)確，這時候加大它的比重，這就是互補；加速度計要濾掉高頻信號，陀螺儀要濾掉低頻信號，互補濾波器就是根據(jù)傳感器特性不同，通過不同的濾波器（高通或低通，互補的），然后再相加得到整個頻帶的信號，例如，加速度計測傾角，其動態(tài)響應(yīng)較慢，在高頻時信號不可用，所以可通過低通抑制高頻；陀螺響應(yīng)快，積分后可測傾角，不過由于零漂等，在低頻段信號不好。通過高通濾波可抑制低頻噪聲。將兩者結(jié)合，就將陀螺和加表的優(yōu)點融合起來，得到在高頻和低頻都較好的信號，互補濾波需要選擇切換的頻率點，即高通和低通的頻 率， 這就是濾波。 低通濾波 (Lowpass filter) 是一種過濾方式，規(guī)則為低頻信號能正常通過，而超過設(shè)定臨界值的高頻信號則被阻隔、減弱。但是阻隔、減弱的幅度則會依據(jù)不同的頻率以及不同的濾波程序（目的）而改變。它有的時候也被叫做高頻去除過濾（ highcut filter）或者最高去除過濾（ treblecut filter)。低通過濾是高通過濾的對立。 低通濾波器的離散公式： Y(k) = (Tc/(Tc+Ts))*Y(k1) + (Ts/(Tc+Ts))*X(k) 其中 Tc 為濾波時間系數(shù)， Ts 為采樣時間， X(k)為當(dāng)前采樣值 ,Y(k)為濾波器輸出值 ,Y(k－ 1)為濾波器的上次輸出。 高通濾波 (highpass filter) 是一種過濾方式，規(guī)則為高頻信號能正常通過，而低于設(shè)定臨界值的低頻信號則被阻隔、減弱。但是阻隔、減弱的幅度則會依據(jù)不同的頻率以及不同的濾波程序（目的）而改變。它有的時候也被叫做低頻去除過濾（ lowcut filter）。高通濾波是低通濾波的對立。 互補濾波 的 Matlab 仿真： % Complement filter Simulation System % angle= * (angle + angle_dot * dt) + * ACCEL_Z。 clc。 clear。 dt=。%2ms angle = 0。 angle_dot = 0。 for i=0::50 ACCEL_Z = sin(i) + 5*(1+(1(1))*rand(1))。 GYRO_X = cos(i) + 5*(1+(1(1))*rand(1))。 angle_dot = GYRO_X。 angle= * (angle + angle_dot * dt) + * ACCEL_Z。 % ACCEL_Z = m紫紅 *星號 angle_dot = r紅：點線 angle = k黑－實線 plot (i,ACCEL_Z,39。m39。,i,angle_dot,39。r39。,i,angle,39。k39。)。 hold on end 在 此仿真系統(tǒng)中 陀螺儀給的值為余弦函數(shù) + 5*[1 1]之間的隨機， 加速度的值為正弦函數(shù) +5*[1 1 ]之間的隨機數(shù) ACCEL_Z = sin(i) + (1+(1(1))*rand(1))。 GYRO_X = cos(i) + (1+(1(1))*rand(1))。 angle= * (angle + angle_dot * dt) + * ACCEL_Z。 ACCEL_Z = sin(i) + (1+(1(1))*rand(1))。 GYRO_X = cos(i) + (1+(1(1))*rand(1))。 angle= * (angle + angle_do