論文架構

本論文共分為六章，第一章「緒論」，介紹研究動機、文獻回顧與計畫書架 構。第二章「姿態量測序言」，介紹使用的座標、座標轉換以及四元數表示法。

第三章「系統動態模型」，介紹感測器整合系統、擴增型卡爾曼濾波器和新式估 測器。第四章「不同感測器失效情形」，介紹利用追星儀及一軸或兩軸陀螺儀設 計的動態模型。第五章「模擬結果與討論」，記錄目前系統模擬和分析後的結果。

第七章「結論」，報告此計劃目前為止的成果。

第二章：姿態量測序言

2.1 座標系統

在導航與定位的過程中，除了仰賴感測器取得量側值外，往往需要進行一些 座標系轉換以推算出符合使用者習慣的航向角及位置。本節將介紹一般常使用的 座標系及不同座標系之間的轉換。

2.1.1 地心座標系（Earth Centered Inertial Frame）

地心座標系（ECI）是以地心為原點，其三個軸固定不動，不隨著地球旋轉。

其三軸與原點規定如下：

原點：位於地球的質量中心。

X 軸：在春分點時由地心指向太陽之方向。

Z 軸：為地球的自轉軸，由原點指向北極點，即平行於 CIO（Conventional International Origin）之平均北極。

Y 軸：由右手定則

Z × X

^來決定。

ECI Coordinate Frame X axis

Z axis

Y axis

ECI Coordinate Frame X axis

Z axis

Y axis X axis

Z axis

Y axis

圖(2.1) ECI 座標系

2.1.2 地心地固座標系（Earth Centered Earth Fixed Frame, ECEF）

也稱為傳統地面座標系（Convention Terrestrial Reference System, CTRS），是以 地心為原點，其三個軸相對於地球是固定的，因此隨著地球旋轉。其三軸與原點 規定如下：

原點：位於地球的質量中心。

X 軸：通過格林威治之天文子午圈，即經度的零度。

Z 軸：為地球的自轉軸，由原點指向北極點，即平行於 CIO（Conventional International Origin）之平均北極。

Y 軸：由右手定則

Z × X

^來決定。

2.1.3 附體座標系(Body frame)

它是一組正交座標系，其三軸的定義通常是對準於載具（vehicle）前方－右 側方－下方，原點位於載具的重心，如下圖所示，其附體座標定義於衛星上，並 隨著衛星移動及旋轉。

Z

X Y

Pitch y Yaw z Roll x

Z

X Y

Pitch y Yaw z Roll x

Pitch y Yaw z Roll x

圖(2.2) 附體座標系

2.1.4 附體座標系與地心座標系之關係

2.1.5 姿態表示法

數學上有多種的姿態表示法，有方向餘弦法、尤拉角法、四元數法。因為本 報告主要以四元數法為主，尤拉角法為輔，因此本節介紹四元數法及尤拉角法。

尤拉角法（Euler angle）

以尤拉角為姿態表示，其變數只有三個而且互相獨立。若以前述三個旋轉角 度，將 ECI 座標依序旋轉航向角（Yaw）、俯仰角（Pitch）及側滾角（Roll）得到 附體座標，則描述附體座標上的位置向量與 ECI 座標上的位置向量，兩者之間的

cos cos cos sin sin

cos sin sin cos sin cos cos sin sin sin cos sin

cos cos sin sin sin cos sin sin cos sin cos cos

, , ,

( ) ( ) ( ) ( )

( )

單範的性質限制（normalization constraint）

q

₀² +

q

₁²+

q

₂²+

q

₃² =1 (2.10)

第三章：系統動態模型

3.1

感測器整合系統

本計畫採用陀螺儀與追星儀來獲得衛星在太空中的姿態。陀螺儀具有高精度 角速度量測、高輸出頻寬（16Hz）、經過一次積分獲得角度，缺點是輸出訊號的 雜訊及飄移值（drift）會隨著積分運算而造成誤差累積。使用追星儀的優點是可 直接獲得角度，缺點是其訊號輸出頻寬太低（4Hz），無法滿足即時控制（real-time control）的需求。因此利用陀螺儀與追星儀所組成的「感測器整合系統」（sensor fusion system），來正確估測衛星在太空中的姿態是需要的。

本方法的主要架構是將陀螺儀（16Hz）量測得到的衛星在附體座標上的角 速度資訊輸入一描述角度與角速度關係之動態模型

，進行姿態估測，然後利用追

星儀（4Hz）得到之四元數資訊更正估測系統的姿態誤差，此誤差可能來自於初 始條件（initial conditions）的設定、陀螺儀訊號的雜訊、飄移值、等。姿態估測 系統的流程圖如圖一所示。

Gyros

STR

Sensor Data Processing

Prediction

Lyapunov Gains Attitude

Residuals

Attitude Gyro drift Correction

Spacecraft Attitude Estimator

ˆ( 1)

Sensor Data Processing

Prediction

Lyapunov Gains Attitude

Residuals

Attitude Gyro drift Correction

Spacecraft Attitude Estimator

ˆ( 1)

陀螺儀角速度量測：

Gyro,m I3 3 misalignment Gyro_ideal

d w ω

= _× ×

ε ω

+ +

cos cos cos sin sin

cos sin sin cos sin cos cos sin sin sin cos sin

cos cos sin sin sin cos sin sin cos sin cos cos

misalignment

追星儀角度量測（四元素表示法）：

在考量不同的雜訊來源，四元數量測雜訊的模擬可分為兩種情況：（1）一種 為獨立給入，（2）以四元數乘積（quaternion multiplication）方式給入，其中本論 文在模擬的時候是採用獨立給入的方式。 with noise ideal

STR STR

q

=

q

+

v

_×

noise n

n

再利用四元數乘積（quaternion multiplication）方式加入理想狀況下四元數的

角度量測值 ，進而得到具雜訊的四元數量測值 。以四元數乘積方式

給入雜訊的特點在於即使考慮誤差，其四元數的平方和仍然等於 1。其關係式如 下表示：

ideal

q

STR

q

_STR^{with noise_}

_

0 1 2 3 0

1 0 3 2 1

2 3 0 1 2

3 2 1 0 3

with noise ideal

STR STR noise

ideal

with noise Gyro m

3.2 系統的觀察性分析

由於擴增型卡爾曼濾波器（Extended Kalman filter，EKF）可以有效的過濾 系統的雜訊，且可以整合不同輸出頻率的感測器，因此一般導航系統常採用此技 巧來估測姿態。在此例中，可藉此方法來整合不同輸出頻率的陀螺儀與追星儀，

並藉此提高感測系統的輸出精度。

3.3.1 擴增型卡爾曼濾波器（Extended Kalman Filter）

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

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

由於卡爾曼濾波器只適用於線性的系統，由於此論文的系統為非線性系統，

對於非線性的系統需線性化才能使用，因此對於被線性化的卡爾曼濾波器又稱為 擴增卡爾曼濾波器(Extended Kalman Filter, EKF)。EKF 流程圖如下所示：

State at t

_k x(k)

Input at t

_k u(k)

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

( )

3.3.2 儲存記憶褪去式擴增型卡爾曼估測器（Adaptive Fading Extended Kalman Filter，AFEKF）

由於先前假設陀螺儀的訊號飄移量為一常數，而實際應用中並非如此。此假 設所造成的誤差很可能會使得 EKF 在估測系統狀態時失效。此一現象可被理解 為狀態觀察器內所使用的物理系統的數學模型無法完全描述真實系統的動態，而 EKF 的強健性（robustness）不足，使得估測失效。為解決此一問題，我們使用 AFEKF 來解決。此方法的特別之處在於增加了一個忽略係數

λ

，此係數調整 EKF

State at t_k x(k)

Input at t_k u(k)

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

( )

Residual covariance

( )

Residual covariance

( )

Filter gain

( )

Filter gain

( )

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

( )

3.4 設計新式估測器進行姿態估測

(3.8) semi-definite）。藉由 Lyapuonv first stability theorem，可知 ，且 收歛到某一

常數（不一定為零）。故為了驗證 、 是否會同時收斂於零，我們將 代

（asymptotically stable）。

e

q

e

_d

第四章：不同感應器失效情形

x Gz with noise

STR

4 4 4 3

4.2 二軸陀螺儀資訊失效

Gz with noise

x

4.2.1 架構二軸陀螺儀失效之新式估測器

第五章：模擬結果與討論

模擬參數設定

目前正在運作的衛星姿態判斷時序表及硬體架構如下：陀螺儀的輸出頻率 16Hz，追星儀的輸出頻率 4Hz，每 0.25 秒（4Hz），系統會讀取四筆陀螺儀輸出 值及一筆追星儀的輸出值，進行衛星姿態判定。為了採用同樣的時序架構及硬體 設備，在目前的驗證過程中，我們設計一姿態估測系統其取樣頻率為 16Hz，每 一取樣時間點都使用該時間點下的陀螺儀輸出值，每 4 個取樣時間，讀取一追星 儀輸出值。待確認無誤後，於衛星系統實際應用中，仍是每 0.25 秒進行一次姿 態估測，只是每次估測會進行 4 個時間點的姿態計算。其中陀螺儀量測雜訊 與 四元數量測雜訊 皆為高斯分佈，其平均值為零，標準差分別為 3.998e-005 deg/sec 與 1.5e-005。

w v

5.1 使用 AFEKF 估測四元數與三軸訊號飄移值

假設我們想要的估測的訊號飄移值為常數，並配合傳統的 EKF 來進行模擬。

模擬參數：

ideal

ωGyro （陀螺儀理想輸出角速度）：0.001 rad/sec = 0.05729 deg/sec

d（陀螺儀角速度訊號飄移量）：3 deg/hr = 1.4537e-005 rad/sec = 8.3333e-004 deg/sec

圖（5.1）姿態四元數真實值與估測值（幾乎重疊）

圖（5.2）三軸訊號飄移真實值與估測值

圖（5.3）訊號飄移量誤差收斂情形

由圖（5.2）、圖（5.3）可以發現系統在估測訊號飄移量的時候，在模擬時 間達到約 500 秒時產生發散的情況。我們猜測因為實際系統為連續時間動態系 統，而估測器為一離散系統，因此必須對連續時間系統離散化後，才能套用擴增 型卡爾曼濾波器的運算步驟。對連續時間系統進行離散化的過程，會引入些許的 誤差，此誤差的大小取決於取樣時間的長短、離散化的方式。先前的模擬過程中，

其取樣頻率為 16Hz，離散化採用最簡單的 Euler Explicit 的方式，其取樣頻率略 低，且離散方式僅以一階方式近似，可能造成連續時間系統與離散系統間的誤差 超過預期。為探究此一問題，我們直接估測一離散系統的狀態值，去除取樣時間、

離散化的誤差，直接驗證估測系統的設計是否有誤。

圖（5.4）三軸訊號飄移真實值與估測值

圖（5.5）訊號飄移量誤差收斂情形

由圖（5.4）與圖（5.5）的模擬結果可以看出，即使在多模擬了五倍的時間 下，估測值依然穩定收歛到正確值，並且沒有發散的現象。因此我們可以推論之 前的案例發散現象來自於系統離散化過程的誤差。

此離散化的誤差可被視為一真實系統與估測系統對系統動態描述的誤差。此 一誤差原本可藉由較高的回授增益值（feedback gain）加以補償，使得估測系統 不至於發散。然而由於擴增型卡爾曼濾波器的特性，其回授增益值會逐漸的降低 以換取更高的估測精度，因此就喪失的估測系統的強健性（robustness）。此一現 象說明了估測系統在模擬開始時收斂，但是卻在模擬的後半段發散。

所以我們使用更強健性的儲存記憶褪去式擴增型卡曼濾波器來進行模擬。

圖（5.6）姿態四元數真實值與估測值（幾乎重疊）

圖（5.7）三軸訊號飄移真實值與估測值

圖（5.8）訊號飄移量誤差收斂情形

圖（5.9）尤拉角的真實值與估測值

圖（5.10）尤拉角誤差收斂情形

圖（5.6）（5.9）是將四元數資訊轉換成尤拉角，估測值與真實值的比較和

在文檔中 以追星儀配合一或二軸陀螺儀進行姿態判定之研究 (頁 13-0)