【正文】 nSpeedIntegral=0。 Leftcounter_S1=0。 //右計(jì)數(shù)器使能 g_chrleftPulsesum = g_chrleftPulsesum + (int)g_chrleftPulse。 // g_inCarSpeed擴(kuò)大了10倍 //if(icount 4000) //速度設(shè)定 //{ if(CAR_SPEED_SET =300) CAR_SPEED_SET = CAR_SPEED_SET + 10。 //保存上一次速度控制量 g_inSpeedOutNew = ( D + g_inSpeedIntegral)。//int ANGLE_CONTROL_D = 80。 //得到角速度99陀螺儀靜態(tài)點(diǎn)采集的數(shù)據(jù) ATD0CTL5 = 0X08。 //左右電機(jī)PWM占空比 float rightMotor。 leftMotor =0。 PWMDTY23 =(uint)( leftMotor)。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //清除標(biāo)志位 while(!TFLG1_C0F)。 //開始采集pixel的值 while(!TFLG1_C0F)。 //開通AN10 uchr_ad_data[i] =ATD_uchar_convert() 。 //清除標(biāo)志位 while(!TFLG1_C1F)。}/***************************************************************************程序功能:采集CCD2數(shù)據(jù),通過上位機(jī)生成圖像入口參數(shù):無出口參數(shù):unsigned char uchr_ccd_data(128)共128個(gè)數(shù)據(jù),其性質(zhì)為全局?jǐn)?shù)組電氣連接:PWM6CCD_CLK PORTK_PK5CCD_SI2 CCD_AOAN11 850us (OK)*******************************************************/define CCD_SI2 PORTK_PK5 //定義CCD操作接口void CCD_Collect2(void) { //*********定義發(fā)送數(shù)據(jù)數(shù)組,并初始化起始位 int i=0。 //清除標(biāo)志位 delay_1us()。i++) { while(!TFLG1_C3F)。 //清除標(biāo)志位 while(!TFLG1_C2F)。}void main(void。 //清除標(biāo)志位 while(!TFLG1_C3F)。 //********以后每次撲捉CLK的下降沿128次 for(i=1。 while(!TFLG1_C3F)。 //清除標(biāo)志位 PWME = 0x0f。 //清除標(biāo)志位 while(!TFLG1_C0F)。i++) { while(!TFLG1_C1F)。 //清除標(biāo)志位 delay_1us()。 //清除標(biāo)志位 while(!TFLG1_C1F)。 //清除標(biāo)志位 while(!TFLG1_C1F)。 PWMDTY23 = (uint)leftMotor。 else rightMotor =rightMotor DEATH_CONTROL。 MotorOutput(g_inAngleOut + g_inSpeedOut)。void stand_control(void) { ATD0CTL5 = 0X00。 //本次速度完全控制結(jié)束 SpeedControlUpdate()。 if(g_inSpeedIntegral =Speed_Motor_Min) g_inSpeedIntegral = Speed_Motor_Min。 g_chrRightPulsesum=0。 //左計(jì)數(shù)器使能 Rightcounter_S0=1。 //左電機(jī)計(jì)數(shù)從PORTA g_chrleftPulse = g_chrleftPulse。float g_inCarSpeed=0。 //1ms 中斷初始化ok uchr_ad_data[0] = 0xff。 unsigned char PWMflag =0。 unsigned char uchr_ad_data[133]。j6。 //采樣前不放電,0x02分辨率為12位 0x00分辨率為8位 0x01分辨率為10位 ATD0CTL2 = 0X40。 //左對齊,PWM7通道頻率為160KHz PWMPER6 = 10。 //ClockA,ClockB對總線頻率0分頻64MHz PWMCAE = 0X00。 //啟動(dòng)主定時(shí)器模塊,使用快速清零(TFFCA=1) TSCR2 |= 0X03。 //設(shè)置波特率為28800Kbps(寫一個(gè)字) AMAP = 0 總線頻率64MHz SCI0CR1 = 0X00。 //0通道中斷有效 PITCFLMT_PITE=1。 while(CRGFLG_LOCK == 0)。 //fbus=fpll/2。 //PORTK_PK1,PORTK_PK3輸出 DDRK_DDRK4 =1。 PORTE_PE3 = 0。 DDRE_DDRE6 = 1。也可能是陀螺儀零偏置錯(cuò)誤，同理，微調(diào)即可。每10個(gè)直立控制周期進(jìn)行一次速度控制。如此循環(huán)，車模很快就會傾倒。由于在速度控制過程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過改變電機(jī)轉(zhuǎn)速來實(shí)現(xiàn)。而使用陀螺儀獲得的角速度比對卡爾曼濾波后求微分獲得的角速度更準(zhǔn)確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒有問題。角度控制頻率高，速度控制周期頻率慢。控制輸入有三個(gè)，分別對應(yīng)角度，角速度，車輪轉(zhuǎn)速。接著是動(dòng)態(tài)整定還是保持車輪恒定PWM旋轉(zhuǎn)，同時(shí)，搖擺車身。Kg決定了加速度計(jì)的權(quán)重。以下就是我們組程序中卡爾曼濾波的簡化算法實(shí)現(xiàn)X1=X0+gyro*dt。需要注意的是，Q,R兩個(gè)參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會產(chǎn)生相應(yīng)變化，對應(yīng)到我們的系統(tǒng)，在車模運(yùn)行狀態(tài)不同（傾角不同，PWM不同）情況下，Q,R都是不同的。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，就是式子（2）的。在該系統(tǒng)中，采用加速度傳感器估計(jì)出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測方程式中，為包含固定偏差的陀螺儀輸出角速度，為加速度計(jì)經(jīng)處理后得到的角度值，為陀螺儀測量噪聲，為加速度傳感器測量噪聲，為陀螺儀漂移誤差，和相互獨(dú)立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。無論是加速度計(jì)還是陀螺儀，都有自己的優(yōu)勢和劣勢。于是，我們對兩個(gè)傳感器進(jìn)行了數(shù)據(jù)采樣，觀測其輸出信號的關(guān)系。第五章 卡爾曼濾波卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。缺點(diǎn)在于電池高度過高，導(dǎo)致重心整體偏高。歐姆龍編碼器體積較大，我們將其放置在車輪上方，自己設(shè)計(jì)了碳纖維支架以及齒輪，使編碼器可靠安裝于車身上。下圖為速度閉環(huán)的控制框圖。在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實(shí)際測量的角度與車模角度有偏差，因此車模實(shí)際不是保持與地面垂直，而是存在一個(gè)傾角。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對于這個(gè)需要快速調(diào)整的系統(tǒng)，要求不大。為了使系統(tǒng)具有更好的線性，首先需要對電機(jī)死區(qū)進(jìn)行補(bǔ)償。對應(yīng)不同的電壓。（2）通過電機(jī)速度控制，實(shí)現(xiàn)車模恒速運(yùn)行和靜止。因此式（21）可變?yōu)? （22）按照上面的控制方法，可把倒立擺模型變?yōu)閱螖[模型，能夠穩(wěn)定在垂直位置。圖 在車輪上參照系中車體受力分析倒立擺之所以不能像單擺一樣可以穩(wěn)定在垂直位置，就是因?yàn)樵谒x平衡位置的時(shí)候，所受到的回復(fù)力與位移方向相同，而不是相反！因此，倒立擺便會加速偏離垂直位置，直到倒下。阻尼力越大，單擺越會盡快在垂直位置穩(wěn)定下來。下面通過對比單擺模型來說明保持車模穩(wěn)定的控制規(guī)律。這兩個(gè)條件缺一不可，實(shí)際上就是控制中的負(fù)反饋機(jī)制。因此在速度、方向控制的時(shí)候，應(yīng)該盡量平滑，以減少對于直立控制的干擾。因此從控制角度來看，由控制車模兩個(gè)電機(jī)旋轉(zhuǎn)方向及速度實(shí)現(xiàn)對車模的控制。為了分析方便，根據(jù)比賽規(guī)則，假設(shè)維持車模直立、運(yùn)行的動(dòng)力都來自于車模的兩個(gè)后車輪，后輪轉(zhuǎn)動(dòng)由兩個(gè)直流電機(jī)驅(qū)動(dòng)。由于車模同時(shí)受到三種控制的影響，從車模直立控制的角度，其它兩個(gè)控制就成為它的干擾。通過手掌移動(dòng)抵消木棒的傾斜角度和趨勢，從而保持木棒的直立。為了使得同學(xué)們能夠比較清楚理解其中的物理過程?？諝獾淖枘崃εc單擺運(yùn)行速度成正比，方向相反。為什么倒立擺在垂直位置時(shí)，在受到外部擾動(dòng)的情況下，無法保持穩(wěn)定呢？分析倒立擺的受力。此外，為了使得倒立擺能夠盡快地在垂直位置穩(wěn)定下來，還需要增加阻尼力，與偏角的速度成正比，方向相反。電機(jī)的運(yùn)動(dòng)控制有三個(gè)作用：（1） 通過電機(jī)加速度控制實(shí)現(xiàn)車模直立穩(wěn)定。施加在電機(jī)上一個(gè)階躍電壓，電機(jī)的速度變化曲線為(（24）式中，E為電壓；為單位階躍函數(shù)；為慣性環(huán)節(jié)時(shí)間常數(shù)；為電機(jī)轉(zhuǎn)速常數(shù)。 車模方向控制略第二章 程序控制說明 主程序框架 中斷服務(wù)程序框架由于摩擦力等因素的存在，電機(jī)與PWM給定信號存在非線性死去特性。常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。下面先分析一下引起車模速度變化的原因。原本利用負(fù)反饋進(jìn)行速度控制反而成了“正”反饋。這樣，可以將速度控制視為對于直立控制的一種擾動(dòng)。通過選型，決定使用歐姆龍E6A2CW3C雙向增量式測速編碼器。方案一的優(yōu)勢在于無需對于機(jī)械結(jié)構(gòu)進(jìn)行任何改動(dòng)，可以直接安裝。第四章 車模硬件電路設(shè)計(jì)詳見原理圖。既然如此為何我們需要使用兩個(gè)傳感器。陀螺儀的噪聲比較小，但是他的誤差會對積分造成漂移。首先我們建立系統(tǒng)的狀態(tài)方程和測量方程，由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實(shí)角度可以用來做一個(gè)狀態(tài)向量。則狀態(tài)k的最優(yōu)化估算值： （3）其中,為卡爾曼增益： （4）此時(shí)，我們已經(jīng)得到了k狀態(tài)下最有的估算值，但是為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差： （5）其中為單位陣，對于本系統(tǒng)則有。可以看到的是，卡爾曼濾波方程形式共有14個(gè)公式，同時(shí)很多參數(shù)的運(yùn)算涉及浮點(diǎn)數(shù)，這對于單片機(jī)的高效運(yùn)行時(shí)極為不利的，因此我們需要通過一些方法對卡爾曼濾波的公式進(jìn)行化簡，從而在不失精度的情況下，盡可能減小運(yùn)算量。所以將Kg2設(shè)定為0。但是dt越大，陀螺儀漂移造成的影響也就越大。如果紅色抖動(dòng)非常厲害，可以適當(dāng)減小Kg的大小。系統(tǒng)雖然是高階非線性系統(tǒng)，但是由于直立期間，傾斜角度非常小，所以可等等效為一個(gè)線性系統(tǒng)。我們在本設(shè)計(jì)中使用了角度、速度雙閉環(huán)的控制算法。雖然角度和角速度是不同的量，但是角速度就是對角度求微分。、速度雙閉環(huán)控制對于直立車模速度的控制相對于普通車模的速度控制則比較復(fù)雜。由于負(fù)反饋，使得車模往前傾角需要更大。正如前文所述，直立控制和速度控制存在強(qiáng)耦合，所以這里我們使用的分立控制周期的方法。dt:陀螺儀積分參數(shù)Kg：加速度計(jì)參數(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時(shí)刻，車身重心不在軸線上解決方法：微調(diào)加速度計(jì)零偏置，使得角度為0時(shí)，重心落在軸線上。 DDRE_DDRE5 = 1。 PORTE_PE5 = 1。 //***********CCD接口初始化 DDRK_DDRK5