基于特征奇异值的航天器相对姿态测量矢量选取方法

摘要

本发明公开的基于特征奇异值的航天器相对姿态测量矢量选取方法，涉及航天器相对姿态测量中的测量矢量选取方法，属于自主导航领域。本发明通过测量仪器获取p个被测平面上点的位置矢量，得到对应测量矢量；计算任意三个不共面的测量矢量对应的矩阵DTD的特征值之和；比较任意三个不共面的测量矢量对应矩阵DTD的特征值之和，选取其中最小的特征值之和对应的三个测量矢量作为最优测量矢量；利用最小二乘法对三个最优位置矢量拟合得到测量误差最小的被测平面法向量，进而得到测量精度较高的航天器相对姿态。本发明能够实现在测量得到的全部测量矢量中找到使得航天器相对姿态测量误差最小的三个测量矢量，提高三点测量法的测量精度，即提高平面法向量或者相对姿态的测量精度。

说明书

技术领域

本发明涉及一种基于特征奇异值的航天器相对姿态测量矢量选取方法，尤其适用于在利用三点相对位置矢量的航天器相对姿态测量中的测量矢量选取方法，属于自主导航领域。

背景技术

对航天目标的临近飞行是未来航天活动最复杂的任务之一，如航天器的交会对接以及小天体的绕飞着陆等。由于这些飞行活动具有瞬时性，采用传统的基于地面遥控的导航、控制模式已无法满足实现高精度操作的需要，这要求航天器必须具有自主导航功能。其中，航天器相对目标的姿态确定是自主导航的主要任务之一，是当前的一个重要研究问题。

目前，相对姿态测量的方式主要有两种，一种是利用计算机视觉的光学成像方法，在这类方法中，特征像点的定位匹配误差是引起姿态误差的主要原因；另一种是通过测量目标上的点的距离的矢量方法，例如，由激光雷达(LIDAR)生成的点云数据已被用来确定临近目标体的相对姿态[1](参见Fenton R C,Fullmer R R,Pack R T.Design of lidar-based sensors and algorithms for determining the relative motion of an impaired spacecraft[J].Proc Spie,2005,5778:809-818.)。实际上，在点云数据中任意选择三个不共线的点就能拟合出目标的某一平面，进而得到该平面对于航天器的两姿态角的相对姿态，在实际测量中存在误差，所以如何选取三个点来拟合平面以提高相对姿态测量精度是需要解决的技术问题。

发明内容

利用三点相对位置矢量和最小二乘法，能够得到平面法向量在航天器某固连坐标系中的表达，进一步得到某一平面对于航天器的相对姿态，完成航天器相对姿态测量。针对上述的技术问题，本发明公开的基于特征奇异值的航天器相对姿态测量矢量选取方法要解决的技术问题是，实现在测量仪器测量得到的全部测量矢量中找到使得航天器相对姿态测量误差最小的三个测量矢量，提高三点测量法的测量精度，即提高平面法向量或者相对姿态的测量精度。所述的三点测量法指上述的利用三点相对位置矢量和最小二乘法得到平面法向量的方法。

本发明公开的基于特征奇异值的航天器相对姿态测量矢量选取方法，首先，通过测量仪器获取p个被测平面上的点的位置矢量，得到对应的测量矢量。其次，计算任意三个不共面的测量矢量对应的矩阵D^TD的特征值之和。然后，比较任意三个不共面的测量矢量对应的矩阵D^TD的特征值之和，选取其中的最小的特征值之和对应的三个测量矢量作为最优测量矢量，进而得到三点相对位置矢量拟合平面的三个最优位置矢量。最后，利用上述得到的三个最优位置矢量，利用最小二乘法，拟合得到测量误差最小的被测平面法向量，进而得到测量精度较高的航天器相对姿态。

本发明公开的基于特征奇异值的航天器相对姿态测量矢量选取方法，包括如下步骤：

步骤1，通过测量仪器获取p个被测平面上的点的位置矢量r，得到对应的测量矢量m。

航天器上的测量仪器优选激光雷达，向被测平面同时发射大量测量束，获取平面大量点的位置矢量r_a＝[x_a>a>a]^T(a＝1,2,3,...,p)，假设一共得到p个位置矢量，a为任意1到p之间的编号，r_a的坐标在测量仪器固连坐标系∑^r下表示，是∑^r的基底向量；对于每一个测量束，定义测量仪器的测量矢量为m_a＝[r_a>a>a]^T(a＝1,2,3,...,p)，三个分量分别表示位置矢量的长度、俯仰角和方位角；定义测量均方差为dm＝[dr>T，其中dr、dθ和dφ分别是测量矢量的三个分量的均方差，测量均方差已知。

步骤2，利用步骤1得到的p个测量矢量，计算任意三个不共面的测量矢量对应的矩阵D的奇异值平方和，即矩阵D^TD的特征值之和∑λ。

任意三个不共面的位置矢量r都能通过最小二乘法拟合得到被测平面法向量，任选三个不共面的位置矢量r，对应三个测量矢量m，分别对应的编号为i、j、k。定义被测平面法向量为n，dn为被测平面法向量为n的误差，dn的公式如公式(1)所示：

$\begin{array}{c}dn=-{\left(\begin{array}{c}{r_i}^T\\ {r_j}^T\\ {r_k}^T\end{array}\right)}^{-1}\left(\begin{array}{c}{{\mathrm{dr}}_i}^{T}n\\ {{\mathrm{dr}}_j}^{T}n\\ {{\mathrm{dr}}_k}^{T}n\end{array}\right)\\ =-{\left(\begin{array}{c}{r_i}^T\\ {r_j}^T\\ {r_k}^T\end{array}\right)}^{-1}\left(\begin{array}{c}\left[\begin{array}{ccc}dr& d\theta & d\phi \end{array}\right]\left(\begin{array}{ccc}1& 0& 0\\ 0& r_i& 0\\ 0& 0& r_i{\mathrm{sin\theta }}_i\end{array}\right)\left(\begin{array}{c}{{\hat{r}}_i}^T\\ {{\hat{\theta }}_i}^T\\ {{\hat{\phi }}_i}^T\end{array}\right)\\ \left[\begin{array}{ccc}dr& d\theta & d\phi \end{array}\right]\left(\begin{array}{ccc}1& 0& 0\\ 0& r_j& 0\\ 0& 0& r_j{\mathrm{sin\theta }}_j\end{array}\right)\left(\begin{array}{c}{{\hat{r}}_j}^T\\ {{\hat{\theta }}_j}^T\\ {{\hat{\phi }}_j}^T\end{array}\right)\\ \left[\begin{array}{ccc}dr& d\theta & d\phi \end{array}\right]\left(\begin{array}{ccc}1& 0& 0\\ 0& r_k& 0\\ 0& 0& r_k{\mathrm{sin\theta }}_k\end{array}\right)\left(\begin{array}{c}{{\hat{r}}_k}^T\\ {{\hat{\theta }}_k}^T\\ {{\hat{\phi }}_k}^T\end{array}\right)\end{array}\right)n\\ =-{\left(\begin{array}{c}{r_i}^T\\ {r_j}^T\\ {r_k}^T\end{array}\right)}^{-1}\left(\begin{array}{ccc}\left(\begin{array}{ccc}dr& r_{i}d\theta & r_i{\mathrm{sin\theta }}_{i}d\phi \end{array}\right)& 0^{1\times 3}& 0^{1\times 3}\\ 0^{1\times 3}& \left(\begin{array}{ccc}dr& r_{j}d\theta & r_j{\mathrm{sin\theta }}_{j}d\phi \end{array}\right)& 0^{1\times 3}\\ 0^{1\times 3}& 0^{1\times 3}& \left(\begin{array}{ccc}dr& r_{k}d\theta & r_k{\mathrm{sin\theta }}_{k}d\phi \end{array}\right)\end{array}\right)\left(\begin{array}{c}\left(\begin{array}{c}{{\hat{r}}_i}^T\\ {{\hat{\theta }}_i}^T\\ {{\hat{\phi }}_i}^T\end{array}\right)\\ \left(\begin{array}{c}{{\hat{r}}_j}^T\\ {{\hat{\theta }}_j}^T\\ {{\hat{\phi }}_j}^T\end{array}\right)\\ \left(\begin{array}{c}{{\hat{r}}_k}^T\\ {{\hat{\theta }}_k}^T\\ {{\hat{\phi }}_k}^T\end{array}\right)\end{array}\right)n\\ =-{\left(\begin{array}{c}{r_i}^T\\ {r_j}^T\\ {r_k}^T\end{array}\right)}^{-1}\left(\begin{array}{ccc}{{\mathrm{dr}}_{is}}^T& 0^{1\times 3}& 0^{1\times 3}\\ 0^{1\times 3}& {{\mathrm{dr}}_{js}}^T& 0^{1\times 3}\\ 0^{1\times 3}& 0^{1\times 3}& {{\mathrm{dr}}_{ks}}^T\end{array}\right)Pn\\ =Dpn\end{array}---\left(1\right)$

其中，dr_a＝[dx_a>a>a]^T(a＝1,2,3,...,p)为位置向量误差；在每一个位置向量r的末端都建立一个球坐标系∑^sa(a＝1,2,3,...,p)，分别是球坐标系∑^sa的基底向量；dr_as(a＝1,2,3,...,p)为dr_a在相应的球坐标系下的坐标；D、P的定义如公式(1)所示。

在公式(1)中，被测平面法向量误差dn的大小是由D的奇异值或者D^TD的特征值决定的，因此，需计算D^TD的特征值之和∑λ，如公式(2)所示：

$\begin{array}{c}\Sigma {\lambda }_{ijk}=\frac{(\frac{{\left(dr\right)}^2}{{r_i}^2}+{\left(d\theta \right)}^2+{\sin }^2{\theta }_i{\left(d\phi \right)}^2)(1-{\cos }^2{\theta }_{jk})}{1-{\cos }^2{\theta }_{ij}-{\cos }^2{\theta }_{jk}-{\cos }^2{\theta }_{ik}+2{\mathrm{cos\theta }}_{ij}{\mathrm{cos\theta }}_{jk}{\mathrm{cos\theta }}_{ik}}\\ +\frac{(\frac{{\left(dr\right)}^2}{{r_j}^2}+{\left(d\theta \right)}^2+{\sin }^2{\theta }_j{\left(d\phi \right)}^2)(1-{\cos }^2{\theta }_{ik})}{1-{\cos }^2{\theta }_{ij}-{\cos }^2{\theta }_{jk}-{\cos }^2{\theta }_{ik}+2{\mathrm{cos\theta }}_{ij}{\mathrm{cos\theta }}_{jk}{\mathrm{cos\theta }}_{ik}}\\ +\frac{(\frac{{\left(dr\right)}^2}{{r_k}^2}+{\left(d\theta \right)}^2+{\sin }^2{\theta }_k{\left(d\phi \right)}^2)(1-{\cos }^2{\theta }_{ij})}{1-{\cos }^2{\theta }_{ij}-{\cos }^2{\theta }_{jk}-{\cos }^2{\theta }_{ik}+2{\mathrm{cos\theta }}_{ij}{\mathrm{cos\theta }}_{jk}{\mathrm{cos\theta }}_{ik}}\\ (i,j,k=1,2,3,\mathrm{...},p)\end{array}---\left(2\right)$

其中θ_ab(a,b＝1,2,3,...,p)为r_a与r_b之间的夹角。对于p个测量矢量能够得到个特征值之和∑λ。

步骤3，比较步骤2得到的特征值之和∑λ，选取其中的最小的特征值之和∑λ对应的三个测量矢量作为最优测量矢量。

比较步骤2中的个特征值之和Σλ，以其中的最小的特征值之和Σλ对应的三个测量矢量作为最优测量矢量，对应的三个位置矢量即为三点相对位置矢量拟合平面的最优位置矢量，编号分别定义为opti1、opti2、opti3。

步骤4，利用步骤3得到的三个最优位置矢量，利用最小二乘法，拟合得到测量误差dn最小的被测平面法向量n，进而得到测量精度较高的航天器相对姿态。

利用步骤3得到的三个最优位置矢量，利用最小二乘法，拟合得到测量精度较高的被测平面法向量n，其测量误差dn最小，被测平面法向量n的拟合公式如公式(3)所示：

$n={\left(\begin{array}{c}{r_{opti1}}^T\\ {r_{opti2}}^T\\ {r_{opti3}}^T\end{array}\right)}^{-1}\left(\begin{array}{c}1\\ 1\\ 1\end{array}\right)---\left(3\right)$

被测平面法向量n可以转化为被测平面对于航天器的两个相对姿态角，即得到测量精度较高的航天器相对姿态，所述的俯仰角和方位角如公式(4)所示：

$\left(\begin{array}{c}{\theta }_{rp}=\arccos \left(\frac{n·\hat{z}}{n}\right)\\ {\phi }_{rp}=\arccos \left(\frac{n_{xy}·\hat{x}}{n_{xy}}\right)\end{array}\right)---\left(4\right)$

其中θ_rp为被测平面对于航天器的相对姿态的俯仰角，φ_rp为被测平面对于航天器的相对姿态的方位角，n_xy为当平面法向量n的z轴分量为零时的值，n_xy、n分别为n_xy和n的模长。

有益效果：

本发明公开的基于特征奇异值的航天器相对姿态测量矢量选取方法，利用基于特征奇异值的误差分析手段，能够快速有效地筛选出最优的三个测量矢量，利用最优的三个测量矢量对应的三个位置矢量拟合得到的平面法向量具有最小的测量误差，能够实现平面法向量以及被测平面对于航天器的相对姿态的精确测量。

附图说明

图1为本发明公开的基于特征奇异值的航天器相对姿态测量矢量选取方法的流程图；

图2为航天器测量平面上的点的距离，得到平面上的点的相对位置矢量的示意图；

图3为测量仪器坐标系和球坐标系的关系图；

图4为将被测平面法向量转化为航天器的相对姿态角的示意图。

具体实施方式

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

为了验证方法的可行性，考虑航天器对某一目标平面进行测量。假设航天器距离被测平面100米，是被测平面的标称方程，得到该平面法向量标称值为航天器上携带的测量仪器为一种激光测距仪，其测距均方差为dr＝0.05m，测角均方差为dθ＝dφ＝0.125°。

本实施例公开的基于特征奇异值的航天器相对姿态测量矢量选取方法，具体步骤如下：

步骤1，通过测量仪器获取5个被测平面上的点的位置矢量r，得到对应的测量矢量m。

如图2所示，航天器上的激光雷达，向被测平面同时发射测量束，获取平面上若干点的位置矢量。为方便起见，假设一共得到5个位置矢量，表示为r_a＝[x_a>a>a]^T(a＝1,2,3,4,5)，对应的激光测距仪的测量矢量为m_a＝[r_a>a>a]^T(a＝1,2,3,4,5)。激光雷达坐标系和球坐标系的关系如图3所示。测量矢量m的标称值如表1所示：

表1各测量矢量m的数值

测量矢量m的序号r/米θ/弧度φ/弧度1141.4211.7410.7852150.0001.7311.1303141.4211.1501.6714144.5680.269-0.1125141.4211.150-0.101

步骤2，利用步骤1得到的5个测量矢量，计算任意三个测量矢量对应的矩阵D的奇异值平方和，即矩阵D^TD的特征值之和∑λ。

在本实施例中，任意三个测量矢量或者位置矢量都不共面。对于任意三个测量矢量或者位置矢量，计算相应的D^TD的特征值之和Σλ，如公式(2)所示，计算的结果如表3所示。

步骤3，比较步骤2得到的特征值之和Σλ，选取其中的最小的特征值之和Σλ对应的三个测量矢量作为最优测量矢量。

比较步骤2中的10个特征值之和∑λ，其中的最小的特征值之和∑λ为∑λ₁₃₄，所以，m₁、m₃和m₄为最优测量矢量，r₁、r₃和r₄为三点相对位置矢量拟合平面的最优位置矢量。

步骤4，利用步骤3得到的三个最优位置矢量，利用最小二乘法，拟合得到测量误差dn最小的被测平面法向量n，进而得到测量精度较高的航天器相对姿态。

被测平面法向量n的拟合公式如公式(3)所示。

考虑到测量误差的随机性，本实施例对每个测量矢量都进行若干次测量，每次测量都加入随机误差，本实施例取测量次数为10000次。然后，计算每三个测量矢量拟合出的被测平面法向量相对于被测平面法向量的标称值的差的模长的方差，用以比较每三个测量矢量的拟合精度与对应的矩阵D^TD的特征值之和∑λ的关系。得到结果如表3所示，其中各组按特征值之和∑λ的从小到大的顺序排列。

表3特征值之和∑λ与拟合方差

编号组合特征值之和∑λ拟合方差1、3、43.343e-51.855e-62、4、53.769e-52.092e-63、4、53.825e-51.998e-61、3、54.557e-52.758e-62、3、55.016e-53.149e-61、4、55.330e-53.002e-62、3、45.751e-53.465e-61、2、41.839e-41.229e-51、2、34.633e-43.150e-51、2、56.154e-44.193e-5

由表3可见，特征值之和∑λ最小的组合为测量矢量m₁、m₃和m₄或者位置矢量r₁、r₃和r₄的组合，它们具有最小的拟合被测平面的拟合方差，在概率意义下，拟合得到的被测平面误差最小。而其它组合的拟合方差的从小到大的顺序与它们的特征值之和∑λ的从小到大的顺序基本一致。当两个组合的特征值之和∑λ十分接近时，它们的拟合方差也十分接近，会出现顺序的异常，但这并不影响拟合方差与特征值之和Σλ的正相关的关系。

在实际应用中，航天器对被测平面只进行一次测量，对最优测量矢量m₁、m₃和m₄叠加误差后获得的一次测量值为m₁＝[141.423>T、m₃＝[141.351>T和m₄＝[144.611>T，拟合得被测平面法向量(已单位化)为n＝[0.57652>T。被测平面法向量n可以转化为被测平面对于航天器的两个相对姿态角，即得到测量精度较高的航天器相对姿态，所述的俯仰角和方位角如公式(4)所示。计算得，θ_rp＝54.861°，φ_rp＝44.990°。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。