火星大气进入段消除测量数据中系统误差的两步滤波方法

技术领域

本发明涉及火星大气进入段消除测量数据中系统误差的两步滤波方法。属于航天导航技 术领域。

背景技术

火星大气进入段是整个火星进入、下降、着陆阶段不确定因素最多，状态变化最快，气 动环境最复杂、精确着陆任务，火星大气进入段对着陆精度影响最大的阶段，因此要实现飞 行器的精确状态估计是必须得到保证的。未来采样返回与载人登陆等火星任务要求着陆误差 不超过1km，对火星大气进入段导航精度的要求势必会更高，故有必要对火星大气进入段的 导航算法进行研究。然而，决定火星大气进入段导航精度有两个先决条件：

一、火星大气进入段导航滤波的动力学模型必须精确；

二、高精度的传感器和进入导航算法。

滤波方法是非常常见的一种确定飞行器状态估计的方法。传统的滤波方法要求动力学系 统和量测系统的精确已知。但是，在实际工程应用中动力学系统和量测系统很难精确得到。 在火星大气进入段中，由于飞行器初始进入界面的状态具有不确定性，动力学模型中的时变 参数具有不确定性，大气密度具有不确定性，飞行器本身特性具有不确定性等等不确定性。 这些不确定性导致动力学系统很难精确得到，具有未知的偏差输入。另外现有量测手段有限， 现有的可以应用于火星大气进入段的量测手段有惯性导航量测方法和无线电测距方法，使得 量测系统存在未知的量测系统误差，例如无线电测距信标误差、无线电信号传递误差以及接 收误差。这些动力学系统偏差和测量系统中的未知测量系统误差对确定飞行器的最终状态影 响很大，过大的偏差和误差会导致状态误差的增大甚至发散，引起飞行器的导航误差、降低 导航精度。

基于以上的实际情况，建立了火星大气进入段消除测量数据中系统误差的两步滤波方法， 该方法对滤波量测系统的未知误差不敏感，可以很好地应用于火星大气进入段飞行器的状态 精确估计以满足未来火星的探测任务。

发明内容

1、目的：本发明的目的是提供一种火星大气进入段消除测量数据中系统误差的两步滤波 方法，以减小飞行器状态估计误差，提高其状态估计精度。

2、技术方案：本发明的目的是通过以下技术方案来实现的。

本发明一种火星大气进入段消除测量数据中系统误差的两步滤波方法，其步骤如下：

步骤一、建立基于火星大气进入段飞行器的工程实际方程：离散时间下的动力学系统和 量测系统

x_k+1=f(x_k)+B_ku_k+w_k   (1)

z_k=h(x_k)+H_kd_k+v_k   (2)

其中x_k表示系统状态量，z_k是测量系统测量值，u_k是状态方程的确定性控制项,d_k是未知的 量测系统误差；非线性方程f(·）和h(·)分别是状态转移方程和量测方程并且关于x_k可微；矩阵 B_k和H_k具有恰当的维数；w_k和v_k分别是动力学系统噪声向量和量测噪声向量，其方差阵分别 为Q_k和R_k，它们是不相关的高斯白噪声满足以下式子：

$\left(\begin{array}{c}E\left[w_k\right]=0\\ \mathrm{Cov}[w_k,w_j]=E\left[w_k{w}_j^T\right]=Q_k{\delta }_{\mathrm{kj}}\\ E\left[v_k\right]=0\\ \mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\delta }_{\mathrm{kj}}\\ \mathrm{Cov}[w_k,v_j]=E\left[w_k{v}_j^T\right]=0\end{array}\right);$   (3)

步骤二、给定初始值：和

其中，为初始状态的估计值，为初始状态的方差阵；

步骤三、两步滤波方法：

（1）、未知的量测系统误差估计

${\tilde{R}}_k{=C}_k{P}_{k/k-1}^x{C}_k^T+R_k$   (4)

$M_k={\left(H_k^T{\tilde{R}}_k^{-1}{H}_k\right)}^{-1}{H}_k^T{\tilde{R}}_k^{-1}$   (5)

${\hat{d}}_{k/k}=M_k(z_k-C_k{\hat{x}}_{k/k-1})$   (6)

$P_{k/k}^d={\left(H_k^T{\tilde{R}}_k^{-1}{H}_k\right)}^{-1}$   (7)

其中

$C_k={\frac{\partial h\left(x\right)}{\partial x}|}_{x={\hat{x}}_{k/k-1}}$   (8)

式中，为t_k时刻的新息均方误差估计，M_k为t_k时刻的未知的量测系统误差的滤波增益，为t_k时刻的未知的量测系统误差的估计，为状态的一步预测，z_k为t_k时刻的量测数据，为t_k时刻状态估计的均方误差，为t_k时刻未知量测系统误差估计的均方误差，C_k为量测阵；

（2）、更新

（a）、量测更新

$K_k=P_{k/k-1}^x{C}_k^T{\tilde{R}}_k^{-1}$   (9)

${\hat{x}}_{k/k}={\hat{x}}_{k/k-1}+K_k(z_k-C_k{\hat{x}}_{k/k-1}-H_k{\hat{d}}_{k/k})$   (10)

$P_{k/k}^x=P_{k/k-1}^x-K_k\left(\tilde{R}{-H}_k{P}_{k/k}^d{H}_k^T\right)K_k^T$   (11)

$p_{k/k}^{\mathrm{dx}}={\left(P_{k/k}^{\mathrm{xd}}\right)}^T=-{\left(K_k{H}_k{P}_{k/k}^d\right)}^T$   (12)

式中，为t_k-1时刻到t_k时刻的状态估计的一步预测均方误差，K_k为t_k时刻的状态滤波增益， 为t_k时刻的状态估计，为状态估计与未知量测系统误差估计的协方差，为的转 置矩阵；

（b）、时间更新

${\hat{x}}_{k+1/k}=A_k{\hat{x}}_{k/k}+B_k{u}_k+G_k{\hat{d}}_{k/k}$   (13)

$P_{k+1/k}^x=\left(\begin{array}{cc}{A}_k& G_K\end{array}\right)\left(\begin{array}{cc}{P}_{k/k}^x& P_{k/k}^{\mathrm{xd}}\\ P_{k/k}^{\mathrm{dx}}& P_{k/k}^d\end{array}\right)\left(\begin{array}{c}{A}_k^T\\ G_k^T\end{array}\right)+Q_k$   (14)

其中

$A_k={\frac{\partial f\left(x\right)}{\partial x}|}_{x={\hat{x}}_k},G_k=0.$   (15)

式中，A_k为t_k-1时刻到t_k时刻的一步转移矩阵，G_k为常数矩阵，在状态方程中无未知输入的情 况下，即在此情况下，G_k取为0矩阵；

步骤四、令k=k+1，返回步骤三往下进行。直到k等于火星大气进入时间截止对应的时 刻T时，至此完成火星大气进入段消除测量数据中系统误差的两步滤波方法。

其中，步骤一中要根据实际情况合理建立相应的矩阵B_k和H_k。其中非线性方程f(·)和h(·) 关于x_k可微是指，x_k是函数f(·)和h(·)定义域上的一点，且f(x_k)和h(x_k)的导数有定义。且式（3） 中δ_kj是克罗内克函数，在数学中，克罗内克函数δ_kj是一个二元函数，克罗内克函数的自变量 （输入值）一般是两个整数，如果两者相等，则其输出值为1，否则为0。

其中，在步骤一中所述的“建立基于火星大气进入段飞行器的工程实际方程”，其步骤如 下：

飞行器沿飞行轨迹进入火星大气，其对应的简化动力学系统为如下方程：

$\stackrel{·}{r}=v\sin \gamma$

$\stackrel{·}{v}=-(D+g_M\sin \gamma )$

$\stackrel{·}{\gamma }=(\frac{v}{r}-\frac{g_M}{v})\cos \gamma +\frac{1}{v}L\cos \sigma$

$\stackrel{·}{\theta }=\frac{v\cos \gamma \sin \psi }{r\cos \lambda }$   (16)

$\stackrel{·}{\lambda }=\frac{v}{r}\cos \gamma \cos \psi$

$\stackrel{·}{\psi }=\frac{v}{r}\sin \psi \cos \gamma \tan \lambda +\frac{L\sin \sigma }{v\cos \gamma }$

其中r飞行器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角, ψ是航向角,控制量σ是滚转角。火星引力GM_Mars=4.28221×10¹³m³/s²。

L，D分别是气动升力和助力定义为式：其中C_L和C_D为升力系 数和阻力系数。火星大气密度ρ近似满足指数表达形式，其表达形式为而 ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向 半径，大小为3437.2km。

目前火星进入段的量测方式主要靠加速度计和陀螺仪，另外有学者提出可以考虑现有在 轨的国外三颗通信卫星进行测量导航。由于目前状态下无法形成有效的导航网，量测系统存 在未知的量测系统误差。其对应的量测系统可以表示为：

${\tilde{a}}^B=a^B+a_a$   (17)

其中加速度计量测值,a^B加速度真实值,a_a未知的加速计系统误差包含偏差随机游走 等。

$\tilde{R}=R+a_R$   (18)

$R=\sqrt{{(r-r_i)}^T(r-r_i)}$   (19)

其中是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐 标系下的位置，r_i是第i个通信卫星在火星中心惯性坐标系下的位置，a_R是未知的量测系统误 差。

1)火星大气进入段对应的离散动力学系统可以改写为如下形式：

x_k+1=f(x_k)+B_ku_k+w_k   (20)

其中x_k为火星大气进入段动力学系统中的状态量包含式（16）中的左边的各分量，飞行器 到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ，航向角ψ。B_k为对应的动力学系统控制量对动力学系统的驱动阵，u_k是控制量对应于滚转角σ。

2)对应的离散量测方程为

z_k=h(x_k)+a_x+v_k   (21)

其中

$z=\left(\begin{array}{c}\widetilde{a^B}\\ \tilde{R}\end{array}\right)$   (22)

$h\left(x\right)=\left(\begin{array}{c}{a}^B\\ R\end{array}\right)$   (23)

式中，为加速度计的量测值，其具体表达式见式（17）；为无线电量测值，其具体表 达式见式（18）。h(x)包含加速度真实值和无线电测距真实值，其对应的真实表达式见式 （17）和式（19）。

其中，步骤二中根据实际情况估计初值。在火星大气进入段之前，飞行器状态估计值和 估计均方误差由大气层外飞行器飞行段末端得到。需要特别强调的是，飞行器初始状态估计 值要是无偏的。

其中，步骤三中由式（14）可以发现，虽然G_k=0，但是A_k的行数应该和G_k的行数保持 一致，而G_k的列数应该和d_k的行数保持一致。

3、优点和功效：

本发明统筹考虑了火星实际大气进入过程中，非线性、非高斯随机系统在量测系统存在 系统误差条件下的飞行器状态估计问题。通过应用消除测量数据中系统误差的两步滤波方法， 飞行器在火星大气进入段，该方法不仅可以估计出量测系统的测量数据中存在的系统误差， 而且可以运用估计出来的测量数据中存在的系统误差来更好的估计出飞行器的状态。通过计 算机仿真验证，该方法对系统的状态可以进行精确的估计，其中飞行器的位置误差可以达到 10m，飞行器的速度误差可以达到1m/s。因此该方法可以很好的满足未来火星探测任务的着 陆误差精度要求。

附图说明

图1为非线性增量系统的概率密度函数图解模型

图2为大气进入段飞行器扩展卡尔曼滤波方法与消除测量数据中系统误差的两步方法状 态估计误差比较

图3为火星大气进入段的消除测量数据中系统误差的两步滤波方法具体实施的流程图

图中的代号、符号说明如下：

EKF为扩展卡尔曼滤波方法。

ERTSF为消除测量数据中系统误差的两步滤波方法。

x_k为状态向量。

u_k是状态方程的确定性控制项。

d_k是未知的量测系统误差。

非线性方程f(·)和h(·)分别是状态转移方程和量测方程。

z_k为量测向量。

矩阵B_k和H_k分别是控制向量驱动阵和量测阵。。

w_k和v_k分别是动力学系统噪声向量和量测噪声向量

为t₀时刻状态估计的均方误差

具体实施方式

见图3，本发明一种火星大气进入段消除测量数据中系统误差的两步滤波方法，其具体 实施步骤如下：

飞行器沿飞行轨迹进入火星大气，其对应的简化动力学系统为如下方程。

$\stackrel{·}{r}=v\sin \gamma$

$\stackrel{·}{v}=-(D+g_M\sin \gamma )$

$\stackrel{·}{\gamma }=(\frac{v}{r}-\frac{g_M}{v})\cos \gamma +\frac{1}{v}L\cos \sigma$

$\stackrel{·}{\theta }=\frac{v\cos \gamma \sin \psi }{r\cos \lambda }$   (24)

$\stackrel{·}{\lambda }=\frac{v}{r}\cos \gamma \cos \psi$

$\stackrel{·}{\psi }=\frac{v}{r}\sin \psi \cos \gamma \tan \lambda +\frac{L\sin \sigma }{v\cos \gamma }$

其中r飞行器到火星中心的距离，v是飞行器的速度,θ是经度，λ是纬度,γ是飞行路径角, ψ是航向角,控制量σ是滚转角。火星引力GM_Mars=4.28221×10¹³m³/s²。

L，D分别是气动升力和助力定义为式：其中C_L和C_D为升力系 数和阻力系数。火星大气密度ρ近似满足指数表达形式，其表达形式为而 ρ₀为参考密度，h_s为火星大气标高，大小为7500m，r_s为距离火星表面40km的火星参考径向 半径，大小为3437.2km。

目前火星进入段的量测方式主要靠加速度计和陀螺仪，另外有学者提出可以考虑现有在 轨的国外三颗通信卫星（见图1）进行测量导航。由于目前状态下无法形成有效的导航网， 量测系统存在未知的量测系统误差。其对应的量测系统可以表示为：

${\tilde{a}}^B=a^B+a_a$   (25)

其中加速度计量测值,a^B加速度真实值,a_a未知的加速计系统误差包含偏差随机游走 等。

$\tilde{R}=R+a_R$   (26)

$R=\sqrt{{(r-r_i)}^T(r-r_i)}$   (27)

其中是无线电测量值,R是飞行器与通信卫星间的真实距离，r是飞行器在火星中心惯性坐 标系下的位置，r_i是第i个通信卫星在火星中心惯性坐标系下的位置，a_R是未知的量测系统误 差。

步骤一：建立工程实际方程：火星大气进入段对应的离散动力学系统可以改写为如下形 式：

x_k+1=f(x_k)+B_ku_k+w_k   (28)

其中x_k为火星大气进入段动力学系统中的状态量包含式（24）中的左边的各分量，飞行器 到火星中心的距离r，飞行器的速度v，经度θ，是纬度λ，飞行路径角γ，航向角ψ。B_k为对应的动力学系统控制量对动力学系统的驱动阵，u_k是控制量对应于滚转角σ。

对应的离散量测方程为

z_k=h(x_k)+a_x+v_k   (29)

其中

$z=\left(\begin{array}{c}\widetilde{a^B}\\ \tilde{R}\end{array}\right)$   (30)

$h\left(x\right)=\left(\begin{array}{c}{a}^B\\ R\end{array}\right)$   (31)

式中，为加速度计的量测值，其具体表达式见式（25）；为无线电量测值，其具体表 达式见式（26）。h(x)包含加速度真实值和无线电测距真实值，其对应的真实表达式见式 （25）和式（27）。

步骤二、给定初始值：

初始值为飞行器在大气外飞行末端（即大气进入界面）状态估计得到，如表一

表一火星大气进入的初始量及真实量

其中真实量为提前规划的火星大气进入点。实际上也存在一定的不确定性。初始状态估计均 方误差$P_0^x={10}^6\left(\begin{array}{cccccc}10& & & & & \\ & 0.1& & & & \\ & & {10}^{-10}& & & \\ & & & {10}^{-10}& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right).$

三个通信信标的初始位置如表二，而惯性导航和无线电测距的未知量测系统误差如表三。

表二火星表面通信信标的初始位置

表三未知量测系统误差

步骤三、两步滤波方法：

（1）、未知的量测系统误差估计

其中动力学系统噪声方差阵为$R_k=\left(\begin{array}{cccccc}{10}^{-10}& & & & & \\ & {10}^{-10}& & & & \\ & & {10}^{-10}& & & \\ & & & 10& & \\ & & & & 10& \\ & & & & & 10\end{array}\right),$量测噪声方差阵为 $Q=\left(\begin{array}{cccccc}10& & & & & \\ & {10}^{-10}& & & & \\ & & {10}^{-10}& & & \\ & & & 0.1& & \\ & & & & {10}^{-10}& \\ & & & & & {10}^{-10}\end{array}\right).$并且根据实际的量测系统选取相应的矩阵。当三个加速度 计系统误差一样，三颗卫星到飞行器的无线电测量系统误差一样，则得到相应矩阵 $G={\left(\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right)}^T,$$H={\left(\begin{array}{cccccc}1& 1& 1& 0& 0& 0\\ 0& 0& 0& 1& 1& 1\end{array}\right)}^T.$对于每个加速度计、无线电测量系统误差可以 重新设置相应的矩阵。

按照公式（4）-（8）进行滤波，对火星大气进入段未知的量测系统误差以及未知量测系 统误差估计的均方误差进行估计。

（2）、更新

（a）、量测更新

按照式（9）-式（12）对系统进行量测估计，包括对状态估计，状态估计的均方误差以 及状态估计与未知量测系统误差估计的协方差进行更新。

（b）、时间更新

按照式（13）-式（15）对系统进行时间更新，包括对状态估计的一步预测均方误差和状 态的一步预测进行更新。

步骤四、令k=k+1，返回步骤三往下进行。直到k等于火星大气进入时间截止对应的时 刻T时，至超音速降落伞打开为止。至此完成火星大气进入段消除测量数据中系统误差的两 步滤波方法。

其中截止时间主要取决于飞行器与火星表面的高度及速度，能否满足超音速降落伞打开。

从图2可以看出，在相同的初始条件下，消除测量数据中系统误差的两步滤波方法得到 的状态估计比扩展卡尔曼滤波的精度高，能够消除系统误差对滤波的影响，提高滤波的稳定 性。由于火星大气进入段飞行器与火星表面的高度及速度对于超音速降落伞打开的重要性， 所以有必要对火星大气进入段飞行器与火星表面的高度及速度的估计精度进行进一步的研 究。从图2的子图1和子图2可以看出，消除测量数据中系统误差的两步滤波方法给出的火 星大气进入段飞行器与火星表面的高度及速度的估计误差精度相对于由扩展卡尔曼滤波方法 给出的估计误差精度提高了很多。可以进一步看出，消除测量数据中系统误差的两步滤波方 法相对于扩展卡尔曼滤波的优越性。

以上所述仅为本发明较佳的实施方式，但本发明的保护范围并不局限于此，任何熟悉本 技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化和替换都应涵盖在本发 明的保护范围之内，另外本发明提供的方法可以集成到火星大气进入飞行器位置速度估计软 件中。