基于卡爾曼濾波的倒立擺系統(tǒng)角度信號處理研究

周輝權，孫 華，冀 淵，丁 偉

（西華大學機械工程與自動化學院，四川 成都 610039）

0 引 言

倒立擺系統(tǒng)是一個非線性、強耦合、多變量和自然不穩(wěn)定的系統(tǒng),倒立擺通常是用來檢驗控制策略效果的系統(tǒng),也是控制理論研究中較為理想的實驗裝置[1]。又因它與火箭飛行器及單足機器人有很大的相似之處,引起國內外學者的廣泛關注。其控制方法在軍工、航天、機器人領域和一般工業(yè)過程中都有著廣泛的用途。如機器人行走過程中的平衡控制,火箭發(fā)射中的垂直度控制和衛(wèi)星飛行中的姿態(tài)控制等均涉及到倒置問題,而其中反饋信號的濾波成為一個要解決與研究的問題。

1 卡爾曼濾波的介紹

卡爾曼濾波算法是由美國學者RudolfE.Kalman[2]在20世紀60年代初提出的一種最小方差意義上的最優(yōu)預測估計方法，便于計算機實時處理，它提供了直接處理隨機噪聲干擾的解決方案，將參數誤差看作噪聲以及把預估計量作為空間狀態(tài)變量，充分利用所測量的數據，用遞推法將系統(tǒng)及測量隨機噪聲濾掉，得到準確的空間狀態(tài)值。

卡爾曼濾波算法流程為：

1）第一步為預估：

2）第二步為增益矩陣計算：

3）第三步為狀態(tài)更新：

4）卡爾曼濾波器是一種線性的離散時間有限維系統(tǒng)。每次完成一次估計后，都要把新的方差遞歸，使濾波后的狀態(tài)估計誤差的相關矩陣P(k+1|k+1)的跡最小化。這意味著，卡爾曼濾波器是狀態(tài)向量X(k)的線性最小方差估計。EKF算法收斂的必要條件是：P(k|k)陣與P(k+l|k)陣為對稱正定陣。若P(k|k)→∞、K(k+1)→0，則算法發(fā)散。而P陣初值的選取也影響EKF算法的收斂，對不同的非線性系統(tǒng)取值不同，仍主要依靠試湊[3]。

2 倒立擺系統(tǒng)中傳感器的介紹

2.1 加速度傳感器

倒立擺系統(tǒng)中加速度計傳感器選擇為飛思卡爾公司的MMA7361，MMA7361是一塊三軸的模擬加速度計傳感器，可以工作在（±1.5）G和（±6）G兩種狀態(tài)，工作在（±1.5）G模式時為800 mV/g，體積小僅為3 mm×5 mm×1.0 mm。

該加速度傳感器可以測量重力或者物體運動所產生的加速度，該加速度傳感器是通過微機械加工技術在硅片上形成的一個機械懸臂。它與相鄰的電極形成兩個電容。由于加速度使得機械懸臂與兩個電極之間的距離發(fā)生變化，從而改變了兩個電容的參數。通過集成的開關電容放大電路測量電容參數的變化，形成了與加速度成正比的電壓輸出。

加速度在靜止的狀態(tài)下，輸出的電壓和傾角存在有一種三角函數關系，在靜態(tài)情況下，通過運算可以很準確地得到倒立擺系統(tǒng)的真實角度。但是在運動過程中，由于外界產生了非重力加速度，對其電壓輸出產生很大的干擾，它疊加在重力分量上，使得輸出信號無法精確地反映倒立擺系統(tǒng)真實的角度[4]。

2.2 陀螺儀傳感器

陀螺儀傳感器選擇的是村田公司的ENC-03RC,這塊傳感器的測量范圍為-300°/s~+300°/s，輸出0~3.3 V的模擬信號，在未測量到角速度信號時輸出的電壓為1.35 V。

陀螺儀輸出的是角速度，信號不受運動的影響，所以器件本身輸出信號噪聲非常小，倒立擺系統(tǒng)的角度可以通過角速度積分而得到，這樣可以獲得比較準確的角度。但是由于陀螺儀本身的角速度信號有微小的偏差，經過積分運算后，就會有累計誤差，如果不處理，這個誤差會一直積累，最終會導致數據飽和，根本無法獲得真實的角度。

2.3小結

通過實驗發(fā)現(xiàn)，不管是單獨使用加速度計還是陀螺儀都無法得到倒立擺系統(tǒng)的精確角度。它們兩者各有優(yōu)缺點。加速度計在靜態(tài)下，準確性高，但是在動態(tài)時極其容易受到干擾。對于陀螺儀而言，由于由于陀螺儀本身的角速度信號有微小的偏差，在靜態(tài)的時候，一直會存在累計誤差產生，然而在動態(tài)的時候，它的抗干擾能力相當強?？梢钥闯觯绻瑫r使用加速度計和陀螺儀傳感器，配合一種算法，就完全解決了倒立擺系統(tǒng)最棘手的準確角度問題。顯然，要解決這個問題，必須要用一種精妙的慮波算法，把兩個傳感器的數據融合。顯然，卡爾曼慮波就是較好的選擇。

3 實現(xiàn)結果

卡爾曼濾波在時域中采用遞推方式進行，速度快，便于實時處理。本次試驗將陀螺儀積分得到的角度值和加速度儀得到的有誤差絕對角度值，將兩者當作有噪聲干擾的觀測值，通過分配不同的權重，最終濾波得出最優(yōu)預測估計值。濾波圖像，見圖1。

圖1 持續(xù)擾動濾波試驗圖

由圖1可知，在擾動的情況下，通過卡爾曼慮波算法，可以比較準確地得到倒立擺系統(tǒng)的正確角度。

4 結 論

為了提高倒立擺系統(tǒng)的控制性能，提高倒立擺系統(tǒng)的魯棒性。本文在介紹了卡曼濾波原理的基礎上，并使用卡曼濾波對角度信號進行了很好的處理，從實驗結果來看，該方法效果比較理想，對倒立擺系統(tǒng)的穩(wěn)定性和抗干擾性有很大的提高，動態(tài)響應好，具有較大的實際應用價值。

[1] 王曉娟，王宣銀.基于模糊卡爾曼濾波的移動機器人定位研究[J].浙江大學,2011（2）:204-206.

[2] 王睿，閻鵬，劉紅英，等.基于擴展卡爾曼濾波的艦機相對位姿估測[J].北京航空航天大學學報，2006（11）：1349-1353.

[3] 李理敏，龔文斌，劉會杰，等.基于自適應擴展卡爾曼濾波的載波跟蹤算法[J].航空學報，2012（7）：1319-1328.

[4] 陳振，劉向東，勒永強，等.采用擴展卡爾曼濾波磁鏈觀測器的永磁同步電機轉矩控制[J].中國電機工程學報，2008，28（33）：75-81.

[5] 單波，徐燕.預測控制算法及其在倒立擺中的應用[J].華北電力大學學報，2001，8（2）：46-51.

[6] 叢爽，張冬軍，魏衡華.單級倒立擺三種控制方法的對比研究[J].系統(tǒng)工程與電子技術，2001（11）：1013-1016.