CN103438892B - 一种改进的基于ekf的天文自主定轨算法 - Google Patents

Abstract

一种改进的基于EKF的天文自主定轨算法，涉及一种控制领域的导航方法。所述方法步骤如下：一、EKF滤波器参数初始化；二、参考坐标系由地心惯性坐标系转为滤波坐标系；三、在滤波坐标系中线性化和离散化滤波器状态方程；四、在滤波坐标系中进行EKF滤波；五、参考坐标系由滤波坐标系转回为地心惯性坐标系。本发明在传统的基于EKF天文自主定轨算法中，提出并引入滤波坐标系，同时增加坐标变换和坐标反转换等步骤使得传统的基于EKF天文自主定轨算法在线性化和离散化的时候，航天器的位置矢量在滤波坐标系中的各分量能够相等，从而使得状态方程线性化的展开点处于一个线性度较好的位置，进而使得定轨算法的稳定性有所提高。

Description

一种改进的基于EKF的天文自主定轨算法

技术领域

本发明涉及一种控制领域的导航方法，具体涉及一种改进的基于EKF的天文自主定轨算法。

背景技术

自20世纪60年代以来，航天事业的发展迎来了天文导航技术的辉煌，天文导航技术在美国阿波罗载人登月计划中得到了成功应用。近年来，随着新一轮月球和火星探测等一系列深空探测活动的开展，天文导航以其自主性强、精度高、成本低廉等特点在航天领域也得到了越来越广泛的应用，成为一种航天器的重要自主导航方法。天文导航技术目前已成为卫星、深空探测和载人航天中必不可少的关键技术，在未来人类探索宇宙的星际航行中也必将发挥重要的作用。

目前导航系统中应用最为广泛的滤波方法为扩展卡尔曼滤波（Extended Kalman Filter，EKF）、Unscented卡尔曼滤波（UKF）、Unscented粒子滤波（UPF）3种滤波方法。其中EKF计算量较小，但是导航精度相对也比较低，同时因为EKF算法在工程上会出现滤波发散的问题。美国著名的深空1号卫星是一颗完全自主性的深空探测器，在自主天文定轨方案上也因为EKF算法的可能发散而没有采纳，而是选取了批处理最小二乘方法。因此有必要对基于EKF的天文学定轨算法进行改进以提高其稳定性。

EKF算法是一种近似方法，它将非线性模型在状态估计值附近进行泰勒级数展开，并取一阶截断，用得到的一阶近似项作为原状态方程和测量方程的近似表达形式，从而实现线性化同时假设线性化后的状态依然服从高斯分布，然后对线性化后的系统采用标准卡尔曼滤波获得状态估计。由于采用了局部线性化技术，EKF能得到局部最优解，但它能否收敛于全局最优解，取决于函数的非线性强度以及展开点（即泰勒级数展开点）的选择。

基于EFK的天文学自主定轨算法在展开点处对滤波器的非线性状态方程和非线性观测方程进行了线性化和离散化。为了选取一个好的展开点，常用的思想是通过施加一个偏置量，使得当前的状态偏置到一个线性度较好的地方，使得线性化后的近似方程与原方程之间的误差相对较小。

发明内容

本发明的目的在于对传统的基于EFK的天文学自主定轨算法，通过坐标变换的方法，提出一种改进的基于EKF的天文自主定轨算法，使得展开点处具有较好的线性度，从而达到提高传统的基于EKF的天文自主定轨算法稳定性目的。

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

在基于EKF的天文自主定轨算法进行到线性化和离散化步骤之前，先将滤波器状态变量的参考坐标系由地心惯性坐标系变为滤波坐标系，滤波坐标系是本发明为了使滤波器状态方程和滤波器观测方程在线性化时具有较好的展开点而设置的一个坐标系，凡是能使位置矢量在该坐标系的三个轴上的投影分量大小相等的坐标系都称为滤波坐标系。从而使得滤波器状态方程和滤波器观测方程在线性化的过程中，不会因为位置矢量各个分量的数值在数量级上不一致而导致滤波器发散。如图1所示，共分为五个步骤，具体步骤如下：

步骤一、EKF滤波器参数初始化：

1）给滤波迭代次数                                                赋初值为；

2）设滤波器的状态变量，其中为地心到航天器的位置矢量，为航天器的速度矢量。给初始时刻的状态变量估计值赋初值，其中上角标说明该变量为地心惯性坐标系；

 3）滤波时间赋初值；

4）给状态模型噪声的协方差矩阵赋值；

5）给观测模型噪声的协方差矩阵赋值；

6）给预估状态协方差赋初值。

 步骤二、参考坐标系由地心惯性坐标系转为滤波坐标系：

不同于传统的基于EKF的天文自主定轨算法，本发明在自主定轨算法的线性化和离散化之前，需要将参考坐标系由地心惯性坐标系转换为滤波坐标系。图2为滤波坐标系示意图。

滤波坐标系是本发明为了使得滤波器的状态方程和滤波器观测方程在进行线性化时具有较好的展开点而设置的一个坐标系，凡是能使位置矢量在该坐标系的三个轴上的投影分量大小相等的坐标系都称为滤波坐标系。本发明中用表示滤波坐标系。

 1）获取上一时刻航天器位置矢量的估计值以及这一时刻的观测信息矢量，其中表示上一时刻滤波器状态变量估计向量的前三个元素组成的列向量；

 2）通过下式计算与其期望的参考坐标表示之间的夹角：

；

其中表示矩阵或向量的转置；

 3）通过下式计算和两个矢量所在平面的单位法方向量：

；

4）通过下式计算地心惯性坐标系到滤波坐标系的转动四元数：

；

5）通过下式计算地心惯性坐标系到滤波坐标系的余弦转换矩阵：

 

其中为转动四元数的第个分量；

6）通过下式计算在滤波坐标系中的坐标表示和地心惯性坐标系中的速度矢量坐标表示在滤波坐标系中的坐标表示：

 组合滤波坐标系中的位置矢量和速度矢量，得到滤波器状态变量在滤波坐标系中的坐标表示为：，这时候的前三个分量相等，即满足以下等式：

7）通过下式计算观测信息矢量在滤波坐标系中的坐标表示：。

 步骤三、在滤波坐标系中线性化和离散化滤波器状态方程：

此步骤与传统的基于EKF天文自主定轨算法的不同之处在于，滤波器状态方程的雅克比矩阵和状态转移矩阵的求解都是在滤波坐标系中进行的。在以位置和速度为滤波器状态变量的基于EKF的天文自主定轨算法中，求解的雅克比矩阵中各个元素与位置有关而与速度无关；而且在步骤二，已近将位置矢量的三个分量调整为大小相等，所以位置矢量的三个分量对雅克比矩阵的贡献可以保持相近，这有助于提高EKF滤波器的稳定性。

1）计算滤波器状态方程的雅克比矩阵，其中为对时间的导数；

 2）计算时刻的状态转移矩阵，其中为单位矩阵，为滤波时间步长。

    步骤四、在滤波坐标系中进行EKF滤波：

此步骤与传统的基于EKF天文自主定轨算法的不同之处在于，状态预估和状态更新两个步骤都是在滤波坐标系下进行的。

1）在滤波坐标系中进行状态预估：

 

其中表示滤波器的状态方程，表示的是时刻滤波器状态变量的初步计算值，为时刻的预估状态协方差，为时刻滤波器的预估状态协方差的初步计算值；

2）在滤波坐标系中进行状态更新：

 

其中表示滤波器的观测方程，为时刻的观测方程的雅克比矩阵，为增益矩阵，为单位矩阵，为时刻的预估状态协方差，为时刻滤波器状态变量的估计值在滤波坐标系下的表示。

步骤五、参考坐标系由滤波坐标系转回为地心惯性坐标系：

此步骤与传统的基于EKF天文自主定轨算法的不同之处在于，传统的基于EKF天文自主定轨算法没有这个步骤。

改进的基于EKF天文自主定轨算法通过步骤四滤波得出的时刻的滤波器状态变量的估计值是在滤波坐标系下的坐标表示，为了得到其在原来的地心惯性坐标系中的坐标表示，需要对结果进行坐标反变换：

  。

 通过可得到航天器的位置矢量的估计值和速度矢量的估计值；递增滤波迭代次数，然后跳回至步骤二，通过以上步骤的周期运行，就可以从滤波器状态变量的估计值中获得航天器当前时刻位置矢量的估计值以及速度矢量的估计值。

本发明与现有技术相比具有如下优点：

在传统的基于EKF天文自主定轨算法中，提出并引入滤波坐标系，同时增加坐标变换和坐标反转换等步骤使得传统的基于EKF天文自主定轨算法在线性化和离散化的时候，航天器的位置矢量在滤波坐标系中的各分量能够相等，从而使得状态方程线性化的展开点处于一个线性度较好的位置，进而使得定轨算法的稳定性有所提高，适用于深空探测器等需要天文自主导航功能的航天器。

附图说明

图1为本发明的流程图；

图2为滤波坐标系示意图；

图3为探月卫星的位置时间曲线；

图4为传统的基于EKF天文自主定轨算法的定位误差时间曲线；

图5为传统的基于EKF天文自主定轨算法的定速误差时间曲线；

图6为改进的基于EKF天文自主定轨算法的定位误差时间曲线；

图7为改进的基于EKF天文自主定轨算法的定速误差时间曲线。

具体实施方式

下面结合实施例对本发明的技术方案作进一步的说明，但并不局限如此，凡是对本发明技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，均应涵盖在本发明的保护范围中。

本实施例将改进的基于EKF天文自主定轨算法运用到探月卫星的天文学定轨过程中，其中滤波器的观测值为地球视矢量、月球视矢量。

先给出基于日地月信息和EKF的天文学定轨算法的基本描述：

在地球惯性坐标系中，航天器的位置矢量为，速度矢量为。设状态变量，阳质心的坐标为，月球质心的坐标为。在只考虑日月摄动的情况下，滤波器的状态方程为：

；

其中、、、、、分别表示的是变量、、、、、对时间的导数，为地球引力常数，为月球引力常数，为太阳引力常数。为航天器到地球球心的距离，为航天器到太阳质心的距离，为航天器到太阳质心的距离。

基于地球视矢量信息的观测信息矢量。

基于月球视矢量信息的观测信息矢量。

滤波器的状态方程简写为。

滤波器的观测方程为，简写为。

执行步骤一：改进的基于EKF天文自主定轨算法需要首先设定一些初始值。在实际工程中，由于探月卫星在近地环绕阶段可以使用GPS或者通过地面遥测来获得初始的轨道信息，因此的初始值可以在近地环绕阶段使用其他定轨方式来确定。状态模型噪声的协方差矩阵赋值取决于模型的精度，在仅考虑日月摄动的情况下可以取值为，预估状态协方差的初始值可以取为，观测模型噪声的协方差矩阵应该根据敏感器的噪声水平来确定，对于敏感器的信噪比在60dB左右的情况，可以设置，滤波时间赋初值，滤波迭代次数赋初值为。

 执行步骤二：根据步骤一计算出地心惯性坐标系到滤波坐标系的余弦转换矩阵，然后计算滤波器状态变量在滤波坐标系中的坐标表示，计算观测信息矢量在滤波坐标系中的坐标表示。

执行步骤三：计算滤波器状态方程的雅克比矩阵

 

 其中：

。

注意以上计算到的各个矢量的坐标表示是在滤波参考坐标系下的，继续按照步骤三计算时刻的状态转移矩阵。

执行步骤四：在滤波坐标系中进行EKF滤波得到时刻滤波器状态变量的估计值，在这个过程中观测方程的雅克比矩阵为：

 

。

执行步骤五：进行坐标反变换，计算出第步滤波器的状态变量的估计值在地心惯性坐标系中的表示形式；递增，然后返回执行步骤二。

通过以上步骤的周期运行，就可以从滤波器状态变量的估计值中获得探月卫星当前时刻位置矢量的估计值以及速度矢量的估计值。

图3给出了传统的基于EKF的天文自主定轨算法仿真运行2万秒过程中，探月卫星的位置时间曲线，6000s左右的时候探月卫星从环地轨道过渡到奔月轨道，并开始使用传统的基于EKF天文自主定轨算法来确定轨道，随着探月卫星不断地飞向月球的过程中，卫星位置矢量的x轴分量越来越大，这是奔月轨迹此阶段近似平行于地心惯性坐标系x轴的结果。从图4可以看出，传统的基于EKF的天文自主定轨算法随着时间的增加逐渐发散，这是由于卫星位置矢量的x轴分量过大，逐渐远远超过了位置矢量在y轴和z轴上的分量，从而导致了计算雅克比矩阵时线性化误差不断增大，并最终导致了定轨算法的发散。从图5所示的传统的基于EKF的天文自主定轨算法的定速误差结果可以看出，x轴向定速误差的均值明显非零。

在上述同等条件下仿真改进的基于EKF的天文自主定轨算法，图6和图7给出了改进的基于EKF的天文自主定轨算法运行2万秒之后的定轨结果。从图6可以看出滤波器在这2万秒之内都是稳定的，没有发散的迹象。从图7中也可以看出x轴向的定速误差的均值接近零，这使得图6中的定轨误差不会随着时间的推移越变越大。可见，本发明所提出的方法确实能够提高基于EKF的天文自主定轨算法的稳定性，具有很强的理论和工程实际意义。

Claims (4)

1.一种改进的基于EKF的天文自主定轨算法，其特征在于所述算法步骤如下：

一、参考坐标系由地心惯性坐标系转为滤波坐标系，具体步骤如下：

1)获取上一时刻航天器位置矢量的估计值以及这一时刻的观测信息矢量zⁱ(k)，其中表示上一时刻滤波器状态变量估计向量的前三个元素组成的列向量；

2)通过下式计算与其期望的参考坐标表示r_c之间的夹角θ：

$\left\{\begin{array}{c}{r}_c={\left[\begin{array}{ccc}1& 1& 1\end{array}\right]}^T\\ \mathrm{\θ}=\arccos \left(\frac{{\hat{r}}^i{(k-1)}^T{r}_c}{\left|{\hat{r}}^i(k-1)\right|\left|r_c\right|}\right)\end{array}\right.;$

其中T表示矩阵或向量的转置；

3)通过下式计算和r_c两个矢量所在平面的单位法向量e：

$e=\frac{{\hat{r}}^i(k-1)\×r_c}{\left|{\hat{r}}^i(k-1)\right|\left|r_c\right|};$

4)通过下式计算地心惯性坐标系i到滤波坐标系l的转动四元数q：

$q=\left[\begin{array}{cc}e*\sin \left(\frac{\mathrm{\θ}}{2}\right)& \cos \left(\frac{\mathrm{\θ}}{2}\right)\end{array}\right];$

5)通过下式计算地心惯性坐标系i到滤波坐标系l的余弦转换矩阵C：

$C=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right];$

其中q_j为转动四元数q的第j个分量；

6)通过下式计算在滤波坐标系l中的坐标表示和地心惯性坐标系i中的速度矢量坐标表示在滤波坐标系l中的坐标表示

$\left\{\begin{array}{c}{\hat{r}}^l(k-1)={C\hat{r}}^i(k-1)\\ {\hat{v}}^l(k-1)={C\hat{v}}^i(k-1)\end{array}\right.;$

组合滤波坐标系l中的位置矢量和速度矢量，得到滤波器状态变量在滤波坐标系l中的坐标表示为： ${\hat{x}}^l(k-1)=\left[\begin{array}{c}{\hat{r}}^l(k-1)\\ {\hat{v}}^l(k-1)\end{array}\right],$ 这时的前三个分量相等，即满足以下等式：

$\begin{array}{c}{\hat{x}}_1^l(k-1)={\hat{x}}_2^l(k-1)\\ {\hat{x}}_2^l(k-1)={\hat{x}}_3^l(k-1)\end{array};$

7)通过下式计算观测信息矢量zⁱ(k)在滤波坐标系l中的坐标表示z^l(k)：

z^l(k)＝Czⁱ(k)；

二、在滤波坐标系中线性化和离散化滤波器状态方程，具体步骤如下：

1)计算滤波器状态方程的雅克比矩阵其中为x^l对时间的导数；

2)计算k-1时刻的状态转移矩阵Φ(k-1)＝I+Aδ，其中I为单位矩阵，δ为滤波时间步长；

三、在滤波坐标系中进行EKF滤波，具体步骤如下：

1)在滤波坐标系中进行状态预估：

$\left\{\begin{array}{c}{\hat{x}}^l(k,k-1)={\hat{x}}^l(k-1)+f[{\hat{x}}^l(k-1),t_{k-1}]\·\mathrm{\δ}+A\·f[{\hat{x}}^l(k-1),t_{k-1}]\·\frac{{\mathrm{\δ}}^2}{2}\\ P(k,k-1)=\mathrm{\Φ}(k-1)P(k-1)\mathrm{\Φ}{(k-1)}^T+Q\\ t_k=t_{k-1}+\mathrm{\δ}\end{array}\right.;$

其中f表示滤波器的状态方程，表示的是k时刻滤波器状态变量的初步计算值，P(k-1)为k-1时刻的预估状态协方差，P(k,k-1)为k时刻滤波器的预估状态协方差的初步计算值，Q为状态模型噪声的协方差矩阵；

2)在滤波坐标系中进行状态更新：

$\left\{\begin{array}{c}H\left(k\right)=\frac{\∂g}{{\∂x}^l}{|}_{x^l={\hat{x}}^l(k,k-1)}\\ K\left(k\right)=P(k,k-1)H{\left(k\right)}^T{[H\left(k\right)P(k,k-1)H{\left(k\right)}^T+R]}^{-1}\\ {\hat{x}}^l\left(k\right)={\hat{x}}^l(k,k-1)+K\left(k\right)\{z^l\left(k\right)-g[{\hat{x}}^l(k,k-1),t_k\left]\right\}\\ P\left(k\right)=[I-K\left(k\right)H\left(k\right)]P(k,k-1){[I-K\left(k\right)H\left(k\right)]}^T+K\left(k\right)R\left(k\right)K^T\left(k\right)\end{array}\right.;$

其中g表示滤波器的观测方程，H(k)为k时刻的观测方程的雅克比矩阵，K(k)为增益矩阵，I为单位矩阵，P(k)为k时刻的预估状态协方差，为k时刻滤波器状态变量的估计值在滤波坐标系下的表示，R为观测模型噪声的协方差矩阵；

四、参考坐标系由滤波坐标系转回为地心惯性坐标系。

2.根据权利要求1所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤一中，将参考坐标系由地心惯性坐标系转为滤波坐标系之前，需要对EKF滤波器参数初始化，具体步骤如下：

1)给滤波迭代次数k赋初值为k＝1；

2)设滤波器的状态变量 $x=\left[\begin{array}{c}r\\ v\end{array}\right],$ 其中r为地心到航天器的位置矢量，v为航天器的速度矢量；给初始时刻的状态变量x估计值赋初值，其中上角标i说明该变量为地心惯性坐标系；

3)滤波时间赋初值t₀＝0；

4)给状态模型噪声的协方差矩阵Q赋值；

5)给观测模型噪声的协方差矩阵R赋值；

6)给预估状态协方差P赋初值。

3.根据权利要求1所述的改进的基于EKF的天文自主定轨算法，其特征在于所述步骤四的具体步骤如下：

通过下式对步骤三的结果进行坐标反变换：

${\hat{x}}^i\left(k\right)=C^T{\hat{x}}^l\left(k\right);$

通过可得到航天器的位置矢量的估计值和速度矢量的估计值。

4.根据权利要求3所述的改进的基于EKF的天文自主定轨算法，其特征在于递增滤波迭代次数k＝k+1，然后跳回至步骤一，通过以上步骤的周期运行，可从滤波器状态变量的估计值中获得航天器当前时刻位置矢量的估计值和速度矢量的估计值。