技术领域
[0001] 本
发明涉及在深空探测器处于捕获段时，基于目标天体图像和预测天体图像进行星历修正的自主天文导航方法，是一种非常适用于深空探测器捕获段的导航方法。
背景技术
[0002] 捕获段是指从深空探测器进入目标影响球开始到点火
制动的全过程，处于该阶段的深空探测器飞行速度快，飞行弧段短，控制
精度要求高，且机会唯一。深空探测器减速制动的入轨点距离目标行星表面非常近(近地点)，被捕获是整个深空探测任务的一个关键时间
节点，捕获阶段相对目标天体的相对导航精度和相对于太阳或者地球的绝对导航精度对后续探测任务有直接影响。然而深空探测器在捕获段航行速度快、空间电离环境未知、大气环境复杂，这些因素都对深空探测器的入轨精度有很大的影响，也制约着深空探测器捕获后绕飞、着陆等阶段的导航精度，当入轨精度不能达到指标要求时，甚至无法完成科学探测任务，导致整个任务的失败。
[0003] 目标天体星历数据是影响深空探测器捕获段导航性能的主要因素之一。目标天体星历数据是描述目标天体
位置、速度等特征的一类天体
数据库，由长期天文观测拟合而成的。若一段时间没有天文观测信息进行修正，目标天体星历数据误差会随时间的递推而增加。
太阳系内行星(除地球外)目前的星历误差约为200m～100km。
[0004] 处于捕获段的深空探测器距离地球较远，飞行速度较快，飞行距离远，时间长，环境未知性较强，传统上依靠地面测控的深空探测器导航与控制方法在实时性、成本和资源上受到种种限制，存在很多不足，很难满足深空探测一些特殊任务对高精度导航与控制的需要，因而对自主导航的要求更为迫切。
[0005] 传统基于天体星历修正的导航方法主要有两种，一种方法为将无线电导航信息与天文导航信息组合进行几何解析，求解目标天体星历及星历误差，此后将无线电导航导航结果转换至目标天体质心惯性
坐标系中，获得较高精度的导航信息。但是这种方法由于没有实时修正状态模型的模型误差，仍然无法消除目标天体星历误差对探测器状态模型精度的影响，导航精度仍较低。另一种方法(
专利号：201310006575.X一种基于星历修正的深空探测器天文/无线电组合导航方法)为利用天文和无线电两个
导航系统所估计的探测器位置，结合目标天体、探测器、地球的几何关系得到目标天体星历误差量测量，并获得目标天体星历误差量测量，利用卡尔曼滤波方法估计目标天体星历误差，并反馈至导航系统模型中，再进行天文/无线电测控导航信息融合。但是这两种方法都必须依赖无线电测控信息的协助，才可确定目标天体星历误差，仍然无法自主实时修正目标天体星历误差及其对状态模型误差和导航结果的影响。
发明内容
[0006] 本发明要解决的技术问题是：克服目标天体星历误差对相对地球导航精度的影响，弥补现有天文/无线电测控导航方法难以自主消除目标天体星历误差对探测器状态模型的影响这一不足，合理有效利用天文导航所提供的导航信息，为深空探测器捕获段提供一种高精度自主修正目标天体星历的天文导航方法。
[0007] 本发明解决其技术问题所采用的技术方案为：首先建立目标天体星历误差状态模型和量测模型，并根据预测天体图像与实际天体图像的位置差获取目标天体星历误差量测量；其次将目标天体星历误差状态模型和基于太阳和八大行星引
力的轨道动力学模型联立作为天文导航系统状态模型，并将目标天体星历误差量测模型和天文星光
角距量测模型作为天文导航系统的量测模型，根据探测器的状态模型和量测模型都呈现非线性特性，且系统噪声为非高斯噪声的特点，采用Unscented卡尔曼滤波方法，估计探测器位置、速度和目标天体星历误差，并将所估计的目标天体星历误差反馈至状态模型中，修正目标天体星历数据，提高状态模型的模型精度，并最终提高相对于目标天体和相对于日心的探测器导航信息的估计精度。
[0008] 具体包括以下步骤：
[0009] 1.建立目标天体星历误差状态模型
[0010] 根据目标天体星历误差在捕获段时间段内慢变这一特点，建立目标天体星历误差状态模型为：
[0011]
[0012] 式中，bx,by,bz为目标天体星历三轴位置误差， 为目标天体星历三轴位置误差的微分， 为目标天体星历误差状态模型误差；
[0013] 2.建立目标天体星历误差量测模型
[0014] Bm＝[bx by bz]T+wm (2)
[0015] 式中，wm为目标天体星历误差的测量噪声；Bm为目标天体星历误差量测量。
[0016] 3.量测量目标天体星历误差的获取
[0017] 图2给出了天文导航系统所用目标天体敏感器的成像原理图，目标天体卫星敏感器成像过程与之类似。目标天体敏感器主要由
光学透镜和二维成像面阵组成，在敏感器测量坐标系O′XcYcZc中沿目标天体到探测器的矢量方向 目标天体反射太阳光线射向目标天体敏感器，此时，目标天体在敏感器测量坐标系中的坐标为(xc,yc,zc)；目标天体敏感器的光学透镜以焦距f将目标天体的光线折射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像
亮度信号储存。
[0018] (1)目标天体矢量方向的获取
[0019] 根据目标天体敏感器的成像原理，目标天体矢量方向的获取步骤如下所示：
[0020] A.天体图像的处理
[0021] 由于目标天体在二维成像面阵上的图像并不是一个点，而是一个圆，通过质心识别等
图像处理技术确定目标天体图像在二维成像平面坐标系OX2dY2d的质心(x2d,y2d)，这一中心可以用像元像线坐标系OplXplYpl中的像元像线(p,l)表示。
[0022] B.质心坐标从像元像线坐标系转换至二维成像平面坐标系的转换
[0023] 根据像元像线坐标系与二维成像平面坐标系之间的关系，将目标天体质心坐标从像元像线坐标系转换至二维成像平面坐标系：
[0024]
[0025] 式中，(x2d,y2d)为目标天体在二维成像平面OX2dY2d中的坐标，p和l分别为目标天体在敏感器二维成像平面的像元和像线， 为由毫米转为
像素的敏感器转换矩阵,p0和l0分别为目标天体敏感器中心在像元像线坐标系OXplYpl中的像元和像线。
[0026] C.质心坐标从二维成像平面坐标系转换至敏感器测量坐标系的转换
[0027] 根据透镜成像原理，将目标天体质心坐标从二维成像平面坐标系转换至敏感器测量坐标系：
[0028]
[0029] 式中，f为目标天体敏感器的焦距， 为在目标天体敏感器测量坐标系中从目标天体至探测器的单位矢量。同理可获得目标天体图像中第i颗背景
恒星在敏感器测量坐标系中的单位矢量 i＝1,2,3。
[0030] 因此，可以根据预测图像获得预测目标天体质心所对应的单位矢量 实际图像中目标天体质心所对应的单位矢量
[0031] (2)目标天体图像半径的获取
[0032] 由于目标天体在二维成像面阵上的图像并不是一个点，而是一个圆，利用
边缘检测算法，提取火星图像轮廓，计算目标天体图像轮廓距离目标天体质心的距离，获得在二维成像平面坐标系中目标天体实际图像的半径 和在二维成像平面坐标系中目标天体预测图像的半径
[0033] (3)探测器到目标天体质心距离的解算
[0034] 探测器到实际目标天体质心之间的距离可以表示为：
[0035]
[0036] 式中，Rtarget为目标天体半径，可由地面天文数据库获得， f为目标天体敏感器的焦距， 为实际目标天体质心在二维成像平面坐标系中的坐标；
[0037] 探测器到星历所预测的目标天体质心之间的距离可以表示为：
[0038]
[0039] 式中， 为根据星历所预测的目标天体质心在二维成像平面坐标系中的坐标。
[0040] (4)目标天体星历误差的解算
[0041] 在敏感器测量坐标系中，目标天体星历误差矢量 可以根据探测器到实际目标天体质心的矢量和探测器到星历所预测目标天体质心的矢量计算得出：
[0042]
[0043] 由此可得，在目标天体质心惯性坐标系中目标天体星历误差可以表示为：
[0044]
[0045] 式中，Aib为探测器本体系至目标天体质心惯性坐标系的
姿态转移矩阵，Abc为探测器敏感器测量坐标系至本体系的转移矩阵；
[0046] 4.建立深空探测器天文导航系统的状态模型
[0047] 在目标天体质心惯性坐标系中，建立深空探测器基于太阳和八大行星引力轨道动力学模型，并与步骤1所建立的目标天体星历误差模型联立，构成天文导航系统的状态模型；
[0048]
[0049] 式中，x,y,z为探测器在目标天体质心惯性坐标系中三轴位置，vx,vy,vz为探测器在目标天体质心惯性坐标系中三轴速度，bx,by,bz为目标天体星历三轴位置误差， 为探测器在目标天体质心惯性坐标系中三轴位置的微分， 为探测器在目标天体质心惯性坐标系中三轴速度的微分， 为目标天体星历三轴位置误差的微分，μs、μm和μic分别为太阳、目标天体和第ic颗行星的引力常数；rps为日心到探测器的距离；rpm为目标天体质心到探测器的距离；rms为日心到目标天体质心的距离； 为第ic颗行星到探测器的距离；
为第ic颗行星质心到火心的距离；(xs,ys,zs)， 分别为太阳、第ic颗行星在目标
天体质心惯性坐标系中的三轴位置坐标，可根据时间由行星星历表获得，wx,wy,wz,
分别为状态模型中探测器三轴位置和速度状态模型误差；ic表示太阳和八大行
星中从内至外的第ic颗行星，如目标天体为火星，则ic＝1,2,3...,N(ic≠4)，N＝8，由于ic＝4表示第4颗行星(火星)，因此不包含在求和表达式中。
[0050] 式中的各变量都是与时间t有关的变量，可简写为：
[0051]
[0052] 式中，X(t)＝[x,y,z,vx,vy,vz,bx,by,bz]T为天文导航系统状态模型的状态向量，为X(t)的微分，f(X(t),t)为天文导航系统状态模型的系统非线性连续状态转移函数，为天文导航系统状态模型误差。
[0053] 5.建立天文导航系统量测模型
[0054] 天文导航系统量测模型选取星光角距量测模型和步骤2中所建立的星历误差量测模型作为量测模型，其中目标天体和两个卫星与三颗背景恒星之间的星光角距θ1i、θ2i和θ3i(i＝1,2,3)表达式为：
[0055]
[0056] 式中， 为在目标天体质心惯性坐标系中目标天体到探测器的单位矢量， 为目标天体图像中第i个恒星在目标天体质心惯性坐标系中的单位矢量； 为在目标天体质心惯性坐标系中目标天体的第一个卫星到探测器的单位矢量， 为目标天体第一个卫星图像中第i个恒星在目标天体质心惯性坐标系中的单位矢量； 为在目标天体质心惯性坐标系中第二个目标天体的卫星到探测器的单位矢量， 为目标天体第二个卫星图像中第i个恒星在目标天体质心惯性坐标系中的单位矢量。
[0057] 与目标天体星历误差量测模型式联立，得天文导航系统量测模型为：
[0058]
[0059] 设天文导航系统量测量Z＝[θ11,θ12,θ13,θ21,θ22,θ23,θ31,θ32,θ33,bx,by,bz]T，天文导航系统量测噪声 vθ11,vθ12,vθ13,vθ21,vθ22,vθ23,vθ31,vθ32,vθ33, 分别为量测量θ11,θ12,θ13,θ21,θ22,θ23,θ31,θ32,θ33,bx,by,bz的观测误差，由于各变量都是与时间t有关的变量，则可根据式(12)建立天文导航系统量测模型的表达式为：
[0060] Z(t)＝h[X(t),t]+v(t) (13)
[0061] 式中，h[X(t),t]为天文导航系统非线性连续量测函数。
[0062] 6.对步骤4和步骤5中的状态模型和量测模型进行离散化
[0063] X(k)＝F(X(k-1),k-1)+W(k-1) (14)
[0064] Z(k)＝H(X(k),k)+V(k) (15)
[0065] 式中，k＝1,2,…，F(X(k-1),k-1)为f(X(t),t)离散后从第k-1时刻到第k时刻的非线性状态转移函数，H(X(k),k)为h(X(t),t)离散后第k时刻的非线性量测函数，W(k)和V(k)为w(t)和v(t)离散后第k时刻的等效噪声，且W(k)和V(k)互不相关。
[0066] 7.天文导航量测量的获取及处理
[0067] 天文导航系统的量测量Z＝[θ11,θ12,θ13,θ21,θ22,θ23,θ31,θ32,θ33,bx,by,bz]T可分为两部分：(1)星光角距；(2)目标天体星历误差。其中量测量星光角距的获取可根据在火星敏感器测量坐标系中目标天体至探测器的单位矢量 与目标天体图像中第i颗背景恒星的单位矢量 计算两个矢量之间的星光角距θ1i：
[0068]
[0069] 同理可获得目标天体卫星与其背景恒星的星光角距θ2i,θ3i。而量测量目标天体星历误差的获取可根据步骤3中的式(3)～式(8)获得；
[0070] 8.对天文导航系统进行Unscented卡尔曼滤波
[0071] 根据天文导航系统状态模型、天文导航系统量测模型、天文导航敏感器获得的量测量，进行天文导航系统Unscented卡尔曼滤波，获得在目标天体质心惯性
坐标系中表示深空探测器位置、速度和目标天体星历误差的估计状态向量
和 估 计 均 方 误 差 阵 P k ，其 中
为第k时刻探测器在目标天体质心惯性坐标系中的三轴位置
估计、三轴速度估计、目标天体星历三轴估计误差。
[0072] 9.天文导航系统对目标天体星历误差进行反馈校正
[0073] 将第8步中获得的目标天体星历误差和目标天体星历估计均方误差阵反馈回深空探测器的状态模型中，并重新确定状态模型的模型误差协方差阵，最后将校正后的模型误差协方差阵输入至天文导航系统Unscented卡尔曼滤波中，修正下一时刻的导航结果。
[0074] 10.计算日心惯性坐标系中的探测器位置、速度估计状态向量
[0075] 将第8步获得的第k时刻探测器在目标天体质心惯性坐标系中的位置、速度估计状态向量 转换至日心惯性坐标系中，得到日心惯性坐标系中的估计状态向量
[0076]
[0077] 式中， 为探测器第k时刻在火心惯性坐标系中的位置、速度估计状态向量为目标天体在日心惯性坐标系中第k时刻的位
置、速度，可由目标天体星历数据库获得； 为目标天体第k时刻的位置、速度误差，
且
[0078] 最终将第8步和第10步获得的第k时刻在目标天体质心惯性坐标系中和在日心惯性坐标系中的估计状态向量 和估计均方误差阵Pk输出，估计状态向量分别表示在目标天体质心惯性坐标系中和日心惯性坐标系中探测器的位置、速
度信息，输出的估计均方误差阵Pk表示了滤波估计的性能，并将这些导航信息分别返回天文导航系中，用于k+1时刻的位置、速度导航信息，k＝1,2,...。
[0079] 本发明的原理是：由于天文导航敏感器可以直接测量目标天体图像，而利用天体星历数据也可预测天文导航系统所获得的目标天体图像，但预测天体图像与实际天体图像在敏感器成像平面的位置不同，位置的变化反映了目标天体星历误差的大小，本发明利用该信息自主修正目标天体星历误差。首先结合捕获段持续时间短、目标天体星历误差在捕获段变化小的特点，建立目标天体星历误差状态模型和量测模型，并根据预测天体图像与实际天体图像的位置差获取目标天体星历误差量测量；其次将目标天体星历误差状态模型和基于太阳和八大行星引力的轨道动力学模型联立作为天文导航系统状态模型，并将目标天体星历误差量测模型和天文星光角距量测模型作为天文导航系统的量测模型，根据探测器的状态模型和量测模型都呈现非线性特性，且系统噪声为非高斯噪声的特点，采用Unscented卡尔曼滤波方法，估计探测器位置、速度和目标天体星历误差，并将所估计的目标天体星历误差反馈至状态模型中，修正目标天体星历数据，提高状态模型的模型精度；提高对相对于目标天体和相对于日心的探测器导航信息的估计精度。
[0080] 本发明与
现有技术相比的优点在于：一方面实现了不再需要无线电测控信息就可以自主获得目标天体的星历误差，实现了对目标天体星历误差的高精度估计；另一方面天文导航系统可自主获得相对目标天体和相对日心的探测器高精度导航信息，并自主修正了导航系统的状态模型，减小了目标天体星历误差对状态模型精度的影响。
附图说明
[0081] 图1为本发明基于自主星历修正的天文导航
流程图。
[0082] 图2为本发明天文导航系统所用天文导航敏感器的成像原理图。
具体实施方式
[0083] 如图1所示，前述技术方案中所涉及的目标天体可以为火星、金星、木星、土星等太阳系内的天体，以下以火星作为
实施例，说明本发明的具体实施过程：
[0084] 1.建立火星星历误差状态模型
[0085] 根据火星星历误差在捕获段时间段内慢变这一特点，建立火星星历误差状态模型为：
[0086]
[0087] 式中，bx,by,bz为火星星历三轴位置误差， 为火星星历三轴位置误差的微分， 为火星星历误差状态模型误差；
[0088] 2.建立火星星历误差量测模型
[0089] Bm＝[bx by bz]T+wm (2)
[0090] 式中，wm为火星星历误差的测量噪声；Bm为火星星历误差量测量。
[0091] 3.量测量火星星历误差的获取
[0092] 图2给出了天文导航系统所用火星敏感器的成像原理图，火卫一敏感器、火卫二敏感器成像过程与之类似。火星敏感器主要由光学透镜和二维成像面阵组成，在敏感器测量坐标系O′XcYcZc中沿火星到探测器的矢量方向 火星反射太阳光线射向火星敏感器，此时，火星在火星敏感器测量坐标系中的坐标为(xc,yc,zc)；火星敏感器的光学透镜以焦距f将火星的光线折射后成像在二维成像面阵上，二维成像面阵将照在每个成像单元上的图像亮度信号储存。
[0093] (1)火星矢量方向的获取
[0094] 根据敏感器的成像原理，天体矢量方向的获取步骤如下所示：
[0095] A.天体图像的处理
[0096] 由于火星在二维成像面阵上的图像并不是一个点，而是一个圆，通过质心识别等图像处理技术确定火星图像在二维成像平面坐标系OX2dY2d的质心(x2d,y2d)，这一中心可以用像元像线坐标系OplXplYpl中的像元像线(p,l)表示。
[0097] B.质心坐标从像元像线坐标系转换至二维成像平面坐标系的转换
[0098] 根据像元像线坐标系与二维成像平面坐标系之间的关系，将火星质心坐标从像元像线坐标系转换至二维成像平面坐标系：
[0099]
[0100] 式中，(x2d,y2d)为火星在二维成像平面OX2dY2d中的坐标，p和l分别为火星在火星敏感器二维成像平面的像元和像线， 为由毫米转为像素的敏感器转换矩阵,p0和l0分别为火星敏感器中心在像元像线坐标系OXplYpl中的像元和像线。
[0101] C.质心坐标从二维成像平面坐标系转换至敏感器测量坐标系的转换
[0102] 根据透镜成像原理，将火星质心坐标从二维成像平面坐标系转换至敏感器测量坐标系：
[0103]
[0104] 式中，f为火星敏感器的焦距， 为在火星敏感器测量坐标系中从火星至探测器的单位矢量。同理可获得火星图像中第i颗背景恒星在火星敏感器测量坐标系中的单位矢量i＝1,2,3。
[0105] 因此，可以根据预测火星图像获得预测火星图像中火星质心所对应的单位矢量实际火星图像中火星质心所对应的单位矢量
[0106] (2)火星图像半径的获取
[0107] 由于火星在二维成像面阵上的图像并不是一个点，而是一个圆，利用边缘检测算法，提取火星图像轮廓，计算火星图像轮廓距离火星质心的距离，获得在二维成像平面坐标系中火星实际图像的半径 和在二维成像平面坐标系中火星预测图像的半径
[0108] (3)火星探测器到火星质心距离的解算
[0109] 火星探测器到实际火星质心之间的距离可以表示为：
[0110]
[0111] 式中，Rtarget＝3397km为火星半径， f为火星敏感器的焦距，为实际火星质心在二维成像平面坐标系中的坐标；
[0112] 火星探测器到火星星历所预测的火星质心之间的距离可以表示为：
[0113]
[0114] 式中， 为根据火星星历所预测的火星质心在二维成像平面坐标系中的坐标。
[0115] (4)火星星历误差的解算
[0116] 在敏感器测量坐标系中，火星星历误差矢量 可以根据探测器到实际火星质心的矢量和探测器到火星星历所预测火星质心的矢量计算得出：
[0117]
[0118] 由此可得，在火心惯性坐标系中火星星历误差可以表示为：
[0119]
[0120] 式中，Aib为探测器本体系至火心惯性坐标系的姿态转移矩阵，Abc为探测器敏感器测量坐标系至本体系的转移矩阵；
[0121] 4.建立深空探测器天文导航系统的状态模型
[0122] 首先初始化位置、速度、火星星历误差，设X＝[x y z vx vy vz bx by bz]T为在火心惯性坐标系中深空探测器天文导航系统的状态向量，x,y,z,vx,vy,vz分别为探测器在火心惯性坐标系中三轴的位置和速度，bx,by,bz为火星星历三轴位置误差，上述变量都是与t有关的函数，根据探测器的轨道设计和火星星历数据库轨道，选取探测器的位置、速度初值为x0,y0,z0, 火星星历三轴位置误差初值为 则深空探测器天文导航系统的状态向量初值为
[0123] 考虑太阳和火星、地球等八大行星对探测器的引力作用，选取火心惯性坐标系，联立步骤1获得的火星星历误差状态模型，可得深空探测器在火心惯性坐标系中状态模型：
[0124]
[0125] 式中，x,y,z为探测器在火心惯性坐标系中三轴位置，vx,vy,vz为探测器在火心惯性坐标系中三轴速度，bx,by,bz为火星星历三轴位置误差， 为探测器在火心惯性坐标系中三轴位置的微分， 为探测器在火心惯性坐标系中三轴速度的微分， 为火星星历三轴位置误差的微分，μs、μm和 分别为太阳、火星和第ic颗行星的引力常数；rps为日心到探测器的距离；rpm为火星到探测器的距离；rms为日心到火心的距离； 为第ic颗行星到探测器的距离； 为第ic颗行星质心到火心的距离；(xs,ys,zs)， 分别为太阳、第ic
颗行星在火心惯性坐标系中的三轴位置坐标，可根据时间由行星星历表获得，wx,wy,wz,分别为状态模型中探测器三轴位置和速度的状态模型误差；ic表示太阳和八大
行星中从内至外的第ic颗行星，ic＝1,2,3...,N(ic≠4)，N＝8，由于ic＝4表示第4颗行星(火星)，因此不包含在求和表达式中。
[0126] 式(9)中的各变量都是与时间t有关的变量，可简写为：
[0127]
[0128] 式中，X(t)＝[x,y,z,vx,vy,vz,bx,by,bz]T为天文导航系统状态模型的状态向量，为X(t)的微分，f(X(t),t)为天文导航系统状态模型的系统非线性连续状态转移函数，为天文导航系统状态模型误差。
[0129] 5.建立天文导航系统量测模型；
[0130] 天文导航系统量测模型选取火星、火卫一、火卫二与各自背景恒星的星光角距量测模型和星历误差量测模型作为量测模型。
[0131] 火星与第i颗背景恒星之间星光角距θmi的大小在不同坐标系中是相同的，因此其表达式为：
[0132]
[0133] 式中，vmi为θmi的测量噪声， 为在火星敏感器测量坐标系中从火星至探测器的单位矢量， 为在火心惯性坐标系中火星到探测器的单位矢量，可表示为：
[0134]
[0135] 式中，(x,y,z)为探测器在火心惯性坐标系中三轴位置坐标， 为火星图像中第i颗背景恒星在火星敏感器测量坐标系中的单位矢量， 为在火星图像中第i颗背景恒星星光在火心惯性坐标系中的单位矢量，i＝1,2,3，可由恒星星历数据库直接得到；
[0136] 同理可得火卫一和火卫二与其第i颗背景恒星之间星光角距θpi和θdi的表达式为：
[0137]
[0138]
[0139] 式中，vpi,vdi分别为θpi,θdi的测量噪声。
[0140] 与火星星历误差量测模型式联立，得天文导航系统量测模型为：
[0141]
[0142] 设天文导航系统量测量Z＝[θm1,θm2,θm3,θp1,θp2,θp3,θd1,θd2,θd3,bx,by,bz]T，天文导航系统量测噪声 vθm1,vθm2,vθm3,vθp1,vθp2,vθp3,vθd1,vθd2,vθd3, 分别为量测量θm1,θm2,θm3,θp1,θp2,θp3,θd1,θd2,θd3,bx,by,bz的观测误差，由于各变量都是与时间t有关的变量，则可根据式(15)建立天文导航系统量测模型的表达式为：
[0143] Z(t)＝h[X(t),t]+v(t) (16)
[0144] 式中，h[X(t),t]为天文导航系统非线性连续量测函数。
[0145] 6.对步骤4和步骤5中的状态模型和量测模型进行离散化
[0146] X(k)＝F(X(k-1),k-1)+W(k-1) (17)
[0147] Z(k)＝H(X(k),k)+V(k) (18)
[0148] 式中，k＝1,2,…，F(X(k-1),k-1)为f(X(t),t)离散后从第k-1时刻到第k时刻的非线性状态转移函数，H(X(k),k)为h(X(t),t)离散后第k时刻的非线性量测函数，W(k)和V(k)为w(t)和v(t)离散后第k时刻的等效噪声，且W(k)和V(k)互不相关。
[0149] 7.天文导航系统量测量的获取及处理
[0150] 天文导航系统的量测量Z＝[θm1,θm2,θm3,θp1,θp2,θp3,θd1,θd2,θd3,bx,by,bz]T可分为两部分：(1)星光角距；(2)火星星历误差。其中量测量星光角距的获取可根据在火星敏感器测量坐标系中火星至探测器的单位矢量 与火星图像中第i颗背景恒星的单位矢量 计算两个矢量之间的星光角距θmi
[0151]
[0152] 同理可获得火卫一与其背景恒星、火卫二与其背景恒星之间的星光角距θpi,θdi。
[0153] 而量测量火星星历误差的获取可根据步骤3中的式(3)～式(8)获得；
[0154] 8.对天文导航系统进行Unscented卡尔曼滤波
[0155] 根据状态模型式(17)、天文导航系统量测模型式(18)、由天文导航敏感器获得的量测量式(8)和式(19)，进行天文导航系统Unscented卡尔曼滤波。具体步骤如下：
[0156] A.初始化
[0157]
[0158] 式中， 为第0时刻(初始时刻)在火心惯性坐标系中探测器的三轴位置和速度估计值，x0为第0时刻(初始时刻)在火心惯性坐标系中探测器的三轴位置和速度真实值，P0为状态向量的初始均方误差阵。
[0160] 在天文导航系统第k-1时刻状态向量 附近选取一系列样本点，这些样本点的均值和均方误差阵分别为 和Pk-1。设状态向量为6×1维，那么13个天文导航系统的样本点χ0,k,...,χ12,k及其权重W0…W12分别如下：
[0161]
[0162] 式中,当Pk-1＝ATA时， 取A的第j行，当Pk-1＝AAT时， 取A的第j列，得第k-1时刻采样点χk-1的统一表达式为：
[0163]
[0164] C.时间更新
[0165] 天文导航系统状态向量的一步预测χk|k-1为：
[0166] χk|k-1＝F1(χk-1,k-1) (23)
[0167] 天文导航系统所有采样点状态向量的一步预测加权后结果 为：
[0168]
[0169] 式中，Wj为第j个采样点的权值；
[0170] 天文导航系统状态向量的估计均方误差阵一步预测 为：
[0171]
[0172] 式中，Qk′为k时刻天文导航系统的状态模型误差协方差阵；
[0173] 天文导航系统采样点对应的量测估计向量Zk|k-1
[0174] Zk|k-1＝H1(χk|k-1,k) (26)
[0175] 天文导航系统所有采样点量测估计加权向量
[0176]
[0177] D.量测更新
[0178] 天文导航系统量测均方误差阵 为：
[0179]
[0180] 式中，Rk为k时刻天文导航系统的量测噪声协方差阵；
[0181] 天文导航系统状态向量量测量均方误差阵
[0182]
[0183] 天文导航系统滤波增益Kk为：
[0184]
[0185] 天文导航系统的估计状态向量 和估计均方误差阵Pk为：
[0186]
[0187]
[0188] 式中，天文导航系统的估计状态向量为第k时刻探测器在火心惯性坐标系中的三轴位置估计、三轴
速度估计、火星星历三轴估计误差。
[0189] 9.天文导航系统对火星星历误差进行反馈校正
[0190] 将第8步的D中获得的火星星历误差和火星星历估计均方误差阵反馈回深空探测器的状态模型中，并重新确定状态模型的模型误差协方差阵Qk，最后将校正后的模型误差协方差阵Qk输入至天文导航系统Unscented卡尔曼滤波中，修正下一时刻的导航结果。
[0191] 10.计算日心惯性坐标系中的探测器位置、速度估计状态向量
[0192] 将式(31)获得的探测器第k时刻在火心惯性坐标系中的位置、速度估计状态向量转换至日心惯性坐标系中，得到日心惯性坐标系中的估计状态向量
[0193]
[0194] 式中， 为探测器第k时刻在火心惯性坐标系中的位置、速度估计状态向量可由式(31)获得； 为火星在日心惯性坐标系中
第k时刻的位置、速度，可由火星星历数据库获得； 为火星第k时刻的位置、速度误
差，且
[0195] 最终将式(31)～式(33)获得的第k时刻在火心惯性坐标系中和在日心惯性坐标系中的估计状态向量 和估计均方误差阵Pk输出，估计状态向量 分别表示在火心惯性坐标系中和日心惯性坐标系中探测器的位置、速度信息，输出的估计均方误差阵Pk表示了滤波估计的性能，并将这些导航信息分别返回天文导航系中，用于k+1时刻的位置、速度导航信息，k＝1,2,...。
[0196] 本发明
说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
