兩輪自平衡車解讀

1、 兩輪自平衡送餐車【摘要】本項目為“兩輪自平衡車送餐機器人”系統(tǒng)的研究與實現(xiàn)，從加速度計和陀螺儀傳感器得出的角度。運用卡爾曼濾波優(yōu)化，補償陀螺儀的漂移誤差和加速度計的動態(tài)誤差，得到更優(yōu)的傾角近似值。根據(jù)PID控制調(diào)節(jié)參數(shù)，實現(xiàn)兩輪直立行走。通過電磁傳感器對電磁線的檢測和GPS模塊精確定位，實現(xiàn)了平衡車的自動送餐功能?！娟P(guān)鍵字】：加速度計陀螺儀卡爾曼濾波PID控制調(diào)節(jié)電磁傳感器GPS模塊【Abstract】：ThisisaprojectofresearchandRealizationofatwowheeledselfbalancingrobotcarroomsystem,fromtheaccel

2、erometerandgyrosensorofangle.UsingtheCalmanfilteroptimization,thedynamicerrorofgyroscopedrifterrorandaccelerationcompensationplan,togetbetterapproximationsangle.AccordingtothePIDcontrolparameters,achievetwoupright.ThroughthedetectionandaccuratepositioningofGPSmoduleelectromagneticsensorsonthemagnetw

3、ire,thebalanceofthecarautomaticroomfunction.【Keyword】：saccelerometergyroscopeCalmanfilteringPIDcontrolelectromagneticsensorGPSmoduleTOC o 1-5 h z HYPERLINK l bookmark4 o Current Document 第一章引言2第二章基本原理32.1兩輪自平衡送餐車整體框架3 HYPERLINK l bookmark6 o Current Document 2.2送餐車直立控制3 HYPERLINK l bookmark8 o Curre

4、nt Document 2.3速度控制42.4方向控制5第三章硬件電路設(shè)計與實現(xiàn)53.1主控芯片5 HYPERLINK l bookmark10 o Current Document 3.2電機驅(qū)動方案5 HYPERLINK l bookmark14 o Current Document 3.3電磁信號處理電路6 HYPERLINK l bookmark16 o Current Document 3.4紅外遙控模塊6 HYPERLINK l bookmark18 o Current Document 第四章系統(tǒng)軟件控制流程圖7第五章卡爾曼濾波8 HYPERLINK l bookmark20 o

5、 Current Document 4.1卡爾曼濾波簡介8 HYPERLINK l bookmark22 o Current Document 4.2卡爾曼濾波實現(xiàn)的效果8 HYPERLINK l bookmark24 o Current Document 4.3卡爾曼濾波原型9 HYPERLINK l bookmark33 o Current Document 4.4卡爾曼濾波化簡10 HYPERLINK l bookmark45 o Current Document 4.5卡爾曼濾波參數(shù)調(diào)試12 HYPERLINK l bookmark47 o Current Document 5總結(jié)13

6、第一章引言兩輪自平衡送餐車具有運動靈活、智能控制、操作簡單等優(yōu)點。因此它適用于在狹小空間內(nèi)運行，可以穿梭于餐廳的任意的位置，也可以將自平衡電動車改裝成兩輪自平衡機器人。由于兩輪自平衡送餐車的兩輪結(jié)構(gòu)，使得它的重心在上、支點在下，故在非控制狀態(tài)（或靜態(tài)）下為一不穩(wěn)定系統(tǒng)。然而，可以利用倒立擺系統(tǒng)的控制原理，通過微處理器的控制使它能夠如倒立擺一樣穩(wěn)定在一個平衡位置處，并能在保持平衡的狀態(tài)下按照使用者的指令要求正常運行。兩輪自平衡送餐車實際上是一級直線式倒立擺和旋轉(zhuǎn)式倒立擺的結(jié)合體，它的控制原理與倒立擺系統(tǒng)的基本一致。更形象地說，自平衡送餐車的工作原理更像人行走的過程。對于人而言，當(dāng)人體的重心向前傾

7、斜并失去平衡時，人通過自身的感覺器官能夠察覺到自己身體的傾斜（角度），于是他會做出一個反應(yīng)一一向前邁出一步來防止自己摔倒在地上。如果身體一直前傾，為了保持平衡，人就會一步又一步地往前走。因此，如果將兩個由電機驅(qū)動的車輪看成人的雙腿，再加上能夠測量車體相對于水平面傾角大小和速度的傳感器，最后通過微處理器的控制便能夠?qū)崿F(xiàn)車體自平衡的效果。因而當(dāng)人站在車上時，只要將身體帶動車體一起往前傾（或后傾）就可以實現(xiàn)送餐車送餐前進(jìn)（或后退）。因此兩輪自平衡送餐車具有很大的開發(fā)意義，實現(xiàn)兩輪送餐車后還可以做很多的其他的項目，比如兩輪自平衡電動車等等。第二章基本原理2.1兩輪自平衡送餐車整體框架圖1系統(tǒng)框架圖2.

8、2送餐車直立控制世界上還沒有任何一個天才雜技演員可以蒙著眼睛使得木棒在自己指尖上直立，因為沒有了眼睛觀察進(jìn)行負(fù)反饋，當(dāng)木棒在人的指尖上直立時，眼睛觀察到木棒的傾斜角度和傾斜趨勢（角速度），相應(yīng)的通過人腦做出反應(yīng)，使手掌移動以抵消木棒的傾斜角度和趨勢，從而保持木棒的直立。這就形成了反饋機制。其反饋機制如圖2拭行川.購佇制訓(xùn)象麻棒的圖2反饋機制車模平衡控制也是通過負(fù)反饋來實現(xiàn)的，與上面保持木棒直立比較則相對簡單。因為車模有兩個輪子著地，車體只會在輪子滾動的方向上發(fā)生傾斜?？刂戚喿愚D(zhuǎn)動，抵消在一個維度上傾斜的趨勢便可以保持車體平衡了。如圖2.2所示。.車體垂直車輪保持O車體向右傾仏車輪向右加速適行.

9、23速度控制對于直立車模速度的控制相對于普通車模的速度控制則比較復(fù)雜。由于在速度控制過程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過改變電機轉(zhuǎn)速來實現(xiàn)。要實現(xiàn)對車模速度的控制，必須測量車模的實時速度，改變車模的傾角，以及如何根據(jù)速度誤差控制車模傾角。安裝在電機輸出軸上的光電編碼器來測量得到車模的車輪速度。通過給定車模直立控制的設(shè)定值，在角度控制調(diào)節(jié)下，車模將會自動維持在一個角度。在車模直立控制下，為了能夠有一個往前的傾斜角度，車輪需要往后運動，這樣會引起車輪速度下降（因為車輪往負(fù)方向運動了）。由于負(fù)反饋，使得車模往前傾角需要更大。如此循環(huán)，車模很快就會傾倒。原本利用負(fù)反饋進(jìn)行速度控

10、制反而成了“正”反饋。如圖4所示。牟輪速1.2圖4車模傾角控制速度中的正反饋車模的速度控制本質(zhì)上是通過調(diào)節(jié)車模的傾角實現(xiàn)的，由于車模是一個非最小相位系統(tǒng)，因此該反饋控制如果比例和速度過大，很容易形成正反饋，使得車模失控，造成系統(tǒng)的不穩(wěn)定性。因此速度的調(diào)節(jié)過程需要非常緩慢和平滑。24方向控制實現(xiàn)送餐車方向控制是保證車模沿著送餐道路的關(guān)鍵。直立送餐車所在的電磁組的道路中心線鋪設(shè)有一根漆包線，里面通有100mA的20kHz交變電流。因此在送餐道路中心線周圍產(chǎn)生一個交變磁場。通過道路電磁中心線偏差檢測與電機差動和GPS模塊實現(xiàn)方向控制，從而進(jìn)一步保證送餐路上。（1）電磁中心線的偏差檢測道路電磁中心線檢

11、測簡單的方法可以通過安裝在送餐車前方的兩個電磁感應(yīng)線圈實現(xiàn)。線圈一般采用10mH的工字型電感。（2）電機差動控制利用電磁線偏差檢測信號分別與送餐車速度控制信號進(jìn)行加和減，形成左右輪差動控制電壓，使得送餐車左右輪運行角速度不一致進(jìn)而控制車模方向。（3）GPS模塊根據(jù)GPS模塊上讀到精確的經(jīng)緯度值，實現(xiàn)坐標(biāo)的定位，從而實現(xiàn)自動送餐的功能。第三章硬件電路設(shè)計與實現(xiàn)31主控芯片32電機驅(qū)動方案我們采用BTS7960芯片作為電機驅(qū)動芯片。BTS7960通路內(nèi)阻為16m4，最大電流為43A，具有集成度高、外接電路少、內(nèi)阻小等特點，因此，我們采用4片BTS7960芯片組成全橋驅(qū)動電路進(jìn)行驅(qū)動。BTS7960

12、電路圖如圖5所示汨二扎5gnd-ii|干二L_I_II_IcrlbnINVSINHOCT；ISOCTISSLGKDUlV1汽TGKDGKD-ItarINVSINHOCT；瑁OCTIGKDLsfsDoesGKDTV1USINVSINHOCT；瑁OCTIR.GKDETSffiEU5R1JR廳廠打鏗，|1平HPTVITT3斗0送杞】|円|.GKD|-GND即-D曲C140才h【Eof_Rj歩GKDI”39+HC244INVSINHOCT；OCTIGKDETS9&3BCiiOJ-iFGKD舶乂至的-U広如肘巨巨111177717圖5BTS7960電路圖533電磁信號處理電路路線的導(dǎo)線中通有20KHz的

13、方波信號，通過檢測方波信號的強弱來判斷導(dǎo)線之于車模的相對位置，以此做出控制。為了檢測信號強弱，使用電感電容構(gòu)成諧振電路。電感使用10mH工字電感，電容使用6.4nF電容。圖6電磁檢波放大電路原理圖3.4紅外遙控模塊編碼格式1、0和1的編碼遙控器發(fā)射的信號由一串O和1的二進(jìn)制代碼組成.不同的芯片對0和1的編碼有所不同。通常有曼徹斯特編碼和脈沖寬度編碼。TC9012的O和1采用PWM方法編碼，即脈沖寬度調(diào)制，其O碼和1碼如圖1所示（以遙控接收輸出的波形為例）。O碼由O.56ms低電平和0.56ms高電平組合而成.脈沖寬度為1.12ms。1碼由0.56ms低電平和1.69ms高電平組合而成.脈沖寬度

14、為2.25ms。在編寫解碼程序時.通過判斷脈沖的寬度，即可得到0或1。按鍵的編碼當(dāng)我們按下遙控器的按鍵時，遙控器將發(fā)出如圖2的一串二進(jìn)制代碼，我們稱它為一幀數(shù)據(jù)。根據(jù)各部分的功能?？蓪⑺鼈兎譃?部分，分別為引導(dǎo)碼、地址碼、地址碼、數(shù)據(jù)碼、數(shù)據(jù)反碼。遙控器發(fā)射代碼時.均是低位在前，高位在后。分析可以得到.引導(dǎo)碼高電平為4.5ms，低電平為4.5ms。當(dāng)接收到此碼時.表示一幀數(shù)據(jù)的開始。單片機可以準(zhǔn)備接收下面的數(shù)據(jù)。地址碼由8位二進(jìn)制組成，共256種.地址碼重發(fā)了一次。主要是加強遙控器的可靠性.如果兩次地址碼不相同.則說明本幀數(shù)據(jù)有錯.應(yīng)丟棄。不同的設(shè)備可以擁有不同的地址碼.因此。同種編碼的遙控器

15、只要設(shè)置地址碼不同，也不會相互干擾。地址碼為十六進(jìn)制的OEH（注意低位在前）。在同一個遙控器中.所有按鍵發(fā)出的地址碼都是相同的。數(shù)據(jù)碼為8位，可編碼256種狀態(tài)，代表實際所按下的鍵。數(shù)據(jù)反碼是數(shù)據(jù)碼的各位求反，通過比較數(shù)據(jù)碼與數(shù)據(jù)反碼.可判斷接收到的數(shù)據(jù)是否正確。如果數(shù)據(jù)碼與數(shù)據(jù)反碼之間的關(guān)系不滿足相反的關(guān)系.則本次遙控接收有誤.數(shù)據(jù)應(yīng)丟棄。在同一個遙控器上，所有按鍵的數(shù)據(jù)碼均不相同。數(shù)據(jù)碼為十六進(jìn)制的OCH,數(shù)據(jù)反碼為十六進(jìn)制的0F3H（注意低位在前）.兩者之和應(yīng)為OFFH。單片機遙控接收電路紅外遙控接收可采用較早的紅外接收二極管加專用的紅外處理電路的方法。如CXA20106，電路復(fù)雜，現(xiàn)在

16、一般不采用。較好的接收方法是用一體化紅外接收頭，它將紅外接收二極管、放大、解調(diào)、整形等電路做在一起，只有三個引腳.分別是5V電源、地、信號輸出。紅外接收頭的信號輸出接單片機的INTO或INT1腳，增加了一只PNP型三極管對輸出信號進(jìn)行放大。第四章系統(tǒng)軟件控制流程圖如圖7所示，為系統(tǒng)軟件整體結(jié)構(gòu)框圖，主要包括：系統(tǒng)參數(shù)的手動調(diào)節(jié);電磁傳感器；角度傳感器和陀螺儀模塊；路徑控制模塊；電機控制模塊。躍忡控制摸塊圖7系統(tǒng)軟件作流程圖系統(tǒng)接通電源后，經(jīng)初始化；通過系統(tǒng)參數(shù)的手動調(diào)節(jié)，使系統(tǒng)能夠按照準(zhǔn)確的設(shè)想方式運行；然后通過電磁傳感器采集數(shù)據(jù)和角度傳感器及陀螺儀對車模與地面的角度及角速度角度的感知；經(jīng)過相

17、應(yīng)處理得出當(dāng)前送餐路徑和送餐車的信息；同時，電機測速模塊測得模型車當(dāng)前的運行速度，反饋給系統(tǒng)；最后，經(jīng)吋L控吊.叮馬 路徑控制系統(tǒng)綜合當(dāng)前賽道路徑信息和送餐車速度值作出相應(yīng)的處理，來控制電機的運行。第五章卡爾曼濾波41卡爾曼濾波簡介近年來被應(yīng)用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等?？柭鼮V波是一種高效率的遞歸濾波器（自回歸濾波器）,它能夠從一系列的不完全及包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)。簡單來說，卡爾曼濾波器是一個“optimalrecursivedataprocessingalgorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。對于解決很大部分的問題，他是最優(yōu)，效率最高

18、甚至是最有用的。他的廣泛應(yīng)用已經(jīng)超過30年，包括機器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等，本次用于兩輪自平衡車很好的實現(xiàn)了功能。42卡爾曼濾波實現(xiàn)的效果當(dāng)決定使用加速度計（反應(yīng)角度）以及陀螺儀（反應(yīng)角速度）以后，我們對參考的論文中為何要同時使用加速度計以及陀螺儀產(chǎn)生過一些疑問。因為如果需要角速度，我們可以通過這樣的方法對加速度計進(jìn)行微分，計算出角速度，或者可以通過對于陀螺儀積分，從而獲得角度。既然如此為何我們需要使用兩個傳感器。于是，我們對兩個傳感器進(jìn)行了數(shù)據(jù)采樣，觀測其輸出信號的關(guān)系。由于考慮電機轉(zhuǎn)動產(chǎn)生的震動會對傳感器產(chǎn)生比較大的干擾，因此我們測試了不同PW

19、M占空比下傳感器輸出，并用MATLAB繪制曲線，并進(jìn)行相關(guān)運算。陀螺儀的數(shù)據(jù)測試：10095908580757065-60-圖9PWM巧0%占空比下，車模直立狀態(tài)下的陀螺儀輸出圖10PWM100%占空比下，車模直立狀態(tài)下的陀螺儀輸出圖11PWM占空比與陀螺儀方差關(guān)系總結(jié)：隨著PWM占空比的不斷提高，陀螺儀的噪聲也更加顯著。4.3卡爾曼濾波原型首先我們建立系統(tǒng)的狀態(tài)方程和測量方程，由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實角度可以用來做一個狀態(tài)向量。在該系統(tǒng)中，采用加速度傳感器估計出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測方程算。 0-1+3+3gb_00丿b_0_gy

20、ro0_aacce=10式中，為包含固定偏差的陀螺儀輸出角速度，CP為加速度計經(jīng)處理后得到gyroacce的角度值，為陀螺儀測量噪聲，3為加速度傳感器測量噪聲，b為陀螺儀漂ga移誤差，和3相互獨立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。令T為gas系統(tǒng)采樣周期，得到離散系統(tǒng)的狀態(tài)方程和測量方程：r1-TT3(k)TX(k)=0sX(k-1)+s3(k1)+gs1J0gyro_0_V(k)=1i0X(k)+3(k)a同時，要估算k時刻的實際角度，就必須根據(jù)k-1時刻的角度值，再根據(jù)預(yù)測得到的k時刻的角度值得到k時刻的高斯噪聲的方差，在此基礎(chǔ)之上卡爾曼濾波器進(jìn)行遞歸運算直至估算出最優(yōu)的角度值。在此

21、，須知道系統(tǒng)過程噪聲協(xié)方差陣Q以及測量誤差的協(xié)方差矩陣R,對卡爾曼濾波器進(jìn)行校正，Q與R矩陣的形式如下：(q0Q=_0cceR二rISgyro丿-式中。q_acce和q_gyro分別是加速度傳感器和陀螺儀測量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度，數(shù)值越小表明信任程度越高。在該系統(tǒng)中陀螺儀的值更為接近準(zhǔn)確值，因此取q_gyro的值小于q_acce的值。當(dāng)前狀態(tài)：X(kIk-1)二AX(k-11k-1)+BU(k)(1)4.4卡爾曼濾波化簡X-A*X+B*gyro10P=A*P*A+Q10Kg=P*H/H*P*H*+R11X=X+Kg*(acc-H*X)011P=(I-Kg*H

22、)*P01上述五個公式便是卡爾曼濾波的五條數(shù)學(xué)公式在本項目中的使用。但是，以上五個公式,僅僅只是矩陣形式。雖然可以在MATLAB里進(jìn)行仿真，但是卻不能使用單片機進(jìn)行有效運 X11=X01+dt*gyro-dt*X02X=X1202P=Pdt*Pdt*(Pdt*P1101030204P=Pdt*P120204P=Pdt*P130304P=P+Q14Kg)+Qi1然后，將卡爾曼濾波矩陣形式轉(zhuǎn)化為方程形式:2(/(P+R)1111KgP01P02P03P04X01X021/(P+R)TOC o 1-5 h z1311P11P12P13P14X11X12(1-Kg)1(1-Kg)1-P*Kg112-P

23、*Kg)*Kg1)*Kg112122+(accX11+(accX121：這里，X01即為我們需要的角度卡爾曼濾波值?？梢钥吹降氖牵柭鼮V波方程形式共有14個公式，同時很多參數(shù)的運算涉及浮點數(shù)，這對于單片機的高效運行時極為不利的，因此我們需要通過一些方法對卡爾曼濾波的公式進(jìn)行化簡，從而在不失精度的情況下，盡可能減小運算量。需要注意的是，Q,R兩個參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會產(chǎn)生相應(yīng)變化，對應(yīng)到我們的系統(tǒng)，在車模運行狀態(tài)不同(傾角不同，PWM不同)情況下，Q,R都是不同的。根據(jù)相關(guān)參考文獻(xiàn)，Q,R雖然都是關(guān)于時間的變量，但是由于卡爾曼濾波有很好的收斂性，所以可以將

24、Q,R都取比較極端的參數(shù)。用常量來定義。然后再看下圖召=X01旳*兀X=Xqj好=局-加*妗3-出*(02-出*弘)+Q&二Ra-滬Ra好3=P3-酬備耳4=熱+02附認(rèn)他+R)”K引二耳/(盡+R)“鳥話/(IK%!堆=召3-打1*直104=4_冬*住2Xm=Xn+(acc-XnrKglX燉=Xl2+陽_産11)化2注意方框中的公式，根據(jù)我們的觀察，不難發(fā)現(xiàn)，整個方框中都是為了獲得卡爾曼增益(矩陣Kg),我們設(shè)想，能否使用一個常數(shù)來等效替代卡爾曼增益那？根據(jù)我們在實驗中的觀察，卡爾曼增益是一個收斂的變量，并且針對到我們的這個系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個常數(shù)。(所有這些工作都在MA

25、TLAB下完成)所以，我們將方框中的所有公式完全省略，通過實驗整定，選取一個近似Kg來替代方框中的所有運算。同時，通過NATLAB觀測各變量的變化趨勢，我們嘗試讓Kg2=0（Kgl,Kg2本身就是非常小的變量，所以可以讓其等于0）。并發(fā)現(xiàn)Kg2對于整個卡爾曼濾波的影響非常有限。所以將Kg2設(shè)定為0。以下就是我們組程序中卡爾曼濾波的簡化算法實現(xiàn)Xl=X0+gyro*dt;X0=Xl+（acc-Xl）*kg;化簡后的卡爾曼濾波框圖如下：4.5卡爾曼濾波參數(shù)調(diào)試化簡后的卡爾曼濾波主要是對dt以及Kg兩個參數(shù)進(jìn)行整定。需要特別指出的是，這里的dt并不只是一個采樣間隔。由于陀螺儀的輸出和加速度計輸出的量綱并不相同，所以陀螺儀采樣值dt并不直接反應(yīng)一個角度，而是與實際角度相差一個系數(shù)。因此此處的dt可以等效理解為dt=采樣間隔*比例系數(shù)。dt越大,積分速度越快，卡爾曼輸出追隨實際角度的情況越好（當(dāng)然不能太大，不然可會出現(xiàn)超前相位）。但是dt越大，陀螺儀漂移造成的影響也就越大。Kg決定了加速度計的權(quán)重。Kg越大，實