基于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= (ẋ，ẏ，ż)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 滤波相比具有更高的跟踪精度。