基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法
【专利摘要】本发明公布了一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法，属于飞行器组合导航【技术领域】。该方法包含以下步骤：首先以发射惯性坐标系为参考坐标系，进行惯性导航算法编排，建立飞行器在发射惯性坐标系下的位置、速度、姿态解算模型；在此基础上，建立了包含基本导航参数误差和惯性仪表误差在内的导航系统状态方程，根据原始星敏感器测量信息经过转换模块得到姿态线性量测方程；最后采用鲁棒滤波方法对所建立的状态方程中各个状态量进行估计和修正。本发明能够有效减小地球物理场对飞行器导航系统的影响，且保证在飞行器导航系统模型不准确的情况下，滤波器能够稳定工作，提高组合导航系统的精度。
【专利说明】基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法
【技术领域】
[0001] 本发明公开了一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法，涉 及组合导航【技术领域】。
【背景技术】
[0002] 近年来，随着航空航天技术的发展，近空间高超声速飞行器、空天飞行器等新型飞 行器呈现长航时、大空域飞行特点，其对导航系统性能的要求日益提高。
[0003] 传统航空飞行器通常以地理坐标系为参考坐标系进行捷联惯性导航算法编排，在 捷联惯性导航系统算法编排中对重力场、曲率半径变化等采用直接忽略或简化处理，主要 考虑到地理系编排较为直观，且在短时间、短距离飞行中，该处理方法对导航系统精度影响 不大。而近空间高超飞行器、空天飞行器等新型飞行器飞行时间长、飞行距离远，地球重力、 曲率半径等变化较大。这些因素如果在飞行器飞行中忽略或简化处理，将会在很大程度上 影响导航精度。
[0004] 此外，单纯依靠惯性导航系统提供的导航信息难以满足飞行器的导航需求，需要 采用组合导航方式提供导航信息。在导航信息融合领域，kalman滤波作为一种线性最小方 差估计方法，得到了广泛的应用，其要求系统模型必须准确已知且输入噪声为严格的Guass 噪声。而空天飞行器面临的飞行环境复杂，不确定因素较多，通常会对导航系统带来不可预 知的影响，难以获得导航系统的精确模型。在上述条件下，Kalman滤波器的滤波性能将受 到限制。因此，研究基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法，一方面将能 有效减小因地球物理场描述不准确而对导航系统带来的影响，另一方面能提高组合导航系 统的鲁棒性能，具有突出的应用前景。
【发明内容】
[0005] 本发明所要解决的技术问题是：针对现有技术的缺陷，提供一种基于发射惯性坐 标系的空天飞行器组合导航鲁棒滤波方法，本发明以发射惯性坐标系为参考坐标系，进行 惯性导航算法编排，并采用鲁棒滤波方法对导航系统误差进行估计和修正。
[0006] 本发明为解决上述技术问题采用以下技术方案：
[0007] -种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法，所述方法具体步 骤包括：
[0008] 步骤一、以发射惯性坐标系为参考坐标系，进行惯性导航算法编排，建立飞行器在 发射惯性坐标系下的速度、位置、姿态解算模型；
[0009] 步骤二、根据步骤一中飞行器在发射惯性坐标系下的速度、位置、姿态解算模型， 得到对应的速度、位置、姿态误差模型，结合惯性仪表的误差模型，构建包含基本导航参数 误差和惯性仪表误差在内的惯性导航系统的误差状态方程以及线性化量测方程；
[0010] 步骤三、对步骤二得出的惯性导航系统的误差状态方程和线性化量测方程进行离 散化处理以及状态量、测量量的更新，采用鲁棒滤波方法对所建立的状态方程中各个状态 量进行估计和修正。
[0011] 作为本发明的进一步优选方案，步骤一所述的解算模型的表达形式具体为：
[0012] (101)飞行器在发射惯性坐标系下的速度解算模型为：
[0013]
【权利要求】
1. 一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法，其特征在于，所述 方法具体步骤包括： 步骤一、以发射惯性坐标系为参考坐标系，进行惯性导航算法编排，建立飞行器在发射 惯性坐标系下的速度、位置、姿态解算模型； 步骤二、根据步骤一中飞行器在发射惯性坐标系下的速度、位置、姿态解算模型，得到 对应的速度、位置、姿态误差模型，结合惯性仪表的误差模型，构建包含基本导航参数误差 和惯性仪表误差在内的惯性导航系统的误差状态方程以及线性化量测方程； 步骤三、对步骤二得出的惯性导航系统的误差状态方程和线性化量测方程进行离散化 处理以及状态量、测量量的更新，采用鲁棒滤波方法对所建立的状态方程中各个状态量进 行估计和修正。
2. 如权利要求1所述的一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方 法，其特征在于，步骤一中，所述解算模型的表达形式具体为： (101) 飞行器在发射惯性坐标系下的速度解算模型为：
其中，v1为飞行器相对于发射惯性系的速度，fi1为v1的一阶导数，f b为加速度计输出的 比力，G1为地心惯性坐标系下的地球万有引力，g为载体坐标系b系到发射惯性坐标系1系 的姿态转换矩阵，C)为地心惯性坐标系i系到发射惯性坐标系1系的姿态转换矩阵； (102) 飞行器在发射惯性坐标系下的位置解算模型为：
其中，P1为飞行器相对于发射惯性坐标系的位置，夕为P1的一阶导数； (103) 飞行器在发射惯性坐标系下的姿态解算模型为：
其中，q为飞行器相对于发射惯性系的姿态四元数，#为q的一阶导数，瑪|为载体相对 于发射惯性系的角速率在载体系上的投影，?表示四元数乘法。
3. 如权利要求1所述的一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方 法，其特征在于，步骤二所述的惯性导航系统的误差状态方程中，误差状态变量X的具体表 达形式为： (4)
公式⑷中，δ qi、δ q2、δ q3分别为惯性导航系统误差中姿态四元数误差δ q的矢量 部分，SPx、SPy、δΡζ分别为惯性导航系统误差中X轴、Y轴、Z轴方向的位置误差状态量， δνχ、SVy、6\分别为惯性导航系统中X轴、Υ轴、Ζ轴方向的速度误差状态量，εΜ、％、 ε "分别为X轴、Υ轴、Ζ轴方向的陀螺随机游走误差，▽ ax、V ay、V az分别为X轴、Υ轴、Ζ 轴方向的加速度计随机游走误差。
4. 如权利要求3所述的一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方 法，其特征在于：所述惯性导航系统的误差状态方程具体表达为：
公式（5)中，X(t)为系统状态变量，对?)为X(t)的一阶导数，A(t)为系统矩阵；B(t) 为噪声系数矩阵，W(t)为噪声矩阵； 根据星敏感器测量得出的载体相对于惯性坐标系的姿态信息，经过转换得出载体相对 于发射惯性坐标系的姿态信息，采用发射惯性坐标系下姿态线性化观测原理，建立发射惯 性坐标系下的姿态观测量和惯性导航系统误差状态量之间的线性化量测方程：
公式（6)中，Z(t)为系统姿态观测量矩阵，Ha(t)为姿态量测系数矩阵，Ns(t)为姿态观 测噪声阵。
5.如权利要求1所述的一种基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方 法，其特征在于，步骤三中所述的采用鲁棒滤波方法对所建立的状态方程中各个状态量进 行估计和修正，其具体步骤包括： (301) 将惯性导航系统的误差状态方程和线性化量测方程离散化处理：
上述公式中，xk为tk时刻系统状态量，Xg为tkH时刻系统状态量，Ak为tkH时刻至t k 时刻系统的状态转移矩阵，Bk为时刻至tk时刻系统的噪声驱动矩阵，Wg为tk时刻系 统的噪声矩阵，Z k为tk时刻系统姿态观测量矩阵，Hk为tk时姿态量测系数矩阵，N k为tk时 刻姿态观测量的噪声矩阵； (302) 采用鲁棒滤波策略对惯性导航系统误差进行估计：
上述公式中，Lk为状态量的线性组合系数矩阵，Sk为权系数矩阵，?为转换后的权系数 矩阵，Kk为tk时刻带鲁棒滤波因子的滤波增益矩阵，Θ为鲁棒滤波因子，pk为t k时刻带鲁 棒滤波因子的协方差矩阵，Pk+1为tk+1时刻带鲁棒滤波因子的协方差矩阵，Rk为根据t k时刻 量测噪声的先验知识得到的正定对称矩阵，14+1为tk+1时刻的滤波估计值，为tk时刻的 状态估计值，Qk为根据tk时刻系统噪声的先验知识得到的正定对称矩阵，I为与Pk+1维数相 同的单位矩阵； (303) 经过步骤（302)得到滤波估计值;后，利用滤波估计值对惯性导航系统姿态 误差和陀螺随机游走误差进行修正。
【文档编号】G01C21/20GK104215244SQ201410419869
【公开日】2014年12月17日 申请日期:2014年8月22日 优先权日:2014年8月22日
【发明者】赵慧, 熊智, 潘加亮, 孙永荣, 郁丰, 刘建业, 许建新, 柏青青, 王融, 王洁, 程娇娇, 林爱军, 戴怡洁, 施丽娟, 孔雪博 申请人:南京航空航天大学