卡曼濾波器理論和原理

卡曼濾波器是一種利用最佳化回歸數據演算法(optimal recursive data processing algorithm)的估測器，它可以間接從不準確及不確定的量測值來獲得系統的狀態變數，尤

其對於雜訊來源為高斯白雜訊(White Gaussian Noise)時，即這些偏差跟前後時間沒有關係 且符合高斯分佈(Gaussian Distribution)，卡曼濾波器可以得到最小化的均方誤差。

考慮一線性系統

其中 為系統雜訊(plant noise)， 為量測雜訊(measurement noise)，卡曼濾波器 之估測主要根據兩項準則： equation)，其中 ˆx 為狀態估測值； 稱為狀態估測協方差矩陣(state covariance matrix)，定

義 ；Q為系統雜訊協方差矩陣，定義 ；

大，此時卡曼增益K較小，因此在計算下一時間之狀態估測值時，將不會太過信賴感測 器所量到的值；反之，當雜訊較小時，R也小，此時卡曼增益K較大，在計算下一個狀 態估測值時，便會較相信感測器所得到的值來修正新的狀態值。

圖 3.1 卡曼濾波器流程圖

3.1.3 擴增型卡曼濾波器

卡曼濾波器是用於線性系統的狀態估測，但事實上，工程上大部分的系統都是非線 性系統，對於這個問題，之後的相關研究對卡曼濾波器有了新的修改，出現了擴增型卡 曼濾波器。這個方法即是將卡曼濾波器的應用從線性系統推廣到非線性系統。

考慮一非線性系統

( 1) [ , ( ), ( )] ( ( ) [ , ( )] ( )

) x k f k x k u k w y k h k x k v k

+ = +

= +

k (3.4)

擴增型卡曼濾波器的設計方法與卡曼濾波器相同，同樣分為兩個步驟：

(1)狀態的設計

ˆ( 1 ) [ , (ˆ ), ( )]

3.2 理想振動式陀螺儀之角速度估測

3.2.2 系統觀察性(observability)

建立狀態方程式之後，接下來進行的工作則是驗證系統是否為可觀察，由非線性系

其中x 為狀態變數，_{i ( 1)}

rank O rank rank

z

rank z rank

z

輸出方程式為：

3.3.2 系統觀察性(observability)

由(3.11)式之定理，我們可求得系統之觀察性矩陣：

3.4 非理想振動陀螺儀之角速度估測與參數鑑別

& & &

&

3.4.2 系統觀察性(observability)

22

&& & & && &&

&& & & && &&

&&& && && &&& &&&

&&& && && &&& &&&

&&& &&&

&& & (3.25)

由於系統過於複雜，因此僅以忽略阻尼的情況下做討論，利用 mathematica 軟體可 解得

3.5 振動陀螺儀之角位移估測

傳統振動陀螺儀對於角位移的量測，一般都是利用角速度對時間進行積分來獲得，

然而，在微機電系統中，積分所得到之訊號，容易因電壓漂移(drift)而產生很大的誤差，

因此，一般我們在感測器中，都盡可能地不使用積分的方法來獲得所要量測的物理量。

在相關文獻中，Friendland 和 Hutton 最早利用動態方程式以能量的概念推導數學式，並 以數學的方式計算出角位移[13]，考慮理想振動陀螺儀之動態方程式

&& &x (3.27)

假設ω_x =ω_y =ω ，求解(3.30)式可得

角度θ可經由以下表示

&& & & &

&& & & y & y

第四章

&& & & &

&& & & & y

(4.1)

&& & &

m (4.2)

化

收斂於實際系統狀態之情形。圖 4.4 為角速度之即時估測。透過模擬我們可以知道，擴 增型卡曼濾波觀察器可以很快將雜訊濾除，取得系統在 x 方向的位移、速度以及 方向 的位移、速度，精確的估測出角速度的值。表 4.2 為觀察器系統的估測誤差。

y

表 4.1 模擬參數

圖 4.1 陀螺儀的質量塊於旋轉座標x−y平面上之運動軌跡

圖 4.2 感測器輸出與觀察器輸出：感測器位移輸出帶有標準差2.857 10× ⁻³μm 之隨機雜訊；速度輸出帶有標準差53.85 10× ⁻³μm s/ 之隨機雜訊

圖 4.3 觀察器狀態收斂於實際系統狀態之情形

圖 4.4 角速度之即時估測

圖 4.8 為觀察器狀態收斂於實際系統狀態之情形。圖 4.9 為角速度之即時估測。圖 4.10 為參數之即時鑑別。透過模擬我們可以知道，擴增型卡曼濾波觀察器可以很快將雜訊濾 除，取得系統在 x 方向的位移、速度以及 方向的位移、速度，精確的估測出角速度的 值，並且鑑別出參數

y

k 、x k_y。表 4.4 為觀察器系統的估測誤差。

表 4.3 模擬參數

圖 4.6 陀螺儀的質量塊於旋轉座標x−y平面上之運動軌跡

圖 4.7 感測器輸出與觀察器輸出：感測器位移輸出帶有標準差2.857 10× ⁻³μm 之隨機雜訊；速度輸出帶有標準差53.85 10× ⁻³μm s/ 之隨機雜訊

圖 4.8 觀察器狀態收斂於實際系統狀態之情形

圖 4.9 角速度之即時估測

圖 4.10 即時參數鑑別

表 4.4 估測誤差

4.3.2 理想振動陀螺儀之角位移估測模擬二

4.4 非理想振動陀螺儀模擬一(Ω、k 、_x k_y、k_xy未知)

圖 4.12 陀螺儀的質量塊於旋轉座標x−y平面上之運動軌跡

圖 4.13 感測器輸出與觀察器輸出：感測器位移輸出帶有標準差2.857 10× ⁻³μm 之隨機雜訊；速度輸出帶有標準差53.85 10× ⁻³μm s/ 之隨機雜訊

圖 4.14 觀察器狀態收斂於實際系統狀態之情形

圖 4.15 角速度之即時估測

圖 4.16 即時參數鑑別

表 4.6 估測誤差

4.4.2 非理想振動陀螺儀之角位移估測模擬一

由(3.30)式，

2 1

2 2 2 2 2

1 2( )

2tan ( ) ( xy xy

)

x y x y

θ ω

ω

− ⎛ + ⎞

= ⎜⎝ − + − ⎟⎠

&&

& & ，此模擬中ω² =k_x = 為未知，為k_y 了要使用此關係式計算出角位移，我們利用擴增型卡曼濾波觀察器將 x 、 x& 、 、 的 雜訊濾除，鑑別出

y y&

k 或x k_y之值，並且藉由鑑別出k_xy之值作回授控制，消除耦合力，將

系統控制為理想系統動態。圖 4.17 的模擬圖中，顯示觀察器系統約在 50ms 內收歛至實

為了估測角速度Ω並鑑別參數d 、_x d_y、d_xy、k 、_x k_y、k_xy，我們將擴增型卡曼濾 波器使用在(3.5.1)節中所推導出之狀態方程式，並使用 Matlab 進行演算法模擬。表 4.7 為模擬參數。圖 4.18 為質量塊於旋轉座標x−y平面上之運動軌跡，質量塊在x−y平面 上以Ω的速度產生進動，且運動軌跡為一橢圓，而橢圓率隨著耦合項d_xy、k_xy的存在而 改變，圖中顯示橢圓率除了一開始變動，之後即維持一固定橢圓率，顯示耦合力經由控 制很快的即被消除。圖 4.19 顯示出系統穩態時，觀察器的輸出將原本感測器輸出所存在 之雜訊濾除。圖 4.20 為觀察器狀態收斂於實際系統狀態之情形。圖 4.21 為角速度之即時 估測。圖 4.22 為參數之即時鑑別。透過模擬我們可以知道，擴增型卡曼濾波觀察器可以 很快將雜訊濾除，取得系統在 x 方向的位移、速度以及 方向的位移、速度，精確的估 測出角速度的值，並且鑑別出參數

y

d 、x d_y、d_xy、k 、_x k_y、k_xy。表 4.8 為觀察器系統的 估測誤差。

表 4.7 模擬參數

圖 4.18 陀螺儀的質量塊於旋轉座標x−y平面上之運動軌跡

圖 4.19 感測器輸出與觀察器輸出：感測器位移輸出帶有標準差2.857 10× ⁻³μm 之隨機雜訊；速度輸出帶有標準差53.85 10× ⁻³μm s/ 之隨機雜訊

圖 4.20 觀察器狀態收斂於實際系統狀態之情形

圖 4.21 角速度之即時估測

圖 4.22 即時參數鑑別

表 4.8 估測誤差

4.5.2 非理想振動陀螺儀之角位移估測模擬二

4.6 討論

本節根據前幾節的模擬結果與傳統方法做比較，在此，我們將先前模擬的參數套用 於傳統方法進行模擬，由於閉路控制法無法抑制雜訊的干擾，容易因控制誤差造成估測 值發散，必須額外使用消除雜訊的方法才能進行回授控制，因此，本部份僅以開路控制 法來做模擬比較。圖 4.24 為使用開路控制法操作之下，角速度的估測情形。

圖 4.24 角位移之估測(開路控制法)

比較圖 4.4，擴增型卡曼濾波觀察器的估測方法約在 10ms 即收斂至誤差 1%以內，

而開路控制法收斂約在 200ms 才收斂，且仍有雜訊存在。表 4.9 為開路控制法之估測誤 差。

表 4.9 估測誤差(開路控制法)

兩種方式比較之下，本方法在收斂速度上，比開路控制法快上許多，並且具有系統 參數鑑別與過濾雜訊的優點，而開路控制法只能在系統參數皆已知且為理想系統的情況 下才能進行估測。比較表 4.8 和表 4.9，使用本方法進行估測，即使是非理想的系統同時 做參數鑑別的模擬案例，相較於理想系統的開路控制法所得到的估測結果，在精準度上 也不會比較差。

從另一個觀點與閉路控制法做比較，本方法與閉路控制法最大的不同在於本方法在

表 4.10 估側誤差比較

第五章 結論與未來計畫

5.1 結論

本文以擴增型卡曼濾波器為基礎，將其作為振動陀螺儀系統的觀察器，估測角速度 並且做即時參數鑑別，同時藉由濾除雜訊將正確的狀態值回授控制，消除陀螺儀的耦合 力，利用以往只能使用在理想振動陀螺儀系統的數學關係式計算出角位移。相較於傳統 的方法，大部分都是在系統參數已知的情況下做控制，又或者是忽略感測器可能產生的 雜訊，將其假設為正確無誤的訊號進行控制。未來若能透過此方面改善，振動陀螺儀可 使用的範圍將更廣泛，可解決環境因素的干擾所造成系統的不穩定。

5.2 未來計畫

雖然本文對振動陀螺儀的操作方法提出了一個新的方法，但是尚屬於模擬的一個階 段，未來若要更完整的對此方法的可行性進行更進一步的討論，建立實驗架構是必然 的，然而對於振動陀螺儀的實體製作，現在還是相當困難的，或許我們可以考慮先從一 簡易的振動結構做起，將我們的演算法套入系統內，其後有了完整的基礎之後再推廣到 振動陀螺儀系統中。若此方法能夠透過實驗來證實其結果，將會是一套更完善的理論。

另外，在(2.3)節中，本論文所提出的完整振動陀螺儀模型，用於估測上仍有很大的限制，

但也並無可用處，若將系統動態描述的更嚴謹，並配合結構設計，相信在估測與控制上 的將會是另一個值得研究的方向。

參考文獻

[1] A. Lawrence, Modern Inertial Technology, Springer-Verlag, New York, 1998.

[2] D.S. Eddy and D.R. Sparks, “Application of MEMS technology in automotive sensors and actuators,” Proceedings of the IEEE, Vol. 86, No. 8, pp.1747-1755,August 1998.

[3] Andrei M. Shkel, Cenk Acar, Chris Painter, “Two Types of Micromachined Vibratory Gyroscopes,” sensors IEEE,2005.

[4] Sungsu Park, “Adaptive control strategies for MEMS Gyroscopes,” Doctoral Thesis,U.C.

Berkeley,2002.

[5] S. Jagannathan and Mohammed Hameed, “Adaptive Force-Balancing Control of MEMS Gyroscope with Actuator Limits,” In The 2004 American Control Conference Boston.

[6] A. Shkel, R.T. Howe, “Modeling and Simulation of Micromachined Gyroscopes in The Presence of Imperfections,” Technical Proceedings of the 1999 International Conference on Modeling and Simulation of Microsystems,University of California-Berkeley,1999.

[7] Sitaraman V. Iyer, “Modeling and Simulation of Non-idealities in a Z-axis CMOS-MEMS Gyroscope,” Doctoral Thesis, Carnegie Mellon University, 2003.

[8] Chris Painter, “Micromachined Vibratory Rate Integrating Gyroscope:Design, Modeling, and Experimental Demonstration,” qualifying examination report,University of California, Irvine, 2002.

[9] M.Hou and P.C. Muller, “Design of Observers for Linear Systems with Unknown Inputs,”

IEEE Transaction on Automatic control, vol.37, No.6, June 1992.

[10] Boutheina Sfaihi and Olfa Boubaker, “Full Order Observer Design for Linear Systems with Unknown Inputs,” IEEE International Conference on Industrial Technology,2004

[11] Bar-Shalom, Yaakov. and Li, Xiao-Rong, Estimation with applications to tracking and

[11] Bar-Shalom, Yaakov. and Li, Xiao-Rong, Estimation with applications to tracking and

在文檔中 擴增型卡曼濾波器應用於微型振動陀螺儀之即時控制系統 (頁 34-0)