CN106441309B - 基于协方差交叉融合的火星进入段分布式自主导航方法 - Google Patents

基于协方差交叉融合的火星进入段分布式自主导航方法 Download PDF

Info

Publication number
CN106441309B
CN106441309B CN201611000780.5A CN201611000780A CN106441309B CN 106441309 B CN106441309 B CN 106441309B CN 201611000780 A CN201611000780 A CN 201611000780A CN 106441309 B CN106441309 B CN 106441309B
Authority
CN
China
Prior art keywords
mars
detector
navigation
state
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201611000780.5A
Other languages
English (en)
Other versions
CN106441309A (zh
Inventor
娄泰山
王妍
丁国强
刁智华
王晓雷
毛聪聪
李亚玺
王磊
陈虎
吴青娥
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhengzhou University of Light Industry
Original Assignee
Zhengzhou University of Light Industry
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhengzhou University of Light Industry filed Critical Zhengzhou University of Light Industry
Priority to CN201611000780.5A priority Critical patent/CN106441309B/zh
Publication of CN106441309A publication Critical patent/CN106441309A/zh
Application granted granted Critical
Publication of CN106441309B publication Critical patent/CN106441309B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Astronomy & Astrophysics (AREA)
  • Navigation (AREA)

Abstract

本发明提出了一种基于协方差交叉融合的火星进入段分布式自主导航方法，其步骤为：建立火星大气进入段导航***的三自由度动力学模型；建立火星大气进入段的量测模型；建立分布式容错自主导航***；离散和线性化分布式容错自主导航***的三自由度动力学模型和量测模型；建立基于EKF导航滤波算法的子滤波器；建立基于协方差交叉融合算法的信息融合器。本发明采用容错能力较强的分布式信息融合的方式进行自主导航，有利于提高无线电通信的利用效率，减少了自主导航滤波的计算量，提高了火星大气进入段自主导航***的容错能力，增强了导航滤波的稳定性，有效地提高了火星大气进入段对探测器的自主导航精度。

Description

基于协方差交叉融合的火星进入段分布式自主导航方法
技术领域
本发明涉及火星大气进入段多传感器信息融合的技术领域，具体涉及一种基于协方差交叉融合的火星进入段分布式自主导航方法，可用于提高火星大气进入段自主导航的精度和容错能力。
背景技术
在火星大气进入段，火星探测器面临着峰值过载、高温高热等恶劣的环境，探测器被安装在防护罩内，导致大量的导航敏感器无法工作，仅仅依靠惯性测量单元(简记为IMU)进行自主导航，导航的精度难以满足未来火星探测的要求。基于火星大气进入段可依靠甚高频无线电进行通信的研究，学者们提出IMU和无线电测距的组合导航方案，为探测器在进入段的自主导航提供了额外的信息，提高了探测器的自主导航精度。但是，上述组合导航方法是集中式滤波的组合导航***，面临着计算负担重、容错能力差等问题，而且不能保证滤波器的实时性。再者，由于探测器与火星在轨信标或表面信标之间的甚高频无线电通信会存在被探测器周围的等离子鞘减弱甚至信号丢失的情况，会使集中式滤波的容错能力进一步降低。因此，针对火星大气进入段的组合导航，选用合适的导航滤波方法对导航敏感器进行信息融合，提高计算的灵活性，减少计算量，增强容错能力是亟待解决的问题。
相对于集中式滤波，分布式(或称为分散化)滤波利用信息分配原则来消除各子状态估计的相关性，具有计算量小、设计灵活、容错性能好等特点。协方差交叉融合算法是基于局部估值和保守误差方差的凸组合，有效地避免了状态估计的发散，具有较好的一致性，能保证滤波状态估计的信息融合具有较高的精度。本发明采用分布式滤波的结构，在各个子滤波器中采用扩展卡尔曼滤波(简记为EKF)滤波算法，然后利用协方差交叉融合算法对各个子滤波器进行信息融合以及反馈，构造一种分布式容错滤波方法，用于提高火星大气进入段的自主导航精度和容错能力。
发明内容
本发明解决了现有火星大气进入段组合导航的非线性自主导航滤波算法无法消除有色噪声影响导航精度和容错能力较差的技术问题，本发明提出一种基于协方差交叉融合的火星进入段分布式自主导航方法，在各个子滤波器中采用扩展卡尔曼滤波滤波算法，利用协方差交叉融合算法对各个子滤波器进行信息融合和反馈，考虑了导航***模型中的有色噪声问题和多传感器容错信息融合的问题，有效地提高了火星大气进入段对探测器的自主导航精度，同时也提高了自主导航的容错能力。
为了解决上述技术问题，本发明的技术方案是：一种基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，步骤如下：
步骤一：建立火星大气进入段导航***的三自由度动力学模型：设火星大气进入段的大气层相对火星静止，状态向量取为r,v,γ,θ,λ,ψ分别表示探测器的高度、速度、飞行路径角、经度、纬度和航向角，ba为惯性测量单元IMU中加速度计的常值漂移；建立火星大气进入段的三自由度动力学模型：
其中，g(r)＝μ/r2为火星重力加速度，μ为火星的重力加速度常数，w(t)为零均值的高斯白噪声，t表示时间；
步骤二：建立火星大气进入段的量测模型：采用分布式导航滤波算法将IMU和探测器与一个信标的距离作为一个子***，共有三个子导航***，则相应的火星大气进入段的三个量测方程为：
其中，h1(·)、h2(·)、h3(·)分别表示三个子导航***的非线性量测函数，v1(t)、v2(t)和v3(t)分别为三个子导航***的量测噪声，R1表示火星探测器与表面信标1之间的距离，R2表示火星探测器与表面信标2之间的距离，R3表示火星探测器与在轨信标之间的距离，av为IMU输出的速度系下三个轴向加速度；ηa为IMU测量信息的量测噪声向量，ξR1、ξR2和ξR3分别为双向测距的量测噪声向量；ηa、ξR1、ξR2和ξR3为零均值的高斯白噪声；
其中，火星探测器与火星在轨信标或火星表面信标之间在火星质心惯性系下的距离：
其中，rl表示火星探测器的坐标，ri(i＝1,2,3)分别表示火星表面信标1、火星表面信标2、火星在轨信标的坐标；
步骤三：建立分布式容错自主导航***：三个量测方程结合动力学模型建立子滤波器分别进行状态估计，然后在信息融合器中采用协方差交叉融合算法对探测器的状态进行信息融合输出状态估计值，并反馈到导航***中进行时间更新；
步骤四：离散和线性化分布式容错自主导航***的三自由度动力学模型和量测模型：对三自由度动力学模型和三个量测方程分别进行离散化：
xk+1＝F(xk)+wk，
zik＝hi(xk)+vik,i＝1,2,3；
其中，xk(k＝1,2,3,…)表示k时刻的示状态值，F(xk)为f(x(t),t)离散后的非线性状态函数，hi(xk)为hi(x(t),t)离散后的非线性量测函数，wk和vik互不相关，wk的方差矩阵为Qk，vik的方差矩阵为Rik；
线性化离散后的动力学模型：将离散后的非线性状态函数F(xk)围绕估计值按泰勒级数展开到一阶项，得相应的线性动力学方程：
xk+1＝Φkxk+uk+wk
且：
其中，Φk为状态转移矩阵，为估计值的后验状态值；
线性化离散后的量测模型：将三个非线性离散函数hi(xk)围绕预测估计值按泰勒级数展开到一阶项，得相应的线性量测方程：
zik＝Hikxk+yik+vik,(i＝1,2,3)；
且
其中，为估计值的先验状态值，yik为量测控制项；
步骤五：建立基于EKF导航滤波算法的子滤波器：采用EKF滤波算法实现对子导航***的探测器的状态估计，得到子滤波器的探测器的高度r、速度v、飞行路径角γ、经度θ、纬度λ、航向角ψ、状态估计和误差方差矩阵
步骤六：建立基于协方差交叉融合算法的信息融合器：基于三个状态估计以及相应的误差方差矩阵采用协方差交叉融合算法进行信息融合得到第k+1步的状态估计和估计误差方差矩阵PCI,k+1，然后再将得到的和PCI,k+1传递给三个子滤波器对其状态和误差方差矩阵进行更新。
所述探测器的阻力加速度D和升力加速度L分别表示为：
其中，CD为阻力系数，CL为升力系数，S为探测器的参考面积，m为探测器的质量，为动压，ρ是火星进入段的大气密度，相应的指数大气密度模型为：
式中ρ0是火星进入段大气的标称密度，r0为火星的径向基准位置，hs为火星大气的定标高度。
所述IMU输出的速度系下三个轴向加速度av表示为；
av＝[-D -D·L/D·sinφ D·L/D·cosφ]T。
所述步骤五中EKF滤波算法的步骤为：
选取状态估计和误差方差矩阵的初始值分别为和P0；
由第k步的状态估计值和误差方差矩阵可知，第k+1步的预测状态估计为
第k+1步的预测误差方差矩阵为：
其次，第k+1步的滤波增益矩阵Kk为
最后，第k+1步的状态估计为
第k+1步的估计误差方差矩阵为
其中，I表示单位矩阵。
所述步骤六中协方差交叉融合算法的步骤为：
其中，三个加权系数ω1,ω2,ω3>0，满足ω1+ω2+ω3＝1，且由下式决定：
其中，||·||F表示Frobenius范数。
所述按泰勒级数展开到一阶项的方法是：针对一个无穷可微的函数f(x)，泰勒级数展开到一阶项为：
f(x)＝f(a)+f′(a)(x-a)，
其中，f′(a)表示函数f(x)在点x＝a处的一阶导数。
与传统的集中式导航滤波方法相比，本发明采用容错能力较强的分布式信息融合的方式进行自主导航，有利于提高无线电通信的利用效率，减少了自主导航滤波的计算量，提高了火星大气进入段自主导航***的容错能力，增强了导航滤波的稳定性，有效地提高了火星大气进入段对探测器的自主导航精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明火星大气进入段的组合导航方案。
图2为本发明的流程图。
图3为本发明分布式容错滤波的结构图。
图4为本发明分布式容错滤波与集中式滤波的误差对比图。
图5为本发明一个信标出现故障时分布式容错滤波的误差图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图2和图3所示，一种基于协方差交叉融合的火星进入段分布式自主导航方法，它包括以下六个步骤：
步骤一、建立火星大气进入段导航***的动力学模型。
在火星大气进入段，探测器被看成无动力的质点。忽略火星的自转，同时设火星大气进入段的大气层相对火星静止。状态向量取为建立火星大气进入段的三自由度动力学模型：
其中，t表示时间，r,v,γ,θ,λ,ψ分别表示探测器的高度、速度、飞行路径角、经度、纬度和航向角，ba为惯性测量单元(简记为IMU)中加速度计的常值漂移，g(r)＝μ/r2为火星重力加速度，μ为火星的重力加速度常数，w(t)为零均值的高斯白噪声，D和L分别表示为探测器的阻力加速度和升力加速度，且：
式中，CD为阻力系数，CL为升力系数，S为探测器的参考面积，m为探测器的质量，为动压，ρ是火星进入段的大气密度，相应的指数大气密度模型为：
式中，ρ0是火星进入段大气的标称密度，r0为火星的径向基准位置，hs为火星大气的定标高度。
其中，步骤一中的参数值分别为：火星的重力加速度μ＝42828.29×109m3/s2，火星的径向基准位置r0＝3437.2km，火星大气的定标高度hs＝7.5km。
其中，本发明的验证中升阻比L/D＝CL/CD、弹道系数B＝CDS/m和标称大气密度ρ0的参考值和估计值如表1所示。
表1验证中的参数值和估计值
步骤二、建立火星大气进入段的量测模型。
本发明以火星探测器的IMU提供的加速度、以及探测器与火星在轨信标或火星表面信标之间的距离信息作为火星探测器进入段自主导航的量测信息。为了减少计算量，增强导航***的容错性能，采用分布式导航滤波算法将IMU和探测器与一个信标的距离作为一个子***，共有三个子导航***，则相应的火星大气进入段的三个量测方程为
式中，h1(·)、h2(·)、h3(·)分别表示三个子导航***的非线性量测函数，v1(t)、v2(t)和v3(t)分别为三个子导航***的量测噪声，R1、R2和R3分别表示火星探测器与表面信标1、表面信标2和在轨信标之间的距离，ηa为IMU测量信息的量测噪声向量，为零均值的高斯白噪声，ξR1，ξR2和ξR3分别为双向测距的量测噪声向量,均为零均值的高斯白噪声；av为IMU输出的速度系下三个轴向加速度，且：
av＝[-D -D·L/D·sinφ D·L/D·cosφ]T (8)。
其中，火星探测器与火星在轨信标或火星表面信标之间通过双向无线电通信，可以获得二者在火星质心惯性系下的距离(双向无线电通信测距示意图参见图1)：
式中，rl表示火星探测器的坐标，ri(i＝1,2,3)分别表示火星表面信标1、火星表面信标2、火星在轨信标的坐标。
其中，本发明验证中选择两个火星在轨信标和一个火星表面固定信标，相应的信标的初始位置和速度分别如表2所示。
表2在轨信标和固定信标的初始位置和速度
本发明验证中量测IMU的三个轴向加速度常值漂移ba取为[0.3cos(t/100)0.3cos(t/200) 0.3sin(t/100)]Tm/s2。
步骤三、建立分布式容错自主导航***。
火星大气进入段的自主导航方案中有火星表面信标、火星在轨信标1和火星在轨信标2三个信标与火星探测器进行无线电通信，分别提供三个距离信息zi(i＝1,2,3)。为了减少导航滤波的计算量，提高自主导航***的容错性能力，对三组量测信息结合动力学模型建立子滤波器分别状态估计得状态估计值和误差方差矩阵然后再在信息融合器中采用协方差交叉融合算法对探测器的状态进行信息融合得状态估计值和误差方差矩阵PCI，输出状态估计值误差方差矩阵PCI以及相应的系数ωi(i＝1,2,3)反馈到导航***中进行时间更新。相应的分布式容错自主导航***结构图参见图3。
步骤四、离散和线性化分布式容错自主导航***的三自由度动力学模型和量测模型。
对上述动力学模型(1)和量测模型(5)～(7)分别进行离散化，得到：
xk+1＝F(xk)+wk (10)
zik＝hi(xk)+vik,i＝1,2,3 (11)，
式中，xk(k＝1,2,3,…)表示k时刻的示状态值，F(xk)为f(x(t),t)离散后的非线性状态函数，hi(xk)(i＝1,2,3)为hi(x(t),t)离散后的非线性量测函数，wk和vik(i＝1,2,3)互不相关，wk的方差矩阵为Qk，vik的方差矩阵为Rik，vik(i＝1,2,3)之间的相关性未知。
接着，将离散化后的动力学模型和量测模型式(10)和式(11)的四个方程线性化。也就是将式(10)中的离散后的非线性状态函数F(xk)围绕估计值按泰勒级数展开到一阶项，可得相应的线性动力学方程：
xk+1＝Φkxk+uk+wk (12)
且：
其中，Φk为状态转移矩阵，为估计值的后验状态值。
然后，将式(11)中的三个非线性离散函数hi(xk)(i＝1,2,3)围绕预测估计值按泰勒级数展开到一阶项，可得相应的线性量测方程：
zik＝Hikxk+yik+vik,(i＝1,2,3) (15)
且：
其中，为估计值的先验状态值，yik可以看作量测控制项。上述线性化动力学方程和线性化量测方程中，uk和yik可看作非随机的外作用项。
其中，步骤四中“将式(10)中的F(xk)围绕估计值按泰勒级数展开到一阶项”和“将式(11)中的三个非线性离散函数hi(xk)(i＝1,2,3)围绕预测估计值按泰勒级数展开到一阶项”中的泰勒级数展开到一阶项的方法是：针对一个无穷可微的函数f(x)，泰勒级数展开到一阶项为：
f(x)＝f(a)+f′(a)(x-a) (18)
式中，f′(a)表示函数f(x)在点x＝a处的一阶导数。
步骤五、建立基于EKF导航滤波算法的子滤波器。
针对上述离散化后的非线性导航***模型式(10)和式(11)，在子滤波器中采用经典的EKF滤波算法实现对子导航***的探测器的状态估计。所采用的EKF滤波算法实现步骤为：
首先，选取状态估计和误差方差矩阵的初始值分别为和P0。
接着，由第k步的状态估计值和误差方差矩阵可知，第k+1步预测状态估计为
第k+1步的一步预测误差方差矩阵为
其次，第k+1步的滤波增益矩阵Kk为
最后，第k+1步的状态估计为
第k+1步的估计误差方差矩阵为
其中，I表示单位矩阵。通过以上式(18)～式(22)共5步的循环可对火星探测器在子滤波器中的滤波状态进行实时估计，得到子滤波器的初步估计值，也即探测器的高度、速度、飞行路径角、经度、纬度和航向角，为下一步的协方差交叉融合提供状态估计和误差方差矩阵
步骤六、建立基于协方差交叉融合算法的信息融合器。
针对步骤五中子滤波器得到火星探测器的初步估计值，基于三个状态估计以及相应的误差方差矩阵采用协方差交叉融合算法进行信息融合得到第k+1步的状态估计和估计误差方差矩阵PCI,k+1，然后再将得到的和PCI,k+1传递给三个子滤波器对其状态和误差方差矩阵进行更新。所采用的协方差交叉融合算法为：
式中，三个系数ω1,ω2,ω3>0，且满足ω1+ω2+ω3＝1，且由下式决定：
其中，||·||F表示Frobenius范数，计算式为
本发明通过以上六个步骤，分别建立IMU和探测器与信标距离的分布式容错自主导航的量测模型，然后利用EKF导航滤波算法对火星探测器的状态进行初步的估计，然后利用协方差交叉融合算法对火星探测器的状态进行融合，从而减少导航滤波的计算量，提高火星大气进入段自主导航***的容错能力，并增强导航滤波的稳定性，达到高效实时高精度估计火星探测器导航状态的目的。
其中，本发明验证中需要对火星探测器的参数进行初始化，相应的探测器的参考值和估计值如表3所示。
表3探测器的参考值和估计值
本发明的分布式容错滤波与集中式EKF滤波的误差对比图4所示，本发明的一个信标出现故障时分布式容错滤波误差如图5所示。从图4可以看出，当三个信标都没有故障时，本发明分布式容错导航滤波(简记为DCIEKF)方法与集中式EKF导航滤波方法的误差几乎相当。但是，当其中一个信标在250s-300s出现故障时，集中式EKF导航滤波方法已经发散(图中未表示出)，而本发明的分布式容错导航滤波方法由于缺乏一个导航信息，导致在250s-300s滤波误差出现较大的波动，但是整体上滤波效果还是可以接受的。由此可以看出，本发明的分布式容错导航滤波方法具有较高的精度和容错能力。
由于火星大气进入段的探测器面临高温高热等恶劣的环境，探测器被包裹在防护罩内，导致仅仅只有IMU一直工作。基于火星大气进入段IMU可依靠甚高频无线电进行通信的研究，为火星探测器在进入段的自主导航提供了额外的信息，有利于提高探测器的自主导航精度。但是，探测器与火星在轨信标或火星表面信标之间的甚高频无线电通信也会存在被探测器周围的等离子鞘减弱甚至信号丢失的情况，所以采用集中式导航有可能不能完全获得相应的导航信息，而且也浪费了宝贵的计算机存储空间。因此，本发明采用容错能力较强的分布式信息融合的方式进行自主导航，有利于提高无线电通信的利用效率，达到提高火星探测器的自主导航精度。同时，本发明采用协方差交叉融合算法基于局部估值和保守误差方差的凸组合，避免了状态估计的发散，具有较好的一致性，保证了火星大气进入段的自主导航具有较高的精度和容错能力。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，其步骤如下：
步骤一：建立火星大气进入段导航***的三自由度动力学模型：设火星大气进入段的大气层相对火星静止，状态向量取为r,v,γ,θ,λ,ψ分别表示探测器的高度、速度、飞行路径角、经度、纬度和航向角，ba为惯性测量单元IMU中加速度计的常值漂移；建立火星大气进入段的三自由度动力学模型：
其中，g(r)＝μ/r2为火星重力加速度，μ为火星的重力加速度常数，D表示探测器的阻力加速度，L表示探测器的升力加速度，w(t)为零均值的高斯白噪声，t表示时间；
步骤二：建立火星大气进入段的量测模型：采用分布式导航滤波算法将IMU和探测器与一个信标的距离作为一个子***，共有三个子导航***，则相应的火星大气进入段的三个量测方程为：
其中，h1(·)、h2(·)、h3(·)分别表示非线性量测函数，v1(t)、v2(t)和v3(t)分别为量测噪声，R1表示火星探测器与表面信标1之间的距离，R2表示火星探测器与表面信标2之间的距离，R3表示火星探测器与在轨信标之间的距离，av为IMU输出的速度系下三个轴向加速度；ηa为IMU测量信息的量测噪声向量，ξR1、ξR2和ξR3分别为双向测距的量测噪声向量；ηa、ξR1、ξR2和ξR3为零均值的高斯白噪声；
火星探测器与火星在轨信标或火星表面信标之间在火星质心惯性系下的距离：
其中，rl表示火星探测器的坐标，ri分别表示火星表面信标1、火星表面信标2、火星在轨信标的坐标，i＝1,2,3；
步骤三：建立分布式容错自主导航***：三个量测方程结合动力学模型建立子滤波器分别进行状态估计，然后在信息融合器中采用协方差交叉融合算法对探测器的状态进行信息融合输出状态估计值，并反馈到导航***中进行时间更新；
步骤四：离散和线性化分布式容错自主导航***的三自由度动力学模型和量测模型：对三自由度动力学模型和三个量测方程分别进行离散化：
xk+1＝F(xk)+wk，
zik＝hi(xk)+vik,i＝1,2,3；
其中，xk表示k时刻的示状态值，k＝1,2,3,…，F(xk)为f(x(t),t)离散后的非线性状态函数，hi(xk)为hi(x(t),t)离散后的非线性量测函数，wk和vik互不相关，wk的方差矩阵为Qk，vik的方差矩阵为Rik；
线性化离散后的动力学模型：将离散后的非线性状态函数F(xk)围绕后验状态值按泰勒级数展开到一阶项，得相应的线性动力学方程：
xk+1＝Φkxk+uk+wk
且：
其中，Φk为状态转移矩阵，为估计值的后验状态值；
线性化离散后的量测模型：将三个非线性离散函数hi(xk)围绕先验状态值按泰勒级数展开到一阶项，得相应的线性量测方程：
zik＝Hikxk+yik+vik,i＝1,2,3；
且
其中，为估计值的先验状态值，yik可以看作量测控制项；
步骤五：建立基于EKF导航滤波算法的子滤波器：采用EKF滤波算法实现对子导航***的探测器的状态估计，得到子滤波器的探测器的高度、速度、飞行路径角、经度、纬度、航向角、第k+1步的先验状态值和误差方差矩阵
步骤六：建立基于协方差交叉融合算法的信息融合器：基于三个状态估计以及相应的误差方差矩阵采用协方差交叉融合算法进行信息融合得到第k+1步的状态估计和估计误差方差矩阵然后再将得到的和PCI,k+1传递给三个子滤波器对其状态和误差方差矩阵进行更新；
所述步骤六中协方差交叉融合算法的步骤为：
其中，三个加权系数ω1,ω2,ω3＞0，满足ω1+ω2+ω3＝1，且由下式决定：
其中，||·||F表示Frobenius范数。
2.根据权利要求1所述的基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，所述探测器的阻力加速度D和升力加速度L分别表示为：
其中，CD为阻力系数，CL为升力系数，S为探测器的参考面积，m为探测器的质量，为动压，ρ是火星进入段的大气密度，相应的指数大气密度模型为：
式中ρ0是火星进入段大气的标称密度，r0为火星的径向基准位置，hs为火星大气的定标高度。
3.根据权利要求1所述的基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，所述IMU输出的速度系下三个轴向加速度av表示为；
av＝[-D -D·L/D·sinφ D·L/D·cosφ]T。
4.根据权利要求1所述的基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，所述步骤五中EKF滤波算法的步骤为：
选取状态估计和误差方差矩阵的初始值分别为和P0；
由第k步的估计值和误差方差矩阵可知，第k+1步的先验状态估计为
第k+1步的预测误差方差矩阵为：
其次，第k+1步的滤波增益矩阵Kk为
最后，第k+1步的状态估计为
第k+1步的估计误差方差矩阵为
其中，I表示单位矩阵。
5.根据权利要求1所述的基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，所述按泰勒级数展开到一阶项的方法是：针对一个无穷可微的函数f(x)，泰勒级数展开到一阶项为：
f(x)＝f(a)+f′(a)(x-a)，
其中，f′(a)表示函数f(x)在点x＝a处的一阶导数。
CN201611000780.5A 2016-11-14 2016-11-14 基于协方差交叉融合的火星进入段分布式自主导航方法 Active CN106441309B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611000780.5A CN106441309B (zh) 2016-11-14 2016-11-14 基于协方差交叉融合的火星进入段分布式自主导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611000780.5A CN106441309B (zh) 2016-11-14 2016-11-14 基于协方差交叉融合的火星进入段分布式自主导航方法

Publications (2)

Publication Number Publication Date
CN106441309A CN106441309A (zh) 2017-02-22
CN106441309B true CN106441309B (zh) 2019-03-12

Family

ID=58207838

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611000780.5A Active CN106441309B (zh) 2016-11-14 2016-11-14 基于协方差交叉融合的火星进入段分布式自主导航方法

Country Status (1)

Country Link
CN (1) CN106441309B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107820260A (zh) * 2017-10-06 2018-03-20 廖忠民 平动点基干网络
CN109211233B (zh) * 2018-09-25 2020-10-23 常熟理工学院 基于加速度传感器的电梯运动检测及异常位置停车判断方法
CN111089580B (zh) * 2018-10-23 2023-02-10 北京自动化控制设备研究所 一种基于协方差交叉的无人战车同时定位与地图构建方法
CN111883265B (zh) * 2020-06-30 2024-07-09 南京理工大学 一种应用于火控***的目标状态估计方法
CN113375634B (zh) * 2021-04-30 2022-10-14 北京临近空间飞行器***工程研究所 基于大气模型和飞行器法向过载组合的高度测量方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7580907B1 (en) * 2004-01-14 2009-08-25 Evolved Machines, Inc. Invariant object recognition
CN103344245B (zh) * 2013-07-09 2015-11-18 北京航空航天大学 火星进入段imu和甚高频无线电组合导航的ud-skf方法
CN104266650B (zh) * 2014-10-25 2016-11-02 哈尔滨工业大学 一种基于采样点继承策略的火星着陆器大气进入段导航方法
CN105300387B (zh) * 2015-11-03 2018-04-10 北京航空航天大学 一种火星大气进入段非线性非高斯秩滤波方法
CN105844058B (zh) * 2016-04-19 2019-02-05 沈阳化工大学 一种污水处理过程的分布式扩展卡尔曼滤波状态估计方法

Also Published As

Publication number Publication date
CN106441309A (zh) 2017-02-22

Similar Documents

Publication Publication Date Title
CN106441309B (zh) 基于协方差交叉融合的火星进入段分布式自主导航方法
Hajiyev et al. Robust adaptive unscented Kalman filter for attitude estimation of pico satellites
Rhudy et al. Evaluation of matrix square root operations for UKF within a UAV GPS/INS sensor fusion application
Pascoal et al. Navigation system design using time-varying complementary filters
Lombaerts et al. Safe maneuvering envelope estimation based on a physical approach
CN103884340B (zh) 一种深空探测定点软着陆过程的信息融合导航方法
Johansen et al. Nonlinear observer for tightly integrated inertial navigation aided by pseudo-range measurements
Ramprasadh et al. Multistage-fusion algorithm for estimation of aerodynamic angles in mini aerial vehicle
Karmozdi et al. Implementation of translational motion dynamics for INS data fusion in DVL outage in underwater navigation
Lei et al. An adaptive navigation method for a small unmanned aerial rotorcraft under complex environment
CN113587926A (zh) 一种航天器空间自主交会对接相对导航方法
CN103344245A (zh) 火星进入段imu和甚高频无线电组合导航的ud-skf方法
Pourtakdoust et al. An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS
CN105136150B (zh) 一种基于多次星敏感器测量信息融合的姿态确定方法
Tan et al. Design and error analysis of accelerometer-based inertial navigation systems
Warier et al. Finite time stable attitude estimation of rigid bodies with unknown dynamics
Emran et al. Hybrid low-cost approach for quadrotor attitude estimation
Kaygısız et al. Enhancing positioning accuracy of GPS/INS system during GPS outages utilizing artificial neural network
Deilamsalehy et al. Heterogeneous multisensor fusion for mobile platform three-dimensional pose estimation
Mathisen et al. Robust navigation system for uavs in gnss-and magnetometer-denied environments
Kang et al. Autonomous observability of networked multisatellite systems
Hua et al. Introduction to nonlinear attitude estimation for aerial robotic systems
de Celis et al. An estimator for UAV attitude determination based on accelerometers, GNSS sensors, and aerodynamic coefficients
Ziyang et al. Information fusion distributed navigation for UAVs formation flight
Hajiyev et al. Integration of algebraic method and EKF for attitude determination of small information satellites

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant