【正文】 一個“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。他的廣泛應(yīng)用已經(jīng)超過30年，包括機器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。當(dāng)決定使用加速度計（反應(yīng)角度）以及陀螺儀（反應(yīng)角速度）以后，我們對參考的論文中為何要同時使用加速度計以及陀螺儀產(chǎn)生過一些疑問。既然如此為何我們需要使用兩個傳感器。由于考慮電機轉(zhuǎn)動產(chǎn)生的震動會對傳感器產(chǎn)生比較大的干擾， 隨著PWM占空比的不斷提高，陀螺儀的噪聲也更加顯著。我們不難獲得以下一些結(jié)論：陀螺儀的噪聲還在可以接受范圍，但是加速度計噪聲已經(jīng)遠(yuǎn)遠(yuǎn)超出可控范圍接受對于加速度計做一個補充說明，加速度計在車模直立狀態(tài)下，電機PWM占空比100%噪聲情況下，幅值映射到實際角度大約是60度左右。方差（可以等效理解為噪聲大?。╇S著占空比不斷增加。陀螺儀的噪聲比較小，但是他的誤差會對積分造成漂移。簡而言之：加速度計短時間內(nèi)不可信，但是長時間內(nèi)可信。這也就是為什么車模直立必須使用兩個傳感器來完成信號獲取。于是，我們需要一種算法，能夠在短時間內(nèi)置信陀螺儀，而長時間內(nèi)置信加速度計的算法。首先我們建立系統(tǒng)的狀態(tài)方程和測量方程，由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實角度可以用來做一個狀態(tài)向量。令為系統(tǒng)采樣周期，得到離散系統(tǒng)的狀態(tài)方程和測量方程：同時，要估算k時刻的實際角度，就必須根據(jù)k1時刻的角度值，再根據(jù)預(yù)測得到的k時刻的角度值得到k時刻的高斯噪聲的方差，在此基礎(chǔ)之上卡爾曼濾波器進(jìn)行遞歸運算直至估算出最優(yōu)的角度值。和分別是加速度傳感器和陀螺儀測量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度，數(shù)值越小表明信任程度越高。當(dāng)前狀態(tài): （1）式中，是利用k預(yù)測的結(jié)果，是k 1 時刻的最優(yōu)結(jié)果。則狀態(tài)k的最優(yōu)化估算值： （3）其中,為卡爾曼增益： （4）此時，我們已經(jīng)得到了k狀態(tài)下最有的估算值，但是為了使卡爾曼濾波器不斷的運行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差： （5）其中為單位陣，對于本系統(tǒng)則有。（3）、（4）、（5）式為卡爾曼濾波器狀態(tài)更新方程。上述五個公式便是卡爾曼濾波的五條數(shù)學(xué)公式在本項目中的使用。雖然可以在MATLAB里進(jìn)行仿真，但是卻不能使用單片機進(jìn)行有效運算?？梢钥吹降氖?，卡爾曼濾波方程形式共有14個公式，同時很多參數(shù)的運算涉及浮點數(shù)，這對于單片機的高效運行時極為不利的，因此我們需要通過一些方法對卡爾曼濾波的公式進(jìn)行化簡，從而在不失精度的情況下，盡可能減小運算量。根據(jù)相關(guān)參考文獻(xiàn)，Q,R雖然都是關(guān)于時間的變量，但是由于卡爾曼濾波有很好的收斂性，所以可以將Q，R都取比較極端的參數(shù)。然后再看下圖注意方框中的公式，根據(jù)我們的觀察，不難發(fā)現(xiàn)，整個方框中都是為了獲得卡爾曼增益（矩陣Kg），我們設(shè)想，能否使用一個常數(shù)來等效替代卡爾曼增益那？根據(jù)我們在實驗中的觀察，卡爾曼增益是一個收斂的變量，并且針對到我們的這個系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個常數(shù)。同時，通過NATLAB觀測各變量的變化趨勢，我們嘗試讓Kg2=0（Kg1，Kg2本身就是非常小的變量，所以可以讓其等于0）。所以將Kg2設(shè)定為0。X0=X1+(accX1)*kg。需要特別指出的是，這里的dt并不只是一個采樣間隔。因此此處的dt可以等效理解為dt=采樣間隔*比例系數(shù)。但是dt越大，陀螺儀漂移造成的影響也就越大。Kg越大，實際輸出的漂移就越小，但是濾波效果的噪聲也就越大。首先是靜態(tài)整定。目標(biāo)是達(dá)到上圖效果。如果紅色抖動非常厲害，可以適當(dāng)減小Kg的大小。大致調(diào)節(jié)到上圖所示的樣子。解決方法，逐漸增加dt即可。第六章 程序控制說明在完成了信號處理的層面之后，就是對于控制算法的設(shè)計。系統(tǒng)雖然是高階非線性系統(tǒng)，但是由于直立期間，傾斜角度非常小，所以可等等效為一個線性系統(tǒng)。被控變量有兩個：角度以及車輪轉(zhuǎn)速。同時，無論是控制角度還是控制電機，執(zhí)行機構(gòu)都為電機，所以系統(tǒng)也具有強耦合性。這些算法都需要更高深的理論基礎(chǔ)，同時，在高階倒立擺系統(tǒng)中，這些算法擁有不可替代性。我們在本設(shè)計中使用了角度、速度雙閉環(huán)的控制算法。由于摩擦力等因素的存在，電機與PWM給定信號存在非線性死去特性。首先我們忽略速度控制，僅僅只考慮角度閉環(huán)系統(tǒng)。那么就可以使用PID控制器。雖然角度和角速度是不同的量，但是角速度就是對角度求微分。常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。因此最后我們選擇了PD控制器。這是由于未加速速度閉環(huán)的結(jié)果，未對車輪轉(zhuǎn)速進(jìn)行必要的控制，則最終由于電機轉(zhuǎn)速的飽和，而發(fā)生了無法對倒立擺進(jìn)行調(diào)整的后果。、速度雙閉環(huán)控制對于直立車模速度的控制相對于普通車模的速度控制則比較復(fù)雜。下面先分析一下引起車模速度變化的原因。在重力的作用下，車模就會朝傾斜的方向加速前進(jìn)。假設(shè)車模開始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。由于負(fù)反饋，使得車模往前傾角需要更大。原本利用負(fù)反饋進(jìn)行速度控制反而成了“正”反饋。但根據(jù)實際經(jīng)驗，是可以通過速度控制進(jìn)行車模傾角控制的。所以使用PID控制器來完成控制。正如前文所述，直立控制和速度控制存在強耦合，所以這里我們使用的分立控制周期的方法。這樣，可以將速度控制視為對于直立控制的一種擾動。 直立、速度閉環(huán)控制框圖第七章車模參數(shù)調(diào)試圖解在完成了所有控制算法方案的制定，以及程序代碼的編寫之后，接下來需要對一些參數(shù)在實際系統(tǒng)運行中進(jìn)行整定以及微調(diào)。本章節(jié)著重介紹在調(diào)試環(huán)節(jié)中不同現(xiàn)象對應(yīng)的原因以及參數(shù)調(diào)整方法。dt:陀螺儀積分參數(shù)Kg：加速度計參數(shù)（卡爾曼增益）角度閉環(huán)，速度開環(huán)P：直立算法P參數(shù)角度閉環(huán)，速度開環(huán)D：直立算法D參數(shù)速度閉環(huán)P：速度控制P參數(shù)速度閉環(huán)I：速度控制I參數(shù)原因：開環(huán)P過大，系統(tǒng)震蕩解決方法：減小開環(huán)P或者增加開環(huán)D原因：開環(huán)D參數(shù)過大，系統(tǒng)抗干擾性降低解決方法：減小開環(huán)D，或者適當(dāng)增加開環(huán)P原因：傾角值為0時刻，車身重心不在軸線上解決方法：微調(diào)加速度計零偏置，使得角度為0時，重心落在軸線上。原因：陀螺儀積分速度過慢，導(dǎo)致卡爾曼濾波相位滯后解決方法：適當(dāng)增加dt原因：速度閉環(huán)對系統(tǒng)整體的控制程度過小解決方法：適當(dāng)增加速度閉環(huán)的各個參數(shù)原因：速度閉環(huán)調(diào)節(jié)速度過慢解決方法：適當(dāng)增加閉環(huán)P參數(shù)原因：速度閉環(huán)存在靜態(tài)誤差解決方法：適當(dāng)增加閉環(huán)I參數(shù)原因：速度閉環(huán)調(diào)節(jié)速度過慢解決方法：適當(dāng)增加閉環(huán)P參數(shù)，或者也可以使用PID（而非PI）進(jìn)行速度閉環(huán)的控制原因：速度控制對于階躍響應(yīng)存在超調(diào)解決方法：減小閉環(huán)P參數(shù)，或者加入閉環(huán)D參數(shù)，也可以不用階躍信號，而是通過斜波信號慢慢將速度提升。解決方法：增大Kg。 //PORTB作為輸入 DDRA = 0X00。 DDRE_DDRE5 = 1。 DDRE_DDRE3 = 1。 PORTE_PE5 = 0。 PORTE_PE6 = 0。 PORTE_PE5 = 1。 PORTE_PE6 = 0。 //全部禁止上拉電阻 DDRK_DDRK2 =1。 //電機IO接口 PORTK_PK2 =0。 //***********CCD接口初始化 DDRK_DDRK5 =1。 //***********數(shù)碼管接口電路 DDRM = 0XFF。 }//*****************設(shè)置系統(tǒng)時鐘為64MHzvoid SystemCLK_64M(void){ CLKSEL = 0。 //打開PLL SYNR = 0Xc7。fpll=fvco/(2*POSTDIV)。若POSTDIV=0,fbus=fvco/2。 //fbus = 2*16*(9+1)/(1+1)=80m。 //fpll = fvco。 //_asm匯編語言，nop做空操作；延時 _asm(nop)。 //頻率穩(wěn)定后再操作 CLKSEL_PLLSEL = 1。 // PIT disabled, PITCE_PCE0 = 1。 //8位，16位定時器共同作用 PITMTLD0 = 79。 //16位定時器初值設(shè)定 time_out period = (79+1)*(799+1)/64000000 = 1ms PITINTE_PINTE0 = 1。 //定時器使能,初始化一次即可}/************************************************功能 :總線頻率為64MHz,設(shè)置SCI0模塊波特率為19200bps入口參數(shù):無出口參數(shù):無 40000/1152****************************************************/void SCI0_init(void){ //SCI0BD = 208。 //設(shè)置波特率為19200Kbps(寫一個字) AMAP = 0 總線頻率32MHz //SCI0BD = 208。 //設(shè)置波特率為57600Kbps(寫一個字) AMAP = 0 總線頻率64MHz //SCI0BD = 35。 //設(shè)置波特率為38400Kbps(寫一個字) AMAP = 0 總線頻率64MHz //SCI0BD = 139。 //SCI0工作在正常情況下,禁止奇偶校正,使用8為數(shù)據(jù)位(共10位) SCI0CR2 = 0X2c。 for(i=0。i++) { while(!SCI0SR1_TDRE)。 //發(fā)送數(shù)據(jù) }}/****************************************************功能 :初始化定時器模塊(TIM模塊輸入撲捉功能初始化) Ic0上升沿 , Ic1輸入撲捉下降沿入口參數(shù):無出口參數(shù):無******************************************************/void Time_init_CCD(void){ //ok() TSCR1 |= 0X90。 //禁止定時溢出中斷定時器時鐘頻率為4MHz //定時器頻率為總線頻率的8分頻 TIOS = 0Xf0。 //IC0,IC2上升沿 , IC1,IC3輸入撲捉下降沿 TIE = 0X00。 PWMPOL = 0XCf。 //PWM0,1選擇ClockA作為時鐘,PWM2,3選擇ClockB作為時鐘 PWMPRCLK = 0X00。 //全部使用左對齊 PWMCTL = 0X30。 //PWM頻率為4KHz PWMPER23 = 15000。 //占空比為0 PWMDTY23 = 0。 //SB的分頻因子為40 ClockSB=1600KHz PWMPER7 = 10。 //左對齊,PWM6通道頻率為160KHz PWMDTY7 =5。 //設(shè)置占空比為50% PWME = 0X0f。 unsigned char resulut = ATD_single_send_uchar()。 //全部使用模擬輸入 ATD0CTL1 = 0X00。 //A/D模塊快速清零,禁止外部觸發(fā),禁止中斷 ATD0CTL3 = 0X88。 //采樣時間為ATD時鐘周期的4倍,ATD時鐘為系統(tǒng)時鐘的10分頻 ATD0CTL5 = 0X20。 //等待轉(zhuǎn)換結(jié)束 return (ATD0DR0)。 for(j=0。j++) _asm(nop)。 //數(shù)碼管的表 PTM = ShuMa[i]。 0x40) PTJ_PTJ6 =1。}//*********中斷代碼執(zhí)行選擇標(biāo)志 unsigned char state_flag=0。 //采樣數(shù)據(jù)保存的數(shù)組 unsigned char uchr_ad_data2[133]。 //靜態(tài)采集點 int icount。 //CCD數(shù)據(jù)采集完成發(fā)送標(biāo)志 unsigned char Sendflag2。 extern unsigned char runflag。 //停車保護(hù)標(biāo)志//****************硬件初始化部分 v