【正文】 于方案二。經(jīng)過綜合比較和測試，選用方案三。第四章 車模硬件電路設(shè)計詳見原理圖。第五章 卡爾曼濾波卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計動態(tài)系統(tǒng)的狀態(tài)。簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。對于解決很大部分的問題，他是最優(yōu)，效率最高甚至是最有用的。他的廣泛應用已經(jīng)超過30年，包括機器人導航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導彈追蹤等等。近年來更被應用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。當決定使用加速度計（反應角度）以及陀螺儀（反應角速度）以后，我們對參考的論文中為何要同時使用加速度計以及陀螺儀產(chǎn)生過一些疑問。因為如果需要角速度，我們可以通過這樣的方法對加速度計進行微分，計算出角速度，或者可以通過對于陀螺儀積分，從而獲得角度。既然如此為何我們需要使用兩個傳感器。于是，我們對兩個傳感器進行了數(shù)據(jù)采樣，觀測其輸出信號的關(guān)系。由于考慮電機轉(zhuǎn)動產(chǎn)生的震動會對傳感器產(chǎn)生比較大的干擾， 隨著PWM占空比的不斷提高，陀螺儀的噪聲也更加顯著。之后我們對加速度傳感器進行了測試。我們不難獲得以下一些結(jié)論：陀螺儀的噪聲還在可以接受范圍，但是加速度計噪聲已經(jīng)遠遠超出可控范圍接受對于加速度計做一個補充說明，加速度計在車模直立狀態(tài)下，電機PWM占空比100%噪聲情況下，幅值映射到實際角度大約是60度左右。根本不能直接使用。方差（可以等效理解為噪聲大小）隨著占空比不斷增加。加速度計雖然噪聲很大，但是長時間來看，他的數(shù)學期望是非常穩(wěn)定的。陀螺儀的噪聲比較小，但是他的誤差會對積分造成漂移。無論是加速度計還是陀螺儀，都有自己的優(yōu)勢和劣勢。簡而言之：加速度計短時間內(nèi)不可信，但是長時間內(nèi)可信。陀螺儀短時間內(nèi)可信而長時間內(nèi)不可信。這也就是為什么車模直立必須使用兩個傳感器來完成信號獲取。因為單個傳感器不可靠，只有取其精華去其糟粕，發(fā)揮兩個傳感器的共同優(yōu)勢，才能夠取得好的效果。于是，我們需要一種算法，能夠在短時間內(nèi)置信陀螺儀，而長時間內(nèi)置信加速度計的算法。這就需要卡爾曼濾波。首先我們建立系統(tǒng)的狀態(tài)方程和測量方程，由于傾角和傾角角速度存在導數(shù)關(guān)系，系統(tǒng)傾斜真實角度可以用來做一個狀態(tài)向量。在該系統(tǒng)中，采用加速度傳感器估計出陀螺儀常值偏差b，以此偏差作為狀態(tài)向量得到相應的狀態(tài)方程和觀測方程式中，為包含固定偏差的陀螺儀輸出角速度，為加速度計經(jīng)處理后得到的角度值，為陀螺儀測量噪聲，為加速度傳感器測量噪聲，為陀螺儀漂移誤差，和相互獨立，此處假設(shè)二者為滿足正態(tài)分布的白色噪聲。令為系統(tǒng)采樣周期，得到離散系統(tǒng)的狀態(tài)方程和測量方程：同時，要估算k時刻的實際角度，就必須根據(jù)k1時刻的角度值，再根據(jù)預測得到的k時刻的角度值得到k時刻的高斯噪聲的方差，在此基礎(chǔ)之上卡爾曼濾波器進行遞歸運算直至估算出最優(yōu)的角度值。在此,須知道系統(tǒng)過程噪聲協(xié)方差陣Q以及測量誤差的協(xié)方差矩陣R, 對卡爾曼濾波器進行校正，Q與R矩陣的形式如下：式中。和分別是加速度傳感器和陀螺儀測量的協(xié)方差，其數(shù)值代表卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度，數(shù)值越小表明信任程度越高。在該系統(tǒng)中陀螺儀的值更為接近準確值，因此取的值小于的值。當前狀態(tài): （1）式中，，是利用k預測的結(jié)果，是k 1 時刻的最優(yōu)結(jié)果。則有對應于的協(xié)方差為： （2）式中，是對應的協(xié)方差，表示的轉(zhuǎn)置矩陣，是系統(tǒng)過程的協(xié)方差，式子（1）、（2）即對系統(tǒng)的狀態(tài)更新。則狀態(tài)k的最優(yōu)化估算值： （3）其中,為卡爾曼增益： （4）此時，我們已經(jīng)得到了k狀態(tài)下最有的估算值，但是為了使卡爾曼濾波器不斷的運行下去直到找到最優(yōu)的角度值，我們還要更新k狀態(tài)下的協(xié)方差： （5）其中為單位陣，對于本系統(tǒng)則有。當系統(tǒng)進入k+1狀態(tài)時，就是式子（2）的。（3）、（4）、（5）式為卡爾曼濾波器狀態(tài)更新方程。計算完時間更新方程后，再重復上一次計算得到的后驗估計，作為下一次的計算的先驗估計，這樣，周而復始、循環(huán)往復地運算下去直至找到最優(yōu)的結(jié)果。上述五個公式便是卡爾曼濾波的五條數(shù)學公式在本項目中的使用。但是，以上五個公式，僅僅只是矩陣形式。雖然可以在MATLAB里進行仿真，但是卻不能使用單片機進行有效運算。然后，將卡爾曼濾波矩陣形式轉(zhuǎn)化為方程形式：這里，X01即為我們需要的角度卡爾曼濾波值?？梢钥吹降氖?，卡爾曼濾波方程形式共有14個公式，同時很多參數(shù)的運算涉及浮點數(shù)，這對于單片機的高效運行時極為不利的，因此我們需要通過一些方法對卡爾曼濾波的公式進行化簡，從而在不失精度的情況下，盡可能減小運算量。需要注意的是，Q,R兩個參數(shù)是關(guān)于傳感器和系統(tǒng)的方差，他們隨著系統(tǒng)的工作狀況不同而會產(chǎn)生相應變化，對應到我們的系統(tǒng)，在車模運行狀態(tài)不同（傾角不同，PWM不同）情況下，Q,R都是不同的。根據(jù)相關(guān)參考文獻，Q,R雖然都是關(guān)于時間的變量，但是由于卡爾曼濾波有很好的收斂性，所以可以將Q，R都取比較極端的參數(shù)。用常量來定義。然后再看下圖注意方框中的公式，根據(jù)我們的觀察，不難發(fā)現(xiàn)，整個方框中都是為了獲得卡爾曼增益（矩陣Kg），我們設(shè)想，能否使用一個常數(shù)來等效替代卡爾曼增益那？根據(jù)我們在實驗中的觀察，卡爾曼增益是一個收斂的變量，并且針對到我們的這個系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個常數(shù)。（所有這些工作都在MATLAB下完成）所以，我們將方框中的所有公式完全省略，通過實驗整定，選取一個近似Kg來替代方框中的所有運算。同時，通過NATLAB觀測各變量的變化趨勢，我們嘗試讓Kg2=0（Kg1，Kg2本身就是非常小的變量，所以可以讓其等于0）。并發(fā)現(xiàn)Kg2對于整個卡爾曼濾波的影響非常有限。所以將Kg2設(shè)定為0。以下就是我們組程序中卡爾曼濾波的簡化算法實現(xiàn)X1=X0+gyro*dt。X0=X1+(accX1)*kg。化簡后的卡爾曼濾波框圖如下：化簡后的卡爾曼濾波主要是對dt以及Kg兩個參數(shù)進行整定。需要特別指出的是，這里的dt并不只是一個采樣間隔。由于陀螺儀的輸出和加速度計輸出的量綱并不相同，所以陀螺儀采樣值*dt并不直接反應一個角度，而是與實際角度相差一個系數(shù)。因此此處的dt可以等效理解為dt=采樣間隔*比例系數(shù)。dt越大，積分速度越快，卡爾曼輸出追隨實際角度的情況越好（當然不能太大，不然可能會出現(xiàn)超前相位）。但是dt越大，陀螺儀漂移造成的影響也就越大。Kg決定了加速度計的權(quán)重。Kg越大，實際輸出的漂移就越小，但是濾波效果的噪聲也就越大。所以dt和kg是一對矛盾，不能太過于極端。首先是靜態(tài)整定。將車模保持在穩(wěn)定直立狀態(tài)，讓車輪以恒定PWM（80%以上）轉(zhuǎn)動，然后調(diào)節(jié)參數(shù)。目標是達到上圖效果。紅色為卡爾曼濾波輸出，藍色為加速度計直接輸出。如果紅色抖動非常厲害，可以適當減小Kg的大小。接著是動態(tài)整定還是保持車輪恒定PWM旋轉(zhuǎn)，同時，搖擺車身。大致調(diào)節(jié)到上圖所示的樣子。目前存在一個相位差，這個相位差在后期直立算法的調(diào)試中是致命的，必須克服掉。解決方法，逐漸增加dt即可。達到上圖效果即可認為卡爾曼濾波參數(shù)整定完成。第六章 程序控制說明在完成了信號處理的層面之后，就是對于控制算法的設(shè)計。在設(shè)計算法之前，讓我們對于系統(tǒng)進行一個簡單分析。系統(tǒng)雖然是高階非線性系統(tǒng)，但是由于直立期間，傾斜角度非常小，所以可等等效為一個線性系統(tǒng)?？刂戚斎胗腥齻€，分別對應角度，角速度，車輪轉(zhuǎn)速。被控變量有兩個：角度以及車輪轉(zhuǎn)速。所以這是一個MIMO系統(tǒng)，因此不能使用簡單的PID等SISO控制器進行控制。同時，無論是控制角度還是控制電機，執(zhí)行機構(gòu)都為電機，所以系統(tǒng)也具有強耦合性。對于倒立擺的控制，參考的文獻中有使用各種算法進行控制，比如狀態(tài)反饋、自適應算法、神經(jīng)網(wǎng)絡(luò)算法、模糊算法等等。這些算法都需要更高深的理論基礎(chǔ)，同時，在高階倒立擺系統(tǒng)中，這些算法擁有不可替代性。但是對于我們的兩輪車式倒立擺，這僅僅只是一個一階倒立擺，可以使用更簡單的方法進行控制。我們在本設(shè)計中使用了角度、速度雙閉環(huán)的控制算法。角度控制頻率高，速度控制周期頻率慢。由于摩擦力等因素的存在，電機與PWM給定信號存在非線性死去特性。為了使系統(tǒng)具有更好的線性，首先需要對電機死區(qū)進行補償。首先我們忽略速度控制，僅僅只考慮角度閉環(huán)系統(tǒng)。那么在這個前提下，只有兩個速度量：角度和角速度；被控變量只有角度。那么就可以使用PID控制器。這里需要進行解釋，為何有兩個輸入變量任然可以使用SISO系統(tǒng)才能使用PID。雖然角度和角速度是不同的量，但是角速度就是對角度求微分。而使用陀螺儀獲得的角速度比對卡爾曼濾波后求微分獲得的角速度更準確，而D控制器輸入變量正好是角速度，所以此處使用PID完全沒有問題。常用的PID控制器及其變形有P控制器、PI控制器、PD控制以及PID控制器。分析系統(tǒng)要求，僅有P控制器，系統(tǒng)極易發(fā)散造成不穩(wěn)定，而積分環(huán)節(jié)對于這個需要快速調(diào)整的系統(tǒng)，要求不大。因此最后我們選擇了PD控制器?？刂葡到y(tǒng)框圖如下：在實際調(diào)試過程中，倒立擺可以進行短時間的直立，但是會朝向一個方向加速運行，最后車模會倒下。這是由于未加速速度閉環(huán)的結(jié)果，未對車輪轉(zhuǎn)速進行必要的控制，則最終由于電機轉(zhuǎn)速的飽和，而發(fā)生了無法對倒立擺進行調(diào)整的后果。為了解決這個問題，必須要引入速度閉環(huán)控制。、速度雙閉環(huán)控制對于直立車模速度的控制相對于普通車模的速度控制則比較復雜。由于在速度控制過程中需要始終保持車模的平衡，因此車模速度控制不能夠直接通過改變電機轉(zhuǎn)速來實現(xiàn)。下面先分析一下引起車模速度變化的原因。假設(shè)車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實際測量的角度與車模角度有偏差，因此車模實際不是保持與地面垂直，而是存在一個傾角。在重力的作用下，車模就會朝傾斜的方向加速前進。首先對一個簡單例子進行分析。假設(shè)車模開始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。在車模直立控制下，為了能夠有一個往前的傾斜角度，車輪需要往后運動，這樣會引起車輪速度下降（因為車輪往負方向運動了）。由于負反饋，使得車模往前傾角需要更大。如此循環(huán)，車模很快就會傾倒。原本利用負反饋進行速度控制反而成了“正”反饋。在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。但根據(jù)實際經(jīng)驗，是可以通過速度控制進行車模傾角控制的。而速度閉環(huán)控制仍然可以看做是一個SISO系統(tǒng)。所以使用PID控制器來完成控制。由于系統(tǒng)對于靜態(tài)誤差存在一定要求，所以使用PI控制器。正如前文所述，直立控制和速度控制存在強耦合，所以這里我們使用的分立控制周期的方法。每10個直立控制周期進行一次速度控制。這樣，可以將速度控制視為對于直立控制的一種擾動。下圖為速度閉環(huán)的控制框圖。 直立、速度閉環(huán)控制框圖第七章車模參數(shù)調(diào)試圖解在完成了所有控制算法方案的制定，以及程序代碼的編寫之后，接下來需要對一些參數(shù)在實際系統(tǒng)運行中進行整定以及微調(diào)。整定方法遵從先直立控制再速度控制；先P參數(shù)，后I參數(shù)，最后D參數(shù)的整定順序。本章節(jié)著重介紹在調(diào)試環(huán)節(jié)中不同現(xiàn)象對應的原因以及參數(shù)調(diào)整方法。在下文中，會使用如下幾個變量在此說明。dt:陀螺儀積分參數(shù)Kg：加速度計參數(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，或者適當增加開環(huán)P原因：傾角值為0時刻，車身重心不在軸線上解決方法：微調(diào)加速度計零偏置，使得角度為0時，重心落在軸線上。也可能是陀螺儀零偏置錯誤，同理，微調(diào)即可。原因：陀螺儀積分速度過慢，導致卡爾曼濾波相位滯后解決方法：適當增加dt原因：速度閉環(huán)對系統(tǒng)整體的控制程度過小解決方法：適當增加速度閉環(huán)的各個參數(shù)原因：速度閉環(huán)調(diào)節(jié)速度過慢解決方法：適當增加閉環(huán)P參數(shù)原因：速度閉環(huán)存在靜態(tài)誤差解決方法：適當增加閉環(huán)I參數(shù)原因：速度閉環(huán)調(diào)節(jié)速度過慢解決方法：適當增加閉環(huán)P參數(shù)，或者也可以使用PID（而非PI）進行速度閉環(huán)的控制原因：速度控制對于階躍響應存在超調(diào)解決方法：減小閉環(huán)P參數(shù)，或者加入閉環(huán)D參數(shù)，也可以不用階躍信號，而是通過斜波信號慢慢將速度提升。原因：陀螺儀漂移之后和加速度傳感器之間的關(guān)系發(fā)生了變化，導致濾波角度出現(xiàn)比較大的問題。解決方法：增大Kg。附件源代碼//******************端口初始化void PAC_Init(void){ //**********計數(shù)器接口初始化 ok() DDRB = 0X00。 //PORTB作為輸入 DDRA = 0X00。 DDRE_DDRE2 = 1。 DDRE_DDRE5 = 1。 DDRE_DDRE6 = 1。 DDRE_DDRE3 = 1。 //***********計數(shù)器清零 PORTE_PE2 = 0。 PORTE_PE5 = 0。 PORTE_PE3 = 0。 PORTE_PE6 = 0。 //***********計數(shù)使能 PORTE_PE2 = 1。 PORTE_PE5 =