CN111536981B - 一种嵌入式的双目非合作目标相对位姿测量方法 - Google Patents

Abstract

本发明公开了一种嵌入式的双目非合作目标相对位姿测量方法，该方法包括以下步骤：首先，对双目相机进行离线参数标定，获取双目相机的内外参数及畸变参数；其次，用两个图像处理板分别处理左右相机采集的非合作目标图像，提取非合作目标主体的边框角点、对接环等特征信息；再次，用一个主控板将图像处理板传输过来的左右图像特征进行匹配，恢复匹配特征的三维坐标；最后，在主控板上计算得到非合作目标相对于双目相机的位置和姿态，得到非合作目标坐标系与双目相机坐标系之间的相对位姿。本发明通过双目相机结合非合作目标主体的特征信息，计算出非合作目标的相对位姿，实现了在轨维修航天器时非合作目标相对位姿测量的实时性和准确性。

Description

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

技术领域

本发明公开了在嵌入式领域的一种双目非合作目标相对位姿测量方法，尤其涉及计算机视觉、智能控制等领域的非合作目标相对位姿解算方法。

背景技术

空间目标相对位姿测量是未来空间在轨维修航天器的关键技术，特别是空间非合作目标，由于其运动情况和空间结构均未知，如何快速、准确地识别目标，并解算出目标的相对位姿是主要难点。

欧空局开展的地球静止轨道恢复器项目通过绳系飞网或飞爪对废弃航天器进行抓捕，在抓捕过程中利用激光测距、视觉测量等手段获取非合作目标的相对位姿参数。日本国家信息和通信技术研究中心提出通过双目立体视觉对非合作目标进行整体监控以及相对位姿测量的在轨服务系统。美国国防高级研究计划局的PHOENIX计划采用三目立体视觉对逼近阶段的非合作航天器相对位姿参数进行测量。近年来，国内学者针对这一问题也开展了大量研究，提出了基于单目视觉、双目视觉的非合作航天器相对位姿测量方法。但基于单目测量方法需要提供非合作航天器的形状及尺寸信息，基于双目测量方法要么在初始阶段需操作人员参与，要么仅仅在近距离、小模型下进行了实验验证。

发明内容

针对空间非合作目标相对位姿解算所存在的问题，本发明提供了一种嵌入式的双目非合作目标相对位姿测量方法，以实现在轨维修航天器时不需人工参与、实时解算出目标的相对位姿。

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

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

1)：利用黑白棋盘格对双目相机进行离线标定，得到双目相机的内外参数和畸变参数，具体步骤如下：

1-1)：坐标系的建立主要包括双目相机坐标系和非合作目标主体坐标系。

双目相机坐标系{O_c-X_cY_cZ_c}：左相机的光心作为原点O_c；左右相机光心连线，并指向右相机作为X_c轴；左相机光轴作为Z_c轴；通过原点O_c，X_c轴和Z_c轴的叉乘作为Y_c轴。

非合作目标主体坐标系{O_s-X_sY_sZ_s}：非合作目标主体上对接环中心作为原点O_s；经过原点O_s，平行于对接环表面的法向量并指向双目相机作为Z_s轴；经过原点O_s，对接环中心与左上角边框角点A的连线为X_s轴；经过原点O_s，X_s轴和Z_s轴的叉乘作为Y_s轴。

1-2)：离线标定是指利用张正友提出的基于黑白棋盘格的双目立体视觉系统标定算法获取双目相机的内外参数和畸变参数。左相机内参数包括焦距和/>主点坐标/>和/>斜率s^l；右相机内参数包括焦距/>和/>主点坐标/>和/>斜率s^r；双目相机外参数包括右相机相对于左相机的旋转矩阵R和平移向量T；畸变参数包括径向畸变参数{k₁，k₂，k₃}和切向畸变参数{p₁，p₂}。

径向畸变模型为：

切向畸变模型为：

其中，(x,y)为图像平面上的点，(x_corrected,y_corrected)为畸变校正后的点，r²＝x²+y²，{k₁，k₂，k₃}为径向畸变参数，{p₁，p₂}为切向畸变参数。

2)：利用两块图像处理板分别对左右相机采集的非合作目标图像进行处理，提取非合作目标主体的边框角点、对接环等特征信息，具体步骤如下：

2-1)：左右图像预处理是指对左右相机采集的非合作目标图像进行畸变矫正、二值化、模糊化、边缘检测、提取边缘、加粗边缘、边缘内部填充等预处理。

2-2)：主体边框角点提取是指在图像预处理基础上利用霍夫直线变换寻找目标主体边框的四条边缘及其角点。首先，利用霍夫直线变换筛选累加器大于阈值的所有直线；其次，根据直线的角度分为平行直线和垂直直线，在平行直线和垂直直线中分别利用聚类成两类；然后，在两类直线中选取其中累加器值最大的直线，即为目标主体的四条边框直线；之后，在寻找到目标主体的四条边框直线后，计算出四个角点；最后，以左上角点作为第一个点，按顺时针顺序排列四个角点，其中左图像四个角点记为右图像四个角点记为/>

2-3)：主体对接环提取是指在图像预处理基础上利用霍夫椭圆检测方法提取目标主体上的对接环，并得到对接环中心。根据边缘检测拟合对接环时，去掉边缘点太少的椭圆，去掉长轴和短轴之比太大的椭圆，去掉长短轴太长和太短的椭圆，去掉椭圆中心点距离太近的椭圆，去掉边缘点在椭圆上的点占边缘点总数较少的点，则得到比较鲁棒的椭圆，即为目标主体上的对接环。最后，根据提取的目标主体上对接环，计算出左图像对接环中心坐标和右图像对接环中心坐标/>

2-4)：主体表面特征点提取是指利用特征点提取算法快速提取左图像目标主体表面特征点和右图像目标主体表面特征点/>其中N为特征点的个数。

3)：利用一块主控板对2)中提取的左右图像特征进行匹配，结合1)中得到的双目相机参数恢复匹配特征的三维坐标，具体步骤如下：

3-1)：左右图像特征匹配是指将图像处理板传输到主控板上的左右图像特征点，基于双目极线约束找到左右图像中对应的匹配特征点。

3-2)：恢复匹配特征的三维坐标是指利用已标定的双目相机参数计算出匹配特征在双目相机坐标系下的三维坐标。和/> 分别为左右图像中一对匹配的特征点，其在双目相机坐标系下的三维坐标记为M_i。则

其中，和/>是常数，/>和/>分别为左右相机内参数矩阵，I是单位矩阵，T₀＝[0 00]^T，R和T是右相机相对于左相机坐标系的旋转矩阵和平移向量。

记

由上式可得

其中，A_i为系数矩阵。则通过最小二乘法解算出匹配特征点对在双目相机坐标系下的三维坐标M_i。从而，计算出对接环中心/>对应的三维坐标为M_doc；边框角点/>对应的三维坐标为M_cori，i＝1,2,3,4。

4)：在主控板上，根据3)中匹配特征的三维坐标，计算得到非合作目标相对于双目相机的位置和姿态。

相对位姿实时计算是指通过匹配特征点的三维坐标计算出目标坐标系与双目相机坐标系之间的相对位姿{R_rel T_rel}。通过1-1)建立的坐标系可知，在非合作目标主体坐标系下，对接环中心坐标为O_s，边框四个角点的坐标为M_corsi，i＝1,2,3,4。则

M_cori＝R_relM_corsi+T_rel，i＝1,2,3,4

T_rel＝-M_doc。

即解算出非合作目标在双目相机坐标系下的相对位姿。

与现有技术相比，本发明的有益效果在于：本发明的嵌入式非合作目标相对位姿测量方法通过两块图像处理板分别对目标的左右图像进行特征提取，之后将特征提取信息传输到主控板上进行特征匹配，并利用标定好的双目相机参数计算出匹配特征点的三维坐标，进而解算出非合作目标的相对位姿。本发明的嵌入式非合作目标相对位姿测量方法是被动式的，且在轨维修航天器时不需人工参与，能够实时解算出目标的cm量级相对位姿精度。

附图说明

以下结合附图和具体实施方式来进一步说明本发明。

图1为本发明实施例的嵌入式双目非合作目标相对位姿测量方法的流程示意图；

图2为本发明实施例的双目相机坐标系建立示意图；

图3为本发明实施例的非合作目标主体坐标系建立示意图；

图4为本发明实施例的非合作目标相对位姿测量方法的相对位置测量结果示意图。

图5为本发明实施例的非合作目标相对位姿测量方法的Z轴方向相对姿态测量结果示意图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面对照附图并结合实施例，对本发明作进一步说明。本发明以非合作目标尺寸3m*3m，目标表面由热控多层包覆为实施例。该实施例仅用于解释本发明，而不能解释为对本发明的限制。

参见图1，本发明的实施例公开了一种嵌入式的双目非合作目标相对位姿测量方法，包括以下步骤：

步骤1：利用黑白棋盘格对双目相机进行离线标定，得到双目相机的内外参数和畸变参数。

在具体实施时，该步骤包括不同位置和姿态下黑白棋盘格的左右图像采集、双目相机坐标系和非合作目标主体坐标系的建立、离线标定双目相机的内外参数。

双目相机坐标系的建立如图2所示：双目相机坐标系{O_c-X_cY_cZ_c}：左相机的光心作为原点O_c；左右相机光心连线，并指向右相机作为X_c轴；左相机光轴作为Z_c轴；通过原点O_c，X_c轴和Z_c轴的叉乘作为Y_c轴。

非合作目标主体坐标系的建立如图3所示：非合作目标主体坐标系{O_s-X_sY_sZ_s}：非合作目标主体上对接环中心作为原点O_s；经过原点O_s，平行于对接环表面的法向量并指向双目相机作为Z_s轴；经过原点O_s，对接环中心与左上角边框角点A的连线为X_s轴；经过原点O_s，X_s轴和Z_s轴的叉乘作为Y_s轴。

离线标定双目相机的内外参数：利用张正友提出的基于黑白棋盘格的双目立体视觉系统标定算法获取双目相机的内外参数和畸变参数。左相机内参数包括焦距和/>主点坐标/>和/>斜率s^l；右相机内参数包括焦距/>和/>主点坐标/>和/>斜率s^r；双目相机外参数包括右相机相对于左相机的旋转矩阵R和平移向量T；畸变参数包括径向畸变参数{k₁，k₂，k₃}和切向畸变参数{p₁，p₂}。

径向畸变模型为：

切向畸变模型为：

其中，(x，y)为图像平面上的点，(x_corrected,y_corrected)为畸变校正后的点，r²＝x²+y²，{k₁，k₂，k₃}为径向畸变参数，{p₁，p₂}为切向畸变参数。

步骤2：利用两块图像处理板分别对左右相机采集的非合作目标图像进行处理，提取非合作目标主体的边框角点、对接环等特征信息。

在具体实施时，该步骤包括：图像预处理、目标主体边框角点提取、目标主体对接环提取、目标主体表面特征点提取。

图像预处理：对左右相机采集的非合作目标图像进行畸变矫正、二值化、模糊化、边缘检测、提取边缘、加粗边缘、边缘内部填充等预处理。

目标主体边框角点提取：在图像预处理基础上利用霍夫直线变换寻找目标主体边框的四条边缘及其角点。首先，利用霍夫直线变换筛选累加器大于阈值的所有直线；其次，根据直线的角度分为平行直线和垂直直线，在平行直线和垂直直线中分别利用聚类成两类；然后，在两类直线中选取其中累加器值最大的直线，即为目标主体的四条边框直线；之后，在寻找到目标主体的四条边框直线后，计算出四个角点；最后，以左上角点作为第一个点，按顺时针顺序排列四个角点，其中左图像四个角点记为右图像四个角点记为/>

目标主体对接环提取：在图像预处理基础上利用霍夫椭圆检测方法提取目标主体上的对接环，并得到对接环中心。根据边缘检测拟合对接环时，去掉边缘点太少的椭圆，去掉长轴和短轴之比太大的椭圆，去掉长短轴太长和太短的椭圆，去掉椭圆中心点距离太近的椭圆，去掉边缘点在椭圆上的点占边缘点总数较少的点，则得到比较鲁棒的椭圆，即为目标主体上的对接环。最后，根据提取的目标主体上对接环，计算出左图像对接环中心坐标和右图像对接环中心坐标/>

目标主体表面特征点提取：指利用特征点提取算法快速提取左图像目标主体表面特征点和右图像目标主体表面特征点/>i＝1,2,…,N，其中N为特征点的个数。

步骤3：利用一块主控板对步骤2中提取的左右图像特征进行匹配，结合步骤1中得到的双目相机参数恢复匹配特征的三维坐标。

在具体实施时，该步骤包括：提取特征信息传输、左右图像特征匹配、匹配特征三维坐标计算。

提取特征信息传输：将两块图像处理板提取的左右图像特征信息分别传输到主控板。

左右图像特征匹配：基于双目极线约束找到左右图像特征中对应的匹配特征点。在双目立体视觉中，基于极线约束，将左右图像上对应点匹配的搜索空间从整幅图像缩小到特定直线上，极大地提高了匹配效率。

匹配特征的三维坐标计算：利用已标定的双目相机参数计算出匹配特征在双目相机坐标系下的三维坐标。假设和/>分别为左右图像中一对匹配的特征点，其在双目相机坐标系下的三维坐标记为M_i。则

其中，和/>是常数，/>和/>分别为左右相机内参数矩阵，I是单位矩阵，T₀＝[0 00]^T，R和T是右相机相对于左相机坐标系的旋转矩阵和平移向量。

记

由上式可得

其中，A_i为系数矩阵。则通过最小二乘法解算出匹配特征点对在双目相机坐标系下的三维坐标M_i。从而计算出对接环中心/>对应的三维坐标为M_doc；边框角点/>对应的三维坐标为M_cori，i＝1,2,3,4。

步骤4：在主控板上，根据步骤3中匹配特征的三维坐标，计算得到非合作目标相对于双目相机的位置和姿态。

在具体实施时，该步骤包括：非合作目标相对位姿实时计算。

非合作目标相对位姿实时计算：通过匹配特征点的三维坐标计算出目标坐标系与双目相机坐标系之间的相对位姿{R_relT_rel}。如图3所示，在非合作目标主体坐标系下，对接环中心坐标为O_s，边框四个角点的坐标为M_corsi，i＝1,2,3,4。则

M_cori＝R_relM_corsi+T_rel，i＝1,2,3,4

T_rel＝-M_doc。

即解算出非合作目标在双目相机坐标系下的相对位姿。

本实施例中，对for循环、多重循环、if判断声明等代码进行优化，以有利于DSP处理器进行更好的处理。

本发明提出的嵌入式双目非合作目标相对位姿测量方法的实验结果如图4和图5所示。横坐标为采集的目标图像帧数，纵坐标为测量结果与真值的差。

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

Claims (1)

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

1)：利用黑白棋盘格对双目相机进行离线标定，得到双目相机的内外参数和畸变参数，具体步骤如下：

1-1)：坐标系的建立主要包括双目相机坐标系和非合作目标主体坐标系，双目相机坐标系{O_c-X_cY_cZ_c}：左相机的光心作为原点O_c；左右相机光心连线，并指向右相机作为X_c轴；左相机光轴作为Z_c轴；通过原点O_c，X_c轴和Z_c轴的叉乘作为Y_c轴；

非合作目标主体坐标系{O_s-X_sY_sZ_s}：非合作目标主体上对接环中心作为原点O_s；经过原点O_s，平行于对接环表面的法向量并指向双目相机作为Z_s轴；经过原点O_s，对接环中心与左上角边框角点A的连线为X_s轴；经过原点O_s，X_s轴和Z_s轴的叉乘作为Y_s轴；

1-2)：离线标定是指利用张正友提出的基于黑白棋盘格的双目立体视觉系统标定算法获取双目相机的内外参数和畸变参数，左相机内参数包括焦距和/>主点坐标/>和/>斜率s^l；右相机内参数包括焦距/>和/>主点坐标/>和/>斜率s^r；双目相机外参数包括右相机相对于左相机的旋转矩阵R和平移向量T；畸变参数包括径向畸变参数{k₁，k₂，k₃}和切向畸变参数{p₁，p₂}；

径向畸变模型为：

切向畸变模型为：

其中，(x，y)为图像平面上的点，(x_corrected，y_corrected)为畸变校正后的点，r²＝x²+y²，{k₁，k₂，k₃}为径向畸变参数，{p₁，p₂}为切向畸变参数；

2)：利用两块图像处理板分别对左右相机采集的非合作目标图像进行处理，提取非合作目标主体的边框角点和对接环；具体步骤如下：

2-1)：左右图像预处理是指对左右相机采集的非合作目标图像进行畸变矫正、二值化、模糊化、边缘检测、提取边缘、加粗边缘和边缘内部填充；

2-2)：主体边框角点提取是指在图像预处理基础上利用霍夫直线变换寻找目标主体边框的四条边缘及其角点，首先，利用霍夫直线变换筛选累加器大于阈值的所有直线；其次，根据直线的角度分为平行直线和垂直直线，在平行直线和垂直直线中分别利用聚类成两类；然后，在两类直线中选取其中累加器值最大的直线，即为目标主体的四条边框直线；之后，在寻找到目标主体的四条边框直线后，计算出四个角点；最后，以左上角点作为第一个点，按顺时针顺序排列四个角点，其中左图像四个角点记为右图像四个角点记为/>

2-3)：主体对接环提取是指在图像预处理基础上利用霍夫椭圆检测方法提取目标主体上的对接环，并得到对接环中心；根据边缘检测拟合对接环时，去掉边缘点太少的椭圆，去掉长轴和短轴之比太大的椭圆，去掉长短轴太长和太短的椭圆，去掉椭圆中心点距离太近的椭圆，去掉边缘点在椭圆上的点占边缘点总数较少的点，得到的椭圆即为目标主体上的对接环；最后，根据提取的目标主体上对接环，计算出左图像对接环中心坐标和右图像对接环中心坐标/>

2-4)：主体表面特征点提取是指利用特征点提取算法快速提取左图像目标主体表面特征点和右图像目标主体表面特征点/>i＝1,2,…,N，其中N为特征点的个数；

3)：利用一块主控板对2)中提取的左右图像特征进行匹配，结合1)中得到的双目相机参数恢复匹配特征的三维坐标；

3-1)：左右图像特征匹配是指将图像处理板传输到主控板上的左右图像特征点，基于双目极线约束找到左右图像中对应的匹配特征点；

3-2)：恢复匹配特征的三维坐标是指利用已标定的双目相机参数计算出匹配特征在双目相机坐标系下的三维坐标；和/>分别为左右图像中一对匹配的特征点，其在双目相机坐标系下的三维坐标记为M_i；则

其中，和/>是常数，/>和/>分别为左右相机内参数矩阵，I是单位矩阵，T₀＝[0 0 0]^T，R和T是右相机相对于左相机坐标系的旋转矩阵和平移向量；

记

由上式可得

其中，A_i为系数矩阵，则通过最小二乘法解算出匹配特征点对在双目相机坐标系下的三维坐标M_i，从而，计算出对接环中心/>对应的三维坐标为M_doc；边框角点/>对应的三维坐标为M_cori，i＝1,2,3,4；

4)：在主控板上，根据3)中匹配特征的三维坐标，计算得到非合作目标相对于双目相机的位置和姿态；

相对位姿实时计算是指通过匹配特征点的三维坐标计算出目标坐标系与双目相机坐标系之间的相对位姿{R_rel T_rel}；通过1-1)建立的坐标系可知，在非合作目标主体坐标系下，对接环中心坐标为O_s，边框四个角点的坐标为M_corsi，i＝1,2,3,4；则

M_cori＝R_relM_corsi+T_rel，i＝1,2,3,4

T_rel＝-M_doc

即解算出非合作目标在双目相机坐标系下的相对位姿。