[发明专利]火星进入段IMU和甚高频无线电组合导航的UD-SKF方法

权利要求书

1.一种火星进入段IMU和甚高频无线电组合导航的UD-SKF方法，其特征在于：它包括以下四个步骤： 

步骤一、建立火星进入段的动力学方程 

在火星进入段，将探测器看作是一个无动力的质点，假设进入段的大气相对火星是静止的，同时忽略火星的自转，建立火星进入段的三自由度动力学方程： 

(1) 

式中，r,v,θ,λ,γ,ψ分别表示探测器距离火星中心的半径距离即高度、探测器的速度、经度、纬度、飞行路径角和航向角，火星重力加速度g(r)＝μ/r²，μ是火星重力加速度常数，D和L分别为阻力加速度和升力加速度： 

式中C_D和C_L分别为阻力系数和升力系数，S为探测器参考面积，m为探测器质量， 为动压，ρ是火星大气密度，此处采用指数大气密度模型： 

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

取状态向量为X=[r θ λ v γ ψ]^T，考虑到系统噪声向量w(t)，则动力学程（1）简写为 

式中，f(X(t),t)为系统非线性连续状态转移函数，w(t)为零均值的高斯白噪声； 

步骤二、建立火星进入段的量测方程 

以基于星间甚高频无线电测量通信的自主导航方案为基础，也即是将探测器与火星在轨信标或火星表面固定信标之间的双向测距信息作为测量信息和IMU提供的测量信息一起组合为新的测量信息，建立火星进入段量测方程： 

Z=h(X(t),t)+b+v(t)          (6) 

式中， 

(7) 

式中，a^v为速度系下的加速度，表示双向测距信息，维数m由有效的火星在轨信标和火星表面固定信标数目决定，b为常值偏差向量，v(t)为零均值的高斯白噪声，η_a为IMU测量信息的测量噪声向量，ξ_R为双向测距的测量噪声向量； 

其中，IMU的输出是由加速度计测量得到的本体系三个轴向的线加速度，测量模型为 

式中，表示各个轴的加速度测量值，a表示真实的加速度，b_a加速度计常值漂移，测量噪声η_a为零均值的高斯白噪声； 

其中，双向测距指的是探测器与火星在轨信标或火星表面固定信标之间通过双向无线电通信测量得到的二者之间的距离，表示为: 

式中，r_l和r_k分别表示火星质心惯性系下火星探测器和火星在轨信标或火星表面固定信标，b_R为双向测距的常值偏差，测距噪声ξ_R为零均值的高斯白噪声； 

步骤三、对上述动力学方程（5）和量测方程（6）进行离散化 

X_k+1=F(X_k)+w_k          （11） 

式中，k=1,2,3,…，F(X_k)为f(X(t),t)离散后的非线性状态转移函数，为h(X(t),t)离散后的非线性量测函数，w_k和v_k互不相关，且其方差矩阵分别为Q_k和R_k； 

然后将方程（11）和（12）进行线性化，也即将式（11）中的非线性离散函数F(X_k)围绕估计值展开成泰勒级数，并略去二阶以上项，得线性化之后的动力学方程 

X_k+1=Φ_k+1/kX_k+U_k+w_k           （13） 

式中， 

然后，再将式（12）中的非线性离散函数围绕估计值和展开成泰勒级数，并略去二阶以上项，得线性化之后的增量量测方程： 

Z_k=H_kX_k+Y_k+v_k                         （16） 

式中， 

通过上述过程，就得到了线性化后的动力学方程和量测方程，式中U_k和Y_k为非随机的外作用项； 

步骤四、采用UD-SKF滤波算法，并输出导航信息 

考虑式（6）中的常值偏差向量b未能精确建模，故施密特-卡尔曼滤波算法即SKF考虑将其方差融入到算法中，也即是考虑了偏差与状态的互协方差，但是不估计这些偏差，同时，在计算增益矩阵时采用UD分解以确保滤波的数值稳定性；其中步骤四所采用的UD-SKF滤波算法实现步骤为： 

步骤4.1.将常值偏差向量b扩维进入状态向量，则动力学方程（13）和量测方程 

（16）变为 

式中，我们考虑的常值偏差向量满足条件：b_k=b_k-1。其方差矩阵B₀为 

B₀=Cov{b₀}=Cov{b_k}                (21) 

和偏差与状态的互协方差矩阵C_k为 

并且初始值为C₀=0，式中，为卡尔曼滤波第k步的状态估计； 

同时，相应与动力学方程（19）和量测方程（20）第k步的误差方差阵为 

式中，为C_k转置矩阵，P_k为状态X_k的误差方差阵 

然后，初始化状态向量X₀及误差方差阵P₀； 

步骤4.2.由第k步的状态估计可得，第k+1步的状态一步预测为 

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

则得到状态和偏差的一步预测误差方差矩阵P_k+1|k和C_k+1|k

C_k+1|k=Φ_k+1|kC_k                   (28) 

步骤4.4.第k+1步的滤波增益矩阵为 

其中，由于不需要估计偏差，故强制令偏差项的增益矩阵为零； 

则状态的增益矩阵K_k+1为 

由于式（31）中的Ω_k+1在滤波过程中无法保持非负定性，此时式（30）中的增益矩阵K_k+1在求逆运算中会产生较大的误差，甚至会发散；为了保持滤波的数值稳定性，采用UD分解来计算增益矩阵K_k+1，会起到很好的效果；UD分解的详细步骤如下： 

1）将对称正定矩阵Ω_k分解成一个对角阵D和一个单位上三角阵U的形式： 

Ω_k=UDU^T                    (32) 

2）将式（32）代入是式（30）中，得到 

3）令则有 

4）通过以下的迭代过程得到 

DX₂=X₁                  (36) 

U^TX=X₂

5）最后令然后即得到K_k； 

步骤4.5.第k+1步的状态估计为 

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

则状态的估计误差方差矩阵P_k+1为 

和偏差与状态的互协方差矩阵为 

C_k+1=C_k+1|k-K_k+1(H_k+1C_k+1|k+B₀)           (40) 

通过以上6步循环进行即得到探测器的实时状态估计值，包含探测器的高度、速 度、经度、纬度、飞行路径角和航向角； 

通过上述四个步骤建立IMU和信标无线电组合导航的量测方程，然后利用UD-SKF滤波算法消除测量信息中的误差的影响，并增强滤波的稳定性，达到高效实时估计探测器导航状态的目的。