杨冬，宋才水，王鹏

(北京电子工程总体研究所，北京 100854)

0 引言

随着科技的发展和人类空间活动的增加，开展空间科学研究的重要性已越来越突出。为获取不同高度上空间环境参数垂直分布的科学数据，火箭的探测高度也由数百千米提升到万千米以上。

由于卫星、高空火箭等中高轨目标受力情况复杂，精确的二阶运动微分方程的解析解难以求得，所以数值解法是解决轨道积分问题的主要手段[1]。Runge-Kutta法和Adams-Cowell法是求解卫星运动微分方程时最常用的数值积分算法[1]，优点是预报精度高，缺点是计算复杂，实时性不好[1-2]。对比卫星，考虑到高空探测火箭在轨飞行时间短、机动大的特点，通常应用于卫星精密定轨的传统数值积分理论难以实现，其对定位算法的快速实时性有着更高需求。

高空探测火箭在3 000 km高度以下时，GPS卫星导航可实现高精度定轨，而3 000 km以上，GPS的应用还未实现，因此，大多通过地基多站测距、箭上自主定位解算方式进行导航[3]。

卡尔曼滤波是非线性系统实现快速估计的有效方法[4-6]，已经被越来越多地应用于动态定位数据处理、实时或准实时的定轨需求，尤其是GPS动态数据处理、惯性导航等。但由于具有依赖于初始状态估计并受测量噪声影响大等缺点，估计过程中协方差易出现病态，从而会导致滤波结果的不稳定。设计好卡尔曼滤波算法的关键是正确描述观测误差与模型误差的统计特性，导航系统误差模型越准确，算法精度越高。若建立的函数模型和误差模型与实际状态存在误差，不仅得不到最优估计，而且可能造成滤波发散。

本文提出了一种实时高精度定位解算的方法，采用试验数据和误差辨识，对地基导航系统误差建立较准确的模型，结合基于轨道动力学的扩展卡尔曼滤波算法，实现全程实时高精度定轨。该方法省去了轨道积分过程，提高了定位精度和解算效率，可以很好的应用在箭上自主解算。

1 常规基于最小二乘的单历元解算算法

1.1 定位原理

通过测量5个(及以上)地面导航站到导航接收机的距离，便可联立方程组(见式(1))解算出导航接收机的位置。

(1)

式中:Ri(i=1，2，3，4，5)为导航接收机测得到各地面导航站的伪距；(x，y，z)为导航接收机的位置坐标；(xi，yi，zi)为地面导航站的站址坐标；c为光速；Δt为导航接收机钟差。

(2)

记

可将线性化后的方程记为

B=A·X.

(3)

利用最小二乘法[7]，可得其解为

X=(ATA)-1ATB.

(4)

1.2 定位精度

设各站的伪距测量误差为ε，则可将定位定速解算方程进一步写为

B=AX+ε.

(5)

(ATA)-1AT(AX+ε)-X=

(ATA)-1ATε.

(6)

设Covε=EεεT=R，可得解算误差的协方差阵为

Cov(ΔX)= ΔXΔXT=[(ATA)-1ATε]·

[(ATA)-1ATε]T=

(ATA)-1ATRA(ATA)-1.

(7)

如果伪距测量误差ε的分量互不相关且有同样的方差σ2时，R=σ2I，误差协方差可简化为

(8)

总的定位解算误差可表示为

(9)

常采用(ATA)-1前3个对角项之和的平方根作为空间位置精度因子[8](以下简称PDOP)。在实际应用中，PDOP的大小取决于测量站分布与目标的相对位置，受限于国内布站基线长度[9]，随着高空

探测火箭的高度逐渐增加，其与地面各测量站的几何关系受到破坏，几何精度因子PDOP迅速增大，从而使得定位精度降低[10-11]。在飞行高度达20 000 km时，经过计算机仿真，定位精度可达到km量级，典型轨道条件下的仿真结果如图1所示。

2 滤波解算模型

本节首先分析高空探测火箭受力情况，并利用试验数据及误差辨识工作很好的验证和完善了导航系统误差模型，从而建立动力学卡尔曼滤波解算模型。

2.1 受力情况分析

近地航天器的轨道一般分为主动段和被动段[12-13]。主动段是指从起飞到头体分离的一段，被动段包括自由段和再入段，其中自由段也称作真空段，航天器不受控制力作用而做无动力的抛体运动，此时，大气阻力与类阻力影响几乎没有，地球的自转形变摄动、地球反辐射压力、地球扁率摄动、月球扁率摄动和相对论效应摄动均可忽略不计。高空探测火箭所受作用力主要包括：地球质心引力(即二体问题作用力)、除质心外的非球形摄动力、太阳和月球引力、太阳辐射压力等。

这些作用力中，二体问题作用力是起支配作用的，其他作用力相对较小，统称为摄动力。摄动力的影响可以用摄动量级[14](与地球质心引力的比值)表示，根据摄动量级大小和摄动性质，由问题所需精度决定所需考虑的摄动力,如表1所示。

图1 位置解算误差曲线Fig.1 Error curve of position calculation

表1 摄动力量级Table 1 Magnitude of perturbation forces

2.2 建立状态方程

在分析空间飞行器的运动时，如果把地球看作匀质球体，并忽略其他摄动力的影响。这时，卫星与地球构成二体运动，即天体力学中的二体问题。根据牛顿万有引力定律，在惯性坐标系中，飞行器受到的引力为

(10)

由此可得卫星的加速度矢量为

(11)

但由于空间飞行器除受到地球的质心引力作用外，还受到各种摄动力的影响，这些摄动力将会使空间飞行器的实际运行轨道偏离由二体力学所确定的轨道，因此对其进行修正，从而建立飞行器的轨道摄动方程：

(12)

在分析天体对空间飞行器的引力作用时，常引入引力位函数R，使得

F1=grad(R).

(13)

其特性之一是其对3个坐标轴的导数分别等于单位质量的质点沿该坐标轴方向所受的力，或者说该导数等于单位质量沿3个坐标轴方向的加速度。

假设在地固直角坐标系下研究飞行器的运动，并结合式(12)将式(13)给出的轨道摄动方程写为分量形式：

(14)

式中:μ=GM为地心引力常数；R由各种摄动力的位函数组成，如果只考虑地球非球形引力摄动的影响，R可由式(15)给出：

(15)

式中:r为飞行器与地心间的距离；λ为飞行器星下点的地心经度；φ为飞行器星下点的地心纬度；Re为地球的平均赤道半径；Pn，Pnm为勒让德多项式。

根据式(14)和式(15)，并忽略摄动力位函数中的二阶以上项，可建立系统的状态方程如下：

(16)

简写为

).

(17)

离散化处理：X(n+1)=F(X(n))+W(n),

式中:W(n)为系统噪声矩阵，包括位置测不准、摄动力干扰造成的加速度误差。

W(n)=(0, 0, 0,σaxn+Δfx,σayn+Δfy,σazn+Δfz)T,

(18)

式中:σaxn,σayn,σazn为n时刻位置测不准产生的加速度方差。

(19)

式中:σxn为x向坐标测量方差；PDOPx为x向PDOP值；σr为径向距离测量方差，将根据试验数据进行误差辨识的结果取值。

根据摄动量级大小和摄动性质，在考虑摄动力影响时，取n时刻摄动力干扰产生的加速度方差为最大量级，为

Δfx=ax(n)×10-3,

Δfy=ay(n)×10-3,

Δfz=az(n)×10-3.

(20)

系统噪声方差阵为：Q(n)=E[W(n)W(n)T]，假设W(n)各项相互独立，均服从正态分布，则

Q(n)=diag[0，0，0，(σaxn+Δfx)2，

(σayn+Δfy)2，(σazn+Δfz)2],

(21)

式中:diag()表示对角线矩阵。

2.3 建立量测方程

根据地基导航系统的定位定速计算结果，建立量测离散方程如下：

Y(n)=HX(n-1)+V(n),

(22)

式中：Y(n)为实际输出坐标和速度；H为观测方程的观测向量，H=diag(1，1，1，1，1，1)

观测噪声V(n)表示导航系统定位定速误差为

V(n)=(σxn，σyn，σzn，σvx，σvy，σvz)T.

(23)

σvx，σvy，σvz为速度测量随机误差为

(24)

观测噪声方差阵为：R(n)=E[V(n)V(n)T]，假设V(n)各项相互独立，均服从正态分布，则

).

(25)

2.4 基于挂飞试验数据的误差辨识

基于地面挂飞试验中各导航站的伪距观测值的测量误差，首先采用GPS接收机定位数据对伪距测量的系统误差和随机误差进行辨识，然后将其用于空间轨道飞行条件下，卡尔曼滤波的误差模型中。

在地面挂飞试验中，导航接收机与GPS可同时定位，考虑将GPS接收机定位数据作为基准数据，对各导航站的原始伪距观测值进行双站伪距残差统计，从而得到单站伪距测量误差。为减少系统性误差对精度评估的影响，对原始伪距观测值进行了对流层误差修正、钟差修正、电离层双频修正，对修正后的伪距进行分析。

(26)

式中:随机噪声εi的均值Ei(ε)可用来衡量修正后的伪距的测量系统误差，标准差σi(ε)可用来衡量修正后的伪距的测量随机误差。

(27)

考虑到：

将式(26)代入式(27)，进行化简，则得

(28)

若记εi的均值为Ei(ε)，ξij的均值为Eij(ξ)、标准差为σij(ξ)，则存在如下关系：

(29)

各时刻的GPS接收机的定位数据及各导航站伪距观测值ρi均给出，则根据式(27)即可得到双站伪距残差的时间序列，根据最小二乘原则和式(29)，可求得伪距误差εi的均值Ei和标准差σi，其中Ei反映了单站伪距测量系统误差，σi反映了单站伪距测量随机误差，不失一般性，假设五站测量的伪距误差具有相同的特性，并相互独立，则五站σi平方求均值再开根号得到σr，反映了整个地基导航系统的伪距测量随机误差。

据此采用地面挂飞试验数据，辨识出的伪距测量随机误差如表2所示。

表2 伪距测量随机误差Table 2 Stochastic error of pseudorange measurement

式(19)中，径向距离测量方差σr取值1.176 9。

2.5 卡尔曼滤波过程

设初始状态向量为X(k)，初始状态向量协方差为pk=cov(X(k))，根据上述公式计算a(k),Qk,Rk,Fk。

利用状态方程和量测方程可进行扩展Kalman滤波，滤波方程为：

3 仿真分析

仿真结果如图2所示。

图2 滤波算法理论仿真结果Fig.2 Theoretical simulation results of filtering algorithm

3.1 仿真条件

高空探测火箭典型轨道：以STK中HPOP模块生成的轨道为基准，飞行高度约2×105km；

计算起始点：3 000 km及以上；

PDOP值：设置典型布站形式，计算轨道全程PDOP值的变化；

添加误差：根据多站测距导航系统的误差特性，对基准轨道添加高斯白噪声，作为仿真试验中的实际观测值，其中，位置测量随机误差标准差取2 km(1σ)，速度测量随机误差标准差取5 m/s(1σ)。

3.2 仿真结果

从仿真结果可看出，添加的原始误差得到有效滤除，定位精度由6 km提高到200 m，速度精度由15 m/s提高到0.1 m/s，尤其是速度精度得到了大幅提高。

4 结束语

针对GPS导航系统无法满足高空探测火箭在3 000 km以上的定位需求问题，本文从高空探测火箭快速实时定位解算需求出发，针对地基导航、箭上自主解算实时定位方式，结合导航系统误差模型和轨道动力学特性，提出了一种实时高精度定位解算的方法。采用真实试验数据和误差辨识，完善地基导航系统误差模型，建立动力学卡尔曼滤波算法方程对定位数据进行滤波，实现快速实时高精度定轨。仿真结果表明，该方法可有效降低随机误差的影响，将定位精度由6 km提高到200 m，速度精度由15 m/s提高到0.1 m/s，显著提高定位测速精度。该方法省去了轨道积分过程，提高定位精度和解算效率。

[1] 管洪杰，姚志成，刘岩.轨道数值积分方法适用性研究[J].科学技术与工程，2013，13(36)：10883-10886.

GUAN Hong-jie，YAO Zhi-cheng，LIU Yan.Analysis of Applicability of Orbit Numerical Integration Method[J].Science Technology and Engineering，2013，13(36)：10883-10886.

[2] 张洪波，谢愈，陈克俊，等.非惯性运动目标弹道预报技术探讨[J].现代防御技术，2011，39(6)：26-31.

ZHANG Hong-bo，XIE Yu，CHEN Ke-jun，et al.Investigation on Trajectory Prediction of Maneuverable Target[J].Modern Defence Technology，2011，39(6)：26-31.

[3] 王永诚，张令坤.多站时差定位技术研究[J].现代雷达，2003，25(2)：1-4.

WANG Yong-cheng，ZHANG Ling-kun.Position Location Using TDOA Measurements in Multisites[J].Modern Radar，2003，25(2)：1-4.

[4] 邱凤云.Kalman滤波理论及其在通信与信号处理中的应用[D].济南:山东大学，2008：1-5.

QIU Feng-yun.Kalman Filtering with Its Application to Communication and Signal Processing[D].Jinan:Shandong University，2008：1-5.

[5] 宋迎春.动态定位中的卡尔曼滤波研究[D].长沙:中南大学，2006：1-10.

SONG Ying-chun.Research on Kalman Filter in Kinematic Positioning[D].Changsha:Central South University，2006：1-10.

[6] 丁传炳，王良明，常思江.卡尔曼滤波在GPS制导火箭弹中的应用[J].南京理工大学学报，2010，34(2)：157-160.

DING Chuan-bing，WANG Liang-ming，CHANG Si-jiang.Application of Kalman Filtering in GPS-Guided Rocket Projectiles[J].Journal of Nanjing University of Science and Technology，2010，34(2)：157-160.

[7] Elliott D Kaplan，Christopher J Hegarty.Understanding GPS Principles and Applications[M].2nd. Beijing：Publishing House of Electronics Industry，2008.

[8] 庄奇祥，菅曙光，樊能娥.三维位置几何因子探讨[J].天文学报，1991，32(2)：113-120.

ZHUANG Qi-xiang，JIAN Shu-guang，FAN Neng-e.An Investigation of the Three Dimensional Position Dilution of Precision[J].Acta Astronomica Sinica，1991，32(2)：113-120.

[9] 常青，柳重堪，张其善.GPS的几何精度因子和定位解的递推算法[J].通信学报，1998，19(12)：83-88.

CHANG Qing，LIU Zhong-kan，ZHANG Qi-shan.The Recurrence Algorithm for GDOP and Positioning Solution in GPS[J].Journal of China Institute of Communications，1998，19(12)：83-88.

[10] 李锐，王雪梅.陆基导航系统最优布站方法研究[J].微计算机信息，2009，25(7-1)：194-195.

LI Rui，WANG Xue-mei.Research on Base Station Location for the Land-Based Navigation System[J].Micro-Computer Information，2009，25(7-1)：194-195.

[11] 杨冬，宋才水.机动式地基无线电导航系统布站策略分析[J].现代防御技术，2016，44(3)：25-31.

YANG Dong，SONG Cai-shui.Deployment Strategy Analysis of Motorized Ground-Based Navigation System[J].Modern Defence Technology，2016，44(3)：25-31.

[12] 贾沛然，沈为异.弹道导弹弹道学[M].长沙：国防科技大学出版社，1980.

JIA Pei-ran，SHEN Wei-yi.Ballistics Missile[M].Changsha：National University of Defence Technology Press，1980.

[13] 张毅，杨辉耀，李俊莉.弹道导弹弹道学[M].长沙：国防科技大学出版社，1999.

ZHANG Yi，YANG Hui-yao，LI Jun-li.Ballistics Missile[M].Changsha：National University of Defence Technology Press，1999.

[14] 袁幸伟.摄动因素对航天器轨道设计的影响分析[D].哈尔滨:哈尔滨工业大学，2010：31-43.

YUAN Xing-wei.Influence Analysis of Perturbation Factors on Spacecraft Orbit Design[D].Harbin:Harbin Institute of Technology，2010：31-43.