第四章 感測器整合系統
4.3 感測器整合系統模型
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 運算所獲得之 速度、加速度或角度、角速度，相互對照下極為精準，故實際上則以此增
65
圖(5.5) 平台訊號與 IMU 陀螺儀訊號對照圖 測試結果如下表(5.1)：
三軸平均 偏壓值
Gain Noise Standard Deviation 加速規 2.4873 V 208.0373(mV)
g
0.0174V 0.21379m s/ 2
陀螺儀 2.4980 V 1.27183( ) / mV
s 0.0124V 9.749 / s 表(5.1) IMU 測試結果
5.2.2 GPS 測試分析
GPS 輸出訊號則為數位訊號、輸出頻率為 1Hz，但是 GPS 所輸出的數 位訊號(NMEA 0183)則為字串訊號(ASCII CODE)，頇透過 DSP 做訊號擷取 後，分析與拆解其字串，以獲得大筆字串中所需的資訊，再將字串訊號轉 換為可帶入演算法中的浮點數，以 DSP 擷取 GPS 之字串訊號拆解如圖 (5.6)。
66
圖(5.6)GPS 實際字串訊號分析
為確認 DSP 所寫入之同步處理 IMU 與 GPS 訊號之程式語言是否正確，
在此以電腦模擬一個每秒變動之 GPS 訊號，再以 RS232 連結 DSP 接收端。
IMU 訊號再經過 AD 轉換後與模擬之 GPS 訊號轉換為浮點數後之 DSP 同步訊 號處理如圖(5.7)。
圖(5.7)DSP 同步處理 IMU 實際訊號與 GPS 模擬訊號
緯度
經度 NAEA 0183(GPGGA)
67
第六章 結論與未來計畫 6.1 結論
本文利用擴增式卡爾曼估測器建立多種估測系統動態模型，針對不同 輸出與輸入系統情況下模擬運動物體的六個自由度之姿態與路徑估測整 合系統。在姿態表示上以四元數法為主，因此更可解決姿態矩陣奇異性的 問題。
許多架構在直覺上認為輸出系統所給予的資訊，足已使估測系統準確 估測到運動物體的路徑與姿態，但從本文章節中所建立的多種估測系統動 態模型中，藉由觀察性矩陣與 EKF 模擬驗證，發現事實並非如此，單單僅 以一顆 GPS 並無法獲得運動物體精準姿態而使物體運動路徑誤差甚大。由 於陀螺儀所獲得的角速度為直接影響四元數，進而直接影響座標系轉換矩 陣，間接影響 GPS 欲修正之 IMU 雜訊誤差，亦間接影響到所估測之運動物 體路徑與姿態，因此能否精準估測四元數變成一大挑戰。以 EKF 估測系統 模擬結果如下表(6.1)。
表(6.1) 模擬結果
本文成功建立出利用兩顆 GPS 與高度計做為輸出系統、僅以兩顆 GPS 利用所獲得的導航座標系資訊帶入輸出系統，這兩種估測系統，其有效估 測其訊號偏壓，且有效抑制加速規與陀螺儀的積分誤差及降低其量測雜訊 與 GPS 位置訊號誤差的影響，進而獲得精準度更高的運動物體估測路徑與
68
姿態，誤差甚至低於百分之一，且亦減少感測器的使用，以降低商業成本。
本文中亦利用 DSP 成功的整合不同輸出頻率的加速規、陀螺儀與 GPS 的實 際訊號。
6.2 未來計畫
將有效的估測系統架構寫入 DSP，完成整合並加以實驗。目前 DSP 以 完成同步 IMU 與 GPS 的訊號擷取與分析，當估測系統架構模擬完成後即可
將有效的估測系統架構寫入 DSP，完成整合並加以實驗。目前 DSP 以 完成同步 IMU 與 GPS 的訊號擷取與分析，當估測系統架構模擬完成後即可