【文章內(nèi)容簡(jiǎn)介】 一種測(cè)量車模速度的傳感器。同時(shí)傳感器采集的信息要傳到控制器，我們需要2 個(gè)定時(shí)/計(jì)數(shù)器或 2 個(gè)外部中斷來(lái)記錄速度信息。電機(jī)的轉(zhuǎn)速?zèng)Q定了車模前進(jìn)的速度，而轉(zhuǎn)速與電機(jī)電壓成正比，所以我們需要 2 路 PWM 改變電機(jī)電壓控制車模速度。速度控制與平衡控制是分時(shí)進(jìn)行的，在速度控制時(shí)假設(shè)小車處于平衡，通過(guò) PWM 給車加上一個(gè)設(shè)定的速度，但執(zhí)行很短一段時(shí)間又要去執(zhí)行平衡控制，平衡是最重要的，這樣不斷循環(huán)，就可達(dá)到勻速且平衡運(yùn)動(dòng)的目標(biāo)。14 方向控制小車方向控制相對(duì)簡(jiǎn)單些，我們要改變兩輪車的方向，就要不同的驅(qū)動(dòng)動(dòng)力源，所以就要兩個(gè)直流減速電機(jī)。通過(guò)左右電機(jī)的差速，可以實(shí)現(xiàn)車模的轉(zhuǎn)向。這也是車速控制需要 2 路 PWM 和 2 個(gè)定時(shí)/ 計(jì)數(shù)器或 2 個(gè)外部中斷的原因。同時(shí)我們要讓小車自己選擇行走方向，需要一個(gè)小車的“眼睛” ?？梢圆捎眉t外、CCD 攝像頭、超聲波、無(wú)線藍(lán)牙等充當(dāng)機(jī)器人的“ 眼睛” 。 系統(tǒng)總體設(shè)計(jì)框架由以上任務(wù)分解，可得圖 所示總體框架圖，控制器需要收集車模的傾角、方向、角速度、速度等信息。根據(jù)這些信息并經(jīng)過(guò)控制器進(jìn)行處理輸出PWM 控制左右電機(jī)以達(dá)到目的。圖 系統(tǒng)總控制框圖15第 4 章 硬件選型及硬件電路 主控芯片 STC12C5A60S2根據(jù)第二第三章介紹，需要的硬件類型基本確定，現(xiàn)結(jié)合實(shí)際情況以及經(jīng)濟(jì)成本等確定適合的硬件，在可以達(dá)到課題目的的前提下，做到充分利用硬件資源，節(jié)約成本。所以選用 STC12C5A60S2，這款單片機(jī)基本可以達(dá)到本課題的大致要求，而且價(jià)錢便宜適合本次開(kāi)發(fā)研究。以下就是 STC12C5A60S2 單片機(jī)的簡(jiǎn)單介紹。STC12C5A60S2 特點(diǎn)如下：(1) 增強(qiáng)型 8051CPU，1T 單時(shí)鐘/單周期單片機(jī)，指令代碼完全兼容 8051，開(kāi)發(fā)難度較低，同時(shí) 1T 單片機(jī)指令執(zhí)行速度是 8051 的 812 倍。這樣可以滿足小車快速的要求(2) 工作電壓 （5V 單片機(jī)），電壓范圍較寬，且電壓容易做到。支持工作頻率 0—35MHz。且有強(qiáng)抗靜電，強(qiáng)抗干擾的特點(diǎn)。(3) 用戶應(yīng)用程序空間為 62K，片上集成 128 字節(jié)的 RAM。有 EEPROM 和看門狗等(4) 40 個(gè)通用 I/O 口，復(fù)位后為：準(zhǔn)雙向口/弱上拉?？稍O(shè)置為四種模式：準(zhǔn)雙向口/弱上拉、強(qiáng)推挽/強(qiáng)上拉、開(kāi)漏、輸入/高阻。每個(gè) I/0 口可產(chǎn)生驅(qū)動(dòng)電流 20mA。(5) 下載方便，ISP/IAP 下載，無(wú)需專用編程器或仿真器，僅通過(guò) 直接下載。(6) 2 路 PWM/PCA,可輸出 PWM 方波，并可改變方波占空比。8 路 10 位高精度A/D 轉(zhuǎn)換，轉(zhuǎn)換 25 萬(wàn)次每秒。(7) 有兩路與 8051 兼容的 16 位定時(shí)器 T0/和 T1，還有兩路可由獨(dú)立比特率發(fā)生器做串口通訊的波特率發(fā)生器和 PCA 模塊實(shí)現(xiàn)的 2 個(gè) 16 位定時(shí)器。 電源模塊前面兩章提到直流電機(jī)，因?yàn)樾枰筠D(zhuǎn)矩、大轉(zhuǎn)速，我們選用 12V 的大轉(zhuǎn)矩直流減速電機(jī)，同時(shí)小車需要自由移動(dòng)因此選用 12V 暴力 9800 毫安聚合物鋰電池，該電池工作電流大可達(dá) 3A，可承受電機(jī)啟動(dòng)產(chǎn)生的大電流。帶負(fù)載能力強(qiáng)，基本滿足平衡車的負(fù)載要求。同時(shí)單片機(jī)、電機(jī)驅(qū)動(dòng)、傳感器等芯片需要 5V 電壓，為了方便采用 12V 降壓模塊 LM2596S，它可輸入 3V—40V 電壓，16輸出 —35V 電壓可調(diào)，且強(qiáng)抗干擾，輸出紋波30mv,最大輸出電流 3A。所以可以保證傳感器元件之間和電機(jī)的較大干擾下仍可輸出穩(wěn)定電壓和電流。LM2596S 芯片電路原理圖如圖 所示。圖 LM2596S 電路原理圖 電機(jī)驅(qū)動(dòng)模塊電機(jī)啟動(dòng)電流很大，因此一般的橋式電路難以提供大電流，且電機(jī)需要PWM 控制，為此我們選用 L298N，它有 IN1IN4 控制兩路電機(jī)正反轉(zhuǎn)，啟動(dòng)性能較好，啟動(dòng)轉(zhuǎn)矩大。工作電壓可達(dá) 36V/4A，并可同時(shí)驅(qū)動(dòng)兩臺(tái)直流電機(jī)。芯片有 A/B 兩相可以輸入 2 路 PWM,。L298N 原理圖如圖 17圖 L298N 原理圖 傳感器模塊(1) 光電編碼模塊首先我們需要知道車輪的速度信息，在此中我們需要光電編碼測(cè)量小車的速度，采用的編碼盤 4200 個(gè)光柵一圈，可以很精確的獲取車模的速度信息。轉(zhuǎn)動(dòng)碼盤用不透明的物理?yè)踝」鈺r(shí)，就會(huì)有脈沖輸出，每圈 4200 個(gè)脈沖。同時(shí)在編碼上有 AB 兩相脈沖，AB 兩相相位相差 90 度，通過(guò)控制器讀取 AB 相的脈沖，若 A 相先有脈沖則電機(jī)正轉(zhuǎn)，若 B 相先有脈沖則電機(jī)反轉(zhuǎn)。通過(guò)記錄單位時(shí)間的脈沖數(shù)就可知車模的速度，也可知道車模運(yùn)行大概的距離。從而可以較好的實(shí)現(xiàn)速度反饋控制。(2) 陀螺儀和加速度計(jì)在前面已經(jīng)分析了車模平衡需要車模的傾角、角速度等信息，這也是平衡的關(guān)鍵。我們選擇 MPU6050 它集成了陀螺儀和加速度計(jì)兩者的芯片，工作電壓 。MPU6050 內(nèi)部寄存器復(fù)雜，需要仔細(xì)研讀 Datesheet，才能操作芯片讀取數(shù)據(jù)。但使用確比較方便，它內(nèi)部有三個(gè)高速 16 位 A/D 轉(zhuǎn)換，可以直接讀出。通過(guò) 400KHz 的 I2C 和 SPI 通訊，要熟悉 I2C 通訊協(xié)議。集成的陀螺儀部分可測(cè)量角速度范圍為177。250、177。500、177。1000、177。2022（dps） ，加速度計(jì)可測(cè)量角加速度范圍為177。177。177。177。16g，并都可以程序控制測(cè)量范圍。本課題選用177。2022 （dps）和177。2g 。且陀螺儀 177。2022（dps ）時(shí)它的靈敏度最低分辨率是,加速度計(jì)177。2g 時(shí)靈敏度最低分辨率是 16384/g，因?yàn)樗?A/D 轉(zhuǎn)換出18來(lái)的是 065536 的一個(gè)值是最低靈敏度的整數(shù)倍，因此在計(jì)算這些參數(shù)時(shí)，都需要除以相應(yīng)的最低分辨率。MPU6050 電路原理圖如圖 圖 MPU6050 電路原理圖(3) 紅外循跡傳感器為了讓下車可以根據(jù)設(shè)定的任意路線行走，需要紅外循跡模塊，該模塊工作電壓 35V,可以發(fā)射一定頻率的紅外線，經(jīng)過(guò)障礙物反射回來(lái)并接受，此時(shí)紅外模塊比較電路處理會(huì)輸出高電平，若障礙物為黑線可以吸收紅外線，不能返回紅外線，此時(shí)輸出低電平，因此根據(jù)檢測(cè)電平就可不斷調(diào)整方向，使小車兩輪產(chǎn)生差速改變轉(zhuǎn)向。19 系統(tǒng)硬件控制結(jié)構(gòu)圖圖 小車硬件組裝外觀圖20第 5 章 傳感器數(shù)據(jù)融合和濾波 慣性傳感器數(shù)據(jù)融合和濾波的必要性在第四章章中，我們提到要用集成陀螺儀和加速度計(jì)的 MPU6050 采集車模的傾角和角速度，其實(shí)他們不能直接得到有效的角度和角速度信息。陀螺儀是測(cè)量車模的角速度，當(dāng)然可以用角速度積分的方法可以得到車模的傾角，但存在漂移誤差，在一段時(shí)間后會(huì)偏離實(shí)際角速度值，這個(gè)偏差雖然不大，但時(shí)間一長(zhǎng)，積累誤差就會(huì)越來(lái)越大。最終難以控制車模平衡。另一方面，加速度計(jì)可以測(cè)量車模的 X、Y、Z 三個(gè)轉(zhuǎn)軸的角加速度，通過(guò)幾何關(guān)系和物理受力分析可解得車模的傾角等信息。研究發(fā)現(xiàn)加速度計(jì)雖靜態(tài)效果較好數(shù)據(jù)比較準(zhǔn)確，但加速度計(jì)動(dòng)態(tài)響應(yīng)慢，在車模傾角變化時(shí)，為了保持車模的時(shí)刻平衡，需要很快的檢測(cè)車模位置信息。因此兩者單獨(dú)使用都無(wú)法達(dá)到預(yù)期目標(biāo)，我們需要綜合運(yùn)用它們，取長(zhǎng)補(bǔ)短達(dá)到目的。同時(shí)它們都存在高斯白噪聲，所以需要傳感器數(shù)據(jù)融合和濾波。不僅在測(cè)傾角信息需要融合濾波，在速度測(cè)量時(shí)，在變化很大速度時(shí)，小車難以控制，需要讓波形平滑，所以也需要濾波，濾波有卡爾曼濾波和互補(bǔ)濾波等，下面這要介紹卡爾曼和互補(bǔ)濾波。下圖 就是陀螺儀角度誤差引起的漂移誤差曲線。圖 角度積分漂移誤差 卡爾曼數(shù)據(jù)融合Kalman 匈牙利數(shù)學(xué)家，Kalman 濾波器是他的博士論文提出的。是一種最優(yōu)自回歸數(shù)據(jù)處理算法，處理效率高，廣泛應(yīng)用與導(dǎo)航、控制、傳感器數(shù)據(jù)融合。可以對(duì)動(dòng)態(tài)過(guò)程進(jìn)行狀態(tài)估計(jì)，實(shí)現(xiàn)運(yùn)動(dòng)狀態(tài)的預(yù)測(cè)，從而可以消去誤差，使波形平滑。在本課題中使用的陀螺儀和加速度計(jì)獲取車模的動(dòng)態(tài)過(guò)程，這些傳感器受到外界很多不確定的干擾、自身漂移、測(cè)量誤差等影響，所獲得的角度信息也就不準(zhǔn)確。實(shí)驗(yàn)發(fā)現(xiàn)這些干擾會(huì)使小車無(wú)法平衡。所以只能用觀測(cè)到的參考量和設(shè)備自身的誤差率來(lái)估計(jì)車模狀態(tài)。21那么卡爾曼濾波具體是怎么樣的以及怎樣運(yùn)用到本設(shè)計(jì)，下面就結(jié)合本設(shè)計(jì)測(cè)量傳感器陀螺儀和加速度計(jì)分析 Kalman 濾波 五個(gè)方程怎樣使系統(tǒng)輸[16]出最優(yōu)。（1）找出陀螺儀和加速度計(jì)之間的線性微分方程角度( )和角速度( )存在微分關(guān)系,考慮到儀器有高斯白噪聲（ ）?gyro_ gyro_e是陀螺儀的噪聲， 為加速度計(jì)的噪聲） ，得到如下微分方程 。acel ry+go_erdt??（51） =aclrel（52）（ 為加速度計(jì)測(cè)量 軸角加速度計(jì)算而來(lái)，計(jì)算式為acel?， 為加速度計(jì) X 軸的角加速度， 為加速_xtn=lzacel _acelz度計(jì) Z 軸的角速度計(jì)）（2）建立系統(tǒng)狀態(tài)方程和測(cè)量方程由 51,52 式可得系統(tǒng)傾角 可以做一個(gè)狀態(tài)向量，以 accelθ 為陀螺儀的常?值偏差作另一個(gè)向量，可得系統(tǒng)的狀態(tài)方程和觀測(cè)方程 如下[15] _er011_0_0gyroacelacel???????????????????????? ???（53） ?lrll??????（54）設(shè)系統(tǒng)采樣時(shí)間為 T,把 53,54 離散化可得對(duì)應(yīng)的離散系統(tǒng)狀態(tài)方程和測(cè)量方程如下： ()(1)_(1)_er()000xkxkgyrokgyokT???????????????????????（55） ?()()()yacel?（56）可以通過(guò) k1 時(shí)刻的角度值估計(jì) k 時(shí)刻實(shí)際角度，并根據(jù) k 時(shí)刻預(yù)測(cè)值得到 k 時(shí)刻的高斯噪聲方差，并不斷遞歸更新數(shù)值直到最優(yōu)角度值。需要知道系統(tǒng)過(guò)程噪聲協(xié)方差陣 Q 以及誤差協(xié)方差矩陣 R,22且 Q= R=_0qacelgyro????????_racel上式中，q_gyro 和 q_accel 分別是陀螺儀的和加速度計(jì)的測(cè)量協(xié)方差，數(shù)值越小表明信任程度越高，本課題中陀螺儀的值更可信任，所以它的值要小些。以下就是卡爾曼濾波器根據(jù)它們的協(xié)方差估計(jì)系統(tǒng)最優(yōu)值。利用系統(tǒng)的離散狀態(tài)方程，可以根據(jù)上一狀態(tài)預(yù)測(cè)現(xiàn)在狀態(tài)，設(shè)現(xiàn)在狀態(tài)為 k，可得上一狀態(tài)預(yù)測(cè)現(xiàn)在的狀態(tài)方程如下：x(k|k1)=Ax(k1|k1)+BU(K) (57)x(k|k1)是利用上一狀態(tài)預(yù)測(cè)現(xiàn)在的結(jié)果， x(k1|k1)是上一狀態(tài)的最優(yōu)值。U(K)為狀態(tài)控制量。通過(guò) 57 式系統(tǒng)由 k1 時(shí)預(yù)測(cè)了 k 時(shí)現(xiàn)在的狀態(tài)，但 x(k|k1)的協(xié)方差也要更新用 p(k|k1)表示如下 (|1)(|1)TpkApQ????(58)P(k1|k1)是 x(k1|k1)的協(xié)方差,A= ,B=.0?????????現(xiàn)在我們預(yù)測(cè)了現(xiàn)在狀態(tài)，再加上我們用儀器測(cè)量現(xiàn)在狀態(tài)值，綜合預(yù)測(cè)值和測(cè)量值可得到現(xiàn)在最優(yōu)估計(jì)值 x(k|k)。 (|)(|1)()(|1)xkkmykHx????(59)Km 為卡爾曼增益，H 為測(cè)量系統(tǒng)參數(shù) ()|)/(|)TTmppR?(510)到此，已得到 K 狀態(tài)下最優(yōu)值 x(k|k),同時(shí)車模需要時(shí)刻變化，也需要時(shí)刻估算下一狀態(tài)，讓濾波形成一個(gè)循環(huán)，這樣我們還要知道 x(k|k)時(shí)的協(xié)方差p(k|k)。 (|)()Hpk|1)pkIm???(511)I= ，這樣進(jìn)入 k+1 時(shí)刻，又回到式 57，也就是總是前一狀態(tài)、現(xiàn)在、下一1??????狀態(tài)，這樣可以不斷遞歸運(yùn)算，時(shí)刻計(jì)算當(dāng)前最優(yōu)值。所以利用 Kalman 濾波可以有效的解決陀螺儀和加速度計(jì)兩者的缺陷，不斷進(jìn)行時(shí)間預(yù)測(cè)和測(cè)量更新，彌補(bǔ)陀螺儀的漂移和加速度計(jì)的動(dòng)態(tài)性能較差的缺點(diǎn)。從而為控制車模提供準(zhǔn)確的姿態(tài)信息，這點(diǎn)是本課題的核心。是小車平衡的基礎(chǔ)。23 互補(bǔ)濾波針對(duì)卡爾曼濾波過(guò)程復(fù)雜，計(jì)算量大，軟件編程量大。可以采用一種互補(bǔ)濾波的濾波方法，保留加速度計(jì)的靜態(tài)角度，也就是用低通濾去快速變化的信號(hào)，對(duì)陀螺儀采用高通濾波濾去靜態(tài)角度信息。然后將兩者進(jìn)行權(quán)重，也可準(zhǔn)確得出傾角信息?；パa(bǔ)濾波器計(jì)算式為： ()1kgyrodtkgyroacel?????(512) 為最優(yōu)角度值， 陀螺儀高通濾波系數(shù)， 是加速度計(jì)低通濾波系kacel數(shù)， 為陀螺儀當(dāng)前角速度值， 加速度計(jì)的測(cè)量?jī)A角值。yrkcel且， ， 為濾波時(shí)間常數(shù)。godt???dtacel???通過(guò)式 512 當(dāng)時(shí)間常數(shù)小于 的低通濾波被濾掉，時(shí)間常數(shù)大于 的高通濾?波被濾掉。通過(guò) mpu6050 實(shí)驗(yàn)可以得到表 所示的角度變化情況，設(shè)定 為 （更信任陀螺儀） ，dt=, 則=., =,當(dāng)車模 100 度每秒的角速度kgyrokacel從 0 旋轉(zhuǎn)到 10 度然后靜止不動(dòng)。表 互補(bǔ)濾波角度變化情況 次數(shù) 0 1 2 3 4 5 6 7 8 9濾波角度 從第 0