小車直立基礎(chǔ)

1、精選優(yōu)質(zhì)文檔-傾情為你奉上    最近有很多網(wǎng)友在問，關(guān)于自平衡小車制作的問題，其實(shí)這在飛思卡爾智能車比賽的時(shí)候，清華的卓晴老師的那篇文檔里面說的很清楚，很多沒參加比賽的人不知道或者不重視這個(gè)文檔，我用自己的理解來寫一個(gè)總結(jié)希望對(duì)大家有幫助，大神勿拍磚。         首先確定一下我們的目標(biāo)是什么？我們得讓車站起來，小車整個(gè)身體只有電機(jī)可以控制輪子，自然對(duì)小車的控制就落在了對(duì)電機(jī)的控制上。很多參加比賽的朋友由于車模是組委會(huì)發(fā)的，沒去想過什么樣的電機(jī)是合適的？ 自然啟動(dòng)會(huì)快，反映越快的

2、直流電機(jī)最合適。對(duì)于直流電機(jī)的控制調(diào)速，大家都知道最常用的是脈寬調(diào)制即PWM方式，這時(shí)候就得選好電機(jī)驅(qū)動(dòng)芯片了，跟你的電機(jī)匹配，電流要扛的住。再一個(gè)，把小車想成一個(gè)騎獨(dú)輪車的雜技演員，維持平衡，你的保證能前后轉(zhuǎn)吧，所以驅(qū)動(dòng)得做成H橋型的?？偨Y(jié)一下我們第一個(gè)目標(biāo)就是做好驅(qū)動(dòng)電路，可以實(shí)現(xiàn)對(duì)電機(jī)的正反兩個(gè)方向的大概調(diào)速控制（注意是大概，無反饋調(diào)節(jié)，如果想試試PID也可以先做一下速度反饋調(diào)節(jié)）。第二個(gè)問題，稍微有點(diǎn)控制思想的就應(yīng)該明白，小車是自平衡，自然是自己在控制，那它控制的依據(jù)是什么？根據(jù)什么去控制？對(duì)于兩輪小車當(dāng)它倒下去的時(shí)候，只有一個(gè)維度的兩個(gè)量，那就是和直立位置的角度值和在倒下去的時(shí)候的快

3、慢值（角速度），其實(shí)他倆是一個(gè)量，因?yàn)閷?duì)角速度積分就是角度值。角速度只是反映了倒下去的快慢。即變化量。好比你拿一根筷子直立在手指上，你看見它要倒下去的時(shí)候肯定手會(huì)跟著移動(dòng)，你一方面看到的是筷子倒下去的角度，另一個(gè)是筷子倒的時(shí)候的快慢，角度大速度快你自然移動(dòng)的快，幅度也大，角度小速度慢，你自然移動(dòng)的慢幅度也小。以上純腦袋想的，從科學(xué)的嚴(yán)謹(jǐn)性來說，還是去看卓老大的文檔，從建立數(shù)學(xué)模型，到自控原理的計(jì)算分析最后得出來。所以第二個(gè)目標(biāo)出來了，我們要去實(shí)時(shí)測出小車偏離直立位置的角度，這個(gè)其實(shí)是自平衡小車的第一個(gè)難點(diǎn)。我們說測角度一般使用加速度計(jì)就可以了，加速度計(jì)是分為模擬的和數(shù)字的兩種，都是可以用的，只

4、是在實(shí)際情況中，加速度計(jì)測量的角度是不準(zhǔn)確的，因?yàn)樵谛≤囘\(yùn)動(dòng)過程中存在震動(dòng)加速度，這會(huì)使輸出值不準(zhǔn)確，不能真實(shí)反映小車的偏轉(zhuǎn)角度。這是器件本身的問題，有些人說用簡單的數(shù)字濾波（中值、均值等），這些濾波是濾除的干擾信號(hào)，這本身的錯(cuò)誤信號(hào)怎么慮出？再來考慮另一個(gè)器件 陀螺儀，我們知道陀螺儀是測量角速度的，但是角速度轉(zhuǎn)換為角度是需要一個(gè)積分過程，假如在輸入時(shí)有一個(gè)極小的誤差，那么隨著積分這個(gè)誤差將會(huì)越來越大，最后得出的角度自然也是不準(zhǔn)確的。（器件的使用是最基本的，希望大家能把這兩個(gè)傳感器的使用先搞明白，明白角度具體是怎么計(jì)算出來的？）這個(gè)時(shí)候才有我們常說的卡爾曼濾波、互補(bǔ)濾波的登場，很多人

5、在制作過程中總是覺得卡爾曼或者互補(bǔ)濾波很高端的東西，視線全被它們蒙蔽了，實(shí)際上它們最終的目的任然是得到最準(zhǔn)確的角度偏離值。對(duì)于這一目標(biāo)，傳感器的性能、電路設(shè)計(jì)同樣是很重要的。至于卡爾曼濾波和互補(bǔ)濾波的優(yōu)劣不在這討論，我們只說說用的較多的卡爾曼濾波，卡爾曼的前世今生大家在網(wǎng)上搜一下，千篇一律，沒一個(gè)說到點(diǎn)上了的。其實(shí)作為應(yīng)用我們只需知道卡爾曼輸入的兩個(gè)量，一個(gè)是測量值，一個(gè)是預(yù)測值，程序都是成型的，重點(diǎn)還是在參數(shù)的調(diào)試上。整個(gè)算法中影響輸出的就是Kg的值，可以簡單的理解為一種加權(quán)行為，相信誰更多一點(diǎn)而已。代碼如下：說明：簡化版卡爾曼濾波volatile float QingJiao = 0;&#

6、160; /最終準(zhǔn)確角度輸出變量定義volatile float Gyro_Data = 0; /陀螺儀float Q =1,R =3900;    /調(diào)整卡爾曼的滯后 3900  static float RealData = 0,RealData_P =10000;float NowData = 0,NowData_P =0 ;float Kg = 0,gyroscope_rate = 0,gyroscope_rat = 0,accelerometer_angle;volatile float gyroscope_angle=0&

7、#160;                                                      /用卡爾曼濾波時(shí)不用此變量int  Gyro1_zero=0;void kalman_update(void)&

8、#160;  if(zeroflag>1000)                                       /與開機(jī)自檢有關(guān)，沒用到的可以刪去                  

9、;        zeroflag=1001;                                        /確保zeroflag不會(huì)溢出/-           Acc_z = Acc_z

10、- 28850;      /加速度計(jì)采集的AD值減去直立時(shí)的輸出值                       Gyro1_zero=zerosub/1000;   /陀螺儀開機(jī)自檢累加1000次后取均值 得到陀螺儀零偏值                 &

11、#160;    Gyro1  = Gyro1  - Gyro1_zero;             /陀螺儀AD采集值減去陀螺儀零偏值                    Gyro_Data = Gyro1;            &#

12、160;    accelerometer_angle=    Acc_z*180/(47915.71-12843.7);    /加速度計(jì)計(jì)算出的角度 歸一化到-90 到+90     gyroscope_rate = Gyro1*0.0235*0.005;  /0.0235 是轉(zhuǎn)換角度的比例值 0.005是控制周期      gyroscope_rat =gyroscope_rat

13、-Gyro1*0.0235*0.005;     /積分角速度得到角度/卡爾曼五個(gè)公式的算法實(shí)現(xiàn)                                                     

14、0;                            NowData = RealData -gyroscope_rate;    NowData_P = Q+RealData_P;    Kg = NowData_P/(NowData_P+R);    RealData = NowData + Kg*(accelerometer_angle - No

15、wData);    RealData_P = (1-Kg)*NowData_P;      QingJiao =  RealData;    /將準(zhǔn)確角度結(jié)果給QingJiao        整個(gè)調(diào)試過程有三個(gè)參數(shù)需要調(diào)整，Q  R  及那個(gè)0.0235 。具體的調(diào)試這個(gè)真是說不清楚，往往算法的調(diào)試都是經(jīng)驗(yàn)，嘗試多了就有規(guī)律了。個(gè)人感覺做到這，就得用去三分之二時(shí)間。  好了，假如你已經(jīng)得到準(zhǔn)確角度，自然是開始以此作為控制量，那我們要控制成啥樣？想一想也知道是要把這個(gè)角度值控制成0度（我自己將直立時(shí)定義為0度），那么自然使用常用的PID算法，偏差自然就是QingJiao-0=QingJiao,當(dāng)然你可以反過來，這其實(shí)根據(jù)你自己對(duì)方向的定義。我們來個(gè)最簡單的位置式PD算法：fValue = (float) P *QingJiao -(float) D*Gyro_Data;     P就是PID的P參數(shù) D就是PID的D