盧 山，張世源

(1. 上海航天控制技術(shù)研究所，上海 201109；2. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 201109)

0 引 言

近年來,中國航天領(lǐng)域的在軌服務(wù)任務(wù)日趨頻繁,對于空間非合作目標(biāo),如故障或失效的飛行器、空間垃圾等的相對導(dǎo)航技術(shù)研究是當(dāng)下的熱點(diǎn)。對非合作目標(biāo)的相對姿態(tài)、相對位置等信息進(jìn)行精確的濾波估計(jì),是實(shí)現(xiàn)在軌服務(wù)任務(wù)的基礎(chǔ),直接影響控制和制導(dǎo)的精度[1]。

為了體現(xiàn)追蹤航天器與目標(biāo)航天器間相對姿態(tài)與相對位置耦合的實(shí)時效應(yīng),提升相對狀態(tài)估計(jì)的精度,文獻(xiàn)[2-4]建立了非合作航天器相對運(yùn)動的軌道-姿態(tài)的一體化濾波模型。這一類模型直接將量測設(shè)備獲取的原始特征點(diǎn)的光學(xué)信息用于相對狀態(tài)的估計(jì),使?fàn)顟B(tài)變量維度過高(超過30維),在現(xiàn)有星上計(jì)算機(jī)的硬件能力下難以實(shí)現(xiàn)實(shí)時解算。本文考慮在非合作目標(biāo)質(zhì)心位置未知的情況下,將追蹤航天器上量測設(shè)備輸出的目標(biāo)航天器形心的信息引入濾波方程,建立了追蹤航天器與目標(biāo)航天器相對運(yùn)動的軌道-姿態(tài)的一體化濾波模型。相較以往研究中提出的模型,該模型的形式更加簡潔且便于計(jì)算,并具有以下特點(diǎn):狀態(tài)方程由軌道相對運(yùn)動模型與姿態(tài)相對運(yùn)動模型構(gòu)成,軌道相對運(yùn)動模型為線性,姿態(tài)相對運(yùn)動模型為非線性,且軌道與姿態(tài)相互解耦;觀測方程為非線性,且與軌道相關(guān)的狀態(tài)變量和與姿態(tài)相關(guān)的狀態(tài)變量相互耦合。

利用上述模型進(jìn)行非合作航天器相對狀態(tài)量的估計(jì)屬于非線性濾波問題。文獻(xiàn)[5]提出了一種混合階球面單形-徑向容積卡爾曼濾波器(Mixed-degree spherical simplex-radial cubature Kalman filter,MSSRCKF)。MSSRCKF結(jié)合了三階球面單形規(guī)則和五階徑向規(guī)則,使得算法能夠以接近容積卡爾曼濾波器(Cubature Kalman filter,CKF)[6]的計(jì)算復(fù)雜度獲得接近高階CKF(High-degree CKF,HCKF)[7]的五階精度,適用于高維度的強(qiáng)非線性系統(tǒng)。但由于本文建立的非合作航天器相對運(yùn)動一體化濾波模型的狀態(tài)方程中包含部分線性項(xiàng),當(dāng)使用非線性濾波器對模型全局進(jìn)行處理時,部分計(jì)算成本將被浪費(fèi)。文獻(xiàn)[8-11]針對狀態(tài)方程為非線性、觀測方程為線性的情況,將非線性濾波器的時間更新公式與線性濾波器的量測更新公式相結(jié)合,使濾波器能夠?qū)δＰ椭械木€性與非線性模塊分別采用相對應(yīng)的濾波方式,提高了計(jì)算效率。文獻(xiàn)[12]使用Rao-Blackwellized粒子濾波器對狀態(tài)方程部分狀態(tài)變量為線性、線性項(xiàng)與非線性項(xiàng)相互作用的系統(tǒng)模型進(jìn)行估計(jì),實(shí)現(xiàn)了非線性濾波算法采樣點(diǎn)數(shù)量的降維。但上述研究均無法用于解決狀態(tài)方程中線性項(xiàng)與非線性項(xiàng)相互解耦、觀測方程中線性項(xiàng)與非線性項(xiàng)相互耦合這一更為復(fù)雜的情況。針對上述問題,本文通過在時間更新公式中加入線性項(xiàng)與非線性項(xiàng)之間互協(xié)方差矩陣的更新,將適用于線性系統(tǒng)的卡爾曼濾波器(Kalman filter,KF)[13]與MSSRCKF進(jìn)行了融合,提出了線性-非線性混合卡爾曼濾波器(Linear-nonlinear hybrid Kalman filter,L-NLKF)。該濾波器能夠?qū)σ粋€濾波方程中的線性與非線狀態(tài)變量分別采用不同的濾波方式進(jìn)行處理,在保證估計(jì)精度的同時,顯著降低了計(jì)算量。

1 航天器相對運(yùn)動的軌道-姿態(tài)一體化濾波模型

坐標(biāo)系定義如下:

OI-XIYIZI:地心慣性坐標(biāo)系FI。原點(diǎn)OI為地心,XI位于赤道平面內(nèi),指向春分點(diǎn),ZI沿地球自轉(zhuǎn)軸方向,向上為正,YI軸與XI軸和ZI軸構(gòu)成右手直角坐標(biāo)系。

OO-XOYOZO:第二軌道坐標(biāo)系FO。原點(diǎn)OO位于航天器質(zhì)心,ZO軸指向地心,XO軸在軌道平面內(nèi)垂直于ZO軸指向航天器飛行方向,YO軸與XO軸和ZO軸構(gòu)成右手直角坐標(biāo)系。根據(jù)該定義可以分別得到追蹤星第二軌道坐標(biāo)系FCO與目標(biāo)星第二軌道坐標(biāo)系FTO。

OB-XBYBZB:本體坐標(biāo)系FB。原點(diǎn)OB位于航天器質(zhì)心,XB軸沿航天器主慣量軸指向前,ZB軸垂直于XB軸指向下,YB軸與XB軸和ZB軸構(gòu)成右手直角坐標(biāo)系。根據(jù)該定義可以分別得到追蹤星本體坐標(biāo)系FCB與目標(biāo)星第二軌道坐標(biāo)系FTB。

1.1 軌道相對運(yùn)動模型

由于目標(biāo)航天器為非合作,其軌道角速度未知,故將軌道相對運(yùn)動模型投影在坐標(biāo)系FCO中。假設(shè)追蹤星在近距離捕獲階段軌道半徑不發(fā)生較大改變,其軌道角速度n近似為常數(shù)。記坐標(biāo)系FCB中兩星質(zhì)心的相對位置矢量為ρ=[x,y,z]T,線性化后的目標(biāo)星質(zhì)心相對追蹤星質(zhì)心的相對軌道動力學(xué)方程表示為

(1)

式中:wx,wy,wz為攝動力和模型誤差引起的激勵噪聲。

由于非合作航天器的質(zhì)心位置未知,而形心位置可以通過激光雷達(dá)數(shù)據(jù)建立的三維點(diǎn)云得到,故建立目標(biāo)星形心坐標(biāo)系FTF,其三軸指向與坐標(biāo)系FTB一致。將兩星的相對位置、相對速度以及目標(biāo)星質(zhì)心在坐標(biāo)系FTF下的坐標(biāo)(a,b,c)作為狀態(tài)量,建立狀態(tài)方程

(2)

(3)

(4)

(5)

經(jīng)離散化后的狀態(tài)方程表示為

Xk+1=ΦXk+Γwk

(6)

式中:Φ∈R9×9為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣;?！蔙9×3為系統(tǒng)的干擾矩陣。

1.2 姿態(tài)相對運(yùn)動模型

(7)

由單位四元數(shù)表示的相對姿態(tài)運(yùn)動學(xué)方程為

(8)

ω=ωIC-ωIT

(9)

式中:ωIC和ωIT分別為追蹤星和目標(biāo)星相對于坐標(biāo)系FI的角速度。

坐標(biāo)系FCB下追蹤星相對目標(biāo)星的姿態(tài)動力學(xué)方程為

(10)

1.3 狀態(tài)空間模型

1) 狀態(tài)方程:

根據(jù)上述建立的相對運(yùn)動模型,狀態(tài)向量x定義為

(11)

得到具有加性噪聲的連續(xù)非線性動態(tài)系統(tǒng)

(12)

式中:w為零均值高斯白噪聲,協(xié)方差為Q;F(x)為關(guān)于狀態(tài)向量x的非線性函數(shù),可結(jié)合式(2)、(8)、(10)得到。

易知,此處軌道與姿態(tài)的相關(guān)狀態(tài)量是解耦的。由于連續(xù)非線性動態(tài)系統(tǒng)不適用于計(jì)算機(jī)數(shù)值仿真,本文采用四階龍格-庫塔法[15]對其進(jìn)行離散化處理。

2) 觀測方程:

對于非合作目標(biāo)的測量,星上量測設(shè)備輸出的是坐標(biāo)系FTF與單機(jī)測量系之間的相對位置與相對姿態(tài)信息。假設(shè)單機(jī)測量系與坐標(biāo)系FCB重合,用rzx表示目標(biāo)星質(zhì)心在坐標(biāo)系FTF中的坐標(biāo)(a,b,c);rld為激光雷達(dá)的測量值,由追蹤星質(zhì)心指向目標(biāo)星形心;rCT為兩星質(zhì)心連線(追蹤星指向目標(biāo)星)。上述矢量存在幾何關(guān)系如下

(13)

建立觀測方程

(14)

2 混合階球面單形-徑向卡爾曼濾波器

考慮如下的離散非線性動態(tài)系統(tǒng)

(15)

為了提高濾波精度以及降低計(jì)算復(fù)雜度,文獻(xiàn)[5]將3階球面單形積分準(zhǔn)則與5階徑向積分準(zhǔn)則相結(jié)合,提出了一種混合階球面單形—徑向容積卡爾曼濾波器(MSSRCKF)。MSSRCKF具有優(yōu)異的非線性濾波性能。在維度較高的非線性系統(tǒng)中,MSSRCKF濾波精度接近于具有五階泰勒精度的HCKF,而計(jì)算時間僅與CKF相當(dāng),適用于非合作航天器相對運(yùn)動的軌道-姿態(tài)一體化濾波模型。

MSSRCKF的求積節(jié)點(diǎn)設(shè)置為:

(16)

(17)

對應(yīng)的權(quán)重系數(shù)為:

(18)

1) 時間更新

(19)

2) 量測更新

(20)

3 線性-非線性混合卡爾曼濾波器

式(12)為本文建立的非合作航天器相對運(yùn)動模型的狀態(tài)方程,其中F(x)由式(2)、(8)、(10)構(gòu)成。式(2)為描述航天器軌道相對運(yùn)動的線性方程,式(8)和(10)為描述航天器姿態(tài)相對運(yùn)動的非線性方程,且式(2)與式(8)、(10)中的狀態(tài)變量相互解耦。使用MSSRCKF進(jìn)行濾波估計(jì)時,在式(19)所示的時間更新步驟中,該算法將對式(2)、(8)、(10)中包含的全部狀態(tài)變量按式(16)所示的求積節(jié)點(diǎn)設(shè)置要求進(jìn)行確定性采樣。

確定性采樣法是對服從高斯分布的狀態(tài)變量經(jīng)過非線性變化后的分布情況進(jìn)行近似估計(jì)的方法[16]。由于式(2)為線性,服從高斯分布的狀態(tài)變量通過該方程后的分布仍服從高斯分布,其均值和方差可直接由線性卡爾曼濾波公式求得。使用MSSRCKF的確定性采樣法對式(2)進(jìn)行處理時,并不會提高對應(yīng)狀態(tài)變量的估計(jì)精度,反而會對計(jì)算成本造成浪費(fèi)。

針對上述問題,本文將應(yīng)用于線性系統(tǒng)的KF與MSSRCKF進(jìn)行了融合,提出了線性-非線性混合卡爾曼濾波器(Linear-Nonlinear Hybrid Kalman Filter,L-NLKF)。

(21)

過程噪聲wk也相應(yīng)劃分為w1,k∈R9與w2,k∈R7,對應(yīng)的協(xié)方差矩陣分別為Q1,k∈R9×9與Q2,k∈R7×7。根據(jù)狀態(tài)變量的劃分情況以及協(xié)方差矩陣的性質(zhì),對協(xié)方差矩陣Pk進(jìn)行分塊

(22)

式中:P1,k∈R9×9為x1,k的協(xié)方差矩陣;P2,k∈R7×7為x2,k的協(xié)方差矩陣;P12,k∈R9×7為x1,k與x2,k的互協(xié)方差矩陣。

(23)

式中:f(·)由式(5)和(7)得到。

由于L-NLKF主要針對軌道-姿態(tài)一體化濾波模型中狀態(tài)方程的特殊非線性形式進(jìn)行設(shè)計(jì),只需對MSSRCKF算法的時間更新方式進(jìn)行改進(jìn),故本節(jié)僅給出L-NLKF算法時間更新的計(jì)算步驟:

1) 利用KF進(jìn)行線性狀態(tài)預(yù)測

(24)

2) 利用MSSRCKF進(jìn)行非線性狀態(tài)預(yù)測

(25)

3) 利用卡爾曼濾波器進(jìn)行x1,k協(xié)方差矩陣更新

P1,k+1|k=ΦP1,kΦT+Q1,k

(26)

4) 利用MSSRCKF進(jìn)行x2,k協(xié)方差矩陣更新

(27)

5)x1,k與x2,k的互協(xié)方差矩陣的更新

互協(xié)方差矩陣P12,k的更新是L-NLKF算法的關(guān)鍵,本文將給出較為詳細(xì)的推導(dǎo)過程。

P12,k|k+1=E[(x1,k+1|k-Ex1,k+1|k)(x2,k+1|k-

(28)

(29)

(30)

(31)

(32)

通過這種方式便可將高斯分布的加權(quán)二重積分問題轉(zhuǎn)換為如式(30)所示的高斯分布的加權(quán)一重積分,并能夠使用確定性采樣的數(shù)值方法進(jìn)行近似計(jì)算。式(30)中:ηi,k與ωi分別為求積節(jié)點(diǎn)與對應(yīng)的權(quán)重。若對式(30)中的求積節(jié)點(diǎn)ηi,k重新進(jìn)行求解,在增加了計(jì)算量的同時,由于一部分求積節(jié)點(diǎn)將被矩陣Θ1與Θ2歸零,其中包含的分布信息將被浪費(fèi),且余下未被歸零的求積節(jié)點(diǎn)不再能夠完整的描述狀態(tài)量的分布情況,使得濾波精度下降。

(33)

對x1,k進(jìn)行確定性采樣時,求積節(jié)點(diǎn)ζi,k應(yīng)滿足條件如下:

(34)

④ζi,k應(yīng)滿足協(xié)方差等式

(35)

(36)

其中條件③、④為使ζi,k的前二階矩與x1,k一致,以保證求積精度。根據(jù)上述條件,本文設(shè)置求積節(jié)點(diǎn)如式(37)所示。需要注意的是,滿足上述條件的求積節(jié)點(diǎn)不唯一,本文選擇其中的一種情況進(jìn)行設(shè)置。當(dāng)bi的解析表達(dá)式難以直接得到時,可以將bi中元素設(shè)為未知數(shù),結(jié)合上述條件利用計(jì)算機(jī)求解多元非線性方程,得到bi的數(shù)值解。

(37)

6) 合并線性項(xiàng)與非線性項(xiàng)

(38)

(39)

4 仿真校驗(yàn)

表1 航天器參數(shù)設(shè)置Table 1 Spacecraft parameter setting

濾波時長設(shè)置為120 s,步長為1 s;使用均方根誤差(RMSE)來各濾波算法的精度。

(40)

共進(jìn)行NM=100次蒙特卡洛仿真,并將相對姿態(tài)四元數(shù)的計(jì)算結(jié)果轉(zhuǎn)換為歐拉角,分別記為偏航角ψ、俯仰角θ、滾轉(zhuǎn)角φ。濾波器估計(jì)誤差對比如圖1-圖5所示。

圖1 相對位置估計(jì)的RMSEFig.1 RMSE of the relative position estimation

圖2 相對速度估計(jì)的RMSEFig.2 RMSE of the relative velocity estimation

圖4 相對姿態(tài)估計(jì)的RMSEFig.4 RMSE of the relative attitude estimation

圖5 相對角速度估計(jì)的RMSEFig.5 RMSE of the relative angular velocity estimation

由圖可知,L-NLKF與MSSRCKF誤差收斂后的RMSE基本一致。

為了定量描述濾波算法在總體上的精度,定義航天器相對位置、相對速度、目標(biāo)質(zhì)心位置與相對角速度的平均RMSE(Time-averaged RMSE,TARMSE),如式(41)所示

(41)

表2 濾波算法性能比較(情況1)Table 2 Performance comparison of filtering algorithms(case 1)

由于航天器間相對角速度的大小對濾波估計(jì)的影響較大,考慮目標(biāo)快速旋轉(zhuǎn)情況,將相對角速度初值擴(kuò)大100倍,即ω0=[1.32,1.32,2.64]T×10-1rad/s,其余初值不變,再進(jìn)行一組蒙特卡洛仿真。統(tǒng)計(jì)結(jié)果如表3所示。

表3 濾波算法性能比較(情況2)Table 3 Performance comparison of filtering algorithms(case 2)

對比表2、表3可知,與航天器間相對角速度接近于零(無相對轉(zhuǎn)動)相比,目標(biāo)快速旋轉(zhuǎn)時,相對位置與目標(biāo)形心位置的估計(jì)精度顯著提升。這是由于目標(biāo)快速旋轉(zhuǎn)時,目標(biāo)形心與質(zhì)心的偏差矢量也在追蹤航天器的觀測中發(fā)生明顯變化,即更易對相關(guān)狀態(tài)變量進(jìn)行測量。

定義濾波器單步平均運(yùn)算時間為

(42)

計(jì)算得到L-NLKF的單步平均運(yùn)算時間為1.2 ms;MSSRCKF的單步平均運(yùn)算時間為2.2 ms,即L-NLKF的單步計(jì)算時間僅是MSSRCKF的55%。結(jié)合表2、3中的數(shù)據(jù)可知,在非合作目標(biāo)相對追蹤航天器低速、高速旋轉(zhuǎn)的兩種情況下,L-NLKF均能在保證估計(jì)精度的基礎(chǔ)上,大幅縮短計(jì)算時間。

5 結(jié) 論

本文提出了L-NLKF算法,進(jìn)行了理論推導(dǎo),提高了計(jì)算效率。主要工作包括以下幾個方面:

1) 根據(jù)星上量測設(shè)備獲取的目標(biāo)形心的相對位置與相對姿態(tài)信息,建立了非合作航天器相對運(yùn)動的軌道-姿態(tài)一體化模型。相較以往的研究,該模型維度較低、更加便于計(jì)算。

2) 針對非線性濾波過程中,狀態(tài)方程包含部分線性項(xiàng)的問題,將KF與MSRRCKF進(jìn)行了融合,提出了L-NLKF。該算法能夠?qū)顟B(tài)方程中的線性項(xiàng)與非線性項(xiàng)分別用不同的濾波方法進(jìn)行處理,同時利用分塊的方式降低矩陣運(yùn)算的維度顯著提升了計(jì)算效率。

3) 航天器相對運(yùn)動的仿真實(shí)驗(yàn)對比表明,本文提出的L-NLKF在保證了濾波精度的前提下,計(jì)算時間明顯少于MSSRCKF。