使用 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）是將四元數資訊轉換成尤拉角，估測值與真實值的比較和

5.2 使用新式估測器估測四元數與訊號飄移值

【Case1】 訊號飄移值為常數

假設我們想要的估測的訊號飄移值為常數，並使用與 5.1 節相同的參數設 定條件進行模擬。

圖（5.11）使用新式估測器之姿態四元數真實值與估測值（幾乎重疊）

圖（5.12）使用新式估測器之三軸訊號飄移真實值與估測值

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

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

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

【Caes2】 訊號飄移量隨時間變化

假設待估測的訊號飄移值隨時間變化，並使用與 5.1 節相同的參數設定條 件進行模擬。

d（陀螺儀角速度訊號飄移量）： ^{4 4} 2 ^deg_sec 8.33 10 8.33 10 cos( )

d

_{= × − + × −} 2400

π t

圖（5.16）使用新式估測器之四元數真實值與估測值（幾乎重疊）

圖（5.17）使用新式估測器之三軸訊號飄移真實值與估測值

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

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

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

【Case3】 訊號飄移值為步階情況

假設待估測的訊號飄移值為步階訊號，並使用與 5.1 節相同的參數設定條 件進行模擬。

d（陀螺儀角速度訊號飄移量）：

8.3333e-004 deg/sec → - 8.3333e-004 deg/sec → 8.3333e-004 deg/sec

圖（5.21）使用新式估測器之四元數真實值與估測值（幾乎重疊）

圖（5.22）使用新式估測器之三軸訊號飄移真實值與估測值

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

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

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

【Case4】 新式估測器系統強健性測試

為了新式估測器的系統強健性，假設陀螺儀量測角速度為隨時間變化組合 而成，如圖（5.26）所示，其中訊號飄移量使用與 5.1 節相同的參數設定條件進 行模擬。

圖（5.26）陀螺儀量測角速度隨時間變化

圖（5.27）使用新式估測器之四元數真實值與估測值（幾乎重疊）

圖（5.28）使用新式估測器之三軸訊號飄移真實值與估測值

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

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

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

5.3 追星儀正常運作，陀螺儀失效情況

【Case5】 一軸陀螺儀失效

如前所述：在此案例中，姿態估測系統將估測失效軸之角速度及正常軸的 訊號飄移量，其中陀螺儀角速度為隨時間變化組合而成，訊號飄移量使用與 5.1 節相同的參數設定條件進行模擬。

圖（5.32）使用新式估測器之四元數真實值與估測值（幾乎重疊）

圖（5.33）使用新式估測器之正常軸訊號飄移及失效軸角速度的真實值與估測值

圖（5.34）正常軸訊號飄移及失效軸角速度的誤差收斂情形

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

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

【Case6】 二軸陀螺儀失效

在此案例中，直接估測兩軸失效軸之角速度及一軸正常軸的訊號飄移量。

其中陀螺儀角速度為隨時間變化組合而成，訊號飄移量使用與 5.1 節相同的參數 設定條件進行模擬。

圖（5.37）使用新式估測器之四元數真實值與估測值（幾乎重疊）

圖（5.38）使用新式估測器之正常軸訊號飄移及失效軸角速度的真實值與估測值

圖（5.39）正常軸訊號飄移及失效軸角速度的誤差收斂情形

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

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

5.4 新式估測器模擬結果討論

本論文估測效果的判定為計算真實值與估測值的誤差標準差（Error Standard deviation），公式如下表示：

1

Case1 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

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 ，並可由觀察器增益值的選定來取捨系統的收斂速度與

姿態準確度。

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

determination and control system for environmental satellites.」Decision and

Control, 1995., Proceedings of the 34th IEEE Conference on Volume 2, 1995 Page(s):1544 - 1552 vol.2 .

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