火星动力下降段减弱动力学系统误差的两步滤波方法

摘要

一种火星动力下降段减弱动力学系统误差的两步滤波方法，它包括以下步骤：一、建立工程实际方程；二、给定初始值；三、状态量滤波；四、动力学系统偏差滤波；五、更新相关系数、校正状态估计和动力学偏差估计；六、令k=k+1，返回步骤三往下进行，直到k等于火星动力下降段时间截止对应的时刻T时，即火星着陆器着陆为止，至此完成火星动力下降段减弱动力学系统误差的两步滤波方法。本方法统筹考虑了火星动力下降过程中，非线性、非高斯随机系统在动力学系统误差条件下的航天器位置速度估计问题。通过该方法在计算过程中引入对动力学系统误差进行估计和补偿，减弱了动力学系统误差对滤波引起的导航误差，有效保证航天器在火星动力下降段的位置速度估计。

说明书

技术领域

本发明涉及火星动力下降段减弱动力学系统误差的两步滤波方法。属于航天导航技术领 域。

背景技术

Kalman方法是非常常见的一种确定航天器位置速度方法。它要求动力学系统和量测系统 的精确已知。在实际工程中动力学系统难精确得到，存在一定的不确定性和动力学系统误差。 在火星动力下降段中，由于飞行器动力学系统采用加速度计和陀螺仪量（IMU）的惯性导航进 行航迹递推。而加速度计测量的为非惯性力，即火星重力加速度无法测得。通常只能利用事 先估计的重力带入计算，这样将产生较大的误差。如2012年美国的好奇号在其着陆点附近存 在着一定的重力加速度误差。未来火星任务将在一些具有科学意义的地点着陆，这些地点的 地貌复杂，其重力加速度将在火星着陆器着陆前更加难以精确测定，存在的重力加速度误差 将影响着高精度的着陆。另外，由于初始安装在着陆器上的测量仪器存在一定的安装误差， 这些误差将使得动力学系统和量测系统存在一定的未知系统误差。过大的误差会导致位置速 度误差的增大甚至发散，引起航天器导航误差、降低导航精度。为了方便描述，下文以火星 重力加速度为例，对火星重力加速度进行有效的估计和校正从而进一步提高导航精度。

现有技术中，可以用于确定航天器位置速度的方法有多种。

现有技术一，基于泰勒展开的扩展Kalman滤波估计方法。该方法忽略了动力学系统中的 误差，如火星重力加速度误差。该方法给出了非线性动力学方程和非线性测量方程的泰勒展 开加权融合的估算公式。

现有技术二，基于sigma点集（为正态分布采样策略）的无迹Kalman滤波方法。先根据 正态分布的均值和方差计算出sigma点集，并确定出各点的权值，再通过动力学方程计算出 航天器的位置速度，然后通过量测方程得到的量测数据对航天器的位置速度进行调整修正。 它同样不考虑火星重力加速度误差对动力学系统的影响。

现有技术一适用于动力学系统和量测系统精确可知条件下。在火星动力下降过程中，火 星重力加速度具有一定的不可估计的误差，因此不太适用于火星动力下降段。

现有技术二在测量手段有限，测量数据少，难以动力学系统误差估算和减弱。因此动力 学系统中的误差将影响航天器的位置速度估计，难以进行调整修正，因此不太适用于火星动 力下降段。

现有技术一二对于未知的火星环境，其动力学系统存在误差，如火星重力加速度误差。 即使事先对着陆地点的重力加速度进行估计也还存在一定的误差。特别是在一些地形地貌复 杂的具有科学探测的着陆点附近，火星重力加速度将难以进行事先估计校正。因此不太适用 于火星动力下降段。

发明内容

1、目的：本发明的目的是提供一种火星动力下降段减弱动力学系统误差的两步滤波方法， 以减小航天器位置速度误差，提高其精度。

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

本发明一种火星动力下降段减弱动力学系统误差的两步滤波方法，它包括以下步骤：

步骤一、建立工程实际方程：离散的动力学系统和量测系统

$x_{k+1}=f(x_k,u_k)+B_k{b}_{\mathrm{gk}}+w_k^x---\left(1\right)$

$b_{\mathrm{gk}+1}=A_k{b}_{\mathrm{gk}}+w_k^b---\left(2\right)$

z_k=h(x_k)+v_k  (3)

其中x_k表示系统状态量，z_k是测量系统测量值,b_gk是未知的动力学系统误差（即火星重力加速 度误差）。非线性方程f(·)和h(·)分别是状态转移方程和量测方程并且关于可x_k微。矩阵B_k为 未知的动力学系统误差（即火星重力加速度误差）对动力系统的驱动矩阵，矩阵A_k为未知的 动力学系统误差（即火星重力加速度误差）转移矩阵，矩阵B_k,A_k具有恰当的维数。是动 力学系统误差的噪声，和v_k分别是动力学系统噪声和量测噪声，它们是不相关的高斯白噪 声满足以下式子。

$E\left[\left(\begin{array}{c}{w}_k^x\\ w_k^b\\ v_k\end{array}\right){\left(\begin{array}{c}{w}_k^x\\ w_k^b\\ v_k\end{array}\right)}^T\right]=\left(\begin{array}{ccc}{Q}_k^x& 0& 0\\ 0& Q_k^b& 0\\ 0& 0& R_k\end{array}\right){\delta }_{\mathrm{kj}}---\left(3\right)$ 其中R_k≥0，δ_kj是δ函数,当k=j时δ_kj=1当k≠j时δ_kj=0.

步骤二、给定初始值：

$E\left[x_0\right]={\hat{x}}_0,$$E\left[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\right]=P_0^x\ge 0$

$E\left[b_{g0}\right]={\hat{b}}_{g0},$$E\left[(b_{g0}-{\hat{b}}_{g0}){(b_{g0}-{\hat{b}}_{g0})}^T\right]=P_0^{\mathrm{bg}}\ge 0$

$E\left[(x_0-{\hat{x}}_0){(b_{g0}-{\hat{b}}_{g0})}^T\right]=P_0^{\mathrm{xbg}}\ge 0$

为初始状态的估计值，为初始状态估计均方误差，为初始误差的估计值通常取0，为初始误差的估计均方误差，为初始状态和初始误差的估计均方误差。将这些以上初始 值通过以下式子计算得到滤波初始条件。

${\bar{x}}_0(+)={\hat{x}}_0-V_0{\hat{b}}_{g0},$${\bar{b}}_{g0}(+)={\hat{b}}_{g0},$$V_0=P_0^{\mathrm{xbg}}{\left(P_0^{\mathrm{bg}}\right)}^{-1},$${\bar{P}}_0^x(+)=P_0^x-V_0{P}_0^{\mathrm{bg}}{V}_0^T,$${\bar{P}}_0^{\mathrm{bg}}(+)=P_0^{\mathrm{bg}}$

其中，为滤波初始状态的估计值，为滤波初始状态估计均方误差，为滤波初 始误差的估计值通常取0，为滤波初始误差的估计均方误差阵，V₀为滤波初始状态和初 始误差的相关系数。

步骤三、状态量滤波

${\hat{x}}_k(-)=f({\hat{x}}_{k-1},u_{k-1})++{\bar{u}}_{k-1}---\left(4\right)$

${\bar{P}}_k^x(-)={\Phi }_{k-1}{\bar{P}}_{k-1}^x(+){\Phi }_{k-1}^T+{\bar{Q}}_{k-1}^x---\left(5\right)$

${\bar{K}}_k^x={\bar{P}}_k^x(-)H_k^T{[H_k{\bar{P}}_k^x(-)H_k^T+R_k]}^{-1}---\left(6\right)$

${\bar{P}}_k^x(+)=(I-{\bar{K}}_k^x{H}_k){\bar{P}}_k^x(-)---\left(7\right)$

${\bar{\eta }}_k^x=z_k-H_k{\bar{x}}_k(-),$${\bar{x}}_k(+)={\bar{x}}_k(-)+{\bar{K}}_k^x{\bar{\eta }}_k^x---\left(8\right)$

其中

${\Phi }_k=\frac{\partial f\left(x\right)}{\partial x}{|}_{x={\hat{x}}_k(+)}$$H_k=\frac{\partial h\left(x\right)}{\partial x}{|}_{x={\hat{x}}_k(-)}.---\left(10\right)$

式中：为t_k-1时刻的状态量，u_k-1为t_k-1时刻的控制输入量，为t_k-1时刻的滤波校正输 入量。为状态的滤波一步预测。为t_k-1时刻的滤波状态估计均方误差，Φ_k-1为t_k-1时 刻到t_k时刻的一步转移矩阵；为系统的噪声的滤波校正方差阵，为一步预测均方误 差。H_k为量测阵，为状态增益。I为单位阵，为t_k时刻的滤波状态估计均方误差。 为量测新息，为滤波状态估计。为状态的一步预测由式（17）计算得到。

步骤四、动力学系统偏差滤波

${\overline{b_g}}_k(-)=A_{k-1}{\overline{b_g}}_{k-1}(+)---\left(11\right)$

${\bar{P}}_k^{b_g}(-)=A_{k-1}{\bar{P}}_{k-1}^{b_g}(+)A_{k-1}^T+Q_{k-1}^{b_g}---\left(12\right)$

${\bar{K}}_k^{b_g}={\bar{P}}_k^{b_g}(-)N_k^T{[H_k{\bar{P}}_k^x(-)H_k^T+R_k+N_k{\bar{P}}_k^{b_g}(-)N_k^T]}^{-1}---\left(13\right)$

${\bar{P}}_k^{b_g}(+)=(I-{\bar{K}}_k^{b_g}{N}_k){\bar{P}}_k^{b_g}(-)---\left(14\right)$

${\bar{\eta }}_k^{b_g}=z_k-H_k{\bar{x}}_k(-)-N_k{\overline{b_g}}_k(-),$${\overline{b_g}}_k(+)={\overline{b_g}}_k(-)+{\bar{K}}_k^{b_g}{\bar{\eta }}_k^{b_g}---\left(15\right)$

其中步骤三步骤四中的对应式子为

$N_k^T=H_k{U}_k,$$U_k={\bar{U}}_k[I-Q_{k-1}^{b_g}{\left[{\bar{P}}_k^{b_g}(-)\right]}^{-1}],$${\bar{U}}_k=({\Phi }_{k-1}{V}_{k-1}+B_{k-1})A_{k-1}^{-1}$

$V_k=U_k-{\bar{K}}_k^x{N}_k,$${\bar{u}}_k=({\bar{U}}_{k+1}-U_{k+1})A_k{\overline{b_g}}_k(+)\mathrm{and}{\bar{Q}}_k^x=Q_k^x+U_{k+1}{Q}_k^{b_g}{\bar{U}}_{k+1}^T$

式中，A_k-1为t_k-1时刻动力学系统误差的转移矩阵，为t_k-1时刻动力学系统误差（火 星重力加速度误差）的滤波估计，为动力学系统误差（火星重力加速度误差）的一步预 测，为t_k-1时刻动力学系统误差的估计均方误差阵，为动力学系统误差的一步预 测均方误差，为动力学系统误差的噪声的方差阵，为动力学系统误差状态增益，R_k为 量测噪声的方差阵。为t_k时刻动力学系统误差的估计均方误差阵，为动力学系统误差 （火星重力加速度误差）的量测新息，为动力学系统误差的状态估计。N_k为t_k时刻动力 学系统偏差对量测系统校正量测阵。为系统噪声方差阵。U_k，V_k为状态估计均方误差 阵U-V分解的对应阵。

步骤五、更新相关系数、校正状态估计和动力学偏差估计：情况如下

$U_k={\bar{U}}_k[I-Q_{k-1}^{b_g}{\left[{\bar{P}}_k^{b_g}(-)\right]}^{-1}],$${\bar{U}}_k=({\Phi }_{k-1}{V}_{k-1}+B_{k-1})A_{k-1}^{-1},$$V_k=U_k-{\bar{K}}_k^x{N}_k---\left(16\right)$

${\hat{x}}_k(-)={\bar{x}}_k(-)+U_k{\overline{b_g}}_k(-),$${\hat{x}}_k(+)={\bar{x}}_k(+)+V_k{\overline{b_g}}_k(+)---\left(17\right)$

$P_k^x(-)={\bar{P}}_k^x(-)+U_k{\bar{P}}_k^b(-)U_k^T,$$P_k^x(+)={\bar{P}}_k^x(+)+U_k{\bar{P}}_k^b(+)U_k^T---\left(18\right)$

为t_k时刻校正后的状态量，为t_k时刻校正后的状态估计均方误差，为t_k时 刻校正后的状态量，为t_k时刻校正后的状态估计均方误差。

步骤六、令k=k+1，返回步骤三往下进行。直到k等于火星动力下降段时间截止对应的 时刻T时，即火星着陆器着陆为止。至此完成火星动力下降段减弱动力学系统误差的两步滤 波方法。

其中，步骤一中要根据实际情况合理建立相应的矩阵B_k,A_k。

其中，步骤二中根据实际情况估计初值，为火星动力下降段之前的伞降段末端状态估计 值和估计均方误差。且动力学系统偏差和测量系统中的未知测量系统误差为不相关量。

其中，在步骤一中所述的建立工程实际方程，其步骤如下：

a、分析动力学不确定性之间的相互关系，并进行相应的数值计算分析；

b、得到这些不确定性因素对动力学模型的影响主要是通过哪些量对动力学模型产生影响 从而引起误差的传播；

c、将动力学系统x_k+1=f(x_k,u_k)改写为考虑动力学系统偏差的动力学系统 $x_{k+1}=f(x_k,u_k)+B_k{b}_{\mathrm{gk}}+w_k^x.$

3、优点和功效：

本发明统筹考虑了火星动力下降过程中，非线性、非高斯随机系统在动力学系统误差条 件下的航天器位置速度估计问题。通过火星动力下降段减弱火星重力加速度误差的两步滤波 方法在计算过程中引入了对动力学系统误差进行估计和补偿，减弱了动力学系统误差对滤波 引起的导航误差。因而本发明提出的算法可以有效保证航天器在火星动力下降段的位置速度 估计。

附图说明

图1为三轴位移误差在在本文方法和传统EKF方法下的计算结果

图2为三轴速度误差在在本文方法和传统EKF方法下的计算结果

图3上图为高度方向位移误差，下图为高度方向速度误差（利用本文方法）

图4上图为火星重力加速偏差值和估计值，下图为火星重力加速度估计误差

图5为本发明所述方法流程图

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

TEKF为火星动力下降段减弱动力学系统误差的两步滤波方法

EKF为火星动力下降段的扩展Kalman滤波方法

具体实施方式

本发明涉及火星动力下降段减弱动力学系统误差的两步滤波方法，其航天器在火星动力 下降过程中，其依靠IMU的火星着陆动力学系统如下方程：

$\stackrel{·}{x}=f\left(x\right)={\left(\begin{array}{c}v\\ T_m^l(\tilde{a}-b_a-{\xi }_a)+g\\ K(\tilde{\omega }-b_{\omega }-{\xi }_{\omega })\\ {\xi }_a\\ {\xi }_{\omega }\end{array}\right)}_{15\times 1}+B_k{b}_g=\left(\begin{array}{c}v\\ T_m^l(\tilde{a}-b_a)+g\\ K(\tilde{\omega }-b_{\omega })\\ 0\\ 0\end{array}\right)+B_k{b}_g+\left(\begin{array}{c}0\\ -T_m^l{\xi }_a\\ -K{\xi }_{\omega }\\ {\xi }_a\\ {\xi }_{\omega }\end{array}\right)---\left(19\right)$

$b_g=b_g+w_k^b---\left(20\right)$

其中为动力学系统的状态量，r=[r₁,r₂,r₃]^T为相对火星着陆点坐标系下的三 轴位移，v=[v₁,v₂,v₃]^T为火星着陆器相对火星着陆点坐标系下的三轴速度，e=[φ,θ,ψ]^T为三轴 Euler姿态角，φ,θ,ψ分别为滚转角、俯仰角、航向角。为加速度计测量值，为陀螺仪测量 值，b_a为加速度计偏差，b_ω为陀螺仪偏差，g为火星重力加速度，b_g为火星重力加速度偏差。 ξ_a为加速度计测量白噪声，ξ_ω为陀螺仪测量白噪声。为测量仪器坐标系到火星着陆点坐标 系的转换矩阵。K为姿态运动学矩阵。矩阵B_k=[0_1×5 1 0_1×9]^T，矩阵A_k=1。

目前火星动力下降段，目前美国喷气推力实验室（JPL）开发了一种集高度计和速度计为 一体的小体积、低能耗的微型传感器（MCAV），它采用激光测距测速来实现高度、速度信息 的同时输出。其对应的测量系统方程可以如下式来表示：

$z=h\left(x\right)=\left(\begin{array}{c}{r}_3\\ T_l^{m}v\end{array}\right)---\left(21\right)$

其中，r₃为高度方向，为火星着陆点坐标系到测量仪器坐标系的转换矩阵，v=[v₁,v₂,v₃]^T为 火星着陆器相对火星着陆点坐标系下的三轴速度，即为测量仪器测得速度数据。

本发明一种火星动力下降段减弱动力学系统误差的两步滤波方法，见图5所示，它包括 以下步骤：

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

x_k+1=f(x_k,w_k)+B_kb_gk  （22）

其中x为火星动力下降段动力学系统中的状态量具体包含式（19）中的左边的各分量，即 矩阵B_k=[0_1×5 1 0_1×9]^T。其对应的离散量测方程为

z_k=h(x_k)+v_k  (23)

其中

$z_k=\left(\begin{array}{c}h\\ v^b\end{array}\right)={\left(\begin{array}{c}{r}_3\\ v_x\\ v_y\\ v_z\end{array}\right)}_m---\left(24\right)$

式中，测量高度h=r₃，v^b=[v_x v_y v_z]^T是加速度计在体坐标系下的测量值，可以通过式 （21）进行换算。v_k为量测噪声。

步骤二、给定初始值：

初始值为飞行器在伞降段末端状态估计得到，如表一

表一 火星动力下降段的真实值及初始估计值

其中本文中火星动力下降段的真实值及初始估计值为仿真选取值。也可以是另外两组不相同 的数值。初始状态估计均方误差$P_0=\left(\begin{array}{ccccc}400I_{3\times 3}& & & & \\ & I_{3\times 3}& & & \\ & & 1\times {10}^{-8}{I}_{3\times 3}& & \\ & & & 5\times {10}^{-6}{I}_{3\times 3}& \\ & & & & 2\times {10}^{-11}{I}_{3\times 3}\end{array}\right),$初始状态和 初始误差的估计均方误差初始误差的估计均方误差初始误差的估计

步骤三、状态量滤波：

按照公式（4）-（10）进行滤波，对火星动力下降段的状态进行估计。其中动力学系统噪声 方差阵为$Q_k^x=\left(\begin{array}{ccccc}{0}_{3\times 3}& & & & \\ & 4\times {10}^{-8}{I}_{3\times 3}& & & \\ & & 9\times {10}^{-12}{I}_{3\times 3}& & \\ & & & 1\times {10}^{-10}{I}_{3\times 3}& \\ & & & & 1\times {10}^{-16}{I}_{3\times 3}\end{array}\right),$量测噪声方差阵为 $R_k=\left(\begin{array}{cc}1& \\ & 0.01I_{3\times 3}\end{array}\right),$动力学系统误差的噪声的方差阵t_k-1时刻到t_k时刻的一步转移 矩阵Φ_k-1=I+F_k-1·Δt，t_k时刻的量测阵$H_k=\frac{\partial h\left(x\right)}{\partial x}{|}_{x={\hat{x}}_k(-)}=\left(\begin{array}{c}\frac{\partial r\left(3\right)}{\partial x}\\ \frac{\partial T_l^{m}v}{\partial x}\end{array}\right)=\left(\begin{array}{ccccc}\left(\begin{array}{ccc}0& 0& 1\end{array}\right)& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& T_l^m& \frac{\partial T_l^{m}v}{\partial e^T}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right).$其中，I_3×3为3阶单位矩阵，$F_{k-1}=\frac{\partial f\left(x\right)}{\partial x}{|}_{x=x_{k-1}}=\left(\begin{array}{ccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \frac{\partial T_m^l(\tilde{a}-b_a-{\xi }_a)}{\partial e}& -T_m^l& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& \frac{\partial K(\tilde{\omega }-b_{\omega }-{\xi }_{\omega })}{\partial e}& 0_{3\times 3}& -K\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right),$Δt为测量 时间间隔。按照公式（16）校正后再代入量测阵H_k中。

步骤四、动力学系统偏差滤波：

按照式（11）-式（15）对动力学系统偏差进行滤波估计，其中根据实际动力学系统分析 选取相应矩阵A_k-1=1。

步骤五、更新相关系数、校正状态估计和动力学偏差估计：

按照式（16）-式（18）对步骤三、四中的状态及其均误差进行校正。

步骤六、令k=k+1，返回步骤三往下进行。直到k等于火星动力下降段时间截止对应的 时刻T时。至此完成火星动力下降段减弱动力学系统误差的两步滤波方法。

其中截止时间主要取决于飞行器与火星表面的高度及速度，能否满足软着陆的条件要求。

通过火星动力下降段减弱动力学系统误差的两步滤波方法得到着陆器三轴位移误差在在 本文方法和传统EKF方法下的计算结果见图1，图2为三轴速度误差在在本文方法和传统EKF 方法下的计算结果；图3上图为高度方向位移误差，下图为高度方向速度误差（利用本文方 法）。图4上图为火星重力加速偏差值和估计值，下图为火星重力加速度估计误差。通过以上 步骤，火星动力下降段减弱动力学系统误差的两步滤波方法能够成功消除动力学模型中的偏 差，有效地提高导航的精度。

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