【正文】 （3）其中,為卡爾曼增益： （4）此時(shí)，我們已經(jīng)得到了k狀態(tài)下最有的估算值，但是為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差： （5）其中為單位陣，對(duì)于本系統(tǒng)則有。和分別是加速度傳感器和陀螺儀測(cè)量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度，數(shù)值越小表明信任程度越高。首先我們建立系統(tǒng)的狀態(tài)方程和測(cè)量方程，由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實(shí)角度可以用來(lái)做一個(gè)狀態(tài)向量。這也就是為什么車模直立必須使用兩個(gè)傳感器來(lái)完成信號(hào)獲取。陀螺儀的噪聲比較小，但是他的誤差會(huì)對(duì)積分造成漂移。我們不難獲得以下一些結(jié)論：陀螺儀的噪聲還在可以接受范圍，但是加速度計(jì)噪聲已經(jīng)遠(yuǎn)遠(yuǎn)超出可控范圍接受對(duì)于加速度計(jì)做一個(gè)補(bǔ)充說(shuō)明，加速度計(jì)在車模直立狀態(tài)下，電機(jī)PWM占空比100%噪聲情況下，幅值映射到實(shí)際角度大約是60度左右。既然如此為何我們需要使用兩個(gè)傳感器。他的廣泛應(yīng)用已經(jīng)超過(guò)30年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。第四章 車模硬件電路設(shè)計(jì)詳見(jiàn)原理圖。缺點(diǎn)在于重心依然較高。方案一的優(yōu)勢(shì)在于無(wú)需對(duì)于機(jī)械結(jié)構(gòu)進(jìn)行任何改動(dòng)，可以直接安裝。合理降低重心是最為行之有效的一個(gè)方法。通過(guò)選型，決定使用歐姆龍E6A2CW3C雙向增量式測(cè)速編碼器。在經(jīng)過(guò)幾次測(cè)試后我們發(fā)現(xiàn)，如果使用彈性連接，且將MEMS傳感器安裝在前部地盤上，由于彈性將使得角度信號(hào)產(chǎn)生滯后，對(duì)于信號(hào)的處理帶來(lái)很大影響，因此，這部分使用了剛性連接，為了減輕重量，考慮使用2mm碳纖維版作為連接部件。這樣，可以將速度控制視為對(duì)于直立控制的一種擾動(dòng)。所以使用PID控制器來(lái)完成控制。原本利用負(fù)反饋進(jìn)行速度控制反而成了“正”反饋。假設(shè)車模開(kāi)始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。下面先分析一下引起車模速度變化的原因。這是由于未加速速度閉環(huán)的結(jié)果，未對(duì)車輪轉(zhuǎn)速進(jìn)行必要的控制，則最終由于電機(jī)轉(zhuǎn)速的飽和，而發(fā)生了無(wú)法對(duì)倒立擺進(jìn)行調(diào)整的后果。常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。那么就可以使用PID控制器。 車模方向控制略第二章 程序控制說(shuō)明 主程序框架 中斷服務(wù)程序框架由于摩擦力等因素的存在，電機(jī)與PWM給定信號(hào)存在非線性死去特性。調(diào)整車模直立時(shí)間常數(shù)很小，此時(shí)電機(jī)基本上運(yùn)行在加速階段。施加在電機(jī)上一個(gè)階躍電壓，電機(jī)的速度變化曲線為(（24）式中，E為電壓；為單位階躍函數(shù)；為慣性環(huán)節(jié)時(shí)間常數(shù)；為電機(jī)轉(zhuǎn)速常數(shù)。在將來(lái)的比賽中，如果規(guī)則增加了靜止要求，或者需要通過(guò)路橋等障礙，速度控制將會(huì)發(fā)揮作用。電機(jī)的運(yùn)動(dòng)控制有三個(gè)作用：（1） 通過(guò)電機(jī)加速度控制實(shí)現(xiàn)車模直立穩(wěn)定。只要保證在、條件下，可以維持車模直立狀態(tài)。此外，為了使得倒立擺能夠盡快地在垂直位置穩(wěn)定下來(lái)，還需要增加阻尼力，與偏角的速度成正比，方向相反。控制倒立擺底部車輪，使得它作加速運(yùn)動(dòng)。為什么倒立擺在垂直位置時(shí)，在受到外部擾動(dòng)的情況下，無(wú)法保持穩(wěn)定呢？分析倒立擺的受力。如果沒(méi)有阻尼力，單擺會(huì)在垂直位置左右擺動(dòng)?？諝獾淖枘崃εc單擺運(yùn)行速度成正比，方向相反。圖 車模可以簡(jiǎn)化成倒立的單擺。為了使得同學(xué)們能夠比較清楚理解其中的物理過(guò)程。但相對(duì)于上面的木棒直立相對(duì)簡(jiǎn)單。通過(guò)手掌移動(dòng)抵消木棒的傾斜角度和趨勢(shì)，從而保持木棒的直立。車模在加速和減速的時(shí)候，直立控制一直在起作用，它會(huì)自動(dòng)改變車模的傾角，移動(dòng)車模的重心，使得車模實(shí)現(xiàn)加速和減速。由于車模同時(shí)受到三種控制的影響，從車模直立控制的角度，其它兩個(gè)控制就成為它的干擾?？梢约僭O(shè)車模的電機(jī)可以虛擬地被拆解成三個(gè)不同功能的驅(qū)動(dòng)電機(jī)，它們同軸相連，分別控制車模的直立平衡、前進(jìn)行走、左右轉(zhuǎn)向。為了分析方便，根據(jù)比賽規(guī)則，假設(shè)維持車模直立、運(yùn)行的動(dòng)力都來(lái)自于車模的兩個(gè)后車輪，后輪轉(zhuǎn)動(dòng)由兩個(gè)直流電機(jī)驅(qū)動(dòng)。 本科生單片機(jī)課程設(shè)計(jì)報(bào)告湖南大學(xué)電氣與信息工程學(xué)院本科生單片機(jī)課程設(shè)計(jì)報(bào)告課 題:光電平衡小車專業(yè)班級(jí):電自六班目錄第一章 車模整體設(shè)計(jì)方案 3 3……………………………………………………………………………..5……………………………………………………………………………..9……………………………………………………………………………10……………………………………………………………………………10第二章程序控制說(shuō)明 11 11 12 1速度雙閉環(huán)控制 14第二章 車模機(jī)械設(shè)計(jì) 15 15 15 16第三章 車模硬件電路設(shè)計(jì) 18第四章 卡爾曼濾波 18 18 18 20 21 23第五章車模參數(shù)調(diào)試圖解 29附錄源代碼 30附錄硬件電路圖 30個(gè)人總結(jié) 30第一章 車模整體設(shè)計(jì)方案智能車是在車模結(jié)構(gòu)的框架上，搭上硬件結(jié)構(gòu)，通過(guò)MC9S12XS128單片機(jī)的處理能力，將傳感器采集到的信息處理分析后得出運(yùn)算結(jié)果，指揮電機(jī)做出適應(yīng)賽道及戰(zhàn)術(shù)策略的響應(yīng)的一套系統(tǒng)。因此從控制角度來(lái)看，由控制車模兩個(gè)電機(jī)旋轉(zhuǎn)方向及速度實(shí)現(xiàn)對(duì)車模的控制。圖 車模運(yùn)動(dòng)控制分解示意圖直流電機(jī)的力矩最終來(lái)自于電機(jī)驅(qū)動(dòng)電壓產(chǎn)生的電流。因此在速度、方向控制的時(shí)候，應(yīng)該盡量平滑，以減少對(duì)于直立控制的干擾。 車模直立控制控制車模直立的直觀經(jīng)驗(yàn)來(lái)自于雜技表演。這兩個(gè)條件缺一不可，實(shí)際上就是控制中的負(fù)反饋機(jī)制。因?yàn)檐嚹Ｓ袃蓚€(gè)輪子著地，因此車體只會(huì)在輪子滾動(dòng)的方向上發(fā)生傾斜。下面通過(guò)對(duì)比單擺模型來(lái)說(shuō)明保持車模穩(wěn)定的控制規(guī)律。圖 普通的單擺受力分析當(dāng)物體離開(kāi)垂直的平衡位置之后，便會(huì)受到重力與懸線的作用合力，驅(qū)動(dòng)重物回復(fù)平衡位置。阻尼力越大，單擺越會(huì)盡快在垂直位置穩(wěn)定下來(lái)。阻尼力會(huì)使得單擺最終停止在垂直位置。圖 在車輪上參照系中車體受力分析倒立擺之所以不能像單擺一樣可以穩(wěn)定在垂直位置，就是因?yàn)樵谒x平衡位置的時(shí)候，所受到的回復(fù)力與位移方向相同，而不是相反！因此，倒立擺便會(huì)加速偏離垂直位置，直到倒下。這樣站在小車上（非慣性系）看倒立擺，它就會(huì)受到額外的力(慣性力)，該力與車輪的加速度方向相反，大小成正比。因此式（21）可變?yōu)? （22）按照上面的控制方法，可把倒立擺模型變?yōu)閱螖[模型，能夠穩(wěn)定在垂直位置。其中， 決定了車模是否能夠穩(wěn)定到垂直位置，它必須大于重力加速度；決定了車?；氐酱怪蔽恢玫淖枘嵯禂?shù)，選取合適的阻尼系數(shù)可以保證車模盡快穩(wěn)定在垂直位置。（2）通過(guò)電機(jī)速度控制，實(shí)現(xiàn)車模恒速運(yùn)行和靜止。（3）通過(guò)電機(jī)差速控制，可以實(shí)現(xiàn)車模方向控制。對(duì)應(yīng)不同的電壓。由上一節(jié)式（23）計(jì)算所得到的加速度控制量a再乘以一個(gè)比例系數(shù)，即為施加在電機(jī)上的控制電壓，這樣便可以控制車模保持直立狀態(tài)。為了使系統(tǒng)具有更好的線性，首先需要對(duì)電機(jī)死區(qū)進(jìn)行補(bǔ)償。這里需要進(jìn)行解釋，為何有兩個(gè)輸入變量任然可以使用SISO系統(tǒng)才能使用PID。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對(duì)于這個(gè)需要快速調(diào)整的系統(tǒng)，要求不大。為了解決這個(gè)問(wèn)題，必須要引入速度閉環(huán)控制。假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實(shí)際測(cè)量的角度與車模角度有偏差，因此車模實(shí)際不是保持與地面垂直，而是存在一個(gè)傾角。在車模直立控制下，為了能夠有一個(gè)往前的傾斜角度，車輪需要往后運(yùn)動(dòng)，這樣會(huì)引起車輪速度下降（因?yàn)檐囕喭?fù)方向運(yùn)動(dòng)了）。在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。由于系統(tǒng)對(duì)于靜態(tài)誤差存在一定要求，所以使用PI控制器。下圖為速度閉環(huán)的控制框圖。 車模地盤安裝測(cè)速編碼器對(duì)于攝像頭組以及光電組而言，起到速度閉環(huán)的作用，而對(duì)于兩輪車的選題，編碼器不僅起到速度閉環(huán)，還對(duì)直立控制有很重要的作用，因此，編碼器對(duì)于電磁組兩輪車而言，是不可或缺的一部分。歐姆龍編碼器體積較大，我們將其放置在車輪上方，自己設(shè)計(jì)了碳纖維支架以及齒輪，使編碼器可靠安裝于車身上。為了降低重心，我們對(duì)車模各個(gè)部件進(jìn)行了稱重，具體數(shù)據(jù)如下：部件質(zhì)量（克）前底板100電路板（包含驅(qū)動(dòng)）72MEMS電路板10測(cè)速編碼器36電池300從上表中不難看出，質(zhì)量最大的部件是電池，同時(shí)，電池的位置可以方便更改，所以我們決定嘗試改動(dòng)電池的安裝位置。缺點(diǎn)在于電池高度過(guò)高，導(dǎo)致重心整體偏高。方案三：背面橫向安裝方案三的優(yōu)勢(shì)在于將電池重心降到最低，對(duì)于直立控制而言存在較大優(yōu)勢(shì)。第五章 卡爾曼濾波卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。近年來(lái)更被應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識(shí)別，圖像分割，圖像邊緣檢測(cè)等等。于是，我們對(duì)兩個(gè)傳感器進(jìn)行了數(shù)據(jù)采樣，觀測(cè)其輸出信號(hào)的關(guān)系。根本不能直接使用。無(wú)論是加速度計(jì)還是陀螺儀，都有自己的優(yōu)勢(shì)和劣勢(shì)。因?yàn)閱蝹€(gè)傳感器不可靠，只有取其精華去其糟粕，發(fā)揮兩個(gè)傳感器的共同優(yōu)勢(shì)，才能夠取得好的效果。在該系統(tǒng)中，采用加速度傳感器估計(jì)出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測(cè)方程式中，為包含固定偏差的陀螺儀輸出角速度，為加速度計(jì)經(jīng)處理后得到的角度值，為陀螺儀測(cè)量噪聲，為加速度傳感器測(cè)量噪聲，為陀螺儀漂移誤差，和相互獨(dú)立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。在該系統(tǒng)中陀螺儀的值更為接近準(zhǔn)確值，因此取的值小于的值。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，就是式子（2）的。但是，以上五個(gè)公式，僅僅只是矩陣形式。需要注意的是，Q,R兩個(gè)參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會(huì)產(chǎn)生相應(yīng)變化，對(duì)應(yīng)到我們的系統(tǒng)，在車模運(yùn)行狀態(tài)不同（傾角不同，PWM不同）情況下，Q,R都是不同的。（所有這些工作都在MATLAB下完成）所以，我們將方框中的所有公式完全省略，通過(guò)實(shí)驗(yàn)整定，選取一個(gè)近似Kg來(lái)替代方框中的所有運(yùn)算。以下就是我們組程序中卡爾曼濾波的簡(jiǎn)化算法實(shí)現(xiàn)X1=X0+gyro*dt。由于陀螺儀的輸出和加速度計(jì)輸出的量綱并不相同，所以陀螺儀采樣值*dt并不直接反應(yīng)一個(gè)角度，而是與實(shí)際角度相差一個(gè)系數(shù)。Kg決定了加速度計(jì)的權(quán)重。將車模保持在穩(wěn)定直立狀態(tài)，讓車輪以恒定PWM（80%以上）轉(zhuǎn)動(dòng)，然后調(diào)節(jié)參數(shù)。接著是動(dòng)態(tài)整定還是保持車輪恒定PWM旋轉(zhuǎn)，同時(shí)，搖擺車身。達(dá)到上圖效果即可認(rèn)為卡爾曼濾波參數(shù)整定完成?？刂戚斎胗腥齻€(gè)，分別對(duì)應(yīng)角度，角速度，車輪轉(zhuǎn)速。對(duì)于倒立擺的控制，參考的文獻(xiàn)中有使用各種算法進(jìn)行控制，比如狀態(tài)反饋、自適應(yīng)算法、神經(jīng)網(wǎng)絡(luò)算法、模糊算法等等。角度控制頻率高，速度控制周期頻率慢。那么在這個(gè)前提下，只有兩個(gè)速度量：角度和角速度；被控變量只有角度。而使用陀螺儀獲得的角速度比對(duì)卡爾曼濾波后求微分獲得的角速度更準(zhǔn)確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒(méi)有問(wèn)題?？刂葡到y(tǒng)框圖如下：在實(shí)際調(diào)試過(guò)程中，倒立擺可以進(jìn)行短時(shí)間的直立，但是會(huì)朝向一個(gè)方向加速運(yùn)行，最后車模會(huì)倒下。由于在速度控制過(guò)程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過(guò)改變電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)。首先對(duì)一個(gè)簡(jiǎn)單例子進(jìn)行分析。如此循環(huán)，車模很快就會(huì)傾倒。而速度閉環(huán)控制仍然可以看做是一個(gè)SISO系統(tǒng)。每10個(gè)直立控制周期進(jìn)行一次速度控制。整定方法遵從先直立控制再速度控制；先P參數(shù)，后I參數(shù)，最后D參數(shù)的整定順序。也可能是陀螺儀零偏置錯(cuò)誤，同理，微調(diào)即可。附件源代碼//******************端口初始化void PAC_Init(void){ //**********計(jì)數(shù)器接口初始化 ok() DDRB = 0X00。 DDRE_DDRE6 = 1。 PORTE_PE3 = 0。 PORTE_PE3 = 0。 //電機(jī)IO接口 DDRK_DDRK3 =1。 //PORTK_PK1,PORTK_PK3輸出 DDRK_DDRK4 =1。 //使PLL與系統(tǒng)隔離 PLLCTL_PLLON = 1。 //fbus=fpll/2。 POSTDIV = 0。 while(CRGFLG_LOCK == 0)。 //通道0使能 PITMUX_PMUX = 0。 //0通道中斷有效 PITCFLMT_PITE=1。 //設(shè)置波特率為19200Kbps(寫一個(gè)字) AMAP = 0 總線頻率64MHz //SCI0BD = 69。 //設(shè)置波特率為28800Kbps(寫一個(gè)字) AMAP = 0 總線頻率64MHz SCI0CR1 = 0X00。i133。 //啟動(dòng)主定時(shí)器模塊,使用快速清零(TFFCA=1) TSCR2 |= 0X03。 //禁止輸出撲捉申請(qǐng)中斷}/*****************************************************函數(shù)功能:對(duì)PWM初始化,設(shè)置PWM0,PWM1級(jí)聯(lián),PWM2,PWM3級(jí)聯(lián)入口參數(shù):無(wú)出口參數(shù):無(wú)******************************************************/void PWM_init(void){