最近有很多網友在問，關于用單片機控制的自平衡小車制作的問題，其實這在飛思卡爾智能車比賽的時候，清華的卓晴老師的那篇文檔里面說的很清楚，很多沒參加比賽的人不知道或者不重視這個文檔，我用自己的理解來寫一個總結希望對大家有幫助，大神勿拍磚。
首先確定一下我們的目標是什么？我們得讓車站起來，小車整個身體只有電機可以控制輪子，自然對小車的控制就落在了對單片機程序對電機的控制上。很多參加比賽的朋友由于車模是組委會發的，沒去想過什么樣的電機是合適的？ 自然啟動會快，反映越快的直流電機最合適。對于直流電機的控制調速，大家都知道最常用的是脈寬調制即PWM方式，這時候就得選好電機驅動芯片了，跟你的電機匹配，電流要扛的住。再一個，把小車想成一個騎獨輪車的雜技演員，維持平衡，你的保證能前后轉吧，所以驅動得做成H橋型的，其電路圖可以在http://www.raoushi.com 找到,總結一下我們第一個目標就是做好驅動電路，可以實現對電機的正反兩個方向的大概調速控制（注意是大概，無反饋調節，如果想試試PID也可以先做一下速度反饋調節）。
第二個問題，稍微有點控制思想的就應該明白，小車是自平衡，自然是自己在控制，那它控制的依據是什么？根據什么去控制？對于兩輪小車當它倒下去的時候，只有一個維度的兩個量，那就是和直立位置的角度值和在倒下去的時候的快慢值（角速度），其實他倆是一個量，因為對角速度積分就是角度值。角速度只是反映了倒下去的快慢。即變化量。好比你拿一根筷子直立在手指上，你看見它要倒下去的時候肯定手會跟著移動，你一方面看到的是筷子倒下去的角度，另一個是筷子倒的時候的快慢，角度大速度快你自然移動的快，幅度也大，角度小速度慢，你自然移動的慢幅度也小。以上純腦袋想的，從科學的嚴謹性來說，還是去看卓老大的文檔，從建立數學模型，到自控原理的計算分析最后得出來。所以第二個目標出來了，我們要去實時測出小車偏離直立位置的角度，這個其實是自平衡小車的第一個難點。
我們說測角度一般使用加速度計就可以了，加速度計是分為模擬的和數字的兩種，都是可以用的，只是在實際情況中，加速度計測量的角度是不準確的，因為在小車運動過程中存在震動加速度，這會使輸出值不準確，不能真實反映小車的偏轉角度。這是器件本身的問題，有些人說用簡單的數字濾波（中值、均值等），這些濾波是濾除的干擾信號，這本身的錯誤信號怎么慮出？再來考慮另一個器件 陀螺儀，我們知道陀螺儀是測量角速度的，但是角速度轉換為角度是需要一個積分過程，假如在輸入時有一個極小的誤差，那么隨著積分這個誤差將會越來越大，最后得出的角度自然也是不準確的。（器件的使用是最基本的，希望大家能把這兩個傳感器的使用先搞明白，明白角度具體是怎么計算出來的？）這個時候才有我們常說的卡爾曼濾波、互補濾波的登場，很多人在制作過程中總是覺得卡爾曼或者互補濾波很高端的東西，視線全被它們蒙蔽了，實際上它們最終的目的任然是得到最準確的角度偏離值。對于這一目標，傳感器的性能、電路設計同樣是很重要的。至于卡爾曼濾波和互補濾波的優劣不在這討論，我們只說說用的較多的卡爾曼濾波，卡爾曼的前世今生大家在網上搜一下，千篇一律，沒一個說到點上了的。其實作為應用我們只需知道卡爾曼輸入的兩個量，一個是測量值，一個是預測值，程序都是成型的，重點還是在參數的調試上。整個算法中影響輸出的就是Kg的值，可以簡單的理解為一種加權行為，相信誰更多一點而已。
代碼如下：說明：簡化版卡爾曼濾波單片機c語言程序
volatile float QingJiao = 0; //最終準確角度輸出變量定義
volatile float Gyro_Data = 0; //陀螺儀
float Q =1,R =3900; //調整卡爾曼的滯后 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 ; //用卡爾曼濾波時不用此變量
int Gyro1_zero=0;
void kalman_update(void)
{
if(zeroflag>1000) //與開機自檢有關，沒用到的可以刪去
{ zeroflag=1001; //確保zeroflag不會溢出
//-------------------------------------------------------------------------------------------------------------------------------
Acc_z = Acc_z - 28850; //加速度計采集的AD值減去直立時的輸出值
Gyro1_zero=zerosub/1000; //陀螺儀開機自檢累加1000次后取均值 得到陀螺儀零偏值
Gyro1 = Gyro1 - Gyro1_zero; //陀螺儀AD采集值減去陀螺儀零偏值
Gyro_Data = Gyro1;
accelerometer_angle= Acc_z*180/(47915.71-12843.7); //加速度計計算出的角度 歸一化到-90 到+90
gyroscope_rate = Gyro1*0.0235*0.005; //0.0235 是轉換角度的比例值 0.005是控制周期
gyroscope_rat =gyroscope_rat -Gyro1*0.0235*0.005; //積分角速度得到角度
//卡爾曼五個公式的算法實現
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; //將準確角度結果給QingJiao
}
}
整個調試過程有三個參數需要調整，Q R 及那個0.0235 。具體的調試這個真是說不清楚，往往算法的調試都是經驗，嘗試多了就有規律了。個人感覺做到這，就得用去三分之二時間。
好了，假如你已經得到準確角度，自然是開始以此作為控制量，那我們要控制成啥樣？想一想也知道是要把這個角度值控制成0度（我自己將直立時定義為0度），那么自然使用常用的PID算法，偏差自然就是QingJiao-0=QingJiao,當然你可以反過來，這其實根據你自己對方向的定義。我們來個最簡單的位置式PD算法：
fValue = (float) P *QingJiao -(float) D*Gyro_Data;
P就是PID的P參數 D就是PID的D參數，QingJiao反映幅度，Gyro_Data反映快慢。這也是需要不斷調試出來的。再把fvalue值給PWM輸出就可以了。實際在做的時候，往往沒那么簡單，所以一定要一步一步做好之后再做后面的，假如你第二部沒做好，在第三部時你怎么也直立不起來，你不知道到底是PD參數不對，還是卡爾曼出來的角度本身不準。所以個人感覺，得到準確角度是整個過程至關重要的一步。小車的直立是一直是一個動態過程，即使最好的狀態一動不動，也是在動態控制中，只是看不出而已。
以上只針對直立控制，即最基本的自平衡。