單片機(jī)課程設(shè)計(jì)--光電平衡小車

1、<p>　　本科生單片機(jī)課程設(shè)計(jì)報(bào)告</p><p>　　課 題:光電平衡小車</p><p><b>　　車模整體設(shè)計(jì)方案</b></p><p>　　1.1兩輪自平衡智能車整體框架</p><p>　　智能車是在車模結(jié)構(gòu)的框架上，搭上硬件結(jié)構(gòu)，通過(guò)MC9S12XS128單片機(jī)的處理能力，將傳感器采集到

2、的信息處理分析后得出運(yùn)算結(jié)果，指揮電機(jī)做出適應(yīng)賽道及戰(zhàn)術(shù)策略的響應(yīng)的一套系統(tǒng)。其硬件結(jié)構(gòu)框架如下圖：</p><p>　　1.2 直立行走任務(wù)分解</p><p>　　電磁組比賽要求車模在直立的狀態(tài)下以兩個(gè)輪子著地沿著賽道進(jìn)行比賽，相比四輪著地狀態(tài)，車模控制任務(wù)更為復(fù)雜。為了能夠方便找到解決問(wèn)題的辦法，首先將復(fù)雜的問(wèn)題分解成簡(jiǎn)單的問(wèn)題進(jìn)行討論。為了分析方便，根據(jù)比賽規(guī)則，假設(shè)維持車模直立、

3、運(yùn)行的動(dòng)力都來(lái)自于車模的兩個(gè)后車輪，后輪轉(zhuǎn)動(dòng)由兩個(gè)直流電機(jī)驅(qū)動(dòng)。因此從控制角度來(lái)看，由控制車模兩個(gè)電機(jī)旋轉(zhuǎn)方向及速度實(shí)現(xiàn)對(duì)車模的控制。車模運(yùn)動(dòng)控制任務(wù)可以分解成以下三個(gè)基本任務(wù)：</p><p>　?。?） 控制車模直立：通過(guò)控制兩個(gè)電機(jī)正反向運(yùn)動(dòng)保持車模直立狀態(tài)；</p><p>　　（2） 控制車模速度：通過(guò)控制兩個(gè)電機(jī)轉(zhuǎn)速速度實(shí)現(xiàn)車模行進(jìn)控制；</p><p>

4、;　?。?） 控制車模轉(zhuǎn)向：通過(guò)控制兩個(gè)電機(jī)之間的轉(zhuǎn)動(dòng)差速實(shí)現(xiàn)車模轉(zhuǎn)向控制。</p><p>　　以上三個(gè)任務(wù)都是通過(guò)控制車模兩個(gè)后輪驅(qū)動(dòng)電機(jī)完成的?？梢约僭O(shè)車模的電機(jī)可以虛擬地被拆解成三個(gè)不同功能的驅(qū)動(dòng)電機(jī)，它們同軸相連，分別控制車模的直立平衡、前進(jìn)行走、左右轉(zhuǎn)向，如圖2.1所示。</p><p>　　圖 2.1 車模運(yùn)動(dòng)控制分解示意圖</p><p>　　直流電

5、機(jī)的力矩最終來(lái)自于電機(jī)驅(qū)動(dòng)電壓產(chǎn)生的電流。因此只要電機(jī)處于線性狀態(tài)，上述拆解可以等效成三種不同控制目標(biāo)的電壓疊加之后，施加在電機(jī)上。</p><p>　　在上述三個(gè)任務(wù)中保持車模直立是關(guān)鍵。由于車模同時(shí)受到三種控制的影響，從車模直立控制的角度，其它兩個(gè)控制就成為它的干擾。因此在速度、方向控制的時(shí)候，應(yīng)該盡量平滑，以減少對(duì)于直立控制的干擾。三者之間的配合如圖2.2所示。</p><p>　　

6、圖 2.2 三層控制之間相互配合，底層盡量減少對(duì)于上層的干擾</p><p>　　上述三個(gè)控制各自獨(dú)立進(jìn)行控制，它們各自假設(shè)其它兩個(gè)控制都已經(jīng)達(dá)到穩(wěn)定。比如速度控制時(shí)，假設(shè)車模已經(jīng)在直立控制下保持了直立穩(wěn)定，通過(guò)改變電機(jī)的電壓控制車模加速和減速。車模在加速和減速的時(shí)候，直立控制一直在起作用，它會(huì)自動(dòng)改變車模的傾角，移動(dòng)車模的重心，使得車模實(shí)現(xiàn)加速和減速。</p><p>　　1.3 車模直

7、立控制</p><p>　　控制車模直立的直觀經(jīng)驗(yàn)來(lái)自于雜技表演。一般的人通過(guò)簡(jiǎn)單練習(xí)就可以讓一個(gè)直木棒在手指尖上保持直立。這需要兩個(gè)條件：一個(gè)是托著木棒的手掌可以移動(dòng)；另一個(gè)是眼睛可以觀察到木棒的傾斜角度和傾斜趨勢(shì)（角加速度）。通過(guò)手掌移動(dòng)抵消木棒的傾斜角度和趨勢(shì)，從而保持木棒的直立。這兩個(gè)條件缺一不可，實(shí)際上就是控制中的負(fù)反饋機(jī)制，參見(jiàn)圖2.3。</p><p>　　世界上還沒(méi)有任何一

8、個(gè)天才雜技演員可以蒙著眼睛使得木棒在自己手指上直立，因?yàn)闆](méi)有了負(fù)反饋。</p><p>　　圖 2.3 通過(guò)反饋保持木棒的直立</p><p>　　車模直立也是通過(guò)負(fù)反饋實(shí)現(xiàn)的。但相對(duì)于上面的木棒直立相對(duì)簡(jiǎn)單。因?yàn)檐嚹Ｓ袃蓚€(gè)輪子著地，因此車體只會(huì)在輪子滾動(dòng)的方向上發(fā)生傾斜。控制輪子轉(zhuǎn)動(dòng)，抵消傾斜的趨勢(shì)便可以保持車體直立了。如圖2.4所示。</p><p>　　圖 2

9、.4 通過(guò)車輪運(yùn)動(dòng)控制保持車體直立</p><p>　　那么車輪如何運(yùn)行，才能夠最終保持車體垂直穩(wěn)定？為了回答這個(gè)問(wèn)題，一般的做法需要建立車模的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)數(shù)學(xué)模型，通過(guò)設(shè)計(jì)最優(yōu)控制來(lái)保證車模的穩(wěn)定。為了使得同學(xué)們能夠比較清楚理解其中的物理過(guò)程。下面通過(guò)對(duì)比單擺模型來(lái)說(shuō)明保持車模穩(wěn)定的控制規(guī)律。重力場(chǎng)中使用細(xì)線懸掛著重物經(jīng)過(guò)簡(jiǎn)化便形成理想化的單擺模型。直立著的車?？梢钥闯煞胖迷诳梢宰笥乙苿?dòng)平臺(tái)上的倒立著的單擺。

10、如圖2.5所示。</p><p>　　圖 2.5 車?？梢院?jiǎn)化成倒立的單擺</p><p>　　普通的單擺受力分析如圖2.6所示。</p><p>　　圖 2.6 普通的單擺受力分析</p><p>　　當(dāng)物體離開(kāi)垂直的平衡位置之后，便會(huì)受到重力與懸線的作用合力，驅(qū)動(dòng)重物回復(fù)平衡位置。這個(gè)力稱之為回復(fù)力，其大小為在此回復(fù)力作用下，單擺便進(jìn)行周

11、期運(yùn)動(dòng)。在空氣中運(yùn)動(dòng)的單擺，由于受到空氣的阻尼力，單擺最終會(huì)停止在垂直平衡位置?？諝獾淖枘崃εc單擺運(yùn)行速度成正比，方向相反。阻尼力越大，單擺越會(huì)盡快在垂直位置穩(wěn)定下來(lái)。圖2.7顯示出不同阻尼系數(shù)下，單擺的運(yùn)動(dòng)曲線。</p><p>　　圖 2.7 單擺在不同阻尼下的運(yùn)動(dòng)情況</p><p>　　總結(jié)單擺能夠穩(wěn)定在垂直位置的條件有兩個(gè)：</p><p>　?。?） 受

12、到與位移（角度）相反的恢復(fù)力；</p><p>　?。?） 受到與運(yùn)動(dòng)速度相反的阻尼力。</p><p>　　如果沒(méi)有阻尼力，單擺會(huì)在垂直位置左右擺動(dòng)。阻尼力會(huì)使得單擺最終停止在垂直位置。阻尼力過(guò)小（欠阻尼）會(huì)使得單擺產(chǎn)生震蕩，阻尼力過(guò)大（過(guò)阻尼）會(huì)使得單擺到達(dá)平衡位置時(shí)間拉長(zhǎng)。存在一個(gè)阻尼臨界阻尼系數(shù)，使得單擺最快穩(wěn)定在平衡位置。為什么倒立擺在垂直位置時(shí)，在受到外部擾動(dòng)的情況下，無(wú)法保持

13、穩(wěn)定呢？分析倒立擺的受力，如圖2.8所示。</p><p>　　圖 2.8 在車輪上參照系中車體受力分析</p><p>　　倒立擺之所以不能像單擺一樣可以穩(wěn)定在垂直位置，就是因?yàn)樵谒x平衡位置的時(shí)候，所受到的回復(fù)力與位移方向相同，而不是相反！因此，倒立擺便會(huì)加速偏離垂直位置，直到倒下。如何通過(guò)控制使得倒立擺能夠像單擺一樣，穩(wěn)定在垂直位置呢？要達(dá)到這一目的，只有兩個(gè)辦法：一個(gè)是改變重力的

14、方向；另一個(gè)是增加額外的受力，使得恢復(fù)力與位移方向相反才行。由此，能夠做的顯然只有第二種方式。</p><p>　　控制倒立擺底部車輪，使得它作加速運(yùn)動(dòng)。這樣站在小車上（非慣性系）看倒立擺，它就會(huì)受到額外的力(慣性力)，該力與車輪的加速度方向相反，大小成正比。這樣倒立擺所受到的回復(fù)力為</p><p><b>　　? （2-1）</b></p><

15、p>　　式中，假設(shè)控制車輪加速度與偏角成正比，比例為。顯然，如果，（g是重力加速度）那么回復(fù)力的方向便于位移方向相反了。</p><p>　　此外，為了使得倒立擺能夠盡快地在垂直位置穩(wěn)定下來(lái)，還需要增加阻尼力，與偏角的速度成正比，方向相反。因此式（2-1）可變?yōu)?lt;/p><p><b>　?。?-2）</b></p><p>　　按照上面

16、的控制方法，可把倒立擺模型變?yōu)閱螖[模型，能夠穩(wěn)定在垂直位置。因此，可得控制車輪加速度的控制算法</p><p><b>　?。?-3）</b></p><p>　　式中，為車模傾角； '為角速度；k1、k2均為比例系數(shù)；兩項(xiàng)相加后作為車輪加速度的控制量。只要保證在、條件下，可以維持車模直立狀態(tài)。其中， 決定了車模是否能夠穩(wěn)定到垂直位置，它必須大于重力加速度；決

17、定了車?；氐酱怪蔽恢玫淖枘嵯禂?shù)，選取合適的阻尼系數(shù)可以保證車模盡快穩(wěn)定在垂直位置。</p><p>　　因此控制車模穩(wěn)定，需要下列兩個(gè)條件：</p><p>　?。?）能夠精確測(cè)量車模傾角的大小和角速度的大小；</p><p>　?。?）可以控制車輪的加速度。</p><p><b>　　1.4車模速度控制</b><

18、;/p><p>　　車模運(yùn)行速度是通過(guò)控制車輪速度實(shí)現(xiàn)的，車輪通過(guò)車模兩個(gè)后輪電機(jī)經(jīng)由減速齒輪箱驅(qū)動(dòng)，因此通過(guò)控制電機(jī)轉(zhuǎn)速可以實(shí)現(xiàn)對(duì)車輪的運(yùn)動(dòng)控制。</p><p>　　電機(jī)的運(yùn)動(dòng)控制有三個(gè)作用：</p><p>　?。?） 通過(guò)電機(jī)加速度控制實(shí)現(xiàn)車模直立穩(wěn)定。</p><p>　　（2）通過(guò)電機(jī)速度控制，實(shí)現(xiàn)車模恒速運(yùn)行和靜止。雖然本屆比賽規(guī)則

19、中沒(méi)有要求車模速度恒定，也沒(méi)有要求車模在比賽之前和沖過(guò)終點(diǎn)之后保持靜止?fàn)顟B(tài)。但是通過(guò)速度控制，可以提高車模穩(wěn)定性。在將來(lái)的比賽中，如果規(guī)則增加了靜止要求，或者需要通過(guò)路橋等障礙，速度控制將會(huì)發(fā)揮作用。</p><p>　?。?）通過(guò)電機(jī)差速控制，可以實(shí)現(xiàn)車模方向控制。電機(jī)運(yùn)動(dòng)控制是通過(guò)改變施加在其上的驅(qū)動(dòng)電壓實(shí)現(xiàn)的。對(duì)于電機(jī)的電磁模型、動(dòng)力學(xué)模型以及車模的動(dòng)力學(xué)模型進(jìn)行分析和簡(jiǎn)化，可以將電機(jī)運(yùn)動(dòng)模型簡(jiǎn)化成如下的一

20、階慣性環(huán)節(jié)模型。施加在電機(jī)上一個(gè)階躍電壓，電機(jī)的速度變化曲線為</p><p><b>　　?（2-4）</b></p><p>　　式中，E為電壓；為單位階躍函數(shù)；為慣性環(huán)節(jié)時(shí)間常數(shù)；為電機(jī)轉(zhuǎn)速常數(shù)。</p><p>　　對(duì)應(yīng)不同的電壓，電機(jī)的速度變化曲線如圖2.9示。</p><p>　　圖 2.9 電機(jī)在不同電壓下

21、的速度</p><p>　　由圖2.9可以看出，電機(jī)運(yùn)動(dòng)明顯分為兩個(gè)階段：第一個(gè)階段是加速階段；第二個(gè)階段為恒速階段。其中，在加速階段，電機(jī)帶動(dòng)車模后輪進(jìn)行加速運(yùn)動(dòng)，加速度近似和施加在電機(jī)上的電壓成正比，加速階段的時(shí)間長(zhǎng)度取決于時(shí)間常數(shù)，該常數(shù)由電機(jī)轉(zhuǎn)動(dòng)慣量、減速齒輪箱、車模的轉(zhuǎn)動(dòng)慣量決定；在恒速階段，電機(jī)帶動(dòng)車模后輪進(jìn)行恒速運(yùn)行，運(yùn)行速度與施加在電機(jī)上的電壓成正比。調(diào)整車模直立時(shí)間常數(shù)很小，此時(shí)電機(jī)基本上運(yùn)行在

22、加速階段。由上一節(jié)式（2-3）計(jì)算所得到的加速度控制量a再乘以一個(gè)比例系數(shù)，即為施加在電機(jī)上的控制電壓，這樣便可以控制車模保持直立狀態(tài)。車模運(yùn)行速度調(diào)整時(shí)間相對(duì)很長(zhǎng)，此時(shí)，電機(jī)速度與施加在其上的電壓成正比。通過(guò)傳統(tǒng)的PID反饋控制，便可以精確控制電機(jī)的運(yùn)行速度，從而控制車模的運(yùn)行速度。</p><p>　　1.5 車模方向控制</p><p><b>　　略</b>&

23、lt;/p><p><b>　　程序控制說(shuō)明</b></p><p><b>　　2.1 主程序框架</b></p><p><b>　　中斷服務(wù)程序框架</b></p><p><b>　　2.2電機(jī)死區(qū)補(bǔ)償</b></p><p>

24、　　由于摩擦力等因素的存在，電機(jī)與PWM給定信號(hào)存在非線性死去特性。為了使系統(tǒng)具有更好的線性，首先需要對(duì)電機(jī)死區(qū)進(jìn)行補(bǔ)償。</p><p>　　2.3角度單閉環(huán)控制</p><p>　　首先我們忽略速度控制，僅僅只考慮角度閉環(huán)系統(tǒng)。那么在這個(gè)前提下，只有兩個(gè)速度量：角度和角速度；被控變量只有角度。那么就可以使用PID控制器。這里需要進(jìn)行解釋，為何有兩個(gè)輸入變量任然可以使用SISO系統(tǒng)才能使

25、用PID。雖然角度和角速度是不同的量，但是角速度就是對(duì)角度求微分。而使用陀螺儀獲得的角速度比對(duì)卡爾曼濾波后求微分獲得的角速度更準(zhǔn)確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒(méi)有問(wèn)題。</p><p>　　常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對(duì)于這個(gè)需要快速調(diào)整的系統(tǒng)，要求不大。因此最后我們選擇了PD

26、控制器。</p><p><b>　　控制系統(tǒng)框圖如下：</b></p><p>　　在實(shí)際調(diào)試過(guò)程中，倒立擺可以進(jìn)行短時(shí)間的直立，但是會(huì)朝向一個(gè)方向加速運(yùn)行，最后車模會(huì)倒下。這是由于未加速速度閉環(huán)的結(jié)果，未對(duì)車輪轉(zhuǎn)速進(jìn)行必要的控制，則最終由于電機(jī)轉(zhuǎn)速的飽和，而發(fā)生了無(wú)法對(duì)倒立擺進(jìn)行調(diào)整的后果。</p><p>　　為了解決這個(gè)問(wèn)題，必須要引入

27、速度閉環(huán)控制。</p><p>　　2.4角度、速度雙閉環(huán)控制</p><p>　　對(duì)于直立車模速度的控制相對(duì)于普通車模的速度控制則比較復(fù)雜。由于在速度控制過(guò)程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過(guò)改變電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)。下面先分析一下引起車模速度變化的原因。</p><p>　　假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器

28、實(shí)際測(cè)量的角度與車模角度有偏差，因此車模實(shí)際不是保持與地面垂直，而是存在一個(gè)傾角。在重力的作用下，車模就會(huì)朝傾斜的方向加速前進(jìn)。</p><p>　　首先對(duì)一個(gè)簡(jiǎn)單例子進(jìn)行分析。假設(shè)車模開(kāi)始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。在車模直立控制下，為了能夠有一個(gè)往前的傾斜角度，車輪需要往后運(yùn)動(dòng)，這樣會(huì)引起車輪速度下降（因?yàn)檐囕喭?fù)方向運(yùn)動(dòng)了）。由于負(fù)反饋，使得車模往前傾角需要更大。如此循環(huán)

29、，車模很快就會(huì)傾倒。原本利用負(fù)反饋進(jìn)行速度控制反而成了“正”反饋。在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。但根據(jù)實(shí)際經(jīng)驗(yàn)，是可以通過(guò)速度控制進(jìn)行車模傾角控制的。</p><p>　　而速度閉環(huán)控制仍然可以看做是一個(gè)SISO系統(tǒng)。所以使用PID控制器來(lái)完成控制。由于系統(tǒng)對(duì)于靜態(tài)誤差存在一定要求，所以使用PI控制器。</p><p>

30、　　正如前文所述，直立控制和速度控制存在強(qiáng)耦合，所以這里我們使用的分立控制周期的方法。每10個(gè)直立控制周期進(jìn)行一次速度控制。這樣，可以將速度控制視為對(duì)于直立控制的一種擾動(dòng)。</p><p>　　下圖為速度閉環(huán)的控制框圖。</p><p>　　直立、速度閉環(huán)控制框圖</p><p><b>　　車模機(jī)械設(shè)計(jì)</b></p><

31、p><b>　　3.1車模地盤(pán)安裝</b></p><p>　　C車模地盤(pán)由兩部分組成，電機(jī)部分以及前地盤(pán)部分。兩部分的連接可以采用彈性連接或者剛性連接。在經(jīng)過(guò)幾次測(cè)試后我們發(fā)現(xiàn)，如果使用彈性連接，且將MEMS傳感器安裝在前部地盤(pán)上，由于彈性將使得角度信號(hào)產(chǎn)生滯后，對(duì)于信號(hào)的處理帶來(lái)很大影響，因此，這部分使用了剛性連接，為了減輕重量，考慮使用2mm碳纖維版作為連接部件。</p&g

32、t;<p>　　圖3.1 車模地盤(pán)安裝</p><p>　　3.2車模編碼器安裝</p><p>　　測(cè)速編碼器對(duì)于攝像頭組以及光電組而言，起到速度閉環(huán)的作用，而對(duì)于兩輪車的選題，編碼器不僅起到速度閉環(huán)，還對(duì)直立控制有很重要的作用，因此，編碼器對(duì)于電磁組兩輪車而言，是不可或缺的一部分。</p><p>　　但是由于直立行走，相較于攝像頭組與光電組，電磁

33、車編碼器安裝的額位置受到了較大制約。同時(shí)，為了控制的簡(jiǎn)便，我們使用了雙向增量式編碼器。通過(guò)選型，決定使用歐姆龍E6A2-CW3C雙向增量式測(cè)速編碼器。</p><p>　　歐姆龍編碼器體積較大，我們將其放置在車輪上方，自己設(shè)計(jì)了碳纖維支架以及齒輪，使編碼器可靠安裝于車身上。</p><p>　　圖3.2 編碼器安裝</p><p>　　3.3車模電池位置改裝<

34、/p><p>　　C車模的電機(jī)標(biāo)稱值為4.17W，在三組車模中屬于最弱的一只，但是對(duì)于直立車而言，電機(jī)的功率對(duì)于車模直立的效果有著決定性因素。在無(wú)法修改電機(jī)的情況下，必須對(duì)結(jié)構(gòu)進(jìn)行優(yōu)化。合理降低重心是最為行之有效的一個(gè)方法。</p><p>　　為了降低重心，我們對(duì)車模各個(gè)部件進(jìn)行了稱重，具體數(shù)據(jù)如下：</p><p>　　從上表中不難看出，質(zhì)量最大的部件是電池，同時(shí)，

35、電池的位置可以方便更改，所以我們決定嘗試改動(dòng)電池的安裝位置。</p><p>　　為此，我們擬定了三種安裝方式：</p><p><b>　　方式一：原始位置。</b></p><p>　　圖3.3.1 電池安裝方案一。</p><p>　　方案一的優(yōu)勢(shì)在于無(wú)需對(duì)于機(jī)械結(jié)構(gòu)進(jìn)行任何改動(dòng)，可以直接安裝。缺點(diǎn)在于電池高度過(guò)高

36、，導(dǎo)致重心整體偏高。</p><p>　　方案二：背面縱向安裝</p><p>　　圖3.3.2電池安裝方案二。</p><p>　　方案二的優(yōu)勢(shì)在于轉(zhuǎn)向慣量較小，轉(zhuǎn)向靈活，重心低于方案一。缺點(diǎn)在于重心依然較高。</p><p>　　方案三：背面橫向安裝</p><p>　　圖3.3.3電池安裝方案三</p>

37、;<p>　　方案三的優(yōu)勢(shì)在于將電池重心降到最低，對(duì)于直立控制而言存在較大優(yōu)勢(shì)。缺點(diǎn)在于轉(zhuǎn)向慣量略大于方案二。</p><p>　　經(jīng)過(guò)綜合比較和測(cè)試，選用方案三。</p><p><b>　　車模硬件電路設(shè)計(jì)</b></p><p><b>　　詳見(jiàn)原理圖。</b></p><p>

38、<b>　　卡爾曼濾波</b></p><p>　　4.1卡爾曼濾波簡(jiǎn)介</p><p>　　卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。簡(jiǎn)單來(lái)說(shuō)，卡爾曼濾波器是一個(gè)“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。對(duì)于解決很大

39、部分的問(wèn)題，他是最優(yōu)，效率最高甚至是最有用的。他的廣泛應(yīng)用已經(jīng)超過(guò)30年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。近年來(lái)更被應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識(shí)別，圖像分割，圖像邊緣檢測(cè)等等。</p><p>　　5.2使用卡爾曼濾波的原因</p><p>　　當(dāng)決定使用加速度計(jì)（反應(yīng)角度）以及陀螺儀（反應(yīng)角速度）以后，我們對(duì)參考的論文中為何要同時(shí)使用加速

40、度計(jì)以及陀螺儀產(chǎn)生過(guò)一些疑問(wèn)。因?yàn)槿绻枰撬俣?，我們可以通過(guò)這樣的方法對(duì)加速度計(jì)進(jìn)行微分，計(jì)算出角速度，或者可以通過(guò)對(duì)于陀螺儀積分，從而獲得角度。既然如此為何我們需要使用兩個(gè)傳感器。</p><p>　　于是，我們對(duì)兩個(gè)傳感器進(jìn)行了數(shù)據(jù)采樣，觀測(cè)其輸出信號(hào)的關(guān)系。由于考慮電機(jī)轉(zhuǎn)動(dòng)產(chǎn)生的震動(dòng)會(huì)對(duì)傳感器產(chǎn)生比較大的干擾， </p><p>　　隨著PWM占空比的不斷提高，陀螺儀的噪聲也更加顯

41、著。之后我們對(duì)加速度傳感器進(jìn)行了測(cè)試。</p><p>　　我們不難獲得以下一些結(jié)論：</p><p>　　1、陀螺儀的噪聲還在可以接受范圍，但是加速度計(jì)噪聲已經(jīng)遠(yuǎn)遠(yuǎn)超出可控范圍接受</p><p>　　2、對(duì)于加速度計(jì)做一個(gè)補(bǔ)充說(shuō)明，加速度計(jì)在車模直立狀態(tài)下，電機(jī)PWM占空比100%噪聲情況下，幅值映射到實(shí)際角度大約是60度左右。根本不能直接使用。</p&g

42、t;<p>　　3、方差（可以等效理解為噪聲大?。╇S著占空比不斷增加。</p><p>　　4、加速度計(jì)雖然噪聲很大，但是長(zhǎng)時(shí)間來(lái)看，他的數(shù)學(xué)期望是非常穩(wěn)定的。</p><p>　　5、陀螺儀的噪聲比較小，但是他的誤差會(huì)對(duì)積分造成漂移。</p><p>　　無(wú)論是加速度計(jì)還是陀螺儀，都有自己的優(yōu)勢(shì)和劣勢(shì)。簡(jiǎn)而言之：加速度計(jì)短時(shí)間內(nèi)不可信，但是長(zhǎng)時(shí)間內(nèi)可

43、信。陀螺儀短時(shí)間內(nèi)可信而長(zhǎng)時(shí)間內(nèi)不可信。</p><p>　　這也就是為什么車模直立必須使用兩個(gè)傳感器來(lái)完成信號(hào)獲取。因?yàn)閱蝹€(gè)傳感器不可靠，只有取其精華去其糟粕，發(fā)揮兩個(gè)傳感器的共同優(yōu)勢(shì)，才能夠取得好的效果。于是，我們需要一種算法，能夠在短時(shí)間內(nèi)置信陀螺儀，而長(zhǎng)時(shí)間內(nèi)置信加速度計(jì)的算法。</p><p>　　這就需要卡爾曼濾波。</p><p>　　5.3卡爾曼濾波

44、原型</p><p>　　首先我們建立系統(tǒng)的狀態(tài)方程和測(cè)量方程，由于傾角和傾角角速度存在導(dǎo)數(shù)關(guān)系，系統(tǒng)傾斜真實(shí)角度可以用來(lái)做一個(gè)狀態(tài)向量。在該系統(tǒng)中，采用加速度傳感器估計(jì)出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應(yīng)的狀態(tài)方程和觀測(cè)方程</p><p>　　式中，為包含固定偏差的陀螺儀輸出角速度，為加速度計(jì)經(jīng)處理后得到的角度值，為陀螺儀測(cè)量噪聲，為加速度傳感器測(cè)量噪聲，為陀螺儀漂移誤差，

45、和相互獨(dú)立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。令為系統(tǒng)采樣周期，得到離散系統(tǒng)的狀態(tài)方程和測(cè)量方程：</p><p>　　同時(shí)，要估算k時(shí)刻的實(shí)際角度，就必須根據(jù)k-1時(shí)刻的角度值，再根據(jù)預(yù)測(cè)得到的k時(shí)刻的角度值得到k時(shí)刻的高斯噪聲的方差，在此基礎(chǔ)之上卡爾曼濾波器進(jìn)行遞歸運(yùn)算直至估算出最優(yōu)的角度值。在此,須知道系統(tǒng)過(guò)程噪聲協(xié)方差陣Q以及測(cè)量誤差的協(xié)方差矩陣R, 對(duì)卡爾曼濾波器進(jìn)行校正，Q與R矩陣的形式如下：&l

46、t;/p><p>　　式中。和分別是加速度傳感器和陀螺儀測(cè)量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對(duì)其傳感器數(shù)據(jù)的信任程度，數(shù)值越小表明信任程度越高。在該系統(tǒng)中陀螺儀的值更為接近準(zhǔn)確值，因此取的值小于的值。當(dāng)前狀態(tài):</p><p><b>　?。?）</b></p><p>　　式中，，，是利用k預(yù)測(cè)的結(jié)果，是k - 1 時(shí)刻的最優(yōu)結(jié)果。</p&

47、gt;<p>　　則有對(duì)應(yīng)于的協(xié)方差為：</p><p><b>　?。?）</b></p><p>　　式中，是對(duì)應(yīng)的協(xié)方差，表示的轉(zhuǎn)置矩陣，是系統(tǒng)過(guò)程的協(xié)方差，式子（1）、（2）即對(duì)系統(tǒng)的狀態(tài)更新。</p><p>　　則狀態(tài)k的最優(yōu)化估算值：</p><p><b>　　（3）</b&

48、gt;</p><p>　　其中,為卡爾曼增益：</p><p><b>　?。?）</b></p><p>　　此時(shí)，我們已經(jīng)得到了k狀態(tài)下最有的估算值，但是為了使卡爾曼濾波器不斷的運(yùn)行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差：</p><p><b>　?。?）</b></p

49、><p>　　其中為單位陣，對(duì)于本系統(tǒng)則有。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，就是式子（2）的。（3）、（4）、（5）式為卡爾曼濾波器狀態(tài)更新方程。計(jì)算完時(shí)間更新方程后，再重復(fù)上一次計(jì)算得到的后驗(yàn)估計(jì)，作為下一次的計(jì)算的先驗(yàn)估計(jì)，這樣，周而復(fù)始、循環(huán)往復(fù)地運(yùn)算下去直至找到最優(yōu)的結(jié)果。</p><p>　　5.4卡爾曼濾波化簡(jiǎn)</p><p>　　上述五個(gè)公式便是卡爾曼濾波的五條數(shù)

50、學(xué)公式在本項(xiàng)目中的使用。但是，以上五個(gè)公式，僅僅只是矩陣形式。雖然可以在MATLAB里進(jìn)行仿真，但是卻不能使用單片機(jī)進(jìn)行有效運(yùn)算。</p><p>　　然后，將卡爾曼濾波矩陣形式轉(zhuǎn)化為方程形式：</p><p>　　這里，X01即為我們需要的角度卡爾曼濾波值。</p><p>　　可以看到的是，卡爾曼濾波方程形式共有14個(gè)公式，同時(shí)很多參數(shù)的運(yùn)算涉及浮點(diǎn)數(shù)，這對(duì)于單

51、片機(jī)的高效運(yùn)行時(shí)極為不利的，因此我們需要通過(guò)一些方法對(duì)卡爾曼濾波的公式進(jìn)行化簡(jiǎn)，從而在不失精度的情況下，盡可能減小運(yùn)算量。</p><p>　　需要注意的是，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都是不同的。</p><p>　　根據(jù)相關(guān)參考文獻(xiàn)，Q,R雖然都是關(guān)于時(shí)間的

52、變量，但是由于卡爾曼濾波有很好的收斂性，所以可以將Q，R都取比較極端的參數(shù)。用常量來(lái)定義。</p><p><b>　　然后再看下圖</b></p><p>　　注意方框中的公式，根據(jù)我們的觀察，不難發(fā)現(xiàn)，整個(gè)方框中都是為了獲得卡爾曼增益（矩陣Kg），我們?cè)O(shè)想，能否使用一個(gè)常數(shù)來(lái)等效替代卡爾曼增益那？根據(jù)我們?cè)趯?shí)驗(yàn)中的觀察，卡爾曼增益是一個(gè)收斂的變量，并且針對(duì)到我們的

53、這個(gè)系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個(gè)常數(shù)。（所有這些工作都在MATLAB下完成）</p><p>　　所以，我們將方框中的所有公式完全省略，通過(guò)實(shí)驗(yàn)整定，選取一個(gè)近似Kg來(lái)替代方框中的所有運(yùn)算。同時(shí)，通過(guò)NATLAB觀測(cè)各變量的變化趨勢(shì)，我們嘗試讓Kg2=0（Kg1，Kg2本身就是非常小的變量，所以可以讓其等于0）。并發(fā)現(xiàn)Kg2對(duì)于整個(gè)卡爾曼濾波的影響非常有限。所以將Kg2設(shè)定為0。</p>

54、<p>　　以下就是我們組程序中卡爾曼濾波的簡(jiǎn)化算法實(shí)現(xiàn)</p><p>　　X1=X0+gyro*dt;</p><p>　　X0=X1+(acc-X1)*kg;</p><p>　　化簡(jiǎn)后的卡爾曼濾波框圖如下：</p><p>　　5.5卡爾曼濾波參數(shù)整定</p><p>　　化簡(jiǎn)后的卡爾曼濾波主要是對(duì)

55、dt以及Kg兩個(gè)參數(shù)進(jìn)行整定。需要特別指出的是，這里的dt并不只是一個(gè)采樣間隔。由于陀螺儀的輸出和加速度計(jì)輸出的量綱并不相同，所以陀螺儀采樣值*dt并不直接反應(yīng)一個(gè)角度，而是與實(shí)際角度相差一個(gè)系數(shù)。因此此處的dt可以等效理解為dt=采樣間隔*比例系數(shù)。</p><p>　　dt越大，積分速度越快，卡爾曼輸出追隨實(shí)際角度的情況越好（當(dāng)然不能太大，不然可能會(huì)出現(xiàn)超前相位）。但是dt越大，陀螺儀漂移造成的影響也就越大。

56、</p><p>　　Kg決定了加速度計(jì)的權(quán)重。Kg越大，實(shí)際輸出的漂移就越小，但是濾波效果的噪聲也就越大。</p><p>　　所以dt和kg是一對(duì)矛盾，不能太過(guò)于極端。</p><p><b>　　首先是靜態(tài)整定。</b></p><p>　　將車模保持在穩(wěn)定直立狀態(tài)，讓車輪以恒定PWM（80%以上）轉(zhuǎn)動(dòng)，然后調(diào)節(jié)參

57、數(shù)。</p><p>　　目標(biāo)是達(dá)到上圖效果。紅色為卡爾曼濾波輸出，藍(lán)色為加速度計(jì)直接輸出。如果紅色抖動(dòng)非常厲害，可以適當(dāng)減小Kg的大小。</p><p><b>　　接著是動(dòng)態(tài)整定</b></p><p>　　還是保持車輪恒定PWM旋轉(zhuǎn)，同時(shí)，搖擺車身。</p><p>　　大致調(diào)節(jié)到上圖所示的樣子。目前存在一個(gè)相位差

58、，這個(gè)相位差在后期直立算法的調(diào)試中是致命的，必須克服掉。解決方法，逐漸增加dt即可。</p><p>　　達(dá)到上圖效果即可認(rèn)為卡爾曼濾波參數(shù)整定完成。</p><p><b>　　程序控制說(shuō)明</b></p><p>　　在完成了信號(hào)處理的層面之后，就是對(duì)于控制算法的設(shè)計(jì)。在設(shè)計(jì)算法之前，讓我們對(duì)于系統(tǒng)進(jìn)行一個(gè)簡(jiǎn)單分析。系統(tǒng)雖然是高階非線性系統(tǒng)

59、，但是由于直立期間，傾斜角度非常小，所以可等等效為一個(gè)線性系統(tǒng)?？刂戚斎胗腥齻€(gè)，分別對(duì)應(yīng)角度，角速度，車輪轉(zhuǎn)速。被控變量有兩個(gè)：角度以及車輪轉(zhuǎn)速。所以這是一個(gè)MIMO系統(tǒng)，因此不能使用簡(jiǎn)單的PID等SISO控制器進(jìn)行控制。同時(shí)，無(wú)論是控制角度還是控制電機(jī)，執(zhí)行機(jī)構(gòu)都為電機(jī)，所以系統(tǒng)也具有強(qiáng)耦合性。對(duì)于倒立擺的控制，參考的文獻(xiàn)中有使用各種算法進(jìn)行控制，比如狀態(tài)反饋、自適應(yīng)算法、神經(jīng)網(wǎng)絡(luò)算法、模糊算法等等。這些算法都需要更高深的理論基礎(chǔ)，同

60、時(shí)，在高階倒立擺系統(tǒng)中，這些算法擁有不可替代性。但是對(duì)于我們的兩輪車式倒立擺，這僅僅只是一個(gè)一階倒立擺，可以使用更簡(jiǎn)單的方法進(jìn)行控制。我們?cè)诒驹O(shè)計(jì)中使用了角度、速度雙閉環(huán)的控制算法。角度控制頻率高，速度控制周期頻率慢。</p><p><b>　　6.1電機(jī)死區(qū)補(bǔ)償</b></p><p>　　由于摩擦力等因素的存在，電機(jī)與PWM給定信號(hào)存在非線性死去特性。為了使系統(tǒng)

61、具有更好的線性，首先需要對(duì)電機(jī)死區(qū)進(jìn)行補(bǔ)償。</p><p>　　6.2角度單閉環(huán)控制</p><p>　　首先我們忽略速度控制，僅僅只考慮角度閉環(huán)系統(tǒng)。那么在這個(gè)前提下，只有兩個(gè)速度量：角度和角速度；被控變量只有角度。那么就可以使用PID控制器。這里需要進(jìn)行解釋，為何有兩個(gè)輸入變量任然可以使用SISO系統(tǒng)才能使用PID。雖然角度和角速度是不同的量，但是角速度就是對(duì)角度求微分。而使用陀螺儀

62、獲得的角速度比對(duì)卡爾曼濾波后求微分獲得的角速度更準(zhǔn)確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒(méi)有問(wèn)題。</p><p>　　常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對(duì)于這個(gè)需要快速調(diào)整的系統(tǒng)，要求不大。因此最后我們選擇了PD控制器。</p><p><b>　　控制系

63、統(tǒng)框圖如下：</b></p><p>　　在實(shí)際調(diào)試過(guò)程中，倒立擺可以進(jìn)行短時(shí)間的直立，但是會(huì)朝向一個(gè)方向加速運(yùn)行，最后車模會(huì)倒下。這是由于未加速速度閉環(huán)的結(jié)果，未對(duì)車輪轉(zhuǎn)速進(jìn)行必要的控制，則最終由于電機(jī)轉(zhuǎn)速的飽和，而發(fā)生了無(wú)法對(duì)倒立擺進(jìn)行調(diào)整的后果。</p><p>　　為了解決這個(gè)問(wèn)題，必須要引入速度閉環(huán)控制。</p><p>　　6.3角度、速度雙

64、閉環(huán)控制</p><p>　　對(duì)于直立車模速度的控制相對(duì)于普通車模的速度控制則比較復(fù)雜。由于在速度控制過(guò)程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過(guò)改變電機(jī)轉(zhuǎn)速來(lái)實(shí)現(xiàn)。下面先分析一下引起車模速度變化的原因。</p><p>　　假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實(shí)際測(cè)量的角度與車模角度有偏差，因此車模實(shí)際不是保持與地面垂直，而是存在一個(gè)傾

65、角。在重力的作用下，車模就會(huì)朝傾斜的方向加速前進(jìn)。</p><p>　　首先對(duì)一個(gè)簡(jiǎn)單例子進(jìn)行分析。假設(shè)車模開(kāi)始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。在車模直立控制下，為了能夠有一個(gè)往前的傾斜角度，車輪需要往后運(yùn)動(dòng)，這樣會(huì)引起車輪速度下降（因?yàn)檐囕喭?fù)方向運(yùn)動(dòng)了）。由于負(fù)反饋，使得車模往前傾角需要更大。如此循環(huán)，車模很快就會(huì)傾倒。原本利用負(fù)反饋進(jìn)行速度控制反而成了“正”反饋。在直立控制下

66、的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。但根據(jù)實(shí)際經(jīng)驗(yàn)，是可以通過(guò)速度控制進(jìn)行車模傾角控制的。</p><p>　　而速度閉環(huán)控制仍然可以看做是一個(gè)SISO系統(tǒng)。所以使用PID控制器來(lái)完成控制。由于系統(tǒng)對(duì)于靜態(tài)誤差存在一定要求，所以使用PI控制器。</p><p>　　正如前文所述，直立控制和速度控制存在強(qiáng)耦合，所以這里我們使用的分立控制周期

67、的方法。每10個(gè)直立控制周期進(jìn)行一次速度控制。這樣，可以將速度控制視為對(duì)于直立控制的一種擾動(dòng)。</p><p>　　下圖為速度閉環(huán)的控制框圖。</p><p>　　圖6.3 直立、速度閉環(huán)控制框圖</p><p>　　第七章車模參數(shù)調(diào)試圖解</p><p>　　在完成了所有控制算法方案的制定，以及程序代碼的編寫(xiě)之后，接下來(lái)需要對(duì)一些參數(shù)在實(shí)際

68、系統(tǒng)運(yùn)行中進(jìn)行整定以及微調(diào)。整定方法遵從先直立控制再速度控制；先P參數(shù)，后I參數(shù)，最后D參數(shù)的整定順序。本章節(jié)著重介紹在調(diào)試環(huán)節(jié)中不同現(xiàn)象對(duì)應(yīng)的原因以及參數(shù)調(diào)整方法。</p><p>　　在下文中，會(huì)使用如下幾個(gè)變量在此說(shuō)明。</p><p>　　dt:陀螺儀積分參數(shù)</p><p>　　Kg：加速度計(jì)參數(shù)（卡爾曼增益）</p><p>　　

69、角度閉環(huán)，速度開(kāi)環(huán)P：直立算法P參數(shù)</p><p>　　角度閉環(huán)，速度開(kāi)環(huán)D：直立算法D參數(shù)</p><p>　　速度閉環(huán)P：速度控制P參數(shù)</p><p>　　速度閉環(huán)I：速度控制I參數(shù)</p><p>　　原因：開(kāi)環(huán)P過(guò)大，系統(tǒng)震蕩</p><p>　　解決方法：減小開(kāi)環(huán)P或者增加開(kāi)環(huán)D</p>&

70、lt;p>　　原因：開(kāi)環(huán)D參數(shù)過(guò)大，系統(tǒng)抗干擾性降低</p><p>　　解決方法：減小開(kāi)環(huán)D，或者適當(dāng)增加開(kāi)環(huán)P</p><p>　　原因：傾角值為0時(shí)刻，車身重心不在軸線上</p><p>　　解決方法：微調(diào)加速度計(jì)零偏置，使得角度為0時(shí)，重心落在軸線上。也可能是陀螺儀零偏置錯(cuò)誤，同理，微調(diào)即可。</p><p>　　原因：陀螺儀積

71、分速度過(guò)慢，導(dǎo)致卡爾曼濾波相位滯后</p><p>　　解決方法：適當(dāng)增加dt</p><p>　　原因：速度閉環(huán)對(duì)系統(tǒng)整體的控制程度過(guò)小</p><p>　　解決方法：適當(dāng)增加速度閉環(huán)的各個(gè)參數(shù)</p><p>　　原因：速度閉環(huán)調(diào)節(jié)速度過(guò)慢</p><p>　　解決方法：適當(dāng)增加閉環(huán)P參數(shù)</p>&

72、lt;p>　　原因：速度閉環(huán)存在靜態(tài)誤差</p><p>　　解決方法：適當(dāng)增加閉環(huán)I參數(shù)</p><p>　　原因：速度閉環(huán)調(diào)節(jié)速度過(guò)慢</p><p>　　解決方法：適當(dāng)增加閉環(huán)P參數(shù)，或者也可以使用PID（而非PI）進(jìn)行速度閉環(huán)的控制</p><p>　　原因：速度控制對(duì)于階躍響應(yīng)存在超調(diào)</p><p>

73、　　解決方法：減小閉環(huán)P參數(shù)，或者加入閉環(huán)D參數(shù)，也可以不用階躍信號(hào)，而是通過(guò)斜波信號(hào)慢慢將速度提升。</p><p>　　原因：陀螺儀漂移之后和加速度傳感器之間的關(guān)系發(fā)生了變化，導(dǎo)致濾波角度出現(xiàn)比較大的問(wèn)題。</p><p>　　解決方法：增大Kg。</p><p><b>　　附件源代碼</b></p><p>　　

74、//******************端口初始化</p><p>　　void PAC_Init(void)</p><p><b>　　{</b></p><p>　　//**********計(jì)數(shù)器接口初始化 ---->ok(5.9)</p><p>　　DDRB = 0X00;

75、 //PORTB作為輸入</p><p>　　DDRA = 0X00;</p><p>　　DDRE_DDRE2 = 1;</p><p>　　DDRE_DDRE5 = 1;</p><p>　　DDRE_DDRE6 = 1;</p><p>　　DDRE_DDRE3 = 1;</p><p

76、>　　//***********計(jì)數(shù)器清零</p><p>　　PORTE_PE2 = 0;</p><p>　　PORTE_PE5 = 0;</p><p>　　PORTE_PE3 = 0;</p><p>　　PORTE_PE6 = 0;</p><p>　　//***********計(jì)數(shù)使能</p&g

77、t;<p>　　PORTE_PE2 = 1;</p><p>　　PORTE_PE5 = 1;</p><p>　　PORTE_PE3 = 0;</p><p>　　PORTE_PE6 = 0;</p><p>　　//***********************電機(jī)控制電路接口初始化 --->ok(5.9)</p

78、><p>　　PUCR = 0X50; //全部禁止上拉電阻</p><p>　　DDRK_DDRK2 =1; //電機(jī)IO接口</p><p>　　DDRK_DDRK3 =1; //電機(jī)IO接口</p><p>　　PORTK_PK2 =0;</p><p>　　PORTK_

79、PK3 =0;</p><p>　　//***********CCD接口初始化</p><p>　　DDRK_DDRK5 =1; //PORTK_PK1,PORTK_PK3輸出</p><p>　　DDRK_DDRK4 =1;</p><p>　　//***********數(shù)碼管接口電路</p><p>

80、　　DDRM = 0XFF; //輸出</p><p>　　DDRJ_DDRJ6 =1;</p><p><b>　　}</b></p><p>　　//*****************設(shè)置系統(tǒng)時(shí)鐘為64MHz</p><p>　　void SystemCLK_64M(void

81、)</p><p><b>　　{</b></p><p>　　CLKSEL = 0; //使PLL與系統(tǒng)隔離</p><p>　　PLLCTL_PLLON = 1; //打開(kāi)PLL</p><p>　　SYNR = 0Xc7; //設(shè)置 fvc

82、0=2*fosc*(SYNDIV+1)/(REFDIV+1);fpll=fvco/(2*POSTDIV);</p><p>　　//fbus=fpll/2;若POSTDIV=0,fbus=fvco/2;</p><p>　　REFDV = 0X81; //fbus = 2*16*(9+1)/(1+1)=80m;</p><p>　　PO

83、STDIV = 0; //fpll = fvco;</p><p>　　_asm(nop); //_asm匯編語(yǔ)言，nop做空操作；延時(shí)</p><p>　　_asm(nop);</p><p>　　while(CRGFLG_LOCK == 0); //頻率穩(wěn)定后再

84、操作</p><p>　　CLKSEL_PLLSEL = 1;</p><p><b>　　}</b></p><p>　　//******************* PIT中斷服務(wù)</p><p>　　void PIT_Init(void) //1ms周期中斷</p><

85、;p><b>　　{</b></p><p>　　PITCFLMT = 0; // PIT disabled,</p><p>　　PITCE_PCE0 = 1; //通道0使能</p><p>　　PITMUX_PMUX = 0;

86、//8位，16位定時(shí)器共同作用</p><p>　　PITMTLD0 = 79; //8位定時(shí)器初值設(shè)定，80</p><p>　　PITLD0 =799; //16位定時(shí)器初值設(shè)定 time_out period = (79+1)*(799+1)/64000000 = 1ms</p><p>　

87、　PITINTE_PINTE0 = 1; //0通道中斷有效</p><p>　　PITCFLMT_PITE=1; //定時(shí)器使能,初始化一次即可</p><p><b>　　}</b></p><p>　　/**************************************

88、**********</p><p>　　功能 :總線頻率為64MHz,設(shè)置SCI0模塊波特率為19200bps</p><p><b>　　入口參數(shù):無(wú)</b></p><p>　　出口參數(shù):無(wú) 40000/1152</p><p>　　******************************

89、**********************/</p><p>　　void SCI0_init(void)</p><p><b>　　{</b></p><p>　　//SCI0BD = 208; //設(shè)置波特率為9600Kbps(寫(xiě)一個(gè)字) AMAP = 0</p><p>　　//SCI0BD = 10

90、4; //設(shè)置波特率為19200Kbps(寫(xiě)一個(gè)字) AMAP = 0 --->總線頻率32MHz</p><p>　　//SCI0BD = 208; //設(shè)置波特率為19200Kbps(寫(xiě)一個(gè)字) AMAP = 0 --->總線頻率64MHz</p><p>　　//SCI0BD = 69; //設(shè)置波特率為57600Kbps(寫(xiě)一個(gè)字)

91、AMAP = 0 --->總線頻率64MHz</p><p>　　//SCI0BD = 35; //設(shè)置波特率為115200Kbps(寫(xiě)一個(gè)字) AMAP = 0 --->總線頻率64MHz</p><p>　　SCI0BD = 104; //設(shè)置波特率為38400Kbps(寫(xiě)一個(gè)字) AMAP = 0 --->總線頻率64MHz</p

92、><p>　　//SCI0BD = 139; //設(shè)置波特率為28800Kbps(寫(xiě)一個(gè)字) AMAP = 0 --->總線頻率64MHz</p><p>　　SCI0CR1 = 0X00; //SCI0工作在正常情況下,禁止奇偶校正,使用8為數(shù)據(jù)位(共10位)</p><p>　　SCI0CR2 = 0X2c; //允許使用發(fā)送器,使用接收

93、器,允許接收中斷</p><p><b>　　}</b></p><p>　　/***************************************************</p><p>　　函數(shù)功能:完成帶符號(hào)的char型數(shù)據(jù)傳送</p><p>　　入口參數(shù):char num,為要傳送的數(shù)據(jù)</p&

94、gt;<p><b>　　出口參數(shù):無(wú)</b></p><p>　　說(shuō)明 :每次傳送133個(gè)數(shù)據(jù)---->上位機(jī)定義的一幀數(shù)據(jù)</p><p>　　****************************************************/</p><p>　　void SCI0_Char_Send(un

95、signed char num[])</p><p><b>　　{</b></p><p>　　unsigned int i =0;</p><p>　　for(i=0;i<133;i++)</p><p><b>　　{</b></p><p>　　while(!S

96、CI0SR1_TDRE); //等待發(fā)送寄存器空</p><p>　　SCI0DRL = num[i]; //發(fā)送數(shù)據(jù)</p><p><b>　　}</b></p><p><b>　　}</b></p><p>　　/***************

97、*************************************</p><p>　　功能 :初始化定時(shí)器模塊(TIM模塊輸入撲捉功能初始化)</p><p>　　Ic0上升沿 , Ic1輸入撲捉下降沿</p><p><b>　　入口參數(shù):無(wú)</b></p><p><b>　　出口參數(shù):無(wú)&

98、lt;/b></p><p>　　******************************************************/</p><p>　　void Time_init_CCD(void){ //--->ok(5.9)</p><p>　　TSCR1 |= 0X90; //啟動(dòng)主定時(shí)器模塊,使用快速清零(

99、TFFCA=1)</p><p>　　TSCR2 |= 0X03; //禁止定時(shí)溢出中斷---定時(shí)器時(shí)鐘頻率為4MHz</p><p>　　//定時(shí)器頻率為總線頻率的8分頻</p><p>　　TIOS = 0Xf0; //啟用IC0,IC1,IC2,IC3輸入撲捉功能</p><p>　　TCTL4 = 0X

100、99; //IC0,IC2上升沿 , IC1,IC3輸入撲捉下降沿</p><p>　　TIE = 0X00; //禁止輸出撲捉申請(qǐng)中斷</p><p><b>　　}</b></p><p>　　/*****************************************************&l

101、t;/p><p>　　函數(shù)功能:對(duì)PWM初始化,設(shè)置PWM0,PWM1級(jí)聯(lián),PWM2,PWM3級(jí)聯(lián)</p><p><b>　　入口參數(shù):無(wú)</b></p><p><b>　　出口參數(shù):無(wú)</b></p><p>　　******************************************

102、************/</p><p>　　void PWM_init(void)</p><p><b>　　{</b></p><p>　　PWME = 0X00;</p><p>　　PWMPOL = 0XCf; //開(kāi)始輸出極性為高電平</p><p>　　PWMCLK =

103、0XC0; //PWM0,1選擇ClockA作為時(shí)鐘,PWM2,3選擇ClockB作為時(shí)鐘</p><p>　　PWMPRCLK = 0X00; //ClockA,ClockB對(duì)總線頻率0分頻--->64MHz</p><p>　　PWMCAE = 0X00; //全部使用左對(duì)齊</p><p>　　PWMCTL = 0X30;

104、 //使用 PWM0,PWM1級(jí)聯(lián),PWM2,PWM3級(jí)聯(lián)</p><p>　　PWMPER01 = 15000; //PWM頻率為4KHz</p><p>　　PWMPER23 = 15000;</p><p>　　PWMDTY01 = 0; //占空比為0</p><p>　　PWMDTY23 = 0;</p&g

105、t;<p>　　PWMSCLB = 20; //SB的分頻因子為40 --->ClockSB=1600KHz</p><p>　　PWMPER7 = 10; //左對(duì)齊,PWM7通道頻率為160KHz</p><p>　　PWMPER6 = 10; //左對(duì)齊,PWM6通道頻率為160KHz</p>&

106、lt;p>　　PWMDTY7 =5; //設(shè)置占空比為50%</p><p>　　PWMDTY6 =5; //設(shè)置占空比為50%</p><p>　　PWME = 0X0f; //允許PWM0,1,2,3</p><p><b>　　}</b></p><

107、p>　　/*******************************************************</p><p>　　函數(shù)功能:初始化AD轉(zhuǎn)換,轉(zhuǎn)換位數(shù)--8位,轉(zhuǎn)換的通道必須在調(diào)用函數(shù)之前設(shè)置</p><p>　　入口參數(shù):無(wú) ----------ATD的 fclk 可以達(dá)到8.3MHz</p><p><b>　　出口

108、參數(shù):無(wú)</b></p><p><b>　　使用方法:</b></p><p>　　ATD0CTL5 = 0X00; unsigned char resulut = ATD_single_send_uchar();---->采樣AD0通道的數(shù)據(jù),單次轉(zhuǎn)換.</p><p>　　************************

109、************************************/</p><p>　　void ATD_init(void)</p><p><b>　　{</b></p><p>　　ATD0DIEN = 0X00; //全部使用模擬輸入</p><p>　　ATD0CTL1 = 0

110、X00; //采樣前不放電,0x02-->分辨率為12位 0x00-->分辨率為8位 0x01-->分辨率為10位</p><p>　　ATD0CTL2 = 0X40; //A/D模塊快速清零,禁止外部觸發(fā),禁止中斷</p><p>　　ATD0CTL3 = 0X88; //A/D轉(zhuǎn)換結(jié)果右

111、對(duì)齊,每個(gè)序列1個(gè)通道,非FIFO</p><p>　　ATD0CTL4 = 0X04; //采樣時(shí)間為ATD時(shí)鐘周期的4倍,ATD時(shí)鐘為系統(tǒng)時(shí)鐘的10分頻 ---6.4MHZ</p><p>　　ATD0CTL5 = 0X20; //連續(xù)轉(zhuǎn)換,單獨(dú)對(duì)某通道采樣 ,并啟動(dòng)轉(zhuǎn)換---具體通道需要在調(diào)用它之前</p><p

112、>　　} //end of ATD0_Init</p><p>　　/*****************************************************</p><p>　　函數(shù)功能:轉(zhuǎn)換一個(gè)模擬量---8位轉(zhuǎn)換</p><p>　　入口參數(shù): unsigned char result</p><p>　　出

113、口參數(shù): unsigned char result ---啟動(dòng)單次裝換</p><p>　　******************************************************/</p><p>　　unsigned char ATD_uchar_convert(void)</p><p><b>　　{</b><

114、/p><p>　　while(!ATD0STAT2L_CCF0); //等待轉(zhuǎn)換結(jié)束</p><p>　　return (ATD0DR0); //返回轉(zhuǎn)換結(jié)果</p><p><b>　　}</b></p><p>　　/***************************

115、********************************</p><p>　　函數(shù)功能:在系統(tǒng)頻率為64MHz下,延時(shí)約1us</p><p><b>　　入口參數(shù):無(wú)</b></p><p><b>　　出口參數(shù):無(wú)</b></p><p>　　**********************

116、**************************************/</p><p>　　void delay_1us(void)</p><p><b>　　{</b></p><p><b>　　int j;</b></p><p>　　for(j=0;j<6;j++)</

117、p><p>　　_asm(nop);</p><p><b>　　}</b></p><p>　　/******************************************************</p><p>　　函數(shù)功能:數(shù)碼管顯示</p><p><b>　　入口參數(shù):數(shù)