CN107655485A - 一种巡航段自主导航位置偏差修正方法 - Google Patents

Abstract

本发明公开的一种巡航段自主导航位置偏差修正方法，属于深空探测技术领域。本发明建立探测器的动力学模型并对其进行线性化处理；建立自主导航测量模型，光学导航相机获取可见行星的灰度图像，通过星载图像处理完成可见行星质心的识别和跟踪；确定探测器的初始轨道；通过可见行星和探测器之间的相对位置信息校正可见行星的位置；通过可见行星和探测器之间的相对速度信息校正可见行星质心角位置观测信息，获得更精确的观测模型；根据动力学模型及测量模型，基于非线性***滤波算法解算探测器实时状态，实现实时导航。本发明能够提高巡航段自主导航方法估计精度、滤波收敛速度，实现快速精确估计，为深空探测巡航段导航方案设计提供支持。

Description

一种巡航段自主导航位置偏差修正方法

技术领域

本发明涉及一种巡航段自主导航位置偏差修正方法，属于深空探测技术领域。

背景技术

深空探测是人类了解宇宙和太阳系的形成和演化、探索生命起源的主要途径，巡航段是深空探测最长的轨道段，该阶段具有动力学模型和星历数据不精确，飞行时间长等特点。巡航段飞行任务中的姿态信息由星敏感器确定，位置估计仍然依赖于地面通信站。然而在此飞行过程中，如果完全依靠地面测控站进行探测器导航，将严重增加地面站的负担；另外，当通信***发生临时性或永久性故障失效时，自主导航性能的优劣对探测任务的成败起着至关重要的作用。此外，深空探测巡航段的导航精度直接关系到后续目标天体的捕获、接近、绕飞等探测任务的成败，因此巡航段高精度的自主导航方法研究有重要意义。

基于以上特点和要求，参考深空1号(Deep Space 1)任务，星尘(Stardust)任务，智能1号(SMART-1)任务等众多深空探测任务，光学敏感器成为巡航段探测器自主确定位置和速度的主要敏感器。探测器在星际巡航段一般采用光学敏感器观测天体质心的自主导航方法，该方法能够实现探测器位置和速度的自主估计。然而星际巡航过程中，由于探测器和被观测天体在轨运行过程中相距很远且存在相对运动，光从被观测天体到达探测器的过程中，会导致探测器观测天体的方向(视方向)与其真实方向之间产生视向量偏差，即天体真实位置和观测位置之间产生位置矢量偏差，故导航误差随着探测器和被观测天体之间距离的增加而增大，因此需要对此偏差进行校正。

发明内容

针对现有技术中利用小行星测量的巡航段自主导航方法中，小行星数量有限、受筛选准则影响、星历误差大、以及探测器和被观测天体之间存在相对运动的问题，本发明公开的一种巡航段自主导航位置偏差修正方法，要解决的技术问题是提高巡航段自主导航方法的估计精度、滤波收敛速度，实现探测器状态的快速精确估计，为深空探测巡航段导航方案设计提供技术支持。

本发明的目的是通过下述技术方案实现的。

本发明公开的一种巡航段自主导航位置偏差修正方法，建立巡航段探测器的动力学模型并对动力学模型进行线性化处理。建立自主导航测量模型，光学导航相机获取可见行星的灰度图像，通过星载图像处理软件完成可见行星质心的识别和跟踪。通过基于空间的初始轨道确定算法确定探测器的初始轨道，所述的探测器的初始轨道包括探测器的位置矢量和速度矢量。通过可见行星和探测器之间的相对位置信息校正可见行星的位置。通过可见行星和探测器之间的相对速度信息校正可见行星质心角位置观测信息，从而获得更精确的观测模型，即完成位置偏差修正，即光行差效应校正。根据探测器巡航段的动力学模型及测量模型，基于非线性***滤波算法解算探测器实时状态，实现实时导航。

本发明公开的一种巡航段自主导航位置偏差修正方法，包括如下步骤：

步骤1：建立巡航段探测器的动力学模型。

探测器在太阳系中运动，主要受到太阳引力场的作用，其动力学满足开普勒二体方程。为了尽可能精确地建立探测器在巡航段的动力学模型，模型摄动项考虑了大行星的引力影响，太阳光压摄动影响和探测器推力因素，故探测器在日心惯性坐标系下的动力学方程表达式如公式(1)所示：

式中r和v分别表示探测器的位置和速度矢量；为第i个摄动行星的位置矢量；为第i个摄动行星相对探测器的位置矢量，即μ_s为太阳引力常数，μ_i为第i个摄动行星的引力常数；n_t为摄动行星的个数。C_R为探测器表面的反射系数，S_srp为太阳辐射光压因子，m为探测器的质量，k为推力系数，最后一项a表示其他未建模加速度。

选取探测器位置和速度作为状态变量，则则探测器的状态方程如公式(2)所示：

步骤2：建立巡航段自主导航测量模型。

相机成像的过程采用小孔成像的模型，定义可见行星的质心f₁在相机坐标系下的位置坐标为r_p＝[x_c y_c z_c]^T，则其在相机像平面内的像原像素坐标如公式(3)所示：

其中：f为相机焦距，z_c为目标点沿着相机基准线到相机成像平面的距离。

相机系与探测器本体系重合，考虑到相机测量过程中的姿态偏差，即定义x_c,y_c方向的姿态偏差分别为θ₁,θ₂，则相机测量过程中的随机旋转将对特征点的位置测量产生影响，故实际的位置坐标如公式(4)所示：

因为姿态角偏差较小，故简化为公式(5)

忽略分母影响，则简化为公式(6)：

选择可见行星质心的角位置作为相机的观测量，即测量矢量Z包括方位角和俯仰角如公式(7)所示：

其中：和υ_φ分别代表俯仰角和方位角的测量噪声，可见行星的质心位置矢量为R，则俯仰角和方位角计算如公式(8)所示：

步骤3：确定探测器初始轨道。

N_obs次观测的初始轨道确定问题写成公式(9)所示形式：

其中：r_k，R_k分别代表第k次航天器和可见行星在日心惯性系下的位置矢量。未知的距离ρ_k是待求量，表示探测器到可见行星的单位视线矢量。将待求量ρ_k表示成高阶多项式的形式，通过拟合方法求解满足公式(9)约束的待求量ρ_k的近似解，即确定探测器初始轨道。

为进一步提高步骤3中探测器初始轨道确定的求解效率和求解精度，步骤3中将待求量ρ_k表示成三阶多项式的形式，通过最小二乘拟合方法求解满足公式(9)约束的待求量ρ_k的近似解，优选如下方法实现：

待求量ρ_k在日心惯性系下三轴分量如公式(10)所示：

考虑三阶多项式的形式，将公式(10)表示成如公式(11)所示的形式：

公式(11)也应该满足公式(9)中的几何约束，所以，

t_k表示第k次的测量时间。将公式(12)中等式左边移到右边，定义残差如公式(13)所示：

进一步将公式(13)表示成公式(14)所示形式：

为了得到最好的拟合，需要调整多项式拟合的系数a_i，b_i，和c_i，使得残差φ_x,φ_y,和φ_z最小，所以有，

因此，通过拟合方法确定待求多项式中的系数，为进一步提高求解效率，选最小二乘法确定待求多项式中的系数，如公式(16)：

{a}＝{a₁ a₂ a₃ a₄}^T，{b}＝{b₁ b₂ b₃ b₄}^T，{c}＝{c₁ c₂ c₃ c₄}^T，3×3的矩阵T如公式(17)：

至此，如公式(11)所示的三阶多项式的系数已经全部求解，根据系数能够得到满足精度要求的待求量ρ_k的近似解，即完成探测器初始轨道确定。

步骤4：光行差效应校正。

通过可见行星和探测器之间的相对位置信息校正可见行星的位置矢量。通过可见行星和探测器之间的相对速度信息校正步骤2中如公式(7)所示的角位置观测信息，从而获得更精确的观测模型，即完成光行差效应校正。

步骤4具体实现方法如下：

步骤4.1：通过可见行星和探测器之间的相对位置信息校正可见行星的位置。

光行差校正指由于光到达探测器的过程中天体的运动，导致天体真实位置和观测位置之间产生的位移，对其进行校正。针对导致天体真实位置和观测位置之间产生的位移进行校正问题，需要对可见行星位置矢量进行进一步修正。定义观测时间为t_k，k＝1,2...,N_obs，各个测量方向的单位矢量为k＝1,2...,N_obs。因为探测器不能确定自己相对于太阳和地球的距离，而仅已知观测可见行星的位置矢量R_k，和相对可见行星的观测方向的单位矢量因此基于时间t_k-δt_k对可见行星真实位置矢量进行求解，其中δt_k是光从可见行星到达探测器所需的时间。首先确定δt_k，如公式(18)所示：

其中c代表光速。对行星的位置矢量进行校正，如公式(19)所示：

R_updated＝R(t_k-δt_k) (19)

可见行星位置矢量的更新由公式(18)和(19)确定，即完成可见行星位置矢量的校正。

步骤4.2：通过可见行星和探测器之间的相对速度信息校正步骤2中如公式(7)所示的角位置观测信息，得到更精确的测量模型。

定义探测器速度方向和观测方向之间的夹角是θ_obs，真实方向和观测方向之间的畸变角为ε，则修正后满足精度要求的真实天体观测方向如公式(20)所示：

其中：是探测器速度的单位矢量，θ_true是探测器速度和真实观测方向之间的夹角，且θ_true＝θ_obs+ε。不同时刻的畸变角ε表示成公式(21)的形式，

其中：c和v分别代表光速大小和探测器速度大小。

观测方向和真实方向之间的关系如公式(22)所示，

可见行星和探测器之间视线方向的夹角和探测器的速度矢量是已知的，因此真实观测方向可以通过公式(22)求解，结合公式(7)从而得到修正后真实的测量模型，至此完成光行差效应校正。

步骤5：根据步骤1中的探测器的动力学模型、步骤3中确定的探测器初始轨道及步骤4修正后真实的测量模型，基于非线性***滤波算法解算探测器实时状态信息，实现实时导航。

根据步骤1得到的巡航段探测器动力学模型、步骤3中确定的探测器初始轨道及步骤4修正后真实的测量模型，通过导航滤波对探测器的状态进行估计。由于动力学模型及测量模型均呈现非线性，故选用非线性滤波器，解算探测器实时状态信息，实现实时导航。

所述的非线性滤波器为扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、模型预测滤波器(MPF)或粒子滤波(PF)等。

为进一步提高导航滤波精度及收敛速度，所述的非线性滤波器优选扩展卡尔曼滤波(EKF)。

有益效果：

1、现有技术中利用光学敏感器测量小行星的导航方法中，近地小行星数量有限且受到严格筛选准则的约束，可用导航小行星数量难以满足任务要求，加之地面站观测得到的小行星星历信息有较大误差，导致该方法导航精度较低。本发明公开的一种巡航段自主导航位置偏差修正方法，通过选取星历信息更精确的可见行星为观测天体，提高导航算法的估计精度，满足巡航段自主导航的精度需求。

2、针对巡航段，探测器和被观测天体二者在轨运行过程中相距很远，再加上探测器和被观测天体速度的存在，导致光从被观测天体到达探测器的过程中，天体真实位置和观测位置之间产生位置偏差(光行差效应)，本发明公开的一种巡航段自主导航位置偏差修正方法针对该问题对光行差效应进行校正，即通过可见行星和探测器之间的相对位置信息校正可见行星的位置；通过可见行星和探测器之间的相对速度信息校正角位置观测信息，从而获得行星质心更精确的测量模型，提高导航方法的估计精度。

3、本发明公开的一种巡航段自主导航位置偏差修正方法，采用非线性滤波器对探测器的状态进行估计，能够提高自主导航算法的精度及滤波收敛速度。

附图说明

图1为巡航段自主导航位置偏差修正方法的流程图；

图2为具体实施例中采用该自主导航方法时，探测器在日心惯性坐标系下的导航误差曲线。(图2a为探测器x方向位置导航误差曲线、图2b为探测器y方向位置导航误差曲线、图2c为探测器z方向位置导航误差曲线、图2d为探测器x方向速度导航误差曲线、图2e为探测器y方向速度导航误差曲线、图2f为探测器z方向速度导航误差曲线、)

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

实施例1：

本实例针对深空探测巡航段进行仿真验证。利用光学相机测量探测器至可见行星质心的视线信息，同时对光行差效应进行校正，然后利用扩展卡尔曼滤波器(EKF)，对探测器的位置、速度状态进行联合估计，实现高精度实时自主导航。

本实例公开的一种巡航段自主导航位置偏差修正方法，包括如下步骤：

步骤1：建立巡航段探测器的动力学模型。

探测器在太阳系中运动，主要受到太阳引力场的作用，其动力学满足开普勒二体方程。为了尽可能精确地建立探测器在巡航段的动力学模型，模型摄动项考虑大行星的引力影响，太阳光压摄动影响和探测器推力因素，故探测器在日心惯性坐标系下的动力学方程表达式如公式(1)所示：

式中r和v分别表示探测器的位置和速度矢量；为第i个摄动行星的位置矢量；为第i个摄动行星相对探测器的位置矢量，即μ_s为太阳引力常数，μ_i为第i个摄动行星的引力常数；n_t为摄动行星的个数。C_R为探测器表面的反射系数，S_srp为太阳辐射光压因子，m为探测器的质量，k为推力系数，最后一项a表示其他未建模加速度。

选取探测器位置和速度作为状态变量，则则探测器的状态方程如公式(2)所示：

步骤2：建立巡航段自主导航测量模型。

相机成像的过程采用小孔成像的模型，定义可见行星的质心f₁在相机坐标系下的位置坐标为r_p＝[x_c y_c z_c]^T，则其在相机像平面内的像原像素坐标如公式(3)所示：

其中：f为相机焦距，z_c为目标点沿着相机基准线到相机成像平面的距离。

相机系与探测器本体系重合，考虑到相机测量过程中的姿态偏差，即定义x_c,y_c方向的姿态偏差分别为θ₁,θ₂，则相机测量过程中的随机旋转将对特征点的位置测量产生影响，故实际的位置坐标如公式(4)所示：

因为姿态角偏差较小，故简化为公式(5)：

忽略分母影响，则简化为公式(6)：

选择可见行星质心的角位置作为相机的观测量，即测量矢量Z包括方位角和俯仰角如公式(7)所示：

其中：和υ_φ分别代表俯仰角和方位角的测量噪声，可见行星的质心位置矢量为R，则俯仰角和方位角计算如公式(8)所示：

步骤3：确定探测器初始轨道

N_obs次观测的初始轨道确定问题写成公式(9)所示形式，

其中：r_k，R_k分别代表第k次航天器和可见行星在日心惯性系下的位置矢量。未知的距离ρ_k是待求量，表示探测器到可见行星的单位视线矢量。为进一步提高步骤3中探测器初始轨道确定的求解效率和求解精度，步骤3中将待求量ρ_k表示成三阶多项式的形式，通过最小二乘拟合方法求解满足公式(9)约束的待求量ρ_k的近似解，选如下方法实现：

待求量ρ_k在日心惯性系下三轴分量如公式(10)所示：

考虑三阶多项式的形式，将公式(10)表示成如公式(11)所示的形式：

公式(11)也应该满足公式(9)中的几何约束，所以，

t_k表示第k次的测量时间。将公式(12)中等式左边移到右边，定义残差如公式(13)所示：

进一步将公式(13)表示成公式(14)所示形式：

为了得到最好的拟合，需要调整多项式拟合的系数a_i，b_i，和c_i，使得残差φ_x,φ_y,和φ_z最小，所以有，

因此，通过拟合方法确定待求多项式中的系数，为进一步提高求解效率，选最小二乘法确定待求多项式中的系数，如公式(16)：

{a}＝{a₁ a₂ a₃ a₄}^T，{b}＝{b₁ b₂ b₃ b₄}^T，{c}＝{c₁ c₂ c₃ c₄}^T，3×3的矩阵T如公式(17)：

至此，如公式(11)所示的三阶多项式的系数已经全部求解，根据系数能够得到满足精度要求的待求量ρ_k的近似解，即完成探测器初始轨道确定。

步骤4：光行差效应校正。

通过可见行星和探测器之间的相对位置信息校正可见行星的位置矢量。通过可见行星和探测器之间的相对速度信息校正步骤2中如公式(7)所示的角位置观测信息，从而获得更精确的观测模型，即完成光行差效应校正。

步骤4具体实现方法如下：

步骤4.1：通过可见行星和探测器之间的相对位置信息校正可见行星的位置。

光行差校正指由于光到达探测器的过程中天体的运动，导致天体真实位置和观测位置之间产生的位移，对其进行校正。针对这个问题，需要对可见行星位置矢量进行进一步修正。假设观测时间为t_k，k＝1,2...,N_obs，各个测量方向的单位矢量为k＝1,2...,N_obs。因为探测器不能确定自己相对于太阳和地球的距离，而仅已知观测可见行星的位置矢量R_k，和相对可见行星的观测方向的单位矢量因此基于时间t_k-δt_k对可见行星真实位置矢量进行求解，其中δt_k是光从可见行星到达探测器所需的时间。首先确定δt_k，如公式(18)所示：

其中c代表光速。对行星的位置矢量进行校正，如公式(19)所示：

R_updated＝R(t_k-δt_k) (19)

可见行星位置矢量的更新由公式(18)和(19)确定，即完成可见行星位置矢量的校正。

步骤4.2：通过可见行星和探测器之间的相对速度信息校正步骤2中如公式(7)所示的角位置观测信息，得到更精确的测量模型。

定义探测器速度方向和观测方向之间的夹角是θ_obs，真实方向和观测方向之间的畸变角为ε，则修正后满足精度要求的真实天体观测方向如公式(20)所示：

其中：是探测器速度的单位矢量，θ_true是探测器速度和真实观测方向之间的夹角，且θ_true＝θ_obs+ε。不同时刻的畸变角ε表示成公式(21)的形式：

其中：c和v分别代表光速大小和探测器速度大小。

观测方向和真实方向之间的关系如公式(22)所示，

可见行星和探测器之间视线方向的夹角和探测器的速度矢量是已知的，因此真实观测方向可以通过公式(22)求解，结合公式(7)从而得到修正后真实的测量模型，至此完成光行差效应校正。

步骤5：根据步骤1中的探测器的动力学模型、步骤3中确定的探测器初始轨道及步骤4修正后真实的测量模型，基于非线性***滤波算法解算探测器实时状态信息，实现实时导航。

根据步骤1得到的巡航段探测器动力学模型、步骤3中确定的探测器初始轨道及步骤4修正后真实的测量模型，通过导航滤波对探测器的状态进行估计。由于动力学模型及测量模型均呈现非线性，故选用非线性滤波器，为进一步提高导航滤波精度及收敛速度，所述的非线性滤波器优选扩展卡尔曼滤波(EKF)。解算探测器实时状态信息，实现实时导航。

地球是绕太阳运行的可见天体，选取地球为被观测的可见天体，探测器通过观测地球进行导航。对于初始轨道确定部分，用于观测的次数N_obs＝8，时间间隔T＝3×10⁴s。导航相机的焦距为200mm，视场角为3°，分辨率为1024×1024，图像处理精度为0.1个像素，相机测量误差为σ_c＝10^-4rad，姿态偏差θ_1,2＝10^-4rad。考虑探测器规划调度和测量信息处理的时间，取滤波周期为ΔT＝1500s，仿真时间设为T＝1.5×10⁵s。探测器初始位置误差为10⁵km，速度误差为10km/s，探测器的初始位置和速度矢量(r和v)，地球的初始位置和速度矢量(R和V)如表1所示，

表1 初始状态仿真参数

基于位置偏差修正的巡航段自主导航方法的数值仿真结果分别如图2所示，图中分别给出了探测器位置和速度的导航估计误差曲线。从仿真结果可以看出，修正后的导航方法有较高的导航精度和较快的滤波收敛速度，能够实现对探测器的位置及速度进行实时估计，最终得到高精度的状态估计信息。

本发明保护范围不仅局限于实施例，实施例仅用于解释本发明，凡与本发明在相同原理和构思条件下的变更或修改均在本发明公开的保护范围之内。

Claims (6)

1.一种巡航段自主导航位置偏差修正方法，其特征在于：包括如下步骤，

步骤1：建立巡航段探测器的动力学模型；

探测器在太阳系中运动，主要受到太阳引力场的作用，其动力学满足开普勒二体方程；为了尽可能精确地建立探测器在巡航段的动力学模型，模型摄动项考虑了大行星的引力影响，太阳光压摄动影响和探测器推力因素，故探测器在日心惯性坐标系下的动力学方程表达式如公式(1)所示：

<mrow> <mtable> <mtr> <mtd> <mrow> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>v</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mover> <mi>r</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>&amp;mu;</mi> <mi>s</mi> </msub> <msup> <mi>r</mi> <mn>3</mn> </msup> </mfrac> <mi>r</mi> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </munderover> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mo>&amp;lsqb;</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>&amp;rsqb;</mo> <mo>-</mo> <mfrac> <mrow> <msub> <mi>C</mi> <mi>R</mi> </msub> <msub> <mi>S</mi> <mrow> <mi>s</mi> <mi>r</mi> <mi>p</mi> </mrow> </msub> </mrow> <mrow> <msup> <mi>mr</mi> <mn>3</mn> </msup> </mrow> </mfrac> <mi>r</mi> <mo>+</mo> <mfrac> <mi>k</mi> <mi>m</mi> </mfrac> <mi>T</mi> <mo>+</mo> <mi>a</mi> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

式中r和v分别表示探测器的位置和速度矢量；为第i个摄动行星的位置矢量；为第i个摄动行星相对探测器的位置矢量，即μ_s为太阳引力常数，μ_i为第i个摄动行星的引力常数；n_t为摄动行星的个数；C_R为探测器表面的反射系数，S_srp为太阳辐射光压因子，m为探测器的质量，k为推力系数，最后一项a表示其他未建模加速度；

选取探测器位置和速度作为状态变量，则则探测器的状态方程如公式(2)所示：

<mrow> <mover> <mi>x</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfenced open = "{" close = "}"> <mtable> <mtr> <mtd> <msub> <mi>v</mi> <mi>x</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>z</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mfrac> <msub> <mi>&amp;mu;</mi> <mi>s</mi> </msub> <msup> <mrow> <mo>(</mo> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>z</mi> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mrow> <mn>3</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> </mfrac> <mi>x</mi> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </munderover> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mo>&amp;lsqb;</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>&amp;rsqb;</mo> <mo>-</mo> <mfrac> <mrow> <msub> <mi>C</mi> <mi>R</mi> </msub> <msub> <mi>S</mi> <mrow> <mi>s</mi> <mi>r</mi> <mi>p</mi> </mrow> </msub> </mrow> <mrow> <msup> <mi>mr</mi> <mn>3</mn> </msup> </mrow> </mfrac> <mi>x</mi> <mo>+</mo> <mfrac> <mi>k</mi> <mi>m</mi> </mfrac> <msub> <mi>T</mi> <mn>1</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mfrac> <msub> <mi>&amp;mu;</mi> <mi>s</mi> </msub> <msup> <mrow> <mo>(</mo> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>z</mi> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mrow> <mn>3</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> </mfrac> <mi>y</mi> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </munderover> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mo>&amp;lsqb;</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>&amp;rsqb;</mo> <mo>-</mo> <mfrac> <mrow> <msub> <mi>C</mi> <mi>R</mi> </msub> <msub> <mi>S</mi> <mrow> <mi>s</mi> <mi>r</mi> <mi>p</mi> </mrow> </msub> </mrow> <mrow> <msup> <mi>mr</mi> <mn>3</mn> </msup> </mrow> </mfrac> <mi>y</mi> <mo>+</mo> <mfrac> <mi>k</mi> <mi>m</mi> </mfrac> <msub> <mi>T</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mfrac> <msub> <mi>&amp;mu;</mi> <mi>s</mi> </msub> <msup> <mrow> <mo>(</mo> <msup> <mi>x</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>y</mi> <mn>2</mn> </msup> <mo>+</mo> <msup> <mi>z</mi> <mn>2</mn> </msup> <mo>)</mo> </mrow> <mrow> <mn>3</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> </mfrac> <mi>z</mi> <mo>+</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>n</mi> <mi>t</mi> </msub> </munderover> <msub> <mi>&amp;mu;</mi> <mi>i</mi> </msub> <mo>&amp;lsqb;</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>r</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> </msub> <msubsup> <mi>r</mi> <msub> <mi>t</mi> <mi>i</mi> </msub> <mn>3</mn> </msubsup> </mfrac> <mo>&amp;rsqb;</mo> <mo>-</mo> <mfrac> <mrow> <msub> <mi>C</mi> <mi>R</mi> </msub> <msub> <mi>S</mi> <mrow> <mi>s</mi> <mi>r</mi> <mi>p</mi> </mrow> </msub> </mrow> <mrow> <msup> <mi>mr</mi> <mn>3</mn> </msup> </mrow> </mfrac> <mi>z</mi> <mo>+</mo> <mfrac> <mi>k</mi> <mi>m</mi> </mfrac> <msub> <mi>T</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>

步骤2：建立巡航段自主导航测量模型；

相机成像的过程采用小孔成像的模型，定义可见行星的质心f₁在相机坐标系下的位置坐标为r_p＝[x_c y_c z_c]^T，则其在相机像平面内的像原像素坐标如公式(3)所示：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mi>p</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>p</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfrac> <mi>f</mi> <msub> <mi>z</mi> <mi>c</mi> </msub> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>c</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>

其中：f为相机焦距，z_c为目标点沿着相机基准线到相机成像平面的距离；

相机系与探测器本体系重合，考虑到相机测量过程中的姿态偏差，即定义x_c,y_c方向的姿态偏差分别为θ₁,θ₂，则相机测量过程中的随机旋转将对特征点的位置测量产生影响，故实际的位置坐标如公式(4)所示：

<mrow> <msubsup> <mi>r</mi> <mi>p</mi> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msub> <mi>R</mi> <mrow> <mi>a</mi> <mi>t</mi> <mi>t</mi> <mi>i</mi> <mi>t</mi> <mi>u</mi> <mi>d</mi> <mi>e</mi> </mrow> </msub> <msub> <mi>r</mi> <mi>p</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>cos&amp;theta;</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>sin&amp;theta;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>sin&amp;theta;</mi> <mn>2</mn> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>sin&amp;theta;</mi> <mn>1</mn> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>cos&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>cos&amp;theta;</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>r</mi> <mi>p</mi> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>

因为姿态角偏差较小，故简化为公式(5)

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mi>p</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>p</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfrac> <mi>f</mi> <mrow> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>x</mi> <mi>c</mi> </msub> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>y</mi> <mi>c</mi> </msub> <mo>+</mo> <msub> <mi>z</mi> <mi>c</mi> </msub> </mrow> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mi>c</mi> </msub> <mo>-</mo> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <msub> <mi>z</mi> <mi>c</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mi>c</mi> </msub> <mo>-</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <msub> <mi>z</mi> <mi>c</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>

忽略分母影响，则简化为公式(6)：

<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mi>p</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>p</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfrac> <mi>f</mi> <msub> <mi>z</mi> <mi>c</mi> </msub> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mi>c</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>c</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mi>f</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mi>f</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>

选择可见行星质心的角位置作为相机的观测量，即测量矢量Z包括方位角和俯仰角如公式(7)所示：

其中：和υ_φ分别代表俯仰角和方位角的测量噪声，可见行星的质心位置矢量为R，则俯仰角和方位角计算如公式(8)所示：

步骤3：确定探测器初始轨道；

N_obs次观测的初始轨道确定问题写成公式(9)所示形式：

<mrow> <msub> <mi>r</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mi>k</mi> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>,</mo> <mi>k</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2...</mn> <mo>,</mo> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>

其中：r_k，R_k分别代表第k次航天器和可见行星在日心惯性系下的位置矢量；未知的距离ρ_k是待求量，表示探测器到可见行星的单位视线矢量；将待求量ρ_k表示成高阶多项式的形式，通过拟合方法求解满足公式(9)约束的待求量ρ_k的近似解，即确定探测器初始轨道；

步骤4：光行差效应校正；

通过可见行星和探测器之间的相对位置信息校正可见行星的位置矢量；通过可见行星和探测器之间的相对速度信息校正步骤2中如公式(7)所示的角位置观测信息，从而获得校正后更精确的真实观测模型，即完成光行差效应校正；

步骤5：根据步骤1中的探测器的动力学模型、步骤3中确定的探测器初始轨道及步骤4修正后真实的测量模型，基于非线性***滤波算法解算探测器实时状态信息，实现实时导航；

根据步骤1得到的巡航段探测器动力学模型、步骤3中确定的探测器初始轨道及步骤4修正后真实的测量模型，通过导航滤波对探测器的状态进行估计；由于动力学模型及测量模型均呈现非线性，故选用非线性滤波器，解算探测器实时状态信息，实现实时导航。

2.如权利要求1所述的一种巡航段自主导航位置偏差修正方法，其特征在于：步骤4具体实现方法如下，

步骤4.1：通过可见行星和探测器之间的相对位置信息校正可见行星的位置；

光行差校正指由于光到达探测器的过程中天体的运动，导致天体真实位置和观测位置之间产生的位移，对其进行校正；针对导致天体真实位置和观测位置之间产生的位移进行校正问题，需要对可见行星位置矢量进行进一步修正；定义观测时间为t_k，k＝1,2...,N_obs，各个测量方向的单位矢量为因为探测器不能确定自己相对于太阳和地球的距离，而仅已知观测可见行星的位置矢量R_k，和相对可见行星的观测方向的单位矢量因此基于时间t_k-δt_k对可见行星真实位置矢量进行求解，其中δt_k是光从可见行星到达探测器所需的时间；首先确定δt_k，如公式(18)所示：

<mrow> <msub> <mi>&amp;delta;t</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <msub> <mi>&amp;rho;</mi> <mi>k</mi> </msub> <mi>c</mi> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>

其中c代表光速；对行星的位置矢量进行校正，如公式(19)所示：

R_updated＝R(t_k-δt_k) (19)

可见行星位置矢量的更新由式(18)和(19)确定，即完成可见行星位置矢量的校正；

步骤4.2：通过可见行星和探测器之间的相对速度信息校正步骤2中如公式(7)所示的角位置观测信息，得到更精确的测量模型；

定义探测器速度方向和观测方向之间的夹角是θ_obs，真实方向和观测方向之间的畸变角为ε，则修正后满足精度要求的真实天体观测方向如公式(20)所示：

<mrow> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>t</mi> <mi>r</mi> <mi>u</mi> <mi>e</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> <msub> <mi>sin&amp;theta;</mi> <mrow> <mi>t</mi> <mi>r</mi> <mi>u</mi> <mi>e</mi> </mrow> </msub> <mo>-</mo> <mover> <mi>v</mi> <mo>^</mo> </mover> <mi>sin</mi> <mi>&amp;epsiv;</mi> </mrow> <mrow> <msub> <mi>sin&amp;theta;</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>

其中：是探测器速度的单位矢量，θ_true是探测器速度和真实观测方向之间的夹角，且θ_true＝θ_obs+ε；不同时刻的畸变角ε表示成公式(21)的形式，

<mrow> <mi>tan</mi> <mi>&amp;epsiv;</mi> <mo>=</mo> <mfrac> <mrow> <mo>(</mo> <mi>c</mi> <mo>/</mo> <mi>v</mi> <mo>)</mo> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> <mi>T</mi> </msubsup> <mover> <mi>v</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <mrow> <mo>(</mo> <mi>c</mi> <mo>/</mo> <mi>v</mi> <mo>)</mo> </mrow> <msubsup> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> <mi>T</mi> </msubsup> <mover> <mi>v</mi> <mo>^</mo> </mover> </mrow> </mfrac> <mo>=</mo> <mfrac> <mrow> <mo>(</mo> <mi>v</mi> <mo>/</mo> <mi>c</mi> <mo>)</mo> <msub> <mi>sin&amp;theta;</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </mrow> <mrow> <mn>1</mn> <mo>-</mo> <mrow> <mo>(</mo> <mi>v</mi> <mo>/</mo> <mi>c</mi> <mo>)</mo> </mrow> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>

其中：c和v分别代表光速大小和探测器速度大小；

观测方向和真实方向之间的关系如公式(22)所示，

<mrow> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>t</mi> <mi>r</mi> <mi>u</mi> <mi>e</mi> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <mi>v</mi> <mo>/</mo> <mi>c</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mrow> <mo>(</mo> <mi>v</mi> <mo>/</mo> <mi>c</mi> <mo>)</mo> </mrow> <msub> <mi>cos&amp;theta;</mi> <mrow> <mi>t</mi> <mi>r</mi> <mi>u</mi> <mi>e</mi> </mrow> </msub> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>22</mn> <mo>)</mo> </mrow> </mrow>

可见行星和探测器之间视线方向的夹角和探测器的速度矢量是已知的，因此真实观测方向可以通过式(22)求解，结合公式(7)从而得到修正后真实的测量模型，至此完成光行差效应校正。

3.如权利要求1或2所述的一种巡航段自主导航位置偏差修正方法，其特征在于：

为进一步提高步骤3中探测器初始轨道确定的求解效率和求解精度，步骤3中将待求量ρ_k表示成三阶多项式的形式，通过最小二乘拟合方法求解满足公式(9)约束的待求量ρ_k的近似解，选如下方法实现：

待求量ρ_k在日心惯性系下三轴分量如公式(10)所示：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>x</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>p</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>y</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>p</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>z</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>p</mi> <mi>z</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>

考虑三阶多项式的形式，将公式(10)表示成如公式(11)所示的形式：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>a</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>a</mi> <mn>2</mn> </msub> <mi>t</mi> <mo>+</mo> <msub> <mi>a</mi> <mn>3</mn> </msub> <msup> <mi>t</mi> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>a</mi> <mn>4</mn> </msub> <msup> <mi>t</mi> <mn>3</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>b</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>b</mi> <mn>2</mn> </msub> <mi>t</mi> <mo>+</mo> <msub> <mi>b</mi> <mn>3</mn> </msub> <msup> <mi>t</mi> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>b</mi> <mn>4</mn> </msub> <msup> <mi>t</mi> <mn>3</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>z</mi> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>+</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mi>t</mi> <mo>+</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <msup> <mi>t</mi> <mn>2</mn> </msup> <mo>+</mo> <msub> <mi>c</mi> <mn>4</mn> </msub> <msup> <mi>t</mi> <mn>3</mn> </msup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>

公式(11)也应该满足公式(9)中的几何约束，所以，

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>R</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>z</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>R</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> <mo>,</mo> <mi>k</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2...</mn> <mo>,</mo> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>

t_k表示第k次的测量时间；将公式(12)中等式左边移到右边，定义残差如公式(13)所示：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;phi;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <msub> <mi>p</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;phi;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <msub> <mi>p</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;phi;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <msub> <mi>p</mi> <mi>z</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>,</mo> <mi>k</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2...</mn> <mo>,</mo> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>

进一步将公式(13)表示成公式(14)所示形式：

<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>&amp;phi;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>&amp;phi;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>&amp;phi;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> <mn>2</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>

为了得到最好的拟合，需要调整多项式拟合的系数a_i，b_i，和c_i，使得残差φ_x,φ_y,和φ_z最小，所以有，

<mrow> <mtable> <mtr> <mtd> <mrow> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>a</mi> <mi>i</mi> </msub> </mrow> </mfrac> <mi>=0</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>b</mi> <mi>i</mi> </msub> </mrow> </mfrac> <mi>=0</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>c</mi> <mi>i</mi> </msub> </mrow> </mfrac> <mi>=0</mi> </mrow> </mtd> </mtr> </mtable> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>3</mn> <mo>,</mo> <mn>4</mn> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>

因此，通过拟合方法确定待求多项式中的系数，为进一步提高求解效率，选最小二乘法确定待求多项式中的系数，如公式(16)：

<mrow> <mtable> <mtr> <mtd> <mrow> <mo>{</mo> <mi>a</mi> <mo>}</mo> <mo>=</mo> <msup> <mi>T</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>x</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>t</mi> <mi>k</mi> <mn>3</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>{</mo> <mi>b</mi> <mo>}</mo> <mo>=</mo> <msup> <mi>T</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>y</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>t</mi> <mi>k</mi> <mn>3</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>{</mo> <mi>c</mi> <mo>}</mo> <mo>=</mo> <msup> <mi>T</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>+</mo> <msub> <mi>&amp;rho;</mi> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <msub> <mover> <mi>&amp;rho;</mi> <mo>^</mo> </mover> <mrow> <mi>z</mi> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>t</mi> <mi>k</mi> <mn>3</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>

{a}＝{a₁ a₂ a₃ a₄}^T，{b}＝{b₁ b₂ b₃ b₄}^T，{c}＝{c₁ c₂ c₃ c₄}^T，3×3的矩阵T如公式(17)：

<mrow> <mi>T</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>t</mi> <mi>k</mi> <mn>3</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msub> <mi>t</mi> <mi>k</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>t</mi> <mi>k</mi> <mn>2</mn> </msubsup> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>t</mi> <mi>k</mi> <mn>4</mn> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>...</mn> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mn>...</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>t</mi> <mi>k</mi> <mn>3</mn> </msubsup> </mrow> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>t</mi> <mi>k</mi> <mn>4</mn> </msubsup> </mrow> </mtd> <mtd> <mn>...</mn> </mtd> <mtd> <mrow> <msubsup> <mo>&amp;Sigma;</mo> <mrow> <mi>k</mi> <mo>=</mo> <mn>1</mn> </mrow> <msub> <mi>N</mi> <mrow> <mi>o</mi> <mi>b</mi> <mi>s</mi> </mrow> </msub> </msubsup> <msubsup> <mi>t</mi> <mi>k</mi> <mn>6</mn> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>

至此，如公式(11)所示的三阶多项式的系数已经全部求解，根据系数能够得到满足精度要求的待求量ρ_k的近似解，即完成探测器初始轨道确定。

4.如权利要求1或2所述的一种巡航段自主导航位置偏差修正方法，其特征在于：步骤5所述的非线性滤波器为扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、模型预测滤波器(MPF)或粒子滤波(PF)。

5.如权利要求4所述的一种巡航段自主导航位置偏差修正方法，其特征在于：为进一步提高导航滤波精度及收敛速度，步骤5所述的非线性滤波器选扩展卡尔曼滤波(EKF)。

6.一种巡航段自主导航位置偏差修正方法，其特征在于：建立巡航段探测器的动力学模型并对动力学模型进行线性化处理；建立自主导航测量模型，光学导航相机获取可见行星的灰度图像，通过星载图像处理软件完成可见行星质心的识别和跟踪；通过基于空间的初始轨道确定算法确定探测器的初始轨道，所述的探测器的初始轨道包括探测器的位置矢量和速度矢量；通过可见行星和探测器之间的相对位置信息校正可见行星的位置；通过可见行星和探测器之间的相对速度信息校正可见行星质心角位置观测信息，从而获得更精确的观测模型，即完成位置偏差修正，即光行差效应校正；根据探测器巡航段的动力学模型及测量模型，基于非线性***滤波算法解算探测器实时状态，实现实时导航。