最近有很多網(wǎng)友在問，關(guān)于用單片機(jī)控制的自平衡小車制作的問題，其實(shí)這在飛思卡爾智能車比賽的時(shí)候，清華的卓晴老師的那篇文檔里面說的很清楚，很多沒參加比賽的人不知道或者不重視這個(gè)文檔，我用自己的理解來寫一個(gè)總結(jié)希望對(duì)大家有幫助，大神勿拍磚。
首先確定一下我們的目標(biāo)是什么？我們得讓車站起來，小車整個(gè)身體只有電機(jī)可以控制輪子，自然對(duì)小車的控制就落在了對(duì)單片機(jī)程序?qū)﹄姍C(jī)的控制上。很多參加比賽的朋友由于車模是組委會(huì)發(fā)的，沒去想過什么樣的電機(jī)是合適的？ 自然啟動(dòng)會(huì)快，反映越快的直流電機(jī)最合適。對(duì)于直流電機(jī)的控制調(diào)速，大家都知道最常用的是脈寬調(diào)制即PWM方式，這時(shí)候就得選好電機(jī)驅(qū)動(dòng)芯片了，跟你的電機(jī)匹配，電流要扛的住。再一個(gè)，把小車想成一個(gè)騎獨(dú)輪車的雜技演員，維持平衡，你的保證能前后轉(zhuǎn)吧，所以驅(qū)動(dòng)得做成H橋型的，其電路圖可以在http://www.zg4o1577.cn 找到,總結(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í)候的快慢值（角速度），其實(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í)測(cè)出小車偏離直立位置的角度，這個(gè)其實(shí)是自平衡小車的第一個(gè)難點(diǎn)。
我們說測(cè)角度一般使用加速度計(jì)就可以了，加速度計(jì)是分為模擬的和數(shù)字的兩種，都是可以用的，只是在實(shí)際情況中，加速度計(jì)測(cè)量的角度是不準(zhǔn)確的，因?yàn)樵谛≤囘\(yùn)動(dòng)過程中存在震動(dòng)加速度，這會(huì)使輸出值不準(zhǔn)確，不能真實(shí)反映小車的偏轉(zhuǎn)角度。這是器件本身的問題，有些人說用簡(jiǎn)單的數(shù)字濾波（中值、均值等），這些濾波是濾除的干擾信號(hào)，這本身的錯(cuò)誤信號(hào)怎么慮出？再來考慮另一個(gè)器件 陀螺儀，我們知道陀螺儀是測(cè)量角速度的，但是角速度轉(zhuǎn)換為角度是需要一個(gè)積分過程，假如在輸入時(shí)有一個(gè)極小的誤差，那么隨著積分這個(gè)誤差將會(huì)越來越大，最后得出的角度自然也是不準(zhǔn)確的。（器件的使用是最基本的，希望大家能把這兩個(gè)傳感器的使用先搞明白，明白角度具體是怎么計(jì)算出來的？）這個(gè)時(shí)候才有我們常說的卡爾曼濾波、互補(bǔ)濾波的登場(chǎng)，很多人在制作過程中總是覺得卡爾曼或者互補(bǔ)濾波很高端的東西，視線全被它們蒙蔽了，實(shí)際上它們最終的目的任然是得到最準(zhǔn)確的角度偏離值。對(duì)于這一目標(biāo)，傳感器的性能、電路設(shè)計(jì)同樣是很重要的。至于卡爾曼濾波和互補(bǔ)濾波的優(yōu)劣不在這討論，我們只說說用的較多的卡爾曼濾波，卡爾曼的前世今生大家在網(wǎng)上搜一下，千篇一律，沒一個(gè)說到點(diǎn)上了的。其實(shí)作為應(yīng)用我們只需知道卡爾曼輸入的兩個(gè)量，一個(gè)是測(cè)量值，一個(gè)是預(yù)測(cè)值，程序都是成型的，重點(diǎn)還是在參數(shù)的調(diào)試上。整個(gè)算法中影響輸出的就是Kg的值，可以簡(jiǎn)單的理解為一種加權(quán)行為，相信誰更多一點(diǎn)而已。
代碼如下：說明：簡(jiǎn)化版卡爾曼濾波單片機(jī)c語言程序
volatile float QingJiao = 0; //最終準(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 ; //用卡爾曼濾波時(shí)不用此變量
int Gyro1_zero=0;
void kalman_update(void)
{
if(zeroflag>1000) //與開機(jī)自檢有關(guān)，沒用到的可以刪去
{ zeroflag=1001; //確保zeroflag不會(huì)溢出
//-------------------------------------------------------------------------------------------------------------------------------
Acc_z = Acc_z - 28850; //加速度計(jì)采集的AD值減去直立時(shí)的輸出值
Gyro1_zero=zerosub/1000; //陀螺儀開機(jī)自檢累加1000次后取均值 得到陀螺儀零偏值
Gyro1 = Gyro1 - Gyro1_zero; //陀螺儀AD采集值減去陀螺儀零偏值
Gyro_Data = Gyro1;
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 -Gyro1*0.0235*0.005; //積分角速度得到角度
//卡爾曼五個(gè)公式的算法實(shí)現(xiàn)
NowData = RealData -gyroscope_rate;
NowData_P = Q+RealData_P;
Kg = NowData_P/(NowData_P+R);
RealData = NowData + Kg*(accelerometer_angle - NowData);
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è)最簡(jiǎn)單的位置式PD算法：
fValue = (float) P *QingJiao -(float) D*Gyro_Data;
P就是PID的P參數(shù) D就是PID的D參數(shù)，QingJiao反映幅度，Gyro_Data反映快慢。這也是需要不斷調(diào)試出來的。再把fvalue值給PWM輸出就可以了。實(shí)際在做的時(shí)候，往往沒那么簡(jiǎn)單，所以一定要一步一步做好之后再做后面的，假如你第二部沒做好，在第三部時(shí)你怎么也直立不起來，你不知道到底是PD參數(shù)不對(duì)，還是卡爾曼出來的角度本身不準(zhǔn)。所以個(gè)人感覺，得到準(zhǔn)確角度是整個(gè)過程至關(guān)重要的一步。小車的直立是一直是一個(gè)動(dòng)態(tài)過程，即使最好的狀態(tài)一動(dòng)不動(dòng)，也是在動(dòng)態(tài)控制中，只是看不出而已。
以上只針對(duì)直立控制，即最基本的自平衡。