一种基于小行星交会的深空探测器自主天文导航方法

摘要

本发明涉及一种基于小行星交会的深空探测器自主天文导航方法。根据圆形限制性轨道动力学模型建立深空探测器的状态模型；利用敏感器获得小行星以及背景恒星的像元像线信息，把所获得的像元像线信息转换为小行星以及背景恒星的角度信息，建立小行星的角度信息量测模型；结合自适应Unscented卡尔曼滤波估计深空探测器的位置和速度。本发明具有估计精度高，非常适用于与小行星交会期间的自主导航。本发明属于航天导航技术领域，不仅可以为深空探测器提供高精度导航参数，而且可为其自主导航系统设计提供参考。

说明书

技术领域

本发明涉及在深空探测器与小行星交会阶段，基于小行星角度信息的自主天文导航方法，是一种非常适用于与小行星交会段深空探测器的自主导航方法。

背景技术

在太阳系中，大约140000颗以上的小行星聚集在火星和木星轨道之间，还有一部分小行星运行于地球和火星轨道之间，成为近地小行星。小行星和彗星等小天体保留着太阳系早期的许多信息，是研究太阳系起源与演化的重要物证；通过在小行星上寻找水和有机物存在的证据，有助于理解地球和太阳系内生命的起源与演化。近年来小行星探测越来越受到重视。由于深空探测一般采用多科学探测任务，往往在一次深空任务中，通过精确的轨道设计，实现在飞向目标大行星的轨道转移过程中，实现与小行星交会，完成大行星和小行星共同探测的目标。

由于这种深空探测任务对深空探测器的导航品质要求高，若这一阶段导航误差大，无法及时通过轨道控制系统调整探测器的轨道，错过了轨道机动的最佳时机，就有可能无法与小行星交会，实现共同探测的目标，导致深空探测任务失败。

深空探测任务距离地球遥远，通讯延迟大，地面测控设施复杂庞大，运行费用高，而且地面天文望远镜获得的小行星的星历并不准确，误差较大，传统的基于地面无线电测控的导航方式是通过地面轨道预报，结合小行星的星历给深空探测器发送轨道导航控制命令，但由于深空探测任务无线电通信延迟特性和巨大的星历误差，导致基于地面无线电测控的导航方式不能实时为探测器提供准确的导航信息，因此，实现深空探测器的自主导航是深空探测的必然趋势。

与小行星交会阶段的深空探测器，由于其距离太阳和各行星的距离都较远，用于近地逃逸轨道或行星捕获、着陆等的自主导航方法，如基于IMU、测距测速敏感器、地面图像等的自主导航方法都无法使用，此时天文导航是唯一有效的自主导航手段。

目前根据所观测天体的不同，深空探测器在转移轨道上的自主天文导航方法大致可分为三种：基于太阳和大行星的自主天文导航方法、基于脉冲星的自主天文导航方法和基于小行星的自主天文导航方法。前两种方法是利用太阳和大行星或者脉冲星进行定位，但是由于天体距离远，测量精度低，可观测的脉冲星数目少，因此对于小行星交会段的深空探测器并不适用。基于小行星的自主天文导航方法是借助小行星的位置信息对探测器进行定位。因此以火星以远(包括火星)的深空天体作为探测目标的深空探测器，考虑到与小行星交会的特殊性，可以通过观测多颗小行星获取探测器的位置信息，消除小行星星历误差对探测器导航的影响，实现探测器转移轨道小行星交会段的高精度导航。

传统上利用小行星测量信息的深空探测器自主导航存在以下缺点：一方面传统利用小行星测量信息的深空探测器是采用小行星图像的像元像线这一测量信息，但是像元像线量测模型受探测器姿态估计精度的约束，姿态估计的精度直接影响了深空探测器的自主导航精度；另一方面传统上是利用批处理最小二乘等方法减小系统的随机误差，但最小二乘估计方法的精度较低，无法为探测器导航提供高精度的导航信息。对于自主天文导航这种非线性系统而言，在有限维系统中无法从根本上获得最优的滤波算法，现有的解决系统非线性问题的方法都属于次优非线性估计。现有的次优非线性估计方法如精度较高的非线性卡尔曼滤波方法等是建立在状态模型和量测模型的误差分布特性已知的且状态量的先验信息完备的前提下，如果状态和量测误差分布特性不准确，如量测信息以及未建模的加速的误差等动力学模型误差不准确，则将产生巨大的导航误差。

发明内容

本发明要解决的技术问题是：克服基于太阳与大行星及其卫星和脉冲星自主导航方法中太阳和行星及其卫星到探测器的距离较远或导航脉冲星数量少、精度低、对探测器轨道设计要求高等缺点，弥补传统上观测小行星像元像线量测信息受探测器姿态约束这一不足，并解决系统无法为现有的次优非线性估计方法如Unscented卡尔曼滤波提供准确的状态模型和量测模型的误差分布特性和状态量的先验信息，为深空探测器在与小行星交会阶段提供一种高精度的自主天文导航方法。

本发明解决其技术问题所采用的技术方案为：建立高精度的深空探测器状态模型，通过敏感器获得小行星及其相应图像中的背景恒星像元像线信息，之后把像元像线信息转换为方向矢量，建立小行星与背景恒星之间角度信息的量测模型，其中使用自适应Unscented卡尔曼滤波估计模型误差方差阵Q，并结合估计的模型误差方差阵Q确定深空探测器的导航参数。

具体包括以下步骤：

1.建立基于轨道动力学的小行星交会段深空探测器状态模型；

在太阳系内的深空探测任务小行星都处于地球与木星轨道之间的区域，因此需要考虑太阳中心引力、木星中心引力、火星中心引力和地球中心引力对探测器的作用，选取日心黄道惯性坐标系，可得深空探测器的状态模型为

$\left(\begin{array}{c}\stackrel{·}{x}=v_x\\ \stackrel{·}{y}=v_y\\ \stackrel{·}{z}=v_z\\ {\stackrel{·}{v}}_x=-{\mu }_s\frac{x}{r_{\mathrm{ps}}^3}-{\mu }_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mu }_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]-{\mu }_j[\frac{x-x_3}{r_{\mathrm{pj}}^3}+\frac{x_3}{r_{\mathrm{sj}}^3}]+w_x\\ {\stackrel{·}{v}}_y=-{\mu }_s\frac{y}{r_{\mathrm{ps}}^3}-{\mu }_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mu }_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]-{\mu }_j[\frac{y-y_3}{r_{\mathrm{pj}}^3}+\frac{y_3}{r_{\mathrm{sj}}^3}]+w_y\\ {\stackrel{·}{v}}_z=-{\mu }_s\frac{z}{r_{\mathrm{ps}}^3}-{\mu }_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mu }_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]-{\mu }_j[\frac{z-z_3}{r_{\mathrm{pj}}^3}+\frac{z_3}{r_{\mathrm{sj}}^3}]+w_z\end{array}\right)---\left(1\right)$

式中，探测器三轴位置的微分，v_x，v_y，v_z为探测器三轴的速度，为探测器三轴速度的微分，μ_s、μ_m、μ_e和μ_j分别为太阳、火星、地球和木星引力常数，r_ps为日心到探测器的距离，r_pm为火星到探测器的距离，r_sm为火心到日心的距离，r_pe为地心到探测器的距离，r_se为地心到日心的距离，r_pj为木心到探测器的距离，r_sj为木心到日心的距离，(x₁，y₁，z₁)，(x₂，y₂，z₂)、(x₃，y₃，z₃)和(x，y，z)分别为火星、地球、木星和探测器的位置，其中火星、地球和木星的位置可由行星星历表获得，w_x，w_y，w_z分别为探测器三轴的状态模型误差。

式(1)中的各变量都是与时间t有关的变量，可简写为

$\stackrel{·}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)---\left(2\right)$

状态变量为X＝[x，y，z，v_x，v_y，v_z]^T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[w_x，w_y，w_z]^T。

2.建立小行星敏感器图像中小行星与背景恒星之间角度信息量测模型

建立第一小行星和第i颗背景恒星角度信息量测模型为

${\theta }_{1i}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}1}·{\overrightarrow{s}}_{a1i})---\left(3\right)$

式中，为第一小行星相对探测器的单位矢量，为第一小行星敏感器视场范围内第i颗背景恒星星光方向的单位矢量，i＝1，2，3。

建立第二小行星和第i颗背景恒星角度信息量测模型为

${\theta }_{2i}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}2}·{\overrightarrow{s}}_{a2i})---\left(4\right)$

式中，为第二小行星相对探测器的单位矢量，为第二小行星敏感器视场范围内第i颗背景恒星星光方向的单位矢量。

令Z＝[θ_1i，θ_2i]^T，量测噪声分别为测量θ_1i，θ_2i的观测误差，则以第一小行星和第二小行星与各自背景恒星之间的角度信息作为观测量的量测模型可表示为

Z(t)＝h[X(t)，t]+v(t)             (5)

3.对步骤1中式(2)所示的状态方程及步骤2中式(5)所示的量测方程进行离散化

X(k+1)＝F(X(k)，k)+w(k)           (6)

Z(k)＝H(X(k)，k)+v(k)             (7)

式中，k＝1，2，…，F(X(k)，k)为f(X(t)，t)离散后的非线性状态转移函数，H(X(k)，k)为h(X(t)，t)离散后的非线性量测函数，w(k)、v(k)互不相关。

4.获取敏感器角度信息量测量

①小行星敏感器获取第一小行星和第二小行星的两幅图像信息；

②步骤①中的图像信息经质心提取后，获得第一小行星的像元像线(p_m1，l_m1)、第一小行星图像中的第i颗背景恒星的像元像线(p_m1i，l_m1i)、第二小行星的像元像线(p_m2，l_m2)和第二小行星图像中第i颗背景恒星的像元像线(p_m2i，l_m2i)，i＝1，2，3；

③第一小行星、第二小行星和背景恒星的二维像元像线转换为三维矢量方向；

A.首先将第一小行星、第二小行星及其背景恒星的像元像线信息转换为敏感器二维成像平面坐标系中的坐标

$\left(\begin{array}{c}\left(\begin{array}{c}{x}_{m1}^{2d}\\ y_{m1}^{2d}\end{array}\right)=K_1^{-1}(\left(\begin{array}{c}{p}_{m1}\\ l_{m1}\end{array}\right)-\left(\begin{array}{c}{p}_{01}\\ l_{01}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m1i}^{2d}\\ y_{m1i}^{2d}\end{array}\right)=K_1^{-1}(\left(\begin{array}{c}{p}_{m1i}\\ l_{m1i}\end{array}\right)-\left(\begin{array}{c}{p}_{01}\\ l_{01}\end{array}\right))\end{array}\right)---\left(8\right)$

$\left(\begin{array}{c}\left(\begin{array}{c}{x}_{m2}^{2d}\\ y_{m2}^{2d}\end{array}\right)=K_2^{-1}(\left(\begin{array}{c}{p}_{m2}\\ l_{m2}\end{array}\right)-\left(\begin{array}{c}{p}_{02}\\ l_{02}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m2i}^{2d}\\ y_{m2i}^{2d}\end{array}\right)=K_2^{-1}(\left(\begin{array}{c}{p}_{m2i}\\ l_{m2i}\end{array}\right)-\left(\begin{array}{c}{p}_{02}\\ l_{02}\end{array}\right))\end{array}\right)---\left(9\right)$

式中，和为测量得到的第一小行星和第二小行星在对应敏感器二维成像平面坐标系中的坐标，和为测量得到的第i颗背景恒星在第一小行星敏感器和第二小行星敏感器二维成像平面坐标系中的坐标，K₁和K₂分别为第一小行星和第二小行星成像敏感器由毫米转为像素的相机转换矩阵，(p₀₁，l₀₁)、(p₀₂，l₀₂)分别为第一小行星和第二小行星敏感器中心的像元和像线。

B.将第一小行星、第二小行星及其背景恒星的敏感器二维成像平面坐标系中的坐标转换为三维矢量信息坐标

$\left(\begin{array}{c}{\overrightarrow{l}}_{m1}^c=\frac{1}{\sqrt{{\left(x_{m1}^{2d}\right)}^2+{\left(y_{m1}^{2d}\right)}^2+f_1^2}}\left(\begin{array}{c}{x}_{m1}^{2d}\\ y_{m1}^{2d}\\ -f_1\end{array}\right)\\ {\overrightarrow{s}}_{m1i}^c=\frac{1}{\sqrt{{\left(x_{m1i}^{2d}\right)}^2+{\left(y_{m1i}^{2d}\right)}^2+f_1^2}}\left(\begin{array}{c}{x}_{m1i}^{2d}\\ y_{m1i}^{2d}\\ -f_1\end{array}\right)\end{array}\right)---\left(10\right)$

$\left(\begin{array}{c}{\overrightarrow{l}}_{m2}^c=\frac{1}{\sqrt{{\left(x_{m2}^{2d}\right)}^2+{\left(y_{m2}^{2d}\right)}^2+f_2^2}}\left(\begin{array}{c}{x}_{m2}^{2d}\\ y_{m2}^{2d}\\ -f_2\end{array}\right)\\ {\overrightarrow{s}}_{m2i}^c=\frac{1}{\sqrt{{\left(x_{m2i}^{2d}\right)}^2+{\left(y_{m2i}^{2d}\right)}^2+f_2^2}}\left(\begin{array}{c}{x}_{m2i}^{2d}\\ y_{m2i}^{2d}\\ -f_2\end{array}\right)\end{array}\right)---\left(11\right)$

式中，为量测得到的第一小行星和第二小行星相对探测器的单位矢量，为量测得到的在第一小行星和第二小行星敏感器中的第i颗背景恒星相对探测器的单位矢量；

④.将第一小行星及其背景恒星、第二小行星及其背景恒星的矢量方向信息转换为第一小行星与其背景恒星之间的角度信息、第二小行星与其背景恒星之间的角度信息。

由第一小行星敏感器量测得到的第一小行星和第i颗背景恒星角度信息

${\theta }_{m1i}=\arccos (-{\overrightarrow{l}}_{m1}^c·{\overrightarrow{s}}_{m1i}^c)---\left(12\right)$

由第二小行星敏感器量测得到的第二小行星和第i颗背景恒星角度信息

${\theta }_{m2i}=\arccos (-{\overrightarrow{l}}_{m2}^c·{\overrightarrow{s}}_{m2i}^c)---\left(13\right)$

两个敏感器量测得到的第一小行星和第二小行星与各自背景恒星的角度量测量为：

Z_k＝[θ_m11(k)，θ_m12(k)，θ_m13(k)，θ_m21(k)，θ_m22(k)，θ_m23(k)]^T    (14)

式中，Z_k表示第k时刻的系统量测量，θ_m1i(k)，θ_m2i(k)分别表示第k时刻量测得到的第一小行星与其第i颗背景恒星之间的角度信息、第二小行星与其第i颗背景恒星之间的角度信息，i＝1，2，3。

5.轨道自适应Unscented卡尔曼滤波方法

采用自适应Unscented卡尔曼滤波方法，结合步骤①②④的状态模型、量测模型和量测量，一方面利用自适应估计状态模型误差方差算法自适应调节模型误差方差矩阵Q，另一方面减小状态模型、量测模型的模型非线性随机误差，利用Unscented卡尔曼滤波算法，结合所述的状态方程和量测方程进行滤波，利用敏感器获取测量得到的第一小行星、第二小行星和背景恒星的像元像线信息，并将此测量信息转换为量测量第一小行星、第二小行星和背景恒星的角度信息，通过量测量与量测方程相减得到系统量测残差，用系统这一残差校正量测方程的模型误差；利用Unscented采样，并结合状态方程对采样点进行一步预测，并得出与上一步迭代状态值之间协方差阵，以消除状态方程模型误差的影响，最终输出导航信息。其中自适应估计状态模型误差方差算法，具体步骤为：

①初始化窗口系数γ

②自适应调节窗口大小γ

性能指标函数为

$J_k\left(\gamma \right)=\sqrt{\frac{1}{N}\underset{n=1}{\overset{N}{\Sigma }}(v_{1,n}^2+v_{2,n}^2+...+v_{m,n}^2)},(k=\mathrm{1,2},...)---\left(15\right)$

式中v_k表示量测值与量测模型的残差，m为量测量的维数，v_1，n，v_2，n，...，v_m，n分别表示n时刻残差的第1，2，...，m行，N表示总采样数，这里N＝k；借助单纯形数值计算方法，求得使性能指标函数最小的窗口大小γ，使得量测残差最小；

③计算状态模型误差方差的残差Q^*

$Q^{*}=\Delta x_k\Delta {x_k}^T+P_k^{-}-P_k^{+}-{\hat{Q}}_k^{-}---\left(16\right)$

式中，Δx_k为状态变量的残差，表示了量测更新前后状态量的变化，为k时刻系统状态误差方差阵预测值，为k时刻系统状态误差方差阵一步预测值，为当前系统状态误差方差；

④计算状态模型误差方差的估计

${\hat{Q}}_k^{+}={\hat{Q}}_k^{-}+\frac{1}{\gamma }(Q^{*}-{\hat{Q}}_k^{-})---\left(17\right)$

⑤输出自适应调节后的窗口大小γ和状态模型误差方差的估计并返回至Unscented卡尔曼滤波，用于k+1时刻。

本发明的原理是：本发明是以小行星与其背景恒星之间的角度信息为观测量的一种小行星交会段探测器的天文导航方法，利用自适应Unscented卡尔曼滤波对位置、速度等导航参数进行估计。首先建立深空探测器轨道动力学模型，利用圆形限制性轨道动力学模型建立其状态模型，直接在第一小行星敏感器和第二小行星敏感器的图像中分别获取第一小行星和第一小行星背景恒星的像元像线信息以及第二小行星和第二小行星背景恒星的像元像线信息，从而间接地通过像元像线信息获得第一小行星与第一小行星背景恒星之间的角度信息以及第二小行星与第二小行星背景恒星之间的角度信息，建立相应的量测模型。由于状态模型和量测模型都存在误差，因此除了受到测量仪器精度的制约之外，系统的非线性问题是限制深空探测器小行星交会段导航精度的主要因素，因此在轨道确定滤波中使用Unscented卡尔曼滤波方法解决系统的非线性问题。为了避免选择的滤波参数不合适，本发明利用单纯性搜索数值计算方法，估计出合适的滤波参数，并自动调节滤波参数，实现自适应调整滤波参数进行Unscented卡尔曼滤波，实现对位置、速度等导航参数进行高精度估计。

本发明与现有技术相比的优点在于：(1)充分利用敏感器获得的图像信息，不仅利用了小行星的像元像线信息，还利用了图像中的背景恒星信息，为探测器被火星捕获提供高精度的导航性能；(2)利用小行星之间的角度信息，克服了姿态估计误差对小行星像元像线量测模型精度的影响，进一步提高了深空探测器的导航精度；(3)自适应调整滤波参数，克服了原有方法无法准确设置滤波参数这一缺点，自适应估计状态模型误差方差算法为Unscented卡尔曼滤波提供了准确的状态模型和量测模型误差分布特性以及状态量的先验信息，实现深空探测器的高精度导航。

附图说明

图1为本发明基于小行星交会的深空探测器自主天文导航方法的流程图。

图2为本发明中小行星与背景恒星之间角度信息量测模型的示意图。

图3为本发明中第一小行星成像原理示意图。

图4为本发明中自适应滤波方法的流程图。

具体实施方式

如图1所示，本发明的具体实施方法如下：

1.建立基于轨道动力学的小行星交会段深空探测器状态模型；

首先初始化探测器位置、速度，设状态量X＝[x，y，z，v_x，v_y，v_z]^T，x，y，z，v_x，v_y，v_z分别为探测器在日心惯性坐标系中三轴的位置和速度，根据探测器的轨道设计，选取探测器的位置和速度初值为

X＝[-1.853×10¹¹m  -1.168×10¹¹m  -5.032×10¹⁰m

-9.213×10³m/s-1.855×10⁴m/s-8.065×10³m/s]^T

考虑了太阳中心引力、火星中心引力、地球中心引力和木星中心引力对探测器的作用，选取日心黄道惯性坐标系，可得深空探测器的状态模型为

$\left(\begin{array}{c}\stackrel{·}{x}=v_x\\ \stackrel{·}{y}=v_y\\ \stackrel{·}{z}=v_z\\ {\stackrel{·}{v}}_x=-{\mu }_s\frac{x}{r_{\mathrm{ps}}^3}-{\mu }_m[\frac{x-x_1}{r_{\mathrm{pm}}^3}+\frac{x_1}{r_{\mathrm{sm}}^3}]-{\mu }_e[\frac{x-x_2}{r_{\mathrm{pe}}^3}+\frac{x_2}{r_{\mathrm{se}}^3}]-{\mu }_j[\frac{x-x_3}{r_{\mathrm{pj}}^3}+\frac{x_3}{r_{\mathrm{sj}}^3}]+w_x\\ {\stackrel{·}{v}}_y=-{\mu }_s\frac{y}{r_{\mathrm{ps}}^3}-{\mu }_m[\frac{y-y_1}{r_{\mathrm{pm}}^3}+\frac{y_1}{r_{\mathrm{sm}}^3}]-{\mu }_e[\frac{y-y_2}{r_{\mathrm{pe}}^3}+\frac{y_2}{r_{\mathrm{se}}^3}]-{\mu }_j[\frac{y-y_3}{r_{\mathrm{pj}}^3}+\frac{y_3}{r_{\mathrm{sj}}^3}]+w_y\\ {\stackrel{·}{v}}_z=-{\mu }_s\frac{z}{r_{\mathrm{ps}}^3}-{\mu }_m[\frac{z-z_1}{r_{\mathrm{pm}}^3}+\frac{z_1}{r_{\mathrm{sm}}^3}]-{\mu }_e[\frac{z-z_2}{r_{\mathrm{pe}}^3}+\frac{z_2}{r_{\mathrm{se}}^3}]-{\mu }_j[\frac{z-z_3}{r_{\mathrm{pj}}^3}+\frac{z_3}{r_{\mathrm{sj}}^3}]+w_z\end{array}\right)---\left(18\right)$

式中，探测器三轴位置的微分，v_x，v_y，v_z为探测器三轴的速度，为探测器三轴速度的微分，μ_s、μ_m、μ_e和μ_j分别为太阳、火星、地球和木星引力常数，r_ps为日心到探测器的距离，r_pm为火星到探测器的距离，r_sm为火心到日心的距离，r_pe为地心到探测器的距离，r_se为地心到日心的距离，r_pj为木心到探测器的距离，r_sj为木心到日心的距离，(x₁，y₁，z₁)，(x₂，y₂，z₂)、(x₃，y₃，z₃)和(x，y，z)分别为火星、地球、木星和探测器的位置，其中火星、地球和木星的位置可由行星星历表获得，w_x，w_y，w_z分别为探测器三轴的状态模型误差。

式(18)中的各变量都是与时间t有关的变量，可简写为

$\stackrel{·}{X}\left(t\right)=f(X\left(t\right),t)+w\left(t\right)---\left(19\right)$

状态变量为X＝[x，y，z，v_x，v_y，v_z]^T，f(X(t)，t)为系统非线性连续状态转移函数，状态噪声为w＝[w_x，w_y，w_z]^T。

2.建立小行星与背景恒星之间角度信息的量测模型

本发明以第一小行星和第二小行星与各自图像中的三颗背景恒星之间的角度信息作为量测量，如图2所示，图中显示了第一小行星和第二小行星在各自敏感器二维成像平面上的图像信息，以及第一小行星敏感器中的第i颗背景恒星图像和第二小行星敏感器中的第i颗背景恒星图像，根据第一小行星敏感器中的图像信息可以获得第一小行星的矢量方向和第一小行星图像中第i颗背景恒星的矢量方向之间的角度信息θ_1i，同样可以从第二小行星敏感器中的图像信息可以获得第二小行星的矢量方向和第二小行星图像中第i颗背景恒星的矢量方向之间的角度信息θ_2i。为了方便描述，图中只选取了1颗背景恒星，即第i颗，i＝1，2，3，实际应用时每幅小行星图像中选取三颗背景恒星。

第一小行星敏感器图像中第一小行星和三颗背景恒星角度信息的表达式为

$\left(\begin{array}{c}{\theta }_{11}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}1}·{\overrightarrow{s}}_{a11})\\ {\theta }_{12}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}1}·{\overrightarrow{s}}_{a12})\\ {\theta }_{13}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}1}·{\overrightarrow{s}}_{a13})\end{array}\right)---\left(20\right)$

式中，为第一小行星相对探测器的单位矢量，为第一小行星敏感器视场范围内第一颗、第二颗和第三颗背景恒星星光方向的单位矢量，i＝1，2，3。

第二小行星敏感器图像中第二小行星和三颗背景恒星角度信息的表达式为

$\left(\begin{array}{c}{\theta }_{21}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}2}·{\overrightarrow{s}}_{a21})\\ {\theta }_{22}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}2}·{\overrightarrow{s}}_{a22})\\ {\theta }_{23}=\arccos (-{\overrightarrow{l}}_{\mathrm{pa}2}·{\overrightarrow{s}}_{a23})\end{array}\right)---\left(21\right)$

式中，为第二小行星相对探测器的单位矢量，为第二小行星敏感器视场范围内第一颗、第二颗和第三颗背景恒星星光方向的单位矢量。

令Z＝[θ₁₁，θ₁₂，θ₁₃，θ₂₁，θ₂₂，θ₂₃]^T，量测噪声分别为测量θ₁₁，θ₁₂，θ₁₃，θ₂₁，θ₂₂，θ₂₃的观测误差，各变量都是与时间t有关的函数则量测模型可表示为

Z(t)＝h[X(t)，t]+v(t)               (22)

由小行星角度信息的量测方程可以看出，方程中不含姿态矩阵，因此与图像坐标信息和矢量方向信息相比，以小行星角度信息作为观测量的导航方法不受姿态确定精度的影响，可以为探测器位置速度的确定提供较高的导航精度。

3.对步骤1中式(19)所示的状态方程及步骤2中式(22)所示的量测方程进行离散化

X(k+1)＝F(X(k)，k)+w(k)         (23)

Z(k)＝H(X(k)，k)+v(k)           (24)

式中，k＝1，2，…，F(X(k)，k)为f(X(t)，t)离散后的非线性状态转移函数，H(X(k)，k)为h(X(t)，t)离散后的非线性量测函数，w(k)、v(k)互不相关。

4.获取敏感器角度信息量测量

敏感器成像过程如图3所示，图3以第一小行星为例描述了第一小行星敏感器的成像过程，第二小行星敏感器成像过程与之类似。第一小行星敏感器主要由光学透镜和二维成像面阵组成，在第一小行星敏感器测量坐标系OX_cY_cZ_c中第一小行星反射太阳光线沿第一小行星到探测器的方向矢量射向第一小行星敏感器，此时，第一小行星在第一小行星敏感器测量坐标系中的坐标为(x_c，y_c，z_c)；第一小行星敏感器的光学透镜以焦距f₁将火星的光线透射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像亮度信号储存；由于第一小行星在二维成像面阵上的图像并不是一个点，而是一个圆，通过质心识别等图像处理技术确定第一小行星图像在二维成像平面坐标系OX_2dY_2d的质心(x_2d，y_2d)，这一中心可以用像元像线坐标系O_p1X_p1Y_p1中的像元像线(p，l)表示，从小行星的成像原理逆推，可以转换出第一小行星的矢量方向则获取角度信息量测量具体步骤为：

①小行星敏感器获取第一小行星和第二小行星的两幅图像信息；

②步骤①中获取的图像信息经质心提取后，获得第一小行星的像元像线(p_m1，l_m1)、第一小行星图像中的第一颗、第二颗和第三颗背景恒星的像元像线(p_m11，l_m11)，(p_m12，l_m12)，(p_m13，l_m13)、第二小行星的像元像线(p_m2，l_m2)和第二小行星图像中第一颗、第二颗和第三颗背景恒星的像元像线(p_m21，l_m21)，(p_m22，l_m22)，(p_m23，l_m23)；

③第一小行星、第二小行星和背景恒星的二维像元像线转换为三维矢量方向；

A.首先将第一小行星、第二小行星及其背景恒星的像元像线信息转换为敏感器二维成像平面坐标系OX_2dY_2d中的坐标

$\left(\begin{array}{c}\left(\begin{array}{c}{x}_{m1}^{2d}\\ y_{m1}^{2d}\end{array}\right)=K_1^{-1}(\left(\begin{array}{c}{p}_{m1}\\ l_{m1}\end{array}\right)-\left(\begin{array}{c}{p}_{01}\\ l_{01}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m11}^{2d}\\ y_{m11}^{2d}\end{array}\right)=K_1^{-1}(\left(\begin{array}{c}{p}_{m11}\\ l_{m11}\end{array}\right)-\left(\begin{array}{c}{p}_{01}\\ l_{01}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m12}^{2d}\\ y_{m12}^{2d}\end{array}\right)=K_1^{-1}(\left(\begin{array}{c}{p}_{m12}\\ l_{m12}\end{array}\right)-\left(\begin{array}{c}{p}_{01}\\ l_{01}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m13}^{2d}\\ y_{m13}^{2d}\end{array}\right)=K_1^{-1}(\left(\begin{array}{c}{p}_{m13}\\ l_{m13}\end{array}\right)-\left(\begin{array}{c}{p}_{01}\\ l_{01}\end{array}\right))\end{array}\right)---\left(25\right)$

$\left(\begin{array}{c}\left(\begin{array}{c}{x}_{m2}^{2d}\\ y_{m2}^{2d}\end{array}\right)=K_2^{-1}(\left(\begin{array}{c}{p}_{m2}\\ l_{m2}\end{array}\right)-\left(\begin{array}{c}{p}_{02}\\ l_{02}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m21}^{2d}\\ y_{m21}^{2d}\end{array}\right)=K_2^{-1}(\left(\begin{array}{c}{p}_{m21}\\ l_{m21}\end{array}\right)-\left(\begin{array}{c}{p}_{02}\\ l_{02}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m22}^{2d}\\ y_{m22}^{2d}\end{array}\right)=K_2^{-1}(\left(\begin{array}{c}{p}_{m22}\\ l_{m22}\end{array}\right)-\left(\begin{array}{c}{p}_{02}\\ l_{02}\end{array}\right))\\ \left(\begin{array}{c}{x}_{m23}^{2d}\\ y_{m23}^{2d}\end{array}\right)=K_2^{-1}(\left(\begin{array}{c}{p}_{m23}\\ l_{m23}\end{array}\right)-\left(\begin{array}{c}{p}_{02}\\ l_{02}\end{array}\right))\end{array}\right)---\left(26\right)$

式中，和为测量得到的第一小行星和第二小行星在第一小行星敏感器和第二小行星敏感器二维成像平面坐标系中的坐标，和为测量得到的第一颗、第二颗和第三颗背景恒星在第一小行星敏感器和第二小行星敏感器二维成像平面坐标系中的坐标，K₁和K₂分别为第一小行星和第二小行星成像敏感器由毫米转为像素的相机转换矩阵，(p₀₁，l₀₁)、(p₀₂，l₀₂)分别为第一小行星和第二小行星敏感器中心的像元和像线。

B.将第一小行星、第二小行星及其背景恒星的敏感器二维成像平面坐标系中的坐标转换为三维矢量信息坐标

$\left(\begin{array}{c}{\overrightarrow{l}}_{m1}^c=\frac{1}{\sqrt{{\left(x_{m1}^{2d}\right)}^2+{\left(y_{m1}^{2d}\right)}^2+f_1^2}}{\left(\begin{array}{ccc}{x}_{m1}^{2d}& y_{m1}^{2d}& -f_1\end{array}\right)}^T\\ {\overrightarrow{s}}_{m11}^c=\frac{1}{\sqrt{{\left(x_{m11}^{2d}\right)}^2+{\left(y_{m11}^{2d}\right)}^2+f_1^2}}{\left(\begin{array}{ccc}{x}_{m11}^{2d}& y_{m11}^{2d}& -f_1\end{array}\right)}^T\\ {\overrightarrow{s}}_{m12}^c=\frac{1}{\sqrt{{\left(x_{m12}^{2d}\right)}^2+{\left(y_{m12}^{2d}\right)}^2+f_1^2}}{\left(\begin{array}{ccc}{x}_{m12}^{2d}& y_{m12}^{2d}& -f_1\end{array}\right)}^T\\ {\overrightarrow{s}}_{m13}^c=\frac{1}{\sqrt{{\left(x_{m13}^{2d}\right)}^2+{\left(y_{m13}^{2d}\right)}^2+f_1^2}}{\left(\begin{array}{ccc}{x}_{m13}^{2d}& y_{m13}^{2d}& -f_1\end{array}\right)}^T\end{array}\right)---\left(27\right)$

$\left(\begin{array}{c}{\overrightarrow{l}}_{m2}^c=\frac{1}{\sqrt{{\left(x_{m2}^{2d}\right)}^2+{\left(y_{m2}^{2d}\right)}^2+f_2^2}}{\left(\begin{array}{ccc}{x}_{m2}^{2d}& y_{m2}^{2d}& -f_2\end{array}\right)}^T\\ {\overrightarrow{s}}_{m21}^c=\frac{1}{\sqrt{{\left(x_{m21}^{2d}\right)}^2+{\left(y_{m21}^{2d}\right)}^2+f_2^2}}{\left(\begin{array}{ccc}{x}_{m21}^{2d}& y_{m21}^{2d}& -f_2\end{array}\right)}^T\\ {\overrightarrow{s}}_{m22}^c=\frac{1}{\sqrt{{\left(x_{m22}^{2d}\right)}^2+{\left(y_{m22}^{2d}\right)}^2+f_2^2}}{\left(\begin{array}{ccc}{x}_{m22}^{2d}& y_{m22}^{2d}& -f_2\end{array}\right)}^T\\ {\overrightarrow{s}}_{m23}^c=\frac{1}{\sqrt{{\left(x_{m23}^{2d}\right)}^2+{\left(y_{m23}^{2d}\right)}^2+f_2^2}}{\left(\begin{array}{ccc}{x}_{m23}^{2d}& y_{m23}^{2d}& -f_2\end{array}\right)}^T\end{array}\right)---\left(28\right)$

式中，和为量测得到的第一小行星和第二小行星相对探测器的单位矢量，和为量测得到的在第一小行星敏感器和第二小行星敏感器中的第一颗、第二颗和第三颗背景恒星相对探测器的单位矢量；

④将第一小行星、第二小行星和背景恒星的矢量方向信息转换为第一小行星、第二小行星和背景恒星之间的角度信息。

第一小行星敏感器量测得到的第一小行星和三颗背景恒星角度信息

$\left(\begin{array}{c}{\theta }_{m11}=\arccos (-{\overrightarrow{l}}_{m1}^c·{\overrightarrow{s}}_{m11}^c)\\ {\theta }_{m12}=\arccos (-{\overrightarrow{l}}_{m1}^c·{\overrightarrow{s}}_{m12}^c)\\ {\theta }_{m13}=\arccos (-{\overrightarrow{l}}_{m1}^c·{\overrightarrow{s}}_{m13}^c)\end{array}\right)---\left(29\right)$

第二小行星敏感器量测得到的第二小行星和第i颗背景恒星角度信息

$\left(\begin{array}{c}{\theta }_{m21}=\arccos (-{\overrightarrow{l}}_{m2}^c·{\overrightarrow{s}}_{m21}^c)\\ {\theta }_{m22}=\arccos (-{\overrightarrow{l}}_{m2}^c·{\overrightarrow{s}}_{m22}^c)\\ {\theta }_{m23}=\arccos (-{\overrightarrow{l}}_{m2}^c·{\overrightarrow{s}}_{m23}^c)\end{array}\right)---\left(30\right)$

第一小行星与第二小行星与各自图像中三颗背景恒星之间的角度量测量为：

Z_k＝[θ_m11(k)，θ_m12(k)，θ_m13(k)，θ_m21(k)，θ_m22(k)，θ_m23(k)]¹     (31)

式中，Z_k表示第k时刻的系统量测量，θ_m11(k)，θ_m12(k)，θ_m13(k)，θ_m21(k)，θ_m22(k)，θ_m23(k)分别表示第k时刻量测得到的第一小行星和第二小行星与三颗背景恒星之间的角度信息。

5.轨道自适应Unscented卡尔曼滤波方法

自适应Unscented卡尔曼滤波方法的流程图如图4所示，具体步骤为：

①初始化状态误差方差阵P₀，状态模型误差方差阵Q₀，量测模型误差方差阵R

P₀＝diag(5×10¹⁰，5×10¹⁰，5×10⁸，1，1，1×10^-2)；

Q₀＝diag(3，3，3，3×10^-3，3×10^-4，3×10^-5)；

R＝diag(1×10^-7，1×10^-7，1×10^-7，1×10^-7，1×10^-7，1×10^-7，1×10^-7，1×10^-7，1×10^-7)；

②扩维状态量和状态误差方差阵

设k时刻的状态量为$X_k^t={[x,y,z,v_x,v_y,v_z,w_x,w_y,w_z,w_{v_x},w_{x_y},w_z]}^T,$

k时刻的状态误差方差为式中，为状态与状态误差的协方差阵

③Unscented卡尔曼滤波

A.初始化

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

B.计算采样点

在附近选取一系列样本点，这些样本点的均值和协方差分别为和P(k|k)。设状态变量为9×1维，19个样本点χ_0，k，...，χ_18，k及其权重W₀…W₁₈分别如下

${\chi }_{0,k}={\hat{x}}_k,W_0=-2$

${\chi }_{j,k}={\hat{x}}_k+\sqrt{3}{\left(\sqrt{P\left(k\right|k)}\right)}_j,W_j=1/6---\left(33\right)$

${\chi }_{j+6,k}={\hat{x}}_k-\sqrt{3}{\left(\sqrt{P\left(k\right|k)}\right)}_j,W_{j+9}=1/6$

式中，当P(k|k)＝A^TA时，取A的第j行，当P(k|k)＝AA^T时，取A的第j列。

j＝1，2，....，9        (34)

C.时间更新

状态量的一步预测χ_k+1|k为

χ_k|k-1＝f(χ_k-1，k-1)        (35)

所有采样点状态量的一步预测加权后结果为

${\hat{x}}_k^{-}=\underset{j=0}{\overset{18}{\Sigma }}{W}_j{\chi }_{i,k|k-1}---\left(36\right)$

式中，W_j为第j个采样点的权值；

状态量的估计方差一步预测为

$P_k^{-}=\underset{j=0}{\overset{2n}{\Sigma }}{W}_j[{\chi }_{j,k|k-1}-{\hat{x}}_k^{-}]{[{\chi }_{j,k|k-1}-{\hat{x}}_k^{-}]}^T+Q_k---\left(37\right)$

式中，Q_k为k时刻状态模型噪声协方差阵；

采样点对应的量测估计值Z_k|k-1

Z_k|k-1＝h(χ_k|k-1，k)       (38)

所有采样点量测估计加权值

${\hat{z}}_k^{-}=\underset{j=0}{\overset{18}{\Sigma }}{W}_j{Z}_{j,k|k-1}---\left(39\right)$

D.量测更新

量测方差阵为

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{j=0}{\overset{18}{\Sigma }}{W}_j[Z_{j,k|k-1}-{\hat{z}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T+R_k---\left(40\right)$

式中，R_k为量测噪声协方差；

状态变量量测量方差阵

$P_{{\hat{x}}_k{\hat{z}}_k}=\underset{j=0}{\overset{18}{\Sigma }}{W}_j[{\chi }_{j,k|k-1}-{\hat{x}}_k^{-}]{[Z_{j,k|k-1}-{\hat{z}}_k^{-}]}^T---\left(41\right)$

滤波增益K_k为

$K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\hat{z}}_k{\hat{z}}_k}^{-1}---\left(42\right)$

状态量的估计值和估计方差P_k为

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(Z_k-{\hat{z}}_k^{-})---\left(43\right)$

$P_k=P_k^{-}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T---\left(44\right)$

式中，Q_k和R_k分别为系统和量测噪声协方差。

④误差估计算法

误差估计算法具体步骤如下

A.初始化窗口系数γ

B.自适应调节窗口大小γ

性能指标函数为

$J_k\left(\gamma \right)=\sqrt{\frac{1}{N}\underset{n=1}{\overset{N}{\Sigma }}(v_{1,n}^2+v_{2,n}^2+...+v_{m,n}^2)}$(k＝1，2，...)(45)

式中，v_k表示量测值与量测模型的残差，m为量测量的维数，v_1，n，v_2，n，...，v_m，n分别表示n时刻残差的第1，2，...，m行，N表示总采样数，这里N＝k；借助单纯形数值计算方法，求得使性能指标函数最小的窗口大小γ，使得量测残差最小；

C.计算状态模型误差方差的残差Q^*

$Q^{*}=\Delta x_k\Delta {x_k}^T+P_k^{-}-P_k^{+}-{\hat{Q}}_k^{-}---\left(46\right)$

式中，Δx_k为状态变量的残差，表示了量测更新前和量测更新后状态量的变化，为k时刻系统状态误差方差阵预测值，为k时刻系统状态误差方差阵一步预测值，为当前系统状态误差方差；

D.计算状态模型误差方差的估计

${\hat{Q}}_k^{+}={\hat{Q}}_k^{-}+\frac{1}{\gamma }(Q^{*}-{\hat{Q}}_k^{-})---\left(47\right)$

E.输出自适应调节后的窗口大小γ和状态模型误差方差的估计并返回至步骤③Unscented卡尔曼滤波，用于k+1时刻。

将第③步获得的状态量的估计值和估计方差P_k返回滤波器用于k+1时刻，k＝1，2，...并输出状态估计值(包括探测器的位置、速度导航信息)，输出的估计方差P_k表示了滤波估计的性能。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。