國立交通大學
機械工程學系
碩士論文
以追星儀配合一或二軸陀螺儀進行姿態判定之研究
Satellite Attitude Determination Methods Using Star
Sensors and Gyroscopes
研究生：蔡致誠
指導教授：陳宗麟 教授
以追星儀配合一或二軸陀螺儀進行姿態判定之研究
研究生：蔡致誠 指導教授：陳宗麟 博士
國立交通大學機械工程學系
摘要
本論文發展一套以Lyapunov方法為基礎的新式估測演算法用以整合追星儀與陀螺 儀的輸出資訊來進行衛星姿態判定，除了能夠估測並補償陀螺儀的訊號誤差，還能夠適 應於部分陀螺儀發生故障的情況。此演算法的主要優點在於不需要衛星動態模型，不但 能夠解決先前文獻所遭遇的模型誤差，還能夠提供較為簡易的運算過程以節省記憶體空 間與加快運算速度。 本論文運用Matlab做系統的模擬。在陀螺儀具有訊號誤差的狀況，模擬結果指出本 論文所提出的新式估測演算法能夠獲得與擴增型卡爾曼濾波器（Extended Kalman Filter）相當的估測姿態精度（5 ）。在僅使用一軸或兩軸的陀螺儀來估測衛星 姿態的案例下，新式估測演算法還能運用僅剩的追星儀與陀螺儀來繼續提供精確的衛星 姿態判定，以延長衛星的有效工作時間，其姿態精度分別為7 與8 。 4 10 deg− × 4 10 deg− × 4 10 deg− ×Satellite Attitude Determination Methods Using Star Sensors
and Gyroscopes
Student: Chih-Chen Tsai Advisor: Dr. Tsung-Lin Chen
Department of Mechanical Engineering
National Chiao Tung University
Abstract
A novel attitude estimation algorithm based on Lyapunov method is developed in this thesis to tackle with gyroscope failures and single drifts. This algorithm uses a star tracker and a 3-axis gyroscope to determine the satellite attitude. The advantage of this algorithm is to require no knowledge of the satellite dynamic model. As such, this attitude estimation algorithm not only eliminates the model error suffered from previous research but also provides a simple calculation process to save the memory space and reduce the calculation time.
Simulations in the thesis are operated by Matlab. In the condition that the gyroscope with signal drifts, simulation results indicate that the proposed algorithm can obtain the satellite attitude with the accuracy of 5 , which is similar to that of the extended Kalman filter. In the cases of using 1-axis and 2-axis gyroscope, the proposed algorithm can employ the star tracker and the rest of the gyroscope to prolong the effective work time and expand the work site; the attitude accuracy are 7 and 8 , respectively.
deg 10−4 × 4 10 deg− × 4 10 deg− ×
誌謝
本論文得以順利完成，首先要先感謝指導教授給予我研究上的許多知識。老師在 此領域上有著卓越的研究基礎，所以在我遇到問題的時候能夠適時的給於協助，並提供 我許多自己思考的空間，讓我學習到研究應有的態度與解決問題的思考方法，在此獻上 誠摯的謝意與敬意。 此外，更要感謝實驗室的許齡元學長在研究的過程中給予我許多的建議與想法， 並在平日透過傳接球紓發研究的辛苦；還要感謝女友宜倫一直支持著我，讓我在學業與 生活的路上並不孤單，並謝謝實驗室裡所有的學長、同學和學弟，感謝他們於這兩年間 給予我許多美好的回憶。目錄
中文摘要 ... Ⅰ
Abstract ... Ⅱ
誌謝 ... Ⅲ
目錄 ... Ⅳ
表目錄 ... Ⅵ
圖目錄 ... Ⅶ
一、 緒論 ... 1
1.1 研究動機與文獻回顧 ...1 1.2 論文架構 ...3二、 姿態量測序言 ... 4
2.1 座標系統 ...4 2.1.1 地心座標系 ...4 2.1.2 地心地固座標系 ...5 2.1.3 附體座標系 ...5 2.1.4 附體座標系與地心座標系之關係 ...6 2.1.5 姿態表示法 ...8三、系統動態模型 ... 11
3.1 感測器整合系統 ...11 3.2 系統的觀察性分析 ...15 3.3 使用擴增型卡爾曼濾波器進行狀態估測 ...15 3.3.1 擴增型卡爾曼估測器 ...163.3.2 儲存記憶褪去式擴增型卡爾曼估測器 ...17 3.4 設計新式估測器進行姿態估測 ...19
四、不同感應器失效情形 ... 21
4.1 一軸陀螺儀資訊失效 ...21 4.1.1 架構一軸陀螺儀失效時之新式估測器 ...22 4.2 二軸陀螺儀資訊失效 ...23 4.2.1 架構二軸陀螺儀失效時之新式估測器 ...24五、模擬結果與討論 ... 25
5.1 使用 AFEKF 估測四元數與三軸訊號飄移值 ...25 5.2 使用新式估測四元數與訊號飄移值 ...33 5.3 追星儀正常運作，陀螺儀失效情況 ...46 5.4 新式估測器模擬結果討論 ...52六、結論 ... 55
參考文獻 ... 56
表目錄
表［5.1］穩態時之估測誤差標準差 ...52
表［5.2］與傳統估測器比較估測精度 ...52
表［5.3］一軸失效穩態時之估測誤差標準差 ...53
圖目錄
圖(2.1) ECI 座標系 ...4 圖(2.2) 附體座標系 ...5 圖(2.3) 側滾角之示意圖及座標 ...6 圖(2.4) 俯仰角之示意圖及座標 ...7 圖(2.5) 航向角之示意圖及座標 ...7 圖(2.6) 尤拉定理 ...9 圖(3.1) 姿態估測系統的流程圖 ...11 圖(3.2) 衛星上陀螺儀配置概念圖 ...12 圖(3.3) EKF 流程圖 ...16 圖(3.4) AFEKF 流程圖 ...18 圖(5.1) 姿態四元數真實值與估測值（幾乎重疊） ...26 圖(5.2) 三軸訊號飄移真實值與估測值 ...26 圖(5.3) 訊號飄移量誤差收斂情形 ...27 圖(5.4) 三軸訊號飄移真實值與估測值 ...28 圖(5.5) 訊號飄移量誤差收斂情形 ...28 圖(5.6) 姿態四元數真實值與估測值（幾乎重疊） ...29 圖(5.7) 三軸訊號飄移真實值與估測值 ...30 圖(5.8) 訊號飄移量誤差收斂情形 ...30 圖(5.9) 尤拉角的真實值與估測值 ...31 圖(5.10) 尤拉角誤差收斂情形 ...31 圖(5.11) 使用新式估測器之姿態四元數真實值與估測值（幾乎重疊） ..33 圖(5.12) 使用新式估測器之三軸訊號飄移真實值與估測值 ...34 圖(5.13) 訊號飄移量誤差收斂情形 ...34 圖(5.14) 尤拉角的真實值與估測值 ...35 圖(5.15) 尤拉角誤差收斂情形 ...35圖(5.16) 使用新式估測器之姿態四元數真實值與估測值（幾乎重疊） ..36 圖(5.17) 使用新式估測器之三軸訊號飄移真實值與估測值 ...37 圖(5.18) 訊號飄移量誤差收斂情形 ...37 圖(5.19) 尤拉角的真實值與估測值 ...38 圖(5.20) 尤拉角誤差收斂情形 ...38 圖(5.21) 使用新式估測器之姿態四元數真實值與估測值（幾乎重疊） ..39 圖(5.22) 使用新式估測器之三軸訊號飄移真實值與估測值 ...40 圖(5.23) 訊號飄移量誤差收斂情形 ...40 圖(5.24) 尤拉角的真實值與估測值 ...41 圖(5.25) 尤拉角誤差收斂情形 ...41 圖(5.26) 陀螺儀量測角速度隨時間變化 ...42 圖(5.27) 使用新式估測器之姿態四元數真實值與估測值（幾乎重疊） ..43 圖(5.28) 使用新式估測器之三軸訊號飄移真實值與估測值 ...43 圖(5.29) 訊號飄移量誤差收斂情形 ...44 圖(5.30) 尤拉角的真實值與估測值 ...44 圖(5.31) 尤拉角誤差收斂情形 ...45 圖(5.32) 使用新式估測器之姿態四元數真實值與估測值（幾乎重疊） ..46 圖(5.33) 使用新式估測器之正常軸訊號飄移及失效軸角速度的真實值與估測值 ...47 圖(5.34) 正常軸訊號飄移及失效軸角速度的誤差收斂情形 ...47 圖(5.35) 尤拉角的真實值與估測值 ...48 圖(5.36) 尤拉角誤差收斂情形 ...48 圖(5.37) 使用新式估測器之姿態四元數真實值與估測值（幾乎重疊） ..49 圖(5.38) 使用新式估測器之正常軸訊號飄移及失效軸角速度的真實值與估測值 ...50 圖(5.39) 正常軸訊號飄移及失效軸角速度的誤差收斂情形 ...50
圖(5.40) 尤拉角的真實值與估測值 ...51 圖(5.41) 尤拉角誤差收斂情形 ...51
第一章：緒論
1.1 研究動機與文獻回顧 衛星運作於太空中時，必須精準地確認本身位置與姿態，才能進行相關的 空拍、量測…等工作。因此，精確的衛星姿態判定（attitude determination）是十 分重要的。近年來，追星儀與陀螺儀等感測器被廣泛應用在衛星姿態判定系統 中，其中追星儀可用來量測衛星的姿態，量測值包括四元數（quaternion）或量測 矢量（measurement vector）；陀螺儀可用來量測衛星的角速度，經過一次積分後 可獲得衛星的姿態角度。兩者在獲得衛星姿態上各有其優缺點，因此常被整合使 用來延長姿態判定的有效時間，以及提高姿態判定的精度。 追星儀是利用本身的CCD照相機來拍攝衛星在軌道上所見到的星象分布 圖，並藉由分析相片上被視為目標物的恆星位置，來判定衛星本身的姿態，但是 其輸出頻率較低。相對地，陀螺儀有較高的輸出頻率，比追星儀更適用於即時（real time）的回授控制。然而在實際應用中，陀螺儀的輸出雜訊及其訊號飄移現象 （drift），會隨著積分的運算過程累積且放大。因此必須設法估測並補償其訊號 偏差值，才能確保所獲得角度資訊的精確度。 大部分先前研究採用追星儀來量測兩組以上的量測矢量，透過擴增型卡爾曼 濾波器（Extended Kalman Filter，EKF）來估測用以描述衛星姿態的四元數[1~4]。 其主要原因為 EKF 可以應用於非線性系統且有效地抑制雜訊，除此之外 EKF 亦 能整合不同輸出頻率的陀螺儀與追星儀，用以提高姿態判定的精度。EKF 是一種最佳化的演算法，它可以從具有高頻雜訊與訊號飄移的量測資訊 來獲得系統的狀態變數，尤其當雜訊來源為高斯雜訊（Guassian noise）時，更可 以得到最小化的均方誤差（mean square error），但是當待估測系統包含非線性動 態時，演算過程需要經由一階偏微後的線性化模型，這將會產生模型誤差（model error），當演算時間越來越長時，此模型誤差容易導致系統不穩定且發散。除此
之外，運算過程需要計算大量的矩陣，必須佔用較大的記憶體空間和花費較多的 運算時間，然而衛星本身所搭載的記憶體空間有限，因此 EKF 運算過程不適用 於衛星姿態判定的規格需求。 先前文獻研究[2, 5~7]採用不同的量測資訊或是發展其他演算法來估測衛星 的姿態資訊，以解決 EKF 運算過程經由一階偏微後所產生的模型誤差。先前文 獻[2]使用線性的量測模型避免 EKF 線性化過程中導致的模型誤差，並在量測雜 訊與系統變數乘積的情況下，提供了一套因應不同雜訊來源所推導的誤差協方差 矩陣（error covariance matrix），將能在雜訊處理的方面有更好的效果，不過也需 要較強的運算能力。先前文獻[5]使用滑模估測器（Sliding Mode Observer）來對抗 系統非線性項與衛星動態模型誤差，用以估測衛星姿態與角速度，但是當四元數 變號時，其回授增益將會產生一個由正到負或由負到正的脈衝（pulse），容易造 成估測系統不穩定。先前文獻[6]採用 Bootstrap Filter 中的條件機率法（conditional probability distribution function）以適用於高度非線性的量測資訊，用以估測衛星在 參考座標上的方位，不僅需要較多樣本數以獲得精確的姿態精度，同時也需要較 強的運算能力。先前文獻[7]發展一套僅使用追星儀所提供的量測矢量來估測衛星 姿態與角速度的演算法，由於衛星動態中通常存在著外部力矩及未知因素等模型 誤差，所以此論文利用了最佳化作法來解決上述問題並提供準確的姿態精度，但 是其運算頻率較低，使得不容易應用於即時控制。 衛星的壽命周期通常較為漫長，才能夠長期地執行相關任務，然而搭載於衛 星上的感測器容易受到內部溫度或非人為因素而造成失效，像是陀螺儀無法量測 角速度資訊時，因此發展部分感測器失效的姿態判定系統是非常重要的。先前文 獻[5~8]在陀螺儀失效的情況下，利用追星儀來進行衛星的姿態判定，其作法透過 衛星動態模型的獲得角速度資訊，經由一次積分即可獲得角度資訊，但是通常需 要解決動態模型中的模型誤差才能得到準確的角速度資訊。但是在部分陀螺儀失 效下，則尚未發現不需透過衛星動態模型來做姿態判定的研究。 本論文發展一套新式估測演算法用以整合追星儀與陀螺儀的輸出資訊來進
行衛星姿態判定。此演算法以 Lyapunov 方法為基礎，不僅能夠估測並補償陀螺 儀的訊號誤差，亦能夠適應於部分陀螺儀失效的狀況。此演算法的主要優點在於 不需要衛星動態模型，不但能夠解決先前文獻所遭遇的模型誤差，還能夠提供較 為簡易的運算過程以節省記憶體空間與加快運算速度，模擬結果指出本論文所提 出的新式估測演算法能夠達成與 EKF 相當的估測精度。後面章節除了會探討擴 增卡爾曼濾波器與新式估測演算法，當陀螺儀具有量測誤差時，兩者的估測精度 比較，亦會探討新式估測演算法在部分陀螺儀失效的情況，還能運用僅剩的追星 儀與陀螺儀來繼續提供精確的衛星姿態判定，以延長衛星的有效壽命周期。 1.2 論文架構 本論文共分為六章，第一章「緒論」，介紹研究動機、文獻回顧與計畫書架 構。第二章「姿態量測序言」，介紹使用的座標、座標轉換以及四元數表示法。 第三章「系統動態模型」，介紹感測器整合系統、擴增型卡爾曼濾波器和新式估 測器。第四章「不同感測器失效情形」，介紹利用追星儀及一軸或兩軸陀螺儀設 計的動態模型。第五章「模擬結果與討論」，記錄目前系統模擬和分析後的結果。 第七章「結論」，報告此計劃目前為止的成果。
第二章：姿態量測序言
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 附體座標系與地心座標系之關係 前述之地心座標系、附體座標系，皆是用以描述物體於一空間中的位置，其 中附體座標系會隨著物體旋轉，用來描述物體位置最為方便，但是欠缺一固定點 做為參考，因此有必要探討附體座標系與地心座標系之間的關係。關於物體的三 種不同的旋轉方式，分別是側滾角（Roll）、俯仰角（Pitch）及航向角（Yaw）。 我們可藉由這三種不同的角度，來求得附體座標與地心座標之間旋轉的關係。 側滾角（φ ） 在航空學中側滾角表示為機翼的上/下的旋轉，也等效於對附體座標的 x 軸 來旋轉。如圖所示： Z Y z y ψ ψ 圖(2.3) 側滾角之示意圖及座標 旋轉後，附體座標上的位置（x y z）可由 ECI 座標上的位置（X Y Z）乘上轉換 矩陣R x
( )
,φ 得到，( )
( )
( )
( )
( )
( )
1 0 0 , , 0 cos 0 sin cos x X y R x Y R x z Z sin φ φ φ φ φ φ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥= ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ − ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.1) 俯仰角（θ） 在航空學中俯仰角表示為機鼻的上/下的旋轉，也等於對附體座標的 y 軸來 旋轉。如下圖所示：X x z θ θ Z 圖(2.4) 俯仰角之示意圖及座標 旋轉後，附體座標上的位置（x y z）可由 ECI 座標上的位置（X Y Z）乘上轉換 矩陣R y
( )
,θ 得到，( )
( )
( )
( )
( )
( )
cos 0 sin , , 0 1 sin 0 cos x X y R y Y R y z Z 0 θ θ θ θ θ θ ⎡ − ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥= ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.2) 航向角（Ψ） 在航空學中航向角表示為機鼻的左/右的旋轉，也等於對附體座標的 z 軸來 旋轉。如下圖所示： Y X x y Ψ Ψ 圖(2.5) 航向角之示意圖及座標 旋轉後，附體座標上的位置（x y z）可由 ECI 座標上的位置（X Y Z）乘上轉換 矩陣R z( )
,ψ 得到，( )
,( )
, cossin( )
( )
cossin( )
( )
00 0 x X y R z Y R z z Z ψ ψ ψ ψ ψ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎢ ⎥= ⎢ ⎥ = − ⎢ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ 0 1 ψ ⎥⎥ (2.3)
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
, , , b ECI C R x R y R z θ ψ θ ψ θ ψ θ φ φ ψ φ ψ θ φ ψ θ φ φ ψ θ φ ψ φ θ ψ ψ φ θ φ φ θ ψ − − + + − = ⎡ ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ (2.4) b ECI x X y C Y z Z ⎡ ⎤ ⎡ ⎤ ⎢ ⎥= ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ b ECI
C
表示為 ECI 轉換至附體座標之轉換矩陣，為了確保尤拉角之唯一性，故要求 2 2 π π π ψ π θ π φ π − ≤ ≤ − ≤ ≤ − ≤ ≤ 因為 b 為一個單位正交矩陣，故具有下列之性質： ECIC
(
b) (
T b)
1 ECI ECI C = C − det( )
b ECI C =I 由上述方式，我們亦可獲得附體座標的角速度ωb 與尤拉角的角速度之間的 關係：( )
( ) ( )
( )
( )
( ) ( )
( )
( ) ( )
0 0 0 , , , 0 0 0 1 sin 00 cos cos sin
sin 0 cos cos
b x b y b z R x R x R y ω φ ω φ θ φ θ ψ ω φ θ θ θ φ θ φ θ φ ψ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ =⎢ ⎥+ ⎢ ⎥+ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎡ ⎤ ⎡ − ⎤⎢ ⎥ ⎢ ⎥ =⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢− ⎥⎢ ⎥ ⎣ ⎦ ⎣ ⎦ (2.5)
( ) ( )
( ) ( )
( )
( )
( ) ( )
( ) ( )
1 sin tan cos tan
0 cos sin
0 sin sec cos sec
b x b y b z ω φ φ θ φ θ θ φ φ ψ φ θ φ θ ω ω ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ =⎢ − ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ (2.6) 觀察上式可發現當俯仰角（Pitch）為+/- 90∘時，也就是載具垂直向上或垂 直向下飛行，其轉換矩陣會發生奇異性（singularity）。故利用此方程式來計算姿 態的轉換矩陣時，俯仰角會有所限制。 四元數法（Quaternion） 使用四元數表示法其優點為計算姿態轉換矩陣時不會有奇異性問題。在介紹 四元數法之前先介紹尤拉定理： 剛體繞固定點旋轉的運動，可表示為繞某一根通過該固定點之軸線
( )
λ 旋轉 一個角度（μ）的運動。如圖(2.6)所示： X Y Z x z y μ λ 1 λ = ⋅λ X 2 λ = ⋅λ Y 3 λ = ⋅λ Z 圖(2.6) 尤拉定理 若以座標系（XYZ）上之單位向量λ（=[
λ λ1 2 λ3]
T）為轉軸，其中λ1~λ3 為轉軸λ投影至XYZ座標的分量，將座標系（XYZ）對其旋轉一角度μ則可得座 標（xyz），μ為繞λ軸旋轉的角度，因此轉換矩陣C以λ及μ表示為：(
,)
cos 3(
1 cos)
sinb T ECI C λ μ = μ⋅ + −I μ λλ − μ λ⋅ × (2.7) 其中 3 2 3 1 2 1 0 0 0 λ λ λ λ λ λ λ ×=⎡⎢ − − ⎤⎥ ⎢ ⎥ ⎢− ⎥ ⎣ ⎦ 我們可令四元數向量為 0 0 1 2 Q=q + =q q +iq + jq +kq3 (2.8)
(
)
(
)
(
)
(
)
0 1 1 2 2 3 3 cos / 2 sin / 2 sin / 2 sin / 2 q q q q μ λ μ λ μ λ μ = = = = (2.9) 四元數與尤拉角法皆是描述物體的姿態，因此不具有四個自由度，必須滿足 單範的性質限制（normalization constraint） 2 2 2 2 (2.10) 0 1 2 3 1 q +q +q +q = 由四元數所得的轉換矩陣如下所示：(
)
(
)
(
)
(
(
)
(
)
2 2 2 2 0 1 2 3 1 2 0 3 1 3 0 2 2 2 2 2 1 2 0 3 0 1 2 3 2 3 0 1 2 2 2 2 1 3 0 2 2 3 0 1 0 1 2 3 2 2 2 2 2 b ECI q q q q q q q q q q q q C q q q q q q q q q q q q q q q q q q q q q q q q ⎡ + − − + − ⎤ ⎢ ⎥ =⎢ − − + − + ⎥ ⎢ + − − − ⎥ ⎣ ⎦)
2 + (2.11) 由(2.4)、(2.11)式，可獲得四元數與尤拉角之間的轉換關係：(
)
(
)
(
)
(
)
2 3 0 1 1 2 2 2 2 0 1 2 3 1 1 3 0 2 1 2 0 3 1 2 2 2 2 0 1 2 3 2 tan sin 2 2 tan q q q q q q q q q q q q q q q q q q q q φ θ ψ − − − ⎛ + ⎞ = ⎜ ⎟ − − + ⎝ ⎠ = − + ⎛ + ⎞ = ⎜ ⎟ + − − ⎝ ⎠ (2.12) 若姿態連續變化時，則四元數與附體座標角速度之關係如下： 1 2 3 0 3 2 4 1 3 0 1 2 1 0 1 2 || || b x b y b z q q q q q q q q q q q q q q ω ω ω × − − − ⎡ ⎤⎡ ⎤ ⎢ − ⎥⎢ ⎥ ⎢ = ⎢ ⎥ ⎢ − ⎢ ⎥ ⎢− ⎥ ⎣ ⎦ ⎣ ⎦ ⎥ ⎥ (2.13)第三章：系統動態模型
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 ˆ( 1) ˆ( 1) q k d k − − + + measumentq
Bodyω
Spacecraft Attitude Estimator
ˆ( 1) ˆ( 1) q k d k + + ˆ( 1) q k+ − 16 Hz 4 Hz Gyros STR Sensor Data Processing Prediction Lyapunov Gains Attitude Residuals Attitude Gyro drift Correction ˆ( 1) ˆ( 1) q k d k − − + + measument
q
Bodyω
Spacecraft Attitude Estimator
ˆ( 1) ˆ( 1) q k d k + + ˆ( 1) q k+ − 16 Hz 4 Hz 圖(3.1) 姿態估測系統的流程圖
陀螺儀角速度量測： 在本報告所探討的衛星系統中，衛星於附體座標上的角速度是由三組陀螺儀 量測值ωGyro m, （= ⎣⎡ωGx m, ωGy m, ωGz m, ⎤⎦ ）獲得。其中三組陀螺儀配置在附體座標 的三個軸向方向上，如下圖所示： (a) X X Y Y Z Z Gz Gy Gx (a) X X Y Y Z Z Gz Gy Gx 圖(3.2) 衛星上陀螺儀配置概念圖 其中，ωGyro m, 又可以由下式所描述：
(
)
Gyro,m I3 3 misalignment Gyro_ideal d w
ω = × ×ε ω + + Gyro_ideal ω 為陀螺儀於理想狀況下訊號輸出值， 為陀螺儀量測訊號飄移量， 為 陀螺儀量測雜訊， d w misalignment ε 為溫度影響或是配置問題所造成之量測方向誤差，其 中當誤差角度為極小值時，其值為一單位矩陣。 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
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）方式給入，其中本論 文在模擬的時候是採用獨立給入的方式。 （1）獨立給入： 具雜訊的四元數量測值由理想狀況下四元數的角度量測值 加上一高斯 分布雜訊 組合而成。 ideal STR q 4 1 v× _ 4 1
with noise ideal
STR STR q =q +v× （2）四元數乘積給入： 我們令Δ Δ Δ 為描述衛星姿態的三個尤拉角的誤差值，組成四元數量測φ θ ψ, , 雜訊qnoise。 2 2 2 T q ⎡Δφ Δθ Δψ ⎤ Δ = ⎢⎣ ⎥⎦ 0 1 2 3 1 ' n n noise n n q q q q q q q q ⎡ ⎤ ⎢ ⎥ ⎡ − Δ Δ ⎤ ⎢ ⎥ =⎢ ⎥ ⎢ ⎥= Δ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ 再利用四元數乘積（quaternion multiplication）方式加入理想狀況下四元數的 角度量測值 ，進而得到具雜訊的四元數量測值 。以四元數乘積方式 給入雜訊的特點在於即使考慮誤差，其四元數的平方和仍然等於 1。其關係式如 下表示： ideal STR q with noise_ STR q
_ 0 0 1 2 3 1 1 0 3 2 2 2 3 0 1 3 3 2 1 0
with noise ideal
STR STR noise ideal n n n n n n n n n n n n n n n n STR q q q q q q q q q q q q q q q q q q q q q q q = ⊗ ⎡ − − − ⎤⎡ ⎤ ⎢ − ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ =⎢ ⎥ ⎢ ⎥ − ⎢ ⎥ ⎢ ⎥ − ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ 觀察器架構： 假設陀螺儀訊號飄移量 為常數，則一理想狀況下，附體座標上的角速度 與四元數變化的動態模型可如下表示， d ( ) 0 ideal body q A q d ω ⎡ ⎤ ⎡ ⎤ = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ (3.1) 由上式，我們可建立一狀態估測系統，如下式所示：
(
)
[
]
_ , 1 2 3 0 3 2 3 0 1 2 1 0 0 1 2 3 ˆ ˆ ( )(ˆ ) ˆ ˆ 0 ˆ ˆ ˆ ˆ ˆ ˆ 1 ˆ ( ) ˆ ˆ ˆ ˆ 2 || || ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ with noise Gyro m STR T q A q d L q y d q q q q q q A q q q q q q q q y q q q q ω ⎡ ⎤ ⎡ − ⎤ ⎢ ⎥ =⎢ ⎥+ − ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ − − − ⎡ ⎤ ⎢ − ⎥ ⎢ ⎥ = ⎢ − ⎥ ⎢− ⎥ ⎣ ⎦ = (3.2) 其中， 為估測的四元數； 為估測的陀螺儀訊號飄移量； 為觀察器增益。而 在實際的情況下，訊號飄移量d不一定為常數（ ˆ q dˆ L 0 d≠ ），故估測系統與實際系統 將會有誤差產生，此一誤差將於 3.3.1 節中討論。3.2 系統的觀察性分析 為了確定所設計的觀察器能成功的估測衛星姿態及陀螺儀的飄移量，我們 檢驗系統的可觀察性（observability）。由於上述系統為一非線性系統，必須使用 非線性系統的觀察性矩陣來進行判斷。系統觀察性矩陣的公式推導結果如下：
[
4 4 4 3]
, ( ) ( ) 0 ( ) ( )( Gyro m ) h x q Hx h x H I x h x q A q ω d × × = = ∂ = = ∂ = = − 4 4 4 3 1 2 3 , 0 3 2 3 0 1 2 1 0 1 2 3 0 3 2 3 0 1 2 1 0 0 ( ) ( )( ) ( ) 3 ( ) 7 Gyro m I h x q q q x A q d O q h x q q q q x q q q q q q q q q rank q q q q q q rank O ω × × ⎡ ⎤ ⎢ ⎥ ∂ ⎡ ⎤ q q ⎡ ⎤ ⎢ ⎥ ⎢ ∂ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ∂ − = = ⎢− − ⎥ ⎢ ⎥ ∂ ⎢ ⎥ ∂ ⎢− − ⎥ ⎢ ⎥ ⎢ ∂ ⎥ ⎣ ⎦ ⎢ ⎢ ⎥⎥ − − ⎣ ⎦ ⎣ ⎦ ⎛⎡ ⎤⎞ ⎜⎢− − ⎥⎟ ⎜⎢ ⎥ =⎟ ⎜⎢− − ⎥⎟ ⎜⎢ ⎥⎟ ⎜⎣ − − ⎦⎟ ⎝ ⎠ ∴ = ∵ 其中x為系統的 7 個狀態變數，包括：4 個四元數及 3 個陀螺儀訊號飄移值； 為追星儀的四元數輸出值。由於該觀察性矩陣為滿秩（full rank），所以此系 統的 7 個狀態變數皆為可觀察，即我們可以藉由合適的觀察器設計，正確估測上 述 7 個狀態變數。 ( ) h x 3.3 使用擴增型卡爾曼濾波器進行狀態估測由於擴增型卡爾曼濾波器（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 tk x(k) Input at tk u(k) 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 ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = + Evaluation of Jacobians ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = +
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 ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Filter gain ( ) ( ) ( ) ( )1 1 1 | 1 1 − + + + = + k S k H k k P k W T Filter gain ( ) ( ) ( ) ( )1 1 1 | 1 1 − + + + = + k S k H k k P k W T
Updated state covariance
( ) ( ) ( ) ( ) ( )T k W k S k W k k P k k P 1 1 1 | 1 1 | 1 + + + − + = + +
Updated state covariance
( ) ( ) ( ) ( ) ( )T k W k S k W k k P k k P 1 1 1 | 1 1 | 1 + + + − + = + + 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
( ) ( 1| ) ( 1)Res( 1) ˆ 1 | 1 ˆ + + + + = + + k k W k k x k k x Transition to tk+1 (k ) f(kx( ) ( )k uk) ( )vk x +1= , , + Measurement at tk+1 ( ) ( ) ( 1, 1) ( 1) 1 + + + + = + k w k x k h k z v(k) w(k+1) 圖(3.3) EKF 流程圖
3.3.2 儲存記憶褪去式擴增型卡爾曼估測器（Adaptive Fading Extended Kalman Filter，AFEKF） 由於先前假設陀螺儀的訊號飄移量為一常數，而實際應用中並非如此。此假 設所造成的誤差很可能會使得 EKF 在估測系統狀態時失效。此一現象可被理解 為狀態觀察器內所使用的物理系統的數學模型無法完全描述真實系統的動態，而 EKF 的強健性（robustness）不足，使得估測失效。為解決此一問題，我們使用 AFEKF 來解決。此方法的特別之處在於增加了一個忽略係數λ，此係數調整 EKF 在進行計算時的累積記憶量。藉此，避免 EKF 計算所的估測增益值不斷的降低， 進而使估測系統具強健性。 在計算預估狀態協方差（covariance）處，增加忽略係數λ：
(
k k) (
k) ( ) ( ) ( )
F k P k k F k Q( )
k P +1| =λ +1 | T + 更新狀態協方差處，更改為：P(
k+1|k+1)
=(
I−W(
k+1) (
H k+1)
) (
P k+1|k)
其中忽略係數λ為：( )
{
( )
( )
}
(
) ( ) ( ) ( ) (
)
(
) (
) (
)
T(
) ( ) (
)
T T T k H k Q k H k H k k P k H N k H k F k k P k F k H M M N k 1 1 1 1 | 1 1 | 1 trace trace , 1 max + + − + − + = + + = = λ 因此 AFEKF 的流程圖如下：State at tk x(k) Input at tk u(k) 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 ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = + Evaluation of Jacobians ( ) ( ) ( )kk x x x k f k F | ˆ = ∂ ∂ = ( ) ( ) (k k) x x x k h k H | 1 ˆ 1 1 + = ∂ + ∂ = +
State prediction covariance
( ) (k ) ( ) (FkPk k) ( )Fk Q( )k k k P T+ + = + | 1 | 1 λ Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Residual covariance ( ) ( ) ( ) ( ) ( )T k H k k P k H k R k S 1 | 1 1 1 1 + + + + + = + Filter gain ( ) ( ) ( ) ( )1 1 1 | 1 1 − + + + = + k S k H k k P k W T Filter gain ( ) ( ) ( ) ( )1 1 1 | 1 1 − + + + = + k S k H k k P k W T
Updated state covariance
( ) ( ) ( ) (I Wk Hk ) (Pk k k k P | 1 1 1 1 | 1 + + + − = + ) + 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
( ) ( 1| ) ( 1)Res( 1) ˆ 1 | 1 ˆ + + + + = + + k k W k k x k k x Transition to tk+1 (k ) f(kx( ) ( )k uk) ( )vk x +1= , , + Measurement at tk+1 ( ) ( ) ( 1, 1) ( 1) 1 + + + + = + k w k x k h k z v(k) w(k+1) ( ) { ( ) ( )} ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )T T T T k H k Q k H k H k k P k H N k H k F k k P k F k H M M N k 1 1 1 1 | 1 1 | 1 trace trace , 1 max + + − + − + = + + = = λ 圖(3.4) AFEKF 流程圖
3.4 設計新式估測器進行姿態估測 傳統的 AFEKF 需要較大的記憶體運算空間，所以我們使用自行推導的估測 器以簡化運算過程並加快運算時間。以下是我們的估測器推導過程： 根據之前設計的觀察器架構(3.2)，觀察器增益 可分成上下兩部份，分別 對四個四元數與三個訊號飄移量。 L 7 4 4 4 3 4 1 2 T L×
L
L
× × ⎡ ⎤ = ⎣ ⎦ (3.3) 、 定義為實際系統狀態（state）與估測狀態的誤差， q e ed (3.4) ˆ ˆ q d e q q e d d = − = − 利用(3.3)、(3.4)即可以求得誤差變數的一次微分項。 (3.5) , 1 2 ˆ ( ) ( ) ( ) q q Gyro m q d q d q e A e A e d A q e L e e L e ω ⎡ − − − ⎤ ⎡ ⎤ = ⎢ ⎥ ⎢ ⎥ − ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ 其中 , 1 2 3 , , , , 0 3 2 , , , , 3 0 1 , , , , 2 1 0 , , , 3 ( ) 0 0 1 1 0 2 2 0 q Gyro m q q q Gx m Gy m Gz m q Gx m q q q Gx m Gz m Gy m q Gy m q q q Gy m Gz m Gx m q Gz m q q q Gz m Gy m Gx m q A e e e e e e e e e e e e e e e e e 0 1 2 ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω ω − − − − − − ⎡ ⎤ ⎡ ⎡ ⎤ ⎢ − ⎥ ⎢ − ⎢ ⎥ ⎢ ⎥ ⎢ = ⎢ ⎥= ⎢ − ⎥ ⎢ − ⎢ ⎥ ⎢− ⎥⎣ ⎦ ⎢ − ⎢ ⎥ ⎢ ⎣ ⎦ ⎣ , ( Gyro m) q B ω e ⎤ ⎥ ⎥ ⎥ ⎢ ⎥ ⎢ ⎥⎦ = ⎤ ⎡ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎥⎦ ⎣ (3.6) 選擇一 Lyapunov 方程式，令K、ρ為正定矩陣使得V >0， 0 1 0 0 2 T K V e e for e ρ ⎡ ⎤ = ⎢ ⎥ > ⎣ ⎦ ≠0 ⎤⎦ (3.7) 其中e= ⎣⎡eq ed T，然後利用(3.5)、(3.7)求得 Lyapunov 方程式之ㄧ次微分項。(3.8) , 1 2 , 1 2 ˆ ( ) ( ) ( ) 0 0 ˆ ( ( ) ( )) ( ) 0 q Gyro m q d q T q T Gyro m A e A e d A q e L e K V e L e K B L B d KA q e e L ω ρ ω ρ ⎡ − − − ⎤ ⎡ ⎤ = ⎢ ⎥⎢ ⎥ − ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ⎡ − − − ⎤ = ⎢ ⎥ − ⎣ ⎦ 由於 ( )B i 為反對稱矩陣，故e BT ( )i e=0，接著如果L1、L2滿足下列條件： 1
L is a positive definite matrix
2 ( )ˆ T L = −A q (3.9) 再選擇 K = = 為正定單位矩陣，則ρ I 1 0 T q q V = −e L e ≤ 為一半負定矩陣（negative semi-definite）。藉由 Lyapuonv first stability theorem，可知 ，且 收歛到某一
常數（不一定為零）。故為了驗證 、 是否會同時收斂於零，我們將 代 入(3.5)討論， 0 q e → ed q e ed eq →0
(
)
(
)
ˆ ( ) 0 0 ˆ 0 ( ) 0 q d q d e A q e e e A q = − → → ⇒ → ≠ ∵ 由此結果可知，在一段時間後， 和 將同時收斂於零，系統為漸進穩定 （asymptotically stable）。 q e ed第四章：不同感應器失效情形
4.1 一軸陀螺儀資訊失效 當追星儀輸出正常，一軸陀螺儀失效時，姿態估測系統將不再對失效軸進 行飄移訊號值估測，改而直接估測失效軸之角速度及正常軸的訊號飄移量。假設 陀螺儀 D 失效時，則可建構一估測系統如下：[
]
, , 1 _ 2 0 1 2 3 ˆ ˆ ( )ˆ ˆ ˆ ˆ ˆ ( ) ˆ 0 0 ˆ 0 ˆ ˆ ˆ ˆ ˆ Gx m x Gy m y Gz x with noise STR y Gz T d q A q d L d q y L d y q q q q ω ω ω ω ⎡ ⎡ − ⎤⎤ ⎢ ⎢ ⎥⎥ ⎡ ⎤ ⎢ ⎢ − ⎥⎥ ⎢ ⎥ ⎢ ⎢ ⎥⎥ ⎢ ⎥ ⎢ ⎢ ⎥⎥ ⎡ ⎤ ⎣ ⎦ = + ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ = − (4.1) 如前所述，我們將藉由相關的觀察性矩陣，來驗證姿態估測的可行性。系統 的觀察性矩陣，如下式推導：[
]
0 1 2 3 4 4 4 3 , , ( ) ( ) 0 ( ) ( ) T x y Gz T Gx m x Gy m y Gz x q q q q d d h x q Hx h x H I x h x q A q d d ω ω ω ω × × ⎡ ⎤ = ⎣ ⎦ = = ∂ = = ∂ ⎡ ⎤ = = ⎣ − − ⎦4 4 4 3 1 2 3 , , 0 3 2 3 0 1 2 1 0 1 2 3 0 3 2 3 0 1 2 1 0 ( ) ( ) 0 ˆ ( ) 3 ( ) T Gx m x Gy m y Gz h x x O h x x I q q q A q d d q q q q q q q q q q q q q q q q rank q q q q q q rank O ω ω ω × × ∂ ⎡ ⎤ ⎢ ∂ ⎥ ⎢ ⎥ = ∂ ⎢ ⎥ ⎢ ∂ ⎥ ⎣ ⎦ ⎡ ⎤ ⎢ ⎡ − ⎤⎥ ⎢ ⎢ ⎥⎥ ⎡ ⎤ ∂ − − ⎢ ⎥ = ⎣ ⎦ ⎢− ⎥ ⎢ ⎢ ⎥⎥ − − − ∂ ⎢ ⎢ ⎥⎥ ⎢ ⎣ − ⎦⎥ ⎣ ⎦ ⎛⎡ − ⎤⎞ ⎜⎢− ⎥⎟ ⎜⎢ ⎥ =⎟ ⎜⎢− − − ⎥⎟ ⎜⎢ ⎥⎟ ⎜⎣ − ⎦⎟ ⎝ ⎠ ∴ = ∵ 7 因為此觀察性矩陣為滿秩，所以我們可判斷此動態系統為可觀察。 4.1.1 架構一軸陀螺儀失效時之新式估測器 利用 3.4 推導流程即可求得 Lyapunov 方程式的一次微分項 1 2 1 0 0 ˆ ( ) ( ) 0 1 0 0 0 0 x b y m T d K B d B L K A q V e e L ω ρ ⎡ ⎛ ⎛⎡ ⎤⎞ ⎞ ⎛ ⎡ ⎤⎞⎤ ⎢ ⎜− ⎜⎢ ⎥⎟+ − ⎟ ⎜− ⎢ ⎥⎟⎥ ⎢ ⎜ ⎜⎢ ⎥⎟ ⎟ ⎜ ⎢ ⎥⎟⎥ = ⎢ ⎜ ⎜⎢ ⎥⎟ ⎟ ⎜ ⎟⎥ ⎢ − ⎥ ⎣ ⎦ ⎝ ⎠ ⎣ ⎦ ⎝ ⎠ ⎢ ⎝ ⎠ ⎥ ⎢ − ⎥ ⎣ ⎦ 0 1 (4.2) 由於 ( )B i 為反對稱矩陣，e BT ( )i e=0，接著如果L1、L2滿足下列條件： 1
L is a a positive definite matrix
2 1 0 0 ˆ ( ) 0 1 0 0 0 1 T L A q ⎛ ⎡ ⎤⎞ ⎜ ⎢ = −⎜ ⎢ ⎜ ⎢⎣ − ⎥⎦⎟ ⎝ ⎠ ⎟ ⎥ ⎟ ⎥ I (4.3) 再選擇 K = = 為正定單位矩陣，並使用相同推導流程即可驗證系統為漸ρ 進穩定。
4.2 二軸陀螺儀資訊失效 在此案例中，類似於一軸陀螺儀失效的例子，我們直接估測失效軸之角速 度及正常軸的訊號飄移量，並利用此概念建立下列的姿態估測系統。
[
]
, 1 _ 2 0 1 2 3 ˆ ˆ ( )ˆ ˆ ˆ ˆ ˆ ( ) 0 ˆ 0 ˆ 0 ˆ ˆ ˆ ˆ ˆ Gx m x Gy Gz with noise x STR Gy Gz T d q A q L d q y L y q q q q ω ω ω ω ω ⎡ ⎡ − ⎤⎤ ⎢ ⎢ ⎥⎥ ⎡ ⎤ ⎢ ⎢ ⎥⎥ ⎢ ⎥ ⎢ ⎢ ⎥⎥ ⎢ ⎥ ⎢ ⎣ ⎦⎥ ⎡ ⎤ = + − ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎢ ⎥ ⎣ ⎦ = (4.4) 系統的可觀察性可由如下方式獲得。並藉由相關的觀察性矩陣，來驗證姿態 估測的可行性。[
]
0 1 2 3 4 4 4 3 , ˆ ( ) ( ) 0 ( ) ( ) T x Gy Gz T Gx m x Gy Gz x q q q q d h x q Hx h x H I x h x q A q d ω ω ω ω ω × × ⎡ ⎤ = ⎣ ⎦ = = ∂ = = ∂ ⎡ ⎤ = = ⎣ − ⎦ 4 4 4 3 1 2 3 , 0 3 2 3 0 1 2 1 0 1 2 3 0 3 2 3 0 1 2 1 0 0 ( ) ˆ ( ) ( ) 3 ( ) 7 T Gx m x Gy Gz I h x q q q x A q d O q h x q q q q x q q q q q q q q q rank q q q q q q rank O ω ω ω × × ⎡ ⎤ ⎢ ⎥ ∂ ⎡ ⎤ − − q q ⎡ ⎤ ⎢ ⎥ ⎢ ∂ ⎥ ⎡ ⎤ ⎢ ⎥ ∂ − ⎢ ⎥ ⎢ ⎥ = = ⎣ ⎦ ⎢− − ⎥ ⎢ ⎥ ∂ ⎢ ⎥ ∂ ⎢− − ⎥ ⎢ ⎥ ⎢ ∂ ⎥ ⎣ ⎦ ⎢ ⎢ ⎥⎥ ⎣ ⎦ ⎣ ⎦ ⎛⎡ − − ⎤⎞ ⎜⎢− − ⎥⎟ ⎜⎢ ⎥ =⎟ ⎜⎢− − ⎥⎟ ⎜⎢ ⎥⎟ ⎜⎣ ⎦⎟ ⎝ ⎠ ∴ = ∵ 因為該觀察性矩陣為滿秩( full rank )，所以上述 7 個系統狀態皆為可觀察。4.2.1 架構二軸陀螺儀失效之新式估測器 利用 3.4 推導流程即可求得 Lyapunov 方程式的一次微分項 1 2 1 0 0 ˆ 0 ( ) ( ) 0 1 0 0 0 0 x b m T d K B B L K A q V e e L ω ρ ⎡ ⎛ ⎛⎡ ⎤⎞ ⎞ ⎛ ⎡ ⎤⎞⎤ ⎢ ⎜− ⎜⎢ ⎥⎟+ − ⎟ ⎜− ⎢ − ⎥⎟⎥ ⎢ ⎜ ⎜⎢ ⎥⎟ ⎟ ⎜ ⎢ ⎟⎥ = ⎢ ⎜ ⎜⎢ ⎥⎟ ⎟ ⎜ ⎟⎥ ⎢ − ⎣ ⎦ ⎝ ⎠ ⎣ ⎦ ⎝ ⎠ ⎢ ⎝ ⎠ ⎥ ⎢ − ⎥ ⎣ ⎦ 0 1 ⎥ ⎥ (4.5) 由於 ( )B i 為對稱矩陣，e BT ( )i e=0，接著如果L1、L2滿足下列條件： L1 is a a positive definite matrix
2 1 0 0 ˆ ( ) 0 1 0 0 0 1 T L A q ⎛ ⎡ ⎤⎞ ⎜ ⎢ = −⎜ ⎢ − ⎜ ⎢⎣ − ⎥⎦⎟ ⎝ ⎠ ⎟ ⎥ ⎟ ⎥ I (4.6) 再選擇 K = = 為正定單位矩陣，並使用相同推導流程即可驗證系統為漸ρ 進穩定。
第五章：模擬結果與討論
模擬參數設定 目前正在運作的衛星姿態判斷時序表及硬體架構如下：陀螺儀的輸出頻率 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圖（5.1）姿態四元數真實值與估測值（幾乎重疊）
圖（5.3）訊號飄移量誤差收斂情形 由圖（5.2）、圖（5.3）可以發現系統在估測訊號飄移量的時候，在模擬時 間達到約 500 秒時產生發散的情況。我們猜測因為實際系統為連續時間動態系 統，而估測器為一離散系統，因此必須對連續時間系統離散化後，才能套用擴增 型卡爾曼濾波器的運算步驟。對連續時間系統進行離散化的過程，會引入些許的 誤差，此誤差的大小取決於取樣時間的長短、離散化的方式。先前的模擬過程中， 其取樣頻率為 16Hz，離散化採用最簡單的 Euler Explicit 的方式，其取樣頻率略 低，且離散方式僅以一階方式近似，可能造成連續時間系統與離散系統間的誤差 超過預期。為探究此一問題，我們直接估測一離散系統的狀態值，去除取樣時間、 離散化的誤差，直接驗證估測系統的設計是否有誤。
圖（5.4）三軸訊號飄移真實值與估測值
由圖（5.4）與圖（5.5）的模擬結果可以看出，即使在多模擬了五倍的時間 下，估測值依然穩定收歛到正確值，並且沒有發散的現象。因此我們可以推論之 前的案例發散現象來自於系統離散化過程的誤差。 此離散化的誤差可被視為一真實系統與估測系統對系統動態描述的誤差。此 一誤差原本可藉由較高的回授增益值（feedback gain）加以補償，使得估測系統 不至於發散。然而由於擴增型卡爾曼濾波器的特性，其回授增益值會逐漸的降低 以換取更高的估測精度，因此就喪失的估測系統的強健性（robustness）。此一現 象說明了估測系統在模擬開始時收斂，但是卻在模擬的後半段發散。 所以我們使用更強健性的儲存記憶褪去式擴增型卡曼濾波器來進行模擬。 圖（5.6）姿態四元數真實值與估測值（幾乎重疊）
圖（5.7）三軸訊號飄移真實值與估測值
圖（5.9）尤拉角的真實值與估測值
圖（5.6）（5.9）是將四元數資訊轉換成尤拉角，估測值與真實值的比較和 誤差收斂情形，因為尤拉角相較於四元數在平常看時較有物理意義。圖中的突起 處，是因為四元數轉換成尤拉角時的三角函數關係，
(
)
(
)
(
)
(
)
2 3 0 1 1 2 2 2 2 0 1 2 3 1 1 3 0 2 1 2 0 3 1 2 2 2 2 0 1 2 3 2 tan sin 2 2 tan q q q q q q q q q q q q q q q q q q q q φ θ ψ − − − ⎛ + ⎞ = ⎜ ⎟ − − + ⎝ ⎠ = − + ⎛ + ⎞ = ⎜ ⎟ + − − ⎝ ⎠ φ和ψ 是透過 1 tan− 轉換的，所以會有一些突起，θ 是經過sin−1轉換的，就不會有 這樣的問題，不過實際上的姿態估測，從四元數觀察，是確實有估測到的。 由模擬結果可以知道，此演算法可以處理因模型誤差所產生的影響，並成 功的估測到姿態四元數與訊號飄移量，在模擬時間到達 3000 秒的情況下，也不 會有發散的現象。5.2 使用新式估測器估測四元數與訊號飄移值 【Case1】 訊號飄移值為常數
假設我們想要的估測的訊號飄移值為常數，並使用與 5.1 節相同的參數設 定條件進行模擬。
圖（5.12）使用新式估測器之三軸訊號飄移真實值與估測值
圖（5.14）尤拉角的真實值與估測值
【Caes2】 訊號飄移量隨時間變化 假設待估測的訊號飄移值隨時間變化，並使用與 5.1 節相同的參數設定條 件進行模擬。 d（陀螺儀角速度訊號飄移量）： 4 4 deg sec 2 8.33 10 8.33 10 cos( ) 2400 d = × − + × − π t 圖（5.16）使用新式估測器之四元數真實值與估測值（幾乎重疊）
圖（5.17）使用新式估測器之三軸訊號飄移真實值與估測值
圖（5.19）尤拉角的真實值與估測值
【Case3】 訊號飄移值為步階情況
假設待估測的訊號飄移值為步階訊號，並使用與 5.1 節相同的參數設定條 件進行模擬。
d（陀螺儀角速度訊號飄移量）：
8.3333e-004 deg/sec → - 8.3333e-004 deg/sec → 8.3333e-004 deg/sec
圖（5.22）使用新式估測器之三軸訊號飄移真實值與估測值
圖（5.24）尤拉角的真實值與估測值
【Case4】 新式估測器系統強健性測試
為了新式估測器的系統強健性，假設陀螺儀量測角速度為隨時間變化組合 而成，如圖（5.26）所示，其中訊號飄移量使用與 5.1 節相同的參數設定條件進 行模擬。
圖（5.27）使用新式估測器之四元數真實值與估測值（幾乎重疊）
圖（5.29）訊號飄移量誤差收斂情形
5.3 追星儀正常運作，陀螺儀失效情況 【Case5】 一軸陀螺儀失效 如前所述：在此案例中，姿態估測系統將估測失效軸之角速度及正常軸的 訊號飄移量，其中陀螺儀角速度為隨時間變化組合而成，訊號飄移量使用與 5.1 節相同的參數設定條件進行模擬。 圖（5.32）使用新式估測器之四元數真實值與估測值（幾乎重疊）
圖（5.33）使用新式估測器之正常軸訊號飄移及失效軸角速度的真實值與估測值
圖（5.35）尤拉角的真實值與估測值
【Case6】 二軸陀螺儀失效
在此案例中，直接估測兩軸失效軸之角速度及一軸正常軸的訊號飄移量。 其中陀螺儀角速度為隨時間變化組合而成，訊號飄移量使用與 5.1 節相同的參數 設定條件進行模擬。
圖（5.38）使用新式估測器之正常軸訊號飄移及失效軸角速度的真實值與估測值
圖（5.40）尤拉角的真實值與估測值
5.4 新式估測器模擬結果討論 本論文估測效果的判定為計算真實值與估測值的誤差標準差（Error Standard deviation），公式如下表示： 1 2 2 1 1 ˆ ( ) { ( ( ) ( )) } k error i s k x i x i k = =
∑
− 穩態時之估測誤差標準差 收斂時間 φ θ ψ dx dy dz TsCase1 5.25e-4 3.79e-4 4.99e-4 7.76e-5 7.11e-5 7.03e-5 35 Case2 5.64e-4 4.30e-4 5.52e-4 7.93e-5 7.77e-5 8.48e-5 35 Case3 6.15e-4 6.57e-4 6.66e-4 7.81e-5 7.20e-5 7.53e-5 35 Case4 4.58e-4 4.24e-4 4.67e-4 8.08e-5 8.55e-5 8.21e-5 35 unit deg deg deg deg/ s deg/ s deg/ s s
表（5.1）穩態時之估測誤差標準差
穩態時之估測誤差標準差 收斂時間
φ θ ψ dx dy dz Ts
AFEKF 3.02e-4 2.51e-4 2.18e-4 1.75e-6 1.71e-6 1.97e-6 45 Novel
Estimator
5.25e-4 3.79e-4 4.99e-4 7.76e-5 7.11e-5 7.03e-5 35
unit deg deg deg deg/ s deg/ s deg/ s s 表（5.2）與傳統估測器比較估測精度
Case5 穩態時之估測誤差標準差 收斂時間
φ θ ψ dx dy ω z Ts
L1=1 3.95e-4 4.29e-4 4.34e-4 6.79e-5 7.02e-5 8.39e-5 70 L1=3 6.79e-4 5.47e-4 5.71e-4 4.87e-5 4.12e-5 4.42e-5 100
unit deg deg deg deg/ s deg/ s deg/ s s 表（5.3）一軸失效穩態時之估測誤差標準差
Case6 穩態時之估測誤差標準差 收斂時間
φ θ ψ dx ωy ω z Ts
L1=1 4.84e-4 4.65e-4 5.22e-4 8.24e-5 8.15e-5 8.03e-5 75 L1=3 5.56e-4 5.59e-4 6.26e-4 4.59e-5 4.56e-5 4.54e-5 115
unit deg deg deg deg/ s deg/ s deg/ s s 表（5.4）二軸失效穩態時之估測誤差標準差 由表（5.1）中 Case1~3 可以看出，在陀螺儀量測角速度訊號為常數下，新 式估測器皆可以成功的估測不同形式的訊號飄移值與姿態四元數。在 Case4 中使 用隨時間變化的角速度量測值驗證系統的強健性，結果顯示新式估測器可成功估 測所需的變數。 表（5.2）是我們將 5.1 的案例（採用 AFEKF）與 Case1 的案例（採用新式 估測器）比較的模擬結果，由表可以看出，我們自行設計的新式估測器與 AFEKF 相比較的情況下，三軸姿態精度差異不大，陀螺儀訊號飄移量則是 AFEKF 較佳， 至於收歛速度則是新式估測器較為快速，但是考量到 AFEKF 較大的記憶體運算 空間與時間，我們設計的新式估測器則是能利用較小的記憶體空間與較快的運算 速度達到與其相當的效果。
由表（5.3）與表（5.4）可以看出，在一軸或二軸陀螺儀失效情況下，新式 估測器可以成功的估測陀螺儀正常軸的訊號飄移值和失效軸的角速度，只是會犧 牲姿態角度的準確度。並由模擬結果顯示，在不同的觀察器增益值選定下，將會 影響系統的收斂速度與跳動幅度，且二者為反比關係，所以如何在之間做取捨， 將於日後所需的性能規格做調整。
第六章：結論
本論文發展一套新式估測演算法用以整合追星儀與陀螺儀的輸出資訊來進 行衛星姿態判定。此演算法以 Lyapunov 方法為基礎，不僅能夠估測並補償陀螺 儀的訊號誤差，亦能夠適應於部分陀螺儀失效的狀況。此演算法的主要優點在於 不需要衛星動態模型，不但能夠解決先前文獻所遭遇的模型誤差，還能夠提供較 為簡易的運算過程以節省記憶體空間與加快運算速度。 本論文運用Matlab做系統的模擬。在陀螺儀具有訊號誤差的狀況，模擬結果 指出本論文所提出的新式估測演算法能夠獲得與擴增型卡爾曼濾波器（Extended Kalman Filter）相當的估測姿態精度（5 ）。在僅使用一軸或兩軸的陀螺 儀來估測衛星姿態的案例下，新式估測演算法還能運用僅剩的追星儀與陀螺儀來 繼續提供精確的衛星姿態判定，以延長衛星的有效工作時間，其姿態精度分別為 7 與8 ，並可由觀察器增益值的選定來取捨系統的收斂速度與 姿態準確度。 4 10 deg− × 4 10 deg− × 4 10 deg− ×參考文獻
1. Andy Wu; Douglas H. Hein;「Stellar Inertial Attitude Determination For LEO
Spacecraft.」A.I.A.A. Journal on Guidance, Control and Dynamics,
September.-October. 1982, pp. 417-429.
2. D. Choukroun; I. Y. Bar-Itzhack; Y. Oshman;「A Novel Quaternion Kalman
Filter」Presented as Paper 2002-4460 at the 42th AIAA Guidance, Navigation,
and Control Conference, Monterey, Aug. 2002.
3. Rongsheng Li; Yeong-Wei Andy; Douglas H. Hein;「System and method for
calibrating inter-star-tracker misalignments in a stellar inertial attitude determination system」United States Patent, US 6691033 B1, 2004.
4. I.Y. Bar-Itzhack; Y. Oshman;「attitude determination from vector observations
quaternion estimation」IEEE transactions on aerospace and electronic systems
Vol. AES-21, No. 1, January 1985.
5. Koprubasi, K.; Theint, M.-W.L.; 「Attitude and angular rate estimation using
the sliding mode observer with additive quaternion corrections.」American
Control Conference,14-16 June 2006.
6. Sangwoo Cho; Joohwan Chun; 「Satellite attitude acquisition using dual star
sensors with a bootstrap filter.」Sensors, 2002. Proceedings of IEEE Volume 2, 12-14 June 2002 Page(s):1723 - 1727 vol.2.
7. Lin Yurong; Deng Zhenglong; Shao Changsheng; 「Nonlinear predictive filter
for satellite attitude estimation using star sensors.」 Intelligent Control and
Automation, 2000. Proceedings of the 3rd World Congress on Volume 4, 28 June-2 July 2000 Page(s):2949 - 2953 vol.4.
8. Grewal, M.S.; Shiva, M.; 「Application of Kalman filtering to gyroless attitude
Control, 1995., Proceedings of the 34th IEEE Conference on Volume 2, 1995 Page(s):1544 - 1552 vol.2 .
