CN107449402B - 一种非合作目标的相对位姿的测量方法 - Google Patents

Abstract

本发明公开了一种非合作目标的相对位姿的测量方法，包括：选择具有等边三角形特征的待测对象作为非合作目标，建立基于单线结构光的视觉测量系统模型；提取待测对象的三条直线以及单线结构光与待测对象的交点，得到待测对象的五个特征点在所述图像坐标系的坐标；根据待测对象的几何约束条件，计算得到待测对象的五个特征点在相机坐标系上的坐标；根据距离平移不变性和待测对象的几何约束条件，计算得到待测对象的五个特征点在世界坐标系上的坐标，再代入到相机坐标系与世界坐标系的转换方程中，求解得到待测对象的相对位姿，即得到非合作目标的相对位姿。本发明通过单目视觉增加一个辅助线结构光源，实现对未知尺寸的三角形特征的相对位姿测量。

Description

一种非合作目标的相对位姿的测量方法

技术领域

本发明涉及目标相对位姿测量领域，尤其涉及一种非合作目标的相对位姿的测量方法。

背景技术

近年来，利用空间机器人对地球静止轨道卫星进行在轨服务成为研究的热点，相对位姿测量是空间机器人对目标进行最终停靠和捕获的前提。当前地球静止轨道卫星多数为没有安装专门测量标志器的非合作目标，因此，对非合作目标的相对位姿测量是研究的难点。

基于目标特征的测量方法是非合作目标相对位姿测量的方向之一，该方法主要是通过视觉相机获取非合作目标上的自然特征图像，利用透视投影原理，从二维特征图像中恢复出目标的三维位姿。一些学者提出了基于卫星上圆形特征的相对位姿测量方法，该方法通过单目视觉相机获取卫星的圆形对接环图像，从图像中呈现出的椭圆特征计算目标的相对位姿信息，但该方法需要已知圆形的尺寸，且只能获取圆形特征5自由度位姿；一些学者同样基于卫星上圆形特征为识别目标，利用双目视觉的安装约束从中得到目标的相对位姿信息，该方法也只能获取圆形特征5自由度位姿；另一些学者开展基于卫星上矩形特征的相对位姿测量方法，该方法通过双目视觉获取目标的局部矩形特征，利用双目视觉的安装约束恢复目标的全局矩形特征信息，从而计算目标的相对位姿，其中该双目视觉对测量系统两个相机需同时工作，对可靠性提出了要求。

随着GEO轨道在轨服务技术的不断发展，对相对位姿测量技术的要求越来越高，测量方法要求简单、测量系统要求可靠。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

为克服非合作目标尺寸未知、视觉测量设备受视场限制无法获取足够多的可识别特征用于计算相对位姿的不足，本发明提出一种非合作目标的相对位姿的测量方法，通过单目视觉增加一个辅助线结构光源，实现对未知尺寸的三角形特征的相对位姿测量。

为了达到上述目的，本发明采用以下技术方案：

本发明公开了一种非合作目标的相对位姿的测量方法，包括以下步骤：

S1：选择具有等边三角形特征的待测对象作为非合作目标，建立基于单线结构光的视觉测量系统模型，其中所述单线结构光投射在所述待测对象的两条边上，所述视觉测量系统模型包括相机坐标系、图像坐标系、结构光坐标系和世界坐标系；

S2：采用视觉相机拍摄得到含有所述待测对象和所述单线结构光的图像，提取所述待测对象的三条直线以及所述单线结构光与所述待测对象的两条边上的两个交点，得到所述待测对象的五个特征点在所述图像坐标系的坐标，其中所述待测对象的五个特征点包括所述待测对象的三个顶点以及所述单线结构光与所述待测对象的两条边相交的两个交点；

S3：根据所述相机坐标系与所述图像坐标系的关系、所述相机坐标系与所述结构光坐标系的关系以及所述待测对象的几何约束条件，计算得到所述待测对象的五个特征点在所述相机坐标系上的坐标；

S4：根据距离平移不变性和所述待测对象的几何约束条件，计算得到所述待测对象的五个特征点在所述世界坐标系上的坐标，再代入到所述相机坐标系与所述世界坐标系的转换方程中，求解得到所述待测对象的相对位姿，即得到所述非合作目标的相对位姿。

优选地，步骤S1中还包括对相机和单线结构光进行联合标定，得到所述相机坐标系与所述图像坐标系之间的转换关系以及所述相机坐标系和所述结构光坐标系之间的转换关系。

优选地，步骤S2具体包括：采用霍夫变换在相机拍摄的图像上提取所述待测对象的三条直线，通过三条直线方程联立，得到所述待测对象的三个顶点在所述图像坐标系的坐标；再通过质心法计算得到所述单线结构光与所述待测对象的两条边相交的两个交点在图像坐标系的坐标。

优选地，步骤S3和步骤S4中的所述待测对象的几何约束条件具体包括：所述待测对象的等边三角形特征以及两个交点分别在所述待测对象的两条边上的几何约束条件。

优选地，步骤S4中的距离平移不变性具体为：所述待测对象的五个特征点从所述相机坐标系上平移至所述世界坐标系上各点之间的距离保持不变。

与现有技术相比，本发明的有益效果在于：本发明的非合作目标的相对位姿的测量方法通过单目视觉增加一个辅助线结构光源，利用单线结构光增加视觉测量设备的可识别特征数以及提供结构光点深度信息，可以实现对未知尺寸的三角形特征的相对位姿测量，并能得到三角形特征的实际尺寸；其中，本发明的测量方法中只需用到一个相机工作即可，提高系统的可靠性，并且通过该测量方法可以得到三角形特征的6自由度位姿。本发明的非合作目标的相对位姿的测量方法基于单线结构光，在卫星在轨服务的近距离靠近段和捕获段都能够提供高精度相对位姿。

附图说明

图1是本发明优选实施例的非合作目标的相对位姿测量方法的流程示意图；

图2是本发明优选实施例的测量的卫星太阳帆板的三角支架的结构示意图；

图3是本发明优选实施例的非合作目标的相对位姿测量方法的测量模型示意图；

图4是图3中世界坐标系的示意图；

图5是对本发明优选实施例的相对位姿测量方法进行仿真验证的结果示意图。

具体实施方式

下面对照附图并结合优选的实施方式对本发明作进一步说明。

如图1所示，本发明的优选实施例公开了一种非合作目标的相对位姿测量方法，包括以下步骤：

本实施例中选择空间非合作目标卫星的太阳帆板三角支架作为近距离交汇接近目标，卫星模型中太阳帆板10的三角支架20的结构如图2所示，也即本实施例以三角支架20作为非合作目标的待测对象，该三角支架20具有等边三角形特征，实际尺寸未知。

S1：选择具有等边三角形特征的待测对象作为非合作目标，建立基于单线结构光的视觉测量系统模型，如图3所示。用于测量的坐标系包括相机坐标系、图像坐标系、结构光坐标系和世界坐标系。对相机和结构光进行联合标定，得到相机坐标系与图像坐标系的转换关系以及相机坐标系与结构光坐标系的转换关系。

为了获取三角支架20的相对位置和姿态，建立四个坐标系：相机坐标系∑C、图像坐标系∑p、结构光坐标系∑L和世界坐标系∑W，如图3所示。

相机坐标系的原点O_C位于视觉相机光学镜头的中心，Z_C轴指向相机的视线方向，X_C轴和Y_C轴分别平行于视觉相机的成像平面30的两边，三轴形成右手坐标系，其中相机坐标系中点的齐次坐标为^CP＝[^CX,^CY,^CZ,1]^T。

图像坐标系为二维平面坐标系，其坐标原点位于视觉相机的成像平面30的左上角，图像坐标系的u轴和v轴分别平行于相机坐标系的X_C和Y_C轴，图像点的齐次坐标定义为p＝[u,v,1]^T，图像中心点的坐标定义为(u₀,v₀)。

结构光坐标系的原点O_L位于光源的中心，Z_L轴指向光源的发射方向且与相机坐标系的Z_C轴平行，X_L轴和Y_L轴分别平行于相机坐标系的X_C和Y_C轴，三轴形成右手坐标系。其中，线结构光的发射平面(如图3中的线结构光平面40)位于X_LZ_L平面内；结构光坐标系中点的齐次坐标为^LP＝[^LX,^LY,^LZ,1]^T。

世界坐标系建立在三角支架上，如图4所示，原点O_W位于三角支架底边的中点，Y_W轴沿着中垂线方向指向太阳帆10，Z_W轴垂直于三角支架20平面指向太阳帆10的反面，X_W轴形成右手坐标系，世界坐标系中点的齐次坐标为^WP＝[^WX,^WY,^WZ,1]^T。

利用相机的小孔成像原理，相机坐标系与图像坐标系之间的转换关系可以表示为：

其中，为相机的内参数矩阵，可以表示为：

其中，f_u和f_v为图像坐标系u和v轴方向单位化焦距，(u₀,v₀)为图像中心坐标；其中相机的内参数矩阵可以通过相机标定得到。

相机坐标系与结构光坐标系之间的转换关系可以表示为：

其中，

其中，为旋转矩阵，是单位正交矩阵；为平移矩阵；其中矩阵可以通过标定得到。

S2：通过图像霍夫变换(Hough变换)提取三角支架20的三条直线以及与线结构光50的交点，利用直线交点得到三角支架20的三个顶点在图像坐标系的坐标A、B、C，通过质心法得到线结构光50与三角支架20的两个交点在图像坐标系的坐标D、E。

测量过程中通过控制保证结构光始终投射到三角之间的两条边上，使用视觉相机获取含有三角支架20和线结构光50的图像，通过图像Hough变换直线提取可以得到3条直线，利用直线交点可以得到三角支架20的三个顶点图像坐标p_A＝[u_A,v_A,1]^T、p_B＝[u_B,v_B,1]^T、p_C＝[u_C,v_C,1]^T，线结构光50与两条边的交点p_D＝[u_D,v_D,1]^T、p_E＝[u_E,v_E,1]^T。

S3：通过相机坐标系与图像坐标系的关系、相机坐标系与结构光坐标系关系以及A点、B点、C点组成的等边三角形特征和D点在AB直线上，E点在AC直线上的比例关系，得到ABCDE五个特征点在相机坐标系下的坐标。

利用式(1)可以得到上述5个特征点在相机坐标系中的单位方向数：

利用式(4)，可以得到5个特征点在相机坐标系下的空间坐标为：

其中，t_i为待求变量，也即求得该参数可得5个特征点在相机坐标系中的坐标。

由于点D和E在线结构光平面40内，在结构光坐标系中满足下式：

^LY_i＝0,i＝D,E (6)

将式(3)和(5)代入式(6)，t_D,E可以计算为：

将t_D和t_E反代入式(5)即能得到结构光点在相机坐标系下的坐标^CP_D和^CP_E。接下来将利用两个特征点计算点A、B、C在相机坐标系中的坐标，由于三角支架20为等腰三角形，则有2-范数：

||^CP_A-^CP_B||＝||^CP_A-^CP_C|| (8)

由于点D和E分别位于线段AB和AC上，则有：

将式(9)代入相机坐标系，可以得到下列关系式：

利用L-M方法(Levenberg-Marquardt方法)求解式(8)、(9)、(10)和(11)组成的方程组，可以得到三角支架的三个顶点参数t_A、t_B和t_C，进而可以计算在相机坐标系下的坐标^CP_A、^CP_B和^CP_C。至此，得到了5个特征点在相机坐标系中的坐标。

S4：通过距离平移不变性和三角支架的几何约束，得到五个特征点的矢量信息(在世界坐标系上的坐标)，代入世界坐标系和相机坐标系转换方程，最终得到对象卫星太阳帆板10的三角支架20相对于观察卫星的相对位姿。其中距离平移不变性表示从相机坐标系平移到世界坐标系上各个特征点之间的距离保持不变。

根据世界坐标系的定义，特征点B和C的世界坐标可以计算为：

同样，由于：

则顶点A的世界坐标系中坐标为：

^WP_A＝[0,-||^WO-^WP_A||,0,1]^T (14)

接下来计算点D在世界坐标系中的坐标，矢量^WP_B ^WP_D为：

由矢量运算可得：

^WO^WP_D＝^WO^WP_B+^WP_B ^WP_D (16)

进而可得点D的世界坐标系坐标为：

点E的世界坐标系中坐标可以采用相同方法得到，矢量^WP_C ^WP_E为：

由矢量运算可得：

^WO^WP_E＝^WO^WP_C+^WP_C ^WP_E (19)

进而可得点E的世界坐标系坐标为：

相机坐标系与世界坐标系之间的转换关系可以表示为：

其中，为转移矩阵，

其中，为旋转矩阵，是单位正交矩阵，旋转顺序定义为Z→X→Y，对应旋转角为γ→α→β，旋转矩阵可以表示为：

其中，S代表sine，C代表cosine。

非合作目标相对位姿测量问题，即进行转移矩阵的计算。由于世界坐标系下特征点的坐标中^WZ＝0，由世界坐标系和相机坐标系之间的转换关系式(21)，可以得到关于转移矩阵的矩阵方程为：

令矩阵N₁＝[^WP_A,^WP_B,^WP_C,^WP_D,^WP_E]^T,N₂＝[^CP_A,^CP_B,^CP_C,^CP_D,^CP_E]，利用矩阵最小二乘法求解转移矩阵参数为：

由于旋转矩阵为正交矩阵，则第三列旋转列向量

其中，非合作目标的相对姿态欧拉角[α,β,γ]，可由三轴姿态旋转矩阵定义式(22)计算得到：

非合作目标的相对位置即为式(24)中的

至此，完成了对非合作目标三角支架的相对位姿测量(包括非合作目标的相对位置的3个自由度和姿态的3个自由度)，并能够通过计算得到非合作目标上三角支架特征的实际物理尺寸。

下述对本发明优选实施例的非合作目标的相对位姿的测量方法进行仿真验证，根据单线结构光视觉测量系统，建立单目视觉相机和线结构光的数学模型。相机的视场角度为54°，像素个数为1024×1024，图像中心坐标为[512,512]，结构光的安装位置为-500mm。线结构光视觉系统的两个内部参数矩阵为(含误差)：

内部参数误差大小如下表1所示。

表1内部参数误差

假设空间机器人和非合作目标之间的初始相对位姿为：

假设空间机器人和非合作目标之间的最终相对位姿为：

进行输入误差仿真，输入误差包括三角支架三条边的直线图像处理误差和结构光点的图像坐标提取误差。采用Hough变换直线提取的两个参数为ρ和θ，在两个参数的理论数值上叠加随机误差Δρ和Δθ，其中Δρ＝[-2,2]，Δθ＝[-0.1,0.1]。

空间机器人经过100个控制周期，从初始位置到达最终位置，输入误差的仿真结果如图5所示，由图5可以得出，本发明优选实施例的非合作目标的相对位姿的测量方法是有效的；也即本发明优选实施例的基于单线结构光的非合作目标的相对位姿的测量方法在卫星在轨服务的近距离靠近段和捕获段都能够提供高精度相对位姿。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (9)

1.一种非合作目标的相对位姿的测量方法，其特征在于，包括以下步骤：

S1：选择具有等边三角形特征的待测对象作为非合作目标，建立基于单线结构光的视觉测量系统模型，其中所述单线结构光投射在所述待测对象的两条边上，所述视觉测量系统模型包括相机坐标系、图像坐标系、结构光坐标系和世界坐标系；

S2：采用视觉相机拍摄得到含有所述待测对象和所述单线结构光的图像，提取所述待测对象的三条直线以及所述单线结构光与所述待测对象的两条边上的两个交点，得到所述待测对象的五个特征点在所述图像坐标系的坐标，其中所述待测对象的五个特征点包括所述待测对象的三个顶点以及所述单线结构光与所述待测对象的两条边相交的两个交点；

S3：根据所述相机坐标系与所述图像坐标系的关系、所述相机坐标系与所述结构光坐标系的关系以及所述待测对象的几何约束条件，计算得到所述待测对象的五个特征点在所述相机坐标系上的坐标；

S4：根据距离平移不变性和所述待测对象的几何约束条件，计算得到所述待测对象的五个特征点在所述世界坐标系上的坐标，再代入到所述相机坐标系与所述世界坐标系的转换方程中，求解得到所述待测对象的相对位姿，即得到所述非合作目标的相对位姿。

2.根据权利要求1所述的测量方法，其特征在于，步骤S1中还包括对相机和单线结构光进行联合标定，得到所述相机坐标系与所述图像坐标系之间的转换关系以及所述相机坐标系和所述结构光坐标系之间的转换关系。

3.根据权利要求1所述的测量方法，其特征在于，步骤S2具体包括：采用霍夫变换在相机拍摄的图像上提取所述待测对象的三条直线，通过三条直线方程联立，得到所述待测对象的三个顶点在所述图像坐标系的坐标；再通过质心法计算得到所述单线结构光与所述待测对象的两条边相交的两个交点在图像坐标系的坐标。

4.根据权利要求1所述的测量方法，其特征在于，步骤S3和步骤S4中的所述待测对象的几何约束条件具体包括：所述待测对象的等边三角形特征以及两个交点分别在所述待测对象的两条边上的几何约束条件。

5.根据权利要求1所述的测量方法，其特征在于，步骤S4中的距离平移不变性具体为：所述待测对象的五个特征点从所述相机坐标系上平移至所述世界坐标系上各点之间的距离保持不变。

6.根据权利要求1所述的测量方法，其特征在于，步骤S1中的所述视觉测量系统模型具体为：

相机坐标系的原点O_C位于视觉相机光学镜头的中心，Z_C轴指向相机的视线方向，X_C轴和Y_C轴分别平行于视觉相机的成像平面的两边，三轴形成右手坐标系，其中相机坐标系中点的齐次坐标为^CP＝[^CX,^CY,^CZ,1]^T；

图像坐标系为二维平面坐标系，坐标原点位于视觉相机的成像平面的左上角，图像坐标系的u轴和v轴分别平行于相机坐标系的X_C和Y_C轴，图像点的齐次坐标定义为p＝[u,v,1]^T，图像中心点的坐标定义为(u₀,v₀)；

结构光坐标系的原点O_L位于光源的中心，Z_L轴指向光源的发射方向且与相机坐标系的Z_C轴平行，X_L轴和Y_L轴分别平行于相机坐标系的X_C和Y_C轴，三轴形成右手坐标系，其中，单线结构光的发射平面位于X_LZ_L平面内，结构光坐标系中点的齐次坐标为^LP＝[^LX,^LY,^LZ,1]^T；

世界坐标系建立在所述待测对象上，原点O_W位于所述待测对象底边的中点，Y_W轴沿着中垂线方向指向所述待测对象以外的方向，Z_W轴垂直于所述待测对象，X_W轴沿着所述待测对象的底边，三轴形成右手坐标系，世界坐标系中点的齐次坐标为^WP＝[^WX,^WY,^WZ,1]^T。

7.根据权利要求6所述的测量方法，其特征在于，其中所述相机坐标系与所述图像坐标系之间的转换关系为：

其中，为相机的内参数矩阵，表示为：

其中，f_u和f_v为图像坐标系u和v轴方向单位化焦距，(u₀,v₀)为图像中心坐标；

所述相机坐标系与所述结构光坐标系之间的转换关系为：

其中，

其中，为旋转矩阵，是单位正交矩阵；为平移矩阵；

其中相机的内参数矩阵和矩阵均通过标定得到。

8.根据权利要求7所述的测量方法，其特征在于，步骤S2中得到所述待测对象的三个顶点A、B、C在所述图像坐标系上的坐标为：p_A＝[u_A,v_A,1]^T、p_B＝[u_B,v_B,1]^T、p_C＝[u_C,v_C,1]^T，所述单线结构光与所述待测对象的两条边相交的两个交点D、E在所述图像坐标系上的坐标为：p_D＝[u_D,v_D,1]^T、p_E＝[u_E,v_E,1]^T；

步骤S3具体包括：

根据式(1)得到点A、B、C、D、E在所述相机坐标系中的单位方向数为：

根据式(4)，得到点A、B、C、D、E在所述相机坐标系下的空间坐标为：

其中，t_i为待求变量；

由于点D、E在单线结构光的发射平面内，在所述结构光坐标系中满足下式：

^LY_i＝0,i＝D,E (6)

将式(3)和(5)代入式(6)，t_D,E计算为：

将t_D和t_E反代入式(5)得到点D、E在所述相机坐标系下的坐标^CP_D和^CP_E；

由于所述待测对象为等腰三角形，则有2-范数：

||^CP_A-^CP_B||＝||^CP_A-^CP_C|| (8)

由于点D、E分别位于线段AB和AC上，则有：

将式(9)代入所述相机坐标系，得到下列关系式：

利用L-M方法求解式(8)、(9)、(10)和(11)组成的方程组，得到所述待测对象的三个顶点A、B、C的参数t_A、t_B和t_C，进而计算得到在相机坐标系下的坐标^CP_A、^CP_B和^CP_C。

9.根据权利要求8所述的测量方法，其特征在于，步骤S4具体包括：

根据所述世界坐标系的定义，点B、C的世界坐标为：

由于：

则点A的世界坐标系中坐标为：

^WP_A＝[0,-||^WO-^WP_A||,0,1]^T (14)

由于点D在线段AB上，则：

由矢量运算得：

^WO^WP_D＝^WO^WP_B+^WP_B ^WP_D (16)

计算得到点D在世界坐标系的坐标为：

由于点E在线段AC上，则：

由矢量运算得到：

^WO^WP_E＝^WO^WP_C+^WP_C ^WP_E (19)

计算得到点E在世界坐标系的坐标为：

其中，所述相机坐标系与所述世界坐标系之间的转换关系为：

其中，为转移矩阵，

其中，为旋转矩阵，是单位正交矩阵，旋转顺序定义为Z→X→Y，对应旋转角为γ→α→β，旋转矩阵表示为：

其中，S代表sine，C代表cosine；

由于世界坐标系下点A、B、C、D、E的坐标中^WZ＝0，再结合世界坐标系和相机坐标系之间的转换关系式(21)，得到关于转移矩阵的矩阵方程为：

令矩阵N₁＝[^WP_A,^WP_B,^WP_C,^WP_D,^WP_E]^T,N₂＝[^CP_A,^CP_B,^CP_C,^CP_D,^CP_E]，利用矩阵最小二乘法求解转移矩阵参数为：

由于旋转矩阵为正交矩阵，则第三列旋转列向量根据式(24)计算得到所述待测对象的相对位置

由三轴姿态旋转矩阵定义式(22)，得到：

根据式(25)计算得到所述待测对象的相对姿态欧拉角[α,β,γ]。