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

摘要

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

说明书

技术领域

本发明涉及火星大气进入段多传感器信息融合的技术领域，具体涉及一种基于协方差交叉融合的火星进入段分布式自主导航方法，可用于提高火星大气进入段自主导航的精度和容错能力。

背景技术

在火星大气进入段，火星探测器面临着峰值过载、高温高热等恶劣的环境，探测器被安装在防护罩内，导致大量的导航敏感器无法工作，仅仅依靠惯性测量单元(简记为IMU)进行自主导航，导航的精度难以满足未来火星探测的要求。基于火星大气进入段可依靠甚高频无线电进行通信的研究，学者们提出IMU和无线电测距的组合导航方案，为探测器在进入段的自主导航提供了额外的信息，提高了探测器的自主导航精度。但是，上述组合导航方法是集中式滤波的组合导航系统，面临着计算负担重、容错能力差等问题，而且不能保证滤波器的实时性。再者，由于探测器与火星在轨信标或表面信标之间的甚高频无线电通信会存在被探测器周围的等离子鞘减弱甚至信号丢失的情况，会使集中式滤波的容错能力进一步降低。因此，针对火星大气进入段的组合导航，选用合适的导航滤波方法对导航敏感器进行信息融合，提高计算的灵活性，减少计算量，增强容错能力是亟待解决的问题。

相对于集中式滤波，分布式(或称为分散化)滤波利用信息分配原则来消除各子状态估计的相关性，具有计算量小、设计灵活、容错性能好等特点。协方差交叉融合算法是基于局部估值和保守误差方差的凸组合，有效地避免了状态估计的发散，具有较好的一致性，能保证滤波状态估计的信息融合具有较高的精度。本发明采用分布式滤波的结构，在各个子滤波器中采用扩展卡尔曼滤波(简记为EKF)滤波算法，然后利用协方差交叉融合算法对各个子滤波器进行信息融合以及反馈，构造一种分布式容错滤波方法，用于提高火星大气进入段的自主导航精度和容错能力。

发明内容

本发明解决了现有火星大气进入段组合导航的非线性自主导航滤波算法无法消除有色噪声影响导航精度和容错能力较差的技术问题，本发明提出一种基于协方差交叉融合的火星进入段分布式自主导航方法，在各个子滤波器中采用扩展卡尔曼滤波滤波算法，利用协方差交叉融合算法对各个子滤波器进行信息融合和反馈，考虑了导航系统模型中的有色噪声问题和多传感器容错信息融合的问题，有效地提高了火星大气进入段对探测器的自主导航精度，同时也提高了自主导航的容错能力。

为了解决上述技术问题，本发明的技术方案是：一种基于协方差交叉融合的火星进入段分布式自主导航方法，其特征在于，步骤如下：

步骤一：建立火星大气进入段导航系统的三自由度动力学模型：设火星大气进入段的大气层相对火星静止，状态向量取为r,v,γ,θ,λ,ψ分别表示探测器的高度、速度、飞行路径角、经度、纬度和航向角，b_a为惯性测量单元IMU中加速度计的常值漂移；建立火星大气进入段的三自由度动力学模型：

其中，g(r)＝μ/r²为火星重力加速度，μ为火星的重力加速度常数，w(t)为零均值的高斯白噪声，t表示时间；

步骤二：建立火星大气进入段的量测模型：采用分布式导航滤波算法将IMU和探测器与一个信标的距离作为一个子系统，共有三个子导航系统，则相应的火星大气进入段的三个量测方程为：

其中，h₁(·)、h₂(·)、h₃(·)分别表示三个子导航系统的非线性量测函数，v₁(t)、v₂(t)和v₃(t)分别为三个子导航系统的量测噪声，R₁表示火星探测器与表面信标1之间的距离，R₂表示火星探测器与表面信标2之间的距离，R₃表示火星探测器与在轨信标之间的距离，a^v为IMU输出的速度系下三个轴向加速度；η_a为IMU测量信息的量测噪声向量，ξ_R1、ξ_R2和ξ_R3分别为双向测距的量测噪声向量；η_a、ξ_R1、ξ_R2和ξ_R3为零均值的高斯白噪声；

其中，火星探测器与火星在轨信标或火星表面信标之间在火星质心惯性系下的距离：

其中，r_l表示火星探测器的坐标，r_i(i＝1,2,3)分别表示火星表面信标1、火星表面信标2、火星在轨信标的坐标；

步骤三：建立分布式容错自主导航系统：三个量测方程结合动力学模型建立子滤波器分别进行状态估计，然后在信息融合器中采用协方差交叉融合算法对探测器的状态进行信息融合输出状态估计值，并反馈到导航系统中进行时间更新；

步骤四：离散和线性化分布式容错自主导航系统的三自由度动力学模型和量测模型：对三自由度动力学模型和三个量测方程分别进行离散化：

x_k+1＝F(x_k)+w_k，

z_ik＝h_i(x_k)+v_ik,i＝1,2,3；

其中，x_k(k＝1,2,3,…)表示k时刻的示状态值，F(x_k)为f(x(t),t)离散后的非线性状态函数，h_i(x_k)为h_i(x(t),t)离散后的非线性量测函数，w_k和v_ik互不相关，w_k的方差矩阵为Q_k，v_ik的方差矩阵为R_ik；

线性化离散后的动力学模型：将离散后的非线性状态函数F(x_k)围绕估计值按泰勒级数展开到一阶项，得相应的线性动力学方程：

x_k+1＝Φ_kx_k+u_k+w_k

且：

其中，Φ_k为状态转移矩阵，为估计值的后验状态值；

线性化离散后的量测模型：将三个非线性离散函数h_i(x_k)围绕预测估计值按泰勒级数展开到一阶项，得相应的线性量测方程：

z_ik＝H_ikx_k+y_ik+v_ik,(i＝1,2,3)；

且

其中，为估计值的先验状态值，y_ik为量测控制项；

步骤五：建立基于EKF导航滤波算法的子滤波器：采用EKF滤波算法实现对子导航系统的探测器的状态估计，得到子滤波器的探测器的高度r、速度v、飞行路径角γ、经度θ、纬度λ、航向角ψ、状态估计和误差方差矩阵

步骤六：建立基于协方差交叉融合算法的信息融合器：基于三个状态估计以及相应的误差方差矩阵采用协方差交叉融合算法进行信息融合得到第k+1步的状态估计和估计误差方差矩阵P_CI,k+1，然后再将得到的和P_CI,k+1传递给三个子滤波器对其状态和误差方差矩阵进行更新。

所述探测器的阻力加速度D和升力加速度L分别表示为：

其中，C_D为阻力系数，C_L为升力系数，S为探测器的参考面积，m为探测器的质量，为动压，ρ是火星进入段的大气密度，相应的指数大气密度模型为：

式中ρ₀是火星进入段大气的标称密度，r₀为火星的径向基准位置，h_s为火星大气的定标高度。

所述IMU输出的速度系下三个轴向加速度a^v表示为；

a^v＝[-D>T。

所述步骤五中EKF滤波算法的步骤为：

选取状态估计和误差方差矩阵的初始值分别为和P₀；

由第k步的状态估计值和误差方差矩阵可知，第k+1步的预测状态估计为

第k+1步的预测误差方差矩阵为：

其次，第k+1步的滤波增益矩阵K_k为

最后，第k+1步的状态估计为

第k+1步的估计误差方差矩阵为

其中，I表示单位矩阵。

所述步骤六中协方差交叉融合算法的步骤为：

其中，三个加权系数ω₁,ω₂,ω₃>0，满足ω₁+ω₂+ω₃＝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,γ,θ,λ,ψ分别表示探测器的高度、速度、飞行路径角、经度、纬度和航向角，b_a为惯性测量单元(简记为IMU)中加速度计的常值漂移，g(r)＝μ/r²为火星重力加速度，μ为火星的重力加速度常数，w(t)为零均值的高斯白噪声，D和L分别表示为探测器的阻力加速度和升力加速度，且：

式中，C_D为阻力系数，C_L为升力系数，S为探测器的参考面积，m为探测器的质量，为动压，ρ是火星进入段的大气密度，相应的指数大气密度模型为：

式中，ρ₀是火星进入段大气的标称密度，r₀为火星的径向基准位置，h_s为火星大气的定标高度。

其中，步骤一中的参数值分别为：火星的重力加速度μ＝42828.29×10⁹m³/s²，火星的径向基准位置r₀＝3437.2km，火星大气的定标高度h_s＝7.5km。

其中，本发明的验证中升阻比L/D＝C_L/C_D、弹道系数B＝C_DS/m和标称大气密度ρ₀的参考值和估计值如表1所示。

表1验证中的参数值和估计值

步骤二、建立火星大气进入段的量测模型。

本发明以火星探测器的IMU提供的加速度、以及探测器与火星在轨信标或火星表面信标之间的距离信息作为火星探测器进入段自主导航的量测信息。为了减少计算量，增强导航系统的容错性能，采用分布式导航滤波算法将IMU和探测器与一个信标的距离作为一个子系统，共有三个子导航系统，则相应的火星大气进入段的三个量测方程为

式中，h₁(·)、h₂(·)、h₃(·)分别表示三个子导航系统的非线性量测函数，v₁(t)、v₂(t)和v₃(t)分别为三个子导航系统的量测噪声，R₁、R₂和R₃分别表示火星探测器与表面信标1、表面信标2和在轨信标之间的距离，η_a为IMU测量信息的量测噪声向量，为零均值的高斯白噪声，ξ_R1，ξ_R2和ξ_R3分别为双向测距的量测噪声向量,均为零均值的高斯白噪声；a^v为IMU输出的速度系下三个轴向加速度，且：

a^v＝[-D>T(8)。

其中，火星探测器与火星在轨信标或火星表面信标之间通过双向无线电通信，可以获得二者在火星质心惯性系下的距离(双向无线电通信测距示意图参见图1)：

式中，r_l表示火星探测器的坐标，r_i(i＝1,2,3)分别表示火星表面信标1、火星表面信标2、火星在轨信标的坐标。

其中，本发明验证中选择两个火星在轨信标和一个火星表面固定信标，相应的信标的初始位置和速度分别如表2所示。

表2在轨信标和固定信标的初始位置和速度

本发明验证中量测IMU的三个轴向加速度常值漂移b_a取为[0.3cos(t/100)>Tm/s2。

步骤三、建立分布式容错自主导航系统。

火星大气进入段的自主导航方案中有火星表面信标、火星在轨信标1和火星在轨信标2三个信标与火星探测器进行无线电通信，分别提供三个距离信息z_i(i＝1,2,3)。为了减少导航滤波的计算量，提高自主导航系统的容错性能力，对三组量测信息结合动力学模型建立子滤波器分别状态估计得状态估计值和误差方差矩阵然后再在信息融合器中采用协方差交叉融合算法对探测器的状态进行信息融合得状态估计值和误差方差矩阵P_CI，输出状态估计值误差方差矩阵P_CI以及相应的系数ω_i(i＝1,2,3)反馈到导航系统中进行时间更新。相应的分布式容错自主导航系统结构图参见图3。

步骤四、离散和线性化分布式容错自主导航系统的三自由度动力学模型和量测模型。

对上述动力学模型(1)和量测模型(5)～(7)分别进行离散化，得到：

x_k+1＝F(x_k)+w_k(10)

z_ik＝h_i(x_k)+v_ik,i＝1,2,3(11)，

式中，x_k(k＝1,2,3,…)表示k时刻的示状态值，F(x_k)为f(x(t),t)离散后的非线性状态函数，h_i(x_k)(i＝1,2,3)为h_i(x(t),t)离散后的非线性量测函数，w_k和v_ik(i＝1,2,3)互不相关，w_k的方差矩阵为Q_k，v_ik的方差矩阵为R_ik，v_ik(i＝1,2,3)之间的相关性未知。

接着，将离散化后的动力学模型和量测模型式(10)和式(11)的四个方程线性化。也就是将式(10)中的离散后的非线性状态函数F(x_k)围绕估计值按泰勒级数展开到一阶项，可得相应的线性动力学方程：

x_k+1＝Φ_kx_k+u_k+w_k(12)

且：

其中，Φ_k为状态转移矩阵，为估计值的后验状态值。

然后，将式(11)中的三个非线性离散函数h_i(x_k)(i＝1,2,3)围绕预测估计值按泰勒级数展开到一阶项，可得相应的线性量测方程：

z_ik＝H_ikx_k+y_ik+v_ik,(i＝1,2,3)(15)

且：

其中，为估计值的先验状态值，y_ik可以看作量测控制项。上述线性化动力学方程和线性化量测方程中，u_k和y_ik可看作非随机的外作用项。

其中，步骤四中“将式(10)中的F(x_k)围绕估计值按泰勒级数展开到一阶项”和“将式(11)中的三个非线性离散函数h_i(x_k)(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滤波算法实现步骤为：

首先，选取状态估计和误差方差矩阵的初始值分别为和P₀。

接着，由第k步的状态估计值和误差方差矩阵可知，第k+1步预测状态估计为

第k+1步的一步预测误差方差矩阵为

其次，第k+1步的滤波增益矩阵K_k为

最后，第k+1步的状态估计为

第k+1步的估计误差方差矩阵为

其中，I表示单位矩阵。通过以上式(18)～式(22)共5步的循环可对火星探测器在子滤波器中的滤波状态进行实时估计，得到子滤波器的初步估计值，也即探测器的高度、速度、飞行路径角、经度、纬度和航向角，为下一步的协方差交叉融合提供状态估计和误差方差矩阵

步骤六、建立基于协方差交叉融合算法的信息融合器。

针对步骤五中子滤波器得到火星探测器的初步估计值，基于三个状态估计以及相应的误差方差矩阵采用协方差交叉融合算法进行信息融合得到第k+1步的状态估计和估计误差方差矩阵P_CI,k+1，然后再将得到的和P_CI,k+1传递给三个子滤波器对其状态和误差方差矩阵进行更新。所采用的协方差交叉融合算法为：

式中，三个系数ω₁,ω₂,ω₃>0，且满足ω₁+ω₂+ω₃＝1，且由下式决定：

其中，||·||_F表示Frobenius范数，计算式为

本发明通过以上六个步骤，分别建立IMU和探测器与信标距离的分布式容错自主导航的量测模型，然后利用EKF导航滤波算法对火星探测器的状态进行初步的估计，然后利用协方差交叉融合算法对火星探测器的状态进行融合，从而减少导航滤波的计算量，提高火星大气进入段自主导航系统的容错能力，并增强导航滤波的稳定性，达到高效实时高精度估计火星探测器导航状态的目的。

其中，本发明验证中需要对火星探测器的参数进行初始化，相应的探测器的参考值和估计值如表3所示。

表3探测器的参考值和估计值

本发明的分布式容错滤波与集中式EKF滤波的误差对比图4所示，本发明的一个信标出现故障时分布式容错滤波误差如图5所示。从图4可以看出，当三个信标都没有故障时，本发明分布式容错导航滤波(简记为DCIEKF)方法与集中式EKF导航滤波方法的误差几乎相当。但是，当其中一个信标在250s-300s出现故障时，集中式EKF导航滤波方法已经发散(图中未表示出)，而本发明的分布式容错导航滤波方法由于缺乏一个导航信息，导致在250s-300s滤波误差出现较大的波动，但是整体上滤波效果还是可以接受的。由此可以看出，本发明的分布式容错导航滤波方法具有较高的精度和容错能力。

由于火星大气进入段的探测器面临高温高热等恶劣的环境，探测器被包裹在防护罩内，导致仅仅只有IMU一直工作。基于火星大气进入段IMU可依靠甚高频无线电进行通信的研究，为火星探测器在进入段的自主导航提供了额外的信息，有利于提高探测器的自主导航精度。但是，探测器与火星在轨信标或火星表面信标之间的甚高频无线电通信也会存在被探测器周围的等离子鞘减弱甚至信号丢失的情况，所以采用集中式导航有可能不能完全获得相应的导航信息，而且也浪费了宝贵的计算机存储空间。因此，本发明采用容错能力较强的分布式信息融合的方式进行自主导航，有利于提高无线电通信的利用效率，达到提高火星探测器的自主导航精度。同时，本发明采用协方差交叉融合算法基于局部估值和保守误差方差的凸组合，避免了状态估计的发散，具有较好的一致性，保证了火星大气进入段的自主导航具有较高的精度和容错能力。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。