基于羅格里斯參數的航天器遞歸線性姿態估計算法

羅宇陽 龔德仁 陳筠力 段登平 邵曉巍

1.上海交通大學，上海200240

2.上海衛星工程研究所，上海200240

高精度在軌姿態確定是航天器進行精確姿態控制的基礎，對提升航天器完成復雜空間任務的能力有著至關重要的作用。為了提高姿態確定精度，除了采用更精密的傳感器外，還需采用先進算法進行數據處理和姿態估計。早在1965年，Wahba［1］就提出了基于多矢量觀測的最小二乘姿態確定問題

其中:A 為待確定的姿態矩陣;bi為航天器本體系下的單位觀測矢量;ri為慣性坐標系下的單位參考矢量;ξi為第i個傳感器數據的相對權重，滿足ξi=1;n 為傳感器數目。

針對Wahba 問題，國內外學者已經進行了深入研究，形成了兩類解決方法，即非遞歸法和遞歸法。非遞歸法僅利用當前觀測值進行姿態估計，比較典型的算法包括:Shuster［2－3］提出的Davenport －q 方法，以及在此基礎上發展得到的QUEST(Quaternion ESTimator)算法。Markley［4－5］采用矩陣分解提出了SVD(Single Value Decomposition)方法。Mortari［6－8］提出了ESOQ(Estimators of the Optimal Quaternion)，ESOQ2 方法。值得一提的是，Mortari 通過Cayley 變換提出了羅格里斯參數描述的OLAE(Optimal Linear Attitude Estimator)算法［9］，避免了一元四次方程最大根的求解。遞歸法算法保留了以前的觀測信息，通過預測和更新等手段融入新的觀測信息，再利用QUEST 算法進行求解，這樣的做法提高了姿態估計的精度。1989年Shuster［10］提出了濾波QUEST和平滑QUEST 算法。Bar － Itzhack［11］提出了REQUEST (Recursive QUEST)算法。Choukroun［12－13］提出了最優REQUEST 算法，隨后又提出了對向量化K 矩陣進行Kalman 濾波以獲得更高姿態確定精度的方法。2009年Shuster［14］分析了濾波QUEST和REQUEST 之間的關系。2011年GONG［15］提出了基于幾何關系獲得的三種線性姿態估計算法，并對奇異避免和姿態估計協方差進行了分析。

本文以GONG［15］提出的第三種線性姿態估計算法為基礎，根據姿態運動學對其進行遞推，使得歷史測量數據能夠融入到當前的姿態估計中，從而提高估計精度。進一步引入遺忘因子，對歷史測量數據和當前測量數據進行權衡，獲得了加權遞歸算法。仿真結果表明本文提出的遞歸算法能夠有效地提高姿態估計精度，具有一定的理論意義和工程應用價值。

1 基于轉動幾何關系的線性姿態估計算法

1.1 轉動幾何關系

根據歐拉原理，航天器在慣性坐標系中的姿態可以描述成繞固定軸轉動一定角度，因此航天器方向余弦矩陣A 可以寫成

其中:I 為3 ×3 的單位矩陣;a 為轉動軸;θ 為轉動角。a×為由矢量a=［a1，a2，a3］T產生的斜對稱矩陣，具有以下形式

對于任意的觀測對{ri，bi}，有

經過整理后可得

通過式(5)可知，ri，bi，a 和θ 四者之間的空間幾何關系如圖1 所示。

圖1 轉動幾何關系

根據GONG［15］提出的方法，定義基于觀測向量對{ri，bi}的右手坐標系

該坐標系方向的幅值為

根據圖1 中的幾何關系可得

其中:αi為xi和a 的夾角，βi為xi和bi的夾角。

羅格里斯參數是常見的姿態描述之一，其定義為

將式(8)和式(9)代入式(10)可得

1.2 線性姿態估計算法

考慮到傳感器噪聲的影響，式(12)并不是總成立。假設傳感器輸出的觀測向量為

其中vi為觀測向量誤差。選取誤差向量為

其中

定義新的優化準則為

其中ξi為相對權重，i=1，2，…，n。此時姿態確定問題就是尋找一個最優的羅格里斯參數g，使得優化準則式(16)達到最小值。將式(14)和式(15)代入式(16)可得

定義

此時優化準則可簡寫成

式(19)取得極值的一階條件為

2 遞歸線性姿態估計算法

采用遞歸算法可以充分利用歷史測量數據來提高姿態估計精度。首先提出時不變遞歸算法，在此基礎上引入角速度，提出時變遞歸算法。進一步引入遺忘因子，得到加權時變遞歸算法。

2.1 時不變遞歸算法

時不變遞歸算法是針對角速度為0 時的情況。假設在tk時刻，已經處理了mk－1組測量數據，有nk組新的測量數據等待處理。令ξk，i，rk，i和分別為tk時刻的第i 組相對權重、參考向量和觀測向量和分別為tk時刻用來計算最優姿態Gibbs 向量的M 矩陣和υ 向量。根據式(18)可得

其中:μk為tk時刻的權重總和

將式(22)寫成遞歸形式可得

2.2 時變遞歸算法

時變遞歸算法針對航天器旋轉時姿態進行確定，需要利用陀螺的角速度測量進行輔助計算。假設采樣時間較短，在［tk－1，tk］時間段內衛星角速度為常值ωk－1，因此在此段時間內，由衛星旋轉引起的姿態Gibbs 向量為

其中:Δtk－1=tk－tk－1為采樣時間間隔。

假設根據tk－1時刻及以前的測量數據得到的tk－1時刻姿態估計為，預測tk時刻的姿態為，則有

tk－1時刻的估計姿態通過最小化如下性能函數得到

式(29)達到最小值的一階條件為

其中

整理后可得

定義

根據式(33)可得

根據時不變遞歸算法可知，時變遞歸算法為

顯然，當角速度ωk－1=0 時，時變遞歸算法等效為時不變遞歸算法。

2.3 加權時變遞歸算法

對于長期姿態估計過程來說，歷史測量數據會逐漸增多，從而導致新的測量數據權重變小，無法反應當前的測量狀態。為了改變這種情況，在遞歸算法中引入遺忘因子ρ，ρ 越大表示遺忘速度越快，姿態估計值中新的測量數據占的比重也就越大。此時，矩陣和向量的遞歸式變成

其中:0≤ρk≤1，μk=(1 －ρk)μk－1+ρkδμk。當ρk=0時，僅利用了歷史測量數據;當ρk=0.5 時，新舊數據權重相同，就是普通的遞歸線性估計算法;當ρk=1時，僅僅利用了當前測量數據，就是普通的線性估計算法。因此，調節ρk就可以獲得不同的姿態估計性能。

3 仿真示例

采用蒙特卡羅方法進行數值仿真，并對本文提出的遞歸線性姿態估計算法(Recursive linear attitude estimator，簡稱ReLAE)和QUEST 算法、OLAE算法進行比較和分析。假設星敏感器有3個，在航天器本體系中的安裝方位分別為［1，0，0］T，［0，1，0］T和［0，0，1］T。星敏感器噪聲為零均值高斯白噪聲，其標準差為10－3(rad)，校正后的陀螺測量誤差標準為 10－6(rad/s)，航天器角速度為ω=0.001s－1。

定義姿態估計誤差向量為

其中q=［qT，q4］T為真實姿態四元數。姿態估計誤差幅值為

仿真結果如圖2 所示，分析仿真數據可以得到如下幾個結論:

圖2 姿態估計誤差

1)遞歸線性姿態估計算法的精度明顯高于QUEST 和OLAE 算法;

2)隨著遺忘因子的減小，遞推姿態估計算法的精度越來越高。當ρ=0.005 時，穩定后的姿態估計精度比非遞推算法提高了近20 倍。

4 結論

本文對線性姿態估計的遞歸算法進行了研究。通過分析航天器姿態運動學，將歷史測量數據引入到當前的姿態估計中，提高估計精度。并引入遺忘因子，得到加權的遞歸線性姿態估計算法。仿真結果表明:

1)遞歸算法的姿態估計精度比非遞歸算法有了顯著的提高;

2)遺忘因子對姿態估計精度有著較大的影響。遺忘因子越小，姿態估計精度越高，但精度收斂的時間越長。

［1］Wahba G. Problem 65-1:A Least Squares Estimate of Spacecraft Attitude［J］. SIAM Review，1965，7(3):409.

［2］Shuster M D. Approximate Algorithms for Fast Optimal Attitude Computation［C］. AIAA 78-1249，Proceedings of AIAA Guidance and Control Conference，Palo Alto，California，August，1978:88-95.

［3］Shuster M D，OH S D.Three-Axis Attitude Determination from Vector Observations［J］. Journal of Guidance and Control，1981，4(1):70-77.

［4］Markley F L. Attitude Determination Using Vector Observations and the Singular Value Decomposition［J］.The Journal of the Astronautical Sciences，1988，36(3):245-258.

［5］Markley F L，Mortari D.Quaternion Attitude Estimation Using Vector Measurements［J］. The Journal of the Astronautical Sciences，2000，48(2＆3):359-380.

［6］Mortari D.ESOQ:A Closed-Form Solution to the Wahba Problem［J］. The Journal of the Astronautical Sciences，1977，45(2):195-204.

［7］Mortari D.ESOQ2:Single-Point Algorithm for Fast Optimal Attitude Determination［J］. Advances in the Astronautical Sciences，1977，97(2):803-816.

［8］Mortari D. Second Estimator for the Optimal Quaternion［J］.Journal of Guidance，Control and Dynamics，2000，23(4):885-888.

［9］Mortari D，Markley F L，Singla P.Optimal Linear Attitude Estimator［J］. Journal of Guidance，Control and Dynamics，2007，30(6):1619-1627.

［10］Shuster M D.A Simple Kalman Filter and Smoother for Spacecraft Attitude［J］.Journal of the Astronautical Sciences，1989，37(1):89-106.

［11］Bar-Itzhack I Y.REQUEST:A Recursive QUEST Algorithm for Sequential Attitude Determination［J］. Journal of Guidance，Control and Dynamics，1996，19(5):1034-1038.

［12］Choukroun D，Bar-Itzhack I Y，Oshman Y.Optimal REQUEST Algorithm for Attitude Estimation［J］.Journal of Guidance，Control，and Dynamics，2004，27(3):418-425.

［13］Choukroun D. Quaternion Estimation Using Kalman Filtering of the Vectorized K-matrix［C］. AIAA Guidance Navigation，and Control Conference，10-13 August，2009，Chicago，Illinois.

［14］Shuster M D.Filter QUEST or REQUEST［J］.Journal of Guidance，Control and Dynamics，2009，32(2):643-645.

［15］Gong D，Shao X，Wang X，et al.Optimal Linear Attitude Estimators viar Geometric Analysis［J］. Journal of Zhejiang University-SCIENCE A (Applied Physics ＆ Engineering)，2011，12(11):873-883.