陀螺儀

27

因為陀螺儀感測器的測量值會受到外在溫度的影響，所以頇要在每次使用前 的熱機過程中先測量出靜止狀態下穩定的基準值，一般的做法就是持續讀取數次 之後再求出平均值，不過用來測量的物體轉動之角度應用因為每次取樣時都會參 考基準值來計算位移，若基準值不夠準確，累積的誤差會讓結果失真。因此，提 出了陀螺儀基準值的準確度改善的方法：首先以人為的方式擴大所讀取到感應器 回應值的精確位數，因此，當進行平均值計算時就不會因為整數運算的關係被捨 棄太多的準確位數，接著會透過加權平均的概念，來抑制取樣過程中突發的雜訊 影響基準值計算的準確度，意即降低每一次取樣影響計算結果的敏感性。本論文 使用數位濾波器，即卡曼濾波器以減低訊號干擾所造成的訊號跳動。1960 年，

卡曼發表一篇著名的論文，其描述用遞迴方法解決離散資料線性濾波的問題。卡 曼濾波器基本上是以一套數學方程式實現預測-校正型式的估算器。當被推測情 況發生時，估算器做出最佳的判斷讓估計誤差協方差減到最小。從那之後，卡曼 濾波器已成為廣大研究和應用的主題，尤其是在自主或導航領域。

卡曼濾波的基本動態系統模型[24]，假設目前狀態 k 時刻是從 k-1 時刻的狀 態演化而來，符合下列公式：

1

1 

  

 _{k k k}

k Ax Bu w

x (2-8)

其中 k 是時間系數，

x 是系統狀態，

_k A和B為矩陣，

u 是控制訊號，

_k w_k1為過程 雜訊。k 時刻，

x 的測量值

_k z_k滿足下列公式：

k k

k Hx v

z   (2-9)

其中z_k是測量值，H為矩陣，v_k為測量雜訊。

x 中包含系統目前狀態的所有資

_k 訊，但它不能被直接測量。因此，要先測量z_k取得

x 的估計，

_k z_k為受到雜訊干 擾的系統狀態 x 函數。

卡曼濾波器可分兩個部份：時間更新(預測)方程式與測量更新(校正)方程 式。時間更新方程式負責及時的向前推算當前狀態變量和誤差協方差估計的值，

以便取得為下一個時間狀態的先驗估計值。測量更新方程式負責回饋，也就是

29

說，它將先驗估計和新的測量值結合以取得改善的後驗估計值。最後，估計演算 法成為一種解決數值問題的預測－校正演算法。

圖 2.20 卡曼濾波器工作原理圖[24]

原理為首先要利用系統的過程模型來預測下一狀態的系統。假設目前狀態是 k，

根據系統模型可以利用上一狀態(k-1)而預測出目前狀態：

k k

k Ax Bu

xˆ^  ˆ _1

(2-10)

上式中xˆ_k1為上一狀態的結果，xˆ_k^為目前狀態的預估值，

u 為目前狀態 k 的控制

_k 量，如果沒有控制量則為 0。到此，系統結果已經更新了。可是對應於xˆ_k^的協方 差還沒更新。P_k^為估計誤差協方差(estimate error covariance)可表示如下

Q A AP

P_k^  _k1 ^T  (2-11)

上式中P_k^是xˆ_k^對應的估計誤差協方差，P_k1是xˆ_k1對應的估計誤差協方差，

A 表

^T 示 A 的轉置矩陣，Q是過程雜訊協方差。以上公式(2-10)及公式(2-11)就是對系統

為初始估計 時間更新(預測)

測量更新(校正)

(1)向前推算狀態變量

(2)向前推算誤差協方差

(1)計算卡曼增益

(2)由觀測變量 ZK更新估計

(3)更新誤差協方差

的預測。在測量更新階段，首先必頇求出卡曼增益(Kalman Gain)可表示如下 ) 1

( ^{ }

 

P H HP H R

K_{k k} ^T _k ^T (2-12)

上式中，K_k為卡曼增益，R為測量雜訊的協方差，H為測量系統的參數。有了 預估值xˆ_k^，結合測量值z_k，就可以得到現在狀態 k 的最優化估計值

xˆ 可表示如

_k 下

ˆ ) ˆ (

ˆ_k x_k^K_K z_k Hx_k^

x (2-13)

到此為止，已經得到 k 狀態下最佳的估計值

xˆ 。但為了要讓卡曼濾波器不斷的

_k 運行下去，直到系統過程結束，還要更新 k 狀態下

xˆ 的協方差

_k

P 可表示如下

_k

 

 _{k k}

k I K H P

P ( ) (2-14)

上式中 I 為 1 的矩陣，對於單模型單測量，I=1。當系統進入 k+1 狀態時，

P 就

_k 是公式(2-11)的

P 。如此，算法就可以自回歸的運算下去。

_k1

本論文使用卡曼濾波器來減低訊號干擾所造成的訊號跳動其公式如圖 2.20 所示，首先需設定初始值xˆ_k1與P_k1，其中A、B、

u 、

_k Q、H、R皆為可調參 數。從時間更新方程式求得先驗狀態預估值xˆ_k^與先驗估計誤差協方差P_k^後，再 從測量更新方式中計算卡曼增益K_k，其次從過程中得到陀螺儀實際測量值z_k， 求出後驗狀態預估值

xˆ ，再求出後驗估計誤差協方差

_k

P 。每次完成時間及測量

_k 更新方程式後，將上一次計算得到的後驗估計值作為下一次計算的先驗估計初始 值，如此，便可自回歸運算。

在使用卡曼濾波器時，必頇先設定初始估計值(Initial estimates)xˆ_k1與

P 。

_k1 本論文中，令初始估計值xˆ_k1=2048、

P =1、

_k1 A=1、B=1、

u =0、

_k Q=0.0005、

H=1、R=1，由公式(2-10)可以得到下式：

ˆ 1

ˆ_k^  x_k

x (2-15)

31

由公式(2-11)可以得到下式：

Q P

P_k^ _k1 (2-16) 由公式(2-12)、(2-13)與(2-14)可以得到下式：

) /(P R P

K_k  _k^ _k^ (2-17) ˆ )

ˆ (

ˆ_k x_k^K_K z_k x_k^

x (2-18)

 

 _{k k}

k K P

P (1 ) (2-19)

z 為陀螺儀實際測量值，經由卡曼濾波器的處理後可得到較平滑的更新值

k

xˆ 。

_k 而R與Q為主要調整參數，其功能主要用來抑制訊號的跳動。R越大，則抑制能 力越強；R越小，則抑制能力越弱。Q 越小，則抑制能力越強；Q越大，則抑 制能力越弱。

第三章 平衡控制系統設計

在文檔中 中 華 大 學 碩 士 論 文 (頁 36-41)