卫星自主导航是指卫星在不依赖地面站的条件下,仅依靠卫星上的测量设备 实时确定卫星的
位置和速度,在轨完成的飞行任务要求的功能或操作。
航天器通 常采用的依靠地面站无线电测控进行定位导航的方法,由于受地理条件的限制, 难以实现对中低轨道卫星整个轨道的定位导航;同时,随着空间运行的各种航天 器的数目的增加,信息传输量剧增,完全依靠地面站测控,将会引起测控系统的 信息阻塞和地面站负荷过重。因此,采用自主导航技术,提高卫星的自主运行、 自主管理和在轨生存能
力,已经成为现代卫星
导航系统发展的必然趋势。
在卫星自主导航系统中,为了提高可靠性和
精度,通常采用多种传感器同时 进行定轨。目前,星上常用的自主导航传感器有以下几类:利用地球物理特性的 敏感器,如红外地平仪、
磁强计、雷达高度计等;利用天体位置的敏感器,如太 阳敏感器、星敏感器、紫外敏感器等;利用惯性的敏感器,如
陀螺仪、
角加速度 计等。这些传感器的测量方程都具有较强的非线性,如何将其信息有效的组织并 充分利用,是卫星自主导航的关键问题。传统的信息处理方法是采用集中式扩展 卡尔曼
滤波器,但是这种方法存在两个
缺陷:一是采用集中式处理方式,导致计 算负担过重,容错性差、通信负担重;二是这种方法的假设前提十分苛刻,当系 统非线性特性较强或对噪声估计不准确时,将会造成滤波器发散,导致导航精度 下降。
Unscented卡尔曼滤波(UKF)是一种新的信息处理方法,它采用卡尔曼线性 滤波
框架,使用UT变换来处理一步预测方程中的均值和协方差的非线性传递, 不需要求导计算Jacobian矩阵,算法简单,容易实现,利用UKF算法可以获得 比传统的
扩展卡尔曼滤波(EKF)更高的估计精度。近年来,基于UKF算法的轨道 及
姿态确定方法研究得到迅速发展。如何采用信息分配原理,将Unscented卡尔 曼滤波算法和信息融合技术相结合,实现基于UKF的多传感器非线性滤波算法, 使系统能够充分运用各导航系统的信息进行信息互补和信息融合,并具有故障检 测、隔离和重构的功能,对卫星自主导航技术发展有着非常重要的意义。
发明目的:本发明的目的是提供一种具有容错功能的近地轨道卫星多传感器 自主导航定位方法,该方法能够充分利用卫星上的测量设备提供的导航信息实现 自主定轨,并解决了如何用联邦滤波技术对Unscented卡尔曼滤波结果进行信息 融合的问题。
技术方案:为达到上述发明目的,本发明设计了一种基于多传感器信息融合 的联邦UKF滤波器,该滤波器采用地球卫星的轨道动力学方程作为
状态方程,分 别以星敏感器+红外地球敏感器、磁强计+雷达高度计、紫外敏感器作为测量器件 建立三个子滤波器;用无复位联邦滤波技术对各子滤波器的Unscented卡尔曼滤 波结果进行信息融合;用残差检验技术对子滤波器进行故障检测、隔离和重构。 本发明可在测量系统非线性较强、对系统噪声和量测噪声估计不准确的情况下获 得较高的定轨精度。各个子滤波器独立工作,相互之间没有干扰,既保证了输出 导航参数的连续性和
稳定性,也便于工程实现,并可通过主滤波器迅速实现系统 重构,使因部分导航传感器故障导致子系统失效后,组合导航系统仍能继续工作, 具容错性。大大提高了地球
卫星导航系统的适用范围。本发明具体步骤如下:
(1)系统初始化;
读取轨道数据初值,状态初始条件为:
状态矢量X0=[x0,y0,z0,vx0,vy0,vz0]T,x0,y0,z0,vx0,vy0,vz0,分别为卫星在X,Y,Z三个方 向的初始位置和速度;并设系统过程噪声w(k)和量测噪声v(k)为加性噪声,Qk和 Rk分别为系统和量测噪声协方差阵。
(2)建立基于直角
坐标系的地球卫星轨道运动学方程;
选取历元(J2000.0)地球赤道惯性坐标系,建立基于直角坐标系的地球卫 星轨道运动学方程,即状态方程,通过四阶龙格-库塔方法解微分方程计算出地 球卫星的位置(x,y,z)和速度(vx,vy,vz)信息。简化的系统状态方程为:
式中,Re为地球参考赤道半径,状态矢量X=[x,y,z,vx,vy,vz]T,x,y,z,vx,vy,vz分别 为卫星在X,Y,Z三个方向的位置和速度,
为地心距,μ是地球引力常 数,J2为地球引力二阶带谐项系数,ΔFx,ΔFy,ΔFz为地球非球形摄动的高阶摄动项 和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响,在简化模型中这些 摄动力的影响用系统过程噪声w(k)表示。
对于系统过程噪声为零均值白噪声有:
E{w(k)}=0,E{w(k)wT(j)}=Qkδkj,k,j=1,2,3…
式中,δkj为克朗涅克笛
耳塔(Kronecker Delta)函数。
(3)建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测方 程;
以星敏感器和红外地球敏感器的子系统的量测量为星光角距,星光角距是指 从卫星上观测到的导航
恒星星光的矢量方向与地心矢量方向之间的夹角,星光角 距a的量测方程为:
式中为卫星在地心惯性坐标系中的位置矢量,由红外地球敏感器获得:为 地球惯性坐标系下的导航星星光方向的单位矢量,由星敏感器识别;r为地心距; va为测量误差。
设量测噪声为零均值白噪声:
E{va(k)}=0,
k,j=1,2,3…
(4)建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程;
磁强计和雷达高度计构成的子系统的量测量为地
磁场强度矢量和卫星轨道 高度,由于通过磁强计对磁场强度的测量即可单独完成定轨功能,雷达高度计起 辅助定位、提高系统精度的作用,因此本子系统对雷达高度计设计了故障检测阈 值,当雷达高度计测量值和计算值的差值超出
阈值时,将雷达高度计隔离,仅使 用磁强计进行定轨,保证了在雷达高度计失效时子系统2仍能正常工作。
三轴磁强计的测量值为卫星所在位置的
地磁场矢量在卫星本体系中的三 个分量,为简化量测模型,取地磁场强度矢量的模作为磁测自主导航观测量,比 较此值与国际地磁场模型(IGRF)之间的差值来提供导航信息。量测方程为:
其中,Bx、By、Bz为地磁场矢量在卫星本体系中的三个分量;V为地磁场势 函数,r为地心距,φ为地理经度,θ为地心余纬,为地理坐标系下的卫星位置 矢量,vB为量测噪声。
设量测噪声为零均值白噪声:E{vB(k)}=0,
k,j=1,2,3…
星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息,由于海平 面很接近于大地
水准面,所以可认为雷达高度计测得的卫星高度是卫星至星下点 处大地水准面的距离。取测量模型为:
其中,Re为地球参考赤道半径,为地心纬度,f为地球参考椭球的扁率, vH(t)为量测噪声。
设量测噪声为零均值白噪声:
E{vH(k)}=0,
k,j=1,2,3…
从式5和式6可以看出,在地磁场强度和轨道高度的测量模型中,磁场强度 和轨道高度是根据卫星在地球固联坐标系下的位置信息计算的,而卫星的位置矢 量一般是在地心惯性坐标系中给出的,因此,需建立地心惯性坐标系和地球固联 坐标系之间的转换矩阵。从地心赤道惯性坐标系Si到地球固连坐标系Se的坐标变 换矩阵为:
式中:aG为Greenwich赤经。
(5)建立以紫外敏感器的输出值为量测量的子系统量测方程;
紫外敏感器的量测值为卫星的位置矢量。紫外敏感器工作在紫外波段,它可 以同时观测多个天体目标,提供三轴的航天器姿态信息,定姿精度可达0.05°。 另外,通过对地球圆盘的
图像处理,可提取星体坐标系中的地心方向和地心距离 信息。结合定姿过程得到的姿态信息,可计算出卫星在惯性系中的位置矢量。子 系统的量测方程为:
其中,为卫星在地心惯性坐标系中的位置矢量,
为地心距;vu 为测量误差。
设量测噪声为零均值白噪声:
E{vu(k)}=0,
k,j=1,2,3…
(6)依照最小偏度
采样策略选择Sigma采样点;
①选择0≤W0<1,Sigma权值为:
式中,W为权值,n为状态方程维数,Wi m为均值加权值,Wi c为协方差加权 值。
②对应状态为1维情况,
迭代初始向量:
③对于输入维数j=2,…n时,迭代公式为:
(7)根据最小方差估计原则建立离散型UKF算法的预测方程和更新方程,各 子系统分别独立进行Sigma采样点计算以及预测更新和量测更新;
①预测方程:
对所生成的Sigma点加入状态矢量X的均值和协方差信息:
i=0,1,2,…,n+1 (12)
χi(k+1|k)=f[χi(k|k)] (13)
其中L为采样点个数,为tk时刻对tk+1的预测估值,P(k)为误差协方差 阵初值,f(·)为系统状态方程。
Zi(k+1|k)=h[χi(k+1|k)] (16)
式中,P(k+1|k)为预测估值误差协方差阵,h(·)为量测方程,为对tk+1 观测值的预测值。
②更新方程:
P(k+1|k+1)=P(k+1|k)-W(k+1)Pyy(k+1|k)WT(k+1) (22)
式中,W(k+1)为增益矩阵,为滤波值,P(k+1|k+1)为滤波值误差 协方差阵。
(8)利用残差检验法,根据预测滤波残差判断各子滤波器输出值是否有效, 如出现故障则将其隔离,否则将滤波结果输入主滤波器进行信息融合;
残差检验法的故障函数为:
其中,
即χm 2(k)为具有m个
自由度的χ2变量,m 为量测矢量Z的维数;vk为残差,
对于正常滤波,取误警率
其中km(·)为χm 2的概率
密度函数,a为显著性水平,TD为由误警率确定的
门限值。
(9)建立基于UKF算法的无复位联邦UKF滤波方程;
联邦滤波器的信息融合方式采用无复位模式,仅对状态量进行融合和反馈, 各子系统的协方差阵独立进行时间更新和预测更新,不参加主滤波器的融合:
l=1,2,3 (24)
l=1,2,3 (25)
(10)按照上述步骤(1)-(9),输出为地球卫星状态矢量估计值及其方差矩 阵Pg,其中状态估计值包括地球卫星位置和速度矢量[x,y,z,vx,vy,vz]T,状态估计 方差矩阵Ps包括地球卫星位置和速度估计方差[Px g,Py g,Pz g,Pvx g,Pvy g,Pvz g]T。
3、有益效果:本发明的方法有如下优点:(1)采用星敏感器+红外地球敏感 器、磁强计+雷达高度计、紫外敏感器的组合,能够充分利用星上的测量设备提 供的信息进行导航定位,具有完全的自主性;(2)子系统2采用基于UKF技术的 磁强计和雷达高度计组合定轨,并设计故障检测阈值,保证在雷达高度计失效后 系统仍然能继续工作,系统具较高的精度和可靠性;(3)采用UKF算法对子滤波 器进行状态估计,在测量系统非线性较强、或对系统噪声和量测噪声估计不准确 的情况下均能够获得较高的定轨精度;(4)UKF算法采用最小偏度采样策略,减 少了采样点数,在保证精度的前提下提高了计算效率;(5)用无复位联邦滤波技 术对各子滤波器的UKF滤波结果进行信息融合,对每个子系统单独计算Sigma 采样点,各子滤波器独立工作,相互之间没有干扰,既保证了输出导航参数的连 续性和稳定性,也便于工程实现,(6)用残差检验技术对子滤波器进行故障检测, 并可在检测到故障时,通过主滤波器将其隔离,迅速实现系统重构,使因部分导 航传感器故障导致子系统失效后,组合导航系统仍能继续工作,具容错性。
附图说明:
图1卫星多传感器容错联邦UKF系统
流程图;
图2卫星多传感器容错联邦UKF系统结构原理图;
图3标称轨迹示意图;
图4仿真轨迹示意图;
图5标称轨迹位置变化曲线;
图6标称轨迹速度变化曲线;
图7仿真轨迹位置变化曲线;
图8仿真轨迹速度变化曲线;
图9理想情况下联邦UKF滤波误差曲线;
图10对系统噪声估计不足情况下的联邦UKF滤波误差曲线;
图11对量测噪声估计不足情况下的联邦UKF滤波误差曲线;
图12子系统1故障情况下的联邦UKF误差曲线;
图13子系统2故障情况下的联邦UKF误差曲线;
图14子系统3故障情况下的联邦UKF误差曲线;
本实例的具体组成图如图1所示,实施的流程如图2所示,轨道数据由STK (卫星仿真工具包)
软件产生,具体仿真条件如下;
①仿真时间2008年6月1日12:00-2008年6月1日24:00,采样周期T 为10s。标称轨道数据为:半长轴:a=6823.082km;偏心率:e=1.83×10-16;轨道倾 角:i=97.2°;升交点赤经:Ω=157.5°;近升角距:ω=0°。
②为了模拟卫星在轨运行的真实运动情况,在计算卫星轨道时,使用力学模 型JGM-3,并考虑如下摄动因素:地球非球形摄动;太阳引力、月球引力;太阳 光压;大气阻力。
③测量仪器精度:导航星采用分布在天球上的80颗最亮的恒星(星等≤ 2.3),星敏感器的视场是25°×25°,精度为3″;红外地平仪的精度为0.02°;磁 强计测量噪声误差取为50nT,地磁场模型误差取为150nT;雷达高度计测量误差 取为200m。紫外敏感器测距误差500m,地心方向测量误差0.01°。
④轨道初始位置误差为5km,初始速度误差取为10m/s。
本实例以3个子系统的量测输出值作为量测量,结合地球卫星的状态方程, 分别利用基于最小偏度采样的UKF算法对各子系统单独进行时间更新和量测更 新,并利用残差检验法对3个子系统的滤波结果进行分析,判断子系统输出是否 有效,如果有效则将该结果输出给主滤波器,利用无复位联邦滤波器在主滤波器 中对有效的子滤波器的状态估计值进行信息融合,得到地球卫星的位置、速度信 息。
具体的实施步骤如下:
(1)系统初始化;
读取轨道数据初值,状态初始条件为:
状态矢量X0=[x0,y0,z0,vx0,vy0,vz0]T,x0,y0,z0,vx0,vy0,vz0,分别为卫星在X,Y,Z三个方 向的初始位置和速度;并设系统过程噪声w(k)和量测噪声v(k)为加性噪声,Qk和 Rk分别为系统和量测噪声协方差阵。
(2)建立基于直角坐标系的地球卫星轨道运动学方程;
选取历元(J2000.0)地球赤道惯性坐标系,建立基于直角坐标系的地球卫 星轨道运动学方程,即状态方程,通过四阶龙格-库塔方法解微分方程计算出地 球卫星的位置(x,y,z)和速度(vx,vy,vz)信息。简化的系统状态方程为:
式中,Re为地球参考赤道半径,状态矢量X=[x,y,z,vx,vy,vz]T,x,y,z,vx,vy,vz分别 为卫星在X,Y,Z三个方向的位置和速度,
为地心距,μ是地球引力常 数,J2为地球引力二阶带谐项系数,ΔFx,ΔFy,ΔFz为地球非球形摄动的高阶摄动项 和日、月摄动以及太阳光压摄动和大气摄动等摄动力的影响,在简化模型中这些 摄动力的影响用系统过程噪声w(k)表示。
对于系统过程噪声为零均值白噪声有:
E{w(k)}=0,E{w(k)wT(j)}=Qkδkj,k,j=1,2,3… 式中,δkj为克朗涅克笛耳塔(Kronecker Delta)函数。
(3)建立以星敏感器和红外地球敏感器的输出值为量测量的子系统量测方 程;
以星敏感器和红外地球敏感器的子系统的量测量为星光角距,星光角距是指 从卫星上观测到的导航恒星星光的矢量方向与地心矢量方向之间的夹角,星光角 距a的量测方程为:
式中为卫星在地心惯性坐标系中的位置矢量,由红外地球敏感器获得;为 地球惯性坐标系下的导航星星光方向的单位矢量,由星敏感器识别;r为地心距; va为测量误差。
设量测噪声为零均值白噪声:
E{va(k)}=0,
k,j=1,2,3…
(4)建立以磁强计和雷达高度计的输出值为量测量的子系统量测方程;
磁强计和雷达高度计构成的子系统的量测量为地磁场强度矢量和卫星轨道 高度,由于通过磁强计对磁场强度的测量即可单独完成定轨功能,雷达高度计起 辅助定位、提高系统精度的作用,因此本子系统对雷达高度计设计了故障检测阈 值,当雷达高度计测量值和计算值的差值超出阈值时,将雷达高度计隔离,仅使 用磁强计进行定轨,保证了在雷达高度计失效时子系统2仍能正常工作。 三轴磁强计的测量值为卫星所在位置的地磁场矢量在卫星本体系中的三 个分量,为简化量测模型,取地磁场强度矢量的模作为磁测自主导航观测量,比 较此值与国际地磁场模型(IGRF)之间的差值来提供导航信息。量测方程为:
其中,Bx、By、Bz,为地磁场矢量在卫星本体系中的三个分量;V为地磁场势 函数,r为地心距,φ为地理经度,θ为地心余纬,为地理坐标系下的卫星位置 矢量,vB为量测噪声。
设量测噪声为零均值白噪声:
E{vB(k)}=0,
k,j=1,2,3…
星载雷达高度计的测量值为卫星到星下点实际海平面的距离信息,由于海平 面很接近于大地水准面,所以可认为雷达高度计测得的卫星高度是卫星至星下点 处大地水准面的距离。取测量模型为:
其中,Re为地球参考赤道半径,为地心纬度,f为地球参考椭球的扁率, vH(t)为量测噪声。
设量测噪声为零均值白噪声:
E{vH(k)}=0,
k,j=1,2,3…
从式5和式6可以看出,在地磁场强度和轨道高度的测量模型中,磁场强度 和轨道高度是根据卫星在地球固联坐标系下的位置信息计算的,而卫星的位置矢 量一般是在地心惯性坐标系中给出的,因此,需建立地心惯性坐标系和地球固联 坐标系之间的转换矩阵。从地心赤道惯性坐标系Si到地球固连坐标系Se的坐标变 换矩阵为:
式中:aG为Greenwich赤经。
(5)建立以紫外敏感器的输出值为量测量的子系统量测方程;
紫外敏感器的量测值为卫星的位置矢量。紫外敏感器工作在紫外波段,它可 以同时观测多个天体目标,提供三轴的航天器姿态信息,定姿精度可达0.05°。 另外,通过对地球圆盘的图像处理,可提取星体坐标系中的地心方向和地心距离 信息。结合定姿过程得到的姿态信息,可计算出卫星在惯性系中的位置矢量。子 系统的量测方程为:
其中,为卫星在地心惯性坐标系中的位置矢量,
为地心距;vu 为测量误差。
设量测噪声为零均值白噪声:
E{vu(k)}=0,
k,j=1,2,3…
(6)依照最小偏度采样策略选择Sigma采样点;
①选择0≤W0<1,Sigma权值为:
式中,W为权值,n为状态方程维数,Wi m为均值加权值,Wi c为协方差加权 值。
②对应状态为1维情况,迭代初始向量:
③对于输入维数j=2,…n时,迭代公式为:
(7)根据最小方差估计原则建立离散型UKF算法的预测方程和更新方程,各 子系统分别独立进行Sigma采样点计算以及预测更新和量测更新;
①预测方程:
对所生成的Sigma点加入状态矢量X的均值和协方差信息:
i=0,1,2,…,n+1 (12)
χi(k+1|k)=f[χi(k/k)] (13)
其中L为采样点个数,为tk时刻对tk+1的预测估值,P(k)为误差协方差 阵初值,f(·)为系统状态方程。
Zi(k+1|k)=h[χi(k+1)] (16)
式中,P(k+1|k)为预测估值误差协方差阵,h(·)为量测方程,为对tk+1 观测值的预测值。
②更新方程:
P(k+1|k+1)=P(k+1|k)-W(k+1)Pyy(k+1|k)WT(k+1) (22)
式中,W(k+1)为增益矩阵,为滤波值,P(k+1|k+1)为滤波值误差 协方差阵。
(8)利用残差检验法,根据预测滤波残差判断各子滤波器输出值是否有效, 如出现故障则将其隔离,否则将滤波结果输入主滤波器进行信息融合;
残差检验法的故障函数为:
其中,
即χm 2(k)为具有m个自由度的χ2变量,m 为量测矢量Z的维数;vk为残差,
对于正常滤波,取误警率
其中km(·)为χm 2的概率 密度函数,a为显著性水平,TD为由误警率确定的门限值。
(9)建立基于UKF算法的无复位联邦UKF滤波方程;
联邦滤波器的信息融合方式采用无复位模式,仅对状态量进行融合和反馈, 各子系统的协方差阵独立进行时间更新和预测更新,不参加主滤波器的融合:
l=1,2,3 (24)
l=1,2,3 (25)
(10)按照上述步骤(1)-(9),输出为地球卫星状态矢量估计值及其方差矩 阵Pg,其中状态估计值包括地球卫星位置和速度矢量[x,y,z,vx,vy,vz]T,状态估计 方差矩阵Pg包括地球卫星位置和速度估计方差[Px g,Py g,Pz g,Pvx g,Pvy g,Pvz g]T。
对本发明的有益效果说明如下:
(1)本发明的系统组成图如图1所示,程序流程图如图2所示;
(2)设计轨道为太阳同步轨道,轨道高度500km,降交点地方时上午10:30, 轨道倾角97.2°,仿真轨迹如图3-8所示;
(3)为充分说明本发明方法对导航结果的修正作用,分别给出理想状态下的 联邦UKF、对系统噪声估计不足情况下的联邦UKF、对量测噪声估计不足情况下 的联邦UKF滤波的滤波结果,分别如下所示:
①理想情况下联邦UKF滤波误差曲线如图9所示;
②对系统噪声估计不足情况下的联邦UKF滤波误差曲线如图10所示;
③对量测噪声估计不足情况下的联邦UKF滤波误差曲线如图11所示;
分别对比3种情况下的联邦UKF滤波结果,可以看出:
①在理想情况下,组合导航系统能够迅速收敛,且没有发散现象;在系统收 敛后,联邦自适应UKF的位置误差不超过400m,速度误差不超过0.8m/s,能够 很好的完成定轨功能。
②当对系统噪声估计不足时,联邦UKF滤波结果所受影响不大,仍能很好的 完成定轨功能。
③当对量测噪声估计不足时,联邦UKF滤波结果所受影响不大,仍能很好的 完成定轨功能。
④三种情况下的滤波精度如表1所示。
表1不同情况下的联邦UKF在定轨精度比较
滤波结果(1σ) 理想状态 系统噪声估计不 足 量测噪声估计不足 位置精度m 速度精度m/s 238.8 0.65 257.7 0.73 257.2 0.73
从以上分析可以看出,本发明的方法本文采用的联邦UKF万法对低轨卫星位 置和速度的误差的修正效果良好,该方法适合于系统非线性较强、系统状态噪声 阵或量测噪声阵未知的情况。
本发明的采用容错联邦UKF算法来提高非线性系统的稳定性和滤波精度,因 此这里对这种手段所起的作用进行说明。
表2子系统故障情况下定轨精度比较
滤波结果(1σ) 无故障 子系统1故障 子系统2故障 子系统3故障
位置精度m 238.8 263.9 264.5 273.3
速度精度m/s 0.65 0.69 0.85 0.81
子系统故障情况下联邦UKF滤波结果如图12-14和表2所示,分别在不同 时间对3个子系统加入突变故障,可以看出,当单个子系统故障时,主系统可通 过残差检验法迅速检验出故障,并进行系统重构,子系统的故障不会对主系统造 成污染,滤波结果变化较小。
本发明
说明书中未作详细描述的内容属于本领域专业技术人员公知的现有 技术。