導航座標系（Navigation Frame）

這組座標系通常用於導航使用，可以直接提供導航者方位、航向等訊 息。其軸向定義如下。

原點：定義為導航者所處的位置。

N 軸：指向地理北方。

D 軸：指向地心，即為平行地球的重力方向。

E 軸：由右手座標系 D×N 來決定，故指向東方。

因此也簡稱為 NED 座標系。

5

圖(2.1)ECEF 與 NED 座標系 2.1.3 附體座標系(Body Frame)

它是一組正交座標系，其三軸的定義通常是對準於載具（vehicle）

前方－右側方－下方，原點位於載具的重心，如圖(2.2)所示，其附體做 標定亦於物體上，並隨著物體移動及旋轉。

圖(2.2)附體座標系 2.2 座標轉換

前述之地心地固座標系、導航座標系、附體座標系，皆是用以描述物 體於一空間中的位置，其中附體座標系會隨著物體旋轉，用來描述物體位 置最為方便，但是欠缺一固定點做為參考，因此有必要探討附體座標系與 導航座標系之間的關係。關於物體的三種不同的旋轉方式，分別是側滾角

（Roll）、俯仰角（Pitch）及航向角（Yaw）。我們可藉由這三種不同的角

D N

E

x z

y

Z

X Y

6

7

8

觀察式(2.6)可發現當俯仰角（Pitch）為+/- 90∘時，也就是載具垂 直向上或垂直向下飛行，其轉換矩陣會發生奇異性（singularity）。故利

9

用此方程式來計算姿態的轉換矩陣時，俯仰角會有所限制。

2.3.2 四元數法（Quaternion）

四元數為 William R.Hamilton 於 1843 年所提出，使用四元數表示法，

其優點為計算姿態轉換矩陣時不會有奇異性問題。在介紹四元數法之前先

10 頇滿足單範的性質限制（normalization constraint）

2 2 2 2

由(2.4)、(2.11)式，其 NED 座標依序旋轉航向角（Yaw）、俯仰角（Pitch）

及側滾角（Roll）得到之附體座標，則可獲得四元數與尤拉角之間的轉換

11

第三章 感測元件量測原理 3.1 慣性感測元件(IMU)

本文使用的 IMU 為 MEMSense 公司之 AR10-1200S050，具有三軸加速規 與三軸陀螺儀感測元件，其主要輸出訊號為類比電壓訊號，本小節將介紹 加速規與陀螺儀分別所獲得之加速度與角速度之應用原理。

圖(3.1)IMU 元件 3.1.1 加速規

根據據剛體運動學，一個剛體在空間中運動，其加速度可用固定座標 及運動座標之關係來表示，其中在此系統中是以導航座標為固定座標，而 附體座標為運動座標，其方程式如式(3.1)：

圖(3.2)固定座標及附體座標

12

13

bias noise

  d  (3.5)

14

圖(3.3)GPS 模組

利用 GPS 接收的資訊用以獲得運動剛體位置資訊有多種方法，較常見 為被編譯好的位置資訊如 NMEA 0183 國際標準數據格式，其位置誤差約為 10 公尺，本架構則以此 GPS 資訊為主。而 GPS 所接收到的訊號頻率皆約為 1Hz，也因為其接收訊號頻率過於緩慢，故 GPS 無法單獨使用於高速高精 準度運動姿態估測系統中，通常整合訊號擷取頻率較高之加速規與陀螺儀，

在高速動態下用以獲得更加精準的姿態資訊而 GPS 則做為校正補償 IMU 的 訊號漂移之問題。然而 GPS 的接收天線不一定與 IMU 及 Boarmeter 放置於 同一位置上，為了整合加速規與陀螺儀，GPS 所擷取到的位置姿態必頇透 過下列方程式做轉換，方程式如式(3.6)：

/

NED NED b gps

j o b j o

R R C r n (3.6)

Rj：GPS 所量測到之導航座標系之位置資訊。

NED

Ro ：導航座標系之 o 點量測到之標準位置資訊。

NED

Cb ：附體座標系與導航座標系之轉換矩陣。

/ b

rj o：GPS 與 o 點之附體座標系之距離。

ngps：GPS 之量測雜訊與誤差。

15

3.3 氣壓式高度計

本文中使用的氣壓式高度計為 Intersema 公司之 ms5540C 模組，為全 數位輸出，以避免線路雜訊所造成之影響。且具有溫度感測元件做為補償

sea measurment

gradient sea

T P

16

第四章 感測器整合系統

本文主要以 EKF 模擬之架構是將陀螺儀所量測到物體在附體座標上的 角速度資訊帶入以四元數表示法為主的角速度與角度之動態模型，以獲得 附體座標系與導航座標系之轉換矩陣，而加速規所量測到物體在附體座標 上的加速度資訊，透過座標系統轉換矩陣，轉換為導航座標系之加速度，

再帶入加速度、速度、位置關係之動態模型，進行姿態估測，然後整合 GPS 定位系統與 Barometer 所得到之位置資訊更正估測系統的姿態與路徑誤差，

此誤差可能來自初始條件的設定、陀螺儀與加速規的訊號雜訊、飄移量等 等因素，還有 GPS 定位系統資訊如 NMEA 0183 國際標準數據格式的位置誤 差量與 Barometer 訊號之雜訊。於本文將 IMU 感測元件所量測的訊號視為 估測系統輸入訊號，其為運動物體之附體座標系所量測之三軸角速度與三 軸加速度訊號。GPS 則視為估測系統輸出訊號，其所獲得資訊為導航座標 系之運動物體的三軸速度與位置資訊。Barometer 亦視為估測系統輸出訊 號，其所獲得資訊為導航座標系之運動物體的高度資訊。

4.1 擴增式卡爾曼濾波器(Extended Kalman Filter，EKF)

於 1960 年，由 R.E. Kalman 所發表一篇著名的論文中，利用遞迴 (recursive)來解決離散資料的線性濾波問題。由於此時電腦數值計算正 蓬勃的發展，因此卡爾曼估測器在控制與導航系統領域中被大量的研究及 應用。

卡爾曼濾波器是一種最佳化的估測器，可以間接從不準確及不確定的 量測值來獲得系統的狀態變數，尤其對於雜訊來源為高斯雜訊時，卡爾曼 估測器可以得到最小化的均方誤差。由於卡爾曼濾波器只能用於線性系統，

而大部份的系統為非線性系統，非線性系統則頇經線性化才能使用，因此 對於被線性化的卡爾曼濾波器又稱為擴增卡爾曼濾波器(Extended Kalman Filter，EKF)。

17 規、陀螺儀的偏壓量，我們檢驗系統的可觀察性（observability）。由於 上述系統為一非線性系統，必頇使用非線性系統的觀察性矩陣來進行判斷。

State estimate at t_k

k k

xˆ |

State covariance at t_k

 k k P | State covariance at t_k

 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 fkx   k k uk

xˆ 1|  ,ˆ | ,

Measurement prediction

k k hk xk k

zˆ 1|  1,ˆ 1|

Measurement residual

k 1 zk 1 zˆk 1|k

s

Re     

Updated state estimate

  Transition to t_k+1

k  fkx   k uk  vk x 1 , , 

Measurement at t_k+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 點速度對照圖

在文檔中 以DSP實現GPS與IMU感測器整合系統 (頁 15-0)