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