CN108562274A - 一种基于标志器的非合作目标位姿测量方法 - Google Patents

Abstract

本发明公开了一种基于标志器的非合作目标位姿测量方法，首先通过对不同的标志器进行识别，解算标志器位姿，得到各标志器坐标系与相机坐标系之间的相对位姿；进行特征点三维坐标恢复，对标志器进行定位，解算各标志器坐标系与目标器坐标系间的位姿参数；精确估算出追踪航天器与目标航天器之间的相对位姿；通过引入预先设计的标志器可以更精确地测量位姿信息，解决非合作目标位姿测量准确度低的技术问题，同时克服了交会对接近距离阶段，追踪航天器无法获取完整目标航天器特定部件图像以识别定位的缺点。

Description

一种基于标志器的非合作目标位姿测量方法

技术领域

本发明属于空间交会与对接技术中的视觉测量领域，具体涉及一种基于标志器的非合作目标位姿测量方法。

背景技术

空间交会与对接技术是载人航天领域的三大基本技术之一，是实现航天器装配、回收、补给、维修、航天员交换及营救等在轨服务的首要条件。在交会对接最终逼近阶段，一般通过视觉测量提供目标航天器的相对位姿。目前，空间交会对接任务中普遍采用的都是基于合作目标的视觉测量技术，应用相对成熟。然而，空间绝大多数在轨航天器均属于非合作目标，其中包括故障或失效卫星、太空垃圾以及非己方航天器等，这就涉及到针对非合作目标的视觉测量关键技术。

非合作目标位姿测量的研究方向之一是以非合作目标的形状特征如三角架、星箭对接环、发动机喷嘴、矩形太阳能帆板、长方体目标基座等为测量对象，建立合适的参考坐标系，实现非合作目标的位姿解算。但其面临的挑战是如何从三维特征库中建立起与二维图像特征之间的特征点对应关系，并在此基础上设计目标位姿鲁棒估计方法。

发明内容

本发明提出一种基于标志器的非合作目标位姿测量方法，实现目标的精确位姿测量，解决非合作目标位姿测量准确度低的技术问题。

本发明采用如下技术方案，一种基于标志器的非合作目标位姿测量方法，采用二进制平方标记作为标志器，在交会对接逼近阶段向目标航天器抛射多个标志器，基于单目视觉测量原理对不同的标志器进行识别与定位，解算相机坐标系与标志器坐标系之间的相对位姿；同时通过识别非合作目标表面的星箭对接环，利用多幅图像的相机位姿与特征点信息确定标志器在目标器表面的相对位置，从而在对接的近距离阶段通过定位标志器即可解算目标器坐标系与相机坐标系之间的相对位姿，本发明提出了一种基于标志器的非合作目标位姿测量方法，包括以下步骤：

(1)建立相机坐标系、图像坐标系和目标器坐标系；

(2)离线标定，获取相机内参和畸变系数；

(3)对图像进行预处理，得到二值图像；

(4)标志器识别：首先在二值图像中进行轮廓检测，根据约束条件选出候选标志器；随后进行编码提取，对候选标志器的四个顶点进行逆时针排序，通过透视变换获得四边形区域的正面视图，基于最大类间方差阈值法OTSU将四边形区域划分为只包含黑白像素的均匀网格，通过识别四边形区域内部的海明编码确定标志器的序号以及初始顶点的位置；

(5)标志器位姿解算：构建标志器坐标系，利用高效N点透视相机位姿估计算法EPNP解算各标志器坐标系与相机坐标系之间的相对位姿；

(6)椭圆识别：首先进行弧段检测，提取整幅图像的边缘点信息，将边缘点分为梯度大于零和梯度小于零两组集合，即递增组和递减组，然后将边缘点合并为弧段，构造包围盒，去除不满足设定条件的弧段；随后进行弧段选择，将得到的弧段划分到四个象限，基于共圆锥曲线六点特征量CNC准则判定弧段是否属于同一椭圆，并基于象限约束和坐标约束获得有效的三弧段组合；再进行参数计算，基于椭圆上平行弦中点的连线过椭圆中心的几何定理，利用三弧段组合得到穿过椭圆中心的四条直线，取所有交点的代数均值作为椭圆中心；对椭圆参数空间进行降维处理，基于投票原则计算出椭圆的长短半轴和偏转角参数；最后进行后处理，去除三弧段中满足椭圆方程的边缘点占比小于设定值或三弧段长度和与椭圆长短半轴和之比小于设定值的候选椭圆；通过聚类算法将属于同一椭圆的多个检测结果进行合并；选取椭圆识别结果中半径最小的同心椭圆作为最终检测结果，即星箭对接环内环对应的椭圆；

(7)特征点三维坐标恢复：根据椭圆拟合的参数，在图像中构建感兴趣区域ROI，并在ROI区域内部利用累计概率霍夫变换进行直线检测，提取出星箭对接环内部相互垂直的两条直线轮廓；计算两条直线与椭圆边界的交点，将椭圆中心及四个交点作为特征点；利用标志器坐标系与相机坐标系间的位姿参数，基于最小二乘迭代的三角测量算法恢复五个特征点在各标志器坐标系下的三维坐标；

(8)标志器定位：根据三角测量恢复出的特征点三维坐标推算出各特征点在目标器坐标系下的三维坐标；基于最近点迭代ICP算法解算各标志器坐标系与目标器坐标系间的位姿参数；

(9)目标器位姿解算：将相机坐标系到标志器坐标系的变换矩阵和标志器坐标系到目标器坐标系的变换矩阵连乘，得到相机坐标系与目标器坐标系间的位姿参数。

发明达到的有益效果：本发明提出一种基于标志器的非合作目标位姿测量方法，实现目标位姿的精确测量，解决非合作目标位姿测量准确度低的技术问题；通过对不同的标志器进行识别与定位，同时基于单目视觉测量原理确定标志器在目标航天器表面的相对位置，精确估算出相机坐标系与目标器坐标系之间的相对位姿；通过引入预先设计的标志器可以更精确地测量位姿信息，同时克服了交会对接近距离阶段，追踪航天器无法获取完整目标航天器特定部件图像以识别定位的缺点。

附图说明

本发明从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明的基于标志器的非合作目标位姿测量方法的流程图；

图2为本发明实施例中采用的基于二进制平方的标志器示意图；

图3为本发明实施例中对属于第一、三象限的弧段构建包围盒的示意图；

图4为本发明实施例中将弧段划分到四个象限的示意图；

图5为本发明实施例中构造共圆锥曲线六点特征量CNC示意图；

图6为本发明实施例中基于象限约束和坐标约束筛选有效三弧段组示意图；

图7为本发明实施例中椭圆参数方程示意图；

图8为本发明实施例中确定椭圆中心的几何特性示意图；

图9为本发明实施例中利用三弧段组确定椭圆中心示意图；

图10为本发明实施例中椭圆筛选流程图；

图11为本发明实施例中三角测量原理示意图；

图12为本发明实施例中追踪航天器与目标航天器间六自由度位姿测量值与真实值的比较结果图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本发明提出一种基于标志器的非合作目标位姿测量方法。基于标志器的非合作目标位姿测量方法的流程图如图1所示，该方法包括以下步骤：

步骤1：建立相机坐标系、图像坐标系和目标器坐标系，基于相机透视投影模型，以相机光心作为相机坐标系原点，X轴、Y轴分别平行于图像坐标系的u轴、v轴，光轴方向作为Z轴，构建相机坐标系；以星箭对接环中心作为目标器坐标系原点，对接环表面法向量方向作为Z轴，X轴、Y轴分别平行于太阳能帆板的长边和短边，建立目标器坐标系；在本方案中由于相机是固连在追踪航天器上的，因此以相机坐标系替代追踪器坐标系。

步骤2：利用张正友的棋盘格标定法对单目相机进行离线标定，获取相机的内参数，即CCD单目相机在相机坐标系X轴和Y轴的归一化焦距f_x和f_y、CCD相机的主点像素坐标(u₀,v₀)、径向畸变系数k₁和k₂以及切向畸变系数p₁和p₂。

步骤3：图像预处理，得到二值图像，步骤如下：

步骤3-1：高斯滤波平滑，作用为抑制高频噪声，滤波核满足二维高斯分布：

其中，(x,y)为像素点坐标，σ为高斯核的标准差；

步骤3-2：图像灰度化，求出每个像素点R,G,B三个分量的平均值，并分别赋给该像素点，得到灰度图；

步骤3-3：局部自适应阈值化，根据每个像素的邻域块内的像素值分布来确定该像素位置上的二值化阈值，将灰度图转化为二值图像。

步骤4：图2为采用的基于二进制平方的标志器，标志器识别的步骤如下：

步骤4-1：轮廓检测，基于Suzuki和Abe算法获得包含较多噪声的轮廓集合；

步骤4-2：多边形逼近，对集合中每一条轮廓运用Douglas-Peucker算法，获得近似的多边形轮廓及其顶点信息；

步骤4-3：多边形约束，通过设置约束条件筛选出候选标志器，其中约束条件包括多边形的角点数量是否为四、是否为凸多边形、四边形的边长是否满足设定值、轮廓离图像边界距离是否满足设定值以及四边形集合中四顶点之间距离和是否满足设定值。

步骤4-4：对候选标志器顶点按逆时针排序：对于四个顶点零、顶点一、顶点二和顶点三，根据顶点零、顶点一和顶点零、顶点二构成的向量计算有向面积，如果有向面积为负数，即顶点为顺时针排序，交换顶点一和顶点三的位置，使四边形的四个顶点按逆时针排序；

步骤4-5：计算变换矩阵来去除透视投影，获得四边形区域的正面视图；

步骤4-6：对正面视图进行最大类间方差OTSU阈值化：

其中，[0,L-1]为图像的灰度级范围，t为灰度阈值，t^*为最佳灰度阈值，为不同灰度级的类间方差，Argmax(·)表示使目标函数取最大值时的变量值；

步骤4-7：将阈值化后的区域划分为均匀网格，统计方格内非零像素值个数，若方格内非零像素个数超过方格内像素个数的一半，则该方格为白色，否则为黑色；

步骤4-8：按行遍历所有轮廓方格，若轮廓方格中存在白色方格，则舍弃该轮廓所属的候选标志器；

步骤4-9：识别内部编码区域，构造与标志器内部网格大小一致的矩阵，遍历所有网格，将黑色方格对应为数值0，白色方格对应为数值1，依次赋值给矩阵的相应元素，则n×n的网格对应于n×n的0-1矩阵；将矩阵看作由n个n维行向量构成，每一个行向量由数据位和校验位组成，以5×5规格的序号为156的标志器为例，每一个行向量由两个数据位和三个校验位组成，其中数据位00对应于编码10000,数据位01对应于编码10111，数据位10对应于编码01001，数据位11对应于编码01110，通过将该特定标志器的每一行向量与候选标志器的对应行向量做异或运算，统计计算结果中值为1的个数之和作为海明距离；利用平衡二叉树搜索，找出候选标志器与字典(特定标志器集合)中海明距离最小的标志器作为匹配结果，由此可知该候选标志器的序号；

步骤4-10：确定候选标志器的序号后，判断它的旋转状态，将标志器分为初始状态、顺时针旋转90°、顺时针旋转180°、顺时针旋转270°四种状态，分别计算各状态下的标志器与字典中该序号对应的标志器间的海明距离，将海明距离为0的状态作为正确旋转状态；以正确旋转状态下标志器左上角的顶点作为顶点零，按逆时针方向确定顶点一、顶点二和顶点三；

步骤4-11：通过亚像素提取算法对顶点位置进一步细化。

步骤5：标志器位姿解算的步骤如下：

步骤5-1：对于每一个标志器，在正确旋转状态下，以标志器中心作为标志器坐标系原点O_m，以顶点零到顶点三的向量方向作为X_m轴方向，顶点一到顶点零的向量方向作为Y_m轴方向，Z_m轴方向由右手定则确立，构建标志器坐标系O_m-X_mY_mZ_m；

步骤5-2：标志器的实际尺寸为s×s，确定正确旋转状态下的标志器顶点零到顶点三在标志器坐标系下的空间坐标：(-s/2,s/2,0)、(-s/2,-s/2,0)、(s/2,-s/2,0)、(s/2,s/2,0)；

步骤5-3：结合相机内参，利用高效N点透视相机位姿估计(EPNP)算法求解相机坐标系与标志器坐标系间的位姿关系，即旋转矩阵R_cm和平移向量t_cm。

步骤6：椭圆识别步骤如下：

步骤6-1：通过Canny边缘检测算子提取图像边缘点，确定每个边缘点的位置坐标(x_i,y_i)，利用Sobel算子计算每个边缘点的梯度τ_i，得到边缘点信息e_i＝(x_i,y_i,τ_i),其中，i＝1,2,...,n,τ_i＝dy_i/dx_i，n为边缘点数量；

步骤6-2：根据边缘点的梯度方向不同，将边缘点分为两组，即由第二象限弧段组Arc_II和第四象限弧段组Arc_IV构成的递增组、由第一象限弧段组Arc_I和第三象限的弧段组Arc_III构成的递减组：

其中，τ_i表示第i个像素的梯度，e_i表示第i个像素点，Arc_I、Arc_II、Arc_III和Arc_IV分别表示属于第一象限、第二象限、第三象限和第四象限的弧段组，∪表示并集运算。

步骤6-3：对边缘点的八连通区域进行检测，将边缘点合并为弧段；

步骤6-4：对每一条弧段构建包围盒，如图3所示，起始点和末尾点分别为e₁和e_t的弧段,弧段长度为t，顶点(e₁(x),e₁(y))、(e_t(x),e₁(y))、(e_t(x),e_t(y))和(e₁(x),e_t(y))构成包含弧段在内的包围盒，e₁(x)、e₁(y)分别表示边缘点e₁的横坐标和纵坐标，e_t(x)、e_t(y)分别表示边缘点e_t的横坐标和纵坐标，设定最短弧长Th_length，若弧段长度t<Th_length，则舍弃该弧段；

步骤6-5：计算共线三点特征量(CNL)去除直线噪声，根据弧段的起始点e₁、中间点e_i和末尾点e_t，利用下式计算CNL值：

其中，|·|表示计算行列式；

该行列式的几何解释为三角形Δe₁e_ie_t的面积，使用面积与弧段长度之比来判断e₁,e_i,e_t三点是否共线，设t表示弧段长度，Th₀为给定阈值，即若CNL/t<Th₀，则判定该弧段为直线段，并舍弃该弧段。

步骤6-6：将弧段划分到四个象限，如图4所示，根据弧段上下的像素数量不同，对递增组和递减组的弧段再进行划分：

对于递减组Arc_I∪Arc_III的弧段，令δ表示包围盒中，弧段上方像素个数与下方像素个数之差，当弧段上部的像素数多于下部时(δ>0)，将弧段划分到Arc_III，否则划分到Arc_I；

对于递增组Arc_II∪Arc_IV的弧段，当弧段上部的像素数小于下部时(δ<0)，将弧段划分到Arc_II，否则划分到Arc_IV；

步骤6-7：利用共圆锥曲线六点特征量(CNC)准则判定弧段是否属于同一椭圆，如图5所示，对于两段圆弧和其中分别是的中点和两个端点，是的中点和两个端点，连接得到两条线段交于点P₁，连接得到两条线段交于点P₂，连接得到两条线段交于点P₃，由于共线三点中的任一点可由其余两点线性表示，可以得到下式：

其中,P_i为线段交点的像素坐标,为弧段上点的像素坐标,为对应系数。

通过上式求出系数并代入下式计算共圆锥曲线六点特征量(CNC)：

其中,CNC(P,Q)表示两段圆弧的CNC值，为对应系数，i表示直线交点P的索引，j表示弧段上构成直线的像素点的索引；∏(·)表示累乘运算；

设置CNC最小阈值为Th_CNC，若CNC(P,Q)-1<Th_CNC，则两条弧段属于同一椭圆；

步骤6-8：在象限约束以及坐标约束下得到四种有效弧段组合，为了减少无效的弧段组合，通过设置象限约束来选择位于相邻象限的弧段，即弧段属于一、二、四象限，弧段属于二、一、三象限，弧段属于三、二、四象限，弧段属于四、三、一象限；通过设置坐标约束，根据弧段顶点之间的相对位置关系，挑选出可能属于同一椭圆的三弧段组，结果下表1所示。

表1有效三弧段分组

结合CNC判定准则和坐标约束，从每一个有效弧段组合中筛选出属于同一椭圆的三弧段组，以属于一、二、三象限的有效弧段组合为例，三弧段组筛选算法的伪代码如下所示：

伪代码中，表示计算弧段与弧段间的CNC值。

步骤6-9：椭圆中心估计

如图7所示，平面中任意的椭圆可以用椭圆中心位置(x_c,y_c)，椭圆长半轴a，短半轴b，以及偏转角θ来表示。其数学表达式可写为：

如图8所示，对于弧段组p^ab，L_a,L_b分别是两条弧段的左顶点，R_a,R_b分别是上述两条弧段右顶点，M_a,M_b分别为两条弧段的中点，作n_d条平行于L_aM_b的弦，斜率为r₁，作n_d条平行于M_aR_b的弦，斜率为r₂，点集分别是两组弦的中点集合，其中近似位于直线l₁上，斜率为t₁，近似位于直线l₂上，斜率为t₂，表示点集的中间点，表示点集的中间点；利用改进的Theil-Sen算法得到t₁和t₂，算法伪代码如下：

伪代码中，GetSlope()为求直线斜率的函数，输入为一组中点集合midpoints[],输出为最佳的拟合斜率，middle表示中点个数的一半,slope为由两点计算的斜率，S[]表示斜率集合，Median()为求中值函数。

直线l₁和l₂的交点C可由下式计算得到：

分别为点的横坐标和纵坐标，分别为点的横坐标和纵坐标，C.x、C.y分别为交点C的横坐标和纵坐标，如图8所示，利用三弧段组筛选算法得到的有效的三弧段α^a,α^b,α^c可以计算出四条直线，产生至多六个交点，取六个交点的代数均值作为椭圆中心位置；

步骤6-10：计算长短半轴及偏转角

如图9所示，首先将包含椭圆剩余参数即长半轴a，短半轴b以及偏转角θ的参数空间降维到半轴比R＝b/a以及偏转角θ上，半轴比R、偏转角θ可通过下式计算：

其中，

q₁为弧段组(α^a,α^b)的平行弦斜率，q₃为弧段组(α^d,α^c)的平行弦斜率，q₂为弧段组(α^a,α^b)的平行弦中点连线的斜率，q₄为弧段组(α^d,α^c)的平行弦中点连线的斜率，R₊为初始半轴比，K₊为偏转角的初始斜率，γ和β为简化式；由上式可知，输入一组参数q₁,q₂,q₃,q₄便可以求出相应的R,θ。如图8所示，设r₁ ^ab,为弧段组(α^a,α^b)的平行弦斜率，r₁ ^dc,为弧段组(α^d,α^c)的平行弦斜率，直线直线为弧段组(α^a,α^b)的平行弦中点集合所在的直线，直线直线为弧段组(α^d,α^c)的平行弦中点集合所在的直线，根据上述的Theil-Sen算法可以得到直线的斜率集合直线的斜率集合直线的斜率集合直线的斜率集合q₁,q₂,q₃,q₄的取值见下表：

表2用于计算半轴比R、偏转角θ的q₁,q₂,q₃,q₄赋值表

其中，确定q₁,q₃的取值后，通过从斜率集合和中取不同的值赋给q₂,q₄，得到不同的q₁,q₂,q₃,q₄组合，对每一组合通过上式计算R,θ，得到R,θ的一维累加器，根据投票原则取累加器的峰值作为最终的R,θ。

长半轴a由下式计算：

a＝a_x/cos(θ)

其中，

上式中，a_x为长半轴在x轴上的投影，θ为偏转角，(x_c,y_c)为椭圆中心坐标，(x_i,y_i)为三条弧段α^a,α^b,α^c上的每一个边缘点的坐标，R为半轴比，K为偏转角θ对应的正切值，x₀和y₀为简化式；长半轴a在一维累加器中计算，取累加器的峰值作为a。

短半轴b由下式计算：

b＝a·R

至此得到椭圆拟合的五个参数，如图9所示。

步骤6-11：椭圆评价，定义两条椭圆评价准则，第一条：三弧段中有多少边缘点满足拟合的椭圆方程，即计算满足椭圆方程的边缘点个数与边缘点总数的比值，该值越大则椭圆评分越高；第二条：三弧段的弧长总和应大于拟合出的椭圆长半轴与短半轴之和，即计算三弧段长度和与椭圆长、短半轴之和的比值，该值越大则椭圆评分越高；最终剔除评分低于设定阈值的候选椭圆；

步骤6-12：比较两个椭圆ε_i,ε_j的中心距离、半轴距离和偏转角差值来判断椭圆相似性：

δ_a＝(|ε_i.a-ε_j.a|/max(ε_i.a,ε_j.a))<0.1

δ_b＝(|ε_i.b-ε_j.b|/min(ε_i.b,ε_j.b))<0.1

式中，δ_c表示两椭圆的中心距离，δ_a表示两椭圆的长半轴距离，δ_b表示两椭圆间的短半轴距离，δ_θ表示两椭圆间的偏转角距离，ε_i.a、ε_i.b分别表示椭圆ε_i的长半轴和短半轴，ε_j.a、ε_j.b分别表示椭圆ε_j的长半轴和短半轴，ε_i.x_c、ε_i.y_c分别表示椭圆ε_i中心的横坐标和纵坐标，ε_j.x_c、ε_j.y_c分别表示椭圆ε_j中心的横坐标和纵坐标，ε_i.θ、ε_j.θ分别表示椭圆ε_i,ε_j的偏转角；

当以上条件成立时，椭圆ε_i,ε_j归为同一聚类，选择聚类中心作为最终检测出的椭圆，则所有的聚类中心组成椭圆集合；

步骤6-13：目标器表面包含具有同心圆结构的星箭对接环以及尺寸较小的圆形喷嘴，因此选取椭圆集合中同心椭圆中半径小的作为最终检测结果。该步骤流程图如图10所示。

步骤7：特征点三维坐标恢复步骤如下，

步骤7-1：在图像中提取出椭圆感兴趣区域(ROI)，即以拟合出的椭圆中心作为矩形边界的中心点，以椭圆的长轴和短轴分别作为矩形边界的长和宽，椭圆的偏转角作为矩形边界的偏转角，生成内切于矩形边界的椭圆边界，并以椭圆中心作为种子点，基于漫水填充算法提取椭圆边界内部的图像区域。

步骤7-2：在ROI区域中，基于累计概率霍夫变换算法进行直线检测，提取出星箭对接环内部相互垂直的两条直线轮廓，并计算直线与椭圆边界的四个交点，与椭圆中心共组成五个椭圆特征点。为了方便不同视角下的特征点匹配，将特征点按固定顺序保存，即椭圆中心、上顶点、下顶点、左顶点和右顶点。

步骤7-3：计算单个标志器在目标器表面的位置，如图11所示，令三维空间点P＝[x,y,z,1]^T，对应的二维投影点为p＝[u,v,1]^T，由透视投影成像模型可得：

式中，ρ为非零常数因子，K为相机内参矩阵，R和t分别表示标志器坐标系到相机坐标系的旋转矩阵和平移向量；M＝K[R t]称为相机的投影矩阵，因此空间三维点到二维投影点的变换关系可用投影矩阵M来描述。以两幅视图为例，三维点P对应的投影矩阵分别表示为：

M₁＝K[R_cm1 t_cm1]

M₂＝K[R_cm2 t_cm2]

式中，R_cm1,t_cm1和R_cm2,t_cm2分别为两幅视图下相机坐标系相对于第i个标志器坐标系O_mi-X_miY_miZ_mi的旋转矩阵和平移向量，已通过高效N点透视相机位姿估计(EPNP)算法求解得到。

令空间三维点P＝[x,y,z,1]^T在两幅图像上的投影点分别为p₁＝[u₁,v₁,1]^T和p₂＝[u₂,v₂,1]^T，由p₁＝M₁P,p₂＝M₂P，令代入可得：

其中A为P左侧的系数矩阵，通过最小二乘法(LSM)可得到点P在该标志器坐标系下的三维坐标。同理，计算椭圆上的五个特征点在各个标志器下的三维坐标。

步骤8：标志器定位的步骤如下；

步骤8-1：根据三角测量恢复出的特征点在第i个标志器坐标系下的三维坐标，计算各特征点在目标器坐标系下的三维坐标，即：

其中，和分别表示椭圆中心、上顶点、下顶点、左顶点和右顶点在目标器坐标系下的三维坐标，Pk^m(k＝1,2,3,4)和分别表示上下左右四顶点和椭圆中心在第i个标志器坐标系下的三维坐标，Dis(·)表示计算两个三维点的欧式距离，s表示对接环的半径大小；

步骤8-2：迭代最近点求解位姿，经过上述步骤可以得到星箭对接环上五个特征点在第i个标志器坐标系下的三维坐标P_i ^m(i＝1,2,3,4,5)和目标器标志器下的三维坐标P_i ^t(i＝1,2,3,4,5)，基于最近点迭代(ICP)算法计算使得下列目标函数达到最小值时的旋转矩阵R和平移向量t：

式中，J为目标函数，反映累计的重投影误差大小，||·||₂表示求取二范数，R_mt和t_mt分别表示标志器坐标系到目标器坐标系的旋转矩阵和平移向量，P_km(k＝1,2,3,4,5)表示特征点在标志器坐标系下的三维坐标，P_kt(k＝1,2,3,4,5)表示特征点在目标器坐标系下的三维坐标。

步骤9：目标器位姿解算，相机坐标系到标志器坐标系的变换矩阵为R_cm,t_cm分别为相机坐标系相对于第i个标志器坐标系O_mi-X_miY_miZ_mi的旋转矩阵和平移向量，标志器坐标系到目标器坐标系的变换矩阵为R_mt和t_mt分别表示第i个标志器坐标系到目标器坐标系的旋转矩阵和平移向量，因此相机坐标系到目标器坐标系的变换矩阵为：R_ct,t_ct分别为相机坐标系相对于目标器坐标系的旋转矩阵和平移向量。

X偏移量、Y偏移量和Z偏移量分别为t_ct的三个分量。

欧拉角中比较常用的一种是用“偏航-俯仰-滚转”(yaw-pitch-roll)3个角度来描述一个旋转，它等价于ZYX轴的旋转，即

a绕物体的Z轴旋转，得到偏航角yaw；

b绕旋转后的Y轴旋转，得到俯仰角pitch；

c绕旋转后的X轴旋转，得到滚转角roll。

旋转矩阵表示为：

上式中，φ为偏航角，θ为俯仰角，ψ为滚转角，R_z(φ)表示绕z轴的旋转矩阵，R_y(θ)表示绕y轴的旋转矩阵，R_x(ψ)表示绕x轴的旋转矩阵，r_ij(i＝1,2,3；j＝1,2,3)表示旋转矩阵R中的各分量；

由上式可得目标航天器相对于追踪星的姿态参数，即相机的姿态参数：

ψ＝atan2(r₃₂,r₃₃)

φ＝atan2(r₂₁,r₁₁)

其中，φ为偏航角，θ为俯仰角，ψ为滚转角，atan2(y,x)为反正切函数，等价于atan(y/x)，r₁₁,r₂₁,r₃₁,r₃₂,r₃₃为旋转矩阵R对应下标的分量。

至此，便得到了目标航天器相对追踪星的姿态参数：偏航角φ、俯仰角θ、翻滚角ψ以及X偏移量、Y偏移量、Z偏移量。本发明提出的基于标志器的非合作目标位姿测量方法的测量值与真实值的比较结果见图12。

Claims (10)

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

(1)建立相机坐标系、图像坐标系和目标器坐标系；

(2)离线标定，获取相机内参和畸变系数；

(3)对图像进行预处理，得到二值图像；

(4)标志器识别：首先在二值图像中进行轮廓检测，根据约束条件选出候选标志器；随后进行编码提取，对候选标志器的四个顶点进行逆时针排序，通过透视变换获得四边形区域的正面视图，基于最大类间方差阈值法OTSU将四边形区域划分为只包含黑白像素的均匀网格，通过识别四边形区域内部的海明编码确定标志器的序号以及初始顶点的位置；

(5)标志器位姿解算：构建标志器坐标系，利用高效N点透视相机位姿估计算法EPNP解算各标志器坐标系与相机坐标系之间的相对位姿；

(6)椭圆识别：首先进行弧段检测，提取整幅图像的边缘点信息，将边缘点分为梯度大于零和梯度小于零两组集合，即递增组和递减组，然后将边缘点合并为弧段，构造包围盒，去除不满足设定条件的弧段；随后进行弧段选择，将得到的弧段划分到四个象限，基于共圆锥曲线六点特征量CNC准则判定弧段是否属于同一椭圆，并基于象限约束和坐标约束获得有效的三弧段组合；接着对三弧段组合进行参数计算，基于椭圆上平行弦中点的连线过椭圆中心的几何定理，利用三弧段组合得到穿过椭圆中心的四条直线，取所有交点的代数均值作为椭圆中心；对椭圆参数空间进行降维处理，基于投票原则计算出椭圆的长短半轴和偏转角参数；最后进行后处理，去除三弧段中满足椭圆方程的边缘点占比小于设定值或三弧段长度和与椭圆长短半轴和之比小于设定值的候选椭圆；通过聚类算法将属于同一椭圆的多个检测结果进行合并；选取椭圆识别结果中半径最小的同心椭圆作为最终检测结果，即星箭对接环内环对应的椭圆；

(7)特征点三维坐标恢复：根据椭圆拟合的参数，在图像中构建感兴趣区域ROI，并在ROI区域内部利用累计概率霍夫变换进行直线检测，提取出星箭对接环内部相互垂直的两条直线轮廓；计算两条直线与椭圆边界的交点，将椭圆中心及四个交点作为特征点；利用标志器坐标系与相机坐标系间的位姿参数，基于最小二乘迭代的三角测量算法恢复五个特征点在各标志器坐标系下的三维坐标；

(8)标志器定位：根据三角测量恢复出的特征点三维坐标推算出各特征点在目标器坐标系下的三维坐标；基于最近点迭代ICP算法解算各标志器坐标系与目标器坐标系间的位姿参数；

(9)目标器位姿解算：将相机坐标系到标志器坐标系的变换矩阵和标志器坐标系到目标器坐标系的变换矩阵连乘，得到相机坐标系与目标器坐标系间的位姿参数。

2.根据权利要求1所述的一种基于标志器的非合作目标位姿测量方法，其特征在于，在步骤(1)中基于相机透视投影模型，以相机光心作为相机坐标系原点，X轴、Y轴分别平行于图像坐标系的u轴、v轴，光轴方向作为Z轴，构建相机坐标系；以星箭对接环中心作为目标器坐标系原点，对接环表面法向量方向作为Z轴，X轴、Y轴分别平行于太阳能帆板的长边和短边，建立目标器坐标系。

3.根据权利要求1所述的一种基于标志器的非合作目标位姿测量方法，其特征在于，步骤(2)中利用张正友的棋盘格标定法对单目相机进行离线标定，获取相机的内参数，即CCD单目相机在相机坐标系X轴和Y轴的归一化焦距f_x和f_y、CCD相机的主点像素坐标(u₀,v₀)、径向畸变系数k₁和k₂以及切向畸变系数p₁和p₂。

4.根据权利要求1所述的一种基于标志器的非合作目标位姿测量方法，其特征在于，步骤(3)中图像预处理的步骤如下：

31)高斯滤波平滑，滤波核满足二维高斯分布：

其中，(x,y)为像素坐标，σ为高斯核的标准差；

32)图像灰度化，求出每个像素点的R分量、G分量和B分量的平均值赋给该像素点，得到灰度图；

33)局部自适应阈值化，根据每个像素的邻域块内的像素值分布确定该像素位置上的二值化阈值，将灰度图转化为二值图像。

5.根据权利要求1所述的一种基于标志器的非合作目标位姿测量方法，其特征在于，步骤(4)中标志器识别的步骤如下：

41)轮廓检测：基于Suzuki和Abe算法获得轮廓集合；

42)多边形逼近：对轮廓集合中每一条轮廓运用Douglas-Peucker算法，获得多边形轮廓及其顶点信息；

43)多边形约束：通过设置约束条件筛选出候选标志器，其中约束条件包括多边形的角点数量是否为四、是否为凸多边形、四边形的边长是否满足设定值、轮廓离图像边界距离是否满足设定值以及四边形集合中四顶点之间距离和是否满足设定值；

44)对候选标志器顶点按逆时针排序：对于四个顶点零、顶点一、顶点二和顶点三，根据顶点零、顶点一和顶点零、顶点二构成的向量计算有向面积，如果有向面积为负数，即顶点为顺时针排序，交换顶点一和顶点三的位置，使四边形的四个顶点按逆时针排序；

45)计算变换矩阵去除透视投影，获得四边形区域的正面视图；

46)对正面视图进行最大类间方差OTSU阈值化：

其中，[0,L-1]为图像的灰度级范围，t为灰度阈值，t^*为最佳灰度阈值，为不同灰度级的类间方差，Argmax(·)表示使目标函数取最大值时的变量值；

47)将阈值化后的区域划分为均匀网格，统计每个方格内非零像素值个数，若方格内非零像素个数超过方格内像素个数的一半，则该方格为白色，否则为黑色；

48)按行遍历所有轮廓方格，若轮廓方格中存在白色方格，则舍弃该轮廓所属的候选标志器；

49)识别内部编码区域：构造与标志器内部网格大小一致的矩阵，遍历所有网格，将黑色方格对应为数值0，白色方格对应为数值1，依次赋值给矩阵的相应元素，n×n的网格对应于n×n的0-1矩阵；将矩阵看作由n个n维行向量构成，每一个行向量由数据位和校验位组成，将特定标志器的每一行向量与候选标志器的对应行向量做异或运算，统计计算结果中值为1的个数之和作为海明距离；利用平衡二叉树搜索，找出候选标志器与字典，即特定标志器集合中海明距离最小的标志器作为匹配结果，得到候选标志器的序号；

410)判断候选标志器的旋转状态：将标志器分为初始状态、顺时针旋转90°、顺时针旋转180°和顺时针旋转270°四种状态，分别计算各状态下的标志器与字典中该序号的标志器间的海明距离，将海明距离为0的状态作为正确旋转状态；以正确旋转状态下标志器左上角的顶点作为顶点零，按逆时针方向确定顶点一、顶点二和顶点三；

411)通过亚像素提取算法对顶点位置进一步细化。

6.根据权利要求1所述的基于标志器的非合作目标位姿测量方法，其特征在于，步骤(5)中标志器位姿解算的步骤如下：

51)对于每一个标志器，在正确旋转状态下，以标志器中心作为标志器坐标系原点O_m，以顶点零到顶点三的向量方向作为X_m轴方向，顶点一到顶点零的向量方向作为Y_m轴方向，Z_m轴方向由右手定则确立，构建标志器坐标系O_m-X_mY_mZ_m；

52)标志器的实际尺寸为s×s，确定正确旋转状态下的标志器顶点零到顶点三在标志器坐标系下的空间坐标：(-s/2,s/2,0)、(-s/2,-s/2,0)、(s/2,-s/2,0)、(s/2,s/2,0)；

53)利用高效N点透视相机位姿估计EPNP算法求解相机坐标系到标志器坐标系的旋转矩阵R_cm和平移向量t_cm。

7.根据权利要求1所述的基于标志器的非合作目标位姿测量方法，其特征在于，步骤(6)中椭圆识别的步骤如下：

61)通过Canny边缘检测算子提取图像边缘点，确定每个边缘点的位置坐标(x_i,y_i)，利用Sobel算子计算每个边缘点的梯度τ_i，得到边缘点信息e_i＝(x_i,y_i,τ_i),其中，i＝1,2,...,n,τ_i＝dy_i/dx_i，n为边缘点数量；

62)根据边缘点的梯度方向不同，将边缘点分为两组，即由第二象限弧段组Arc_II和第四象限弧段组Arc_IV构成的递增组、由第一象限弧段组Arc_I和第三象限的弧段组Arc_III构成的递减组：

其中，τ_i表示第i个边缘点像素的梯度，e_i表示第i个边缘点，Arc_I、Arc_II、Arc_III和Arc_IV分别表示属于第一象限、第二象限、第三象限和第四象限的弧段组，∪表示并集运算；

63)对边缘点的八连通区域进行检测，将边缘点合并为弧段；

64)对每一条弧段构建包围盒：起始点和末尾点分别为e₁和e_t的弧段,弧段长度为t，顶点(e₁(x),e₁(y))、(e_t(x),e₁(y))、(e_t(x),e_t(y))和(e₁(x),e_t(y))构成包含弧段在内的包围盒，e₁(x)、e₁(y)分别表示边缘点e₁的横坐标和纵坐标，e_t(x)、e_t(y)分别表示边缘点e_t的横坐标和纵坐标，设定最短弧长Th_length，若弧段长度t<Th_length，则舍弃该弧段；

65)基于共线三点特征量CNL准则来去除直线噪声：根据弧段的起始点e₁、中间点e_i和末尾点e_t，利用下式计算CNL值：

其中，|·|表示计算行列式；

该行列式的几何解释为三角形Δe₁e_ie_t的面积，使用面积与弧段长度之比判断e₁、e_i、e_t三点是否共线，t表示弧段长度，Th₀为给定阈值，即若CNL/t<Th₀，则判定该弧段为直线段，舍弃该弧段；

66)将弧段划分到四个象限：根据弧段上下的像素数量不同，对递增组和递减组的弧段进行再划分：

对于递减组Arc_I∪Arc_III的弧段，δ表示每一弧段的包围盒中弧段上方像素个数与下方像素个数之差，当弧段上部的像素数多于下部时，即δ>0，将弧段划分到Arc_III，否则划分到Arc_I；

对于递增组Arc_II∪Arc_IV的弧段，当弧段上部的像素数小于下部时，即δ<0，将弧段划分到Arc_II，否则划分到Arc_IV；

67)利用共圆锥曲线六点特征量CNC准则判定弧段是否属于同一椭圆：对于两段圆弧和其中分别是的中点和两个端点，是的中点和两个端点，连接得到两条直线交于点P₁，连接 得到两条直线交于点P₂，连接得到两条直线交于点P₃，由此得到下式：

其中,P_i为直线交点的像素坐标,为弧段上点的像素坐标,为对应系数；

通过上式求出系数并代入下式计算共圆锥曲线六点特征量CNC值：

其中,CNC(P,Q)表示两段圆弧的CNC值，为对应系数，i表示直线交点P的索引，j表示弧段上构成直线的像素点的索引；∏(·)表示累乘运算；

设置CNC最小阈值为Th_CNC，若CNC(P,Q)-1<Th_CNC，则两条弧段属于同一椭圆；

68)在象限约束以及坐标约束下得到三弧段组：设置象限约束选择位于相邻象限的弧段，即有效弧段组合包括：弧段属于一、二、四象限，弧段属于二、一、三象限，弧段属于三、二、四象限，弧段属于四、三、一象限；结合CNC判定准则和弧段端点的相对位置约束，即坐标约束，从每一个有效弧段组合中筛选出属于同一椭圆的三弧段组；

69)确定椭圆中心：对于弧段组p^ab，L_a,L_b分别是两条弧段的左顶点，R_a,R_b分别是上述两条弧段右顶点，M_a,M_b分别为两条弧段的中点，作n_d条平行于L_aM_b的平行弦，斜率为r₁，作n_d条平行于M_aR_b的平行弦，斜率为r₂，点集分别是两组弦的中点集合，其中近似位于直线l₁上，斜率为t₁，近似位于直线l₂上，斜率为t₂，表示点集的中间点，表示点集的中间点；利用改进的Theil-Sen算法得到斜率t₁和t₂；

直线l₁和l₂的交点C可由下式计算得到：

分别为点的横坐标和纵坐标，分别为点的横坐标和纵坐标，C.x、C.y分别为交点C的横坐标和纵坐标，根据有效的三弧段组的弧段α^a,α^b,α^c可以计算出四条直线，产生至多六个交点，取六个交点的代数均值作为椭圆中心位置；

610)计算长短半轴及偏转角：将包含长半轴a、短半轴b和偏转角θ的参数空间降维到半轴比R＝b/a以及偏转角θ上，半轴比R、偏转角θ可通过下式计算：

其中，

上式中，q₁为弧段组(α^a,α^b)的平行弦斜率，q₃为弧段组(α^d,α^c)的平行弦斜率，q₂为弧段组(α^a,α^b)的平行弦中点连线的斜率，q₄为弧段组(α^d,α^c)的平行弦中点连线的斜率，R₊为初始半轴比，K₊为偏转角的初始斜率，γ和β为简化式；

为弧段组(α^a,α^b)的平行弦斜率，为弧段组(α^d,α^c)的平行弦斜率，直线直线为弧段组(α^a,α^b)的平行弦中点集合所在的直线，直线直线为弧段组(α^d,α^c)的平行弦中点集合所在的直线，根据Theil-Sen算法可以得到直线的斜率集合直线的斜率集合直线的斜率集合直线的斜率集合确定q₁,q₃的取值后，通过从斜率集合和中取不同的值赋给q₂,q₄，得到不同的q₁,q₂,q₃,q₄组合，对每一组合通过上式计算半轴比R、偏转角θ，得到半轴比R、偏转角θ的一维累加器，根据投票原则取累加器的峰值作为最终的半轴比R、偏转角θ；

长半轴a可表示为：

a＝a_x/cos(θ)

其中，

上式中，a_x为长半轴在x轴上的投影，θ为偏转角，(x_c,y_c)为椭圆中心坐标，(x_i,y_i)为三条弧段α^a,α^b,α^c上的每一个边缘点的坐标，R为半轴比，K为偏转角θ对应的正切值，x₀和y₀为简化式；长半轴a在一维累加器中计算，取累加器的峰值作为a；

短半轴b由下式计算：

b＝a·R

得到椭圆拟合的五个参数；

611)椭圆评价：计算三弧段中满足拟合的椭圆方程的边缘点与边缘点总数的比值，比值越大则椭圆评分越高；计算三弧段的弧长总和与拟合的椭圆长半轴与短半轴之和的比值，比值越大则椭圆评分越高；最终剔除评分低于设定阈值的候选椭圆；

612)椭圆聚类：比较两个椭圆ε_i,ε_j的中心距离、半轴距离和偏转角差值判断椭圆相似性：

δ_a＝(|ε_i.a-ε_j.a|/max(ε_i.a,ε_j.a))<0.1

δ_b＝(|ε_i.b-ε_j.b|/min(ε_i.b,ε_j.b))<0.1

式中，δ_c表示两椭圆的中心距离，δ_a表示两椭圆的长半轴距离，δ_b表示两椭圆间的短半轴距离，δ_θ表示两椭圆间的偏转角距离，ε_i.a、ε_i.b分别表示椭圆ε_i的长半轴和短半轴，ε_j.a、ε_j.b分别表示椭圆ε_j的长半轴和短半轴，ε_i.x_c、ε_i.y_c分别表示椭圆ε_i中心的横坐标和纵坐标，ε_j.x_c、ε_j.y_c分别表示椭圆ε_j中心的横坐标和纵坐标，ε_i.θ、ε_j.θ分别表示椭圆ε_i,ε_j的偏转角；

当以上条件成立时，椭圆ε_i,ε_j归为同一聚类，选择聚类中心作为检测出的椭圆，则所有的聚类中心组成椭圆集合；

613)椭圆筛选：选取椭圆集合中同心椭圆中半径小的椭圆作为最终检测结果。

8.根据权利要求1所述的基于标志器的非合作目标位姿测量方法，其特征在于，步骤(7)中特征点三维坐标恢复的步骤如下：

71)在图像中提取出椭圆感兴趣区域ROI，即以拟合的椭圆中心作为矩形边界的中心点，以椭圆的长轴和短轴分别作为矩形边界的长和宽，椭圆的偏转角作为矩形边界的偏转角，生成内切于矩形边界的椭圆边界，并以椭圆中心作为种子点，基于漫水填充算法提取椭圆边界内部的图像区域；

72)在ROI区域中，基于累计概率霍夫变换算法进行直线检测，提取出星箭对接环内部相互垂直的两条直线轮廓，并计算直线与椭圆边界的四个交点，与椭圆中心共组成五个椭圆特征点，将特征点按固定顺序保存，即按椭圆中心、上顶点、下顶点、左顶点和右顶点的顺序保存；

73)计算单个标志器在目标器表面的位置，三维空间点P＝[x,y,z,1]^T，对应的二维投影点为p＝[u,v,1]^T，由透视投影成像模型得到：

式中，ρ为非零常数因子，K为相机内参矩阵，R和t分别表示标志器坐标系到相机坐标系的旋转矩阵和平移向量；M＝K[R t]表示相机的投影矩阵，在两幅视图中，三维空间点P对应的投影矩阵分别表示为：

M₁＝K[R_cm1 t_cm1]

M₂＝K[R_cm2 t_cm2]

式中，R_cm1,t_cm1和R_cm2,t_cm2分别为两幅视图下相机坐标系相对于第i个标志器坐标系O_mi-X_miY_miZ_mi的旋转矩阵和平移向量；

三维空间点P＝[x,y,z,1]^T在两幅图像上的投影点分别为p₁＝[u₁,v₁,1]^T和p₂＝[u₂,v₂,1]^T，由p₁＝M₁P,p₂＝M₂P，得到：

其中A为P左侧的系数矩阵，通过最小二乘法LSM得到三维空间点P在第i个标志器坐标系下的三维坐标，计算椭圆上的五个特征点在各标志器坐标系下的三维坐标。

9.根据权利要求1所述的基于标志器的非合作目标位姿测量方法，其特征在于，步骤(8)中标志器定位的步骤如下：

81)根据三角测量恢复出的特征点在第i个标志器坐标系下的三维坐标，计算各特征点在目标器坐标系下的三维坐标，即：

其中，和分别表示椭圆中心、上顶点、下顶点、左顶点和右顶点在目标器坐标系下的三维坐标，和分别表示上下左右四顶点和椭圆中心在第i个标志器坐标系下的三维坐标，Dis(·)表示计算两个三维点的欧式距离，s表示对接环的半径大小；

82)迭代最近点求解位姿，由星箭对接环上五个特征点在第i个标志器坐标系下的三维坐标和目标器坐标系下的三维坐标，基于最近点迭代ICP算法计算使得下列目标函数达到最小值时的旋转矩阵R和平移向量t：

式中，J为目标函数，反映累计的重投影误差大小，||·||₂表示求取二范数，R_mt和t_mt分别表示标志器坐标系到目标器坐标系的旋转矩阵和平移向量，表示特征点在标志器坐标系下的三维坐标，表示特征点在目标器坐标系下的三维坐标。

10.根据权利要求1所述的基于标志器的非合作目标位姿测量方法，其特征在于，步骤(9)中目标器位姿解算，相机坐标系到标志器坐标系的变换矩阵为R_cm,t_cm分别为相机坐标系相对于第i个标志器坐标系O_mi-X_miY_miZ_mi的旋转矩阵和平移向量，标志器坐标系到目标器坐标系的变换矩阵为R_mt和t_mt分别表示第i个标志器坐标系到目标器坐标系的旋转矩阵和平移向量，因此相机坐标系到目标器坐标系的变换矩阵为：R_ct,t_ct分别为相机坐标系相对于目标器坐标系的旋转矩阵和平移向量；

X偏移量、Y偏移量和Z偏移量分别为t_ct的三个分量；

旋转矩阵可表示为：

上式中，φ为偏航角，θ为俯仰角，ψ为滚转角，R_z(φ)表示绕z轴的旋转矩阵，R_y(θ)表示绕y轴的旋转矩阵，R_x(ψ)表示绕x轴的旋转矩阵，r_ij(i＝1,2,3；j＝1,2,3)表示旋转矩阵R中的各分量；

由上式得到目标航天器相对于追踪航天器，即相机的姿态参数：

ψ＝a tan2(r₃₂,r₃₃)

φ＝a tan2(r₂₁,r₁₁)

上式中，φ为偏航角，θ为俯仰角，ψ为滚转角，a tan2(y,x)为反正切函数，等价于atan(y/x)，r₁₁,r₂₁,r₃₁,r₃₂,r₃₃为旋转矩阵R对应下标的分量；

追踪航天器相对目标航天器的姿态参数：偏航角φ、俯仰角θ、翻滚角ψ以及X偏移量、Y偏移量、Z偏移量。