【正文】 數(shù):數(shù)值出口參數(shù):無******************************************************/void Disp(unsigned char i){ unsigned char ShuMa[]={0x3f,0x06,0x5b,0x4F,0x66,0x6d,0x7d,0x07, 0x7F,0x6f}。j6。 //返回轉(zhuǎn)換結(jié)果 }/***********************************************************函數(shù)功能:在系統(tǒng)頻率為64MHz下,延時約1us入口參數(shù):無出口參數(shù):無************************************************************/void delay_1us(void){ int j。 //連續(xù)轉(zhuǎn)換,單獨對某通道采樣 ,并啟動轉(zhuǎn)換具體通道需要在調(diào)用它之前} //end of ATD0_Init/*****************************************************函數(shù)功能:轉(zhuǎn)換一個模擬量8位轉(zhuǎn)換入口參數(shù): unsigned char result出口參數(shù): unsigned char result 啟動單次裝換******************************************************/unsigned char ATD_uchar_convert(void) { while(!ATD0STAT2L_CCF0)。 //A/D轉(zhuǎn)換結(jié)果右對齊,每個序列1個通道,非FIFO ATD0CTL4 = 0X04。 //采樣前不放電,0x02分辨率為12位 0x00分辨率為8位 0x01分辨率為10位 ATD0CTL2 = 0X40。采樣AD0通道的數(shù)據(jù),單次轉(zhuǎn)換. ************************************************************/void ATD_init(void){ ATD0DIEN = 0X00。 //允許PWM0,1,2,3}/******************************************************* 函數(shù)功能:初始化AD轉(zhuǎn)換,轉(zhuǎn)換位數(shù)8位,轉(zhuǎn)換的通道必須在調(diào)用函數(shù)之前設(shè)置 入口參數(shù):無 ATD的 fclk 出口參數(shù):無 使用方法: ATD0CTL5 = 0X00。 //設(shè)置占空比為50% PWMDTY6 =5。 //左對齊,PWM7通道頻率為160KHz PWMPER6 = 10。 PWMSCLB = 20。 PWMDTY01 = 0。 //使用 PWM0,PWM1級聯(lián),PWM2,PWM3級聯(lián) PWMPER01 = 15000。 //ClockA,ClockB對總線頻率0分頻64MHz PWMCAE = 0X00。 //開始輸出極性為高電平 PWMCLK = 0XC0。 //禁止輸出撲捉申請中斷}/*****************************************************函數(shù)功能:對PWM初始化,設(shè)置PWM0,PWM1級聯(lián),PWM2,PWM3級聯(lián)入口參數(shù):無出口參數(shù):無******************************************************/void PWM_init(void){ PWME = 0X00。 //啟用IC0,IC1,IC2,IC3輸入撲捉功能 TCTL4 = 0X99。 //啟動主定時器模塊,使用快速清零(TFFCA=1) TSCR2 |= 0X03。 //等待發(fā)送寄存器空 SCI0DRL = num[i]。i133。 //允許使用發(fā)送器,使用接收器,允許接收中斷}/***************************************************函數(shù)功能:完成帶符號的char型數(shù)據(jù)傳送入口參數(shù):char num,為要傳送的數(shù)據(jù)出口參數(shù):無說明 :每次傳送133個數(shù)據(jù)上位機定義的一幀數(shù)據(jù)****************************************************/void SCI0_Char_Send(unsigned char num[]){ unsigned int i =0。 //設(shè)置波特率為28800Kbps(寫一個字) AMAP = 0 總線頻率64MHz SCI0CR1 = 0X00。 //設(shè)置波特率為115200Kbps(寫一個字) AMAP = 0 總線頻率64MHz SCI0BD = 104。 //設(shè)置波特率為19200Kbps(寫一個字) AMAP = 0 總線頻率64MHz //SCI0BD = 69。 //設(shè)置波特率為9600Kbps(寫一個字) AMAP = 0 //SCI0BD = 104。 //0通道中斷有效 PITCFLMT_PITE=1。 //8位定時器初值設(shè)定，80 PITLD0 =799。 //通道0使能 PITMUX_PMUX = 0。}//******************* PIT中斷服務(wù)void PIT_Init(void) //1ms周期中斷{ PITCFLMT = 0。 while(CRGFLG_LOCK == 0)。 _asm(nop)。 POSTDIV = 0。 REFDV = 0X81。 //fbus=fpll/2。 //設(shè)置 fvc0=2*fosc*(SYNDIV+1)/(REFDIV+1)。 //使PLL與系統(tǒng)隔離 PLLCTL_PLLON = 1。 //輸出 DDRJ_DDRJ6 =1。 //PORTK_PK1,PORTK_PK3輸出 DDRK_DDRK4 =1。 PORTK_PK3 =0。 //電機IO接口 DDRK_DDRK3 =1。 //***********************電機控制電路接口初始化 ok() PUCR = 0X50。 PORTE_PE3 = 0。 //***********計數(shù)使能 PORTE_PE2 = 1。 PORTE_PE3 = 0。 //***********計數(shù)器清零 PORTE_PE2 = 0。 DDRE_DDRE6 = 1。 DDRE_DDRE2 = 1。附件源代碼//******************端口初始化void PAC_Init(void){ //**********計數(shù)器接口初始化 ok() DDRB = 0X00。原因：陀螺儀漂移之后和加速度傳感器之間的關(guān)系發(fā)生了變化，導(dǎo)致濾波角度出現(xiàn)比較大的問題。也可能是陀螺儀零偏置錯誤，同理，微調(diào)即可。在下文中，會使用如下幾個變量在此說明。整定方法遵從先直立控制再速度控制；先P參數(shù)，后I參數(shù)，最后D參數(shù)的整定順序。下圖為速度閉環(huán)的控制框圖。每10個直立控制周期進行一次速度控制。由于系統(tǒng)對于靜態(tài)誤差存在一定要求，所以使用PI控制器。而速度閉環(huán)控制仍然可以看做是一個SISO系統(tǒng)。在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。如此循環(huán)，車模很快就會傾倒。在車模直立控制下，為了能夠有一個往前的傾斜角度，車輪需要往后運動，這樣會引起車輪速度下降（因為車輪往負(fù)方向運動了）。首先對一個簡單例子進行分析。假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實際測量的角度與車模角度有偏差，因此車模實際不是保持與地面垂直，而是存在一個傾角。由于在速度控制過程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過改變電機轉(zhuǎn)速來實現(xiàn)。為了解決這個問題，必須要引入速度閉環(huán)控制。控制系統(tǒng)框圖如下：在實際調(diào)試過程中，倒立擺可以進行短時間的直立，但是會朝向一個方向加速運行，最后車模會倒下。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對于這個需要快速調(diào)整的系統(tǒng)，要求不大。而使用陀螺儀獲得的角速度比對卡爾曼濾波后求微分獲得的角速度更準(zhǔn)確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒有問題。這里需要進行解釋，為何有兩個輸入變量任然可以使用SISO系統(tǒng)才能使用PID。那么在這個前提下，只有兩個速度量：角度和角速度；被控變量只有角度。為了使系統(tǒng)具有更好的線性，首先需要對電機死區(qū)進行補償。角度控制頻率高，速度控制周期頻率慢。但是對于我們的兩輪車式倒立擺，這僅僅只是一個一階倒立擺，可以使用更簡單的方法進行控制。對于倒立擺的控制，參考的文獻中有使用各種算法進行控制，比如狀態(tài)反饋、自適應(yīng)算法、神經(jīng)網(wǎng)絡(luò)算法、模糊算法等等。所以這是一個MIMO系統(tǒng)，因此不能使用簡單的PID等SISO控制器進行控制。控制輸入有三個，分別對應(yīng)角度，角速度，車輪轉(zhuǎn)速。在設(shè)計算法之前，讓我們對于系統(tǒng)進行一個簡單分析。達到上圖效果即可認(rèn)為卡爾曼濾波參數(shù)整定完成。目前存在一個相位差，這個相位差在后期直立算法的調(diào)試中是致命的，必須克服掉。接著是動態(tài)整定還是保持車輪恒定PWM旋轉(zhuǎn)，同時，搖擺車身。紅色為卡爾曼濾波輸出，藍色為加速度計直接輸出。將車模保持在穩(wěn)定直立狀態(tài)，讓車輪以恒定PWM（80%以上）轉(zhuǎn)動，然后調(diào)節(jié)參數(shù)。所以dt和kg是一對矛盾，不能太過于極端。Kg決定了加速度計的權(quán)重。dt越大，積分速度越快，卡爾曼輸出追隨實際角度的情況越好（當(dāng)然不能太大，不然可能會出現(xiàn)超前相位）。由于陀螺儀的輸出和加速度計輸出的量綱并不相同，所以陀螺儀采樣值*dt并不直接反應(yīng)一個角度，而是與實際角度相差一個系數(shù)?；喓蟮目柭鼮V波框圖如下：化簡后的卡爾曼濾波主要是對dt以及Kg兩個參數(shù)進行整定。以下就是我們組程序中卡爾曼濾波的簡化算法實現(xiàn)X1=X0+gyro*dt。并發(fā)現(xiàn)Kg2對于整個卡爾曼濾波的影響非常有限。（所有這些工作都在MATLAB下完成）所以，我們將方框中的所有公式完全省略，通過實驗整定，選取一個近似Kg來替代方框中的所有運算。用常量來定義。需要注意的是，Q,R兩個參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會產(chǎn)生相應(yīng)變化，對應(yīng)到我們的系統(tǒng)，在車模運行狀態(tài)不同（傾角不同，PWM不同）情況下，Q,R都是不同的。然后，將卡爾曼濾波矩陣形式轉(zhuǎn)化為方程形式：這里，X01即為我們需要的角度卡爾曼濾波值。但是，以上五個公式，僅僅只是矩陣形式。計算完時間更新方程后，再重復(fù)上一次計算得到的后驗估計，作為下一次的計算的先驗估計，這樣，周而復(fù)始、循環(huán)往復(fù)地運算下去直至找到最優(yōu)的結(jié)果。當(dāng)系統(tǒng)進入k+1狀態(tài)時，就是式子（2）的。則有對應(yīng)于的協(xié)方差為： （2）式中，是對應(yīng)的協(xié)方差，表示的轉(zhuǎn)置矩陣，是系統(tǒng)過程的協(xié)方差，式子（1）、（2）即對系統(tǒng)的狀態(tài)更新。在該系統(tǒng)中陀螺儀的值更為接近準(zhǔn)確值，因此取的值小于的值。在此,須知道系統(tǒng)過程噪聲協(xié)方差陣Q以及測量誤差的協(xié)方差矩陣R, 對卡爾曼濾波器進行校正，Q與R矩陣的形式如下：式中。在該系統(tǒng)中，采用加速度傳感器估計出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測方程式中，為包含固定偏差的陀螺儀輸出角速度，為加速度計經(jīng)處理后得到的角度值，為陀螺儀測量噪聲，為加速度傳感器測量噪聲，為陀螺儀漂移誤差，和相互獨立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。這就需要卡爾曼濾波。因為單個傳感器不可靠，只有取其精華去其糟粕，發(fā)揮兩個傳感器的共同優(yōu)勢，才能夠取得好的效果。陀螺儀短時間內(nèi)可信而長時間內(nèi)不可信。無論是加速度計還是陀螺儀，都有自己的優(yōu)勢和劣勢。加速度計雖然噪聲很大，但是長時間來看，他的數(shù)學(xué)期望是非常穩(wěn)定的。根本不能直接使用。之后我們對加速度傳感器進行了測試。于是，我們對兩個傳感器進行了數(shù)據(jù)采樣，觀測其輸出信號的關(guān)系。因為如果需要角速度，我們可以通過這樣的方法對加速度計進行微分，計算出角速度，或者可以通過對于陀螺儀積分，從而獲得角度。近年來更被應(yīng)用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。對于解決很大部分的問題，他是最優(yōu)，效率最高甚至是最有用的。第五章 卡爾曼濾波卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)。經(jīng)過綜合比較和測試，選用方案三。方案三：背面橫向安裝方案三的優(yōu)勢在于將電池重心降到最低，