唐毓燕，江涌

（1. 北京電子工程總體研究所，北京 100854；2. 中國航天科工防御技術(shù)研究院，北京 100854）

0 引言

隨著飛行器技術(shù)與探測跟蹤技術(shù)的不斷發(fā)展，世界強國積極探索構(gòu)建光學(xué)監(jiān)視跟蹤星座［1-2］，用于對高速飛行器進行探測跟蹤，獲取飛行器高精度位置、速度等測量信息。

根據(jù)濾波估計理論，對于高速飛行器跟蹤濾波，常用濾波方法有擴展卡爾曼濾波（extended Kalman filter，EKF）［3］、迭代濾波［4］、高斯和濾波［5］、有界最佳濾波［6］、高階 EKF 濾波［7］、變增益 EKF 濾波［8］、無 跡 卡 爾 曼 濾 波（unscented Kalman filter，UKF）［9］等，在光學(xué)衛(wèi)星觀測應(yīng)用背景下，量測方程的非線性很強，普通濾波方法在進行線性化處理時將引入較大的模型誤差，從而影響濾波效果，而UKF 濾波的核心思想為采用滿足給定概率分布的特定采樣點集通過非線性狀態(tài)方程和觀測方程，得到預(yù)測值與估計值的分布特性，從而避開了對非線性狀態(tài)方程或觀測方程進行線性化近似處理，估計精度與穩(wěn)定性均得以提高。本文重點研究基于UKF 的大氣層外飛行器光學(xué)衛(wèi)星跟蹤濾波方法，解決雙星/多星對大氣層外飛行器的光學(xué)高精度跟蹤濾波估計問題。

1 濾波坐標系選取及轉(zhuǎn)換關(guān)系

1.1 濾波坐標系選取與定義

為了簡化濾波狀態(tài)方程與量測方程表述形式，選取在地心赤道慣性坐標系進行光學(xué)衛(wèi)星對大氣層外飛行器跟蹤濾波，濾波量測方程表述模型同時涉及衛(wèi)星軌道坐標系、衛(wèi)星本體坐標系2 個

坐標系。

地心赤道慣性坐標系（Se）：原點Oe位于地心；OeXeYe平面為赤道面；OeXe軸指向春分點；OeZe軸與地球自轉(zhuǎn)軸重合，指向北天極；OeYe軸由右手螺旋法則確定。

衛(wèi)星軌道坐標系（So）：原點Os位于衛(wèi)星質(zhì)心；OsYo軸為地心向徑，遠離地心方向為正；OsXo軸在軌道面內(nèi)垂直于OsYo軸，指向運動方向為正；OsZo軸由右手螺旋法則確定。

衛(wèi)星本體坐標系（Ss）：原點Os位于衛(wèi)星質(zhì)心；OsXs軸與衛(wèi)星軸線重合，指向前方為正；OsYs軸位于衛(wèi)星縱對稱面內(nèi)，指向上方為正；OsZs軸由右手螺旋法則確定。

坐標系Se與坐標系So之間的幾何關(guān)系如圖1 所示，圖中Ω 為衛(wèi)星軌道升交點赤經(jīng)，u 為衛(wèi)星軌道緯度幅角，i 為衛(wèi)星軌道傾角。

圖1 地心赤道慣性坐標系與衛(wèi)星軌道坐標系間幾何關(guān)系Fig.1 Geometric relationship between geocentric equatorial inertial coordinate system and satellite orbit coordinate system

坐標系So與坐標系Ss之間的幾何關(guān)系如圖2 所示，圖中 ψs、θs、γs分別為光學(xué)衛(wèi)星的偏航角、俯仰角、滾轉(zhuǎn)角。偏航角定義為衛(wèi)星本體坐標系的OsXs軸在衛(wèi)星軌道坐標系OsXoZo平面上的投影與衛(wèi)星軌道坐標系OsXo軸之間的夾角，由OsXo軸逆時針方向旋轉(zhuǎn)到OsXs軸投影線偏航角為正；俯仰角定義為衛(wèi)星本體坐標系OsXs軸和衛(wèi)星軌道坐標系OsXoZo平面的夾角，由OsXoZo平面向上逆時針旋轉(zhuǎn)至OsXs軸時俯仰角為正；滾轉(zhuǎn)角定義為衛(wèi)星本體坐標系OsYs軸與通過OsXs軸的當?shù)劂U垂面之間的夾角，逆OsXs軸看，OsYs軸逆時針轉(zhuǎn)向鉛垂面時滾轉(zhuǎn)角為正。

圖2 衛(wèi)星軌道坐標系與衛(wèi)星本體坐標系間幾何關(guān)系（231 轉(zhuǎn)序）Fig.2 Geometric relationship between satellite orbit coordinate system and satellite body coordinate system（231 transfer order）

1.2 坐標系轉(zhuǎn)換

（1）坐標系Se轉(zhuǎn)換至坐標系So

設(shè)k 時刻地心赤道慣性坐標系Se下目標位置矢量為Rte，k，k 時刻光學(xué)衛(wèi)星在地心赤道慣性坐標系Se下的位置和速度矢量分別為 Rse，k和 Vse，k，則 k 時刻衛(wèi)星軌道坐標系So下目標位置矢量為

（2）坐標系So轉(zhuǎn)換至坐標系Ss

按照231 轉(zhuǎn)序，k 時刻衛(wèi)星本體坐標系Ss下目標的位置矢量為

2 大氣層外飛行器光學(xué)衛(wèi)星跟蹤濾波模型

2.1 大氣層外飛行器軌跡濾波狀態(tài)方程

選擇地心赤道慣性坐標系Se中的目標飛行器質(zhì)心位置矢量 Rte= (x，y，z)T與速度矢量 Vte= (x?，y?，z?)T組成濾波估計的狀態(tài)矢量，有

這里選用CV 模型，則k+1 時刻大氣層外飛行器軌跡濾波狀態(tài)方程可表示為

式中：Xk、Xk+1分別為 k、k+1 時刻的狀態(tài)矢量；Φ 為狀態(tài)變換矩陣；Γ 為確知加速度矢量變換矩陣；G 為過程噪聲變換矩陣；Ak為k 時刻目標飛行器確知的加速度矢量（即重力加速度矢量）；Wk= (ξx，ξy，ξz)T為 k 時刻過程噪聲矢量（ξx、ξy、ξz分別為服從方差為σx、σy、σz的零均值高斯白噪聲變量），相應(yīng)表示形式分別為

其 中 ，Rte，k= (xk，yk，zk)T為 k 時 刻 坐 標 系 Se中 目 標 飛行器位置矢量，T 為時間步長，g0= 9.806 65 m/s2為地球表面重力加速度標準值，Re＝6 371 km 為地球平均半徑。

2.2 光學(xué)衛(wèi)星跟蹤量測方程

設(shè)k 時刻目標飛行器在觀測衛(wèi)星本體坐標系Ss中的方位角、俯仰角分別為 βts，k、εts，k，目標方位角定義為目標視線在衛(wèi)星本體坐標系OsXsZs平面上的投影與衛(wèi)星本體坐標系OsXs軸之間的夾角，由OsXs軸逆時針方向旋轉(zhuǎn)到目標視線投影線方位角為正；目標俯仰角定義為目標視線和衛(wèi)星本體坐標系OsXsZs平面的夾角，由OsXsZs平面向上逆時針旋轉(zhuǎn)至目標視線時俯仰角為正。則有

式中：Rts，k為k 時刻目標飛行器在衛(wèi)星本體坐標系Ss中的位置矢量，可由式（4）計算得到。

于是，n（n≥1）顆觀測衛(wèi)星的聯(lián)合量測方程為

2.3 濾波遞推方程

根據(jù) UKF 濾波算法［9］，以比例修正對稱采樣策略選取采樣點，利用UT 變換理論，可以得到濾波遞推方程。

（1）設(shè)定濾波初值

假設(shè)濾波的初始狀態(tài)估計和估計方差為

（2）比例修正對稱采樣

n 維（n=6）狀態(tài)矢量 X 的均值和方差分別為 Xˉ和Px，則2n + 1 個比例修正對稱采樣點為

式中：λ = α2(n + s) - n，α 為比例縮放因子，0≤α≤1，控制α 的值可以控制采樣點集的范圍，通常設(shè)置為很小的正數(shù)；β 反映狀態(tài)歷史信息的高階特性，對于高斯分布 β=2 最優(yōu)；s 為狀態(tài)矢量 X 的均值 Xˉ和采樣點距離的比例因子，其取值的大小直接反映二階以(n + λ)Px經(jīng)過Cholesky 分解求得的平方根矩陣的第 i 行的轉(zhuǎn)置矢量，當 X 為單變量時，s = 0，當 X 為多變量時，s = 3 - n。

（3）狀態(tài)更新

假設(shè)k 時刻的狀態(tài)估值為X?k|k，估計協(xié)方差矩陣為Pk|k，由比例修正對稱采樣策略，可得到2n+1 個采進行狀態(tài)函數(shù)傳遞可得

一步狀態(tài)預(yù)測的均值和協(xié)方差矩陣為

（4）量測更新

根據(jù)狀態(tài)更新得到的點集γik+1|k，經(jīng)過非線性量測函數(shù)傳遞可得

量測變量一步預(yù)測均值、方差及協(xié)方差如下：

（5）濾波遞推公式

根據(jù)k+1 時刻的量測值Zk+1，可求出濾波增益Kk+1，k+1 時刻狀態(tài)估計和估計方差為

2.4 濾波初值計算方法

濾波初始值設(shè)定即為確定初始時刻的狀態(tài)矢量X?0|0及其協(xié)方差矩陣P0|0。假設(shè)已得到2 顆光學(xué)衛(wèi)星分別在t1、t2時刻對目標飛行器的測量信

（1）構(gòu)造t1時刻觀測矢量及其協(xié)方差矩陣

構(gòu)建t1時刻觀測矢量及其協(xié)方差矩陣

（2）構(gòu)造t1時刻采樣點集

（3）將構(gòu)造的采樣點集由衛(wèi)星本體坐標系Ss轉(zhuǎn)換至地心赤道慣性坐標系Se

首先根據(jù)觀測值計算坐標系Ss中對目標飛行器觀測方向矢量，有

然后轉(zhuǎn)換至坐標系So中，有

然后轉(zhuǎn)換至坐標系Se中，有

（4）計算t1、t2時刻坐標系Se中目標飛行器位置矢量

下面根據(jù)t1時刻1 顆衛(wèi)星坐標系 Se中位 置 矢2 顆衛(wèi)星坐標系 Se中位置矢量解坐標系Se中目標飛行器位置矢量估計值。根據(jù)2 條異面直線公垂線2 個垂足計算方法，可以得到坐標系Se中t1時刻對應(yīng)的2 個垂足點位置矢量 RMe，1、RNe，1為

式中：

于是，可得t1時刻坐標系Se中目標飛行器位置矢量為

同理，可得到t2時刻坐標系Se中目標飛行器位置矢量為

則坐標系Se中目標飛行器位置矢量傳遞點集為

（5）計算初始狀態(tài)矢量X?0|0

（6）計算初始狀態(tài)矢量的協(xié)方差矩陣P0|0

3 典型場景仿真分析

設(shè)有3 顆紅外光學(xué)衛(wèi)星在地球圓軌道運行，軌道高度 500 km，軌道傾角 60°，A 星與 B 星在同一軌道運行且彼此相位夾角15°，C 星與A/B 星升交點赤經(jīng)相差30°且相位與A 星相同，3 顆衛(wèi)星在深空背景下觀測大氣層外飛行器。光學(xué)衛(wèi)星測角系統(tǒng)誤差50 μrad，測角隨機誤差 50 μrad（1σ）。大氣層外一高速飛行器以7 km/s 的初始速度沿拋物線軌跡飛行，3 顆紅外光學(xué)衛(wèi)星運行軌道與目標飛行器飛行軌跡幾何關(guān)系如圖3 所示，3 顆紅外光學(xué)觀測衛(wèi)星觀測目標飛行器距離演變情況如圖4 所示。

圖3 3 顆紅外光學(xué)衛(wèi)星運行軌道與目標飛行器飛行軌跡幾何關(guān)系Fig.3 Geometric relationship between the orbits of three infrared optical satellites and the track of the target vehicle

圖4 3 顆紅外光學(xué)觀測衛(wèi)星觀測目標飛行器距離演變情況Fig.4 Variation of the distances between three infrared optical satellites and the target vehicle

仿真開始約183 s，A 星與C 星發(fā)現(xiàn)目標飛行器，約4 s 后建立雙星跟蹤，跟蹤數(shù)據(jù)率5 Hz，約304 s 時B 星觀測到目標飛行器并加入聯(lián)合跟蹤，跟蹤濾波位置、速度估計誤差如圖5 所示。從圖中可以看出，雙星/多星位置UKF 濾波估計誤差優(yōu)于雙星直接測量誤差，具體大小與衛(wèi)星觀測目標距離、測角系統(tǒng)誤差、雙星/多星觀測幾何構(gòu)型等因素有關(guān)；雙星UKF 濾波約30 s 后速度誤差收斂至10 m/s 以內(nèi)。

圖5 3 顆紅外光學(xué)衛(wèi)星對大氣層外飛行器跟蹤濾波誤差Fig.5 Filtering error of three infrared optical satellites tracking an extraatmospheric vehicle

經(jīng)500 次蒙特卡羅仿真統(tǒng)計，UKF 濾波相比于傳統(tǒng)需線性化處理的EKF 濾波效果對比情況如圖6所示。從圖中可以看出，對于光學(xué)衛(wèi)星觀測大氣層外飛行器應(yīng)用場景，量測方程非線性較強，采用EKF 濾波進行線性化處理將帶來模型誤差導(dǎo)致濾波精度一定程度降低，因此，在本文應(yīng)用場景下UKF 濾波精度要一定程度上優(yōu)于EKF 濾波。

圖6 UKF 濾波相比于EKF 濾波效果對比情況Fig.6 Comparison of the filtering error of UKF and EKF

進一步仿真表明，本文提出的濾波方法適用于大氣層外慣性高速飛行器或過載不超過0.5 的小幅機動飛行器的光學(xué)衛(wèi)星跟蹤，如涉及大過載機動飛行情形，則濾波狀態(tài)方程可選用“當前”統(tǒng)計模型［10］獲得更好的跟蹤效果。

4 結(jié)束語

本文考慮飛行器技術(shù)與探測跟蹤技術(shù)快速發(fā)展的應(yīng)用背景，提出了一種基于UKF 的大氣層外飛行器光學(xué)衛(wèi)星跟蹤濾波方法，解決了合理觀測距離情況下對大氣層外飛行器光學(xué)衛(wèi)星高精度跟蹤問題，可實現(xiàn)30 s 內(nèi)速度估計誤差快速收斂，適用于飛行器過載不超過0.5 的大氣層外高速飛行情形，算法有效性通過了仿真驗證，與EKF 濾波相比具有更高的跟蹤精度。