基于Lawden改进型方程的编队Unscented Kalman Filter滤波估计

温 洲 邵晓巍 陶久亮 龚德仁

1.上海交通大学航空航天学院，上海200240

2.中国航天科技集团公司第一研究院，北京100076

卫星编队是近年来国内外航天领域兴起的热点研究方向之一，它是将多颗卫星组合起来，并在一定空间范围内周期性相互运动的卫星飞行模式。相对于单颗大卫星而言，卫星编队具有独特的优势:相对低廉的成本和维护，更低的更替成本，风险更小，以及多个任务的协同完成。卫星编队不仅可以提高执行太空任务的可能，同时也将开辟许多新的研究应用领域。

目前，卫星编队主要应用于合成孔径雷达(Synthetic Aperture Radar，SAR)来进行地面的高精度测绘和地面动目标跟踪(Ground Motion Target Indicator，GMTI)、分布式气象卫星三位立体成像以及高分辨率合成孔径光学干涉技术等。目前国内外运行或即将运行的卫星编队有德国“TanDEM －X”双星编队系统，瑞典棱镜“Prisma”双星编队系统、美国“A－Train”卫星编队系统以及中国的“神七”伴飞小卫星编队系统等［1－2］。

在卫星编队任务中，很多关键技术的解决都依赖于编队卫星间相对状态的确定，即编队相对位置和相对速度的滤波估计，如编队队形保持技术、多星姿态协同技术、防碰撞技术以及航天器对接技术等。所以高精度的编队相对运动状态对编队任务的顺利完成特别重要。在1996年，Garrison 提出了采用EKF(Extended Kalman Filter)进行卫星编队飞行的相对导航设计方法，以及Eric A，Wan 等学者对2002年出现的UKF 算法进行了相关研究［3］，并应用于卫星编队飞行导航中，当然还有很多其他的滤波估计方法也适用于卫星编队的滤波估计，如文献［4］中提出的有限域RKF 滤波方法以及文献［5］中对UKF 进行改进得到的自适应UKF 滤波算法等。

在编队的实际运行环境中，由于受到各种摄动的影响，比如地球扁状，大气流动，太阳或者月球的引力影响等，其运行轨迹是时变的，其于运动方程将呈现非线性的。如果用线性的Lawden 方程来描述编队相对运动方程，将会产生一定的误差，导致编队相对状态估计的精确度不够高，从而对编队的相对导航及其他任务等产生一定的影响。

本文基于太空任务中对编队相对状态的高精度要求，结合编队的非线性运动性质，基于Lawden 方程改进得到编队非线性运动方程，即Lawden 改进型方程，并采用适合非线性系统的UKF 滤波方法对编队状态进行了滤波估计，仿真结果表明在基于Lawden 改进型方程下，相对于EKF 滤波方法，UKF滤波能将编队的相对距离估计精度提高70%左右，相对速率估计精度提高17%左右，具有一定的理论研究和工程应用参考价值。

1 Lawden 方程介绍

Lawden 方程是简化的编队相对运动方程。首先建立主星的地心轨道坐标系，如下图1 所示，坐标系OeXeYeZe为赤道惯性坐标系，Xe轴由地心指向春分点γ，Ze轴垂直于赤道平面，Ye轴由右手定则确定。

图1 双星编队坐标系示意图

OeXcYcZc为主星的地心轨道坐标系，其中Xc轴由地心指向卫星瞬时时刻状态点，Zc轴垂直于主星轨道平面，与卫星动量矩矢量Hc同向，Yc轴由右手定则确定，并沿着卫星速度方向，其单位向量为(uxuyuz)。Sc代表主星，Sd代表辅星。

rc，rd分别为主星和辅星在惯性坐标系OeXeYeZe下的位置矢量。令星间相对距离位置矢量ρ=rd－rc。根据卫星轨道运动，得到编队的相对运动方程:

根据式(1)可以推导出编队的相对运动方程［6－7］:

当星间相对距离满足max(x，y，z)＜＜r 时，其中r 为主星到地心的距离，式(2)可简化为:

其中

以及

AL为时变系数矩阵，是真近点角f 的函数。可以判断Lawden 方程并非完全意义上的定常线性运动方程，而是某一时间点的瞬时线性运动方程，也可称为单点线性运动方程。

关于Lawden 方程的解析解，有Carter 提出的解法［8］，以及Yamanaka 和Ankersen 提出的解法［9］，还有Broucke 提出的解法等大约五六种不同的编队相对状态求解解法［10］。

2 Lawden 改进型相对运动方程

可由式(1)得到另一种编队的非线性描述方程:

其中

转换成状态空间形式:

或者为

Lawden 改进型方程在充分考虑了各种摄动影响的前提下，将过去时刻的位置状态变量融入相对运动方程中，从而实现编队状态的非线性描述，相比于Lawden 描述方程，更具有非线性和实时特性。

3 编队状态UKF 滤波估计

在进行编队的状态滤波估计方法上，普遍采用的是EKF 滤波算法。EKF 主要是用线性模型近似编队的实时非线性运动模型，无法对实际的非线性编队运动模型进行滤波估计。然而基于本文提出的Lawden 改进型方程，由于其非线性的特性，能够很好的描述编队的实时运动，采用EKF 滤波方法必然会忽略掉其非线性的特性，导致无法较好的对编队状态进行精确估计。

在非线性系统中，UKF 滤波方法通过粒子滤波器产生一定数量的状态采样点，然后利用采样点的分布逼近系统状态分布，完全适合于非线性系统，所以本文采用UKF 滤波算法来进行编队状态的滤波估计。

3.1 UKF 滤波算法基本介绍

UKF 和EKF 的基本框架是大致相同的，主要的区别在于EKF 用线性模型近似系统的非线性模型，而UKF 主要用粒子滤波器来产生一组采样点，然后利用采样点的分布近似逼近系统状态的分布，适合于非线性的卫星编队状态滤波估计。

根据文献［10 －11］关于UKF 的分析研究，可以用如下图2 所示的流程图清晰地表达出来。

图2 UKF 滤波流程图

3.2 UKF 初始参数确定

根据图2 的UKF 流程图，首先需要设定UKF滤波系统的初始参数值:

4 编队状态滤波仿真

根据Lawden 改进型方程，经过离散化的变形，得到单步相对运动方程，由式(10)得到:

其中L=I+Δt·A 为过程矩阵，Δt 为单步运行时间，I 为单位矩阵，D=Δt·C 为单步伪输入量，即D 并非是真是意义上的输入量，而是类似于输入控制量U，也可以认为是单步反馈控制量。

利用仿真来检验Lawden 改进型方程的特性，首先设定仿真参数，如表1 和表2 所示。本文采用双星编队，基于Lawden 改进型方程来进行编队状态EKF 滤波和UKF 滤波估计仿真实验。

表1 主星的轨道根数和编队相关初始值设定

表2 双星编队初始相对状态

4.1 EKF 滤波仿真

基于给定的编队初始条件和轨道参数，经过过程估计和观测估计，即可求得编队的EKF 状态滤波估计值，其位置和速度估计值与真实值的误差分别如图3 和图4 所示。

图3 EKF滤波的三轴向位置估计误差

图4 EKF滤波的三轴向速度估计误差

从上面的仿真结果可以得出，基于Lawden 改进型方程，利用EKF 滤波估计的编队相对距离和相对速率的精度分别为3.0m 和2.8 ×10－3m/s 左右，其中在X，Y，Z 轴向位置分量的相对精度分别为0.067%，0.2%，0.2%，速度分量的相对精度分别为0.36%，0.03%，0.2%。

4.2 UKF 滤波仿真

在设定完系统参数值之后，基于表1 和表2 的数据和图2 的UKF 滤波步骤，通过过程估计和观测估计可以得到编队的UKF 状态滤波估计值，其位置和速度估计值与真实值的误差如图5 和图6所示。

图5 UKF滤波的三轴向位置估计

图6 UKF滤波的三轴向速度估计

从上面的结果可以得出，基于Lawden 改进型方程，利用UKF 滤波估计的编队相对距离和相对速率的绝对精度分别大约为0.8m 和2.0 ×10－3m/s，其中在X，Y，Z 轴向位置分量的相对精度分别为0.027%，0.08%，0.08%，速度分量的相对精度分别为0.3%，0.023%，0.15%。

4.3 EKF 滤波与UKF 滤波估计效果比较

综合比较EKF 和UKF 滤波的编队状态估计精度，其位置和速度分量以及绝对相对距离的估计误差比较如图7 和图8 所示。

图7 编队相对距离的估计误差

图8 编队Z向相对速率的估计误差

显然，在基于Lawden 改进型方程的编队状态估计中，相对于EKF 滤波方法，采用UKF 滤波方法可以得到精度更高的状态估计值，由上图可以大致计算得到在相对距离估计方面，相对于EKF 滤波方法，UKF 可以提高70%的估计精度，而在相对速率估计方面，可以提高25%的估计精度，所以总体而言在编队状态估计中，UKF 滤波方法优于EKF 滤波方法。

5 结论

在编队相对运动模型中，经过非线性运动方程的简化可以得到线性化的Lawden 编队描述方程，但是此简化过程忽略了编队运行的非线性特性，导致描述不精确。本文基于Lawden 描述方程，提出了非线性的Lawden 改进型方程，使其更符合描述编队的非线性运动特性。

然后在编队状态的滤波估计方面，综合分析比较了EKF 与UKF 的特点，其中EKF 常用于可以线性化的编队系统模型，而UKF 适合于非线性系统模型，无需将系统进行线性化改造。这将有利于对无法线性化的系统进行分析。

最后利用EKF 和UKF 滤波方法分别对编队状态进行滤波估计仿真。由仿真结果得到，利用UKF滤波方法得到的编队状态估计精度明显好于采用EKF 滤波方法得到的估计精度，其中相对距离的精度可以提高70%左右，相对速率的精度可以提高25%左右。这也证明了Lawden 改进型方程能够很好的描述非线性的编队运动特性，而且利用UKF 滤波方法可以更加精确地对编队状态进行滤波估计，有利于满足编队的相对导航等任务的高精确状态需求，具有一定的理论和工程参考利用价值。

［1］刘 明.小卫星编队飞行相对运动与队形保持控制研究［D］.上海交通大学，2007.

［2］郝继刚.分布式卫星编队构形控制研究［D］.国防科学技术大学，2006.

［3］Eric A Wan，Rudolph van der Merwe. The Unscented Kalman Filter for Nonlinear Estimation［J］. Adaptive systems for signal Processing，Communications，and Control Symposium 2000，As － SPCC. The IEEE 2000，58(7):153-158.

［4］Zhe Dong，Zheng You. Finite-Horizon Robust Kalman Filtering for Uncertain Discrete Time-Varying Systems With Uncertain-Covariance White Noises［J］.IEEE Signal Processing Letters，2006，13(8).

［5］SONG Qi，HAN Jian-Da. An Adaptive UKF Algorithm for the State and Parameter Estimations of a Mobile Robot［J］.Acta Automatica Sinica，2008，34(1):72-79.

［6］章仁为.卫星轨道姿态动力学与控制［M］. 北京:北京航空航天大学出版社，2006.

［7］陶久亮，邵晓巍，等.基于鲁棒卡尔曼滤波的编队卫星相对导航. 航天控制，2011，29(5):8-14. (TAO Jiuliang，SHAO Xiaowei，et al. Relative Navigation for Spacecraft Formation Flying Based on Robust Kalman Filter［J］.Aerospace Control，2011，29(5):8-14.)

［8］Carter T E. State Transition Matrices for Terminal Rendezvous Studies:Brief Survey and New Examples［J］.Journal of Guidance，Control，and Dynamics，1998，21(1):148-155.

［9］Yamanaka K，Ankersen F. New State Transition Matrix for Relative Motion on an Arbitrary Elliptical Orbit［J］.Journal of Guidance，Control，and Dynamics，2002，25(1):60-66.

［10］Kyle T，Alfriend，Srinivas R，Vadali，Pini Gurfil，Jonathan P，How，Louis S，Breger. Spacecraft Formation Flying Dynamics，control and nabigation［M］. Elsevier Astrodynamics Series，2010.

［11］吴江飞，黄珹.分布逼近的卡尔曼滤波及其在星载卫星定轨中的应用［J］. 天文学报，2005，46(1):55-60.(Wu Jiang-fei，Hunag Cheng.Unscented Kalman Filter and Its Application In GPS-Based Satellite Orbit Determination［J］.Acta Astronomica Sinca，2005，46(1):55-60.)