【正文】 ){ CLKSEL = 0。 // PIT disabled, PITCE_PCE0 = 1。 for(i=0。 //PWM頻率為4KHz PWMPER23 = 15000。 //采樣時間為ATD時鐘周期的4倍,ATD時鐘為系統(tǒng)時鐘的10分頻 ATD0CTL5 = 0X20。 //靜態(tài)采集點 int icount。 //速度控制中的比例控制,用于反饋比例反饋速度差信號//float SPEED_CONTROL_D =53。 //左計數(shù)器清零 g_chrRightPulse = (char)PORTB。 // 加速，加速 //} inDelta = (CAR_SPEED_SET g_inCarSpeed)。float g_inAngleOut。 leftMotor = duty + g_inDirControlOut。 } }/****************************************************************程序功能:采集CCD1數(shù)據(jù),通過上位機生成圖像 下面鏡頭入口參數(shù):無出口參數(shù):unsigned char uchr_ccd_data(128)共128個數(shù)據(jù),其性質(zhì)為全局數(shù)組電氣連接:PWM7CCD_CLK PORTE_PE2CCD_SI CCD_AOAN10 850us (OK)******************************************************************/define CCD_SI PORTK_PK4 //定義CCD操作接口void CCD_Collect(void){ //*********定義發(fā)送數(shù)據(jù)數(shù)組,并初始化起始位 int i=0。 //等待IC0撲捉到CLK的上升沿 x = TC0。 x= TC1。 int x。 //等待IC1撲捉到CLK的下降沿 ATD0CTL5 = 0X0B。 Sendflag2 = 0。 //短暫延時 CCD_SI2 = 0。 //等待IC1撲捉到CLK的下降沿 x= TC1。i=128。 //等待IC0撲捉到CLK的上升沿 x = TC0。 } if(leftMotor =0) //車模向后傾斜,角度為正,電機反轉(zhuǎn) { PORTK_PK2 = 0。 g_inAngleOut =(CAR_ANGLE_SET g_fCarAngle) * ANGLE_CONTROL_P + (CAR_ANGLESPEED_SET kalmanangle_dot) * ANGLE_CONTROL_D。 //控制次數(shù)增加一次 if(g_inSpeedperiod==20) { g_inSpeedperiod=0。 g_chrleftPulsesum=0。/*************************************************************** ok____5月4日函數(shù)功能:獲取電機脈沖數(shù)入口參數(shù):無讀取計數(shù)器的值出口參數(shù):無g_inMotorPulse 正反計數(shù)脈沖總數(shù)小于128說明 :********************************************************************/ void GetMotorPulse(void) //1ms { g_chrleftPulse = (char)PORTA。 //PAC 初始化 PIT_Init()。}//*********中斷代碼執(zhí)行選擇標志 unsigned char state_flag=0。 //全部使用模擬輸入 ATD0CTL1 = 0X00。 //PWM0,1選擇ClockA作為時鐘,PWM2,3選擇ClockB作為時鐘 PWMPRCLK = 0X00。 //設(shè)置波特率為38400Kbps(寫一個字) AMAP = 0 總線頻率64MHz //SCI0BD = 139。 //_asm匯編語言，nop做空操作；延時 _asm(nop)。 //***********CCD接口初始化 DDRK_DDRK5 =1。 DDRE_DDRE5 = 1。正如前文所述，直立控制和速度控制存在強耦合，所以這里我們使用的分立控制周期的方法。、速度雙閉環(huán)控制對于直立車模速度的控制相對于普通車模的速度控制則比較復雜。我們在本設(shè)計中使用了角度、速度雙閉環(huán)的控制算法。如果紅色抖動非常厲害，可以適當減小Kg的大小。所以將Kg2設(shè)定為0。則狀態(tài)k的最優(yōu)化估算值： （3）其中,為卡爾曼增益： （4）此時，我們已經(jīng)得到了k狀態(tài)下最有的估算值，但是為了使卡爾曼濾波器不斷的運行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差： （5）其中為單位陣，對于本系統(tǒng)則有。陀螺儀的噪聲比較小，但是他的誤差會對積分造成漂移。第四章 車模硬件電路設(shè)計詳見原理圖。通過選型，決定使用歐姆龍E6A2CW3C雙向增量式測速編碼器。原本利用負反饋進行速度控制反而成了“正”反饋。常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。施加在電機上一個階躍電壓，電機的速度變化曲線為(（24）式中，E為電壓；為單位階躍函數(shù)；為慣性環(huán)節(jié)時間常數(shù)；為電機轉(zhuǎn)速常數(shù)。此外，為了使得倒立擺能夠盡快地在垂直位置穩(wěn)定下來，還需要增加阻尼力，與偏角的速度成正比，方向相反?？諝獾淖枘崃εc單擺運行速度成正比，方向相反。通過手掌移動抵消木棒的傾斜角度和趨勢，從而保持木棒的直立。為了分析方便，根據(jù)比賽規(guī)則，假設(shè)維持車模直立、運行的動力都來自于車模的兩個后車輪，后輪轉(zhuǎn)動由兩個直流電機驅(qū)動。因此在速度、方向控制的時候，應(yīng)該盡量平滑，以減少對于直立控制的干擾。下面通過對比單擺模型來說明保持車模穩(wěn)定的控制規(guī)律。圖 在車輪上參照系中車體受力分析倒立擺之所以不能像單擺一樣可以穩(wěn)定在垂直位置，就是因為在它偏離平衡位置的時候，所受到的回復力與位移方向相同，而不是相反！因此，倒立擺便會加速偏離垂直位置，直到倒下。（2）通過電機速度控制，實現(xiàn)車模恒速運行和靜止。為了使系統(tǒng)具有更好的線性，首先需要對電機死區(qū)進行補償。假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實際測量的角度與車模角度有偏差，因此車模實際不是保持與地面垂直，而是存在一個傾角。下圖為速度閉環(huán)的控制框圖。缺點在于電池高度過高，導致重心整體偏高。于是，我們對兩個傳感器進行了數(shù)據(jù)采樣，觀測其輸出信號的關(guān)系。在該系統(tǒng)中，采用加速度傳感器估計出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測方程式中，為包含固定偏差的陀螺儀輸出角速度，為加速度計經(jīng)處理后得到的角度值，為陀螺儀測量噪聲，為加速度傳感器測量噪聲，為陀螺儀漂移誤差，和相互獨立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。需要注意的是，Q,R兩個參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會產(chǎn)生相應(yīng)變化，對應(yīng)到我們的系統(tǒng)，在車模運行狀態(tài)不同（傾角不同，PWM不同）情況下，Q,R都是不同的。Kg決定了加速度計的權(quán)重。控制輸入有三個，分別對應(yīng)角度，角速度，車輪轉(zhuǎn)速。而使用陀螺儀獲得的角速度比對卡爾曼濾波后求微分獲得的角速度更準確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒有問題。如此循環(huán)，車模很快就會傾倒。也可能是陀螺儀零偏置錯誤，同理，微調(diào)即可。 PORTE_PE3 = 0。 //fbus=fpll/2。 //0通道中斷有效 PITCFLMT_PITE=1。 //啟動主定時器模塊,使用快速清零(TFFCA=1) TSCR2 |= 0X03。 //左對齊,PWM7通道頻率為160KHz PWMPER6 = 10。j6。 unsigned char PWMflag =0。float g_inCarSpeed=0。 //左計數(shù)器使能 Rightcounter_S0=1。 if(g_inSpeedIntegral =Speed_Motor_Min) g_inSpeedIntegral = Speed_Motor_Min。void stand_control(void) { ATD0CTL5 = 0X00。 else rightMotor =rightMotor DEATH_CONTROL。 //清除標志位 while(!TFLG1_C1F)。 //清除標志位 delay_1us()。 //清除標志位 while(!TFLG1_C0F)。 while(!TFLG1_C3F)。 //清除標志位 while(!TFLG1_C3F)。 //清除標志位 while(!TFLG1_C2F)。 //清除標志位 delay_1us()。 //清除標志位 while(!TFLG1_C1F)。 //開始采集pixel的值 while(!TFLG1_C0F)。 //清除標志位 while(!TFLG1_C0F)。 leftMotor =0。 //得到角速度99陀螺儀靜態(tài)點采集的數(shù)據(jù) ATD0CTL5 = 0X08。 //保存上一次速度控制量 g_inSpeedOutNew = ( D + g_inSpeedIntegral)。 //右計數(shù)器使能 g_chrleftPulsesum = g_chrleftPulsesum + (int)g_chrleftPulse。 //設(shè)定速度float g_inSpeedIntegral=0。 //時鐘初始化 ok SCI0_init()。}/******************************************************函數(shù)功能:數(shù)碼管顯示入口參數(shù):數(shù)值出口參數(shù):無******************************************************/void Disp(unsigned char i){ unsigned char ShuMa[]={0x3f,0x06,0x5b,0x4F,0x66,0x6d,0x7d,0x07, 0x7F,0x6f}。 //設(shè)置占空比為50% PWMDTY6 =5。 //啟用IC0,IC1,IC2,IC3輸入撲捉功能 TCTL4 = 0X99。 //設(shè)置波特率為9600Kbps(寫一個字) AMAP = 0 //SCI0BD = 104。 REFDV = 0X81。 //***********************電機控制電路接口初始化 ok() PUCR = 0X50。原因：陀螺儀漂移之后和加速度傳感器之間的關(guān)系發(fā)生了變化，導致濾波角度出現(xiàn)比較大的問題。在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對于這個需要快速調(diào)整的系統(tǒng)，要求不大。所以這是一個MIMO系統(tǒng)，因此不能使用簡單的PID等SISO控制器進行控制。所以dt和kg是一對矛盾，不能太過于極端。用常量來定義。在此,須知道系統(tǒng)過程噪聲協(xié)方差陣Q以及測量誤差的協(xié)方差矩陣R, 對卡爾曼濾波器進行校正，Q與R矩陣的形式如下：式中。之后我們對加速度傳感器進行了測試。方案二的優(yōu)勢在于轉(zhuǎn)向慣量較小，轉(zhuǎn)向靈活，重心低于方案一。兩部分的連接可以采用彈性連接或者剛性連接。首先