辛寧 邱樂德 張立華 丁延衛(wèi)

（1 航天東方紅衛(wèi)星有限公司，北京 100094）（2 中國(guó)空間技術(shù)研究院，北京 100094）

1 引言

低低跟蹤（SST-LL）重力測(cè)量衛(wèi)星的核心載荷為K 頻段測(cè)距（KBR）系統(tǒng)，該系統(tǒng)包括兩副喇叭天線（分別安裝在兩顆衛(wèi)星上），用于測(cè)量KBR 系統(tǒng)相位中心之間的距離及距離變化率，測(cè)量誤差可達(dá)微米級(jí)［1］。由于重力場(chǎng)反演過程中需要的數(shù)據(jù)是兩顆衛(wèi)星質(zhì)心之間的距離及距離變化率，因此還要確定兩顆衛(wèi)星在軌運(yùn)行過程中衛(wèi)星的質(zhì)心與KBR 系統(tǒng)相位中心在視線方向上的距離。在地面研制階段，KBR 系統(tǒng)相位中心與衛(wèi)星質(zhì)心的位置都是標(biāo)定好的，但衛(wèi)星在發(fā)射階段中產(chǎn)生的振動(dòng)、空間溫度變化及電磁噪聲等，都會(huì)使KBR 系統(tǒng)相位中心與質(zhì)心間的相對(duì)位置發(fā)生改變，影響KBR 系統(tǒng)的在軌測(cè)量誤差。為了反演高精度的地球重力場(chǎng)，進(jìn)行KBR 系統(tǒng)相位中心在軌標(biāo)定，進(jìn)而為KBR 系統(tǒng)幾何修正提供依據(jù)，具有重要的研究意義。

文獻(xiàn)［2］提出了KBR 系統(tǒng)相位中心在軌標(biāo)定的基本原理，將KBR 系統(tǒng)測(cè)量的距離及距離變化率與衛(wèi)星旋轉(zhuǎn)角的關(guān)系作為切入點(diǎn)，設(shè)計(jì)了衛(wèi)星姿態(tài)機(jī)動(dòng)規(guī)律，討論KBR系統(tǒng)相位中心在軌標(biāo)定的可行性。文獻(xiàn)［3］從工程角度出發(fā)，提出將磁力矩器及姿態(tài)控制噴氣發(fā)動(dòng)機(jī)為執(zhí)行部件設(shè)計(jì)衛(wèi)星的姿態(tài)機(jī)動(dòng)，以KBR系統(tǒng)、GPS接收機(jī)、星敏感器和磁強(qiáng)計(jì)為測(cè)量敏感元件的估計(jì)算法。該算法將KBR系統(tǒng)相位中心在軌標(biāo)定問題轉(zhuǎn)換為對(duì)衛(wèi)星姿態(tài)四元數(shù)的求解，并采用Batch估計(jì)算法進(jìn)行嘗試性的仿真研究。不過，這種算法是建立在衛(wèi)星動(dòng)力學(xué)特性精確已知的基礎(chǔ)上，而動(dòng)力學(xué)模型在實(shí)際工程中不可能保證精確，存在著不確定性的誤差，在這種情況下采用Batch估計(jì)算法很難保證標(biāo)定精度。文獻(xiàn)［4］提出估計(jì)狀態(tài)的預(yù)測(cè)濾波法。在預(yù)測(cè)濾波法中，模型誤差無需已知，而是作為解的一部分被估計(jì)，有效地解決了存在顯著模型誤差情況下的非線性問題；但是，該算法對(duì)初始狀態(tài)信息的依賴程度很高，當(dāng)初始狀態(tài)信息存在較大誤差時(shí)，其估計(jì)精度明顯下降，甚至發(fā)散。

綜合上述算法的特點(diǎn)，本文提出了一種利用預(yù)測(cè)卡爾曼濾波算法估計(jì)衛(wèi)星的姿態(tài)四元數(shù)，以擴(kuò)展卡爾曼濾波算法實(shí)現(xiàn)KBR 系統(tǒng)相位中心在軌標(biāo)定的算法。當(dāng)存在較大的衛(wèi)星姿態(tài)動(dòng)力學(xué)模型誤差時(shí)，該算法仍能精確地實(shí)現(xiàn)KBR 系統(tǒng)相位中心的標(biāo)定。同時(shí)，該算法采用三維偏差四元數(shù)代替四元數(shù)作為狀態(tài)變量，避免了估計(jì)過程中四元數(shù)單位約束性被破壞。

2 KBR 系統(tǒng)相位中心在軌標(biāo)定算法

KBR 系統(tǒng)相位中心在軌標(biāo)定算法的基本過程（見圖1）為：首先，利用姿態(tài)控制器發(fā)送指令給磁力矩器及姿態(tài)控制噴氣發(fā)動(dòng)機(jī)，使衛(wèi)星按照設(shè)計(jì)的姿態(tài)機(jī)動(dòng)規(guī)律進(jìn)行姿態(tài)機(jī)動(dòng)；然后，將星敏感器數(shù)據(jù)代入預(yù)測(cè)卡爾曼濾波算法中，估計(jì)出衛(wèi)星的姿態(tài)四元數(shù)；最后，利用擴(kuò)展卡爾曼濾波算法實(shí)現(xiàn)KBR 系統(tǒng)天線相位中心的在軌標(biāo)定。

圖1 數(shù)值仿真算法Fig.1 Numerical simulation algorithm

2.1 衛(wèi)星姿態(tài)機(jī)動(dòng)規(guī)律

設(shè)重力測(cè)量衛(wèi)星為A 星和B 星，則A 星俯仰／偏航機(jī)動(dòng)規(guī)律為［3］

B星俯仰／偏航機(jī)動(dòng)規(guī)律為

式中：φA和φB分別為A 星和B 星的俯仰角；φA和φB分別為A 星和B星的偏航角；C和f0分別為機(jī)動(dòng)的振幅和頻率；t為機(jī)動(dòng)時(shí)間；θ0為初始偏置角。

衛(wèi)星進(jìn)行姿態(tài)機(jī)動(dòng)，當(dāng)θ0為正值時(shí)，定義為衛(wèi)星正像機(jī)動(dòng)；當(dāng)θ0為負(fù)值時(shí)，定義為衛(wèi)星鏡像機(jī)動(dòng)。A 星的姿態(tài)機(jī)動(dòng)過程為：首先進(jìn)行衛(wèi)星俯仰機(jī)動(dòng)，衛(wèi)星的其他方向變化都須保持原標(biāo)稱姿態(tài)；俯仰機(jī)動(dòng)結(jié)束后，衛(wèi)星調(diào)整為三軸穩(wěn)定狀態(tài)，再進(jìn)行偏航機(jī)動(dòng)；在A 星機(jī)動(dòng)過程中，B 星的姿態(tài)始終保持原標(biāo)稱姿態(tài)。B 星的姿態(tài)機(jī)動(dòng)過程同A 星一致。在相位中心在軌標(biāo)定算法中，只取機(jī)動(dòng)過程中的數(shù)據(jù)進(jìn)行標(biāo)定。

2.2 KBR系統(tǒng)模型

KBR系統(tǒng)輸出的雙星距離Robs的表達(dá)式為［5］

式中：RA和RB分別為A 星和B 星的相位中心在J2000慣性坐標(biāo)系下的位置矢量，見式（4）和式（5）；Rbr為KBR 系統(tǒng) 的零偏 誤差；Rnr為KBR 系統(tǒng)的測(cè)量噪聲誤差。

式中：rA和rB分別為A 星和B 星的質(zhì)心在J2000慣性坐標(biāo)系下的位置向量；dpcA和dpcB分別為A 星和B星的相位中心在衛(wèi)星本體坐標(biāo)系下的位置矢量；qA和qB分別為A 星和B 星的姿態(tài)四元數(shù)；M為衛(wèi)星本體坐標(biāo)系轉(zhuǎn)換到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣，其表達(dá)式為

式中：q0，q1，q2，q3為衛(wèi)星姿態(tài)四元數(shù)。

顯然，在構(gòu)建上述系統(tǒng)模型時(shí)，要利用到衛(wèi)星姿態(tài)四元數(shù)q的信息，其計(jì)算公式為

角動(dòng)量H＝Jpω，Jp為衛(wèi)星的轉(zhuǎn)動(dòng)慣量矩陣；T為作用于衛(wèi)星上的總力矩。

衛(wèi)星運(yùn)行期間，其轉(zhuǎn)動(dòng)慣量矩陣Jp存在不確定性，而且，作用在衛(wèi)星上的總外部力矩受外部干擾力矩和其他攝動(dòng)因素的影響，往往無法精確已知［6-7］。如果直接利用衛(wèi)星姿態(tài)動(dòng)力學(xué)方程式（7）計(jì)算衛(wèi)星的四元數(shù)信息，其估計(jì)結(jié)果未必可靠，這是KBR 系統(tǒng)在軌標(biāo)定的一個(gè)難點(diǎn)所在。本文引入預(yù)測(cè)卡爾曼濾波算法來解決這一問題，首先利用預(yù)測(cè)濾波算法估計(jì)外部力矩誤差，然后利用卡爾曼濾波算法對(duì)衛(wèi)星姿態(tài)進(jìn)行估計(jì)，獲得高精度的坐標(biāo)系轉(zhuǎn)換矩陣M，最后利用擴(kuò)展卡爾曼濾波算法獲得KBR 系統(tǒng)相位中心的估計(jì)值。

2.3 應(yīng)用預(yù)測(cè)卡爾曼濾波算法的姿態(tài)四元數(shù)估計(jì)

設(shè)狀態(tài)變量為X1＝［qH］T，在式（7）中增加力矩模型誤差d（tk），得到姿態(tài)估計(jì)的狀態(tài)方程為

式中：ωk，qk，Tk，Hk分別為衛(wèi)星在tk時(shí)刻的角速度、姿態(tài)四元數(shù)、力矩和角動(dòng)量，k為時(shí)間序號(hào)；I為單位陣；f（X1）為系統(tǒng)狀態(tài)模型函數(shù)；G為模型誤差分布矩陣。

姿態(tài)估計(jì)的觀測(cè)方程為

式中：和為tk＋1時(shí)刻的2個(gè)測(cè)量矢量；B1和B2為2個(gè)不平行參考矢量；h為系統(tǒng)觀測(cè)模型函數(shù)；vk＋1為觀測(cè)噪聲。

將觀測(cè)方程式（9）泰勒級(jí)數(shù)展開得［4］

式中：Δt為采樣時(shí)間間隔。

式中：為f（X1）的一階李導(dǎo)數(shù)；f（X1）的二階李導(dǎo)數(shù)

式中：W為3×3階加權(quán)矩陣；M為測(cè)量噪聲協(xié)方差矩陣。

將狀態(tài)方程式（8）離散化，保留二階項(xiàng)，可得到離散的狀態(tài)估計(jì)傳播方程如下。

式中：A［X1（tk）］＝

將tk＋1時(shí)刻星敏感器輸出測(cè)量值，tk時(shí)刻估計(jì)值代入式（13），求得模型誤差d（tk）；然后把d（tk）代入式（14）中，計(jì)算出tk＋1時(shí)刻的一步狀態(tài)估計(jì)值X1（tk＋1｜tk）。

為防止四元數(shù)的正交性奇異問題，對(duì)四元數(shù)誤差進(jìn)行降階處理。設(shè)四元數(shù)誤差為矢部誤差，δq3為標(biāo)部誤差。給定姿態(tài)初始狀態(tài)估計(jì)值及初始估計(jì)均方差誤差P1（t0），利用擴(kuò)展卡爾曼濾波算法［8］就可以得到衛(wèi)星姿態(tài)四元數(shù)的估計(jì)值。其中，擴(kuò)展卡爾曼濾波算法中的姿態(tài)狀態(tài)轉(zhuǎn)移矩陣為

量測(cè)轉(zhuǎn)移矩陣為

則矢部誤差估計(jì)為

式中：Kq（tk＋1）為姿態(tài)反饋增益矩陣。

標(biāo)部誤差估計(jì)為

姿態(tài)四元數(shù)的估計(jì)值為

2.4 應(yīng)用擴(kuò)展卡爾曼濾波算法的KBR 系統(tǒng)相位中心在軌標(biāo)定

獲得衛(wèi)星姿態(tài)四元數(shù)的估計(jì)值后，便可精確計(jì)算兩顆衛(wèi)星的坐標(biāo)轉(zhuǎn)換矩陣M，在此基礎(chǔ)上，設(shè)狀態(tài)變量X2＝［dpcAdpcB］T，建立式（20）中的狀態(tài)方程和觀測(cè)方程，利用擴(kuò)展卡爾曼濾波算法遞推公式［8］即可估計(jì)出KBR 系統(tǒng)相位中心的偏差值。

式中：傳遞函數(shù)矩陣H2（t）＝［eTM1-eTM2］，e＝

3 數(shù)值仿真

設(shè)A 星和B星均運(yùn)行在高度為450km、偏心率為0.001、傾角為89.9°的軌道上，兩星相距239km，它們的初始軌道和姿態(tài)參數(shù)如表1所示。

表1 雙星初始參數(shù)Table 1 Initial parameters of two satellites

仿真中采用的衛(wèi)星攝動(dòng)模型如表2所示，仿真中衛(wèi)星的測(cè)量誤差參數(shù)如表3所示［9］。

表2 雙星動(dòng)力學(xué)模型Table 2 Dynamic model of two satellites

表3 雙星測(cè)量誤差Table 3 Measurement errors of two satellites

衛(wèi)星姿態(tài)機(jī)動(dòng)過程中的機(jī)動(dòng)規(guī)律設(shè)置為φ／φ＝±2＋sin（2πt／250）；為了減小多徑誤差對(duì)KBR 系統(tǒng)測(cè)距誤差的影響，衛(wèi)星每種姿態(tài)機(jī)動(dòng)時(shí)間取機(jī)動(dòng)周期的整數(shù)倍，這里取1000s。為了分析得到數(shù)據(jù)采集的軌道相位與相位中心標(biāo)定誤差的關(guān)系，分別選在赤道附近、北極附近進(jìn)行衛(wèi)星KBR 系統(tǒng)相位中心在軌標(biāo)定。

定義重力測(cè)量衛(wèi)星的本體坐標(biāo)系為OXYZ，原點(diǎn)O位于衛(wèi)星質(zhì)心，X軸指向KBR 系統(tǒng)的相位中心，Z軸指向地心，Y軸與Z軸、X軸構(gòu)成右手直角坐標(biāo)系，則在衛(wèi)星本體坐標(biāo)系下赤道上空力矩模型估計(jì)誤差曲線如圖2所示。

圖2 力矩模型誤差估計(jì)曲線Fig.2 Torques model estimation errors

對(duì)圖2中的估值序列進(jìn)行統(tǒng)計(jì)平均，得到力學(xué)模型誤差的穩(wěn)態(tài)估值為［0.96 18.8 0.97］×10-6N·m。標(biāo)定過程中，KBR系統(tǒng)輸出的距離觀測(cè)值見圖3，雙星距離大概變化560m。標(biāo)定過程中，衛(wèi)星質(zhì)心與KBR系統(tǒng)相位中心之間的距離變化見圖4，其距離變化周期與姿態(tài)角機(jī)動(dòng)周期相同，為250s，變化幅值為1.6mm。KBR系統(tǒng)的多徑傳播誤差見圖5，機(jī)動(dòng)過程中多徑傳播誤差最大為1.45mm，變化周期為250s。北極附近的分析結(jié)果與赤道上空的一致。

在赤道上空，當(dāng)初始狀態(tài)估計(jì)出現(xiàn)很大的誤差，X1（t0）＝［0.5 -0.5 0.5 0.5］T時(shí)，姿態(tài)估計(jì)結(jié)果見圖6和圖7。

圖3 KBR 系統(tǒng)距離觀測(cè)值Fig.3 KBR system range observation

圖4 衛(wèi)星質(zhì)心與KBR 系統(tǒng)相位中心之間的距離變化Fig.4 Distance changing between satellite centers of mass and KBR system phase centers

圖5 KBR 系統(tǒng)相位中心標(biāo)定過程中多徑傳播誤差值Fig.5 Mutipath error during KBR system phase center calibration

圖6 俯仰機(jī)動(dòng)姿態(tài)估計(jì)誤差Fig.6 Attitude estimation errors at pitch maneuver

圖7 偏航機(jī)動(dòng)姿態(tài)估計(jì)誤差Fig.7 Attitude estimation errors at yaw maneuver

單獨(dú)使用卡爾曼濾波算法的估計(jì)值并不能收斂于真值，估計(jì)結(jié)果嚴(yán)重發(fā)散。預(yù)測(cè)卡爾曼濾波算法可以實(shí)現(xiàn)姿態(tài)四元數(shù)的準(zhǔn)確估計(jì)，三軸估計(jì)誤差均在20μrad以內(nèi)。這是由于預(yù)測(cè)卡爾曼濾波算法采用更新量測(cè)矩陣信息，保證了模型誤差估計(jì)值的準(zhǔn)確性，因而其魯棒性更強(qiáng)。需要指出的是，預(yù)測(cè)卡爾曼濾波算法的上述性能是以犧牲運(yùn)算量為代價(jià)的，其運(yùn)算量為4430次乘法和2986次加法，約是預(yù)測(cè)濾波算法的3倍，但是要小于通常采用的卡爾曼濾波算法。

設(shè)相位中心估計(jì)值為dePCi，它與理論值dPCi之間的夾角θier作為標(biāo)定誤差。根據(jù)文獻(xiàn)［10］，如果要將由KBR 系統(tǒng)相位中心偏移引起的測(cè)距誤差控制到1μm，則標(biāo)定誤差θier必須在0.3mrad以內(nèi)。分別采用文獻(xiàn)［3］中的Batch估計(jì)算法及本文提出的算法，在相同的輸入條件下得到相位中心的標(biāo)定結(jié)果，如表4所示。當(dāng)初始狀態(tài)估計(jì)出現(xiàn)很大的誤差時(shí)，使用Batch估計(jì)算法結(jié)果無法收斂，而采用本文提出的算法，可以達(dá)到很好的估計(jì)結(jié)果。從標(biāo)定結(jié)果可以看出，無論在赤道地區(qū)還是在北極地區(qū)，單獨(dú)進(jìn)行正像俯仰／偏航機(jī)動(dòng)或者鏡像俯仰／偏航機(jī)動(dòng)，相位中心的標(biāo)定結(jié)果均不能滿足0.3mrad的精度要求，必須分別進(jìn)行正像／鏡像機(jī)動(dòng)，然后將數(shù)據(jù)進(jìn)行聯(lián)合處理，才可達(dá)到精度要求。其主要原因是：?jiǎn)为?dú)進(jìn)行正像機(jī)動(dòng)或鏡像機(jī)動(dòng)，受多徑傳播誤差影響較大；而將正像機(jī)動(dòng)與鏡像機(jī)動(dòng)數(shù)據(jù)相結(jié)合，可有效抵消多徑傳播誤差影響，使相位中心估計(jì)精度大幅度提高。總體而言，在該算法中，標(biāo)定軌道相位的選取，對(duì)最終標(biāo)定誤差的影響并不是特別顯著，赤道地區(qū)略優(yōu)。從激勵(lì)時(shí)長(zhǎng)看，單次機(jī)動(dòng)1000s的總機(jī)動(dòng)時(shí)間是可以接受的，激勵(lì)時(shí)間再長(zhǎng)，噴氣發(fā)動(dòng)機(jī)燃料消耗較大，不利于衛(wèi)星的長(zhǎng)壽命運(yùn)行。

表4 KBR 系統(tǒng)相位中心在軌標(biāo)定誤差Table 4 KBR system phase center in-orbit calibration errors mrad

4 結(jié)束語

KBR 系統(tǒng)相位中心在軌標(biāo)定的一個(gè)關(guān)鍵問題，是如何高精度地獲取衛(wèi)星在軌機(jī)動(dòng)時(shí)的姿態(tài)轉(zhuǎn)換矩陣。針對(duì)這一問題，本文提出了一種應(yīng)用預(yù)測(cè)卡爾曼濾波算法和擴(kuò)展卡爾曼濾波算法的KBR 系統(tǒng)相位中心在軌標(biāo)定算法，使KBR 系統(tǒng)相位中心標(biāo)定誤差優(yōu)于0.3mrad。仿真結(jié)果表明：該算法魯棒性強(qiáng)，對(duì)初始姿態(tài)誤差不敏感，能有效利用預(yù)測(cè)卡爾曼濾波算法的優(yōu)點(diǎn)。目前，濾波器的設(shè)計(jì)還有些復(fù)雜，在實(shí)際工程應(yīng)用還要結(jié)合KBR 系統(tǒng)的具體工作特性進(jìn)行深入研究，進(jìn)一步優(yōu)化濾波器結(jié)構(gòu)，為重力測(cè)量衛(wèi)星的工程化設(shè)計(jì)服務(wù)。

（References）

［1］佘世剛，王鍇，周毅，等.高精度星間微波測(cè)距技術(shù)［J］.宇航學(xué)報(bào)，2006，27（3）：403-406 She Shigang，Wang Kai，Zhou Yi，et al.The technology of high accuracy inter-satellite microwave ranging［J］.Journal of Astronautics，2006，27（3）：403-406（in Chinese）

［2］Romans L.Optimal determination quaternion from multiple star camera sensors［R］.Pasadena，California：NASA JPL，2003

［3］Wang Furun.Antenna phase center determination of inter-communicating satellites［C］／／Proceedings of the AAS／AIAA Astrodynamics Conference. Washington D.C.：AIAA，2002：212-218

［4］John L.Predictive filtering for attitude estimation without rate sensors［J］.Journal of Guidance，Control and Dynamic，1997，15（3）：522-527

［5］張紅軍，趙艷彬，孫克新.星間微波測(cè)距系統(tǒng)相位中心在軌標(biāo)定研究［J］.上海航天，2010，27（4）：1-5 Zhang Hongjun，Zhao Yanbin，Sun Kexin.Study on phase centers calibration of K-band ranging system during in-flight phase［J］.Aerospace Shanghai，2010，27（4）：1-5（in Chinese）

［6］Hall C D，Tsiotras P，Shen H.Tracking rigid body motion using thrusters and momentum wheels［J］.The Journal of the Astronautical Sciences，2002，50（3）：311-323

［7］Bang H，Choi H D.Attitude control of a bias momentum satellite using momentum of inertia ［J］.IEEE Transactions on Aerospace and Electronic System，2002，38（1）：243-250

［8］王本利，廖鶴，韓毅.基于MME／EKF 算法的衛(wèi)星質(zhì)心在軌標(biāo)定［J］.宇航學(xué)報(bào)，2010，31（9）：2150-2156 Wang Benli，Liao He，Han Yi.On-orbit calibration of satellite center of mass based on MME／EKF algorithm［J］.Journal of Astronautics，2010，31（9）：2150-2156（in Chinese）

［9］Kim J R.Simulaton study of a low-low satellite-to-satellite tracking mission［D］.Austin：University of Texas at Austin，2000

［10］Wang Furun.Study on center of mass calibration and K-band ranging system calibration of the GRACE mission［D］.Austin：University of Texas at Austin，2003