CN101782392A - 基于观测矩阵的深空探测器自主导航路标选取方法 - Google Patents

Abstract

本发明涉及一种基于观测矩阵的深空探测器自主导航路标选取方法，特别适用于利用三个路标进行深空探测器位置和姿态自主确定的导航系统，属于自主导航领域。本发明通过读取导航路标在目标天体坐标系下的位置数据，计算出目标天体坐标系下n个路标相对探测器的位置矢量；通过计算观测矩阵的行向量，得到观测矩阵H；计算任意三个导航路标组合所对应的矩阵(HH^T)^-1的特征值之和，选取最优的三个导航路标。本发明的基于观测矩阵的深空探测器自主导航路标选取方法具有精度高、计算小、解析可行，为近目标天体飞行的深空探测提供了明确可行的导航路标选取方法。

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].2003IEEE InternationalConference on Robotics and Automation.2003，3：3917-22)，利用观测矩阵的条件数对平面内移动的机器人的导航选取方法进行了研究，分析了导航路标的位置对导航精度的影响，并给出了导航路标的选取方法。但是这种方法只适合应用在平面二维运动空间内的物体，并不适用于深空探测器这种空间三维运动的物体。

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

发明内容

本发明的目的是为了解决目前基于路标导航的深空探测器尚没有精确可行的导航路标自主选取方法的问题，针对采用三路标的像素信息进行深空探测器位置、姿态自主确定的导航系统，提出一种基于观测矩阵的深空探测器自主导航路标选取方法。

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

本发明的基于观测矩阵的深空探测器自主导航路标选取方法，具体步骤如下：

步骤1，读取导航路标在目标天体坐标系下的位置数据，计算目标天体坐标系下n个路标相对探测器的位置矢量

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

及轨道预报给出的深空探测器相对目标天体的大致位置矢量

计算目标天体坐标系下n个路标相对探测器的位置矢量

${\stackrel{\→}{r}}_i={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i$

步骤2，为得到观测矩阵H，利用步骤1得到的目标天体坐标系下n个路标相对探测器的位置矢量

计算观测矩阵的行向量

令 ${\stackrel{\→}{n}}_i={\stackrel{\→}{r}}_i/r_i$ 和 ${\stackrel{\→}{n}}_j={\stackrel{\→}{r}}_j/r_j$ 分别为第i个与第j个路标的单位视线向量(r_i和r_j为第i个与第j个路标到探测器的距离)，对n个导航路标中第i个和第j个路标组合的观测矩阵的行向量

进行计算：

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j}(i,j=\mathrm{1,2}...n)$

其中

和

为辅助向量，计算公式如下

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_j}{\sin A_{\mathrm{ij}}}$

其中，A_ij为探测器到第i个和第j个路标观测视线所形成的观测角。

对于n个导航路标，这样任意选取两个路标，两两组合后得到的观测矩阵行向量

共有个值。观测矩阵H由所选取路标对应的行向量构成。

步骤3，利用步骤2得到的观测矩阵行向量，计算任意三个导航路标组合所对应的矩阵(HH^T)^-1的特征值之和利用步骤2得到的

个行向量

对n个导航路标中任意三个导航路标(如第i个、第j个和第k个)组合所对应的矩阵(HH^T)^-1的特征值之和∑λ_ijk进行计算

$\mathrm{\Σ}{\mathrm{\λ}}_{\mathrm{ijk}}=\frac{(\frac{{\sin }^2{\mathrm{\θ}}_{\mathrm{ij}}}{h_{\mathrm{ij}}^2}+\frac{{\sin }^2{\mathrm{\θ}}_{\mathrm{jk}}}{h_{\mathrm{jk}}^2}+\frac{{\sin }^2{\mathrm{\θ}}_{\mathrm{ki}}}{h_{\mathrm{ki}}^2})}{1-{\cos }^2{\mathrm{\θ}}_{\mathrm{ij}}-{\cos }^2{\mathrm{\θ}}_{\mathrm{jk}}-{\cos }^2{\mathrm{\θ}}_{\mathrm{ki}}+2\cos {\mathrm{\θ}}_{\mathrm{ij}}\cos {\mathrm{\θ}}_{\mathrm{jk}}\cos {\mathrm{\θ}}_{\mathrm{ki}}}(i,j,k=\mathrm{1,2}...n)$

其中θ_ij，θ_jk，θ_ki分别为观测矩阵行向量

之间形成的夹角。

若令 $m=\frac{1}{2}n(n-1),$ 则对于n个导航路标，这样任意选取三个路标组合后得到的∑λ_ijk共有

个值。

步骤4，比较步骤3得到的特征值之和∑λ_ijk，选取最优的三个导航路标比较步骤3得到的

个∑λ_ijk值，以其中最小∑λ_ijk值对应的第i个、第j个和第k个路标作为导航路标使用。

至此，完成深空探测器位置和姿态自主确定的导航路标的选取。

有益效果

本发明通过对观测矩阵的计算比较，考虑了导航路标及其与探测器之间位置关系对导航精度的影响，给出了一种导航路标选取方法。该方法相对已有的路标选取方法，不仅适用于三维空间中自主导航路标的选取，同时该方法由于利用解析表达式直接进行求解选取，因此具有解析可行、计算量小等优点，为近目标天体飞行的深空探测器提供了明确可行的导航路标选取方法。

附图说明

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

图2为本发明的具体实施方式中导航路标成像关系示意图。

具体实施方式

为了更好地说明本发明的目的与优点，下面以利用路标进行自主导航的深空探测器为例，结合附图和具体实施方式对本发明做进一步说明。

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

由于拍摄的导航路标像元、像线坐标中包含观测噪声，以及导航路标位置确定误差的存在，因此利用导航路标像素信息进行探测器位置、姿态确定必然存在误差。下面将通过对观测矩阵的分析，讨论导航路标的分布对位置确定精度、姿态确定精度的影响，进而给出导航路标选取的最优观测方法。

注意到对于最小二乘问题，有如下关系存在

X-X^*＝(H^TH)^-1H^T(Y-Y^*)

其中，X为待估量，X^*为先验估计值，Y为观测值，Y^*为预测观测值，H为观测矩阵，令 $H^T(Y-Y^{*})=\tilde{Y},$ 则上式可以写成

$X-X^{*}={\left(H^{T}H\right)}^{-1}\tilde{Y}$

可见矩阵(H^TH)^-1的特征值是系统可观测程度的一种度量方式，矩阵特征值越小，其特征向量所对应的状态偏差向量或状态偏差向量的线性组合所对应的误差越小，即求解精度越高。

考虑到矩阵特征值λ(H^-TH^-1)＝λ(H^-1H^-T)，可见可以利用矩阵(HH^T)^-1的特征值之和对利用三个路标进行导航的探测器位置与姿态的确定精度进行分析。

步骤1，读取导航路标在目标天体坐标系下的位置数据，计算目标天体坐标系下n个路标相对探测器的位置矢量

读取光学相机拍摄到的目标天体表面地形图像，假设从中共提取到n个导航路标，令这n个导航路标在目标天体坐标系下的位置矢量

及轨道预报给出的深空探测器相对目标天体的大致位置矢量

这样可以计算目标天体坐标系下n个路标相对探测器的位置矢量

${\stackrel{\→}{r}}_i={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i$

步骤2，为得到观测矩阵H，利用步骤1得到的目标天体坐标系下n个路标相对探测器的位置矢量

计算观测矩阵的行向量

探测器利用其携带的光学相机可以对导航路标成像，通过提取图像中导航路标的像元、像线坐标，可以获得导航路标在探测器坐标系下的指向方向，导航路标成像关系如图2所示。

令探测器相机坐标系相对目标天体坐标系的位置矢量和转换矩阵分别为

和C_ba，则在探测器相机坐标系下，导航路标的位置矢量

为

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

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

第i个导航路标的像元p_i、像线l_i坐标可以表示为

$p_i=f\frac{c_{11}(x-x_i)+c_{12}(y-y_i)+c_{13}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

$l_i=f\frac{c_{21}(x-x_i)+c_{22}(y-y_i)+c_{23}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

其中x，y，z为探测器在目标天体坐标系下的三轴位置坐标，x_i，y_i，z_i为路标在目标天体坐标系下的三轴位置坐标，c_ba(a＝1，2，3；b＝1，2，3)为转换矩阵C_ba中相应元素，f为光学相机的焦距。设跟踪观测到的导航路标共有n个，则相应的观测量为

$\stackrel{\→}{y}=h(\stackrel{\→}{r},C_{\mathrm{ba}})=\left[\begin{array}{ccccc}{p}_1& l_1& ...& p_n& l_n\end{array}\right]$

设第i个和第j个路标观测视线所形成的观测角为A_ij，则

$\cos A_{\mathrm{ij}}=\frac{{\stackrel{\→}{r}}_i\·{\stackrel{\→}{r}}_j}{r_i{r}_j}=\frac{{\stackrel{\→}{r}}_i^b\·{\stackrel{\→}{r}}_j^b}{r_i{r}_j}$

上式中

为目标天体坐标系下第i个路标和第j个路标相对探测器的位置，r_i，r_j为第i个路标个第j个路标与探测器之间的距离。

该观测角可以利用光学图像中像素、像线坐标表示，即

$A_{\mathrm{ij}}=\arccos \left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right)$

上述几何描述也可用矢量公式表达，如利用

和

的内积关系，有

$(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i)\·(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_j)=|\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i||\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i|\cos A_{\mathrm{ij}}$

可见，上式为探测器位置

与测量夹角A_ij的关系式，而与探测器的姿态状态无关，因此，可以利用上式对探测器的位置状态进行单独求解。

考虑到上式为非线性方程，直接求解比较困难，下面在小偏差线性化假设的条件下，对其线性化量测方程进行推导，可以得到位置偏差量

与测量夹角偏差量δA_ij之间的近似线性关系：

$\mathrm{\δ}{A}_{\mathrm{ij}}={\stackrel{\→}{h}}_{\mathrm{ij}}\·\mathrm{\δ}\stackrel{\→}{r}$

其中，观测矩阵行向量

为

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j}$

和

为辅助向量，定义如下

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_j}{\sin A_{\mathrm{ij}}}$

和

分别为第i个与第j个路标的单位视线向量

${\stackrel{\→}{n}}_i=\frac{{\stackrel{\→}{r}}_i}{r_i}$ ${\stackrel{\→}{n}}_j=\frac{{\stackrel{\→}{r}}_j}{r_j}$

步骤3，利用步骤2得到的观测矩阵行向量值，计算任意三个导航路标组合所对应的矩阵(HH^T)^-1的特征值之和

观测任意三个导航路标(如第i个、第j个和第阶)组合，相应的观测角偏差与线性观测矩阵可以表示为

$\mathrm{\δ}\stackrel{\→}{A}=\left[\begin{array}{c}\mathrm{\δ}{A}_{\mathrm{ij}}\\ \mathrm{\δ}{A}_{\mathrm{jk}}\\ {\mathrm{\δA}}_{\mathrm{ik}}\end{array}\right]$ $H=\left[\begin{array}{c}{\stackrel{\→}{h}}_{\mathrm{ij}}^T\\ {\stackrel{\→}{h}}_{\mathrm{jk}}^T\\ {\stackrel{\→}{h}}_{\mathrm{ik}}^T\end{array}\right]$

则

$\mathrm{\δ}\stackrel{\→}{A}=\mathrm{H\δ}\stackrel{\→}{r}$

矩阵HH^T可以表示为

${\mathrm{HH}}^T=\left[\begin{array}{ccc}{h}_{\mathrm{ij}}^2& h_{\mathrm{ij}}{h}_{\mathrm{jk}}\cos {\mathrm{\θ}}_i& h_{\mathrm{ij}}{h}_{\mathrm{ik}}\cos {\mathrm{\θ}}_k\\ h_{\mathrm{jk}}{h}_{\mathrm{ij}}\cos {\mathrm{\θ}}_i& h_{\mathrm{jk}}^2& h_{\mathrm{jk}}{h}_{\mathrm{ik}}\cos {\mathrm{\θ}}_j\\ h_{\mathrm{ik}}{h}_{\mathrm{ij}}\cos {\mathrm{\θ}}_k& h_{\mathrm{ik}}{h}_{\mathrm{jk}}\cos {\mathrm{\θ}}_j& h_{\mathrm{ik}}^2\end{array}\right]$

其中θ_i，θ_j，θ_k分别为观测矩阵行向量之间形成的夹角，h_ij，h_jk，h_ik分别为向量的模。通过求HH^T的逆，进行整理可得矩阵(HH^T)^-1的特征值之和为

$\mathrm{\Σ}{\mathrm{\λ}}_{\mathrm{ijk}}=\frac{(\frac{{\sin }^2{\mathrm{\θ}}_i}{h_{\mathrm{ik}}^2}+\frac{{\sin }^2{\mathrm{\θ}}_j}{h_{\mathrm{ij}}^2}+\frac{{\sin }^2{\mathrm{\θ}}_k}{h_{\mathrm{jk}}^2})}{1-{\cos }^2{\mathrm{\θ}}_i-{\cos }^2{\mathrm{\θ}}_j-{\cos }^2{\mathrm{\θ}}_k+2\cos {\mathrm{\θ}}_i\cos {\mathrm{\θ}}_j\cos {\mathrm{\θ}}_k}---\left(1\right)$

注意到

$V^2=\frac{1}{36}{h}_{\mathrm{ij}}^2{h}_{\mathrm{jk}}^2{h}_{\mathrm{ik}}^2\×(1-{\cos }^2{\mathrm{\θ}}_i-{\cos }^2{\mathrm{\θ}}_j-{\cos }^2{\mathrm{\θ}}_k+2\cos {\mathrm{\θ}}_i\cos {\mathrm{\θ}}_j\cos {\mathrm{\θ}}_k)$

同时

$S_i^2=\frac{1}{4}{h}_{\mathrm{ij}}^2{h}_{\mathrm{jk}}^2{\sin }^2{\mathrm{\θ}}_i$

$S_j^2=\frac{1}{4}{h}_{\mathrm{jk}}^2{h}_{\mathrm{ik}}^2{\sin }^2{\mathrm{\θ}}_j$

$S_k^2=\frac{1}{4}{h}_{\mathrm{ij}}^2{h}_{\mathrm{ik}}^2{\sin }^2{\mathrm{\θ}}_k$

其中，V为以三个观测矩阵行向量

为棱的四面体的体积，S_i，S_j，S_k分别为以

两两为邻边组成的三角形面积。这样，∑λ_ijk可以表示为由观测矩阵行向量

为棱构成四面体的体积与侧面积的函数

$\mathrm{\Σ}{\mathrm{\λ}}_{\mathrm{ijk}}=\frac{1}{9V^2}(S_i^2+S_j^2+S_k^2)$

步骤4，比较步骤3得到的特征值之和∑λ_ijk，选取最优的三个导航路标比较步骤3得到的

个∑λ_ijk值，以其中最小∑λ_ijk值对应的第i个、第j个和第k个路标作为导航路标使用。

在探测器近天体飞行过程中，固定高度上探测器与目标天体表面各导航路标距离相差不大，因此在选取导航路标时主要考虑的因素是三个观测视线之间的夹角关系。由于目前所研制的光学导航相机视场角一般不大，因此为了获得较高的位置确定与姿态确定精度，所选取的路标点应尽量分散分布。在实际飞行中，可以按照保证观测矩阵构成的矩阵(HH^T)^-1的特征值之和∑λ最小的原则，选取其对应的三个导航路标为确定探测器位置、姿态精度最高的导航路标，即主要利用式(1)完成相应的路标选取工作，可见该路标选取方法利用解析表达式直接进行求解选取，因此具有解析可行、计算量小等优点。

Claims (1)

1.基于观测矩阵的深空探测器导航路标选取方法，其特征在于包含如下步骤：

步骤1，读取导航路标在目标天体坐标系下的位置数据，计算目标天体坐标系下n个路标相对探测器的位置矢量；

步骤2，为得到观测矩阵H，利用步骤1得到的目标天体坐标系下n个路标相对探测器的位置矢量，计算观测矩阵的行向量；

令 ${\stackrel{\→}{n}}_i={\stackrel{\→}{r}}_i/r_i$ 和 ${\stackrel{\→}{n}}_j={\stackrel{\→}{r}}_j/r_j$ 分别为第i个与第j个路标的单位视线向量，r_i和r_j为第i个与第j个路标到探测器的距离，对n个导航路标中第i个和第j个路标组合的观测矩阵的行向量

进行计算：

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j}(i,j=\mathrm{1,2}...n)$

其中

和为辅助向量，计算公式如下

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_j}{\sin A_{\mathrm{ij}}}$

其中，A_ij为探测器到第i个和第j个路标观测视线所形成的观测角；对于n个导航路标，任意选取两个路标，两两组合后得到的观测矩阵行向量共有

个值；

步骤3，利用步骤2得到的观测矩阵行向量，计算任意三个导航路标组合所对应的矩阵(HH^T)^-1的特征值之和；

利用步骤2得到的

个行向量，对n个导航路标中任意三个导航路标如第i个、第j个和第k个组合所对应的矩阵(HH^T)^-1的特征值之和∑λ_ijk进行计算

${\mathrm{\Σ\λ}}_{\mathrm{ijk}}=\frac{(\frac{{\sin }^2{\mathrm{\θ}}_{\mathrm{ij}}}{h_{\mathrm{ij}}^2}+\frac{{\sin }^2{\mathrm{\θ}}_{\mathrm{jk}}}{h_{\mathrm{jk}}^2}+\frac{{\sin }^2{\mathrm{\θ}}_{\mathrm{ki}}}{h_{\mathrm{ki}}^2})}{1-{\cos }^2{\mathrm{\θ}}_{\mathrm{ij}}-{\cos }^2{\mathrm{\θ}}_{\mathrm{jk}}-{\cos }^2{\mathrm{\θ}}_{\mathrm{ki}}+{2\cos \mathrm{\θ}}_{\mathrm{ij}}\cos {\mathrm{\θ}}_{\mathrm{jk}}\cos {\mathrm{\θ}}_{\mathrm{ki}}}(i,j,k=\mathrm{1,2}...n)$

其中θ_ij，θ_jk，θ_ki分别为观测矩阵行向量

之间形成的夹角；

若令 $m=\frac{1}{2}n(n-1),$ 则对于n个导航路标，这样任意选取三个路标组合后得到的∑λ_ijk共有

个值；

步骤4，比较步骤3得到的特征值之和，选取最优的三个导航路标；

比较步骤3得到的

个∑λ_ijk值，以其中最小∑λ_ijk值对应的任意三个路标作为导航路标使用。

Images