單片機(jī)控制的自平衡小車設(shè)計(jì)探討

首先確定一下我們的目標(biāo)是什么？我們得讓車站起來，小車整個身體只有電機(jī)可以控制輪子，自然對小車的控制就落在了對單片機(jī)程序?qū)﹄姍C(jī)的控制上。很多參加比賽的朋友由于車模是組委會發(fā)的，沒去想過什么樣的電機(jī)是合適的？ 自然啟動會快，反映越快的直流電機(jī)最合適。對于直流電機(jī)的控制調(diào)速，大家都知道最常用的是脈寬調(diào)制即PWM方式，這時(shí)候就得選好電機(jī)驅(qū)動芯片了，跟你的電機(jī)匹配，電流要扛的住。再一個，把小車想成一個騎獨(dú)輪車的雜技演員，維持平衡，你的保證能前后轉(zhuǎn)吧，所以驅(qū)動得做成H橋型的，其電路圖可以在http://www.51hei.com找到,總結(jié)一下我們第一個目標(biāo)就是做好驅(qū)動電路，可以實(shí)現(xiàn)對電機(jī)的正反兩個方向的大概調(diào)速控制（注意是大概，無反饋調(diào)節(jié)，如果想試試PID也可以先做一下速度反饋調(diào)節(jié)）。

第二個問題，稍微有點(diǎn)控制思想的就應(yīng)該明白，小車是自平衡，自然是自己在控制，那它控制的依據(jù)是什么？根據(jù)什么去控制？對于兩輪小車當(dāng)它倒下去的時(shí)候，只有一個維度的兩個量，那就是和直立位置的角度值和在倒下去的時(shí)候的快慢值（角速度），其實(shí)他倆是一個量，因?yàn)閷撬俣确e分就是角度值。角速度只是反映了倒下去的快慢。即變化量。好比你拿一根筷子直立在手指上，你看見它要倒下去的時(shí)候肯定手會跟著移動，你一方面看到的是筷子倒下去的角度，另一個是筷子倒的時(shí)候的快慢，角度大速度快你自然移動的快，幅度也大，角度小速度慢，你自然移動的慢幅度也小。以上純腦袋想的，從科學(xué)的嚴(yán)謹(jǐn)性來說，還是去看卓老大的文檔，從建立數(shù)學(xué)模型，到自控原理的計(jì)算分析最后得出來。所以第二個目標(biāo)出來了，我們要去實(shí)時(shí)測出小車偏離直立位置的角度，這個其實(shí)是自平衡小車的第一個難點(diǎn)。

我們說測角度一般使用加速度計(jì)就可以了，加速度計(jì)是分為模擬的和數(shù)字的兩種，都是可以用的，只是在實(shí)際情況中，加速度計(jì)測量的角度是不準(zhǔn)確的，因?yàn)樵谛≤囘\(yùn)動過程中存在震動加速度，這會使輸出值不準(zhǔn)確，不能真實(shí)反映小車的偏轉(zhuǎn)角度。這是器件本身的問題，有些人說用簡單的數(shù)字濾波（中值、均值等），這些濾波是濾除的干擾信號，這本身的錯誤信號怎么慮出？再來考慮另一個器件 陀螺儀，我們知道陀螺儀是測量角速度的，但是角速度轉(zhuǎn)換為角度是需要一個積分過程，假如在輸入時(shí)有一個極小的誤差，那么隨著積分這個誤差將會越來越大，最后得出的角度自然也是不準(zhǔn)確的。（器件的使用是最基本的，希望大家能把這兩個傳感器的使用先搞明白，明白角度具體是怎么計(jì)算出來的？）這個時(shí)候才有我們常說的卡爾曼濾波、互補(bǔ)濾波的登場，很多人在制作過程中總是覺得卡爾曼或者互補(bǔ)濾波很高端的東西，視線全被它們蒙蔽了，實(shí)際上它們最終的目的任然是得到最準(zhǔn)確的角度偏離值。對于這一目標(biāo)，傳感器的性能、電路設(shè)計(jì)同樣是很重要的。至于卡爾曼濾波和互補(bǔ)濾波的優(yōu)劣不在這討論，我們只說說用的較多的卡爾曼濾波，卡爾曼的前世今生大家在網(wǎng)上搜一下，千篇一律，沒一個說到點(diǎn)上了的。其實(shí)作為應(yīng)用我們只需知道卡爾曼輸入的兩個量，一個是測量值，一個是預(yù)測值，程序都是成型的，重點(diǎn)還是在參數(shù)的調(diào)試上。整個算法中影響輸出的就是Kg的值，可以簡單的理解為一種加權(quán)行為，相信誰更多一點(diǎ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不會溢出

//-------------------------------------------------------------------------------------------------------------------------------

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; //積分角速度得到角度

//卡爾曼五個公式的算法實(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

}

}

整個調(diào)試過程有三個參數(shù)需要調(diào)整，Q R 及那個0.0235 。具體的調(diào)試這個真是說不清楚，往往算法的調(diào)試都是經(jīng)驗(yàn)，嘗試多了就有規(guī)律了。個人感覺做到這，就得用去三分之二時(shí)間。

好了，假如你已經(jīng)得到準(zhǔn)確角度，自然是開始以此作為控制量，那我們要控制成啥樣？想一想也知道是要把這個角度值控制成0度（我自己將直立時(shí)定義為0度），那么自然使用常用的PID算法，偏差自然就是QingJiao-0=QingJiao,當(dāng)然你可以反過來，這其實(shí)根據(jù)你自己對方向的定義。我們來個最簡單的位置式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í)候，往往沒那么簡單，所以一定要一步一步做好之后再做后面的，假如你第二部沒做好，在第三部時(shí)你怎么也直立不起來，你不知道到底是PD參數(shù)不對，還是卡爾曼出來的角度本身不準(zhǔn)。所以個人感覺，得到準(zhǔn)確角度是整個過程至關(guān)重要的一步。小車的直立是一直是一個動態(tài)過程，即使最好的狀態(tài)一動不動，也是在動態(tài)控制中，只是看不出而已。

以上只針對直立控制，即最基本的自平衡。