CN101762274B - 基于观测条件数的深空探测器自主定位路标选取方法 - Google Patents

Abstract

本发明涉及一种基于观测条件数的深空探测器自主定位路标选取方法，特别适用于双路标的探测器自主位置确定系统，属于自主导航领域。本发明通过对观测矩阵条件数的计算比较，选取两个路标即能够构建出探测器在目标天体固联坐标系下的位置，并使探测器位置确定精度大大提高，为低轨道飞行的深空探测器提供了精确可行的定位路标自主选取方法。本方法不仅考虑了路标位置对导航精度的影响，还具有计算小、解析可行等特点。

Description

基于观测条件数的深空探测器自主定位路标选取方法

技术领域

本发明涉及一种基于观测条件数的深空探测器自主定位路标选取方法，特别适用于双路标的探测器自主位置确定系统，属于自主导航领域。

背景技术

近目标天体飞行阶段是深空探测任务的核心阶段，主要的科学考察任务将在该阶段完成，因此需要较高的探测器轨道确定精度，以便科学观测数据的采集与处理。由于目标天体的自旋状态、引力场模型等这些动力学参数不确知性和未建模干扰力的存在，探测器所采用的导航方法必须具备较强鲁棒性。同时，深空目标天体一般距离地球较远，采用传统的基于深空网的导航模式已无法满足探测任务的高精度与强实时性要求，这要求探测器具有自主导航能力。

自主光学导航能减少操作的复杂性，增强探测器的自主生存能力，扩展探测器的空间应用潜力。深空天体表面存在大量的陨石坑，其具有较高的可见性与可分辨性，利用这种地形特征作为定位路标的自主导航方法，具有更好的轨道确定性能。基于路标导航的深空探测器自主光学导航方法已成为目前研究热点，其中如何在众多的定位路标中选取合适的路标以便达到期望的性能要求，是基于路标导航的一个关键技术，直接影响了软件算法的计算效率与探测器的自主定位能力，决定了探测任务是否能够成功完成，因此路标导航的自主选取方法是当前科技人员关注的重点问题之一。

在已发展的定位路标自主选取方法中，在先技术[1](参见Debo Sun，Crassidis J L.Observability Analysis of Six-Degree-of-Freedom ConfigurationDetermination Using Vector Observations[J].Journal of Guidance，Control，andDynamics.2002，25(6)：1149-57)，以共线方程作为观测模型，通过极大似然估计理论得到相对位置、姿态估计系统的最优误差方差矩阵和费歇尔(Fisher)信息阵；接着，通过对误差方差阵和信息阵的秩、迹和特征值的分析来确定视线测量相对导航系统的可观性和可观度。这种方法能够定性地分析路标距离探测器远近对导航精度的影响，但却不能定量地对导航系统的可观度进行分析，无法给出探测器在轨自主选择定位路标的准则。

在先技术[2](参见Burschka D，Geiman J，Hager G.Optimal landmarkconfiguration for vision-based control of mobile robots[C].2003 IEEE InternationalConference on Robotics and Automation.2003，3：3917-22)，利用观测矩阵的条件数对平面内移动的机器人的导航选取方法进行了研究，分析了定位路标的位置对导航精度的影响，并给出了定位路标的选取方法。但是这种方法只适合应用在平面二维运动空间内的物体，并不适用于深空探测器这种空间三维运动的物体。

在先技术[3](参见黄翔宇等.深空自主导航系统的可观性分析.宇航学报，2006，27(3)：332-337)，利用误差协方阵的方式对定位路标的观测对导航精度的影响，该方法通过对误差协方差阵的奇异值分析，采用数值方法得到了定位路标位置对导航精度的影响程度。但是由于该方法采用的是数值计算方法，计算量大，因此这种方法只适合前期的任务分析及地面试验分析，而并不能应用于探测器在轨的自主定位路标选取。

发明内容

本发明为解决目前基于路标导航的深空探测器尚没有精确可行的定位路标自主选取方法的问题，针对采用双路标信息进行深空探测器位置确定的导航系统，提出一种深空探测器自主定位路标选取方法，本方法主要基于观测方程的条件数来实现。

本发明的设计思想为：在地面站的支持下，高轨道绕飞阶段探测器对目标天体物理参数进行评估，在确定轨道动力学参数的同时，通过对天体表面图像分析，建立目标天体表面地形特征库，以确定定位路标在目标天体固联坐标系下的三维位置。在低轨道飞行的自主导航阶段，通过光学相机拍摄到的定位路标像元、像线信息，可以获得定位路标在探测器相机固联坐标系下的指向方向。探测器的姿态确定系统给出探测器相对惯性空间的姿态，结合地面站评估出的目标天体自旋姿态状态，能够确定探测器相对目标天体固联坐标系的姿态。这样因为定位路标在目标天体固联坐标系下的三维位置已知，则最少利用两个路标的像元、像线坐标就能够构建出探测器在目标天体固联坐标系下的位置，为低轨道飞行的深空探测器进行定位。

本发明所述的基于观测条件数的深空探测器自主定位路标选取方法的具体实现步骤如下：

步骤1，读取定位路标在目标天体固联坐标系下的位置数据，转换为探测器相机固联坐标系下的坐标值。

读取光学相机拍摄到的目标天体表面地形图像，从中提取到n个定位路标在目标天体固联坐标系下的位置矢量

(i＝1，2...n)，通过地面站做出的轨道预报和姿态递推，初步给出探测器相对目标天体的大致位置矢量探测器相机固联坐标系相对目标天体固联坐标系的转换矩阵为C_ba，则在探测器相机固联坐标系下，第i个定位路标点的位置矢量为

${\stackrel{\→}{r}}_i=C_{\mathrm{ba}}({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

其中，因目标天体固联坐标系为三维坐标系，转换矩阵C_ba为三行三列矩阵。

步骤2，为得到观测方程的条件数，利用步骤1得到的定位路标在探测器相机固联坐标系下的坐标值进行计算，先确定观测方程条件数的参数k_ij。

令x_i，y_i，z_i为探测器相机固联坐标系下第i个定位路标点的三轴位置坐标，x_j，y_j，z_j为探测器相机固联坐标系下第j个定位路标点

的三轴位置坐标，则对n个定位路标中每选取两个定位路标(如第i个和第j个)组合的参数k_ij值进行计算

$k_{\mathrm{ij}}=\frac{2(\frac{z_i^2}{r_i^2}+\frac{z_j^2}{r_j^2}-2\frac{z_i{z}_j}{r_i{r}_j}\cos \mathrm{\α})}{{(\frac{z_i^2}{z_j^2}\frac{r_j}{r_i}+\frac{z_j^2}{z_i^2}\frac{r_i}{r_j})}^2}$ (i，j＝1，2，…，n)

式中r_i和r_j为第i个和第j个定位路标与探测器之间的距离，满足

$r_i=\sqrt{x_i^2+y_i^2+z_i^2}$ $r_j=\sqrt{x_j^2+y_j^2+z_j^2}$

α为所选取的两个路标点视线之间的夹角，满足

$\mathrm{\α}=\arccos \left(\frac{{\stackrel{\→}{r}}_i\·{\stackrel{\→}{r}}_j}{\left|{\stackrel{\→}{r}}_i\right|\left|{\stackrel{\→}{r}}_j\right|}\right)$

n个定位路标中任意选取两个定位路标的组合共有

个，因此得到观测方程条件数的参数k_ij的值有个。

步骤3，根据步骤2得到的参数k_ij，计算观测方程的条件数κ_ij。利用步骤2得到的

个k_ij值，采用下式

${\mathrm{\κ}}_{\mathrm{ij}}=\frac{1-k_{\mathrm{ij}}+\sqrt{1-2k_{\mathrm{ij}}}}{k_{\mathrm{ij}}}$

计算每个参数k_ij对应的条件数κ_ij，得到

个κ_ij值。

步骤4，根据步骤3得到的观测方程的条件数κ_ij，选取最优的定位路标。比较步骤3得到的

个κ_ij值，以其中最小κ_ij值对应的第i个和第j个路标作为定位路标使用，对深空探测器的位置进行精确的定位。至此，完成深空探测器自主定位路标的选取。

有益效果

本发明通过对观测矩阵条件数的计算比较，选取两个路标即能够构建出探测器在目标天体固联坐标系下的位置，并使探测器位置确定精度大大提高，为低轨道飞行的深空探测器提供了精确可行的定位路标自主选取方法。本方法不仅考虑了路标位置对导航精度的影响，还具有计算小、解析可行等特点。

附图说明

图1为本发明方法的流程图。

图2为本发明中的深空探测器观测目标天体的导航关系示意图。

图3为本发明具体实施方式中的观测向量几何关系图。

图4为本发明具体实施方式中的双导航视线几何关系示意图。

具体实施方式

为了更好地说明本发明的目的与优点，下面结合附图和具体实施方式对本发明做进一步说明。

深空探测器在目标天体附件，利用光学相机拍摄目标天体表面图像，提取其中如岩石、陨石坑等地形特征明显的路标点，将其作为深空探测器自主导航的参考点。在深空探测器相对目标天体姿态已知的情况下，利用两个路标点的信息就可以确定深空探测器相对目标天体的位置状态，因此，为了简化实际工程算法、减少算法所占用的星载机计算时间，导航算法中采用两个导航路标来进行深空探测的自主定位。由于在光学相机拍摄到的一幅图像中，可以提取出许多个特征明显的路标点，那么如何在众多的路标点中选择两个路标最为最终的导航路标，以使导航精度达到最高点，这便涉及到导航路标的最优选取问题了。

由于观测噪声和路标位置估计误差的存在，使得探测器位置确定存在误差。观测噪声和参数摄动通过观测方程影响轨道状态的求解，观测方程的参数主要由定位路标与探测器之间的位置关系决定，因此探测器位置确定误差的大小与选取的路标位置有着直接的关系。

对于观测信息为双路标的情况，这里通过观测矩阵的条件数对定位路标与探测器之间位置关系对轨道确定精度的影响进行分析。条件数是观测方程

(H为观测矩阵，b为观测值，

为待估位置状态)求解误差的一种表示手段，它决定了参数摄动带来的相对误差上界。考虑观测方程有参数摄动：

$(H+\mathrm{\δH}){\stackrel{\→}{x}}_b=b+\mathrm{\δb}$

该摄动包括两部分：δH为观测矩阵H的摄动，该摄动主要是由于路标位置估计误差引起的；δb为观测噪声。观测方程的解

的相对误差的上界可以利用条件数κ表示：

$\frac{\left|\right|\stackrel{\→}{x}-{\stackrel{\→}{x}}_b\left|\right|}{\left|\right|\stackrel{\→}{x}\left|\right|}\≤\mathrm{\κ}(\frac{\left|\right|\mathrm{\δH}\left|\right|}{\left|\right|H\left|\right|}+\frac{\left|\right|\mathrm{\δb}\left|\right|}{\left|\right|b\left|\right|})$

上式表明轨道确定相对误差与观测矩阵H、观测值b的相对摄动之间的关系。κ越大，则轨道确定相对误差越大，甚至导致观测矩阵为病态。

考虑到κ(H)²＝κ(H^TH)，这里借助矩阵H^TH的条件数对系统可观测度进行分析。

步骤1，读取定位路标在目标天体固联坐标系下的位置数据，转换为探测器相机固联坐标系下的坐标值。

读取光学相机拍摄到的目标天体表面地形图像，从中提取到n个定位路标在目标天体固联坐标系下的位置矢量

(i＝1，2...n)，通过地面站做出的轨道预报和姿态递推，初步给出探测器相对目标天体的大致位置矢量

探测器相机固联坐标系相对目标天体固联坐标系的转换矩阵为C_ba，则在探测器相机固联坐标系下，第i个定位路标点的位置矢量为

${\stackrel{\→}{r}}_i=C_{\mathrm{ba}}({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

其中，因目标天体固联坐标系为三维坐标系，转换矩阵C_ba为三行三列矩阵。

步骤2，为得到观测方程的条件数，利用步骤1得到的路标在探测器相机固联坐标系下的坐标值进行计算，先确定观测方程条件数的参数k_ij。

当观测量为双路标的像元、像线坐标时，对在探测器相机固联坐标系下的任意两个路标

有

$H^{T}H=f^2\left[\begin{array}{ccc}\frac{1}{z_i^2}+\frac{1}{z_j^2}& 0& -\frac{x_i}{z_i^3}-\frac{x_j}{z_j^3}\\ 0& \frac{1}{z_i^2}+\frac{1}{z_j^2}& -\frac{y_i}{z_i^3}-\frac{y_j}{z_j^3}\\ -\frac{x_i}{z_i^3}-\frac{x_j}{z_j^3}& -\frac{y_i}{z_i^3}-\frac{y_j}{z_j^3}& \frac{x_i^2+y_i^2}{z_i^4}+\frac{x_j^2+y_j^2}{z_j^4}\end{array}\right]$

其中x_i，y_i，z_i和x_j，y_j，z_j分别为任意第i个和第j个路标在探测器相机固联坐标系下的三轴位置坐标。

由于H^TH为对称矩阵，因此可以求得H^TH的条件数为

$\mathrm{\κ}\left(H^{T}H\right)=\frac{\left|\right|{\mathrm{\λ}}_{\min }\left|\right|}{\left|\right|{\mathrm{\λ}}_{\max }\left|\right|}$

其中λ_min，λ_max分别为H^TH的绝对值最小和最大的特征值，通过求解式H^TH矩阵特征值，可得条件数

$\mathrm{\κ}\left(H^{T}H\right)=\frac{1-k_{\mathrm{ij}}+\sqrt{1-2k_{\mathrm{ij}}}}{k_{\mathrm{ij}}}---\left(1\right)$

其中

$k_{\mathrm{ij}}=\frac{2(\frac{z_i^2}{r_i^2}+\frac{z_j^2}{r_j^2}-2\frac{z_i{z}_j}{r_i{r}_j}\cos \mathrm{\α})}{{(\frac{z_i^2}{z_j^2}\frac{r_j}{r_i}+\frac{z_j^2}{z_i^2}\frac{r_i}{r_j})}^2}---\left(2\right)$

式中r_i，r_j(i，j＝1，2，...，n)为第i个和第j个定位路标与探测器之间的距离，满足

α为所选取的两个路标视线之间的夹角，满足

$\mathrm{\α}=\arccos \left(\frac{{\stackrel{\→}{r}}_i\·{\stackrel{\→}{r}}_j}{\left|{\stackrel{\→}{r}}_i\right|\left|{\stackrel{\→}{r}}_j\right|}\right)$

由此可见，因子k_ij越大，κ_ij(H^TH)越小。

步骤3，根据步骤2得到的参数k_ij，计算观测方程的条件数κ_ij。

将n个路标中任意两个路标在探测器相机固联坐标系中的三维坐标值代入(2)式，得到的k_ij值有

个，再将

个k_ij值代入(1)式，得到

个条件数κ_ij值。

下面通过k_ij对路标位置带来的影响进行分析。如图3的左图所示，令α_i，β_i分别为第i个观测视线的俯仰角和方位角，l_i为在X-Y平面上投影的长度，则以下各式成立

$\begin{array}{ccc}\sin {\mathrm{\α}}_i=\frac{z_i}{r_i}& \tan {\mathrm{\β}}_i=\frac{y_i}{x_i}& l_i=\sqrt{x_i^2+y_i^2}\end{array}$

定义截断距离λ_i＝z_isinα_i，该距离代表的物理意义如图3的右图所示。令ξ＝λ_i/λ_j，则(2)式的分母可化为(ξ+1/ξ)²，可见当ξ越接近1时，即两路标截断距离越相近时，因子k_ij值越大，系统条件数越小。参数ξ代表的物理意义为两路标与探测器之间距离关系对系统可观测性的影响。

对于上式分子，利用式几何关系，可化为

$2(\frac{z_i^2}{r_i^2}+\frac{z_j^2}{r_j^2}-2\frac{z_i{z}_j}{r_i{r}_j}\cos \mathrm{\α})$

$=1-\cos 2{\mathrm{\α}}_i\cos 2{\mathrm{\α}}_j-\sin 2{\mathrm{\α}}_i\sin 2{\mathrm{\α}}_j\cos ({\mathrm{\β}}_i-{\mathrm{\β}}_j)$

已知0≤α_i≤90°，所以sin2α_isin2α_j≥0，故cos(β_i-β_j)越小，因子k_ij越大，条件数越小，即在X-Y平面上，两路标观测向量之间夹角越大，轨道确定精度越高。

注意到sinα_isinα_j+cosα_icosα_jcosβ＝cosγ恒成立，其中β为所选取的两个路标点观测视线在X-Y平面上投影向量之间的夹角，γ为两视线之间夹角，如图4所示。定义与z轴成2α_i，2α_j夹角，且在X-Y平面上的投影夹角成β的两个单位矢量为辅助向量，则上式可化为

1-cos2α_icos2α_j-sin2α_isin2α_jcos(β_i-β_j)

＝1-sin(π/2-2α_i)sin(π/2-2α_j)

-cos(π/2-2α_i)cos(π/2-2α_j)cos(β_i-β_j)

＝1-cosγ′

上式中γ′为两个辅助向量之间的夹角，由于该夹角在[0，180°]的范围内，因此该夹角越大，因子k_ij越大，条件数越小。若γ′＝0，条件数无穷大，系统不可观，即两导航视线重合退化为单路标观测情况。该条件代表的物理意义为两定位路标观测矢量之间夹角关系对系统可观测性的影响。

步骤4，根据步骤3得到的观测方程的条件数κ_ij，选取最优的定位路标。

根据步骤3的分析，系统条件数主要与参数ξ和辅助向量夹角γ′有关，这两个参数决定了两路标观测矢量空间关系对系统可观测度的影响。在实际目标天体绕飞阶段，由于探测器与目标天体上各定位路标之间距离相近，因此在满足路标点尽量发散分布的条件下，可以利用(1)式计算系统条件数来选取最优的定位路标，且最小条件数对应的双路标组合即为导航精度最高的路标点。

在步骤3得到的

个条件数κ_ij值中，取最小条件数κ_ij，其对应的两个路标作为定位路标，可以利用其对探测器进行精确定位。

由以上实施步骤可见，该路标选取方法主要利用式(1)完成相应的路标选取工作，由于式(1)为解析表达式，直接利用其进行求解选取，可获得解析可行、计算量小等优点。该方法相对已有的路标选取方法，不仅适用于三维空间中自主导航路标的选取，同时该方法由于利用解析表达式直接进行求解选取，为近目标天体飞行的深空探测器提供了明确可行的导航路标选取方法。

Claims (1)

1.基于观测条件数的深空探测器自主定位路标选取方法，其特征在于：包括如下步骤：

步骤1，读取定位路标在目标天体固联坐标系下的位置数据，并将其转换为探测器相机固联坐标系下的坐标值；

读取光学相机拍摄到的目标天体表面地形图像，从中提取到n个定位路标在目标天体固联坐标系下的位置矢量

(i＝1，2...n)，通过地面站做出的轨道预报和姿态递推，初步给出探测器相对目标天体的大致位置矢量

探测器相机固联坐标系相对目标天体固联坐标系的转换矩阵为C_ba，则在探测器相机固联坐标系下，第i个定位路标点的位置矢量为

其中，因目标天体固联坐标系为三位坐标系，转换矩阵C_ba为三行三列矩阵；

步骤2，利用步骤1得到的定位路标在探测器相机固联坐标系下的坐标值进行计算，确定观测方程条件数的参数k_ij；

令x_i，y_i，z_i为探测器相机固联坐标系下第i个定位路标点

的三轴位置坐标，x_j，y_j，z_j为探测器相机固联坐标系下第j个定位路标点

的三轴位置坐标，则对n个定位路标中每选取两个定位路标，即第i个和第j个定位路标组合的参数k_ij值进行计算

$k_{\mathrm{ij}}=\frac{2(\frac{{z_i}^2}{{r_i}^2}+\frac{{z_j}^2}{{r_j}^2}-2\frac{z_i{z}_j}{r_i{r}_j}\cos \mathrm{\α})}{{(\frac{{z_i}^2{r}_j}{{z_j}^2{r}_i}+\frac{{z_j}^2{r}_i}{{z_i}^2{r}_j})}^2}$ (i，j＝1，2，…，n)

式中r_j和r_j为第i个和第j个定位路标与探测器之间的距离，满足

$r_i=\sqrt{{x_i}^2+{y_i}^2+{z_i}^2}$ $r_j=\sqrt{{x_j}^2+{y_j}^2+{z_j}^2}$

α为所选取的两个路标点视线之间的夹角，满足

$\mathrm{\α}=\arccos \left(\frac{{\stackrel{\→}{r}}_i{\stackrel{\→}{r}}_j}{\left|{\stackrel{\→}{r}}_i\right|\left|{\stackrel{\→}{r}}_j\right|}\right)$

n个定位路标中任意选取两个定位路标的组合共有

个，因此得到观测方程条件数的参数k_ij的值有

个；

步骤3，根据步骤2得到的参数k_ij，采用下式计算观测方程的条件数к_ij：

${\mathrm{\κ}}_{\mathrm{ij}}=\frac{1-k_{\mathrm{ij}}+\sqrt{1-2k_{\mathrm{ij}}}}{k_{\mathrm{ij}}}$

计算每个参数k_ij对应的条件数к_ij，得到

个к_ij值；

步骤4，根据步骤3得到的观测方程的条件数к_ij，选取最优的定位路标；

比较步骤3得到的个к_ij值，以其中最小к_ij值对应的第i个和第j个路标作为定位路标使用，对深空探测器的位置进行精确的定位。

Images