CN102620656B - 一种航天器交会对接相对位姿测量方法 - Google Patents

Abstract

本发明公开了一种航天器交会对接相对位姿测量方法，该方法将双目视觉测量方法与普吕克直线方程相结合来进行航天器交会对接相对位姿的测量；首先采用双目视觉算法计算得到目标航天器中两条非共面直线在追踪航天器下的坐标值，然后根据普吕克直线方程得到这两条直线在两个坐标系下的相对位姿关系，最后通过采用奇异值分解的方法解算出两个航天器的相对位姿信息，本发明由于采用普吕克直线方程和双目视觉测量方法相结合对目标航天器上的两条异面直线进行测量来确定两个航天器相对位置和姿态的方法，可以弥补传统的航天器交会对接测量算法的缺陷，进一步提高算法的精度。

Description

一种航天器交会对接相对位姿测量方法

技术领域

本发明涉及机器视觉、计算方法、数学、数值方法领域，特别涉及一种基于普吕克直线方程的航天器交会对接相对位姿测量方法，适用于航天器交会对接时相对位姿的测量。

背景技术

航天器交会对接技术是指宇宙飞船、航天飞机这两类航天器在轨会合并在结构上连成一个整体的技术。美国20世纪60年代初的双子星座计划主要是依靠航天员的视觉观测来确定航天器之间的相对位置和姿态，俄罗斯的交会对接测量系统主要采用的是微波雷达技术，欧空局从20世纪80年代初期就开始研究交会对接测量技术及敏感器，2011年11月3日凌晨，我国神舟八号飞船与天宫一号实现交会对接，中国载人航天首次空间交会对接试验获得成功。2012年4月，我国的天宫一号与神舟九号载人交会对接任务已经进入全面实施阶段。

在航天器交会对接中，基于双目视觉的方法主要是通过射影几何、齐次坐标等数学工具描述图像的成像原理。传统的方法是在追踪航天器上安装两个CCD传感器，通过对特征点在CCD上成像的分析和计算就可以确定追踪航天器和目标航天器之间的相对位置和姿态信息，本发明选取目标航天器上的两条非共面直线在CCD上的成像分析来计算两个航天器相对位姿的方法则更加简便。在航天器交会对接中，确定六个自由度的相对位姿信息是一个非常重要的问题。经典的卫星姿态描述方法有欧拉角法、四元数法等。欧拉角是由坐标系经过三次旋转得到的三参数描述方法；四参数的四元数通过绕旋转轴一次旋转得到，可以避免欧拉角在大角度时“奇异”以及复杂的三角函数运算，但是这种方法是将位置和姿态分开来测量的，普吕克直线的方法则可以使航天器的位置和姿态统一起来测量。

发明内容

本发明要解决的技术问题是：针对目前航天器交会对接时位置和姿态分开测量的不足，本发明提供一种能够兼顾位置和姿态的测量，得到更精确测量结果的双目立体视觉的测量方法。

为解决上述技术问题，本发明一种航天器交会对接相对位姿测量方法，包括以下步骤：

步骤1、采用双目视觉定位算法确定目标航天器中任意两条非共面直线在追踪航天器坐标系中的坐标值；

步骤2、采用普吕克直线法表示上述两条非共面直线在目标航天器坐标系下的坐标值；

步骤3、采用普吕克直线法表示步骤1中获得的坐标值；

步骤4、根据步骤2和步骤3获得普吕克直线法表示的坐标值，采用普吕克直线方程确定这两条直线在目标航天器和追踪航天器坐标系下的相对位姿关系；

步骤5、根据步骤4获得的上述两条非共面直线在目标航天器和追踪航天器坐标系下的相对位姿关系，采用奇异值分解的方法获得单位对偶四元数表示目标航天器和追踪航天器坐标系间的相对位姿，从而得到两个航天器间的相对位置和姿态信息。

进一步地，本发明航天器交会对接相对位姿测量方法中，所述步骤1中，采用双目视觉定位法确定目标航天器中任意两条非共面直线在追踪航天器坐标系中的坐标值具体如下：由追踪航天器两个摄像机C₁与C₂观察到的目标航天器上任意两条非共面直线和在摄像机C₁图像坐标下的值分别为在摄像机C₂图像坐标下的值分别为则直线为由摄像机C₁坐标系原点O₁与组成的平面S₁和由摄像机C₂坐标系原点O₂与组成的平面S₂的交线，直线为由摄像机C₁坐标系原点O₁与组成的平面S₁′和由摄像机C₂坐标系原点O₂与组成的平面S₂′的交线，则在追踪航天器坐标系下的坐标值可由下面两个公式联立得到：

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{O1}^T{M}_1{\stackrel{\→}{l}}_{C1}=0\\ {\stackrel{\→}{l}}_{O2}^T{M}_2{\stackrel{\→}{l}}_{C1}=0\end{array}\right.$

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{O1\′}^T{M}_1{\stackrel{\→}{l}}_{C2}=0\\ {\stackrel{\→}{l}}_{O2\′}^T{M}_2{\stackrel{\→}{l}}_{C2}=0\end{array}\right.$

其中，M₁和M₂分别为追踪航天器两个摄像机的投影矩阵，为目标航天器两条直线在追踪航天器坐标系下的坐标值。

进一步地，本发明航天器交会对接相对位姿测量方法中，所述步骤2中，采用普吕克直线表示上述两条非共面直线在目标航天器坐标系下的坐标值的方法如下：

$\left\{\begin{array}{c}{\hat{l}}_1=a+{{\mathrm{\ϵl}}_1}^{\′}\\ {\hat{l}}_2=b+{{\mathrm{\ϵl}}_2}^{\′}\end{array}\right.$

其中，l₁′＝P₁×a；l₂′＝P₂×b，P₁和P₂分别为直线和任意一点，为的模值，a为的单位向量，l₁′为单位向量a的矩，为的模值，b为的单位向量，l₂′为单位向量b的矩。

进一步地，本发明航天器交会对接相对位姿测量方法中，所述步骤3中，采用普吕克直线表示两条非共面直线在追踪航天器摄像机坐标系下的坐标值的方法如下：

$\left\{\begin{array}{c}{\hat{l}}_{C1}=a^{\′}+{{\mathrm{\ϵl}}_{C1}}^{\′}\\ {\hat{l}}_{C2}=b^{\′}+{{\mathrm{\ϵl}}_{C2}}^{\′}\end{array}\right.$

其中，l_C1′＝P₁′×a′l_C2′＝P₂′×b′P₁′和P₂′分别为P₁和P₂在追踪航天器摄像机坐标系下对应的点；为的模值，a′为的单位向量，l_C1′为单位向量a′的矩；为的模值，b′为的单位向量，l_C2′为单位向量b′的矩。

进一步地，本发明航天器交会对接相对位姿测量方法中，所述步骤4中，采用普吕克直线方程确定这两条直线在目标航天器和追踪航天器坐标系下的相对位姿关系的方法具体如下：跟踪航天器和目标航天器坐标系之间的旋转和平移可用普吕克直线方程来表示，根据Plüc ker直线满足

其中，表示单位对偶四元数，表示单位对偶四元数的共轭。

进一步地，本发明航天器交会对接相对位姿测量方法中，所述步骤5中，采用奇异值分解的方法获得单位对偶四元数来表示目标航天器和追踪航天器坐标系间的相对位姿，公式如下：

${\left(\begin{array}{cc}{S}_1^T& S_2^T\end{array}\right)}^T{\left(\begin{array}{c}q\\ q^{\′}\end{array}\right)}_{8\×1}=0$

其中

$S_1=\left(\begin{array}{cccc}{a}^{\′}-a& {[a^{\′}+a]}_{\×}& 0_{3\×1}& 0_{3\×3}\\ {l_{C1}}^{\′}-{l_1}^{\′}& {[{l_{C1}}^{\′}+{l_1}^{\′}]}_{\×}& a^{\′}-a& {[a^{\′}+a]}_{\×}\end{array}\right)$

$S_2=\left(\begin{array}{cccc}{b}^{\′}-b& {[b^{\′}+b]}_{\×}& 0_{3\×1}& 0_{3\×3}\\ {l_{C2}}^{\′}-{l_2}^{\′}& {[{l_{C2}}^{\′}+{l_2}^{\′}]}_{\×}& b^{\′}-b& {[b^{\′}+b]}_{\×}\end{array}\right)$

其中，a′和a为单位向量且a′⊥l_C1′，a⊥l₁′，[a′+a]_×称为a′+a的反对称矩阵，设

$W={\left(\begin{array}{cc}{S}_1^T& S_2^T\end{array}\right)}^T$

W为S₁和S₂两个矩阵组合的转置矩阵，对W作奇异值分解，即W＝UDV^T，即可确定单位对偶四元数的值即可测量出两个航天器间的相对位姿信息。

本发明与现有技术相比，具有以下显著的优点和进步：

(1)本发明由于采用双目立体视觉，能够高精度的测量目标相对位置和姿态参数；

(2)本发明由于选择目标航天器的两条非共面直线作为参考直线来测量相对位姿信息，克服了传统方法中选择特征点作为参考点来测量相对位姿的计算复杂性问题；

(3)本发明由于采用普吕克直线方程和双目视觉测量方法相结合对目标航天器上的两条异面直线进行测量来确定两个航天器相对位置和姿态的方法，可以弥补传统的航天器交会对接测量算法的缺陷，进一步提高算法的精度，同时可以使两个航天器交会对接时的位置和姿态信息统一描述，使算法模拟的刚体运动更加接近于实际运载体的运动，从而得到更精确的结果。

(4)本发明由于采用奇异值分解的方法来计算相对位姿，使得算法更加简便。

附图说明

图1为本发明的一种航天器交会对接相对位姿测量方法的原理框图。

图2为双目视觉定位的原理示意图。

具体实施方式

如图1、图2所示，本发明一种航天器交会对接相对位姿测量方法，包括以下步骤：

步骤1、采用双目视觉定位算法确定目标航天器中任意两条非共面直线在追踪航天器坐标系中的坐标值具体如下：由追踪航天器两个摄像机C₁与C₂观察到的目标航天器上任意两条非共面直线和在摄像机C₁图像坐标下的值分别为在摄像机C₂图像坐标下的值分别为则直线为由摄像机C₁坐标系原点O₁与组成的平面S₁和由摄像机C₂坐标系原点O₂与组成的平面S₂的交线，直线为由摄像机C₁坐标系原点O₂与组成的平面S₁′和由摄像机C₂坐标系原点O₂与组成的平面S₂′的交线，则在追踪航天器坐标系下的坐标值可由下面两个公式联立得到：

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{O1}^T{M}_1{\stackrel{\→}{l}}_{C1}=0\\ {\stackrel{\→}{l}}_{O2}^T{M}_2{\stackrel{\→}{l}}_{C1}=0\end{array}\right.---\left(1\right)$

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{O1\′}^T{M}_1{\stackrel{\→}{l}}_{C2}=0\\ {\stackrel{\→}{l}}_{O2\′}^T{M}_2{\stackrel{\→}{l}}_{C2}=0\end{array}\right.---\left(\text{2}\right)$

其中，M₁和M₂分别为追踪航天器两个摄像机的投影矩阵，为目标航天器两条直线在追踪航天器坐标系下的坐标值

摄像机投影矩阵M₁和M₂分别为：

$M_1=\left[\begin{array}{cccc}{m}_{11}^1& m_{12}^1& m_{13}^1& m_{14}^1\\ m_{21}^1& m_{22}^1& m_{23}^1& m_{24}^1\\ m_{31}^1& m_{32}^1& m_{33}^1& m_{34}^1\end{array}\right]---\left(3\right)$

$M_2=\left[\begin{array}{cccc}{m}_{11}^2& m_{12}^2& m_{13}^2& m_{14}^2\\ m_{21}^2& m_{22}^2& m_{23}^2& m_{24}^2\\ m_{31}^2& m_{32}^2& m_{33}^2& m_{34}^2\end{array}\right]---\left(4\right)$

在已知直线与与的值与投影矩阵M₁与M₂后，不需要解方程，就可以得到和在追踪航天器坐标系下的坐标值

步骤2、采用普吕克直线法表示上述两条非共面直线在目标航天器坐标系下的坐标值，具体如下：

$\left\{\begin{array}{c}{\hat{l}}_1=a+{{\mathrm{\ϵl}}_1}^{\′}\\ {\hat{l}}_2=b+{{\mathrm{\ϵl}}_2}^{\′}\end{array}\right.---\left(5\right)$

其中，l₁′＝P₁×a；l₂′＝P₂×b，P₁和P₂分别为直线和任意一点，为的模值，a为的单位向量，l₁′为单位向量a的矩，为的模值，b为的单位向量，l₂′为单位向量b的矩；

步骤3、采用普吕克直线法表示步骤1中获得的坐标值具体为：

$\left\{\begin{array}{c}{\hat{l}}_{C1}=a^{\′}+{{\mathrm{\ϵl}}_{C1}}^{\′}\\ {\hat{l}}_{C2}=b^{\′}+{{\mathrm{\ϵl}}_{C2}}^{\′}\end{array}\right.---\left(6\right)$

其中，l_C1′＝P₁′×a′l_C2′＝P₂′×b′P₁′和P₂′分别为P₁和P₂在追踪航天器摄像机坐标系下对应的点；为的模值，a′为的单位向量，l_C1′为单位向量a′的矩；为的模值，b′为的单位向量，l_C2′为单位向量b′的矩；

步骤4、根据步骤2和步骤3获得普吕克直线法表示的坐标值，采用普吕克直线方程确定这两条直线在目标航天器和追踪航天器坐标系下的相对位姿关系，具体如下：跟踪航天器和目标航天器坐标系之间的旋转和平移可用普吕克直线方程来表示，根据Plücker直线满足

其中，表示单位对偶四元数，表示单位对偶四元数的共轭；q^Tq＝1 q^Tq′＝0进一步得以得到：

$\left\{\begin{array}{c}{a}^{\′}+{{\mathrm{\ϵl}}_{C1}}^{\′}=(q+{\mathrm{\ϵq}}^{\′})(a+{{\mathrm{\ϵl}}_1}^{\′})(q^{*}+{\mathrm{\ϵq}}^{\′*})\\ b^{\′}+{{\mathrm{\ϵl}}_{C2}}^{\′}=(q+{\mathrm{\ϵq}}^{\′})(b+{{\mathrm{\ϵl}}_2}^{\′})(q^{*}+{\mathrm{\ϵq}}^{\′*})\end{array}\right.---\left(8\right);$

步骤5、根据步骤4获得的上述两条非共面直线在目标航天器和追踪航天器坐标系下的相对位姿关系，采用奇异值分解的方法获得单位对偶四元数表示目标航天器和追踪航天器坐标系间的相对位姿，从而得到两个航天器间的相对位置和姿态信息，具体如下：将上述方程分解为标量部分和对偶部分，可得

$\left\{\begin{array}{c}{a}^{\′}={\mathrm{qaq}}^{*}\\ {l_{C1}}^{\′}={\mathrm{qaq}}^{*\′}+{{\mathrm{ql}}_1}^{\′}{q}^{*}+q^{\′}a{q}^{*}\end{array}\right.---\left(9\right)$

$\left\{\begin{array}{c}{b}^{\′}={\mathrm{qbq}}^{*}\\ {l_{C2}}^{\′}={\mathrm{qbq}}^{*\′}+{{\mathrm{ql}}_2}^{\′}{q}^{*}+q^{\′}b{q}^{*}\end{array}\right.---\left(10\right)$

其中，q表示单位四元数，q^*表示单位四元数的共轭。两式右边同时乘以q并利用公式q^*q′+q^*′q＝0可得

$\left\{\begin{array}{c}{a}^{\′}q-\mathrm{qa}=0\\ ({l_{C1}}^{\′}q-{{\mathrm{ql}}_1}^{\′})+(a^{\′}{q}^{\′}-q^{\′}a)=0\end{array}\right.---\left(11\right)$

$\left\{\begin{array}{c}{b}^{\′}q-\mathrm{qb}=0\\ ({l_{C2}}^{\′}q-{{\mathrm{ql}}_2}^{\′})+(b^{\′}{q}^{\′}-q^{\′}b)=0\end{array}\right.---\left(12\right)$

以上两个方程写成矩阵向量为

${\left(\begin{array}{cc}{S}_1^T& S_2^T\end{array}\right)}^T{\left(\begin{array}{c}q\\ q^{\′}\end{array}\right)}_{8\×1}---\left(13\right)$

其中

$S_1=\left(\begin{array}{cccc}{a}^{\′}-a& {[a^{\′}+a]}_{\×}& 0_{3\×1}& 0_{3\×3}\\ {l_{C1}}^{\′}-{l_1}^{\′}& {[{l_{C1}}^{\′}+{l_1}^{\′}]}_{\×}& a^{\′}-a& {[a^{\′}+a]}_{\×}\end{array}\right)$

$S_2=\left(\begin{array}{cccc}{b}^{\′}-b& {[b^{\′}+b]}_{\×}& 0_{3\×1}& 0_{3\×3}\\ {l_{C2}}^{\′}-{l_2}^{\′}& {[{l_{C2}}^{\′}+{l_2}^{\′}]}_{\×}& b^{\′}-b& {[b^{\′}+b]}_{\×}\end{array}\right)$

其中，a′和a为单位向量且a′⊥l_C1′，a⊥l₁′，[a′+a]_×称为a′+a的反对称矩阵，设

$W={\left(\begin{array}{cc}{S}_1^T& S_2^T\end{array}\right)}^T---\left(14\right)$

对W作奇异值分解，即W＝UDV^T，即可确定单位对偶四元数的值即可测量出两个航天器间的相对位姿信息。其中D为对角矩阵，如果秩为6，设和为两零奇异值所对应的向量，则 则q和q′的值为和的线性组合，即

$\left(\begin{array}{c}q\\ q^{\′}\end{array}\right)={\mathrm{\λ}}_1\left(\begin{array}{c}{\stackrel{\→}{u}}_1\\ {\stackrel{\→}{v}}_1\end{array}\right)+{\mathrm{\λ}}_2\left(\begin{array}{c}{\stackrel{\→}{u}}_2\\ {\stackrel{\→}{v}}_2\end{array}\right)---\left(15\right)$

参数λ₁和λ₂由式(15)确定，即

${\mathrm{\λ}}_1^2{\stackrel{\→}{u}}_1^T{\stackrel{\→}{u}}_1+{2\mathrm{\λ}}_1{\mathrm{\λ}}_2{\stackrel{\→}{u}}_1^T{\stackrel{\→}{u}}_2+{\mathrm{\λ}}_2^2{\stackrel{\→}{u}}_2^T{\stackrel{\→}{u}}_2=1---\left(16\right)$

${\mathrm{\λ}}_1^2{\stackrel{\→}{u}}_1^T{\stackrel{\→}{v}}_1+{\mathrm{\λ}}_1{\mathrm{\λ}}_2({\stackrel{\→}{u}}_1^T+{\stackrel{\→}{u}}_2^T{\stackrel{\→}{v}}_1)+{\mathrm{\λ}}_2^2{\stackrel{\→}{u}}_2^T{\stackrel{\→}{v}}_2=0---\left(17\right)$

由于λ₁和λ₂不可能同时为0，假设λ₂≠0，则设s＝λ₁/λ₂，由式(17)得到s的两个解后代入到式(16)中得到最适合的s值，由以上各式即可得到单位对偶四元数的值即可得到两个航天器的相对位姿信息。

Claims (1)

1.一种航天器交会对接相对位姿测量方法，其特征在于，包括以下步骤：

步骤1、采用双目视觉定位算法确定目标航天器中任意两条非共面直线在追踪航天器坐标系中的坐标值,具体为：由追踪航天器两个摄像机C₁与C₂观察到的目标航天器上任意两条非共面直线在摄像机C₁图像坐标下的值分别为在摄像机C₂图像坐标下的值分别为则直线为由摄像机C₁坐标系原点O₁与组成的平面S₁和由摄像机C₂坐标系原点O₂与组成的平面S₂的交线，直线为由摄像机C₁坐标系原点O₁与组成的平面S₁'和由摄像机C₂坐标系原点O₂与组成的平面S₂'的交线，则在追踪航天器坐标系下的坐标值可由下面两个公式联立得到：

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{O1}^T{M}_1{\stackrel{\→}{l}}_{C1}=0\\ {\stackrel{\→}{l}}_{O2}^T{M}_2{\stackrel{\→}{l}}_{C1}=0\end{array}\right.$

$\left\{\begin{array}{c}{\stackrel{\→}{l}}_{{O1}^{\′}}^T{M}_1{\stackrel{\→}{l}}_{C2}=0\\ {\stackrel{\→}{l}}_{{O2}^{\′}}^T{M}_2{\stackrel{\→}{l}}_{C2}=0\end{array}\right.$

其中，M₁和M₂分别为追踪航天器两个摄像机的投影矩阵，为目标航天器两条直线在追踪航天器坐标系下的坐标值；

步骤2、采用普吕克直线法表示上述两条非共面直线在目标航天器坐标系下的坐标值，具体为：

$\left\{\begin{array}{c}{\hat{l}}_1=a+{{\mathrm{\ϵl}}_1}^{\′}\\ {\hat{l}}_2=b+{{\mathrm{\ϵl}}_2}^{\′}\end{array}\right.$

其中，l₁′=P₁×a；l₂′=P₂×b，P₁和P₂分别为直线和任意一点，为的模值，a为的单位向量，l₁′为单位向量a的矩，为的模值，b为的单位向量，l₂′为单位向量b的矩；

步骤3、采用普吕克直线法表示步骤1中获得的坐标值，具体为： $\left\{\begin{array}{c}{\hat{l}}_{C1}=a^{\′}+{{\mathrm{\ϵl}}_{C1}}^{\′}\\ {\hat{l}}_{C2}=b^{\′}+{{\mathrm{\ϵl}}_{C2}}^{\′}\end{array}\right.$

其中，l_C1′=P₁′×a′；l_C2′=P₂′×b′，P₁′和P₂′分别为P₁和P₂在追

踪航天器摄像机坐标系下对应的点；为的模值，a′为的单位向量，l_C1′为单位向量a′的矩；为的模值，b′为的单位向量，l_C2′为单位向量b′的矩；

步骤4、根据步骤2和步骤3获得普吕克直线法表示的坐标值，采用普吕克直线方程确定这两条直线在目标航天器和追踪航天器坐标系下的相对位姿关系，具体为：跟踪航天器和目标航天器坐标系之间的旋转和平移可用普吕克直线方程来表示，根据Plücker直线满足

其中，表示单位对偶四元数，表示单位对偶四元数的共轭；

步骤5、根据步骤4获得的上述两条非共面直线在目标航天器和追踪航天器坐标系下的相对位姿关系，采用奇异值分解的方法获得单位对偶四元数来表示目标航天器和追踪航天器坐标系间的相对位姿，从而得到两个航天器间的相对位置和姿态信息，具体为：

${\left(\begin{array}{cc}{S}_1^T& S_2^T\end{array}\right)}^T{\left(\begin{array}{c}q\\ q^{\′}\end{array}\right)}_{8\×1}=0$

其中

$S_1=\left(\begin{array}{cccc}{a}^{\′}-a& {[a^{\′}+a]}_{\×}& 0_{3\×1}& 0_{3\×3}\\ {l_{C1}}^{\′}-{l_1}^{\′}& {[{l_{C1}}^{\′}+{l_1}^{\′}]}_{\×}& a^{\′}-a& {[a^{\′}+a]}_{\×}\end{array}\right)$

$S_2=\left(\begin{array}{cccc}{b}^{\′}-b& {[b^{\′}+b]}_{\×}& 0_{3\×1}& 0_{3\×3}\\ {l_{C2}}^{\′}-{l_2}^{\′}& {[{l_{C2}}^{\′}+{l_2}^{\′}]}_{\×}& b^{\′}-b& {[b^{\′}+b]}_{\×}\end{array}\right)$

其中，a′和a为单位向量且a′⊥l_c1′，a⊥l₁′，[a′+a]_×称为a′+a的反对称矩阵；

设

$W={\left(\begin{array}{cc}{S}_1^T& S_2^T\end{array}\right)}^T$

W为S₁和S₂两个矩阵组合的转置矩阵，对W作奇异值分解，即W=UDV^T，即可确定单位对偶四元数的值从而测量出两个航天器间的相对位姿信息。