盧 山，張世源

(1. 上海航天控制技術研究所，上海 201109；2. 上海市空間智能控制技術重點實驗室，上海 201109)

0 引 言

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

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

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

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

坐標系定義如下:

OI-XIYIZI:地心慣性坐標系FI。原點OI為地心,XI位于赤道平面內,指向春分點,ZI沿地球自轉軸方向,向上為正,YI軸與XI軸和ZI軸構成右手直角坐標系。

OO-XOYOZO:第二軌道坐標系FO。原點OO位于航天器質心,ZO軸指向地心,XO軸在軌道平面內垂直于ZO軸指向航天器飛行方向,YO軸與XO軸和ZO軸構成右手直角坐標系。根據該定義可以分別得到追蹤星第二軌道坐標系FCO與目標星第二軌道坐標系FTO。

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

1.1 軌道相對運動模型

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

(1)

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

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

(2)

(3)

(4)

(5)

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

Xk+1=ΦXk+Γwk

(6)

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

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

(7)

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

(8)

ω=ωIC-ωIT

(9)

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

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

(10)

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

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

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

(11)

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

(12)

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

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

2) 觀測方程:

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

(13)

建立觀測方程

(14)

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

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

(15)

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

MSSRCKF的求積節(jié)點設置為:

(16)

(17)

對應的權重系數為:

(18)

1) 時間更新

(19)

2) 量測更新

(20)

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

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

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

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

(21)

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

(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)方程的特殊非線性形式進行設計,只需對MSSRCKF算法的時間更新方式進行改進,故本節(jié)僅給出L-NLKF算法時間更新的計算步驟:

1) 利用KF進行線性狀態(tài)預測

(24)

2) 利用MSSRCKF進行非線性狀態(tài)預測

(25)

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

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

(26)

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

(27)

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

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

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

(28)

(29)

(30)

(31)

(32)

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

(33)

對x1,k進行確定性采樣時,求積節(jié)點ζi,k應滿足條件如下:

(34)

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

(35)

(36)

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

(37)

6) 合并線性項與非線性項

(38)

(39)

4 仿真校驗

表1 航天器參數設置Table 1 Spacecraft parameter setting

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

(40)

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

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

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

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

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

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

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

(41)

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

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

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

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

定義濾波器單步平均運算時間為

(42)

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

5 結 論

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

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

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

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