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