CN103245335A - 一种自主在轨服务航天器超近距离视觉位姿测量方法 - Google Patents

Abstract

本发明涉及一种自主在轨服务航天器超近距离视觉位姿测量方法,特别涉及一种用于自主在轨服务航天器的超近距离基于散焦图像的位姿测量方法，属于航天器相对视觉测量与自主导航领域。通过相机同步采集两幅模糊程度不同的散焦图像，图像1与图像2，其中图像1为相机CCD1所成的像，图像2为相机CCD2所成的像。采用基于S变换的DFD算法对目标进行散焦测距，获取目标图像每个像素点的整体深度信息u。图像处理提取特征点图像坐标值，结合图像坐标信息最终获取目标的位置姿态信息。本发明采用单镜头双CCD相机同时获取两幅图像用于散焦测距，能同时采集两幅相机参数不同的散焦图像。测量过程中无需改变相机参数，提高测量系统实时性。

Description

一种自主在轨服务航天器超近距离视觉位姿测量方法

技术领域

本发明涉及一种自主在轨服务航天器超近距离视觉位姿测量方法,特别涉及一种用于自主在轨服务航天器的超近距离基于散焦图像的位姿测量方法，属于航天器相对视觉测量与自主导航领域。

背景技术

近年来，以卫星在轨对接与组装、维修与燃料加注等航天器自主任务的研究受到广泛关注。在该类型的航天器自主任务的执行过程中，追踪航天器需要对目标航天器的位置与姿态信息进行实时的测量。由于两个航天器之间的相对距离很近，采用的传感器通常为相机和激光测距雷达（LIDAR）。激光测距雷达虽然可以较好地获取目标距离信息，但是造价极其昂贵，并且测量过程中会消耗大量能量，增加航天器负担。视觉测量系统采用相机作为传感器，通过对目标卫星上某些已知特征点成像，采用对应的相关算法从而求出目标航天器与追踪航天器之间的相对位置和相对姿态信息。

近些年很多学者围绕航天器交会对接为背景，对航天器相对视觉测量经行了大量研究，提出了不同的算法。曹喜滨等研究了航天器交会对接位姿视觉测量迭代算法（曹喜滨，张世杰.航天器交会对接位姿视觉测量迭代算法[J].哈尔滨工业大学学报.2005,37(008):1123-1126.2005,37(008):1123-1126）;朱仁璋等就交会对接逼近断,研究了一种视觉系统测距求解算法（朱仁璋，林彦，张磊.航天器交会计算机视觉系统测距求解新算法[J].北京航空航天大学学报.2006,32(7):764-768.2006,32(7):764-768）;冯春等研究了一种基于双焦距的航天器相对位姿测量算法（冯春，吴洪涛，乔兵，等.基于双焦单目视觉的航天器间相对位姿确定算法[J].中国空间科学技术.2012(4):37-44）。以上研究大都重点关注航天器交会对接的逼近段（距离从2m至20m左右），航天器本身相对姿态变化不大，主要是位置变化为主。而对于自主在轨服务器而言，追踪航天器与目标航天器之间的距离通常会更小（小于2m），此时在轨服务航天器在进行在轨操作过程中，相对距离相对稳定，反而相对姿态可能发生较大变化。而对于此类情形下的位姿测量问题，相关研究还比较少。此外，以上研究在建模过程中为了简化求解过程，常忽略特征点之间的深度差来简化透视投影方程，这样使得在超近距离时让测量误差增大。本发明在建模过程精细化数学模型，提高位姿测量的精度。

于此同时，光学系统在距离拍摄目标很近时具有景深浅的特点，相机所获取的图像容易因聚焦不当而发生散焦模糊。这种图像的模糊可能会对视觉测量系统的精度产生不良的影响。实际上，由光学原理可以得知，图像的模糊程度（通常定义扩散参数σ_h来表示散焦图像的模糊程度）与目标的物距u（即镜头到目标物体的距离）在相机内部参数（包括相机焦距f，镜头光圈口径D，镜头与相机成像面的距离s）固定时是线性相关的。根据这一原理，利用图像散焦模糊信息可以求解相机拍摄目标的物距u，一般也称u为目标的深度信息。基于这种原理的测距方法称为散焦测距（英文缩写DFD，Depth From Defocus）。散焦测距理论最早由Pentland提出（Pentland A P.A New Sense for Depth of Field[J].Pattern Analysis and Machine Intelligence,IEEE Transactions on,1987,PAMI-9(4):523-531）。DFD算法通过20年不断的发展，不同的学者提出了许多不同的算法。Subbarao M等提出了一种基于S变换的散焦测距算法（Subbarao M,Surya G.Depth from defocus:a spatial domain approach[J].International Journal of Computer Vision,1994,13(3):271-294）；Xian T等基于Subbarao M的S变换理论进行了相关测距算法的进一步研究（Xian T,SubbaraoM.Depth-from-defocus:blur equalization technique:Optics East2006,2006[C].International Society for Optics and Photonics）。上述研究中散焦测距需要相机在不同的相机参数（常见改变光圈大小D或者镜头到相机成像面的距离s）下获取两幅模糊程度不一样的图像，通过对比模糊程度的差异最终求解目标深度信息。如果再用常规的测量相机，需要在测量过程中改变相机的参数，这样会大大降低测量的实时性。本发明采用一种单镜头双CCD的相机，实现了相机同时采集两幅相机参数（镜头到相机成像面的距离s）不一样的散焦图像，提高了测量系统的实时性。

发明内容

本发明要解决的技术问题是提高超近距离测量时的精度、降低测量成本，公开一种自主在轨服务航天器超近距离视觉位姿测量方法。

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

本发明采用一种特制的相机作为测量用相机，所述特制的相机包括一个镜头、两个CCD；镜头采用远心光学镜头，即将镜头的光圈叶片放置于镜头的前向焦平面处，这样使得光线通过镜头后能平行于光轴。这样当镜头与CCD的距离改变时，特征点目标成像的中心点不会随之发生偏移。为了能同时在不同镜头与CCD距离时采集两幅图像，使用一块半反半透镜放置于镜头后方，从镜头过来的光线被该透镜平均分成两个方向，两个方向的光线分别投射在CCD1成像面与CCD2成像面上。两块CCD距镜头的距离略有不同，镜头到CCD1成像面的距离记为s₁，镜头到CCD2成像面的距离记为s₂，且s₂＞s₁。

本发明的一种自主在轨服务航天器超近距离视觉位姿测量方法，包括如下步骤：

步骤一，通过相机同步采集两幅模糊程度不同的散焦图像，图像1与图像2，其中图像1为相机CCD1所成的像，图像2为相机CCD2所成的像。

步骤二，采用基于S变换的DFD算法对目标进行散焦测距，获取目标图像每个像素点的整体深度信息u。

步骤三，图像处理提取特征点图像坐标值，结合图像坐标信息最终获取目标的位置姿态信息。

所述步骤三的图像处理提取特征点图像坐标值的方法，具体步骤如下：

利用特征点光斑所成的像与黑色背景之间的亮度差异提取出特征点光斑区域，通过计算区域重心计算每个特征点图像坐标(U_i,V_i),(i＝1～5)。

所述步骤三获取目标的位置姿态信息的方法，具体步骤如下：

步骤1、首先定义基本坐标系特征点构型。共建立三个坐标系，分别是以相机为中心的相机坐标系O_CX_CY_CZ_C，以目标特征构型为中心的目标坐标系O_TX_TY_TZ_T，以及图像坐标系O_IU_IV_I。在目标航天器上设定五个特征点s₁,s₂,s₃,s₄,s₅作为测量参考点。

步骤2、求相对位置参数；

由已经计算出的每个特征点图像坐标(U_i,V_i),(i＝1～5)在步骤二的深度信息结果中查找对应的特征点的深度信息u_i,(i＝1～5)。通过相机的几何关系方程（1）求出各个特征点在相机坐标系中y轴的分量

$y_i^c=u_i+s-f,(i=1~5)---\left(1\right)$

由特征点s₁与特征点s₃在相机坐标系中y轴分量

与其对应的图像坐标(U₁,V₁),(U₃,V₃)即可求出两航天器相对位置t＝[t_x,t_y,t_z]^T。具体表达式见公式（2）如下。

$t_y=(y_1^c+y_3^c)/2$

$t_x=(U_1+U_3)t_y/\left(2f\right)=(U_1+U_3)(y_1^c+y_3^c)/\left(4f\right)---\left(2\right)$

$t_z=(V_1+V_3)t_y/\left(2f\right)=(V_1+V_3)(y_1^c+y_3^c)/\left(4f\right)$

t_x,t_y,t_z三个参数即为3个相对位置参数，表示目标航天器坐标系中心与相机坐标系中心的相对位置关系。

步骤3、求解相对姿态参数；

由已经计算出的每个特征点图像坐标(U_i,V_i),(i＝1～5)与步骤3中获取的各个特征点在相机坐标系中的y轴分量

根据透视投影方程（方程（3））求解出各个特征点在相机坐标系中的坐标

$\left.\begin{array}{c}{y}_i^c{U}_i=x_i^{c}f\\ y_i^c{V}_i=z_i^{c}f\end{array}\right\},(i=1~5)---\left(3\right)$

已知5个特征点在目标坐标系中定义如下：

$s_1=\left[\begin{array}{c}-a\\ 0\\ 0\end{array}\right],s_2=\left[\begin{array}{c}0\\ b\\ 0\end{array}\right],s_3=\left[\begin{array}{c}a\\ 0\\ 0\end{array}\right],s_4=\left[\begin{array}{c}0\\ 0\\ -c\end{array}\right],s_5=\left[\begin{array}{c}0\\ 0\\ c\end{array}\right]---\left(4\right)$

根据坐标转化矩阵的关系

S_i＝Rs_i+t   (5)

（5）式中，

为了简便求解过程，将（6）式替换为（7）式。

$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]---\left(7\right)$

将特征点在相机坐标系中的坐标

特征点在目标坐标系中的坐标s_i,(i＝1～5)与相对位置向量t＝[t_x,t_y,t_z]^T带入（5）式可得方程组如下。

$\left\{\begin{array}{c}{x}_1^c=-a{R}_{11}+t_x\\ y_1^c=-a{R}_{21}+t_y\\ z_1^c=-a{R}_{31}+t_z\end{array}\right.,\begin{array}{}\end{array}\left\{\begin{array}{c}{x}_3^c=a{R}_{11}+t_x\\ y_3^c=a{R}_{21}+t_y\\ z_3^c=a{R}_{31}+t_z\end{array}\right.$

$\left\{\begin{array}{c}{x}_2^c=-b{R}_{12}+t_x\\ y_2^c=-b{R}_{22}+t_y\\ z_2^c=-b{R}_{23}+t_z\end{array}\right.---\left(8\right)$

$\left\{\begin{array}{c}{x}_4^c=c{R}_{13}+t_x\\ y_4^c=c{R}_{23}+t_y\\ z_4^c=c{R}_{33}+t_z\end{array}\right.,\begin{array}{}\end{array}\left\{\begin{array}{c}{x}_5^c=-{\mathrm{cR}}_{13}+t_x\\ y_5^c=-c{R}_{23}+t_y\\ z_5^c=-{\mathrm{cR}}_{33}+t_z\end{array}\right.$

由方程组（8）可求出：

$R_{11}=(y_3^c{u}_3-y_1^c{u}_1)/\left(2\mathrm{af}\right)$

$R_{12}=(y_3^c{u}_3+y_1^c{u}_1-2y_2^c{u}_2)/\left(2\mathrm{bf}\right)$

$R_{13}=(y_4^c{u}_4-y_5^c{u}_5)/\left(2\mathrm{cf}\right)---\left(9\right)$

$R_{23}=(y_4^c-y_5^c)/\left(2c\right)$

$R_{33}=(y_4^c{v}_4-y_5^c{v}_5)/\left(2\mathrm{cf}\right)$

由三角函数的对应关系（（6）式）可得姿态参数：

θ＝arcsin(-R₁₃)

ψ＝arctan(R₂₃/R₃₃)

θ,ψ,

三个角为姿态欧拉角，即姿态参数，分别表示目标航天器相对于追踪航天器之间俯仰、偏航与滚装的相对角度信息。

通过上述步骤2与步骤3，本发明方法所要测量的三个位置参数量和三个姿态参数量全部获取。这些量可给追踪航天器控制回路确定其与目标航天器的相对状态，以指导追踪航天器上的机械臂等部件对目标航天器进行相关组装、维修、燃料注入等操作。

有益效果

1、本发明的一种自主在轨服务航天器超近距离视觉位姿测量方法，考虑超近距离的场景，在散焦测距时考虑物距与相机坐标系坐标y轴分量的细微差别，采用映射投影关系方程时考虑特征点之间的深度差，使得数学模型更加精确，使得本发明方法在超近距离中有足够精度。

2、本发明的一种自主在轨服务航天器超近距离视觉位姿测量方法，采用单镜头双CCD相机同时获取两幅图像用于散焦测距，能同时采集两幅相机参数不同的散焦图像。测量过程中无需改变相机参数，提高测量系统实时性。

3、本发明的一种自主在轨服务航天器超近距离视觉位姿测量方法，可以用散焦图像中的模糊信息求解距离信息，将原本不利的因素变为可以利用的信息，同时还降低了测量系统对硬件设备的要求。

附图说明

图1为具体实施方式中单镜头双CCD相机光路示意图；

图2为具体实施方式中追踪航天器与目标航天器相对关系示意图；

图3为具体实施方式中CCD1采集到的图像1；

图4为具体实施方式中CCD2采集到的图像2；

图5为具体实施方式中STM-DFD算法基本流程图；

图6为具体实施方式中相机坐标系与图像坐标系示意图；

图7为具体实施方式中目标坐标系示意图；

图8为具体实施方式中物距与相机坐标几何关系图；

图9为具体实施方式中测量方法整体流程框图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

一种特制的相机作为测量用相机，所述特制的相机包括一个镜头、两个CCD；镜头采用远心光学镜头，即将镜头的光圈叶片放置于镜头的前向焦平面处，这样使得光线通过镜头后能平行于光轴。这样当镜头与CCD的距离改变时，特征点目标成像的中心点不会随之发生偏移。为了能同时在不同镜头与CCD距离时采集两幅图像，使用一块半反半透镜放置于镜头后方，从镜头过来的光线被该透镜平均分成两个方向，两个方向的光线分别投射在CCD1成像面与CCD2成像面上。两块CCD距镜头的距离略有不同，镜头到CCD1成像面的距离记为s₁，镜头到CCD2成像面的距离记为s₂，且s₂＞s₁。相机基本光路图见图1。

为方便计算坐标关系，相机安装于追踪航天器的正面中心位置，追踪航天器与目标航天器的相对关系见图2。

本发明的一种自主在轨服务航天器超近距离视觉位姿测量方法，包括如下步骤：

步骤一，通过相机同步采集两幅模糊程度不同的散焦图像，图像1与图像2，其中图像1为相机CCD1所成的像，图像2为相机CCD2所成的像。其中图像1见图3，图像2见图4。

步骤二，采用基于S变换的DFD算法对目标进行散焦测距，获取目标图像每个像素点的整体深度信息u。其中基于S变换的DFD算法流程见图5。

步骤三，图像处理提取特征点图像坐标值，结合图像坐标信息最终获取目标的位置姿态信息。

所述步骤三的图像处理提取特征点图像坐标值的方法，具体步骤如下：

利用特征点光斑所成的像与黑色背景之间的亮度差异提取出特征点光斑区域，通过计算区域重心计算每个特征点图像坐标(U_i,V_i),(i＝1～5)。

所述步骤三获取目标的位置姿态信息的方法，具体步骤如下：

步骤1、首先定义基本坐标系特征点构型。共建立三个坐标系，分别是以相机为中心的相机坐标系O_CX_CY_CZ_C，以目标特征构型为中心的目标坐标系O_TX_TY_TZ_T，以及图像坐标系O_IU_IV_I。在目标航天器上设定五个特征点s₁,s₂,s₃,s₄,s₅作为测量参考点。相机坐标系与图像坐标系见图6，目标坐标系见图7。

步骤2、求相对位置参数；

由已经计算出的每个特征点图像坐标(U_i,V_i),(i＝1～5)在步骤二的深度信息结果中查找对应的特征点的深度信息u_i,(i＝1～5)。通过相机的几何关系方程（1）求出各个特征点在相机坐标系中y轴的分量

$y_i^c=u_i+s-f,(i=1~5)---\left(11\right)$

对于物距u，相机焦距f，镜头与成像面距离s，特征点在相机坐标系中的y轴分量

的几何关系可以由图8表示。

由特征点s₁与特征点s₃在相机坐标系中y轴分量

与其对应的图像坐标(U₁,V₁),(U₃,V₃)即可求出两航天器相对位置t＝[t_x,t_y,t_z]^T。具体表达式见公式（2）如下。

$t_y=(y_1^c+y_3^c)/2$

$t_x=(U_1+U_3)t_y/\left(2f\right)=(U_1+U_3)(y_1^c+y_3^c)/\left(4f\right)---\left(12\right)$

$t_z=(V_1+V_3)t_y/\left(2f\right)=(V_1+V_3)(y_1^c+y_3^c)/\left(4f\right)$

t_x,t_y,t_z三个参数即为3个相对位置参数，表示目标航天器坐标系中心与相机坐标系中心的相对位置关系。

步骤3、求解相对姿态参数；

由已经计算出的每个特征点图像坐标(U_i,V_i),(i＝1～5)与步骤3中获取的各个特征点在相机坐标系中的y轴分量

根据透视投影方程（方程（3））求解出各个特征点在相机坐标系中的坐标

$\left.\begin{array}{c}{y}_i^c{U}_i=x_i^{c}f\\ y_i^c{V}_i=z_i^{c}f\end{array}\right\},(i=1~5)---\left(13\right)$

已知5个特征点在目标坐标系中定义如下：

$s_1=\left[\begin{array}{c}-a\\ 0\\ 0\end{array}\right],s_2=\left[\begin{array}{c}0\\ b\\ 0\end{array}\right],s_3=\left[\begin{array}{c}a\\ 0\\ 0\end{array}\right],s_4=\left[\begin{array}{c}0\\ 0\\ -c\end{array}\right],s_5=\left[\begin{array}{c}0\\ 0\\ c\end{array}\right]---\left(14\right)$

其中，a＝200mm,b＝300mm,c＝200mm,f＝50mm。

根据坐标转化矩阵的关系

S_i＝Rs_i+t   (15)

（5）式中，

为了简便求解过程，将（6）式替换为（7）式。

$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]---\left(17\right)$

将特征点在相机坐标系中的坐标

特征点在目标坐标系中的坐标s_i,(i＝1～5)与相对位置向量t＝[t_x,t_y,t_z]^T带入（5）式可得方程组如下。

$\left\{\begin{array}{c}{x}_1^c=-a{R}_{11}+t_x\\ y_1^c=-a{R}_{21}+t_y\\ z_1^c=-a{R}_{31}+t_z\end{array}\right.,\begin{array}{}\end{array}\left\{\begin{array}{c}{x}_3^c=a{R}_{11}+t_x\\ y_3^c=a{R}_{21}+t_y\\ z_3^c=a{R}_{31}+t_z\end{array}\right.$

$\left\{\begin{array}{c}{x}_2^c=-b{R}_{12}+t_x\\ y_2^c=-b{R}_{22}+t_y\\ z_2^c=-b{R}_{23}+t_z\end{array}\right.---\left(18\right)$

$\left\{\begin{array}{c}{x}_4^c=c{R}_{13}+t_x\\ y_4^c=c{R}_{23}+t_y\\ z_4^c=c{R}_{33}+t_z\end{array}\right.,\begin{array}{}\end{array}\left\{\begin{array}{c}{x}_5^c=-{\mathrm{cR}}_{13}+t_x\\ y_5^c=-c{R}_{23}+t_y\\ z_5^c=-{\mathrm{cR}}_{33}+t_z\end{array}\right.$

由方程组（8）可求出：

$R_{11}=(y_3^c{u}_3-y_1^c{u}_1)/\left(2\mathrm{af}\right)$

$R_{12}=(y_3^c{u}_3+y_1^c{u}_1-2y_2^c{u}_2)/\left(2\mathrm{bf}\right)$

$R_{13}=(y_4^c{u}_4-y_5^c{u}_5)/\left(2\mathrm{cf}\right)---\left(19\right)$

$R_{23}=(y_4^c-y_5^c)/\left(2c\right)$

$R_{33}=(y_4^c{v}_4-y_5^c{v}_5)/\left(2\mathrm{cf}\right)$

由三角函数的对应关系（（6）式）可得姿态参数：

θ＝arcsin(-R₁₃)

ψ＝arctan(R₂₃/R₃₃)

θ,ψ,

三个角为姿态欧拉角，即姿态参数，分别表示目标航天器相对于追踪航天器之间俯仰、偏航与滚装的相对角度信息。

通过上述步骤2与步骤3，本发明方法所要测量的三个位置参数量和三个姿态参数量全部获取。这些量可给追踪航天器控制回路确定其与目标航天器的相对状态，以指导追踪航天器上的机械臂等部件对目标航天器进行相关组装、维修、燃料注入等操作。本发明方法的整体流程框图见图9。

在航天器之间相对位置为：t＝[t_x,t_y,t_z]^T＝[25mm,800mm,-10mm]^T，三个姿态角参数θ,ψ

分别从0度到90度变化。在DFD算法精度和图像处理的相对精度在0.5%时，本实例实验姿态角结果如下表所示：

结果表明，俯仰角平均误差为0.1860度，最大误差为1.08度；偏航角平均误差为0.1435度，最大误差为0.34度；滚转角平均误差为0.0405度，最大误差为0.11度。

Claims (4)

1.一种自主在轨服务航天器超近距离视觉位姿测量方法，其特征在于：具体步骤如下：

步骤一，通过特制的相机同步采集两幅模糊程度不同的散焦图像，图像1与图像2，其中图像1为相机CCD1所成的像，图像2为相机CCD2所成的像；

步骤二，采用基于S变换的DFD算法对目标进行散焦测距，获取目标图像每个像素点的整体深度信息u；

步骤三，图像处理提取特征点图像坐标值，结合图像坐标信息最终获取目标的位置姿态信息。

2.如权利要求1所述的一种自主在轨服务航天器超近距离视觉位姿测量方法，其特征在于：所述特制的相机包括一个镜头、两个CCD；镜头采用远心光学镜头，即将镜头的光圈叶片放置于镜头的前向焦平面处，这样使得光线通过镜头后能平行于光轴；这样当镜头与CCD的距离改变时，特征点目标成像的中心点不会随之发生偏移；为了能同时在不同镜头与CCD距离时采集两幅图像，使用一块半反半透镜放置于镜头后方，从镜头过来的光线被该透镜平均分成两个方向，两个方向的光线分别投射在CCD1成像面与CCD2成像面上；两块CCD距镜头的距离略有不同，镜头到CCD1成像面的距离记为s₁，镜头到CCD2成像面的距离记为s₂，且s₂＞s₁。

3.如权利要求1所述的一种自主在轨服务航天器超近距离视觉位姿测量方法，其特征在于：所述步骤三的图像处理提取特征点图像坐标值的方法，具体步骤如下：

利用特征点光斑所成的像与黑色背景之间的亮度差异提取出特征点光斑区域，通过计算区域重心计算每个特征点图像坐标(U_i,V_i),(i＝1～5)。

4.如权利要求1所述的一种自主在轨服务航天器超近距离视觉位姿测量方法，其特征在于：所述步骤三获取目标的位置姿态信息的方法，具体步骤如下：

步骤1、首先定义基本坐标系特征点构型；共建立三个坐标系，分别是以相机为中心的相机坐标系O_CX_CY_CZ_C，以目标特征构型为中心的目标坐标系O_TX_TY_TZ_T，以及图像坐标系O_IU_IV_I；在目标航天器上设定五个特征点s₁,s₂,s₃,s₄,s₅作为测量参考点；

步骤2、求相对位置参数；

由已经计算出的每个特征点图像坐标(U_i,V_i),(i＝1～5)在步骤二的深度信息结果中查找对应的特征点的深度信息u_i,(i＝1～5)；通过相机的几何关系方程（1）求出各个特征点在相机坐标系中y轴的分量

$y_i^c=u_i+s-f,(i=1~5)---\left(1\right)$

由特征点s₁与特征点s₃在相机坐标系中y轴分量

与其对应的图像坐标(U₁,V₁),(U₃,V₃)即可求出两航天器相对位置t＝[t_x,t_y,t_z]^T；具体表达式见公式（2）如下；

$t_y=(y_1^c+y_3^c)/2$

$t_x=(U_1+U_3)t_y/\left(2f\right)=(U_1+U_3)(y_1^c+y_3^c)/\left(4f\right)---\left(2\right)$

$t_z=(V_1+V_3)t_y/\left(2f\right)=(V_1+V_3)(y_1^c+y_3^c)/\left(4f\right)$

t_x,t_y,t_z三个参数即为3个相对位置参数，表示目标航天器坐标系中心与相机坐标系中心的相对位置关系；

步骤3、求解相对姿态参数；

由已经计算出的每个特征点图像坐标(U_i,V_i),(i＝1～5)与步骤3中获取的各个特征点在相机坐标系中的y轴分量

根据透视投影方程（方程（3））求解出各个特征点在相机坐标系中的坐标

$\left.\begin{array}{c}{y}_i^c{U}_i=x_i^{c}f\\ y_i^c{V}_i=z_i^{c}f\end{array}\right\},(i=1~5)---\left(3\right)$

已知5个特征点在目标坐标系中定义如下：

$s_1=\left[\begin{array}{c}-a\\ 0\\ 0\end{array}\right],s_2=\left[\begin{array}{c}0\\ b\\ 0\end{array}\right],s_3=\left[\begin{array}{c}a\\ 0\\ 0\end{array}\right],s_4=\left[\begin{array}{c}0\\ 0\\ -c\end{array}\right],s_5=\left[\begin{array}{c}0\\ 0\\ c\end{array}\right]---\left(4\right)$

根据坐标转化矩阵的关系

S_i＝Rs_i+t   (5)

（5）式中，

为了简便求解过程，将（6）式替换为（7）式；

$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]---\left(7\right)$

将特征点在相机坐标系中的坐标

特征点在目标坐标系中的坐标s_i,(i＝1～5)与相对位置向量t＝[t_x,t_y,t_z]^T带入（5）式可得方程组如下；

$\left\{\begin{array}{c}{x}_1^c=-a{R}_{11}+t_x\\ y_1^c=-a{R}_{21}+t_y\\ z_1^c=-a{R}_{31}+t_z\end{array}\right.,\begin{array}{}\end{array}\left\{\begin{array}{c}{x}_3^c=a{R}_{11}+t_x\\ y_3^c=a{R}_{21}+t_y\\ z_3^c=a{R}_{31}+t_z\end{array}\right.$

$\left\{\begin{array}{c}{x}_2^c=-b{R}_{12}+t_x\\ y_2^c=-b{R}_{22}+t_y\\ z_2^c=-b{R}_{23}+t_z\end{array}\right.----\left(8\right)$

$\left\{\begin{array}{c}{x}_4^c=c{R}_{13}+t_x\\ y_4^c=c{R}_{23}+t_y\\ z_4^c=c{R}_{33}+t_z\end{array}\right.,\begin{array}{}\end{array}\left\{\begin{array}{c}{x}_5^c=-{\mathrm{cR}}_{13}+t_x\\ y_5^c=-c{R}_{23}+t_y\\ z_5^c=-{\mathrm{cR}}_{33}+t_z\end{array}\right.$

由方程组（8）可求出：

$R_{11}=(y_3^c{u}_3-y_1^c{u}_1)/\left(2\mathrm{af}\right)$

$R_{12}=(y_3^c{u}_3+y_1^c{u}_1-2y_2^c{u}_2)/\left(2\mathrm{bf}\right)$

$R_{13}=(y_4^c{u}_4-y_5^c{u}_5)/\left(2\mathrm{cf}\right)---\left(9\right)$

$R_{23}=(y_4^c-y_5^c)/\left(2c\right)$

$R_{33}=(y_4^c{v}_4-y_5^c{v}_5)/\left(2\mathrm{cf}\right)$

由三角函数的对应关系（（6）式）可得姿态参数：

θ＝arcsin(-R₁₃)

ψ＝arctan(R₂₃/R₃₃)

θ,ψ,

三个角为姿态欧拉角，即姿态参数，分别表示目标航天器相对于追踪航天器之间俯仰、偏航与滚装的相对角度信息；

通过上述步骤2与步骤3，本发明方法所要测量的三个位置参数量和三个姿态参数量全部获取；这些量可给追踪航天器控制回路确定其与目标航天器的相对状态，以指导追踪航天器上的机械臂等部件对目标航天器进行相关组装、维修、燃料注入等操作。

Images