【文章內(nèi)容簡介】 。 當前狀態(tài) : )()1|1()1|( kBUkkAXkkX ????? （ 1） 式中 ， 101sTA ????????，0sTB ???????， ( | 1)X k k? 是利用 k預測的結(jié)果 ， ( 1| 1)X k k??是 k 本科生單片機課程設計報告 21 1 時刻的最優(yōu)結(jié)果 。 則有對應于 )1|( ?kkX 的協(xié)方差為： QAkkAPkkP T ????? )1|1()1|( （ 2） 式中 ， )1|1( ?? kkP 是 )1|1( ?? kkX 對應的協(xié)方差， TA 表示 A 的轉(zhuǎn)置矩陣， Q 是系統(tǒng)過程的協(xié)方差，式子（ 1）、（ 2）即對系統(tǒng)的狀態(tài)更新。 則狀態(tài) k的最優(yōu)化估算值 ( | )Xk k ： ( | ) ( | 1 ) ( ) ( ( ) ( | 1 ) )X k k X k k K k Z k H X k k? ? ? ? ? （ 3） 其中 [1 0]H? ,K 為卡爾曼增益： ( ) ( | 1 ) / ( ( | 1 ) )TTK k P k k H H P k k H R? ? ? ? （ 4） 此時，我們已經(jīng)得到了 k狀態(tài)下最有的估算值 ( | )Xk k ，但是為了使卡爾曼濾波器不斷的運行下去直到找到最優(yōu)的角度值 ， 我們還要更新 k狀態(tài)下 ( | )Xk k 的協(xié)方差： ( | ) ( ( ) ) ( | 1 )P k k I K g k H P k k? ? ? （ 5） 其中 I 為單位陣 ， 對于本系統(tǒng)則有 11I ???????。當系統(tǒng)進入 k+1狀態(tài)時， ( | )Pk k 就是式子（ 2）的 ( 1| 1)P k k??。（ 3）、（ 4）、（ 5）式為卡爾曼濾波器狀態(tài)更新方程。計算完時間更新方程后，再重復上一次計算得到的后驗估計，作為下一次的計算的先驗估計，這樣，周而復始、循環(huán)往復地運算下去直至找到最優(yōu)的結(jié)果。 卡爾曼濾波化簡 1039。1039。110 1 101***** / * * ** ( * )( * ) *X A X B gy roP A P A QK g P H H P H RX X K g ac c H XP I K g H P????? ? ??? 上述五個公式便是卡爾曼濾波的五條數(shù)學公式在本項目中的使用。但是，以上五個公式，僅僅只是矩陣形式。雖然可以在 MATLAB 里進行仿真，但是卻不能使用單片機進行有效運算。 然后，將卡爾曼濾波矩陣形式轉(zhuǎn)化為方程形式： 本科生單片機課程設計報告 22 11 01 0212 0211 01 03 02 04 112 02 0413 03 0414 04 21 11 112 13 1101 11 102 12 103 13 11 204 14 12 201 11*** * ( * )**/ ( )/ ( )* ( 1 )* ( 1 )**(X X dt gy r o dt XXXP P dt P dt P dt P QP P dt PP P dt PP P QK g P P RK g P P RP P K gP P K gP P P K gP P P K gX X a? ? ??? ? ? ? ?????????????????????11 102 12 11 2)*( ) *c c X K gX X ac c X K g?? ? ? 這里， 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ù)來等效替代卡爾曼增益那？根據(jù)我們在實驗 本科生單片機課程設計報告 23 中的觀察，卡爾曼增益是一個收斂的變量，并且針對到我們的這個系統(tǒng)，他的值非常小，直立狀態(tài)下趨近于一個常數(shù)。（所有這些工作都在 MATLAB下完成） 所以，我們將方框中的所有公式 完全省略，通過實驗整定，選取一個近似 Kg來替代方框中的所有運算。同時，通過 NATLAB觀測各變量的變化趨勢，我們嘗試 讓 Kg2=0（ Kg1， Kg2本身就是非常小的變量，所以可以讓其等于 0）。并發(fā)現(xiàn) Kg2對于整個卡爾曼濾波的影響非常有限。所以將 Kg2設定為 0。 以下就是我們組程序中卡爾曼濾波的 簡化 算法實現(xiàn) X1=X0+gyro*dt。 X0=X1+(accX1)*kg。 化簡后的卡爾曼濾波框圖如下： 卡爾曼濾波參數(shù)整定 化簡后的卡爾曼濾波主要是對 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ù)。 本科生單片機課程設計報告 24 目標是達到上圖效果。紅色為卡爾曼濾波輸出，藍色為加速度計直接輸出。如果紅色抖動非常厲害，可以適當減小 Kg的大小。 接著是動態(tài)整定 還是保持車輪恒定 PWM旋轉(zhuǎn)，同時，搖擺車身。 大致調(diào)節(jié)到上圖所示的樣子。目前存在一個相位差，這個相位差 在后期直立算法的調(diào)試中 是致命的，必須克服掉。解決方法，逐漸增加 dt 即可。 達到上圖效果即可認為卡爾曼濾波參數(shù)整定完成。 本科生單片機課程設計報告 25 第六章 程序控制說明 在完成了信號處理的層面之后，就是對于控制算法的設計。在設計算法之前，讓我們對于系統(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)絡算法、模糊算法等等。這些算法都需要更高深的理論基礎，同時，在高階倒立擺系統(tǒng)中，這些算法擁有不可替代性。但是對于我們的兩輪車式倒立擺，這僅僅只是一個一階倒立擺，可以使用更簡單的方法進行控制。我們在本設計中使用了角度、速度雙閉環(huán)的控制算法。 角度控制頻率高 ，速度控制 周期頻率慢 。 電機死區(qū)補償 由于摩擦力等因素的存在，電機與 PWM給定信號存在非線性死去特性。為了使系統(tǒng)具有更好的線性，首先需要對電機死區(qū)進行補償。 角度單閉環(huán)控制 首先我們忽略速度控制，僅僅只考慮角度閉環(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)整的 本科生單片機課程設計報告 26 系統(tǒng)，要求不大。因此最后我們選擇了 PD 控制器。 控制系統(tǒng)框圖如下： 在實際調(diào)試過程中，倒立擺可以進行短時間的直立，但是會朝向一個方向加速運行，最后車模會倒下。這是由于未加速速度閉環(huán)的結(jié)果，未對車輪轉(zhuǎn)速進行必要的控制，則最終由于電機轉(zhuǎn)速的飽和，而發(fā)生了無法對倒立擺進行調(diào)整的后果。 為了解決這個問題，必須要引入速度閉環(huán)控制。 角度、速度雙閉環(huán)控制 對于直立車模速度的控制相對于普通車模的速度控制則比較復雜。由于在速度控制過程中需要始終 保持車模的平衡，因此車模速度控制不能夠直接通過改變電機轉(zhuǎn)速來實現(xiàn)。下面先分析一下引起車模速度變化的原因。 假設車模在上面直立控制調(diào)節(jié)下已經(jīng)能夠保持平衡了，但是由于安裝誤差，傳感器實際測量的角度與車模角度有偏差，因此車模實際不是保持與地面垂直，而是存在一個傾角。在重力的作用下，車模就會朝傾斜的方向加速前進。 首先對一個簡單例子進行分析。假設車模開始保持靜止，然后增加給定速度，為此需要車模往前傾斜以便獲得加速度。在車模直立控制下，為了能夠有一個往前的傾斜角度，車輪需要往后運動，這樣會引起車輪速度下降（因為車輪往負方向運動了）。由于負反饋，使得車模往前傾角需要更大。如此循環(huán)，車模很快就會傾倒。原本利 用負反饋進行速度控制反而成了 “ 正 ” 反饋。 在直立控制下的車模速度與車模傾角之間傳遞函數(shù)具有非最小相位特性，在反饋控制下容易造成系統(tǒng)的不穩(wěn)定性。但根據(jù)實際經(jīng)驗，是可以通過速度控制進行車模傾角控制的。 而速度閉環(huán)控制仍然可以看 做是一個 SISO系統(tǒng)。所以使用 PID控制器來完成控制。由于系統(tǒng)對于靜態(tài)誤差存在一定要求，所以使用 PI控制器。 正如前文所述， 直立控制和速度控制 存在 強耦合 ，所以這里我們使用的分立控制周期的方法。每 10個直立控制周期 進行一次速度控制。這樣，可以將速度控制視為對于直立控制的一種擾動。 下圖為速度閉環(huán)的控制框圖。 本科生單片機課程設計報告 27 本科生單片機課程設計報告 28 圖 直立、速度閉環(huán)控制框圖 本科生單片機課程設計報告 29 第七章車模參數(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 本科生單片機課程設計報告 30 原因：傾角值為 0時刻，車身重心不在軸線上 解決方法：微調(diào)加速度計零偏置，使得角度為 0時，重心落在 軸線上。也可能是陀螺儀零偏置錯誤，同理，微調(diào)即可。 原因：陀螺儀積分速度過慢，導致卡爾曼濾波相位滯后 解決方法：適當增加 dt 原因：速度閉環(huán)對系統(tǒng)整體的控制程度過小 解決方法：適當增加速度閉環(huán)的各個參數(shù) 本科生單片機課程設計報告 31 原因：速度閉環(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)的控制 本科生單片機課程設計報告 32 原因：速度控制對于階躍響應存在超調(diào) 解決方法：減小閉環(huán) P參數(shù)，或者加入閉環(huán) D參數(shù)，也可以不用階躍信號，而是通過