【文章內(nèi)容簡(jiǎn)介】 機(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 電壓，12 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)12輸出 —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 原理圖如圖 13 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)13圖 L298N 原理圖 傳感器模塊(1) 光電編碼模塊首先我們需要知道車輪的速度信息，在此中我們需要光電編碼測(cè)量小車的速度，采用的編碼盤 4200 個(gè)光柵一圈，可以很精確的獲取車模的速度信息。轉(zhuǎn)動(dòng)碼盤用不透明的物理擋住光時(shí)，就會(huì)有脈沖輸出，每圈 4200 個(gè)脈沖。同時(shí)在編碼上有 AB 兩相脈沖，AB 兩相相位相差 90 度，通過控制器讀取 AB 相的脈沖，若 A 相先有脈沖則電機(jī)正轉(zhuǎn)，若 B 相先有脈沖則電機(jī)反轉(zhuǎn)。通過記錄單位時(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)換，可以直接讀出。通過 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)換出14 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)14來的是 065536 的一個(gè)值是最低靈敏度的整數(shù)倍，因此在計(jì)算這些參數(shù)時(shí)，都需要除以相應(yīng)的最低分辨率。MPU6050 電路原理圖如圖 圖 MPU6050 電路原理圖(3) 紅外循跡傳感器為了讓下車可以根據(jù)設(shè)定的任意路線行走，需要紅外循跡模塊，該模塊工作電壓 35V,可以發(fā)射一定頻率的紅外線，經(jīng)過障礙物反射回來并接受，此時(shí)紅外模塊比較電路處理會(huì)輸出高電平，若障礙物為黑線可以吸收紅外線，不能返回紅外線，此時(shí)輸出低電平，因此根據(jù)檢測(cè)電平就可不斷調(diào)整方向，使小車兩輪產(chǎn)生差速改變轉(zhuǎn)向。15 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)15 系統(tǒng)硬件控制結(jié)構(gòu)圖圖 小車硬件組裝外觀圖16 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)16第 5 章 傳感器數(shù)據(jù)融合和濾波 慣性傳感器數(shù)據(jù)融合和濾波的必要性在第四章章中，我們提到要用集成陀螺儀和加速度計(jì)的 MPU6050 采集車模的傾角和角速度，其實(shí)他們不能直接得到有效的角度和角速度信息。陀螺儀是測(cè)量車模的角速度，當(dāng)然可以用角速度積分的方法可以得到車模的傾角，但存在漂移誤差，在一段時(shí)間后會(huì)偏離實(shí)際角速度值，這個(gè)偏差雖然不大，但時(shí)間一長(zhǎng)，積累誤差就會(huì)越來越大。最終難以控制車模平衡。另一方面，加速度計(jì)可以測(cè)量車模的 X、Y、Z 三個(gè)轉(zhuǎn)軸的角加速度，通過幾何關(guān)系和物理受力分析可解得車模的傾角等信息。研究發(fā)現(xiàn)加速度計(jì)雖靜態(tài)效果較好數(shù)據(jù)比較準(zhǔn)確，但加速度計(jì)動(dòng)態(tài)響應(yīng)慢，在車模傾角變化時(shí)，為了保持車模的時(shí)刻平衡，需要很快的檢測(cè)車模位置信息。因此兩者單獨(dú)使用都無法達(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)過程進(jìn)行狀態(tài)估計(jì)，實(shí)現(xiàn)運(yùn)動(dòng)狀態(tài)的預(yù)測(cè)，從而可以消去誤差，使波形平滑。在本課題中使用的陀螺儀和加速度計(jì)獲取車模的動(dòng)態(tài)過程，這些傳感器受到外界很多不確定的干擾、自身漂移、測(cè)量誤差等影響，所獲得的角度信息也就不準(zhǔn)確。實(shí)驗(yàn)發(fā)現(xiàn)這些干擾會(huì)使小車無法平衡。所以只能用觀測(cè)到的參考量和設(shè)備自身的誤差率來估計(jì)車模狀態(tài)。17 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)17那么卡爾曼濾波具體是怎么樣的以及怎樣運(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ì)算而來，計(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）可以通過 k1 時(shí)刻的角度值估計(jì) k 時(shí)刻實(shí)際角度，并根據(jù) k 時(shí)刻預(yù)測(cè)值得到 k 時(shí)刻的高斯噪聲方差，并不斷遞歸更新數(shù)值直到最優(yōu)角度值。需要知道系統(tǒng)過程噪聲協(xié)方差陣 Q 以及誤差協(xié)方差矩陣 R,18 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)18且 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)控制量。通過 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ǔ)。19 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)19 互補(bǔ)濾波針對(duì)卡爾曼濾波過程復(fù)雜，計(jì)算量大，軟件編程量大。可以采用一種互補(bǔ)濾波的濾波方法，保留加速度計(jì)的靜態(tài)角度，也就是用低通濾去快速變化的信號(hào)，對(duì)陀螺儀采用高通濾波濾去靜態(tài)角度信息。然后將兩者進(jìn)行權(quán)重，也可準(zhǔn)確得出傾角信息。互補(bǔ)濾波器計(jì)算式為： ()1kgyrodtkgyroacel?????(512) 為最優(yōu)角度值， 陀螺儀高通濾波系數(shù)， 是加速度計(jì)低通濾波系kacel數(shù)， 為陀螺儀當(dāng)前角速度值， 加速度計(jì)的測(cè)量?jī)A角值。yrkcel且， ， 為濾波時(shí)間常數(shù)。godt???dtacel???通過式 512 當(dāng)時(shí)間常數(shù)小于 的低通濾波被濾掉，時(shí)間常數(shù)大于 的高通濾?波被濾掉。通過 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 到第 4 次陀螺儀快速增加到 10 度，此后角度變化緩慢，加速度計(jì)低通濾波起作用，此時(shí)主要參考加速度計(jì)的測(cè)量值。 速度濾波車模速度可能變化較大，測(cè)量的速度信息不能直接控制電機(jī)，不然會(huì)使電機(jī)干擾太大不能穩(wěn)定。所以需要測(cè)量速度進(jìn)行加權(quán)濾波，加權(quán)濾波就是將比較平穩(wěn)的上一次速度信息作主要權(quán)重，而變化后的速度作次要權(quán)。關(guān)系式為 speed = *speed+ speed*,從上式可以看出速度總是在上一速度和當(dāng)前實(shí)際速度之間，這樣可以使速度很平滑變化，也可使車模收斂于最開始設(shè)定速度并在速度PID 控制下就可使小車勻速運(yùn)行了（速度 PID 見第 6 章介紹） 。例如設(shè)定車模速度為 4，第一次檢測(cè)車模實(shí)際速度為 5，通過 *4+5*=, 在 5 之間20 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)20且靠近 4，這樣通過第六章的速度 PID 調(diào)節(jié) PWM 后，電機(jī)會(huì)控制在 的速度，相當(dāng)于車模減速了，再一次計(jì)算 4*+*= 又一次接近 4，依次類推，可以多次后穩(wěn)定在 3，由 *4+3*=,3 也會(huì)收斂到 4.21 湖南工業(yè)大學(xué)本科畢業(yè)設(shè)計(jì) (論文)21第 6 章 PID 控制 PID 基本介紹PID 是本課題的核心，在第二章建立倒立擺模型分析出，要使小車平衡，就要加入反饋控制，PID 就是一種經(jīng)典的負(fù)反饋控制，同時(shí) PID 也是運(yùn)動(dòng)控制的核心。它可以對(duì)偏差信號(hào)通過比例、積分、微分使偏差信號(hào)逐步減少，從而達(dá)到目標(biāo)值。本文的角度目標(biāo)值處于垂直位置（0 度） ，速度也就是設(shè)定的值，通過 PID 可以調(diào)整電機(jī)正反轉(zhuǎn)和轉(zhuǎn)速動(dòng)態(tài)調(diào)整車模姿態(tài)在目標(biāo)位置。有模擬和數(shù)字 PID，其中模擬 PID 控制框圖如下：比 例 （ P )積 分 （ I )微 分 （ D )+ +E ( t )R （ t )Y ( t ) 圖 模擬 PID 控制框圖上圖 r(t)是給定值，y(t)是實(shí)際輸出值，e(t)是兩者偏差則 e(t)=r(t)y(t) （61）PID 控制方程如下 ()()()()0dettytkekpi???（62）Kp 為比例系數(shù)，ki 為積分系數(shù)，kd 為微分系數(shù)。從上式可以知道 PID 控制就是按偏差控制，e(t)越大控制越強(qiáng)。下面介紹一下 P、I、D 的控制特點(diǎn)。對(duì)于比例主要調(diào)節(jié)系統(tǒng)的動(dòng)態(tài)性能，值越大響應(yīng)速度越快，但太大會(huì)出現(xiàn)震蕩影響系統(tǒng)穩(wěn)定性，所以對(duì) kp 在穩(wěn)定的條件下盡量加大參