第四章 感測器整合系統
4.1 擴增式卡爾曼濾波器(Extended Kalman Filter，EKF)
於 1960 年，由 R.E. Kalman 所發表一篇著名的論文中，利用遞迴 (recursive)來解決離散資料的線性濾波問題。由於此時電腦數值計算正 蓬勃的發展，因此卡爾曼估測器在控制與導航系統領域中被大量的研究及 應用。
卡爾曼濾波器是一種最佳化的估測器，可以間接從不準確及不確定的 量測值來獲得系統的狀態變數，尤其對於雜訊來源為高斯雜訊時，卡爾曼 估測器可以得到最小化的均方誤差。由於卡爾曼濾波器只能用於線性系統，
而大部份的系統為非線性系統，非線性系統則頇經線性化才能使用，因此 對於被線性化的卡爾曼濾波器又稱為擴增卡爾曼濾波器(Extended Kalman Filter，EKF)。
17 規、陀螺儀的偏壓量，我們檢驗系統的可觀察性（observability）。由於 上述系統為一非線性系統，必頇使用非線性系統的觀察性矩陣來進行判斷。
State estimate at tk
k k
xˆ |
State covariance at tk
k k P | State covariance at tk
k k P | Evolution
of the system (true state)
Known input (control or sensor motion)
Estimation of the state
State covariance computation
Evaluation of Jacobians
Evaluation of Jacobians
State prediction covariance
k k F kPk k Fk Q k
P 1| | T
State prediction covariance
k k F kPk k Fk Q k
P 1| | T
Residual covariance
Residual covariance
Filter gain
Filter gain
Updated state covariance
Updated state covariance
State prediction
k k fkx k k uk
xˆ 1| ,ˆ | ,
Measurement prediction
k k hk xk k
zˆ 1| 1,ˆ 1|
Measurement residual
k 1 zk 1 zˆk 1|k
s
Re
Updated state estimate
Transition to tk+1
k fkx k uk vk x 1 , ,
Measurement at tk+1
18
19
表(4.1) 模擬參數
表(4.2) 模擬參數
首先將四元數連續的動態方程式以簡單的一階離散化可得以下方程 式:
20
21
NED NED NED
o k o k o k
22
度，因此必頇設法估測並補償其訊號偏壓值，才能確保 IMU 所獲得資訊的 精準度。
4.4 1 顆 GPS+IMU
於此小節中建立兩種估測系統動態模型，第一種為先假設加速規量測 無誤差之訊號，陀螺儀量測具有誤差雜訊，第二種為假設陀螺儀量測無誤 差之訊號，加速規量測具有誤差雜訊。模擬後並加以討論，其感測元件擺 放位置如下圖(4.3)所示，GPS 放置於 A 點，IMU 放置於 O 點而其 O 點亦為 估測位置。
圖(4.3)感測器擺放示意圖
透過剛體運動學基本理論，A 點導航座標系之速度與位置表示如下
/
/
NED NED NED b b
A o b o A o
NED NED NED b
A o b A o
V V C w r
L L C r
(4.8) 於此節以 A 點之 GPS 所量測的導航座標系之位置資訊做為系統輸出。
/
NED NED NED b
A o b A o
Z L L C r (4.9) 4.4.1 陀螺儀量測具有訊號誤差，加速規量測無訊號誤差
假設加速規量測訊號為標準無雜訊與誤差存在，陀螺儀量測為常數角 速度，估測狀態則包含四元數、三軸角速度、三軸 NED 座標系之 O 點速度、
23
NED NED
b b
24
NED NED
o o
NED NED
o k o k
25
圖(4.4)四元數對照圖
圖(4.5)角速度對照圖
26
圖(4.6)導航座標系 O 點速度對照圖
圖(4.7)導航座標系 O 點位置對照圖
由觀察性矩陣驗證姿態估測的可行性，發現有 lose rank 發生並非滿 秩，故當加速度為無誤差與雜訊情況下，且所估測的角速度僅為常數值，
並無法正確估測到四元數，導致其他估測狀態產生估測錯誤之情形，以 MatLab 模擬結果亦驗證無法精準估測運動物體之軌跡與路徑。
4.4.2 陀螺儀量測無訊號誤差，加速規量測具有訊號誤差
27
NED NED
b b
28
NED NED NED
o b o
NED NED
o k o k
29
圖(4.8)四元數對照圖
圖(4.9)附體座標系 O 點加速度對照圖
30
圖(4.10)導航座標系 O 點速度對照圖
圖(4.11)導航座標系 O 點位置對照圖
以觀察性矩陣驗證姿態估測，發現有 lose rank 發生並非滿秩，可知 即使當角速度為無任何誤差情況下之架構並無法獲得估測狀態之正確值。
MatLab 模擬結果與觀察性矩陣驗證結果相同，無法準確估測運動物體之路 徑與姿態。
31
透過上述兩種估測系統架構了解，單單僅以一顆 GPS 所獲得之位置與 速度資訊，且 IMU 估測狀態僅為估測常數項之角速度或加速度，並無法正 確估測到四元數的數值，亦即無法精準的估測運動物體的姿態與補償 IMU 之誤差訊號，故後續章節中將分別討論以 2 顆 GPS、2 顆 GPS+Barometer、
3 顆 GPS 此三種情形做為輸出系統之估測模型。主要根據不同的系統輸出 建立兩種系統輸入架構，第一種為當加速規與陀螺儀輸入訊號不帶有偏壓 訊號之情行，第二種為加速規與陀螺儀輸入帶有偏壓訊號之情形。若 EKF 估測系統模擬得以精準估測當加速規與陀螺儀不帶有偏壓訊號之情況下 的運動物體軌跡與姿態時，才有機會估測加速規與陀螺儀之偏壓訊號，進 而獲得更精準之運動物體軌跡與姿態。
4.5 2 顆 GPS+IMU
於此小節以兩顆 GPS 作為系統輸出，建立如上述之兩種輸入系統模型，
並加以延伸不同之輸出系統對於估測系統之影響，此時 IMU 所量測之角速 度與加速度則為系統輸入。其感測元件擺放位置如下圖(4.12)所示，將 GPS-A 放置於 A 點，特別的是將 GPS-B 與 IMU 放置於估測位置 O 點。
圖(4.12)感測器擺放示意圖 4.5.1 加速規與陀螺儀輸入訊號不帶有偏壓
估測狀態則包括四元數、三軸常數角速度、三軸常數加速度、導航座
32
NED NED
o o
NED NED b
A o b A o
NED NED b
A o b A o
NED NED
o o
NED NED
o o
NED NED o o
33
NED NED
o b
34
EKF 模擬結果如下所示。
模擬估測狀態為 16 1
b b NED NED T
o o o o
X q w a V L
圖(4.13)四元數對照圖
圖(4.14)角速度對照圖
35
圖(4.15)附體座標系 O 點加速度對照圖
圖(4.16)導航座標系 O 點速度對照圖
36
NED NED
o o
NED NED b
A o A o o
NED NED b
A o b A o
NED NED
o o
37
NED NED
o o
NED NED o o
38
圖(4.18)四元數對照圖
圖(4.19)角速度對照圖
39
圖(4.20)附體座標系 O 點加速度對照圖
圖(4.21)導航座標系 O 點速度對照圖
40
圖(4.22)導航座標系 O 點位置對照圖
模擬結果與觀察性矩陣驗證結果相符，當以兩顆 GPS 所獲得之導航座 標系資訊帶入估測系統中之輸出系統，可精準估測出運動物體路徑與姿態，
以此做為依據，進而模擬當輸入系統之陀螺儀與加速規訊號帶有偏壓之情 形。
4.5.2 加速規與陀螺儀輸入訊號帶有偏壓
以兩顆 GPS 之導航座標系資訊直接帶入估測系統中之輸出系統做為依 據，架構延伸建立一估測狀態則包括四元數、三軸角速度常數偏壓、三軸 加速度常數偏壓、導航座標系三軸 O 點速度、導航座標系三軸 O 點位置即
16 1
b b NED NED T
o o o o
X q dw da V L
相同低以VA oNED/ 、LNEDA o/ 、LNEDo 做為輸出系統，其所帶入之資訊則包含導 航座標系的資訊，其方程式如下
41
NED NED
o o
NED NED
o o
NED NED o o
42
NED NED
o b
43
圖(4.23)四元數對照圖
圖(4.24)陀螺儀訊號偏壓對照圖
44
圖(4.25)加速規訊號偏壓對照圖
圖(4.26)導航座標系 O 點速度對照圖
45
圖(4.27)導航座標系 O 點位置對照圖
將導航座標系資訊直接帶入觀察系統中，如預期所示，可估測精確的 姿態。
4.6 2 顆 GPS+Barometer+IMU
藉由增加可觀察的資訊即增加系統輸出，避免因座標轉換矩陣所造成 四元數估測狀態造成誤判情行，可使觀察性矩陣滿秩，以達到估測系統準 確估測運動物體姿態。在此嘗試以兩顆 GPS 並增加 Barometer 以獲得導航 座標系的高度資訊，建立兩種系統輸入架構並驗證其可行性。其感測元件 擺放位置如下圖(4.28)所示，GPS-A 放置於 A 點，GPS-B 放置於 B 點，而 Barometer 與 IMU 放置於估測位置 O 點。
46
NED NED
o Z o Z
NED NED NED b
A o b A o
NED NED NED b
B o b B o
NED NED
o Z o Z
NED NED
o Z o Z
47
NED NED
o Z o Z
NED NED
o Z o Z
NED NED
o Z o Z
48
NED NED
o b
49
圖(4.29)四元數對照圖
圖(4.30)角速度對照圖
50
圖(4.31)附體座標系 O 點加速度對照圖
圖(4.32)導航座標系 O 點速度對照圖
51
NED NED
o Z o Z
52
NED NED
o Z o Z
NED NED
o Z o Z
53
NED NED
o Z o Z
NED NED
o b
54
圖(4.34)四元數對照圖
圖(4.35)陀螺儀訊號偏壓對照圖
55
圖(4.36)加速規訊號偏壓對照圖
圖(4.37)導航座標系 O 點速度對照圖
56
圖(4.38)導航座標系 O 點位置對照圖
其模擬結果亦如我們先前所預期之結果，觀察性矩陣為滿秩故系統為 可觀察系統，且以 MatLab 模擬結果亦可精準估測運動物體之路徑與姿態。
4.7 3 顆 GPS+IMU
以三顆 GPS 做為系統輸出為一般常見用以估測角度變化之架構，於本 文中亦重新建構其估測系統模型，模擬其估測情況，並以此架構為基準。
估測狀態則包括四元數、三軸角速度常數偏壓、三軸加速度常數偏壓、導 航座標系三軸 O 點速度、導航座標系三軸 O 點位置即
16 1
b b NED NED T
o o o o
X q dw da V L
此時 IMU 所量測之角速度與加速度則為系統輸入，其感測元件擺放位 置如下圖(4.39)所示，GPS-A 放置於 A 點，GPS-B 放置於 B 點，而 GPS-C 與 IMU 放置於 O 點而其 O 點亦為估測位置。
57
NED NED
o b
NED NED
o o
NED NED b
A o b A o
NED NED b
B o b B o
NED NED
o o
58
NED NED
o o
NED NED o o
59
16rank O
觀察性矩陣滿足滿秩的條件，故此架構應為可精確的估測到四元數等 等估測狀態，透過 EKF 估測系統模擬如下：
針對此系觀察系統之 EKF 估測動態架構，模擬估測狀態為
16 1
b b NED NED T
o o o o
X q dw da V L
圖(4.40)四元數對照圖
圖(4.41)陀螺儀訊號偏壓對照圖
60
圖(4.42)加速規訊號偏壓對照圖
圖(4.43)導航座標系 O 點速度對照圖
61
圖(4.44)導航座標系 O 點位置對照圖
由模擬結果發現其估測之角速度偏壓與加速度偏壓之誤差低於百分 之一，亦精確估測到四元數等等之估測狀態，符合預期之結果。
62
第五章 DSP 實現系統訊號整合 5.1 硬體架構
本文採用 DSP(TMS320 F2812)、IMU、Barometer 與 GPS 定位系統來獲 得物體運動路徑與姿態。利用 DSP 同步接收高度計輸出的數位訊號與加速 規、陀螺儀之類比訊號再即時轉換為數位訊號，同時，DSP 亦同步接收 GPS 定位系統之字串訊號(ASCII Code)，再經過字串分析，便可將所有接收到 的資訊帶入 EKF 演算法，以即時獲得演算後之精準姿態。系統整合架構如 圖(5.1)所示。
圖(5.1)系統整合架構圖 5.2 感測元件性能測試與分析
DSP 透過程式語言的撰寫可針對不同類型與頻率的訊號進行同步擷取 與分析。加速規與陀螺儀輸出訊號皆為類比訊號、輸出頻率皆為 1KHz，透 過 DSP 進行類比與數位訊號轉始能將其資訊帶入演算法。利用線性走位平 台進行測試，實驗平台如下圖(5.2)所示，DSP 擷取 IMU 以單軸直線 Sin 波 形式運動之訊號如圖(5.3)。
63
圖(5.2)實驗平台
圖(5.3) 單軸直線運動之 IMU 實際六軸訊號 5.2.1 IMU 測試校正
以線性走位平台進行 DSP 所擷取之 IMU 訊號測試與對照，馬達 Encoder 精度為 0.5um，透過 Alpha-Bara Filter 運算用以抑制馬達 Encoder 雜訊 誤差與補償精度之不足，進而獲得平台位移之位置、速度、加速度或者平 台之旋轉角速度與角度。Alpha-Beta Filter 動態架構如下：
64 依此設定以 MatLab 模擬一標準弦波以 Alpha-Bata Filter 運算所獲得之
64 依此設定以 MatLab 模擬一標準弦波以 Alpha-Bata Filter 運算所獲得之