星間測(cè)量自主導(dǎo)航改進(jìn)狀態(tài)相關(guān)黎卡提方程濾波

，

中國(guó)空間技術(shù)研究院 錢學(xué)森空間技術(shù)實(shí)驗(yàn)室，北京 100094

分布式衛(wèi)星系統(tǒng)是由物理上互不相連、共同協(xié)作完成同一航天任務(wù)的多衛(wèi)星構(gòu)成的空間系統(tǒng)[1]，由于其具有對(duì)地面依賴性小、自主性強(qiáng)、靈活可變、綜合成本低等特點(diǎn)，成為衛(wèi)星技術(shù)發(fā)展的新趨勢(shì)。分布式衛(wèi)星系統(tǒng)實(shí)現(xiàn)自主協(xié)同需具備的核心和基礎(chǔ)能力是自主導(dǎo)航[2]，其自主導(dǎo)航問(wèn)題與普通衛(wèi)星存在較大區(qū)別：既需要確定每個(gè)衛(wèi)星的精確空間位置和速度，又需要確定衛(wèi)星之間的相對(duì)位置速度關(guān)系。在分布式衛(wèi)星系統(tǒng)應(yīng)用任務(wù)中，衛(wèi)星間的相對(duì)信息相對(duì)于絕對(duì)信息更容易獲得，近年來(lái)，利用星間相對(duì)觀測(cè)的自主導(dǎo)航技術(shù)備受關(guān)注。這些相對(duì)觀測(cè)量包括視線角、星間相對(duì)距離和相對(duì)位置矢量等[3-9]。能否基于相對(duì)測(cè)量序列求解各自衛(wèi)星的絕對(duì)位置和速度，在理論上取決于系統(tǒng)的能觀性能，在應(yīng)用上涉及高性能定軌濾波算法。

文獻(xiàn)[10-11]研究了基于星間測(cè)距的雙星自主導(dǎo)航方法，指出沒(méi)有先驗(yàn)信息，僅依靠相對(duì)距離測(cè)量的導(dǎo)航系統(tǒng)是虧秩的，并給出了嚴(yán)格的數(shù)學(xué)證明。文獻(xiàn)[12]針對(duì)雙星編隊(duì)首次提出了僅利用地心慣性坐標(biāo)系中星間相對(duì)位置矢量序列作為觀測(cè)量的自主絕對(duì)導(dǎo)航方法，并采用局部線性化的方法分析了其能觀性，文章指出此方法在半長(zhǎng)軸相等的共面圓軌道情況下不適用，但未給出嚴(yán)格的理論證明。文獻(xiàn)[13]對(duì)文獻(xiàn)[12]提出的導(dǎo)航方法進(jìn)行了物理角度的闡述，把雙星系統(tǒng)視作一個(gè)巨型加速度計(jì)，并對(duì)定軌精度進(jìn)行了分析，并說(shuō)明該方法除幾個(gè)0°傾角的特殊軌道外，在大部分情況下是能觀的。文獻(xiàn)[14]首次對(duì)基于星間位置矢量測(cè)量的雙星自主導(dǎo)航系統(tǒng)使用非線性系統(tǒng)局部能觀性理論進(jìn)行分析，通過(guò)數(shù)學(xué)證明指出在兩衛(wèi)星瞬時(shí)地心距不相等的情況下可以定軌。文獻(xiàn)[2]在上述研究基礎(chǔ)上提出了非線性系統(tǒng)局部k階能觀性的概念，并證明了理想二體軌道模型下該自主導(dǎo)航系統(tǒng)局部4～6階能觀的充要條件及其與非線性系統(tǒng)能觀性秩條件成立的關(guān)系。文獻(xiàn)[15]考慮能觀性強(qiáng)弱對(duì)濾波算法的影響，給出了一個(gè)基于相對(duì)位置矢量自主導(dǎo)航系統(tǒng)能觀性秩條件的能觀度指標(biāo)，并據(jù)此對(duì)無(wú)跡濾波(Unscented Kalman Filter，UKF)算法進(jìn)行改進(jìn)，提高了導(dǎo)航定位的精度。但由于在算法中，指標(biāo)只能基于位置速度的估計(jì)值而非其真值得到，因此不能真實(shí)反映系統(tǒng)能觀度的大小。

衛(wèi)星動(dòng)力學(xué)模型是個(gè)典型的非線性系統(tǒng)，一般采用非線性濾波算法對(duì)軌道進(jìn)行估計(jì)。常用的方法有擴(kuò)展卡爾曼濾波[16-19](Extended Kalman Filter，EKF)和無(wú)跡濾波[20-22]等。狀態(tài)相關(guān)黎卡提方程(State-Dependent Riccati Equation，SDRE)方法[23]通過(guò)構(gòu)造線性結(jié)構(gòu)解決非線性系統(tǒng)的最優(yōu)控制問(wèn)題，隨之應(yīng)用于非線性系統(tǒng)的估計(jì)[24]，構(gòu)成的濾波器被稱為狀態(tài)相關(guān)黎卡提方程濾波(State-Dependent Riccati Equation Filter，SDREF)。在無(wú)隨機(jī)游走的確定性系統(tǒng)中，SDREF可以全部獲得系統(tǒng)的非線性信息。這使得可以將非線性系統(tǒng)作為線性系統(tǒng)來(lái)處理，不存在線性化誤差，且無(wú)需計(jì)算Jacobian矩陣。

在本文的研究中，SDREF被用作利用地心慣性坐標(biāo)系中星間相對(duì)位置矢量序列作為觀測(cè)量的自主導(dǎo)航估計(jì)器，并與EKF算法在濾波步驟、計(jì)算負(fù)擔(dān)和濾波精度等方面進(jìn)行了比較。本文推導(dǎo)并定義了一種新的在線反映能觀性程度的特征量，提出了基于該特征量對(duì)濾波增益陣進(jìn)行在線調(diào)整的改進(jìn)SDREF算法。數(shù)學(xué)仿真表明,該改進(jìn)SDREF算法在保持SDREF計(jì)算量小的優(yōu)點(diǎn)下，導(dǎo)航估計(jì)精度明顯優(yōu)于采用EKF和SDREF算法的結(jié)果。

1 SDREF原理

SDREF是一種參數(shù)化的濾波方法，它將非線性系統(tǒng)改寫為線性化結(jié)構(gòu)，系統(tǒng)矩陣和觀測(cè)矩陣的參數(shù)由當(dāng)前狀態(tài)決定，稱為狀態(tài)相關(guān)系數(shù)(State-Dependent Coefficients，SDC)。SDREF與EKF唯一的不同之處在于EKF是在估計(jì)狀態(tài)附近展開進(jìn)行線性化近似，而SDREF是將非線性系統(tǒng)做參數(shù)化處理，這一轉(zhuǎn)換過(guò)程稱為偽線性化。

考慮如下的連續(xù)非線性系統(tǒng):

(1)

式中：X為n維狀態(tài)向量；Y為m維觀測(cè)向量；f(·)，h(·)分別為n維和m維非線性函數(shù)；w和v分別為n維和m維高斯白噪聲向量，各分量間互不相關(guān)，且滿足

(2)

由文獻(xiàn)[25]可知，如果f(·),h(·)可微且滿足f(0)=0，h(0)=0，則非線性系統(tǒng)(1)可以轉(zhuǎn)化為如下具有SDC的線性形式：

(3)

其狀態(tài)方程和觀測(cè)方程的系數(shù)矩陣以狀態(tài)向量為參數(shù)。由于沒(méi)有進(jìn)行線性化近似，式(1)轉(zhuǎn)化為式(3)的過(guò)程中不會(huì)損失任何信息，可以運(yùn)用線性系統(tǒng)標(biāo)準(zhǔn)Kalman濾波方法對(duì)式(3)進(jìn)行處理。

首先對(duì)式(3)進(jìn)行離散化，得到

(4)

式中：Φ(Xk)為狀態(tài)轉(zhuǎn)移矩陣；H(Xk)為觀測(cè)矩陣；wk和vk分別為離散化的狀態(tài)噪聲和觀測(cè)噪聲，當(dāng)采樣間隔Δt較小時(shí)滿足

狀態(tài)轉(zhuǎn)移矩陣Φ(Xk)如下：

(5)

在計(jì)算時(shí)一般取二階近似：

(6)

步驟1：選定濾波初值

步驟2：狀態(tài)一步預(yù)測(cè)

(7)

步驟3：計(jì)算一步預(yù)測(cè)均方誤差

(8)

步驟4：計(jì)算濾波增益

(9)

步驟5：計(jì)算估計(jì)均方誤差

Pk+1=(I-Kk+1Hk+1)Pk+1/k

(10)

步驟6：狀態(tài)更新

(11)

從上述過(guò)程中可以看出，與EKF算法相比，雖然SDREF沒(méi)有線性化損失信息量，但是f(X)和h(X)在改寫時(shí)拆解出了一個(gè)右乘狀態(tài)向量，其對(duì)狀態(tài)方程和觀測(cè)方程的描述有所欠缺。因此SDREF和EKF兩種算法對(duì)模型的近似程度高低難以給出明確的定論，后文會(huì)通過(guò)理論分析和數(shù)值仿真對(duì)SDREF和EKF算法在相對(duì)位置矢量測(cè)量自主導(dǎo)航系統(tǒng)中的表現(xiàn)進(jìn)行探討。

2 系統(tǒng)模型

考慮基于星間相對(duì)位置矢量測(cè)量的雙星自主導(dǎo)航系統(tǒng)，將兩顆衛(wèi)星記為衛(wèi)星A和衛(wèi)星B，測(cè)量關(guān)系如圖1所示。

O是地球中心，rA和rB分別是地心到兩衛(wèi)星的矢量，rAB是衛(wèi)星A到衛(wèi)星B的位置矢量。已知衛(wèi)星A搭載了星敏感器和星間相對(duì)測(cè)量設(shè)備(包括無(wú)線電或激光測(cè)距儀、光學(xué)或雷達(dá)測(cè)向儀)，可以測(cè)量自身在慣性空間中的姿態(tài)、衛(wèi)星A與衛(wèi)星B間的距離以及衛(wèi)星B在衛(wèi)星A本體坐標(biāo)系中的方位，通過(guò)坐標(biāo)轉(zhuǎn)換可以得到兩衛(wèi)星在慣性空間中的相對(duì)位置矢量。

本文選取地心赤道慣性坐標(biāo)系為參考坐標(biāo)系，坐標(biāo)原點(diǎn)為地球質(zhì)心，基準(zhǔn)面為赤道面，X軸指向春分點(diǎn)，Z軸指向北極，Y軸與X軸和Z軸構(gòu)成右手直角坐標(biāo)系。含J2攝動(dòng)項(xiàng)的衛(wèi)星軌道動(dòng)力學(xué)模型為[26]：

(12)

(13)

選取地心赤道慣性坐標(biāo)系下兩衛(wèi)星的位置速度向量為系統(tǒng)狀態(tài)變量，記

X為12維狀態(tài)變量，則帶J2攝動(dòng)項(xiàng)的系統(tǒng)狀態(tài)方程為：

(14)

其中系統(tǒng)噪聲w包括未考慮的高階地球非球形引力、太陽(yáng)光壓、大氣阻力等攝動(dòng)力。

觀測(cè)方程為：

Y=h(X)+v=rA-rB+v=HX+v

(15)

3 SDREF與EKF計(jì)算量對(duì)比

在基于相對(duì)位置矢量觀測(cè)的雙星導(dǎo)航系統(tǒng)中，由于觀測(cè)矩陣是線性定常的，SDREF與EKF唯一的區(qū)別在狀態(tài)轉(zhuǎn)移矩陣Φk的計(jì)算上，前者由當(dāng)前時(shí)刻的參數(shù)化矩陣求得，而后者由Jacobian矩陣求得。

對(duì)于SDREF，狀態(tài)方程(14)可以改寫為：

(16)

記I3×3為I3，則離散后的狀態(tài)轉(zhuǎn)移矩陣為

(17)

對(duì)于EKF，狀態(tài)方程非線性函數(shù)的Jacobian矩陣

(18)

式中：

狀態(tài)轉(zhuǎn)移矩陣

(19)

分析式(17)中SDREF狀態(tài)轉(zhuǎn)移矩陣的形式，可以得到以下定理。

定理1 在以式(14)為系統(tǒng)方程、式(15)為觀測(cè)方程的導(dǎo)航系統(tǒng)中，若SDREF滿足以下條件：

1)初始時(shí)刻估計(jì)向量協(xié)方差矩陣P0為對(duì)角4×4分塊陣；

2)觀測(cè)矩陣H為對(duì)角1×4分塊陣；

3)狀態(tài)誤差和觀測(cè)誤差為各分量互不相關(guān)的白噪聲，即Qk與Rk為對(duì)角陣。

則后續(xù)每一時(shí)刻的狀態(tài)誤差協(xié)方差矩陣Pk都是對(duì)角4×4分塊陣。

證明：用數(shù)學(xué)歸納法進(jìn)行證明。

1)當(dāng)k=1時(shí)，顯然成立。

一步預(yù)測(cè)均方誤差陣Pg+1/g為：

(20)

Pg+1/g是對(duì)角4×4的分塊陣。

(21)

則濾波增益矩陣Kg+1為：

(22)

Kg+1是對(duì)角4×1分塊陣。

將式(20)和式(22)帶入式(10)中，由于Kg+1，Hg+1和Rg+1都是對(duì)角分塊陣，所以Pg+1是對(duì)角4×4分塊陣。

由式(1)(2)，如果SDREF滿足定理1的條件1)～3)，則每一時(shí)刻的誤差協(xié)方差矩陣Pk都是對(duì)角4×4分塊陣。

在本文分析的自主導(dǎo)航系統(tǒng)中，定理1的條件是比較容易滿足的，此時(shí)Pk，Φk，Kk均為對(duì)角分塊陣，12維矩陣運(yùn)算可簡(jiǎn)化為4維矩陣運(yùn)算，計(jì)算量約是EKF的12.5%。

4 基于能觀度的改進(jìn)濾波算法

能觀度是系統(tǒng)能觀性強(qiáng)弱的度量，表征了系統(tǒng)狀態(tài)可由輸出反映的程度。非線性系統(tǒng)能觀度的大小會(huì)隨著狀態(tài)變化而波動(dòng)，從而對(duì)濾波算法產(chǎn)生影響。在能觀度較大的時(shí)刻，系統(tǒng)由觀測(cè)反映狀態(tài)的能力較強(qiáng)，濾波估計(jì)誤差較小；反之，能觀度較小的時(shí)刻，由觀測(cè)反映狀態(tài)的能力較弱，濾波估計(jì)誤差較大。

目前常用的能觀度評(píng)價(jià)準(zhǔn)則有基于能觀性矩陣的評(píng)價(jià)準(zhǔn)則和基于濾波誤差協(xié)方差陣P的評(píng)價(jià)準(zhǔn)則。基于能觀性矩陣的評(píng)價(jià)準(zhǔn)則需要計(jì)算矩陣的奇異值，求解比較繁瑣，且要基于狀態(tài)真值進(jìn)行計(jì)算。由于在實(shí)際濾波中真值是未知的，因此這種評(píng)價(jià)標(biāo)準(zhǔn)并不能準(zhǔn)確反映系統(tǒng)能觀度的大小。基于濾波誤差協(xié)方差陣P的評(píng)價(jià)準(zhǔn)則不需要求解能觀性矩陣，計(jì)算量比較小，但是在非線性濾波算法中，P陣不能反映真實(shí)的估計(jì)誤差協(xié)方差大小，所以此準(zhǔn)則也不能真實(shí)反映系統(tǒng)能觀度的情況。

本文從能觀度的物理意義出發(fā)，基于濾波算法量測(cè)更新過(guò)程給出了非線性系統(tǒng)能觀度的另外一種表征方式。

(23)

矩陣的Frobenius范數(shù)定義為：若A=(aij)∈Rm×n，規(guī)定

(24)

已知Frobenius范數(shù)具有如下性質(zhì)：

式中：λi(ATA)為矩陣ATA的第i個(gè)特征值。

依據(jù)以上分析，在濾波的時(shí)間更新過(guò)程中對(duì)一步預(yù)測(cè)估計(jì)誤差協(xié)方差陣的計(jì)算公式進(jìn)行修改，引入一個(gè)縮放參數(shù)ρ，將式(8)改寫為：

(25)

由此，依據(jù)當(dāng)前時(shí)刻系統(tǒng)能觀度的大小對(duì)誤差協(xié)方差陣進(jìn)行了修正，在適當(dāng)擴(kuò)大P的基礎(chǔ)上兼顧到能觀度的影響，增強(qiáng)了濾波的穩(wěn)定性，提高了濾波精度。由式(7)、式(9)～式(11)、式(25)構(gòu)成的算法本文稱之為改進(jìn)的狀態(tài)相關(guān)黎卡提方程濾波算法(Modified State-Dependent Riccati Equation Filter, MSDREF)。

5 仿真與分析

考慮分布式衛(wèi)星系統(tǒng)在衛(wèi)星編隊(duì)飛行、自主空間在軌服務(wù)和非合作目標(biāo)測(cè)量等應(yīng)用場(chǎng)景中，星間相對(duì)距離較近，因此選取兩個(gè)軌道相近的衛(wèi)星。兩衛(wèi)星的初始軌道參數(shù)如表1所示。

表1 兩衛(wèi)星初始軌道參數(shù)

衛(wèi)星A搭載了星敏感器和星間相對(duì)測(cè)量設(shè)備(包括無(wú)線電或激光測(cè)距儀、光學(xué)或雷達(dá)測(cè)向儀)，可以測(cè)量自身在慣性空間中的姿態(tài)、衛(wèi)星A與衛(wèi)星B間的距離以及衛(wèi)星B在衛(wèi)星A本體坐標(biāo)系中的方位。利用上述測(cè)量量，相對(duì)位置矢量在慣性空間中的坐標(biāo)可由下式解得：

(26)

仿真時(shí)間為40 000 s，星敏感器觀測(cè)數(shù)據(jù)更新率為0.5 Hz，星間測(cè)距數(shù)據(jù)采樣率為0.2 Hz，設(shè)定濾波周期為10 s，在同樣的初始條件下分別用EKF、SDREF和MSDREF算法對(duì)基于星間相對(duì)位置矢量測(cè)量的自主導(dǎo)航系統(tǒng)進(jìn)行仿真，MSDREF的參數(shù)γ取0.3。

定義位置誤差和速度誤差分別為:

將本算例中自主導(dǎo)航系統(tǒng)在3種算法下的估計(jì)結(jié)果進(jìn)行對(duì)比，如表2所示。可以看出EKF的計(jì)算時(shí)間約是SDREF和MSDREF的7～8倍，這與前述對(duì)3種算法計(jì)算量的分析吻合。在穩(wěn)態(tài)誤差方面，SDREF由于軌道模型非線性程度不高且不存在線性化誤差，其估計(jì)精度與EKF基本相當(dāng)；而MSDREF根據(jù)系統(tǒng)能觀度的變化對(duì)濾波協(xié)方差陣進(jìn)行在線調(diào)整，改善了EKF和SDREF對(duì)能觀度敏感的問(wèn)題，位置和速度估計(jì)的穩(wěn)態(tài)誤差是EKF的21%。

表2 EKF、SDREF和MSDREF估計(jì)結(jié)果對(duì)比

6 結(jié)束語(yǔ)

本文針對(duì)分布式衛(wèi)星系統(tǒng)高精度自主導(dǎo)航的任務(wù)需求，選取典型的基于星間相對(duì)位置觀測(cè)的雙星模型，采用SDREF算法對(duì)自主導(dǎo)航系統(tǒng)進(jìn)行估計(jì)，通過(guò)數(shù)學(xué)證明了在滿足一定條件時(shí)，SDREF的計(jì)算量顯著小于EKF的結(jié)論。文章提出了一種改進(jìn)的SDREF算法，根據(jù)系統(tǒng)能觀度強(qiáng)弱對(duì)標(biāo)準(zhǔn)SDREF算法進(jìn)行在線修正，并選取一組近距離雙星編隊(duì)任務(wù)進(jìn)行數(shù)學(xué)仿真，對(duì)EKF、SDREF和MSDREF的估計(jì)結(jié)果進(jìn)行了對(duì)比。在相同條件下，MSDREF的估計(jì)精度明顯優(yōu)于SDREF和EKF，且保持了SDREF計(jì)算量小的優(yōu)點(diǎn)。本文研究結(jié)果可以為未來(lái)分布式衛(wèi)星工程實(shí)施中高精度自主導(dǎo)航算法的設(shè)計(jì)提供重要參考。