基于羅格里斯參數(shù)的航天器遞歸線性姿態(tài)估計算法

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

1.上海交通大學(xué)，上海200240

2.上海衛(wèi)星工程研究所，上海200240

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

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

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

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

1 基于轉(zhuǎn)動幾何關(guān)系的線性姿態(tài)估計算法

1.1 轉(zhuǎn)動幾何關(guān)系

根據(jù)歐拉原理，航天器在慣性坐標(biāo)系中的姿態(tài)可以描述成繞固定軸轉(zhuǎn)動一定角度，因此航天器方向余弦矩陣A 可以寫成

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

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

經(jīng)過整理后可得

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

圖1 轉(zhuǎn)動幾何關(guān)系

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

該坐標(biāo)系方向的幅值為

根據(jù)圖1 中的幾何關(guān)系可得

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

羅格里斯參數(shù)是常見的姿態(tài)描述之一，其定義為

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

1.2 線性姿態(tài)估計算法

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

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

其中

定義新的優(yōu)化準(zhǔn)則為

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

定義

此時優(yōu)化準(zhǔn)則可簡寫成

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

2 遞歸線性姿態(tài)估計算法

采用遞歸算法可以充分利用歷史測量數(shù)據(jù)來提高姿態(tài)估計精度。首先提出時不變遞歸算法，在此基礎(chǔ)上引入角速度，提出時變遞歸算法。進(jìn)一步引入遺忘因子，得到加權(quán)時變遞歸算法。

2.1 時不變遞歸算法

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

其中:μk為tk時刻的權(quán)重總和

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

2.2 時變遞歸算法

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

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

假設(shè)根據(jù)tk－1時刻及以前的測量數(shù)據(jù)得到的tk－1時刻姿態(tài)估計為，預(yù)測tk時刻的姿態(tài)為，則有

tk－1時刻的估計姿態(tài)通過最小化如下性能函數(shù)得到

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

其中

整理后可得

定義

根據(jù)式(33)可得

根據(jù)時不變遞歸算法可知，時變遞歸算法為

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

2.3 加權(quán)時變遞歸算法

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

其中:0≤ρk≤1，μk=(1 －ρk)μk－1+ρkδμk。當(dāng)ρk=0時，僅利用了歷史測量數(shù)據(jù);當(dāng)ρk=0.5 時，新舊數(shù)據(jù)權(quán)重相同，就是普通的遞歸線性估計算法;當(dāng)ρk=1時，僅僅利用了當(dāng)前測量數(shù)據(jù)，就是普通的線性估計算法。因此，調(diào)節(jié)ρk就可以獲得不同的姿態(tài)估計性能。

3 仿真示例

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

定義姿態(tài)估計誤差向量為

其中q=［qT，q4］T為真實姿態(tài)四元數(shù)。姿態(tài)估計誤差幅值為

仿真結(jié)果如圖2 所示，分析仿真數(shù)據(jù)可以得到如下幾個結(jié)論:

圖2 姿態(tài)估計誤差

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

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

4 結(jié)論

本文對線性姿態(tài)估計的遞歸算法進(jìn)行了研究。通過分析航天器姿態(tài)運(yùn)動學(xué)，將歷史測量數(shù)據(jù)引入到當(dāng)前的姿態(tài)估計中，提高估計精度。并引入遺忘因子，得到加權(quán)的遞歸線性姿態(tài)估計算法。仿真結(jié)果表明:

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

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

［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.