[发明专利]火星动力下降段多源信息组合导航的迭代SKF方法

权利要求书

1.一种火星动力下降段多源信息组合导航的迭代SKF方法，其特征在于：该方法步骤如下：

步骤一、利用火星动力下降段的动力学方程

火星动力下降段的情况比较复杂，建立精确的动力学模型非常困难，在考虑IMU输出的基础之上，利用其构造动力下降段的动力学方程：

r·=v]]>

v·=Cbi(a~-ba)+Cgig-Cbiηa---(1)]]>

Ω·=K(ω~-bω)-Kηω]]>

式中，r=[x y z]^T表示着陆点固连系下的位置向量，v=[v_x v_y v_z]^T表示着陆点固连系下的速度向量，Ω=[σ θ ψ]^T表示着陆点固连系下的姿态角，控制量σ为探测器的滚转角，θ为探测器的经度，ψ为探测器的航向角；表示火星中心惯性系到探测器本体坐标系的转换矩阵，表示火星地理坐标系到火星中心惯性系的转换矩阵；表示IMU中由加速度计输出的本体坐标系下的三个轴向的线加速度，b_a表示加速度计的常值漂移，η_a表示加速度计的噪声；表示IMU中由陀螺输出的本体坐标系下的三个轴向的瞬时旋转角速度，b_ω表示陀螺的常值漂移，η_ω表示陀螺的噪声；g表示地理坐标系的火星重力加速度；K表示姿态运动学矩阵

K=1cosθcosθsinθsinσsinθcosσ0cosθcosσ-cosθsinσ0sinσcosσ---(2)]]>

取状态向量为X=[r^T v^T Ω^T]^T，则动力下降段的动力学方程（1）简化为

X·=f(X)+w---(3)]]>

式中，f(X)为系统非线性连续状态转移函数，系统噪声w=03×1-CbiηaT-KηωTT]]>为零均值的高斯白噪声；

步骤二、火星动力下降段的量测方程

固定在本体坐标系上的微型传感器MCAV输出的是探测器的高度和三个轴的速率，则以其测量信息为基础建立火星动力下降段的量测方程：

Z=h(X)+b+v_m  (4)

式中，

h(X)=[r_z v^b]^T，  (5)

r_z为探测器的Z轴坐标，即探测器距离火星表面的高度，v^b为本体坐标系下的速度，其表达式为

vb=Cibv---(6)]]>

式中，表示探测器本体坐标系到火星中心惯性系的转换矩阵，b为常值偏差向量，测量噪声v_m为零均值的高斯白噪声，且与系统噪声w不相关；

步骤三、离散化上述动力学方程（3）和量测方程（4），得

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

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

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

Xk+1=Φk+1|kXk+Uk+wk---(9)]]>

式中，

Φk+1|k=∂F(Xk)∂Xk|Xk=X^k---(10)]]>

Uk=F(X^k)-∂F(Xk)∂Fk|Xk=X^k·X^k---(11)]]>

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

Z_k=H_kX_k+Y_k+v_k  （12）

式中，

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

步骤四、迭代SKF滤波算法及导航信息输出

由于式（4）中的常值偏差b未能精确知道，故施密特-卡尔曼滤波算法即SKF在不估计这些偏差的基础之上，考虑将其方差融入到滤波算法中，也即是通过考虑偏差与状态的互协方差，增加滤波的精度；同时，由于面对的是非线性滤波，对量测方程进行泰勒级数展开时会产生截断误差，因此采用迭代的方法来减少对非线性量测方程进行线性化产生的误差对滤波过程的影响，从而达到减少发散现象，提高滤波精度，确保滤波的数值稳定性；所述的迭代SKF滤波算法实现步骤如下：

1.扩维状态向量X_k，即将常值偏差向量b加入，则动力学方程（9）和量测方程（12）变为

Xk+1bk+1=Φk+1|k00IXkbk+wk0---(15)]]>

Zk=[Hk,I]Xkbk+vk---(16)]]>

式中，常值偏差向量满足条件：b_k+1=b_k，且其方差矩阵B₀满足

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

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

Ck=E{X~kbkT}=E{(Xk-X^k)bkT},---(18)]]>

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

相应的与动力学方程（15）和量测方程（16）第k步的误差方差阵为

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

Pk=E{X~kX~kT}=E{(Xk-X^k)(Xk-X^k)T}---(20)]]>

开始滤波计算时,需要初始化状态向量和误差方差阵，并设初始化状态向量为X₀及误差方差阵为P₀；

2.时间更新过程

由第k步的状态估计能得，第k+1步的状态一步预测为

X^k+1|k=Φk+1|kX^k,---(21)]]>

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

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

Pk+1|k=Φk+1|kPkΦk+1|kT+Qk---(23)]]>

Ck+1|k=Φk+1|kCk---(24)]]>

3.量测更新过程

使用迭代方法进行量测更新：当i=1,2,3,…时，按如下步骤进行循环计算：

1)计算第i步的滤波增益矩阵为

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

则状态的增益矩阵为

Kk+1i=[Pk+1|k(Hk+1i)T+Ck+1|k](Ωk+1i)-1---(26)]]>

Ωk+1i=Hk+1iPk+1|k(Hk+1i)T+Ck+1|kT(Hk+1i)T+Hk+1iCk+1|k+B0+Rk+1---(27)]]>

2)计算第i步量测信息残差

3)计算第i步的状态估计

X^k+1i=X^k+1|k+Kk+1i{Z~k+1i-Hk+1iX^k+1|k}---(29)]]>

式中，

使用迭代方法进行量测更新过程中，当所得状态向量的估计值满足向量的2范数满足的条件—其中阈值设为ε_limit:

||X^k+1i-X^k+1i-1||2<ϵlimit---(31)]]>

时即结束循环计算；

4.利用量测更新中的状态估计值及相应的参数，计算第k+1步的估计误差方差矩阵为

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

Pk+1=Pk+1|k-Kk+1iΩk+1i(Kk+1i)T---(33)]]>

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

Ck+1=Ck+1|k-Kk+1i(Hk+1iCk+1|k+B0)---(34)]]>

通过以上4步循环进行即得到火星动力下降段探测器的实时状态估计值，包含探测器的位置向量、速度向量和姿态角；

通过以上四个步骤，通过构造动力学方程和建立多源信息组合导航的量测方程，然后利用迭代SKF滤波算法消除测量信息中误差的影响，并确保滤波算法的稳定性，达到高效实时估计探测器导航状态的目的。