基于UKF 的大氣層外飛行器光學衛星跟蹤濾波方法*

唐毓燕，江涌

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

0 引言

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

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

1 濾波坐標系選取及轉換關系

1.1 濾波坐標系選取與定義

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

坐標系。

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

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

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

坐標系Se與坐標系So之間的幾何關系如圖1 所示，圖中Ω 為衛星軌道升交點赤經，u 為衛星軌道緯度幅角，i 為衛星軌道傾角。

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

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

圖2 衛星軌道坐標系與衛星本體坐標系間幾何關系（231 轉序）Fig.2 Geometric relationship between satellite orbit coordinate system and satellite body coordinate system（231 transfer order）

1.2 坐標系轉換

（1）坐標系Se轉換至坐標系So

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

（2）坐標系So轉換至坐標系Ss

按照231 轉序，k 時刻衛星本體坐標系Ss下目標的位置矢量為

2 大氣層外飛行器光學衛星跟蹤濾波模型

2.1 大氣層外飛行器軌跡濾波狀態方程

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

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

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

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

2.2 光學衛星跟蹤量測方程

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

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

于是，n（n≥1）顆觀測衛星的聯合量測方程為

2.3 濾波遞推方程

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

（1）設定濾波初值

假設濾波的初始狀態估計和估計方差為

（2）比例修正對稱采樣

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

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

（3）狀態更新

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

一步狀態預測的均值和協方差矩陣為

（4）量測更新

根據狀態更新得到的點集γik+1|k，經過非線性量測函數傳遞可得

量測變量一步預測均值、方差及協方差如下：

（5）濾波遞推公式

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

2.4 濾波初值計算方法

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

（1）構造t1時刻觀測矢量及其協方差矩陣

構建t1時刻觀測矢量及其協方差矩陣

（2）構造t1時刻采樣點集

（3）將構造的采樣點集由衛星本體坐標系Ss轉換至地心赤道慣性坐標系Se

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

然后轉換至坐標系So中，有

然后轉換至坐標系Se中，有

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

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

式中：

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

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

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

（5）計算初始狀態矢量X?0|0

（6）計算初始狀態矢量的協方差矩陣P0|0

3 典型場景仿真分析

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

圖3 3 顆紅外光學衛星運行軌道與目標飛行器飛行軌跡幾何關系Fig.3 Geometric relationship between the orbits of three infrared optical satellites and the track of the target vehicle

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

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

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

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

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

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

4 結束語

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