專利名稱：小衛(wèi)星姿態(tài)確定系統(tǒng)及方法
技術(shù)領(lǐng)域：
本發(fā)明屬于航天器技術(shù)領(lǐng)域，涉及一種姿態(tài)確定系統(tǒng)及方法，尤其涉及一種 針對小衛(wèi)星平臺的低成本、低功耗、高可靠性的姿態(tài)確定系統(tǒng)及方法。
背景技術(shù)：
小衛(wèi)星廣泛應(yīng)用于通訊、科學(xué)實驗、環(huán)境與資源等眾多領(lǐng)域，成為衛(wèi)星技術(shù) 的重要發(fā)展方向，受到世界各國的關(guān)注。衛(wèi)星平臺的姿態(tài)控制系統(tǒng)是衛(wèi)星正常運(yùn) 行并完成飛行任務(wù)的重要保障系統(tǒng)之一，而衛(wèi)星姿態(tài)的確定是實施姿態(tài)控制的基 礎(chǔ)，因此，針對小衛(wèi)星成本低，質(zhì)量輕，技術(shù)含量高等特點，研究開發(fā)低成本高 可靠性的姿態(tài)確定系統(tǒng)具有十分重要的意義。
目前衛(wèi)星上采用的姿態(tài)測量部件主要有陀螺，星敏感器，紅外地平儀，太陽 敏感器和磁強(qiáng)計等。其中，陀螺為慣性測量部件，測量衛(wèi)星的角速度，但可靠性
較低，壽命較短；星敏感器直接測量衛(wèi)星姿態(tài)，精度較高，但價格較高；紅外地 平儀有掃描式和靜態(tài)式兩種，通過敏感地球邊界獲得衛(wèi)星的滾動角和俯仰角，掃 描式精度較高，但價格較高且存在轉(zhuǎn)動機(jī)構(gòu)，而靜態(tài)式無轉(zhuǎn)動機(jī)構(gòu)，可靠性較高； 太陽敏感器用于測量太陽矢量，有數(shù)字式和模擬式兩種，模擬式結(jié)構(gòu)簡單，成本 較低；磁強(qiáng)計用于測量地磁矢量，成本低，可靠性高。
目前衛(wèi)星上采用的定姿算法主要有確定性定姿算法和狀態(tài)估計法。確定性算 法主要指直接測量(如星敏感器和紅外地平儀)和雙(或多)矢量定姿算法(如 TRIAD算法，QUEST算法)，確定性算法計算簡單但精度較低；狀態(tài)估計法主要指 最小二乘法和Kalman濾波算法，Kalman濾波一般是利用陀螺和其它姿態(tài)測量部 件組合，進(jìn)行高精度姿態(tài)確定，在高精度大衛(wèi)星平臺中應(yīng)用較多，國外有采用僅 利用磁強(qiáng)計或與太陽敏感器組合的Kalman濾波定姿算法，算法較為復(fù)雜。Kalman 濾波算法計算量大，星上一般采用固定增益形式，通過地面上注方式進(jìn)行增益調(diào) 整，實時性差，存在發(fā)散的風(fēng)險。
衛(wèi)星的小型化、低成本化，迫切要求形成一套結(jié)構(gòu)簡單、廉價但又具有較高精度和可靠性的姿態(tài)確定系統(tǒng)。

發(fā)明內(nèi)容
本發(fā)明所要解決的技術(shù)問題是提供一種針對小衛(wèi)星平臺的低成本、低功耗、 高可靠性的姿態(tài)確定系統(tǒng)。
另外，本發(fā)明還提供上述姿態(tài)確定系統(tǒng)的姿態(tài)確定方法。 為解決上述技術(shù)問題，本發(fā)明采用如下4支術(shù)方案 一種小衛(wèi)星姿態(tài)確定系統(tǒng)，其包括
若干姿態(tài)測量部件，用以對反映衛(wèi)星姿態(tài)的信息進(jìn)行測量；姿態(tài)測量部件包 括紅外地平儀、太陽敏感器、磁強(qiáng)計；
中央處理器，用以釆集上述姿態(tài)測量部件測量數(shù)據(jù)，計算環(huán)境模型，并根據(jù) 測量數(shù)據(jù)和環(huán)境模型選4奪相應(yīng)的定姿算法進(jìn)行姿態(tài)確定；其包括
地平儀數(shù)據(jù)采樣及處理單元，用以根據(jù)所述紅外地平儀測量的數(shù)據(jù)判斷紅外 地平儀的狀態(tài)；
太陽敏感器數(shù)據(jù)采樣及處理單元，用以根據(jù)所述太陽敏感器測量的數(shù)據(jù)判斷 太陽敏感器的狀態(tài)及衛(wèi)星本體坐標(biāo)系中的太陽矢量；
磁強(qiáng)計數(shù)據(jù)采樣及處理單元，用以根據(jù)所述磁強(qiáng)計測量的數(shù)據(jù)判斷磁強(qiáng)計的 狀態(tài)及衛(wèi)星本體坐標(biāo)系中的地磁矢量；
環(huán)境模型計算單元，用以獲取軌道坐標(biāo)系下的太陽矢量、地磁矢量； 定姿選擇單元，用以根據(jù)所述地平儀數(shù)據(jù)采樣及處理單元、太陽敏感器數(shù)據(jù)
采樣及處理單元、磁強(qiáng)計數(shù)據(jù)采樣及處理單元、及環(huán)境模型的數(shù)據(jù)，選擇相應(yīng)的 定姿算法進(jìn)行姿態(tài)確定。
作為本發(fā)明的一種優(yōu)選方案，當(dāng)衛(wèi)星不在穩(wěn)態(tài)工作模式時，定姿選擇單元控 制所述姿態(tài)確定系統(tǒng)選擇非穩(wěn)態(tài)定姿算法，通過獲取的測量數(shù)據(jù)選擇定偏航角地 平儀定姿算法、不定偏航角地平儀定姿算法、雙矢量定姿算法、單磁強(qiáng)計定姿算 法中的一種；
當(dāng)衛(wèi)星進(jìn)入穩(wěn)態(tài)工作模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇穩(wěn)態(tài) EKF定姿算法，通過獲取的測量數(shù)據(jù)選擇紅外地平儀/太陽Kalman濾波算法、紅外地平儀/磁Kalman濾波算法、太陽/磁Kalman濾波算法、單石茲Kalman濾波算 法中的一種。
作為本發(fā)明的一種優(yōu)選方案，所述定姿選擇單元控制姿態(tài)確定系統(tǒng)選擇非穩(wěn) 態(tài)定姿算法包括
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇定 偏航角地平儀定姿算法；
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇不 定偏航角地平儀定姿算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽壽文感器正常時，選擇雙矢量 定姿算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽壽文感器故障時，選擇單》茲強(qiáng) 計定姿算法。
作為本發(fā)明的一種優(yōu)選方案，所述定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇 穩(wěn)態(tài)EKF定姿算法包括
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇紅 外地平儀/太陽Kalman濾波算法；
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇紅 外地平儀A磁Kalman濾波算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇太陽/ 磁Kalman濾波算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單磁 Kalman濾波算法。
作為本發(fā)明的一種優(yōu)選方案，所述紅外地平儀為靜態(tài)紅外地平儀，太陽敏感
器為模擬式太陽敏感器，磁強(qiáng)計為三軸磁強(qiáng)計。
上述姿態(tài)確定系統(tǒng)的姿態(tài)確定方法，該方法包括如下步驟 紅外地平儀、太陽敏感器、磁強(qiáng)計對反映衛(wèi)星姿態(tài)的信息進(jìn)行測量； 中央處理器采集上述姿態(tài)測量部件的測量數(shù)據(jù)，計算環(huán)境才莫型，并4艮據(jù)測量
數(shù)據(jù)和環(huán)境;f莫型選4奪相應(yīng)的定姿算法進(jìn)行姿態(tài)確定；當(dāng)衛(wèi)星不在穩(wěn)態(tài)工作模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇非穩(wěn) 態(tài)定姿算法，通過獲取的測量數(shù)據(jù)選擇定偏航角地平儀定姿算法、不定偏航角地
平儀定姿算法、雙矢量定姿算法、單磁強(qiáng)計定姿算法中的一種；
當(dāng)衛(wèi)星進(jìn)入穩(wěn)態(tài)工作模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇穩(wěn)態(tài) EKF定姿算法，通過獲取的測量數(shù)據(jù)選擇紅外地平儀/太陽Kalman濾波算法、紅 外地平儀/磁Kalman濾波算法、太陽/磁Kalman濾波算法、單i茲Kalman濾波算 法中的一種。
作為本發(fā)明的一種優(yōu)選方案，所述定姿選擇單元控制姿態(tài)確定系統(tǒng)選擇非穩(wěn) 態(tài)定姿算法的過程包括
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇定 偏航角地平儀定姿算法；
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇不 定偏航角地平儀定姿算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇雙矢量 定姿算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單磁強(qiáng) 計定姿算法。
作為本發(fā)明的一種優(yōu)選方案，定偏^^角/不定偏;阮角地平^f義定姿算法包括如 下步驟紅外地平儀數(shù)據(jù)直接解算出衛(wèi)星的滾動角和俯仰角；根據(jù)太陽敏感器工 作狀態(tài)，地平儀定姿算法分為兩種情形；
若太陽敏感器數(shù)據(jù)有效，利用紅外地平儀確定的滾動角^和俯仰角e,根 據(jù)姿態(tài)矩陣反推衛(wèi)星的偏航角^:
^ = arctan 2[(&1. S炒-".& ), (H. & + A:2. &)];
其中，kl = cos(e).Sbx+sin(^).Sbz;
k2 = sin(6>). sinO). Sbx + cos(p). Sby -cos(<9). sinO) Sbz;
、"^，^，SjT ， S。 —^,&,^f分別為衛(wèi)星本體坐標(biāo)系和軌道坐標(biāo)系中
的太陽矢量；
10若太陽敏感器數(shù)據(jù)無效，取^ =();
雙矢量定姿算法包括如下步驟利用衛(wèi)星本體坐標(biāo)系中的地磁矢量B6和太 陽矢量S,,對照軌道坐標(biāo)系中的地磁矢量B。和太陽矢量S。，采用簡化后的QUEST 雙矢量定姿算法，確定衛(wèi)星的三軸姿態(tài)；
單磁強(qiáng)計定姿算法包括如下步驟當(dāng)僅三軸磁強(qiáng)計數(shù)據(jù)有效時，利用衛(wèi)星本 體坐標(biāo)系中的地》茲矢量Br對照軌道坐標(biāo)系中的地^茲矢量B。，近似計算衛(wèi)星的 俯仰角P: ^arctan2[(萬。^廣AAJ，(^^+AAJ]。
作為本發(fā)明的一種優(yōu)選方案，所述定姿選才奪單元控制所述姿態(tài)確定系統(tǒng)選擇 穩(wěn)態(tài)EKF定姿算法的過程包括
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇紅 外地平儀/太陽Kalman濾波算法；
當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇紅 外地平儀/磁Kalman濾波算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇太陽/ 磁Kalman濾波算法；
當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單磁 Kalman濾波算法。
作為本發(fā)明的一種優(yōu)選方案，所述紅外地平儀/太陽Kalman濾波算法、紅外 地平儀/磁Kalman濾波算法、太陽/磁Kalman濾波算法、單磁Kalman濾波算法 包括如下步驟姿態(tài)預(yù)估計步驟，狀態(tài)方程建立步驟，觀測量計算及觀測方程建 立步驟，Kalman濾波迭代計算步驟，姿態(tài)校正步驟。
本發(fā)明的有益效果在于
(1)測量部件中不含慣性部件，無轉(zhuǎn)動機(jī)構(gòu)，成本低，結(jié)構(gòu)簡單，可靠性 高，符合小衛(wèi)星低成本、高可靠性的特點；
(3)定姿算法多樣，設(shè)計了四種確定性定姿算法和四種Kalman濾波算法， 各算法之間能進(jìn)行有效融合，星上自主切換，提高了系統(tǒng)的可靠性；
(4 )將四種完整的Kalman濾波定姿算法引入定姿系統(tǒng)，沒有按傳統(tǒng)形式采用固定增益的簡化濾波算法，具有較高定姿精度；同時通過兩項關(guān)鍵技術(shù)，減少
了濾波算法的計算量。兩項關(guān)鍵技術(shù)為A、將四種Kalman濾波定姿算法設(shè)計成 統(tǒng)一的形式，星上根據(jù)測量部件的狀態(tài)自主進(jìn)行重構(gòu)；B、根據(jù)^磁控衛(wèi)星間歇控 制的特點，將整個Kalman濾波定姿算法分解為兩個部分，分時進(jìn)行計算。
本發(fā)明提供的姿態(tài)確定系統(tǒng)已成功應(yīng)用于小型號衛(wèi)星，運(yùn)行情況優(yōu)異，滿足 中等精度小衛(wèi)星平臺的需要，為小衛(wèi)星平臺姿態(tài)確定系統(tǒng)的設(shè)計提供了重要參 考。


圖1為本發(fā)明姿態(tài)確定系統(tǒng)的組成示意圖。 圖2為本發(fā)明姿態(tài)確定方法的流程圖。
具體實施例方式
下面結(jié)合附圖詳細(xì)說明本發(fā)明的優(yōu)選實施例。
請參閱圖l，本發(fā)明揭示了一種小衛(wèi)星姿態(tài)確定系統(tǒng)，其包括若干姿態(tài)測 量部件IO、中央處理器20。
姿態(tài)測量部件10用以對反映衛(wèi)星姿態(tài)的信息進(jìn)行測量；本實施例中，姿態(tài) 測量部件包括紅靜態(tài)紅外地平儀11、模擬式太陽敏感器12、三軸》茲強(qiáng)計13。中 央處理器20用以采集上述姿態(tài)測量部件的測量數(shù)據(jù)，計算環(huán)境4莫型，并才艮據(jù)測 量數(shù)據(jù)和環(huán)境模型選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定。本實施例中，中央處理器 20作為一星載計算機(jī)的一部分，該星載計算機(jī)還包括姿態(tài)控制系統(tǒng)40。
以下分別只于系統(tǒng)各組成詳細(xì)介紹。姿態(tài)測量部件
本發(fā)明從小衛(wèi)星低成本，低功耗、高可靠性的角度出發(fā)選擇姿態(tài)測量部件。 1)靜態(tài)紅外地平4義
靜態(tài)紅外地平儀通過測量地球與太空的邊界，可直接確定衛(wèi)星的滾動角和俯 仰角。主要指標(biāo)測量角范圍 ±10° 質(zhì)量 1. 3kg
功耗 0. 9W
2) 模擬式太陽敏感器
模擬式太陽敏感器安裝在衛(wèi)星的對太陽面上，通過測量敏感器平面與太陽矢 量之間的夾角，得到太陽矢量在衛(wèi)星本體坐標(biāo)系中的表示。主要指標(biāo) 角度分辨率優(yōu)于O. 3° 測量精度 1° 質(zhì)量 0.2kg 功耗 0. 3W
3) 三軸石茲強(qiáng)計
三軸磁強(qiáng)計用于測量衛(wèi)星本體坐標(biāo)系下三軸^茲場矢量強(qiáng)度，不存在一見場問 題，是本發(fā)明系統(tǒng)中最可靠的測量部件，對其進(jìn)行熱備份，進(jìn)一步提高系統(tǒng)可靠 性。主要指標(biāo)
量程(X、 Y、 Z軸) -50000nT~+50000nT
分辨率 2 nT
測量精度 ± 4nT+0. 05%測量值
質(zhì)量(兩探頭+線路盒)1.2kg
功耗(兩探頭+線路盒)1. 0W
本發(fā)明采用的三種測量部件均為低成本姿態(tài)測量部件，總質(zhì)量僅為2. 7kg, 功耗僅為2. 2W,符合小衛(wèi)星的特點。對各測量部件的數(shù)據(jù)均采用多次釆樣進(jìn)行 平均的方式進(jìn)行數(shù)據(jù)釆集，以提高測量精度。中央處理器
中央處理器20包括地平儀數(shù)據(jù)采樣及處理單元21、太陽敏感器數(shù)據(jù)采樣及 處理單元22、磁強(qiáng)計數(shù)據(jù)采樣及處理單元23、環(huán)境模型計算單元24、定姿選擇 單元25。
地平儀數(shù)據(jù)釆樣及處理單元21用以根據(jù)所述紅外地平儀測量的數(shù)據(jù)判斷紅 外地平儀的狀態(tài)并獲取紅外地平儀的四路信號值。
13太陽敏感器數(shù)據(jù)采樣及處理單元22用以根據(jù)所述太陽敏感器測量的數(shù)據(jù)判 斷太陽敏感器的狀態(tài)并獲取衛(wèi)星本體坐標(biāo)系中的太陽矢量Sb。
磁強(qiáng)計數(shù)據(jù)采樣及處理單元23用以根據(jù)所述磁強(qiáng)計測量的數(shù)據(jù)判斷磁強(qiáng)計 的狀態(tài)并獲取衛(wèi)星本體坐標(biāo)系中的地磁矢量Bb。
環(huán)境模型計算單元24用以獲取軌道坐標(biāo)系下太陽矢量So、地磁矢量Bo。
定姿選捧單元25，用以根據(jù)所述地平儀數(shù)據(jù)采樣及處理單元、太陽敏感器 數(shù)據(jù)采樣及處理單元、；茲強(qiáng)計數(shù)據(jù)采樣及處理單元、及環(huán)境才莫型計算單元的數(shù)據(jù)， 選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定。
本發(fā)明采用的定姿算法包括地平儀定姿(包含定偏航角和不定偏航角兩 種)，雙矢量定姿和單磁強(qiáng)計定姿，紅外地平儀/太陽Kalman濾波定姿，紅外地 平儀/磁Kalman濾波定姿，雙矢量Kalman濾波定姿和單石茲Kalman濾波定姿。中 央處理器根據(jù)姿控系統(tǒng)的工作模式和測量部件的工作狀態(tài)自主在各定姿算法間 進(jìn)4亍切:換或重構(gòu)。
各定姿算法重構(gòu)圖如圖2所示。當(dāng)衛(wèi)星不在穩(wěn)態(tài)工作模式時(條件 ),定 姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇非穩(wěn)態(tài)定姿算法，通過獲取的測量數(shù)據(jù)選 擇定偏航角地平儀定姿算法、不定偏航角地平儀定姿算法、雙矢量定姿算法、單 磁強(qiáng)計定姿算法中的一種。
當(dāng)衛(wèi)星進(jìn)入穩(wěn)態(tài)工作模式時(條件⑤)，定姿選擇單元控制所述姿態(tài)確定系 統(tǒng)選擇穩(wěn)態(tài)EKF定姿算法，通過獲取的測量數(shù)據(jù)選擇紅外地平儀/太陽Kalman 濾波算法、紅外地平儀/磁Kalman濾波算法、太陽/磁Kalman濾波算法、單石茲 Kalman濾波算法中的一種。
所述定姿選擇單元控制姿態(tài)確定系統(tǒng)選擇非穩(wěn)態(tài)定姿算法的過程包括當(dāng)?shù)?平儀有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時(條件②條件④)， 選擇定偏航角地平儀定姿算法；當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或 太陽敏感器故障時(條件②條件③)，選擇不定偏航角地平儀定姿算法；當(dāng)?shù)仄?儀失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時(條件①條件④)，選擇 雙矢量定姿算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時 (條件①條件③)，選擇單磁強(qiáng)計定姿算法。所述定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇穩(wěn)態(tài)EKF定姿算法的過程包 括當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時(條件② 條件④)，選擇紅外地平儀/太陽Kalman濾波算法；當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常， 且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時(條件②條件③)，選擇紅外地平儀/磁 Kalman濾波算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常 時(條件①條件④)，選擇太陽/磁Kalman濾波算法；當(dāng)?shù)仄絻x失效或故障，且 衛(wèi)星位于陰影區(qū)或太陽敏感器故障時(條件①條件③)，選擇單磁Kalman濾波算 法。
定姿選擇單元25根據(jù)實際情況依照上述定姿確定方法選擇相應(yīng)定姿算法， 以下具體介紹各定姿算法。
一、 地平儀定姿算法
紅外地平儀數(shù)據(jù)可直接解算出衛(wèi)星的滾動角和俯仰角。才艮據(jù)太陽敏感器工作 狀態(tài)，地平儀定姿算法分為兩種情形。若太陽敏感器數(shù)據(jù)有效，利用紅外地平儀 確定的滾動角^和俯仰角0,根據(jù)姿態(tài)矩陣反推衛(wèi)星的偏航角
^ = arctan 2pi. & - A:2. S」,(W & + A:2. &》] (1)
式中，kl = cos(^).Sbx+sin(e)'Sbz;
k2 = sin(61) sin⑨.+ cos(p). Sby -cos(<9). sin(p) Sbz;
SA = (&,&,&;)t ， S。 = (SOT,S。y，S。z)t分別為衛(wèi)星本體坐標(biāo)系和軌道坐標(biāo)系中的 太陽矢量。
若太陽敏感器數(shù)據(jù)無效，不定衛(wèi)星的偏航角，取^ = 0。
二、 雙矢量定姿算法
雙矢量定姿算法利用衛(wèi)星本體坐標(biāo)系中的地磁矢量B》和太陽矢量、，對照 軌道坐標(biāo)系中的地^磁矢量B。和太陽矢量S。，采用簡化后的QUEST雙矢量定姿算 法，確定衛(wèi)星的三軸姿態(tài)。三、 單磁強(qiáng)計定姿算法
當(dāng)僅三軸磁強(qiáng)計數(shù)據(jù)有效時，利用衛(wèi)星本體坐標(biāo)系中的地石茲矢量B^對照
軌道坐標(biāo)系中的地J茲矢量B。，近似計算衛(wèi)星的俯仰角P:
6 = arctan2+ (2)
四、 擴(kuò)展Kalman濾波定姿算法
擴(kuò)展Kalman濾波定姿算法為本發(fā)明系統(tǒng)的核心部分。
四種Kalman濾波算法結(jié)構(gòu)一致，可細(xì)分為姿態(tài)預(yù)估計，狀態(tài)方程建立，？見 測量計算及觀測方程建立，Kalman濾波迭代計算，姿態(tài)校正五個部分。具體描 述如下
①姿態(tài)預(yù)估計
根據(jù)衛(wèi)星動力學(xué)模型估計衛(wèi)星角速度
=+I_1 x (IgVw + h)] AT (3)
式中'<^=^W/>^m，^W"]T為第k時刻角速度濾波值，初始值
<。/Q=[o,,,o]T; I為衛(wèi)星轉(zhuǎn)動慣量矩陣，h為衛(wèi)星的偏置角動量，AT為計算周期。
根據(jù)衛(wèi)星運(yùn)動學(xué)模型估計衛(wèi)星姿態(tài)四元數(shù)
<*"=&,-叫[-械拳 -1蘇乖] (4)
4一i" = +會0(66。,"4)46一厶71 (5)
式中，Im4么。，,么。2,m，^呻,^級]t為第k時刻姿態(tài)四元數(shù)濾波值，初始
值l。,。[o,o,o,i]t，叫為衛(wèi)星軌道角速度；^。，w,[Aw6—錄,LwJ為第k時刻衛(wèi)星相對軌道系的角速度。Q^^,。表示構(gòu)建矩陣:
為觀測量計算做準(zhǔn)備，對衛(wèi)星姿態(tài)矩陣進(jìn)行預(yù)估計:
0 z— "to
一 0
0
_1— 一氣0
A =
1
。3,4+1" 1
—械
械 —2々A
一2么
械
1
(6)
②狀態(tài)方程建立
分別定義衛(wèi)星角速度 和姿態(tài)四元數(shù)q6。的真實值與估計值< ,t的誤差 ^w = , ， 丫和^b。=[ *。i， *。2, 6。3,&a。4 ]T ,滿足
qto =仏。
4。 ， a, = < + △ 取^6,和的矢量部分作為濾波狀態(tài)量X:
x = [^yte ~ &。i <%02 。3]T
連續(xù)濾波狀態(tài)方程為
(7)
式中，F(xiàn)(f)=
F1(3x3) 03x3
0.5I3x3 03x3.
,Fu-I"[[(I +h)x]-K,x]I]
13;<3為3維單位矩陣￡
將連續(xù)模型離散化:
(8)
17式中，=i6x6十F(o厶r, I6x6為6維單位矩陣'
③觀測量計算及觀測方程建立
Ka 1 man濾波;見測方禾呈離散形式為:
(9)
四種Kalman濾波算法，濾波觀測量Y^，觀測矩陣H^和噪聲V4+1的取值各 不相同，通過適當(dāng)處理將其設(shè)計成統(tǒng)一的形式，具體如下 >地平4義+太陽EKF
濾波M/測量Y^: Y-[^S;, ^ ^ 0]T
式中，M6z]T=[(AS。)x]S6; cV = P-2^0l, 鄰^-2&o2,
p和^為紅外地平儀確定的滾動角和俯仰角。
濾波5見測矩陣H
H =
03x3 H12 03x3 H22
式中，Hl2 =
0 -2(《《) 0
0 0 -2(《+&2
H22 =
2 0 0 0 2 0 0 0 0
測量噪聲vt+1的方差陣R取為:
R =
of00000
0cr，0000
00<T，000
00000
0000《0
00000式中，c7、， 分別為太陽敏感器和紅外地平儀的測量誤差方差,
>地平儀+磁EKF
濾波觀測量Y^: :^ = [55^ 5 w淤0]T
式中，SBA=[Mfa 3& "aK(AB。)x]BJ
濾波觀測矩陣H^: H =
03x3 H12 。3x3 H22.
式中，Hl2 =
:) o
0 -2(^ +《z)
0 0
測量噪聲Vfr+1的方差陣R取為:
R =
00000
0《0000
00《000
00000
0000 20
00000
H22 =
2 0 0 0 2 0 0 0 0
式中，^為》茲強(qiáng)計測量誤差方差。
>雙矢量EKF
濾波觀測量Y^: 3^ = [5& 叫2叫，然~《S6z]T
式中，鄧,[5圪5& 5&]t-[(AB。)x]B^
然6=[& MAr Mfe]T=[(AS0)x]S6。
濾波觀測矩陣H^: H =
03x3 H12 。3x3 H22.
19<formula>formula see original document page 20</formula>R =
200000
0《0000
00000
00000
0000of0
00000《
④Kalman濾波迭代計算
聯(lián)立Kalman濾波狀態(tài)方程和觀測方程:
ao)
按下述簡化的Kalman濾波迭代公式進(jìn)行迭代計算:
P
(ii)
式中，P^為濾波誤差方差陣，Q為系統(tǒng)誤差方差陣'
⑤姿態(tài)4交正
利用濾波值X^^對預(yù)估計值6w ， 4w進(jìn)行校正得到最終的姿態(tài): 角速度校正
姿態(tài)四元lt一交正
21+乜
4/jo，A:+l/fc+l = ^o，A:+lM+l/|4/)o，A:+l/;fe+l|
姿態(tài)四元凄史q轉(zhuǎn)化為姿態(tài)角p,A^ :
p = arcsin[2(q2gi3 +9,^4)] ^ =國arctan2
(13)
(14)
2 (qi恥-q2恥),(-q -q孟+ q32+q"一
通過對濾波觀測量的處理，四種Kalm an濾波算法可采用一套算法實現(xiàn)。
Kalman濾波遞推計算耗時很長，尤其是式(ll)涉及六維矩陣間的四則運(yùn)算， 在實際計算過程中，采取將六維矩陣拆成三維矩陣，將常數(shù)陣轉(zhuǎn)化為矢量等方式 進(jìn)行計算，有效的減少了計算量。式(11)中的第三式濾波誤差方差陣&皿+1的計 算是為了下一拍濾波計算做準(zhǔn)備，不參與該步定姿。而主動磁控衛(wèi)星采取一拍控， 一拍不控的方式對衛(wèi)星施加控制力矩，即在不控的一拍是不需要姿態(tài)信息的，所 以將式(11)中的第三式從Kalman算法中分離至不控的那一拍進(jìn)行計算，計算結(jié) 果設(shè)置為全局變量，作為完整Klaman濾波算法的輸入量，從而進(jìn)一步減少了 Kalman濾波定姿算法的計算量。
這里本發(fā)明的描述和應(yīng)用是說明性的，并非想將本發(fā)明的范圍限制在上述實 施例中。這里所^C露的實施例的變形和改變是可能的，對于那些本領(lǐng)域的普通才支 術(shù)人員來說實施例的替換和等效的各種部件是/>知的。本領(lǐng)域技術(shù)人員應(yīng)該清楚 的是，在不脫離本發(fā)明的精神或本質(zhì)特征的情況下，本發(fā)明可以以其它形式、結(jié) 構(gòu)、布置、比例，以及用其它組件、材料和部件來實現(xiàn)。在不脫離本發(fā)明范圍和 精神的情況下，可以對這里所披露的實施例進(jìn)行其它變形和改變。
2權(quán)利要求
1、一種小衛(wèi)星姿態(tài)確定系統(tǒng)，其特征在于，其包括若干姿態(tài)測量部件，用以對反映衛(wèi)星姿態(tài)的信息進(jìn)行測量；姿態(tài)測量部件包括紅外地平儀、太陽敏感器、磁強(qiáng)計；中央處理器，用以采集上述姿態(tài)測量部件的測量數(shù)據(jù)，并計算環(huán)境模型，然后根據(jù)測量數(shù)據(jù)及環(huán)境模型選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定；其包括地平儀數(shù)據(jù)采樣及處理單元，用以根據(jù)所述紅外地平儀測量的數(shù)據(jù)判斷紅外地平儀的狀態(tài)并獲取處理后的紅外地平儀測量數(shù)據(jù)；太陽敏感器數(shù)據(jù)采樣及處理單元，用以根據(jù)所述太陽敏感器測量的數(shù)據(jù)判斷太陽敏感器的狀態(tài)并獲取衛(wèi)星本體坐標(biāo)系中的太陽矢量；磁強(qiáng)計數(shù)據(jù)采樣及處理單元，用以根據(jù)所述磁強(qiáng)計測量的數(shù)據(jù)判斷磁強(qiáng)計的狀態(tài)并獲取衛(wèi)星本體坐標(biāo)系中的地磁矢量；環(huán)境模型計算單元，用以獲取軌道坐標(biāo)系下的太陽矢量、地磁矢量；定姿選擇單元，用以根據(jù)所述地平儀數(shù)據(jù)采樣及處理單元、太陽敏感器數(shù)據(jù)采樣及處理單元、磁強(qiáng)計數(shù)據(jù)采樣及處理單元、環(huán)境模型計算單元，選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定。
2、 根據(jù)權(quán)利要求1所述的小衛(wèi)星姿態(tài)確定系統(tǒng)，其特征在于當(dāng)衛(wèi)星不在穩(wěn)態(tài)工作模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇 非穩(wěn)態(tài)定姿算法，通過獲取的測量數(shù)據(jù)選擇定偏航角地平儀定姿算法、不定 偏航角地平儀定姿算法、雙矢量定姿算法、單磁強(qiáng)計定姿算法中的一種；當(dāng)衛(wèi)星進(jìn)入穩(wěn)態(tài)工作模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇 穩(wěn)態(tài)EKF定姿算法，通過獲取的測量數(shù)據(jù)選擇紅外地平儀/太陽Kalman濾波 算法、紅外地平儀/磁Kalman濾波算法、太陽/磁Kalman濾波算法、單磁 Kalman濾波算法中的一種。
3、 根據(jù)權(quán)利要求2所述的小衛(wèi)星姿態(tài)確定系統(tǒng)，其特征在于所述定姿選擇單元控制姿態(tài)確定系統(tǒng)選擇非穩(wěn)態(tài)定姿算法包括 當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇定偏航角地平儀定姿算法；當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇不定偏航角地平儀定姿算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇雙 矢量定姿算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單 磁強(qiáng)計定姿算法。
4、 根據(jù)權(quán)利要求2所述的小衛(wèi)星姿態(tài)確定系統(tǒng)，其特征在于所述定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇穩(wěn)態(tài)EKF定姿算法包括當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選 擇紅外地平儀/太陽Kalman濾波算法；當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選 擇紅外地平儀/磁Kalman濾波算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇太 陽/磁Kalman濾波算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單 磁Ka 1腿n濾波算法。
5、 根據(jù)權(quán)利要求1至4任意一項所述的小衛(wèi)星姿態(tài)確定系統(tǒng)，其特征在于所述紅外地平儀為靜態(tài)紅外地平儀，太陽敏感器為模擬式太陽敏感器， -茲強(qiáng)計為三軸》茲強(qiáng)計。
6、 一種權(quán)利要求1至5任意一項所述姿態(tài)確定系統(tǒng)的姿態(tài)確定方法，其特征在 于，該方法包括如下步驟紅外地平儀、太陽敏感器、磁強(qiáng)計對反映衛(wèi)星姿態(tài)的信息進(jìn)行測量； 中央處理器采集上述姿態(tài)測量部件的測量數(shù)據(jù)，計算環(huán)境模型，并根據(jù) 測量數(shù)據(jù)和環(huán)境模型選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定；當(dāng)衛(wèi)星不在穩(wěn)態(tài)控制模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇 非穩(wěn)態(tài)定姿算法，通過獲取的測量數(shù)據(jù)選擇定偏航角地平儀定姿算法、不定偏航角地平儀定姿算法、雙矢量定姿算法、單磁強(qiáng)計定姿算法中的一種；當(dāng)衛(wèi)星進(jìn)入穩(wěn)態(tài)工作模式時，定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇 穩(wěn)態(tài)EKF定姿算法，通過獲取的測量數(shù)據(jù)選擇紅外地平儀/太陽Kalman濾波 算法、紅外地平儀A茲Kalman濾波算法、太陽/,茲Kalman濾波算法、單i茲 Ka 1 man濾波算法中的 一種。
7、 根據(jù)權(quán)利要求6所述的姿態(tài)確定方法，其特征在于所述定姿選擇單元控制姿態(tài)確定系統(tǒng)選擇非穩(wěn)態(tài)定姿算法的過程包括當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選 擇定偏航角地平儀定姿算法；當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選 擇不定偏航角地平儀定姿算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇雙 矢量定姿算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單 磁強(qiáng)計定姿算法。
8、 根據(jù)權(quán)利要求7所述的姿態(tài)確定方法，其特征在于定偏航角/不定偏航角地平儀定姿算法包括如下步驟紅外地平儀數(shù)據(jù)直 接解算出衛(wèi)星的滾動角和俯仰角；根據(jù)太陽敏感器工作狀態(tài)，地平儀定姿算 法分為兩種情形；若太陽敏感器數(shù)據(jù)有效，利用紅外地平儀確定的滾動角P和俯仰角 根 據(jù)姿態(tài)矩陣反推衛(wèi)星的偏航角^ = arctan 2[(W & - A:2. &), (A:l. & + 。. &)];其中，kl = cos(^).Sbx+sin(^).Sbz;<formula>formula see original document page 0</formula>S6 )t ， S。 "&，&,S。,)t分別為衛(wèi)星本體坐標(biāo)系和軌道坐標(biāo)系中的太陽矢量；若太陽敏感器數(shù)據(jù)無效，取^/ = 0;雙矢量定姿算法包括如下步驟利用衛(wèi)星本體坐標(biāo)系中的地^磁矢量B6和太陽矢量&，對照軌道坐標(biāo)系中的地磁矢量B。和太陽矢量S。，采用簡化后的QUEST雙矢量定姿算法，確定衛(wèi)星的三軸姿態(tài)；單磁強(qiáng)計定姿算法包括如下步驟當(dāng)僅三軸磁強(qiáng)計數(shù)據(jù)有效時，利用衛(wèi) 星本體坐標(biāo)系中的地/磁矢量B6 ，對照軌道坐標(biāo)系中的地J茲矢量B。，近似計算 衛(wèi)星的俯仰角^ ^arctan2[(5。^廣S。,J，(^^
9、 根據(jù)權(quán)利要求6所述的姿態(tài)確定方法，其特征在于所述定姿選擇單元控制所述姿態(tài)確定系統(tǒng)選擇穩(wěn)態(tài)EKF定姿算法的過程 包括當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選 擇紅外地平儀/太陽Kalman濾波算法；當(dāng)?shù)仄絻x有效、數(shù)據(jù)正常，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選 擇紅外地平儀A茲Kalman濾波算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陽照區(qū)、太陽敏感器正常時，選擇太 陽A茲Kalman濾波算法；當(dāng)?shù)仄絻x失效或故障，且衛(wèi)星位于陰影區(qū)或太陽敏感器故障時，選擇單 i茲Kalman濾波算法。
10、 根據(jù)權(quán)利要求9所述的姿態(tài)確定方法，其特征在于所述紅外地平儀/太陽Kalman濾波算法、紅外地平4義Af茲Kalman濾波算 法、太陽/磁Kalman濾波算法、單磁Kalman濾波算法包括如下步驟姿態(tài)預(yù)估計步驟，狀態(tài)方程建立步驟，觀測量計算及觀測方程建立步驟，Kalman濾波迭代計算步驟，姿態(tài)校正步驟。
全文摘要
本發(fā)明揭示一種小衛(wèi)星姿態(tài)確定系統(tǒng)及方法，該系統(tǒng)包括若干姿態(tài)測量部件、中央處理器；中央處理器用以采集上述姿態(tài)測量部件的測量數(shù)據(jù)，計算環(huán)境模型，并根據(jù)測量數(shù)據(jù)和環(huán)境模型選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定；其包括地平儀數(shù)據(jù)采樣及處理單元、太陽敏感器數(shù)據(jù)采樣及處理單元、磁強(qiáng)計數(shù)據(jù)采樣及處理單元、環(huán)境模型計算單元、定姿選擇單元。定姿選擇單元根據(jù)所述地平儀數(shù)據(jù)采樣及處理單元、太陽敏感器數(shù)據(jù)采樣及處理單元、磁強(qiáng)計數(shù)據(jù)采樣及處理單元、及環(huán)境模型計算單元的數(shù)據(jù)，選擇相應(yīng)的定姿算法進(jìn)行姿態(tài)確定。本發(fā)明成本低，結(jié)構(gòu)簡單；同時定姿算法多樣，設(shè)計了四種確定性定姿算法和四種Kalman濾波算法，各算法之間能進(jìn)行有效融合，星上自主切換，提高了系統(tǒng)的可靠性。
文檔編號G01C21/00GK101556155SQ20091005161
公開日2009年10月14日 申請日期2009年5月20日 優(yōu)先權(quán)日2009年5月20日
發(fā)明者松 萬, 劉善伍, 劉國華, 吳子軼, 霖 左, 銳 張, 靜 張, 謝祥華, 黃志偉 申請人:上海微小衛(wèi)星工程中心