CN102607526B - 双介质下基于双目视觉的目标姿态测量方法 - Google Patents

Abstract

本发明公开了一种双介质下基于双目视觉的目标姿态测量方法，主要解决航天器在水下模拟实验时，无法对目标航天器的姿态进行测量的问题。具体过程包括：通过双目摄像机采集目标航天器图像对；对采集的左图像进行harris角点检测，找到投影特征点；采用极线约束法则和金字塔快速匹配方法，找出右图像的匹配投影特征点；利用左右图像的投影特征点计算出对应空间特征点的三维坐标；建立折射模型，根据折射模型修正空间特征点的三维坐标；筛选出位于同一平面的特征点，并根据这些特征点三维坐标准确计算出目标航天器的姿态。本发明用于在航天器水下模拟实验时，通过建立折射模型，准确计算出目标航天器上特征点的三维坐标，并实现对航天器姿态的准确测算。

Description

双介质下基于双目视觉的目标姿态测量方法

技术领域

本发明属于图像信息技术领域，涉及在不同介质中光线传播会产生折射的双介质条件下，对目标物体位姿的测量方法，可用于航天器在水下模拟太空环境中进行实验时，对航天器姿态的测量。

背景技术

航天员进行太空活动是我国载人航天技术发展的一个关键步骤。利用航天员的出舱活动可以完成观测地面目标、在轨维修和组装大型空间设施等任务。但是，航天员的出舱活动又是极其昂贵和带有极大风险性的空间作业任务。美国航宇局的资料表明，空间出舱活动1小时，在地面至少要完成100多小时的模拟练习。在失重状态下，人体不能保持平衡，动作姿态完全失控，全身也难以稳定，呈现飘浮的自由态，身体任何局部姿态的改变，都将有可能引起全身的转动，并且由于航天员进行出舱活动时要通过绳索与航天器连接在一起的，因此航天员的出舱活动会对航天器的运动姿态产生影响。本发明主要研究的是航天员在进行模拟实验时的航天器姿态的测量，并通过分析研究得出的数据对航天员的活动做进一步指导，同时对航天器的姿态进行修正。

一般来说，所谓人机整合舱外活动的模拟失重试验是在地面条件下模拟出一种微重力的环境后，将人、舱外航天服和载人航天器看作为一个系统整体，分析影响人和航天器运动姿态的因素，据此提出相关的设计措施。

评价载人航天器采用的各种机构在失重环境下能否工作正常、性能是否可靠、运动姿态是否符合要求等，但是若要在水下完全逼真地再现空间失重，必须提供六自由度运动，模型和空间物体在大小和形状上要相似，模型的质量和惯性矩必须复现空间运动体的质量和惯性矩，水下模拟的影响因素必须减至可接受的水平。所以对航天器位姿的测量研究，不仅可以检验中性浮力水槽所提供的模拟环境是否符合要求，同时得出的位姿数据对真实情况下航天员的出舱活动，航天器的运行状况分析都具有极为重要的指导意义。

当前对航天器姿态研究的主要方法有：

(1)利用GPS进行目标载体的姿态测量方法，如Interferometric attitudedetermination with the Global Positioning System，Guidance and Control Conference，Palo Alto，Calif，August 7-9，1978，Technical Papers.A78-50159 22-01 New York，American Institute of Aeronautics and Astronautics，Inc.，1978，p.96-102.Researchsupported by Lockheed Missiles and Space；

但是利用GPS对目标载体进行姿态测量时，由于信号在传播过程中受到电离层，对流层，多路径和相对论效应等多方面的影响，对目标载体的定位精度一直很难达到理想的情况。

(2)利用激光雷达跟踪与测量方法，如Ulf Larsson，Johan Forsberg，AkeWernersson，On Robot Navigation Using Identical Landmarks：IntegratingMeasurements from a Time-of-Flight Laser，Proceedings of International Conferenceon Multi-sensor Fusion for Intelligent Systems，pp.17-26，1994；

但是利用激光雷达跟踪方法对目标进行姿态测量时，由于激光雷达的波束极窄，在空间搜索目标非常困难，直接影响对非合作目标的截获概率和探测效率，只能在较小的范围内搜索、捕获目标，并对目标姿态进行测量。

(3)利用双目视觉测量方法，如张庆君胡修林-基于双目视觉的航天器间相对位置和姿态的测量方法，宇航学报第29卷第1期，文章编号1000.1328(2008)。

双目视觉的测量方法，是根据特征点在CCD成像平面上的投影，测出每个特征像点在摄像机成像平面上的二维坐标，经过计算和坐标转换推算出航天器上特征点的三维世界坐标，进而求出航天器姿态。该方法具有结构简单，非接触，精度较高等特点，并且其成本低，处理信息量大，速度快等优点，在反求工程、在线检测、定位导航和姿态测量等领域有着广泛的应用；另外，利用这种方法在当有一个摄像机出现故障后，另一个摄像机可利用单目算法完成测量任务，从而能够提高系统可靠性。但是，目前这种双目视觉测量都是在相同介质下对目标航天器姿态的研究，当在双介质条件下，即双目摄像机在空气中，而目标航天器在水中时，由于光线在不同介质间产生折射，使得测出的目标航天器上特征点的三维坐标值不准确，因而计算出目标物体的姿态也是不准确的，从而无法对航天器进行进一步研究。

发明内容

本发明的目的在于利用折射原理，克服上述目标航天器上特征点的三维坐标测量不准确的问题，提出一种双介质下基于双目视觉的目标姿态测量方法，以准确测量出目标特征点的三维坐标，为后续的航天器姿态计算和研究提供数据。

实现本发明目的的技术方案是：首先通过摄像机标定算法对双目摄像机进行标定，得到摄像机的内外参数，然后利用双目摄像机拍摄出目标航天器的左右两幅图像，对两幅图像进行预处理，角点检测和立体匹配，并找出空间特征点在两幅图像中相匹配的特征点，得出特征点在两幅图像中的像素坐标，通过三维重建方法求出目标特征点在空间中的三维坐标，根据特征点所处的不同位置建立折射模型，利用折射模型修正原有的三维坐标值，通过修正的特征点三维坐标值并利用RANSAC算法计算目标航天器的姿态。具体步骤包括如下：

(1)对左右摄像机进行标定，得出摄像机的内参数和外参数，根据该内外参数计算出左摄像机的内参数矩阵M_l1、外参数矩阵M_l2和投影矩阵M_l，右摄像机内参数矩阵M_r1、外参数矩阵M_r2和投影矩阵M_r；通过左右两个摄像机采集目标物体的图像：设左摄像机采集到的图像为I_l(x，y)，右摄像机采集到的图像为I_r(x′，y′)，并对两幅图像依次进行对比度增强和灰度降低处理；

(2)采用Harris角点检测的方法，找出空间目标在左图像I_l(x，y)中的特征点，并得出左图像中每个特征点的图像坐标p_l＝(x，y)^T；

(3)利用极限约束方法对特征点进行匹配，并通过金字塔匹配算法加快匹配过程，以左图像特征点为模板中心，找出空间目标点在右图像I_r(x′，y′)中与左图像特征点相匹配的特征点，得出特征点在右图像的图像坐p_r＝(x′，y′)^T；

(4)根据左右摄像机的参数和得到的多对匹配特征点的图像坐标进行三维重建，得出空间特征点在世界坐标系下的三维坐标；

(5)根据折射原理，建立三维坐标值的修正模型，利用该修正模型对世界坐标系下特征点的三维坐标进行修正，得出准确的三维坐标值，在目标物体上建立目标坐标系，得出目标坐标系下的三维坐标值；

(6)利用特征点在世界坐标系和目标坐标系下的三维坐标值，通过求解目标坐标系到世界坐标系的旋转矩阵，并采用RANSAC算法确定目标物体的三个姿态角，即俯仰角θ，偏航角φ和滚动角Ψ，因此就得到空间目标物体的姿态。

本发明具有如下优点：

(1)本发明采用双介质下的测量方法，通过建立折射模型能准确计算出水中目标航天器上的特征点的三维坐标值，进而能准确计算出目标航天器的姿态。

(2)本发明由于采用双目立体视觉的测量方法，能够准确地测量出目标位置和姿态参数，并且对于运动的物体，双目立体视觉测量除了能够对单幅图进行分析定位，通常还将考虑运动约束，对物体进行运动估计，既测量得到物体对象的静态三维信息，也能够测量得到物体对象在时空中的变化信息，这是双目视觉测量的最重要优势之一。

(3)本发明采用harris角点提取方法提取特征点，如摄像机标定、三维重建等，都是基于特征点的提取，由于不受摄像机姿态及光照的影响，同时计算简单，算子中只用到灰度的一阶差分，因而操作简单，即使存在有图像的旋转、灰度的变化、噪音影响和视点的变换，它也是最稳定的一种点特征提取算法。

(4)本发明采用金字塔加速匹配方法，它是一种减少匹配时间的有效方法，通过从低分辨率图像进行模板匹配，找出粗匹配点，逐步找到原始图像的精确匹配点，故能够提高匹配效率；

(5)本发明利用RANSAC算法，提取出所有位于目标航天器同一平面内的特征点对，克服了由于随机提取的特征点对不在同一平面内计算姿态而产生的错误，能够精确计算姿态参数。

附图说明

图1是本发明的目标航天器姿态的测量流程图；

图2是本发明的使用的金字塔匹配结构图；

图3是本发明使用的空间特征点在左右图像上投影图；

图4是本发明建立的折射模型图；

图5是用本发明方法采集的双目摄像机拍摄的目标卫星图像对；

图6是用本发明方法对目标卫星图像对的预处理结果；

图7是用本发明方法对左图像进行harris角点检测结果；

图8是用本发明方法得出的右图与左图某一特征点相匹配特征点的结果图；

图9是用本发明方法得到的右图像的匹配特征点的图像；

图10是从多对匹配特征点中随机抽取的四组匹配特征点图像。

具体实施方式

参照图1，本发明的实现步骤如下：

步骤1.标定双目摄像机内外参数，并采集目标图像对。

标定摄像机内外参数的目的是确定图像坐标系uv与空间三维世界坐标系O_W-X_WY_WZ_W之间的对应关系，利用该对应关系，从已知空间特征点在图像坐标系uv下的像素坐标来恢复其在世界坐标系下的三维坐标值，其实现步骤是：

(1.1)采用平板标定法对双目摄像机进行标定，得出左右摄像机的内参数和外参数，并根据内外参数计算出左摄像机的内参数矩阵M_l1、外参数矩阵M_l2和投影矩阵M_l，右摄像机内参数矩阵M_r1、外参数矩阵M_r2和投影矩阵M_r。

本发明中采用平板标定法进行摄像机标定，具体实现步骤如下：

(1)通过移动摄像机或标定板获得15-20组不同角度下的标定板图像；

(2)检测图像中的特征点，并求解左右摄像机所有内外参数；

(3)用最小二乘法来精确求解所有的参数，并根据解出的参数，计算出左右摄像机的内外参数矩阵和投影矩阵。

(1.2)利用双目摄像机采集目标图像对，其中左摄像机采集到的图像为I_l(x，y)，右摄像机采集到的图像为I_r(x，y)，由于光照亮度等因素影响，需要分别对两幅图像进行对比度增强和灰度降低的处理，从而简化目标航天器的背景，便于特征点的检测。

步骤2.提取目标图像的Harris角点特征：

(2.1)利用式： $\left\{\begin{array}{c}{I}_x=I_l\⊗{(-\mathrm{1,0,1})}^T\\ I_y=I_l\⊗{(-\mathrm{1,0,1})}^T\end{array}\right.$ 计算左图像I_l(x，y)的梯度图像，

其中

表示卷积运算，I_x为水平梯度图像，即左图像在x方向的一阶偏导数，I_y为垂直梯度图像，即左图像在y方向上的一阶偏导数，T代表转置；

(2.2)利用水平梯度图像I_x和垂直梯度图像I_y构造自相关矩阵：

$M=w\⊗\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]=\left[\begin{array}{cc}A& C\\ C& B\end{array}\right]---1)$

其中w＝exp(-(x²+y²)/2σ²)是高斯平滑窗函数，w中的x，y是高斯窗函数内点的横纵坐标值，σ是尺度空间坐标，A是高斯平滑窗函数同图像在x方向上的二阶偏导数的卷积，B是高斯平滑窗函数同图像在y方向上的二阶偏导数的卷积，C是高斯平滑窗函数同图像在x和y方向的二阶混合偏导数的卷积，该M矩阵是一个二阶实对称矩阵，必然存在两个特征值λ₁和λ₂，一个点是不是角点与该点的曲率有关，对曲率的分析通过建立该点的自相关矩阵M分析，并转化为对矩阵M特征值λ₁和λ₂的分析；

(2.3)将该点是否为角点的判断转化为对M矩阵的特征值λ₁和λ₂的判断，进一步将其转化为对矩阵M的迹trace(M)和行列式det(M)的计算和判断，其中trace(M)＝λ₁+λ₂＝A+B，det(M)＝λ₁λ₂＝AB-C²，当trace(M)和det(M)满足：det(M)-k·trace²(M)＞T时，则点(x，y)是一个角点，并作为左图的一个特征点，这里T是一个固定的阈值，k是参数，取值为k＝0.04～0.06。

步骤3.根据步骤(2.3)得到左图像的特征点p_l＝(x，y)^T在右图像I_r(x，y)中进行快速立体匹配，该立体匹配的匹配示意图如图5所示，具体步骤：

(3.1)根据得到的左图投影特征点p_l＝(x，y)^T，计算右图像中的极线方程，极线方程如式2)所示：

$p_l^T{\mathrm{Fp}}_r=0---2)$

其中，p_l，p_r分别为目标特征点在左右图像中的坐标，F称为基本矩阵，由摄像机标定的内外参数得到；

(3.2)在左图像中以目标特征点为中心创建的一个邻域，在右图像中沿极线方向创建一个同样大小的邻域，其中心点在极线上；

(3.3)利用金字塔加速方法，加快匹配效率，对步骤2得到的左图投影特征点p_l＝(x，y)^T在右图像I_r(x，y)中进行快速立体匹配，找出右图上的匹配特征点p_r＝(x′，y′)^T。

金字塔是一种减少匹配搜索时间的有效方法，图像的金字塔结构又称为多分辨率结构，其基本思想是对图像进行抽样产生一系列不同分辨率的图像。通过从低分辨率图像开始模板匹配，找出粗匹配点，逐步找到原始图像，即最高分辨率图像的精确匹配点。在抽样过程中，上一级图像中某一位置的像素值是由下一级图像对应的几个位置的像素值通过一定规则计算出来，

本发明抽样方法采用像素平均法。金字塔的底层为原图像对，具有最高的分辨率，顶层具有最低分辨率。

参照图2，本步骤的具体实现如下：

(3.3.1)对待匹配的两幅原图像中2×2领域内的像点灰度值取平均，得到分辨率比原图像低一级的图像；

(3.3.2)对得出的低一级图像中2×2领域内的像点灰度值再取平均，得到分辨率更低一级的图像，依次处理，得到一组呈金字塔式的图像，假设有N级，其中N＝0为原图像；

(3.3.3)从待匹配的两幅图的第三级金字塔图像开始进行匹配搜索，以左图特征点作为邻域窗口的中心，建立一个M×M的邻域窗口，并利用该邻域窗口在右图全图范围内进行匹配搜索，采用相关系数C(x，y)作为匹配是否成立的度量，在右图寻找与左图特征点相匹配的特征点，其中：

$C(x,y)=\frac{1}{{\mathrm{\δ}}_1{\mathrm{\δ}}_2{(M+1)}^2}\underset{c,d=-M}{\overset{M}{\mathrm{\Σ}}}[I_l(x,y)-u_1]\×[I_r(x^{\′}+c,y^{\′}+d)-u_2]---3)$

上式中，I_l(x，y)为左图的特征点p_l＝(x，y)^T处像素灰度值，I_r(x′，y′)为右图内一点p_r＝(x′，y′)^T处像素灰度值，δ₁为左图邻域窗口内像素灰度方差，δ₂为右图邻域窗口内像素灰度方差，M为邻域窗口半径，u₁为左图邻域窗口内像素灰度均值，u₂为右图邻域窗口内像素灰度均值，c是右图点p_r＝(x′，y′)^T的水平方向坐标偏移量，d是右图点p_r＝(x′，y′)^T的垂直方向坐标偏移量，c和d的取值范围是-M到M，若相关系数C(x，y)取得最小值，则右图内这一点p_r＝(x′，y′)^T为匹配特征点；

(3.3.4)对低一级分辨率的右图像进行匹配搜索后，找到粗匹配点，并在高一级分辨率右图像中继续进行匹配点的搜索，依次类推，逐步找到原始图像，即最高分辨率图像的精确匹配点。

步骤4.空间特征点的三维重建

(4.1)利用步骤1标定得出的摄像机内参数矩阵和外参数矩阵，表示图像坐标系o-xy与三维空间下的世界坐标系O_W-X_WY_WZ_W之间的对应关系：

$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\α}}_x& 0& u_0& 0\\ 0& {\mathrm{\α}}_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M1M2\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=M\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---4)$

其中：M₁为摄像机内参数矩阵，M₂为摄像机外参数矩阵，M是3行4列的投影矩阵，M＝M₁×M₂；α_x，α_y，u₀，v₀是摄像机的内参数，α_x＝f/d_x，α_y＝f/d_y，(u₀，v₀)是图像坐标系的原点，f为摄像机焦距，d_x，d_y分别是图像坐标系下相邻像素在x轴和y轴方向上的物理尺寸；t是外参数t_x，t_y，t_z构成的平移矩阵，t＝(t_x，t_y，t_z)^T，R是由摄像机外参数α，β，γ构成的3×3正交单位矩阵，该矩阵表示的是世界坐标系和摄像机坐标系的旋转关系，

$R=R(\mathrm{\α},\mathrm{\β},\mathrm{\γ})=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\γ}& -\sin \mathrm{\γ}& 0\\ \sin \mathrm{\λ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right]---5)$

其中，α是世界坐标系X轴绕摄像机坐标系x轴旋转的角度，β是世界坐标系Y轴绕绕摄像机坐标系y轴旋转的角度，γ是世界坐标系Z轴绕摄像机坐标系z轴旋转的角度。

(4.2)参照图3，P_i是一空间特征点，P_l是空间特征点P_i在左图上的投影特征点，P_r是空间特征点P_i在右图上的投影特征点，通过对左摄像机的标定获得其投影矩阵M₁，利用Harris角点检测方法提取出空间特征点P_i在左图的投影特征点P_l的图像坐标为(x，y)，并对式4)进行改写，得出左图像中，空间特征点P_i的世界坐标(X_i，Y_i，Z_i)与左图投影特征点P_l的图像坐标(x，y)的关系为：

$\left\{\begin{array}{c}({\mathrm{xm}}_{31}^1-m_{11}^1)X_i+({\mathrm{xm}}_{32}^1-m_{12}^1)Y_i+({\mathrm{xm}}_{33}^1-m_{13}^1)Z_i=m_{14}^1-{\mathrm{xm}}_{34}^1\\ ({\mathrm{ym}}_{31}^1-m_{21}^1)X_i+({\mathrm{ym}}_{32}^1-m_{22}^1)Y_i+({\mathrm{ym}}_{33}^1-m_{22}^1)Z_i=m_{24}^1-{\mathrm{ym}}_{34}^1\end{array}\right.---6)$

其中x，y为左图像中投影特征点P_l的图像坐标，X_i，Y_i，Z_i为空间特征点P_i的世界坐标，i＝1，...，3，j＝1，...，4，表示投影矩阵M_l的第i行第j列元素；

(4.3)通过对右摄像机标定获取其投影矩阵M_r，并根据金字塔匹配方法得到空间特征点P_i在右图像中的投影特征点P_r的图像坐标(x′，y′)，并对式4)进行改写，得出右图像中，空间特征点P_i的世界坐标(X_i，Y_i，Z_i)与右图投影特征点P_r的图像坐标(x′，y′)的关系为：

$\left\{\begin{array}{c}(x^{\′}{m}_{31}^2-m_{11}^2)X_i+(x^{\′}{m}_{32}^2-m_{12}^2)Y_i+(x^{\′}{m}_{33}^2-m_{13}^2)Z_i=m_{14}^2-x^{\′}{m}_{34}^2\\ (y^{\′}{m}_{31}^2-m_{21}^2)X_i+(y^{\′}{m}_{32}^2-m_{22}^2)Y_i+(y^{\′}{m}_{33}^2-m_{22}^2)Z_i=m_{24}^2-y^{\′}{m}_{34}^2\end{array}\right.---7)$

其中x′，y′为右图像中投影特征点Pr的图像坐标X_i，Y_i，Z_i为空间特征点P_i的世界坐标，

i＝1，...，3，j＝1，...，4，表示投影矩阵M_r的第i行第j列元素；

(4.4)联立6)，7)两式，可将上面两式改写为矩阵形式：

MW＝U                                     8)

其中，M是6)，7)两式中空间特征点P_i的世界坐标X_i，Y_i，Z_i前面系数构成的矩阵，W是空间特征点的世界坐标X_i，Y_i，Z_i组成的矩阵，U是6)，7)两式中等号右面组成的矩阵，由于数据会有噪声，因此利用最小二乘法可求出三维坐标W：

W＝(M^T M)^-1 M^T U                          9)

重复步骤(4.2)-(4.3)计算出其余空间特征点的三维坐标X_i，Y_i，Z_i。

步骤5.修正空间特征点的三维坐标值。

由于目标航天器在水中，而摄像机在空气中，因此通过步骤1-5计算出卫星上每个特征点的三维世界坐标值不准确，需要建立折射模型对每个空间特征点的三维坐标值进行修正，折射模型参照图4，以下是空间特征点的三维坐标值的修正步骤：

(5.1)根据已经计算出的空间特征点P_i的三维世界坐标(X_i，Y_i，Z_i)，计算该点的修正的三维坐标值(X_wi，Y_wi，Z_wi)：

(5.1.1)计算X轴方向的修正坐标为：X_wi＝a₁-w₁+w₃，

其中：w₁是a₁在折射平面的投影，w₁＝a₁×(c/h)；a₁是已经测量出的空间点P的X坐标值，c＝h-b，h是已经测量出的空间点P的Z坐标值，b是右摄像机光心o₁到折射平面的距离；w₃是右摄像机光线经过折射后空间点P_i沿X轴方向坐标的偏移量，w₃＝tan(β₆)/(tan(β₅)+tan(β₆))×(w₁+w₂)，β₅是右摄像机反射光线经折射后的出射角，β₅＝arcsin[sin(β₃)/1.33]，1.33是光从空气进入水中的折射率，β₃是入射角，β₃＝(π/2)-β₁，β₁是右摄像机反射光线同两摄像机光心o₁ o₂连线的夹角，β₁＝arctan(h/a₁)；β₆是左摄像机反射光线经折射后的出射角，β₆＝arcsin[sin(β₄)/1.33]，1.33是光从空气进入水中的折射率，β₄是入射角，β₄＝(π/2)-β₂，β₂是左摄像机反射光线同两摄像机光心o₁ o₂连线的夹角，β₂＝arctan(h/a₂)；w₂＝a₂×(c/h)，w₂是a₂在折射平面的投影距离，a₂＝d-a₁，d是两个摄像机光心o₁ o₂之间的距离；

(5.1.2)Y轴方向修正值不发生改变，即Y_wi＝Y_i；

(5.1.3)计算Z轴方向修正坐标为：Z_wi＝h₁+b，

其中：h₁＝w₃/tan(β₅)，h₁是折射后P点到折射平面的距离，通过上面计算得到特征点Pi的三维修正坐标(Xwi，Ywi，Zwi)；

(5.2)重复利用步骤(5.1.1)-(5.1.3)，计算出目标物体上其余空间特征点的三维修正坐标。

步骤6.根据已知空间特征点在世界坐标系和目标坐标系下的三维坐标值，求解目标坐标系相对世界坐标系旋转矩阵，求出目标航天器的姿态。

(6.1)求出目标上的特征点在世界坐标系O_W-X_WY_WZ_W的三维坐标后，可以通过在目标上建立目标坐标系来求解两坐标系之间的相对姿态，进而得到目标物体与双目相机的相对姿态。设空间中任意一点Pi在世界坐标系和目标坐标系中的坐标分别为(X_Wi，Y_Wi，Z_Wi)和(X_Ti，Y_Ti，Z_Ti)，则两坐标之间的关系如下式所示：

$\left[\begin{array}{c}{X}_{\mathrm{Ti}}\\ Y_{\mathrm{Ti}}\\ Z_{\mathrm{Ti}}\end{array}\right]={\mathrm{kR}}_{\mathrm{WT}}\left[\begin{array}{c}{X}_{\mathrm{wi}}\\ Y_{\mathrm{wi}}\\ Z_{\mathrm{wi}}\end{array}\right]+t_{\mathrm{WT}}---10)$

其中，R_WT是世界坐标系到目标坐标系的旋转矩阵，t_WT是世界坐标系到目标坐标系的平移向量，k是缩放因子。

设各特征点在目标坐标系下的坐标向量为P_Ti＝(X_Ti，Y_Ti，Z_Ti)(i＝1，2...N)，在世界坐标系下的坐标向量为P_wi＝(X_Ti，Y_Ti，Z_Ti)在得到的多对特征点中，任取其中3个特征点P_T1、P_T2、P_T3。若其中任意三点不共线，则有P_T1-P_T2，P_T3-P_T1，(P_T2-P_T1)×(P_T3-P_T1)线性无关的，可得：

A_T＝[P_T2-P_T3 P_T3-P_T1(P_T2-P_T1)×(P_T3-P_T1)]        11)

同理，在世界坐标系下也可得：

由于刚体变换特征点之间相对位置不变，则有：

A_T＝R_WTA                                         13)

从而得到：

(6.2)按式12)，根据已知特征点在目标坐标系和世界坐标系下的坐标值解出矩阵旋转矩阵RWT，其中：

$R_{\mathrm{WT}}=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\cos \mathrm{\θ}\\ \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---15)$

得到矩阵R_w的三个元素r₁₃，r₁₁，r₂₃；

(6.3)依据坐标系转换公式和航天器姿态定义以及矩阵R_w的三个元素r₁₃，r₁₁，r₂₃，计算出目标航天器姿态的俯仰角θ，滚动角φ，偏航角Ψ：

θ＝arcsin(r₁₃)， $\mathrm{\φ}=\mathrm{arc}\sin \left(\frac{r_{23}}{\cos \mathrm{\θ}}\right),$ $\mathrm{\ψ}=\mathrm{arc}\sin \left(\frac{r_{11}}{\cos \mathrm{\θ}}\right);$

(6.4)由于提取的目标航天器上的特征点是随机的，因此有可能提取到不在同一个平面上的特征点，这样计算出的姿态就会产生错误，所以为了最大限度抑制那些不在航天器同一平面上的离群特征点，即外点，本发明采取了RANSAC随机采样算法对所有特征点对进行抽样计算，筛选出所有位于同一平面内的特征点对，再利用步骤(6.1)-(6.3)的方法计算航天器的姿态，从而能够最大程度精确目标航天器的姿态计算，其筛选步骤如下：

(6.4.1)计算出需要进行抽样的次数M：

$M=\frac{\ln (1-p)}{\ln (1-{(1-\mathrm{\ϵ})}^m)}---16)$

其中P是置信概率，ε是数据错误率，m是样本点个数；

(6.4.2)从已知目标航天器平面上的N对特征点中选取线性不相关的三个点组成一个抽样，并根据步骤(6.1)来计算旋转矩阵R_WT，N＞3；

(6.4.3)从剩余特征点中随机选择一个点，根据式17)计算该点与矩阵R_WT和X_W乘积的残差：

a＝||X_T-R_WTX_W||                   17)

其中α是残差，XT是该点在目标坐标系下的坐标，XW该点在世界坐标系下的坐标，RWT是步骤(6.4.2)计算出的旋转矩阵；

(6.4.4)设定阈值T1＝0.05，如果a＜T1，则认为步骤(6.4.2)中所选的这个点为满足位于同一平面的点，即内点，如果a≥T1，则认为步骤(6.4.3)中所选的点为外点，即不满足估计参数的点，并将这个点去除；

重复步骤(6.4.3)至(6.4.4)直到完成M组抽样计算，得到的一致性集合中的内点，并从中抽样选取三个点，根据步骤(6.1)-(6.2)重新计算R_WT，为减小误差，可以进行多次抽样，分别计算出每次抽样得到的旋转矩阵R_WT，并根据步骤(6-3)算出每次抽样的目标航天器的俯仰角，滚动角和偏航角，计算每个姿态角的平均值，所求出的三个姿态角即为所求的姿态参数。

通过解出的三个姿态角，即得出了目标航天器的姿态。

经过上面一共6个步骤，计算出了航天器在在水下进行模拟试验时的三个姿态角，通过对航天器位姿进行研究，就可以检验中性浮力水槽是否符合要求，同时得出的数据可以在航天员进行出舱活动时对航天器运行状况的影响进行分析，从而对航天器姿态进行调整和改进。

本发明的效果可以通过以下实验进一步说明。

本实验将放入水中的卫星模型，作为目标航天器来进行姿态的计算，图5是双目摄像机拍摄的目标航天器图像对，其中图5(a)是目标航天器图像对的左图像，图5(b)是目标航天器图像对的右图像；图6是对左右图像进行预处理的结果图像对，其中图6(a)是对左图像进行预处理的结果图像，图6(b)是对右图像进行预处理的结果图像；图7是对左图像进行harris角点检测的结果；图8(a)是左图像中任意以特征点，图8(b)是利用极限约束和金字塔加速匹配方法进行立体匹配得到的右图像的匹配特征点；图9是通过步骤(3)所述的方法得出左图在右图中的所有匹配特征点；图10(a)和(b)是在多对匹配特征点中随机提取的四组位于同一平面内并且不相关的匹配特征点。

本实验是以右摄像机光心作为世界坐标系和摄像机坐标系的原点，两摄像机光心距离d是0.079米，右摄像机光心距离折射平面距离b为0.84米；根据左右图像上的四组投影特征点的图像坐标，利用步骤1-4所述的方法，计算出左右图像上的投影特征点对应的空间特征点的三维坐标，并且通过步骤5计算出每一个空间特征点的修正三维坐标值。

如表1，是随机从多组匹配特征点中选出的四组不相关的匹配特征点，并由已知左右图像上的投影特征点的图像坐标，计算出四个空间特征点的未经修正的三维坐标，并按照公式求出修正三维坐标值。

表1目标航天器上特征点的三维坐标实验结果

利用计算出的每个特征点的修正三维坐标，以P点作为目标坐标系的原点，建立目标坐标系，并利用步骤(6.1)-(6.3)给出的方法计算出目标航天器的俯仰角θ，滚动角φ和偏航角Ψ分别为：

θ＝2.35°，φ＝29.14°，Ψ＝27.16°

根据步骤(6-4)所述的RANSAC算法，反复进行四组匹配特征点的抽样和三维坐标的计算，共完成M组抽样，并依据上面给出的算法得出每一组抽样计算出的旋转矩阵和三个旋转角。

最终，根据步骤(6.1)-(6.4)给出的方法计算出目标航天器的俯仰角θ，滚动角φ和偏航角Ψ分别为：

θ＝2.44°，φ＝28.34°，Ψ＝27.03°

根据步骤6计算出的三个姿态角θ，φ和Ψ在实验之前测量的理论值为θ＝2.43°，φ＝29.05°，Ψ＝27.04°相对误差均不超过±1°，可见本发明完全能满足对目标航天器姿态测量的要求。

Claims (5)

1.一种双介质下基于双目视觉的目标姿态测量方法，包括如下步骤：

(1)对左右摄像机进行标定，得出摄像机的内参数和外参数，根据该内外参数计算出左摄像机的内参数矩阵M_l1、外参数矩阵M_l2和投影矩阵M_l，右摄像机内参数矩阵M_r1、外参数矩阵M_r2和投影矩阵M_r；通过左右两个摄像机采集目标物体的图像：设左摄像机采集到的图像为I_l(x,y)，右摄像机采集到的图像为I_r(x',y')，并对两幅图像依次进行对比度增强和灰度降低处理；

(2)采用Harris角点检测的方法，找出空间目标在左图像I_l(x,y)中的特征点，并得出左图像中每个特征点的图像坐标P_l=(x,y)^T；

(3)采用极线约束方法进行特征点匹配，并利用金字塔快速匹配方法，以左图像特征点为模板中心，找出空间目标点在右图像I_r(x',y')中与左图像特征点相匹配的特征点，得出特征点在右图像的图像坐标P_r=(x',y')^T；

(4)根据左右摄像机的参数和多对匹配特征点的图像坐标进行三维重建，得出特征点在空间中世界坐标系下的三维坐标；

(5)根据折射原理，建立三维坐标值的修正模型，利用该修正模型对世界坐标系下的三维坐标进行修正，得出准确的三维坐标值，在目标物体上建立目标坐标系，计算目标坐标系下的三维坐标值，

所述的利用修正模型对世界坐标系下的三维坐标进行修正，按如下步骤进行：

(5a)根据已经计算出的空间特征点P_i的三维世界坐标(X_i，Y_i，Z_i)，计算该点的修正的三维坐标值(X_wi，Y_wi，Z_wi)：

(5a1)计算X轴方向的修正坐标为：X_wi＝a₁-w₁+w₃，

其中：w₁是a₁在折射平面的投影，w₁＝a₁×(c/h)；a₁是已经测量出的空间点P的X坐标值，c=h-b，h是已经测量出的空间点P的Z坐标值，b是右摄像机光心o₁到折射平面的距离；w₃是右摄像机光线经过折射后空间点P沿X轴方向坐标的偏移量，w₃＝tan(β₆)/(tan(β₅)+tan(β₆))×(w₁+w₂)，β₅是右摄像机反射光线经折射后的出射角，β₅＝arcsin[sin(β₃)/1.33],1.33是光从空气进入水中的折射率，β₃是入射角，β₃＝(π/2)-β₁，β₁是右摄像机反射光线同两摄像机光心o₁o₂连线的夹角，β₁=arctan(h/a₁)；β₆是左摄像机反射光线经折射后的出射角，β₆＝arcsin[sin(β₄)/1.33],1.33是光从空气进入水中的折射率，β₄是入射角，β₄＝(π/2)-β₂，β₂是左摄像机反射光线同两摄像机光心o₁o₂连线的夹角，β₂=arctan(h/a₂)；w₂＝a₂×(c/h)，w₂是a₂在折射平面的投影距离，a₂=d-a₁，d是两个摄像机光心o₁o₂之间的距离；

(5a2)Y轴方向修正值不发生改变，即Y_wi=Y；

(5a3)计算Z轴方向修正坐标为：Z_wi＝h₁+b,

其中：h₁＝w₃/tan(β₅)，h₁是折射后P点到折射平面的距离，通过上面计算得到特征点P的三维修正坐标(Xwi，Ywi，Zwi)；

(5b)重复利用上面步骤(5a1)-(5a3)，计算出目标物体上其余空间特征点的三维修正坐标；

(6)根据RANSAC随机采样算法筛选出同一平面内的所有特征点，利用这些特征点在世界坐标系和目标坐标系下的三维坐标值，求解空间目标物体的姿态。

2.根据权利要求1所述的双介质下基于双目视觉的目标姿态测量方法，其中步骤(2)所述的采用Harris角点检测的方法，找出空间目标在左图像I_l(x,y)中的特征点，按如下步骤进行：

(2a)利用式 $\left\{\begin{array}{c}{I}_x=I_l\⊗{(-\mathrm{1,0,1})}^T\\ I_y=I_l\⊗{(-\mathrm{1,0,1})}^T\end{array}\right.$ 计算左图像I_l(x,y)梯度图像，其中

表示卷积运算，I_x为水平梯度图像，即图像在x方向的一阶偏导数，I_y为垂直梯度图像，即图像在y方向上的一阶偏导数；

(2b)利用水平梯度图像I_x和垂直梯度图像I_y构造自相关矩阵：

$M=w\⊗\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]=\left[\begin{array}{cc}A& C\\ C& B\end{array}\right],$ w＝exp(-(x²+y²)2σ²)，是高斯平滑窗函数，w中的x，y是高斯窗函数内点的横纵坐标值，σ是尺度空间坐标，A是高斯平滑窗函数同图像在x方向上的二阶偏导数的卷积，B是高斯平滑窗函数同图像在y方向上的二阶偏导数的卷积，C是高斯平滑窗函数同图像在x和y方向的二阶混合偏导数的卷积，M矩阵是一个二阶实对称矩阵，必然存在两个特征值λ₁和λ₂；

(2c)取图像中任意一点(x,y)，将对该点是否为角点的判断转化为对M矩阵的特征值λ₁和λ₂的判断，并进一步将其转化为对矩阵M的迹trace(M)和行列式det(M)的计算和判断，其中trace(M)=λ₁+λ₂=A+B，det(M)=λ₁λ₂=AB-C²，当trace(M)和det(M)满足：det(M)-k·trace²(M)＞T时，则点(x,y)是一个角点，并作为左图的一个特征点，这里T是一个固定的阈值，k是参数，k＝0.04～0.06。

3.根据权利要求1所述的双介质下基于双目视觉的目标姿态测量方法，其中步骤(3)所述的金字塔加速匹配方法，按如下步骤进行：

(3a)对待匹配的两幅原图像中2×2领域内的像点灰度值取平均，得到分辨率比原图像低一级的图像；

(3b)对得出的低一级图像中2×2领域内的像点灰度值再取平均，得到分辨率更低一级的图像，依次处理，得到一组呈金字塔式的图像，假设有N级，其中N=0为原图像；

(3c)从待匹配的两幅图的第三级金字塔图像开始进行匹配搜索，取M×M的邻域窗口，将左图特征点作为邻域窗口的中心，在右图全图范围内寻找匹配的特征点，特征点匹配采用相关系数C(x,y)作为匹配关系是否成立的度量，其中：

$C(x,y)=\frac{1}{{\mathrm{\δ}}_1{\mathrm{\δ}}_2{(M+1)}^2}\underset{c,d=-M}{\overset{M}{\mathrm{\Σ}}}[I_l(x,y)-u_1]\×[I_r(x\′+c,y\′+d)-u_2]$

上式中，I_l(x,y)为左图的特征点p₁=(x,y)^T处像素灰度值，I_r(x',y')为右图内一点p_r=(x',y')^T处像素灰度值，δ₁为左图邻域窗口内像素灰度方差，δ₂为右图邻域窗口内像素灰度方差，M为邻域窗口半径，u₁为左图邻域窗口内像素灰度均值，u₂为为右图邻域窗口内像素灰度均值，c是右图点P_r=(x',y')^T的水平方向坐标偏移量，d是右图点P_r=(x',y')^T的垂直方向坐标偏移量，c和d的取值范围是-M到M，若相关系数C(x,y)取得最小值，则右图内这一点P_r=(x',y')^T为匹配特征点；

(3d)对低一级分辨率的右图像进行匹配搜索后，找到粗匹配点，并在高一级分辨率右图像中继续进行匹配点的搜索，依次类推，逐步找到原始图像，即最高分辨率图像的精确匹配点。

4.根据权利要求1所述的双介质下基于双目视觉的目标姿态测量方法，其中步骤(4)，按如下步骤进行：

(4a)从左右图像的多对匹配投影特征点中，提取一对特征点P_l，P_r；P_l，P_r对应的空间特征点是P_i，根据摄像机标定得到左摄像机的投影矩阵为M₁，利用Harris角点检测方法提取出空间特征点P_i在左图像中的投影特征点Pl的图像坐标(x,y)，得出左图像中，空间特征点P_i的世界坐标(X_i，Y_i，Z_i)与投影点P_l的图像坐标(x,y)的关系为：

$\left\{\begin{array}{c}({\mathrm{xm}}_{31}^1-m_{11}^1)X_i+({\mathrm{xm}}_{32}^1-m_{12}^1)Y_i+({\mathrm{xm}}_{33}^1-m_{13}^1)Z_i=m_{14}^1-{\mathrm{xm}}_{34}^1\\ ({\mathrm{ym}}_{31}^1-m_{21}^1)X_i+({\mathrm{ym}}_{32}^1-m_{22}^1)Y_i+({\mathrm{ym}}_{33}^1-m_{22}^1)Z_i=m_{24}^1-{\mathrm{ym}}_{34}^1\end{array}\right.$

其中x,y为左图像中投影特征点Pl的横纵图像坐标，X_i，Y_i，Z_i为空间特征点P_i的世界坐标，

i=1,…,3，j=1,…,4，表示投影矩阵M_l的第i行第j列元素；

(4b)根据摄像机标定得到右摄像机的投影矩阵为M_r，并根据金字塔匹配提取到的空间特征点P_i在右图像中的投影点P_r的图像坐标为(x',y')，得出右图像中，空间特征点P_i的世界坐标(X_i，Y_i，Z_i)与投影点P_r的图像坐标(x',y')的关系为：

$\left\{\begin{array}{c}({x\′m}_{31}^2-m_{11}^2)X_i+({x\′m}_{32}^2-m_{12}^2)Y_i+({x\′m}_{33}^2-m_{13}^2)Z_i=m_{14}^2-{x\′m}_{34}^2\\ ({y\′m}_{31}^2-m_{21}^2)X_i+({y\′m}_{32}^2-m_{22}^2)Y_i+({y\′m}_{33}^2-m_{22}^2)Z_i=m_{24}^2-{y\′m}_{34}^2\end{array}\right.$

其中x',y'为右图像中投影特征点p_r的横纵图像坐标，X_i，Y_i，Z_i为空间特征点P_i的世界坐标，

i=1,…,3，j=1,…,4，表示投影矩阵M_r的第i行第j列元素；

(4c)联立上面两式，利用最小二乘法求出X_i，Y_i，Z_i，求出点空间点P_i的三维世界坐标，重复步骤(5a)-(5b)计算出其余空间特征点的三维坐标。

5.根据权利要求1所述的双介质下基于双目视觉的目标姿态测量方法，步骤(6)所述的根据RANSAC随机采样算法筛选出同一平面内的所有特征点，按如下步骤进行：

(6a)根据已知的置信概率P和数据错误率ε，样本点个数m计算出需要进行抽样数量M：

$M=\frac{\ln (1-p)}{\ln (1-{(1-\mathrm{\ϵ})}^m)};$

(6b)从已知目标平面上的N个特征点中随机选择同一平面且线性不相关的三个特征点组成一个抽样集合，N>3，并计算出旋转矩阵：R_WT＝A_TA_W ^-1，

其中：

A_T＝[P_T2-P_T3,P_T3-P_T1,(P_T2-P_T3)×(P_T3-P_T1)]，P_T1,P_T2,P_T3分别是三个特征点在目标坐标系下的坐标，

A_W＝[P_W2-P_W3,P_W3-P_W1,(P_W2-P_w3)×(P_W3-P_W1)]，P_W1,P_W2,P_W3分别是三个特征点在世界坐标系下的坐标；

(6c)从剩余特征点对中随机选择一组点对，计算该点与旋转矩阵R_WT与X_W乘积的残差α：

α＝||X_T-R_WTX_W||；

其中X_T是该点在目标坐标系下坐标，X_W是该点在世界坐标系下的坐标；

(6d)设定阈值T1=0.05，如果α<T1，则认为步骤(6b)中所选的点为满足位于同一平面内的点；如果α>T1，则认为(6c)中所选的点为不满足估计的点，即离群外点；

(6e)重复步骤(6c)至(6d)直到完成M组抽样，即可筛选出位于同一平面内的所有特征点。

Images