[发明专利]一种基于采样点继承策略的火星着陆器大气进入段导航方法

摘要

一种基于采样点继承策略的火星着陆器大气进入段导航方法，本发明涉及导航制导与控制技术领域，具体涉及一种火星着陆器大气进入段导航方法。本发明为了解决利用已有的滤波方法进行着陆器状态估计时精度不高的问题。本发明在滤波起始周期，利用无迹卡尔曼滤波的采样点生成规则给出采样点和采样点权值；并根据传播后的采样点和生成的权值给出着陆器状态预测值和陆器状态方差P_x；根据无迹卡尔曼滤波规则，计算观测量预测值和方差P_y，以及协方差P_xy；测量更新，修正着陆器状态的估计值；根据着陆器状态估计以及着陆器状态方差修改传播后的采样点ξ_i，得到修正值的采样点本发明具有更高的状态估计精度。本发明适用于火星着陆器大气进入段的导航。

主权项

一种基于采样点继承策略的火星着陆器大气进入段导航方法，其特征在于它的步骤如下：步骤一：在滤波起始周期，根据着陆器状态x，以及着陆器状态方差P_x，利用无迹卡尔曼滤波的采样点生成规则给出采样点和采样点权值；具体过程如下：2n+1个采样点为：$\begin{array}{cc}{\mathrm{\χ}}_i=x,& i=0\\ {\mathrm{\χ}}_i=x+\sqrt{n+\mathrm{\τ}}{\left(\sqrt{P_x}\right)}_i,& i=1,2...,n\\ {\mathrm{\χ}}_i=x-\sqrt{n+\mathrm{\τ}}{\left(\sqrt{P_x}\right)}_i,& i=n+1,...,2n\end{array}---\left(1\right)$这里n为系统的维数，即着陆器状态个数；τ为可调的比例参数，用来控制样本点到均值的距离；相应的权值ω_i为：$\begin{array}{cc}{\mathrm{\ω}}_i=\mathrm{\τ}/(n+\mathrm{\τ}),& i=0\\ {\mathrm{\ω}}_i=1/\[2(n+\mathrm{\τ})\],& i=1,2...,2n\end{array}---\left(2\right)$步骤二：根据无迹卡尔曼滤波规则由火星大气进入段动力学方程传播步骤一中获得的采样点，并根据传播后的采样点和步骤一生成的权值给出着陆器状态预测值和着陆器状态方差P_x，具体过程如下：根据火星大气进入段动力学方程和由式(1)得到的采样点，利用龙格库塔法进行数值积分，得到一个滤波周期Δt时长后的采样点预测值ξ_i，即传播后的采样点，记这个过程为ξ_i＝f(χ_i),i＝0,1,2…2n                    (9)计算着陆器状态预测值$\hat{x}=\underset{i=0}{\overset{2n}{\Σ}}{\mathrm{\ω}}_i{\mathrm{\ξ}}_i---\left(10\right)$更新着陆器状态方差P_x$P_x=\underset{i=0}{\overset{2n}{\Σ}}{\mathrm{\ω}}_i\[{\mathrm{\ξ}}_i-\hat{x}\]{\[{\mathrm{\ξ}}_i-\hat{x}\]}^T---\left(11\right);$步骤三：滤波器的观测量为y，根据无迹卡尔曼滤波规则，由采样点权值和传播后的采样点计算观测量预测值和方差P_y，以及协方差P_xy，具体过程如下：根据传播后的采样点ξ_i计算采样点对应观测量y_iy_i＝h(ξ_i),i＝0,1,2…2n                     (12)这里观测方程h(·)由观测量定义得到；进而得到观测量预测值和观测量方差P_y$\hat{y}=\underset{i=0}{\overset{2n}{\Σ}}{\mathrm{\ω}}_i{y}_i---\left(13\right)$$P_y=\underset{i=0}{\overset{2n}{\Σ}}{\mathrm{\ω}}_i\[y_i-\hat{y}\]{\[y_i-\hat{y}\]}^T---\left(14\right)$协方差P_xy为$P_{xy}=\underset{i=0}{\overset{2n}{\Σ}}{\mathrm{\ω}}_i\[{\mathrm{\ξ}}_i-\hat{x}\]{\[y_i-\hat{y}\]}^T---\left(15\right)$步骤四：测量更新，即当敏感器测量到新的数据时，修正着陆器状态的估计值，具体过程如下：根据高斯假设Kalman滤波器规则计算滤波增益K＝P_xy(P_y+R)^‑1                         (16)着陆器状态估计${\hat{x}}^{+}=\hat{x}+K(y-\hat{y})---\left(17\right)$着陆器状态方差估计$P_x^{+}=P_x-{\mathrm{KP}}_{xy}^T---\left(18\right)$其中，R为测量噪声方差，K为滤波增益，为状态和观测量的协方差的转置；步骤五：采样点继承：即根据步骤四得到着陆器状态以及着陆器状态方差修改步骤二中传播后的采样点ξ_i，得到修正值的采样点具体过程如下：首先定义部分变量：$\tilde{X}=\left[\begin{array}{ccc}{\mathrm{\ξ}}_0& ...& {\mathrm{\ξ}}_{2n}\end{array}\right]---\left(19a\right)$${\tilde{X}}^{+}=\left[\begin{array}{ccc}{\mathrm{\ξ}}_0^{+}& ...& {\mathrm{\ξ}}_{2n}^{+}\end{array}\right]---\left(19b\right)$w＝[ω₀ … ω_2n]^T                        (19c)W＝diag(w)                         (19d)为传播后的采样点ξ_i组成的矩阵；为采样点的修正值，即这里要求取的变量，为组成的矩阵；w为采样点对应权值组成的向量，W是权值向量为对角线元素的对角阵；根据以上定义有：${\tilde{X}}^{+}w={\hat{x}}^{+}---\left(20a\right)$${\tilde{X}}^{+}W{\tilde{X}}^{+T}=P_x^{+}---\left(20b\right)$假设测量更新后采样点集通过的旋转变换和尺度变换得到，即:${\tilde{X}}^{+}=Q\tilde{X}S---\left(21\right)$其中，Q为一正交矩阵，满足QQ^T＝I，其阶数与系统维数相同，表示对采样点的旋转变换，S为n阶对角矩阵，代表对采样点的尺度变换；将式(21)分别带入到式(20a)和(20b)当中，得到两个约束：$Q\tilde{X}Sw={\hat{x}}^{+}---\left(22a\right)$$Q\tilde{X}SWS{\tilde{X}}^T{Q}^T=P_x^{+}---\left(22b\right)$这里和已经在步骤四中计算得出，因此式(22a)和式(22b)以及QQ^T＝I作为求解Q和S的约束方程；为了尽量保留非线性动力学造成的非高斯分布信息，变换矩阵Q和S的解还应该使trace(L_s‑I)(L_s‑I)^T最小；根据以上约束，Q和S的解通过非线性约束寻优数值算法获得；获得变换矩阵Q和S后，根据式(21)计算采样点修正值并将作为下一滤波周期开始时的采样点，即令下一周期中的完成采样点继承后，顺序执行步骤二至步骤五，直到着陆过程结束。