擴增型卡曼濾波器應用於微型振動陀螺儀之即時控制系統

國 立 交 通 大 學

機械工程學系

碩 士 論 文

擴增型卡曼濾波器應用於微型振動陀螺儀之即時控制系統

Controls of MEMS Vibratory Gyroscope Systems Using Extended

Kalman Filter

研 究 生: 彭彥斌

指導教授: 陳宗麟 博士

擴增型卡曼濾波器應用於微型振動陀螺儀之即時控制系統

Controls of MEMS Vibratory Gyroscope Systems Using Extended

Kalman Filter

研 究 生 : 彭彥斌 Student: Yen-Pin Peng

指導教授 : 陳宗麟 博士 Advisor: Dr. Tsung-Lin Chen

國 立 交 通 大 學

機 械 工 程 學 系

碩 士 論 文

A Thesis

Submitted to Department of Mechanical Engineering College of Engineering

National Chiao Tung University in Partial Fulfillment of the Requirements

for the Degree of Master of Science

in

Mechanical Engineering September 2008

Hsinchu, Taiwan, Republic of China

擴增型卡曼濾波器應用於微型振動陀螺儀之

即時控制系統

學生: 彭彥斌

指導教授: 陳宗麟 博士

國立交通大學 機械工程學系

摘要

本篇論文中，提出了使用在振動陀螺儀上，估測角速度的一個新方法。以往大部分 的作法，都是假設系統量測雜訊可忽略，並且在其他參數都是已知的前提之下進行估 測。不同於以往，我們考慮量測訊號存在雜訊，及所有參數未知的情況下，利用「擴增 型卡曼濾波器(extended Kalman filter)」作為觀察器的演算法正確地估測出角速度，並鑑 別系統參數。 由 MATLAB 軟體的模擬結果顯示本方法在七個參數未知的情況下，及量測雜訊 3 2.857 10× − μm下(訊噪比約為 )，可鑑別出所有參數，同時角速度的估測精確度仍可 達 0.0021 。此外，透過觀察器的狀態回授控制，可將系統結構存在的阻尼及「耦 合力(coupling force)」消除，經由求解控制過後的系統動態方程式，我們可以直接計算出 角度，避免因積分角速度而產生的誤差累積。 20 / sec rad

Controls of MEMS Vibratory Gyroscope Systems Using Extended

Kalman Filter

Student: Yen-Pin Peng

Advisor: Dr.Tsung-Lin Chen

Department of Mechanical Engineering

National Chiao-Tung University

Abstract

In this thesis, we present a novel approach to estimate angular rates of MEMS vibratory gyroscope systems. Different from most of the existing methods, wherein all system parameters were assumed to be known and system measurements were noise-free, this method uses extend Kalman filter (EKF) techniques to estimate all the system parameters, including the angular rate. And, it nulls out the noise from system measurements and thus increases the sensing accuracy for gyroscope systems.

Simulation results indicate that the proposed method can estimate all the system parameter correctly. The estimation accuracy of angular rate is when the standard deviation of the measurement noise is

0.0021rad/ sec

3

2.857 10× − μm(signal-to-noise ratio is about twenty). In addition, the proposed method can be used to compensate the damping terms and “coupling forces” in gyroscope systems. Subsequently, the rotation angles can be calculated directly. As compared to the conventional methods wherein the angles were obtained by integrating angular rates over time, the proposed method does not suffer from the error accumulations and thus more accurate.

誌謝

本篇碩士論文得以順利完成，首先必須要感謝我的指導教授陳宗麟老師，整整兩年 的時間，不斷地指導與鼓勵我，每當在研究遇上瓶頸時，他總能不吝給予建議，使我度 過重重難關。不僅僅是在課業與論文研究上，老師也會在我們學生生活上遇到困難時， 給予幫助和關懷，總之，對老師的感謝不是幾句話能夠說完的，真的很謝謝老師。 再來，就是感謝實驗室的學長們，以過來人的身分分享經驗給我，教導我研究上的 問題，感謝同學、學弟在課業上彼此互相打氣，感謝實驗室所有同仁在兩年裡讓我留下 了美好的回憶。 最後，感謝我的家人當我的精神支柱，沒有他們的支持就沒有這篇論文的產生，在 這邊致上由衷的謝意。

目錄

中文摘要... i

英文摘要...ii

誌謝 ... iii

目錄 ...iv

表目錄 ... viii

圖目錄 ...xi

第一章 緒論 ...1

1.1 陀螺儀簡介 ...1

1.2 文獻回顧 ...2

1.3 研究動機 ...3

1.4 本論文架構 ...4

第二章 振動陀螺儀原理與量測方法...5

2.1 傳統振動陀螺儀動態方程式(單軸－兩個自由度)...5

2.1.1 理想振動陀螺儀動態方程式 ...5

2.1.2 非理想振動陀螺儀動態方程式...7

2.2 傳統操作方式...9

2.2.1 開路控制法...9

2.2.2 閉路控制法...9

2.3 完整振動陀螺儀動態方程式(單軸－三個自由度)...12

2.3.1 振動陀螺儀之完整動態模型 ...12

2.3.2 振動陀螺儀旋轉動態之感測分析...13

2.3.3 振動陀螺儀旋轉動態之機械結構分析 ...16

2.3.4 完整振動陀螺儀模型之觀察器設計 ...18

第三章 振動陀螺儀之擴增型卡曼濾波器估測法...21

3.1 擴增型卡曼濾波器 ...21

3.1.1 卡曼濾波器簡介 ...21

3.1.2 卡曼濾波器理論和原理 ...21

3.1.3 擴增型卡曼濾波器 ...23

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

3.2.1 估測狀態方程式 ...25

3.2.2 系統觀察性...25

3.3 理想振動陀螺儀之角速度估測與參數鑑別 ...26

3.3.1 估測狀態方程式 ...26

3.3.2 系統觀察性...27

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

3.4.1 估測狀態方程式 ...28

3.4.2 系統觀察性...29

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

第四章 模擬結果與討論 ...33

4.1 系統動態方程式之無因次化 ...33

Ω

未知)...34

4.2 理想振動陀螺儀模擬一(

4.2.1 理想振動陀螺儀之角速度估測模擬 ...34

4.2.2 理想振動陀螺儀之角位移估測模擬一 ...37

4.3 理想振動陀螺儀模擬二(

Ω

、

k_x

、

k_y

未知) ...38

4.3.1 理想振動陀螺儀之角速度估測與參數鑑別模擬 ...38

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

4.4 非理想振動陀螺儀模擬一(

Ω

、

k_x

、

k_y

、

k_xy

未知) ...43

4.4.1 非理想振動陀螺儀之角速度估測與參數鑑別模擬一...43

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

4.5 非理想振動陀螺儀模擬二(

Ω

、

k_x

、

k_y

、

k_xy

、

d_x

、

d_y

、

d_xy

未知) .47

4.5.1 非理想振動陀螺儀之角速度估測與參數鑑別模擬二...47

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

4.6 討論...53

第五章 結論與未來計畫 ...56

5.1 結論...56

5.2 未來計畫 ...56

圖目錄

圖 1.1 振動陀螺儀 ...2

圖 2.1 陀螺儀結構 ...6

圖 2.2 彈簧主軸偏離...8

圖 2.3 阻尼主軸偏離...8

圖 2.4 開路控制法流程圖 ...9

圖 2.5 閉路控制法流程圖 ...10

圖 2.6 質量塊沿 方向旋轉之動態...12

z

圖 2.7 電容式梳狀結構 ...13

圖 2.8 感測電容 ...15

圖 2.9 蛇狀彈簧 ...17

圖 2.10 彈簧結構 ...17

圖 3.1 卡曼濾波器流程圖 ...23

圖 3.2 擴增型卡曼濾波器流程圖 ...24

圖 3.3 質量塊運動軌跡 ...31

x−y

平面上之運動軌跡 ...35

圖 4.1 陀螺儀的質量塊於旋轉座標

圖 4.2 感測器輸出與觀察器輸出 ...36

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

圖 4.4 角速度之即時估測 ...37

圖 4.5 角位移之估測...38

x−y

平面上之運動軌跡 ...39

圖 4.6 陀螺儀的質量塊於旋轉座標

圖 4.7 感測器輸出與觀察器輸出 ...40

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

圖 4.9 角速度之即時估測 ...41

圖 4.10 即時參數鑑別...41

圖 4.11 角位移之估測...42

x−y

平面上之運動軌跡 ...44

圖 4.12 陀螺儀的質量塊於旋轉座標

圖 4.13 感測器輸出與觀察器輸出 ...44

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

圖 4.15 角速度之即時估測 ...45

圖 4.16 即時參數鑑別...46

圖 4.17 角位移之估測...47

x−y

平面上之運動軌跡 ...49

圖 4.18 陀螺儀的質量塊於旋轉座標

圖 4.19 感測器輸出與觀察器輸出 ...49

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

圖 4.21 角速度之即時估測 ...50

圖 4.22 即時參數鑑別...51

圖 4.23 角位移之估測...52

表目錄

表 4.1 模擬參數 ...35

表 4.2 估測誤差 ...37

表 4.3 模擬參數 ...39

表 4.4 估測誤差 ...41

表 4.5 模擬參數 ...43

表 4.6 估測誤差 ...46

表 4.7 模擬參數 ...48

表 4.8 估測誤差 ...51

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

表 4.10 估側誤差比較...55

第一章

緒論

1.1 陀螺儀簡介 陀螺儀是量測角速度的一種感測元件，與加速度規一起被廣泛的應用在慣性導航、 姿態估測和穩定控制等領域上[1][2]。要描述一個物體的運動，我們必須得到它的直線運 動以及旋轉運動，直線運動主要藉由加速度規來獲得，而旋轉運動則必須依靠陀螺儀來 獲得。 原始的陀螺儀是根據角動量守恆的原理被設計出來的，被發明於 1850 年，當時法 國物理學家 J.Foucault 為了要研究地球的自轉，發現了在高速旋轉中的轉子，因為慣性 原理，其旋轉軸恆指向一個方向，因而以希臘字 gyro(旋轉)和 skopein(看)兩字的組合來 命名陀螺儀。這項發明最早被拿來使用在航海及航空上，因其旋轉軸恆指向同一個方向 的特性，而能夠在交通工具航行時指引出正確的方向。其後，經過幾年的發展，漸漸有 不同型式的陀螺儀問世。 目前的陀螺儀大致可分為轉子式陀螺儀、光學陀螺儀和振動式陀螺儀。雖然轉子式 陀螺儀、光學式陀螺儀被使用在許多領域上，但體積大、價格昂貴是它們的缺點。相較 之下，振動式陀螺儀是以結構體之共振模態驅動，故耗能較低，可以瞬時起動，無軸承 磨耗及運轉壽命的問題，且有體積小、成本低廉等優點，因此越來越多人投入這方面的 相關研究。目前振動式陀螺儀已應用於各種消費性設備，如數位相機的影像防震、筆記 型電腦的硬碟保護和數位羅盤，以及汽車的電子穩定控制系統等等。另外，隨著自動化 工業和消費性機器人的發展，振動式陀螺儀在自動化工業上有助於滿足在組裝線上提高 自動化程度的要求；而在機器人中，陀螺儀有助於自動控制系統機器人手腳的行動和平 衡。

1.2 文獻回顧 振動陀螺儀量測角速度的方式，是利用物體旋轉時所產生的科氏力，使得能量由驅 動軸傳至感測軸，由兩者間能量傳遞的關係獲得角速度的資訊(在第二章將介紹詳盡的 量測原理)。依照結構上的不同，大致上可以分為 (a)懸臂樑式振動 (b)薄殼振動 (c)環型 振動 (d)音叉式振動 (e) H 型式振動。 (a) (b) (c) (d) (e) 圖 1.1 振動陀螺儀：(a)懸臂樑式振動 (b)薄殼振動 (c)環型振動 (d)音叉式振動 (e) H 型式振動 由於微機電結構微小，在製程上無法達到良好的精準度，因此，對系統做控制是不 可缺少的，而控制的方法又必須要配合系統的操作方式來進行。本論文主要是討論在陀

螺儀製作完成之後，如何藉由不一樣的操作方式來使估測能有更好的準確度，目前的振 動陀螺儀研究文獻，根據量測角速度的操作方式大致可以分成兩類：

(1)開路控制法(Open-loop Mode of Operation)[3]：將質量塊在驅動軸振動的振幅大小 固定住，量測感測軸的振幅，利用兩軸振幅間的關係式，計算出角速度。

(2)閉路控制法(Closed-loop Mode of Operation)[4][5]：適應性控制法(adaptive control)為 許多人使用的一種方法，以系統輸出狀態進行回授控制，將角速度視為未知參數 進行參數估測，根據 Lyapunov 穩定定理(Lyapunov stability theorem)，導出參數收 斂式，使角速度估測值收斂至正確值。 1.3 研究動機 大部分的振動式陀螺在製程上都會產生誤差，例如彈簧的不對稱、質量塊(proof mass) 重量分佈不平均、驅動力的作用線偏離主軸等等，這將使得系統產生耦合力(coupling force)[6]。若耦合力存在，能量不再只由科氏力進行傳遞，如此一來，角速度的資訊不 管是由數學關係式中計算求得，或是從觀察器中估測，都會因為耦合力的影響而產生誤 差。除此之外，外在環境的影響包括溫度、壓力的變化，以及其他雜訊，都會使陀螺儀 的輸出產生錯誤的訊號，導致角速度的估測有所偏差。 以往相關文獻提出的操作方式，並無法完全解決以上的問題同時達到高精度的輸 出。本論文對於此問題首先提出以往文獻所忽略之系統動態，希望藉由此動態以獲得更 多的動態資訊，雖然最後無法根據此動態來得到更好的結果，但也對振動陀螺儀的動態 做了更深入的分析。最後，本文採用了擴增型卡曼濾波器的方法做為系統觀察器，以估 測角速度，除了能夠大幅降低雜訊，還可將系統未知參數鑑別出來，同時，藉由觀察器 的資訊，可進行回授控制將耦合力消除，直接計算出角位移的變化。

1.4 本論文架構 本文共分為五個章節，第一章為緒論，介紹陀螺儀的發展，以及相關文獻回顧，並 說明對此方向的研究動機。第二章介紹振動陀螺儀運作原理以及傳統陀螺儀的各種操作 方法，同時提出了更完整的振動陀螺儀模型。在第三章中，本篇論文提出擴增型卡曼濾 波器作為估測陀螺儀系統之方法，以往陀螺儀被視為線性系統作為估測和控制，在本文 中，我們將陀螺儀參數視為狀態變數，新的系統為非線性系統，以非線性系統的方式探 討其觀察性，進行估測和控制。第四章則對本論文的方法進行電腦模擬，驗證此方法之 可行性。第五章對本論文做出相關討論及總結，並探討未來在此方向必須改良或更進一 步發展分析的方向。

第二章

振動陀螺儀原理與量測方法

本章節前半部份介紹陀螺儀的運作原理，並探討在結構製造不理想下的動態方程 式。在後半部份則對傳統所使用方法做詳細的介紹，同時提出更完整的陀螺儀模型。 2.1 傳統振動陀螺儀動態方程式(單軸－兩個自由度) 2.1.1 理想振動陀螺儀動態方程式 在第一章我們提到了振動陀螺儀分成幾種不一樣的機械結構，然而，在推導其數學 模型時，我們都將振動陀螺儀視為一質量－彈簧－阻尼系統[8]。考慮固定參考座標 X −Y，附於陀螺儀上之旋轉參考座標x−y，振動陀螺儀可視為一懸吊質量以及數個彈 簧所組成的(圖 2.1)，質量塊一般視為一質點，可在 x 方向( iv)以及y方向( jv)自由移動， 故有兩個自由度。質量塊在任一時間之下位置 pv 可表示成 ) pv (= x iv+( ) y jv (2.1) 速度vv可表示成 dp v p dt = v + Ω×v v v _(2.2) (2.3) ( ) ( vv= − Ωx& y iv+ y&+ Ωx) jv 加速度可表示成 dv a v dt = v v+ Ω× v v _(2.4) 2 2 ( 2 ) ( 2

av= &&x− Ω − Ω − Ωx y& &y iv+ &&y− Ω + Ω + Ω ) jy x& &x v (2.5) 由牛頓第二運動定律 Fv =mav 方向： i v 2 ( 2 x x k x d x m x x y y)

− − &= &&− Ω − Ω − Ω&& (2.6)

j v 方向： 2 ( 2 y y k y d y m y y x x)

因在振動式陀螺儀中， ， ， ， ， 故 、 、 、 四項通常忽略不考慮，系統動態方程式可簡化成 2 x k x>> Ωm x k yy >> Ωm 2y k xx >> Ω&m y k yy >> Ω&m x 2 mΩ x mΩ2y m xΩ& m yΩ& (2.8) 2 2 x x y y mx d x k x m y my d y k y m x + + = Ω + + = − Ω

&& & & && & &

對於理想的振動式陀螺儀系統而言，常將阻尼忽略不計，方程式可改寫成 (2.9) 2 2 x y mx k x m y my k y m x + = Ω + = − Ω && & && & (a) (b) 圖 2.1 陀螺儀結構：(a)轉動前 (b)轉動後 在(2.9)式中，是以旋轉座標x−y來描述系統動態，等號右端為科氏力，科氏力為 一慣性力(假想力)，會使質量塊之振動線以Ω的角速度往座標旋轉方向的反方向進動； 若以固定座標X −Y來看，此時質量塊不受任何外力，根據線動量守恆的原理，將維持 原固定方向線振動(圖 2.1)。

2.1.2 非理想振動陀螺儀動態方程式 對於一個振動陀螺儀，製作出來多少都會產生誤差，在未經控制的情況下，動態並 非如理想動態方程式所描述，考慮非理想彈簧所造成的影響[6][7]：假設 x 、 分別為驅 動軸和感測軸， ' y x 、y'為彈簧主軸，因製造上的誤差導致兩座標軸偏離角度α(圖 2.2)， 若主軸上之彈簧常數為k₁、k₂，則彈簧力和彈簧常數的關係式可表示成 ' 1 ' 2 0 ' 0 ' x y F k x F k y ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦⎣ ⎦ ⎣ ⎦ (2.10) 由座標轉換可得 ' cos sin ' sin cos x x y y α α α α ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ = ⎢ ⎥ ⎢₋ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.11) ' ' cos sin sin cos x x y y F F F F α α α α ⎡ ⎤ ⎡ ⎤⎡ = ⎢ ⎥ ⎢₋ ⎥⎢ ⎢ ⎥ ⎣ ⎦⎢ ⎣ ⎦ ⎣ ⎤ ⎥ ⎥⎦ (2.12) 由(2.10)式、(2.11)式、(2.12)式可得 2 2 1 2 1 2 2 2 1 2 1 2

cos sin ( ) sin cos

( ) sin cos sin cos

x y F k k k k x F k k k k y α α α α α α α α ⎡ ⎤ ⎡₌ + − ⎤⎡ ⎤ ⎢ ⎥ ⎢ _{− +} ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦⎣ ⎦ ⎣ ⎦ (2.13) 令 2 2 1 2 1 2 2 2 1 2 1 2

cos sin ( ) sin cos

( ) sin cos sin cos

xx xy yx yy k k k k k k k k k k k k α α α α α α α α ⎡ ⎤ ⎡ + − ⎤ = ⎢ ⎥ ⎢ _{− +} ⎣ ⎦⎥ ⎣ ⎦ (2.14) 則 2 2 1 2 1 2 1 2 2 2 1 2 1 2 1 2 1 2 1 2

cos sin cos(2 )

2 2

sin cos cos(2 )

2 2

( ) sin cos sin(2 )

2 x y xy yx k k k k k k k k k k k k k k k k k k k k α α α α α α α α α + − = + = + + − = + = − − = = − = (2.15)

圖 2.2 彈簧主軸偏離 同理，考慮非理想阻尼所造成的影響：假設 x 、 分別為驅動軸和感測軸，y x 、" 為阻尼主軸，因結構製程問題導致兩座標軸偏離角度 " y β (圖 2.3)，此時 2 2 1 2 1 2 1 2 2 2 1 2 1 2 1 2 1 2 1 2

cos sin cos(2 )

2 2

sin cos cos(2 )

2 2

( ) sin cos sin(2 )

2 x y xy yx d d d d d d d d d d d d d d d d d d d d β β β β β β β β β + − = + = + + − = + = − − = = − = (2.16) 圖 2.3 阻尼主軸偏離 故振動陀螺儀非理想動態方程式寫成 (2.17) 2 2 x xy x xy xy y xy y mx d x d y k x k y m y my d x d y k x k y m x + + + + = Ω + + + + = − Ω

&& & & &

&& & & &

其中k 、_x k_y、k_xy、k_yx如(2.15)式所表示；d 、_x d_y、d_xy、d_yx如(2.16)式所表示。(2.17)

2.2 傳統操作方式 2.2.1 開路控制法 開路控制法的作法為將質量塊於驅動軸( x 軸)之能量維持固定，即振幅不變。起初， 質量塊於感測軸振動的振幅為零，當系統開始有角速度Ω產生時，感測軸( 軸)將會因 科氏力( y 2 x − Ω& )產生能量傳遞，而使感測軸開始振動，直到穩態之後，振幅與角速度Ω成 正比，運動方程式推導如下[3]： 2 sin( ) 2 x y y x X t y d y y x ω ω = + + = − Ω

&& & & (2.18)

假設ω_x =ω_y =ω_n，求解常微分方程(2.18)式，可得 2 y X Y d Ω = (2.19) 圖 2.4 開路控制法流程圖 在(2.19)式之關係式中，X 、Y分別為驅動軸和感測軸之振幅，其中X 為已知，d_y 若為已知參數，藉由量測感測軸之振幅Y，即可利用此關係氏求得角速度，此方法之缺 點為安定時間慢，延遲時間長，同時，也無法在耦合力存在的時候使用。 2.2.2 閉路控制法 適應性控制法為閉路控制法中常被使用的一種方法，此方法為控制和估測同時進 行，在將系統的運動軌跡控制成參考軌跡的同時完成估測[4][5]。控制流程如下，首先定 義參考系統動態方程式為

(2.20) 0 0 m x m m y m mx k x my k y + = + = && && 考慮加入控制後之系統動態方程式為 2 2 x x x x y y mx d x k x m y F my d y k y m x F + + = Ω + + + = − Ω +

&& & &

&& & & (2.21)

假設d 、_x d_y、 k 、x ky均為已知常數，由適應性控制法則選定F 、x Fy 1 2 (2.22) 1 2 ˆ 2 ( ) ( ˆ 2 ( ) x x x x m m m y y y y m m m F d x k x m y k x m x x m x x F d y k y m x k y m y y m y y λ λ λ λ = + − Ω − − − − − = + + Ω − − − − −

& & & &

& & & &

) ( ) ) 將(2.22)式代入(2.21)式 1 2 1 2 ˆ ( ) ( ) ( ) 2( ) ˆ ( ) ( ) ( ) 2( m m m m m m x x x x x x y y y y y y x λ λ λ λ − + − + − = Ω − Ω − + − + − = − Ω − Ω

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

y (2.23) 圖 2.5 閉路控制法流程圖 m y→ y 時，Ω → Ω ，其中收斂速度由ˆ m x→x 、 λ₁、 由(2.23)式知，當 λ₂所決定。

另一方面，我們同樣也可以利用 Lyapunov 穩定定理(Lyapunov stability theorem)來驗證其 收斂性。 選取 Lyapunov function V >0為正定函數 2 2 2 2 1 2 2 1 1 1 1 1 2 x 2 x 2 y 2 y 2 V = λe + e_& + λ e + e_& + γ−Ω%2 _(2.24)

將V 對時間作微分

1 (2.25)

2 x x x x 2 y y y y ˆ

V& =λe e& +e e& && +λe e& +e e& && −γ−Ω &%Ω

) y& 0 整理後可得 2 2 1 (2.26) 1 1 ˆ ( x y) ( 2 y 2 x

V& = − λe& +λe& + Ω − Ω −% γ− & e x& &+ e&

選取 1_ˆ

2e xy 2e yx

γ−

− Ω −& _{& &}+ _{& &}=

(2.27)

ˆ _{2 ( )}

y x

e x e y

γ

Ω = −& _{& & & &}−

因V& ≤0為負半定函數，若刺激訊號足夠，則V 將收斂，Ω → Ω 。此方法優點為收ˆ

斂快速，同時也可做系統參數鑑別，但是對於雜訊抑制的能力相當低，假設量測訊號存 在雜訊，則

x→ +x n_px，x&→ +x& n_vx，y→ +y n_py，y&→ +y& n_vy (2.28) 將(2.28)式帶入(2.22)式 1 2 1 2 ˆ ( ) ( ) 2 ( ) [( ) ] [( ) ] ˆ ( ) ( ) 2 ( ) [( ) ] [( ) ] x x vx x px vy x vx m px m y y vy y py vx y vy m py m m m F d x n k x n m y n k x m x n x m x n x F d y n k y n m x n k y m y n y m y n y λ λ λ λ = + + + − Ω + − − + − − + − = + + + + Ω + − − + − − + − & & & & & & & & (2.29) 再將(2.29)式帶回(2.21)式 1 2 1 2 ˆ ( ) ( ) ( ) 2( ) ˆ ( ) ( ) ( ) 2( ) m m m m m m x x x x x x y N_x y y y y y y y x λ λ λ λ − + − + − = Ω − Ω + − + − + − = − Ω − Ω +

&& && & & &

&& && & & & N (2.30)

其中 1 2 1 _ˆ [ 2 ] x x vx x px vy vx px N d n k n m n m n m n m λ λ = + − Ω − − 1 2 1 _ˆ [ 2 y y vy y py vx vy N d n k n m n m n m n m λ λ = + + Ω − − _py] 由(2.30)式知，控制力F 、_x F_y若取得帶有雜訊的狀態值，在最後的收歛式中將會存 在誤差N 、_x N_y。

2.3 完整振動陀螺儀動態方程式(單軸－三個自由度) 在(2.1)節中我們推導了振動陀螺儀的動態模型，並在(2.2)節中介紹了傳統陀螺儀的 操作方式，而傳統陀螺儀所使用的數學動態模型都是以(2.8)式為基礎，即質量塊只有兩 個自由度。實際上，以往陀螺儀相關研究之文獻，往往忽略振動陀螺儀所存在之一個動 態－陀螺儀本體(即包含感測器之部分)與質量塊是以彈簧作為連結，除了 x 方向、 方 向存在彈簧常數 y x k 、k_y，在旋轉方向也應存在彈簧常數 k_θ，只有在 k_θ趨近於無窮大的 時候，我們才可以將此動態忽略，本節即對此動態做深入推導分析，並討論在何種設計 下此動態不可忽略，又何種情況下可忽略。 2.3.1 振動陀螺儀之完整動態模型 圖 2.6 質量塊沿 方向旋轉之動態 z 考慮一理想振動陀螺儀，當陀螺儀旋轉時，在此暫態時刻，陀螺儀本體與質量塊存 在一角度差(圖 2.6)，因此，質量塊除了以Ω的角速度沿 軸旋轉之外，也將額外產生與 陀螺儀本體之相對旋轉。由(2.8)式所得之沿 z x 方向平移、沿 方向平移之動態方程式， 並考慮沿 方向旋轉之動態，新的系統動態方程式可寫成 y z

(2.31) 2 2 ( ) ( ) x x x y r r s r s s mx d x k x m y my d y k y m x Iθ d_θ θ θ k_θ θ θ θ + + = Ω + + = − Ω + − + − = = Ω

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

& 0 其中，I 為質量塊沿 方向旋轉之轉動慣量，z d_θ為質量塊沿 方向旋轉之阻尼常 數， z k_θ為質量塊沿 方向旋轉之彈簧常數，z θr為質量塊之絕對角位移，θs為陀螺儀本體 之絕對角位移。透過(2.31)式中額外的一條運動方程式，我們將首先探討此旋轉動態是否 可藉由感測器量測出，之後再試圖建立觀察器以估測出角速度和角位移。 2.3.2 振動陀螺儀旋轉動態之感測分析 振動陀螺儀依感測結構分類可分為電容式、壓電式、電磁式，最廣泛被使用的結構 為電容式，因此我們以電容式之感測結構來做分析，而電容式之感測結構又以梳狀結構 最常使用(圖 2.7)。

圖 2.7 電容式梳狀結構

考慮其中一組感測電容，可量測到四個位置的電容(圖 2.8)，利用平行電容板公式 A C d ε = (2.32) (2.32)式中ε為介電係數，A為兩電容板之重疊面積，d 為兩電容板之距離，故質量 塊在無任何位移之前，四個位置的電容為 1 2 3 4 (Wl) C C C C n ε = = = = (2.33) 其中W 為電容板之寬度。

(a)

(b)

(c) (d) 圖 2.8 感測電容：(a)無位移 (b)位移 x (c)位移 x 、角位移θ (d)位移 x 、位移 、角位移y θ 由(2.3.1)節知質量塊與陀螺儀本體存在一相對角位移θ θ_r − ，令_s θ θ_r− _s = ≈ ，質θ 0 量塊經過位移 x 、位移 、角位移y θ後， x 方向四個位置的電容可近似為 1 2 3 4 ( ) ( ) 2 ( ) ( ) 2 ( ) ( ) 2 ( ) ( ) 2 ε θ ε θ ε θ ε θ + ′ = ₋ − − + + ′ = ₋ + + + − ′ = ₊ − − + − ′ = ₊ + + + W l x C l x n y m W l x C l x n y m W l x C l x n y m W l x C l x n y m (2.34)

經整理後可得 3 4 1 2 1 2 3 4 ( ) ε ′ ′ ′ ′ = − ′₊ ′ ′₊ ′ C C C C n x W C C C C (2.35) 藉由(2.35)式，可利用量測到四個位置的電容來計算出位移 x；同理，若 方向經過 位移 y x 、位移y、角位移θ之感測電容分別為C ′5 、C ′6 、C ′7 、C ′8 ，則 5 6 7 8 5 6 7 8 ( ) ε ′ ′ ′ ′ = − ′₊ ′ ′₊ ′ C C C C n y W C C C C (2.36) 最後將(2.35)、(2.36)帶入(2.34)式中即可求得θ。 2.3.3 振動陀螺儀新動態之機械結構分析 在(2.3.1)節中我們推導出振動陀螺儀的完整模型，提出以往研究文獻所忽略的一個 旋轉動態，並在(2.3.2)節證實此動態可藉由感測器量測出。但更進一步，我們必須考慮， 在微機電中，何種結構的設計，會使得此一動態更加明顯；相反地，何種結構的設計， 會使此一動態小至可忽略。由數學動態模型(2.31)式，令 x x k m ω = ， y y k m ω = ， k I θ θ ω = (2.37) 假設k_x=k_y，則ω_x =ω_y，若 x θ ω ω 越小，代表感測器在量測時所能量測到的資訊有關 θ的成分越高，則此動態越明顯，可藉由感測器量出。一般而言，蛇狀彈簧是最常被用 來作為振動陀螺儀的彈簧結構(圖 2.9)，透過有限元素模擬的分析，可求得k 、 k_{x θ}，當a b 越大， x k k θ _將越小， x θ ω ω 也越小，此時旋轉動態越明顯。

圖 2.9 蛇狀彈簧 此外，彈簧的配置數量對 x k k θ _{也有很大的影響，彈簧配置的數量越多，則} x k k θ _越小， 旋轉動態越不明顯(圖 2.10)。 (a) (b) 圖 2.10 彈簧結構：(a)四個彈簧 (b)八個彈簧。(b)彈簧數多於(a)， 陀螺儀本體與質量塊之相對位移在(a)較為明顯。 質量塊的形狀大小也是重要的考量，若質量塊為一薄板，則 2 2 x x x x k m k m k k I k mr k r θ θ θ ω ω = = = θ (2.38)

其中r為質量塊沿 軸旋轉之迴轉半徑。根據(2.38)式可知，z x θ ω ω 與迴轉半徑成反比， 若質量塊的面積越大，則迴轉半徑大， x θ ω ω 小，旋轉動態越明顯；反之，面積越小，則 迴轉半徑小， x θ ω ω 大，則旋轉動態越不明顯。 2.3.4 完整振動陀螺儀模型之觀察器設計 將振動陀螺儀模型推導至更完整之後，我們試圖從旋轉動態獲得更完整的系統資 訊，以估測出角速度和角度。考慮(2.31)式旋轉動態的方程式，假設阻尼為零，令 1 r x = ，θ x₂ = &θ_r，x₃ = θ_s 改寫成狀態方程式

[

]

1 1 2 2 2 2 3 3 1 2 3 0 1 0 0 0 0 0 0 0 1 1 0 1 x x d x x dt x x x y x x θ θ ω ω ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢_{= −} ⎥ ⎢ ⎥ ⎢ ⎥_{+ Ω} ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎡ ⎤ ⎢ ⎥ = _{− ⎢ ⎥} ⎢ ⎥ ⎣ ⎦ (2.39) 若可利用 x 、 兩軸的動態估測出y Ω，則Ω可視為已知輸入，驗證(2.39)式之系統 觀察性 2 2 2 ≠ 1 0 1 ( ) 0 1 0 3 0 C

rank Q rank CA rank

CA ωθ ωθ ⎧⎡ ⎤⎫ ⎧⎡ − ⎫⎤ ⎪_{⎢ ⎥}⎪ ⎪_{⎢ ⎥}⎪ = _{⎨⎢ ⎥⎬}= _{⎨⎢ ⎥⎬} ⎪_{⎢ ⎥}⎪ ⎪_{⎢− ⎥}⎪ ⎣ ⎦ ⎣ ⎦ ⎩ ⎭ ⎩ ⎭ 由於觀察性矩陣並無滿秩，故無法直接建立出觀察器估測出角度。另一方面，若無 法直接估測出角度，我們試圖假設Ω為未知，建立另一觀察器，估測角速度，與兩軸所 估測出之角速度作比較，以作修正。將(2.39)式視為存在未知輸入的狀態方程式，Ω為 未知輸入，由未知輸入觀察器相關文獻之設計方法[9][10]，標準式為 x Ax Bu Dv y Cx = + + = & (2.40)

其中 u 為已知輸入， 為未知輸入，比較(2.39)式、 (2.40)式 v 2 2 0 1 0 0 0 0 0 A ω_θ ω_θ ⎡ ⎤ ⎢ ⎥ = −_{⎢ ⎥} ⎢ ⎥ ⎣ ⎦ ，C =

[

1 0 − ，1

]

0 0 1 D ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ， v= Ω 選取一非奇異矩陣 ，

[

]

10 10 00 1 0 1 T N D ⎡ ⎤ ⎢ ⎥ = _{= ⎢ ⎥} ⎢ ⎥ ⎣ ⎦ 1 0 0 1 1 0 N ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ， 0 0 1 D ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ 透過相似轉換

[

] [

]

1 11 12 2 21 22 1 0 1 0 0 0 0 1 0 0 0 1 0 0 0 1 A A A T AT A A C CT CN CD D T D I θ ω − − ⎡ ⎤ ⎡ _{⎤ ⎢ ⎥} = =_{⎢ ⎥ ⎢}= _⎥ ⎣ _{⎦ ⎢ − ⎥} ⎣ ⎦ = = = − ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ = =_{⎢ ⎥ ⎢ ⎥}= ⎣ ⎦ ⎢ ⎥_{⎣ ⎦} 未知輸入觀察器存在條件為： (1)rank CD=rank D (2)rank sI A11 A12 n , s C, Re( )s CN CD ⎡ − − ⎤ 0 = ∀ ∈ ≥ ⎢ ⎥ ⎣ ⎦ 其中 為系統狀態個數，在本系統中n n= 。為了確定系統是否符合觀察器設計原3 則，將系統矩陣帶入驗證： (1) 0 0 0 0 1 1

rank CD rank rank D rank

⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = _{⎢ ⎥}= = _{⎢ ⎥} ⎢ ⎥− ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ (2) 11 12 2 1 0 0 0 0 1 s sI A A rank s CN CD ωθ − ⎡ ⎤ ⎡ − − ⎤ _{⎢ ⎥} = − ⎢ _{⎥ ⎢ ⎥} ⎣ _{⎦ ⎢ − ⎥} ⎣ ⎦ ， ∀ ∈s C, Re( )s ≥0

若s=0，則 11 12 3 sI A A rank CN CD ⎡ − − ⎤ ≠ ⎢ ⎥ ⎣ ⎦ 由於系統無法符合未知輸入觀察器的設計條件，則無法利用此種觀察器找出未知輸 入Ω。目前觀察器之文獻研究雖然非常多，但並未找到適用於本系統動態上之觀察器， 雖然我們深入研究加入旋轉動態之完整陀螺儀模型，但是最終目的所要達成角速度以及 角度的估測並未成功，因此，重新探討對振動陀螺儀角速度量測的更佳方法。在下一個 章節，我們提出另一個新方法－擴增型卡曼濾波器，做為振動陀螺儀的估測方法。

第三章

振動陀螺儀之擴增型卡曼濾波器估測法

本章首先簡單介紹卡曼濾波器的原理，接下來討論幾種不同情況下，使用擴增型卡 曼濾波器前所需建立的狀態矩陣，包括單純之角速度估測、結合角速度估測與參數鑑 別、不理想系統之角速度量測與參數鑑別(忽略阻尼)、不理想系統之角速度量測與參數 鑑別(考慮阻尼)，並針對不同情況進行觀察性之檢驗，驗證此方法用於振動陀螺儀估測 之可行性。

3.1 擴增型卡曼濾波器(extended Kalman filter)

在上一章我們介紹了傳統陀螺儀最常見的估測方法，本節介紹本論文所使用的方法 －擴增型卡曼濾波器，此方法的特色為收斂快、抗雜訊能力高，亦可做系統參數鑑別。

3.1.1 卡曼濾波器簡介

卡曼濾波器最初由 Rudolf Emil Kalman 所開發設計，在 1960 年，他發表一篇論文－ 利用遞迴方法解決離散線性濾波問題，因這篇論文比其他人的研究成果更通用也更完 整，因此被命名為卡曼濾波器。

卡曼濾波算法的起源可以追朔到 1795 年由年僅 18 歲的高斯(Carl Friedrich Gauss)提 出的最小二乘理論，卡曼濾波算法與許多新技術一樣，也是致力於解決特定問題。之後， 因為數位計算技術的進步，卡曼濾波器被廣泛的應用在許多領域上，如導航系統、控制 系統、核電站設備、人口統計建模、製造業、地層放射性探測等等[11][12]。

3.1.2 卡曼濾波器理論和原理

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

其對於雜訊來源為高斯白雜訊(White Gaussian Noise)時，即這些偏差跟前後時間沒有關係 且符合高斯分佈(Gaussian Distribution)，卡曼濾波器可以得到最小化的均方誤差。 考慮一線性系統 ( 1) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) x k A k x k B k u k w y k c k x k v k + = + + = + k (3.1)

其中 為系統雜訊(plant noise)， 為量測雜訊(measurement noise)，卡曼濾波器

之估測主要根據兩項準則： ( ) w k v k( ) (1)狀態估測的平均值等於實際狀態的平均值，即估測的期望值等於狀態的期望值。 (2)狀態估測與實際狀態的偏差最小化，即誤差方差最小化。 卡曼濾波器的設計方法主要分為兩個步驟： (1)狀態的預測 ˆ( 1 ) ( ) (ˆ ) ( ) ( ) ( ˆ( 1 ) ( 1) (ˆ 1 ) ( 1 ) ( ) ( ) ( )T ( ) ) x k k A k x k k B k u k w k y k k C k x k k P k k A k P k k A k Q k + = + + + = + + + = + (3.2) (2)狀態的修正 1 ( 1) ( 1) ( 1 ) ( 1) ( ) ( 1) ( 1 ) ( 1) ( 1) ( 1 1) ( 1 ) ( 1) ( 1) ( 1) ˆ ( ) ( ) ˆ( 1 1) ˆ( 1 ) ( 1) T T T S k C k P k k C k R k K k P k k C k S k P k k P k k K k S k K k residual y k y k x k k x k k K k residual − + = + + + + + = + + + + + = + − + + + = − + + = + + + ⋅ (3.3)

(3.2) 式 稱 為 預 測 方 程 式 (prediction equation) ， (3.3) 式 稱 為 修 正 方 程 式 (correction equation)，其中 ˆx 為狀態估測值； 稱為狀態估測協方差矩陣(state covariance matrix)，定

義 ；Q為系統雜訊協方差矩陣，定義 ； P ˆ ˆ [( )( ) ]T P=E x−x x−x Q=E ww[ T] R為量測雜訊協 方差矩陣，定義R=E vv[ T]；K稱為卡曼增益(Kalman gain)。 卡曼濾波器中最重要的部份就是卡曼增益運算的部份，它可藉由狀態值的變化來調 整估測協方差矩陣 ，選擇是否要相信先前所估測到的狀態值去預測新的狀態值，或是 選擇相信目前感測器的輸出值來修正新的狀態值。舉例來說，當量測雜訊較大時， P R也

大，此時卡曼增益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 ) [ , (ˆ ), ( )] ˆ( 1 ) [ , (ˆ )] ( 1 ) ( ) ( ) ( )T ( x k k f k x k k u k y k k h k x k k P k k A k P k k A k Q k + = + = + = + ) (3.5) (2)狀態的修正 1 ( 1) ( 1) ( 1 ) ( 1) ( ) ( 1) ( 1 ) ( 1) ( 1) ( 1 1) ( 1 ) ( 1) ( 1) ( 1) ˆ ( ) ( ) ˆ( 1 1) ˆ( 1 ) ( 1) T T T S k C k P k k C k R k K k P k k C k S k P k k P k k K k S k K k residual y k y k x k k x k k K k residual − + = + + + + + = + + + + + = + − + + + = − + + = + + + ⋅ (3.6) 不同於卡曼濾波器， A k( )、C k( )由以下方法求得 ˆ ( ) ˆ ( 1 ) ( ) ( ) ( 1) ( 1) x x k k x x k k f k A k x h k C k x = = + ∂ = ∂ ∂ + + = ∂ (3.7) 圖 3.2 擴增型卡曼濾波器流程圖

3.2 理想振動式陀螺儀之角速度估測 3.2.1 估測狀態方程式 為了使用擴增型卡曼濾波器作為觀察器，首先必須先將系統動態方程式寫成狀態空 間表示法。考慮理想系統動態方程式，將(2.9)式寫成狀態方程式表示法： 0 1 0 0 0 0 2 0 0 1 0 0 2 0 x y x _k x x x d m y y dt k y y m ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ ⎡ ⎤ ⎢− Ω⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥_{= ⎢} ⎢ ⎥ ⎥ ⎢ ⎥ _{⎢ ⎥}⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ _{− Ω −} ⎣ ⎦ ⎢ ⎥ ⎣ ⎦ & & & & (3.8) 為了估測Ω，我們將Ω視為新的狀態變數，稱之為擴增狀態，故(3.8)式可改寫成： 2 2 0 x y x x _{k x} y x _m d y y dt k y y x m ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ ⎢_{− + Ω} ⎥ ⎢ ⎥ _{⎢ ⎥} ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ =_{⎢ ⎥} ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ _{− − Ω} ⎢ ⎥ ⎢ ⎥_Ω ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ & & & & & & (3.9) 輸出方程式為： 1 2 3 4 z x z x z z y z y ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ & & (3.10) 3.2.2 系統觀察性(observability) 建立狀態方程式之後，接下來進行的工作則是驗證系統是否為可觀察，由非線性系 統理論，我們可知系統觀察性矩陣： ( 1) i n z z O x z − ⎡ ⎤ ⎢ ⎥ ∂ ⎢ ⎥ = ⎢ ⎥ ∂ ⎢ ⎥ ⎣ ⎦ & M (3.11)

其中x 為狀態變數，_i ( 1) ( 1) ( 1) n n n d z z dt − − − _{= ， 為狀態變數個數，若觀察性矩陣為滿秩(full} rank)，則系統為可觀察(估測)之系統。 n 由(3.11)式之定理，我們可求得系統之觀察性矩陣： (4) ( ) z z z

rank O rank rank

z x x z ⎧ ⎡ ⎤⎫ ⎪_∂ ⎢ ⎥⎪ _{⎧ ∂ ⎡ ⎤⎫} ⎪ _{⎢ ⎥}⎪ = _{⎨ ⎬}≥ _{⎨ ⎢ ⎥⎬} ⎢ ⎥ ∂ _{⎩∂ ⎣ ⎦⎭} ⎪ _{⎢ ⎥}⎪ ⎪ _{⎣ ⎦}⎪ ⎩ ⎭ & & M (3.12) 又 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 2 2 0 2 0 2 x y z rank rank z x _k y m k x m ⎧⎡ ⎤⎫ ⎪⎢ ⎥⎪ ⎪⎢ ⎥⎪ ⎪⎢ ⎥⎪ ⎪⎢ ⎥⎪ ⎧∂ ⎡ ⎤⎫₌ ⎪_⎢ ⎪ ⎨_{∂ ⎢ ⎥}⎬ ⎨ ⎢ ⎥ ⎣ ⎦ ⎩ ⎭ ⎪_{⎢− Ω ⎥}⎪ ⎪ ⎪ ⎢ ⎥ ⎪ ⎪ ⎢ ⎥ ⎪ _{− Ω − −} ⎪ ⎢ ⎥ ⎪_{⎣ ⎦}⎪ ⎩ ⎭ & & & ⎥⎬ (3.13) 由(3.13)式可明顯看出觀察性矩陣為滿秩，故系統為可觀察之系統。 3.3 理想振動陀螺儀之角速度估測與參數鑑別 3.3.1 估測狀態方程式 考慮(3.8)式之系統狀態方程式，此時我們假設參數未知，為了估測Ω並鑑別參數k_x 和k_y，我們將Ω、k 、_x k_y視為新的狀態變數，可改寫狀態方程式為： 2 2 0 0 0 x y x y x x _{k x} y x _m y y d y k y x dt m k k ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ ⎢− + Ω ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥_{= ⎢} ⎥ ⎢ ⎥ _{− − Ω} ⎢ ⎥ ⎢ ⎥Ω _{⎢ ⎥} ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ & & & & & & (3.14)

輸出方程式為： 1 2 3 4 z x z x z z y z y ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ & & (3.15) 3.3.2 系統觀察性(observability) 由(3.11)式之定理，我們可求得系統之觀察性矩陣： (6) ( ) z z z

rank O rank rank z

x x z z ⎧ ⎡ ⎤⎫ ⎧ ⎡ ⎤⎫ ⎪_∂ ⎢ ⎥⎪ _∂ ⎪ _{⎢ ⎥}⎪ ⎪ ⎢ ⎥⎪ = _{⎨ ⎬}≥ _{⎨ ⎢ ⎥⎬} ⎢ ⎥ ∂ ∂ ⎪ _{⎢ ⎥}⎪ ⎪ _{⎢ ⎥}⎪ ⎣ ⎦ ⎩ ⎭ ⎪ _{⎣ ⎦}⎪ ⎩ ⎭ & & M && (3.16) 又 2 2 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 2 2 0 0 2 0 2 0 2 2 2 0 4 0 2 2 2 0 0 4 x y y y x y x x z rank z x z k x y m m rank k y x m m k k y k x y m m m m k k k x y m m m ⎧ ⎡ ⎤⎫ ∂ ⎪ _{⎢ ⎥}⎪ ⎨_{∂ ⎢ ⎥}⎬ ⎪ _{⎢ ⎥}⎪ ⎣ ⎦ ⎩ ⎭ ⎧⎡ ⎤ ⎪⎢ ⎥ ⎪⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ − Ω ⎢ ⎥ ⎢ ⎥ = _⎨ ⎢ ⎥ − Ω − − − ⎢ ⎥ ⎢ ⎥ Ω ⎢ _{− − Ω − − − −} Ω ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ Ω _{− − Ω} Ω ₋ ⎥ ⎢ ⎥ ⎣ ⎦ & && & & & & ⎫ m x m m − ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎩ ⎭ (3.17) 由(3.17)式經高斯消去法運算後可知觀察性矩陣為滿秩，故系統為可觀察之系統。

3.4 非理想振動陀螺儀之角速度估測與參數鑑別 3.4.1 估測狀態方程式 再來，考慮陀螺儀在製程上造成誤差，使系統產生耦合力，即 ， ，故系統的狀態方程式為 0 xy yx k =k ≠ 0 xy yx d =d ≠ 0 1 0 0 2 0 0 1 0 2 xy xy x x xy xy y y x _{k d k d} x x x d m m m m y y dt y k d k d m m m m ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ _{⎢ ⎥}⎡ ⎤ − − − − + Ω ⎢ ⎥ _{⎢ ⎥}⎢ ⎥ ⎢ ⎥_{= ⎢ ⎥}⎢ ⎥ ⎢ ⎥ _{⎢ ⎥}⎢ ⎥ ⎢ ⎥ _{⎢ ⎥}⎢ ⎥ ⎣ ⎦ _{− − − Ω − −} ⎣ ⎦ ⎢ ⎥ ⎣ ⎦ & & & &y (3.18) 在所有彈簧常數、阻尼常數都未知的情況下，為了估測Ω並鑑別參數d 、_x d_y、d_xy 和k 、_x k_y、k_xy，我們將 、Ω d 、_x d_y、d_xy、k 、_x k_y、k_xy視為新的狀態變數，可改寫(3.18) 式為 2 2 0 0 0 0 0 0 0 xy xy x x xy y xy y x y xy x y xy x x _{d x d y k x k y} y x _{m m m m} y y y d x d y k x k y x m m m m d k dt k k d d d ⎡ ⎤ ⎢ ⎥ ⎡ ⎤ ⎢_{− − − − + Ω} ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ _{− − − − − Ω} ⎢ ⎥ ⎢_Ω⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎣ ⎦ & & & & & &

& & &

& (3.19) 輸出方程式為： 1 2 3 4 z x z x z z y z y ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ = = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ & & (3.20)

3.4.2 系統觀察性(observability) 由(3.11)式，我們可求得系統之觀察性矩陣： (10) ( ) z z z z

rank O rank rank z

x x z z z ⎧ ⎡ ⎤⎫ ⎧ ⎡ ⎤⎫ _{⎪ ⎢ ⎥⎪} ⎪_∂ ⎢ ⎥⎪ ⎪ _∂ ⎢ ⎥⎪ ⎪ _{⎢ ⎥}⎪ ⎪ _{⎢ ⎥}⎪ = _{⎨ ⎬}≥ _⎨ ⎢ ⎥ ∂ ∂ _{⎢ ⎥}⎬ ⎪ _{⎢ ⎥}⎪ ⎪ ⎪ ⎢ ⎥ ⎪ _{⎣ ⎦}⎪ ⎪ ⎩ ⎭ _{⎪ ⎢ ⎥}⎪_⎪ ⎣ ⎦ ⎩ ⎭ & & && M &&& &&&& (3.21) 同理，計算 z z rank z x z z ⎧ ⎡ ⎤⎫ ⎪ ⎢ ⎥⎪ ⎪_∂ ⎢ ⎥⎪ ⎪ _{⎢ ⎥}⎪ ⎨ ⎬ ∂ ⎢ ⎥ ⎪ ⎪ ⎢ ⎥ ⎪ ⎪ ⎢ ⎥ ⎪ _{⎣ ⎦}⎪ ⎩ ⎭ & && &&& &&&& (3.22) 又

[ ]

[ ]

[ ]

4 7 21 7 4 22 7 7 1 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 z z rank z x z z rank A A × × × ⎧ ⎡ ⎤⎫ ⎪ ⎢ ⎥⎪ ⎪_∂ ⎢ ⎥⎪ ⎪ _{⎢ ⎥}⎪ ⎨ ⎬ ∂ ⎢ ⎥ ⎪ ⎪ ⎢ ⎥ ⎪ ⎪ ⎢ ⎥ ⎪ _{⎣ ⎦}⎪ ⎩ ⎭ ⎧⎡⎡ ⎤ ⎤⎫ ⎪⎢⎢ ⎥ ⎥⎪ ⎪⎢⎢ ⎥ ⎥⎪ ⎪_{⎢⎢ ⎥ ⎥}⎪ = _{⎨ ⎬} ⎢⎢ ⎥ ⎥ ⎪_{⎢⎣ ⎦ ⎥}⎪ ⎪_{⎢ ⎥}⎪ ⎪_{⎣ ⎦}⎪ ⎩ ⎭ & && &&& &&&& (3.23) (3.23)式中，若

[ ]

A_{22 7 7×} 為滿秩，則系統觀察性矩陣亦為滿秩，由於此觀察性矩陣過 於繁雜，我們利用 mathematica 軟體進行高斯消去法運算，最後可整理為

22 (4) (4) (4) 2 0 0 2 0 0 2 0 0 2 0 0 2 0 0 2 0 0 2 0 0 y x y x y x y x y x y x y x A y x y x y x y x y x y x y x y x y x y x y − − − − ⎡ ⎤ ⎢_{− − − −} ⎥ ⎢ ⎥ ⎢ − − − − ⎥ ⎢ ⎥ = −_⎢ − − − _⎥ ⎢ _{− − − −} ⎥ ⎢ ⎥ − − − − ⎢ ⎥ ⎢ _{− − − −} ⎥ ⎣ ⎦ & & & &

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

&&& &&& − − − & & (3.24) 陀螺儀系統為兩軸耦合之運動系統，理論上若系統存在兩個頻率，則兩軸最多可鑑 別出八個參數。因此，若系統兩軸共振頻率不同，即ω_x ≠ω_y，系統必為可觀察；若系 統兩軸頻率相同，即ω_x =ω_y =ω_n，為了驗證系統是否依然可觀察，求解動態方程式 2 2 n xy xy n x x y m y y x y m x ω ω ω ω + + = Ω + + = − Ω && & && & (3.25) 由於系統過於複雜，因此僅以忽略阻尼的情況下做討論，利用 mathematica 軟體可 解得 2 2 1 2 3 3 2 2 1 2 1 3 1 3 2 2 1 2 3 3 2 2 1 2 1 3 1 3 ( ) cos( ) ( ) cos( 2 2 ( ) sin( ) ( ) sin( ) ( ) cos( ) ( ) cos( ) 2 2 ( ) sin( ) ( ) sin( ) xy xy xy xy n n A A A A ) x t t A A t t A A y t t A A t t ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω _ω ω _ω ω ω ω ω Ω Ω = − + − Ω Ω + − = − Ω Ω − + (3.26) 其中 2 2 1 3 2 2 2 3 4 2 2 3 2 2 4 ( ) n n xy n ω ω ω ω ω ω ω ω ω = + Ω + = + Ω − = + Ω + Ω2 0 將(3.26)式帶入(3.24)，若 2 2 1 2( 1 2) ( 1 2) ω ω ω ω+ ω ω− ≠ ，即ω₃ ≠ ，則0 為滿秩，系 統觀察性矩陣亦為滿秩，故系統為可觀察之系統。 22 A

3.5 振動陀螺儀之角位移估測 傳統振動陀螺儀對於角位移的量測，一般都是利用角速度對時間進行積分來獲得， 然而，在微機電系統中，積分所得到之訊號，容易因電壓漂移(drift)而產生很大的誤差， 因此，一般我們在感測器中，都盡可能地不使用積分的方法來獲得所要量測的物理量。 在相關文獻中，Friendland 和 Hutton 最早利用動態方程式以能量的概念推導數學式，並 以數學的方式計算出角位移[13]，考慮理想振動陀螺儀之動態方程式 2 2 x y x x y y y ω ω + = Ω + = − Ω && & && &x (3.27) 假設ω_x =ω_y =ω ，求解(3.30)式可得 0 0 0 0

cos cos( ) sin sin( )

sin cos( ) cos sin( )

x a t b t y a t b t θ ω θ θ ω θ θ ω θ θ ω θ = + − = + + + + 其中 a 、 、b θ0為由系統初始值決定之常數，θ為陀螺儀之進動角(圖 3.3)。 圖 3.3 質量塊運動軌跡 令 2 2 2 2 2 2 2 2 ( ) ( ) ( 2 2 ) x y x y a b E H xy yx ab ω ω ω + + + + = = = − = & & & & (3.28)

角度θ可經由以下表示 2 2 2 2 2 2 2 2 2 2 sin 2 ( ) ( ) cos 2 2 xy xy E H x y x y E H ω θ ω ω θ ω + = − − + − = − && & & (3.29) 整理後可得 2 1 2 2 2 2 2 1 2( ) tan 2 ( ) ( xy xy ) x y x y ω θ ω − ⎛ + ⎞ = _{⎜ ⎟} − + − ⎝ ⎠ && & & (3.30) 此數學式的結果是以理想振動陀螺儀之數學模型推導出來的，然而在(2.1.2)節中我 們提到了一般的微機電陀螺儀，結構製程上的問題而造成系統動態不理想是無可避免 的。為了在非理想的陀螺系統中也能夠避開積分的方法求得角位移，我們在系統中加入 回授控制，若成功將系統控制成理想動態方程式(2.9)式，即可使用(3.30)式求得角位移。 要將非理想系統的動態控制成理想系統的動態，必須在所有狀態以及參數都已知的情況 下才能進行，而在本章前幾節已經討論了各種情況的系統觀察性，證實可利用擴增型卡 曼濾波器的方法作為觀察器，即時取得所有狀態和參數，我們考慮將觀察器狀態回授控 制使用在(2.17)式的非理想系統 2 2 x xy x xy x xy y x y mx d x d y k x k y m y u my d x d y k x k y m x u + + + + = Ω + + + + + = − Ω +

&& & & &

&& & & y & y

ˆ ˆ (3.31) 其中 ˆ _ˆ ˆ _ˆ ˆ ˆ _ˆ ˆ _ˆ ˆ x x xy xy y xy y yx u d x d y k y u d x d y k = + + = + + & & & & _x 當觀察器成功收斂至正確的狀態之後， ˆd_x → 、d_x dˆ_y →d_y、dˆ_xy →d 、_xy kˆ_xy →k_xy， 此時原非理想系統動態即控制成理想動態，再利用(3.30)式即可求得角位移。

第四章

模擬結果與討論

在第三章裡，我們對幾種不同情況下的估測進行系統觀察性分析，在本章中，將分 別對這幾種不同的情況做軟體模擬分析，並計算估測值收斂後之標準差，以及誤差百分 比。

4.1 系統動態方程式之無因次化 在使用 MATLAB 模擬系統動態時，因振動陀螺儀系統阻尼常數低、共振頻率極高， 故在模擬上常會有數值運算的誤差，無因次化即是解決數值問題的方法之一[4]。本篇論 文即是採用無因次化的方法進行軟體模擬以解決數值運算誤差的問題。 考慮加入控制力之陀螺儀系統動態 2 2 x xy x xy x xy y xy y mx d x d y k x k y m y F my d x d y k x k y m x F + + + + = Ω + + + + + = − Ω +

&& & & &

&& & & & y

(4.1) 將(4.1)式改寫成向量表示法 2 D K F q q q q m m + + = − Ω +

&& & &

m (4.2) 其中 x xy xy y d d D d d ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦， x xy xy y k k K k k ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦， 0 0 −Ω ⎡ ⎤ Ω = ⎢_Ω ⎥ ⎣ ⎦， x y F F F ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦， x q y ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ (4.3) 令q₀為參考長度，將(4.2)式做長度無因次化 0 0 0 0 2 q D q K q q F q + m q + m q = − Ωq +mq

&& & &

0

(4.4)

令ω₀為參考頻率，則無因次化時間t%=ω₀t，且 d ₀ d

化 2 2 0 0 0 0 0 0 0 0 " ' ' 2 q D q K q q F q mω q mω q ω q mω q0 Ω + + = − + (4.5) 其中q' dq dt = %，比較(4.2)式、(4.5)式，為了方便模擬，我們重新定義 、D K、Ω、F、 q 0 0 0 0 xy x xy y d d m m D d d m m ω ω ω ω ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ， 2 2 0 0 2 2 0 0 xy x xy y k k m m K k k m m ω ω ω ω ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ， 0 0 0 0 ω ω Ω ⎡ ₋ ⎤ ⎢ ⎥ ⎢ ⎥ Ω = Ω ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ， 2 0 0 2 0 0 x y F m q F F m q ω ω ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ， 0 0 x q q y q ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (4.6) 由以上推導結果，模擬的時候將可利用(4.6)式取代(4.3)式，先將方程式轉換為無因 次化，採用無因次化後的數據經電腦模擬運算，在轉換回原系統之方程式，即可避開數 值運算問題所造成之模擬誤差。 4.2 理想振動陀螺儀模擬一(Ω未知) 4.2.1 理想振動陀螺儀之角速度估測模擬 此模擬中，我們假設系統參數k 、_x k_y為已知，同時忽略阻尼不計，估測系統為 ˆ ˆ ˆ 2 ˆ ˆ ˆ ˆ 2 x y ˆ x k x y y k y x + = Ω + = − Ω && & && & (4.7) 為了估測角速度，我們將擴增型卡曼濾波器使用在(3.2.1)節中所推導出之狀態方程 式，並使用 Matlab 進行演算法模擬。表 4.1 為模擬參數。圖 4.1 為質量塊於旋轉座標 x−y平面上之運動軌跡，顯示質量塊之振動方向線以Ω的速度產生進動。圖 4.2 顯示出 系統穩態時，觀察器的輸出將原本感測器輸出所存在之雜訊濾除。圖 4.3 為觀察器狀態

收斂於實際系統狀態之情形。圖 4.4 為角速度之即時估測。透過模擬我們可以知道，擴 增型卡曼濾波觀察器可以很快將雜訊濾除，取得系統在 x 方向的位移、速度以及 方向 的位移、速度，精確的估測出角速度的值。表 4.2 為觀察器系統的估測誤差。 y 表 4.1 模擬參數 圖 4.1 陀螺儀的質量塊於旋轉座標x−y平面上之運動軌跡

圖 4.2 感測器輸出與觀察器輸出：感測器位移輸出帶有標準差 3 2.857 10× − μm 之隨機雜訊；速度輸出帶有標準差 3 之隨機雜訊 53.85 10× − μm s/ 圖 4.3 觀察器狀態收斂於實際系統狀態之情形

圖 4.4 角速度之即時估測 表 4.2 估測誤差 4.2.2 理想振動陀螺儀之角位移估測模擬一 由(3.30)式， 2 1 2 2 2 2 2 1 2( ) tan 2 ( ) ( xy xy ) x y x y ω θ ω − ⎛ + ⎞ = _{⎜ ⎟} − + − ⎝ ⎠ && & & ，此模擬中 2 x y k k ω = = 為已知，故 只要觀察器中， x 方向的位移、速度以及 方向的位移、速度能夠取得正確的值即可求 得角位移。透過擴增型卡曼濾波觀察器的方法，我們很快即可將量測雜訊濾除，使系統 狀態收斂至正確值。圖 4.5 的模擬圖中，顯示觀察器系統約在 10ms 內收歛至實際系統， 收斂後即可使用(3.30)式算出正確的角位移。 y

圖 4.5 角位移之估測 4.3 理想振動陀螺儀模擬二(Ω、k 、_x k_y未知) 4.3.1 理想振動陀螺儀之角速度估測與參數鑑別模擬 此模擬中，我們同樣忽略阻尼不計，不同於上個模擬，假設系統參數k 、_x 為未 知，此時估測系統為 y k ˆ ˆ ˆ ˆ 2 ˆ ˆ ˆ ˆ ˆ 2 x y ˆ x k x y y k y x + = Ω + = − Ω && & && & (4.8) 為了估測角速度並鑑別參數k 、_x ，我們將擴增型卡曼濾波器使用在(3.3.1)節中所 推導出之狀態方程式，並使用 Matlab 進行演算法模擬。表 4.3 為模擬參數。圖 4.6 為質 量塊於旋轉參考體 y k x−y平面上之運動軌跡，顯示質量塊之振動方向線以Ω的速度產生 進動。圖 4.7 顯示出系統穩態時，觀察器的輸出將原本感測器輸出所存在之雜訊濾除。

圖 4.8 為觀察器狀態收斂於實際系統狀態之情形。圖 4.9 為角速度之即時估測。圖 4.10 為參數之即時鑑別。透過模擬我們可以知道，擴增型卡曼濾波觀察器可以很快將雜訊濾 除，取得系統在 x 方向的位移、速度以及 方向的位移、速度，精確的估測出角速度的 值，並且鑑別出參數 y x k 、k_y。表 4.4 為觀察器系統的估測誤差。 表 4.3 模擬參數 圖 4.6 陀螺儀的質量塊於旋轉座標x−y平面上之運動軌跡