CN103512558B - 一种锥形目标双目视频位姿测量方法及靶标图案 - Google Patents

Abstract

本发明涉及一种锥形目标的双目视频位姿测量方法，以及锥形目标的靶标图案。使得锥形目标位于两摄像机的公共视场中，从两摄像机的图像中确定靶标图案的公共子扇区，并提取特征点的图像坐标，再利用三角测量原理求出特征点在摄像机坐标系下的三维坐标，解算出锥形目标的位姿。锥形目标的表面设置靶标图案以圆锥形顶点为中心，将圆锥表面等分成灰黑色间隔的六个子扇区，圆锥顶端区域设置为白色。子扇区内设置编码区域，编码区域的颜色与其所在子扇区的颜色有较大对比度。使靶标图案在两摄像机的公共视场中有3个以上公共特征点。本发明的靶标图案的主特征点分布位置达到最优，使靶标成像对锥形目标位姿变化的响应最强。

Description

一种锥形目标双目视频位姿测量方法及靶标图案

技术领域

本发明涉及一种双目视频位姿测量方法，尤其是一种锥形目标的双目视频位姿测量方法，以及在该方法中应用的锥形目标的靶标图案。

背景技术

炮弹引信及弹丸通常设计成锥形或类锥形，这类目标的位姿测量是目前的难点问题。基于视频的位姿测量技术不接触被测目标、测量精度较高，适合用于锥形目标的位姿测量。实现锥形目标的视频测量，需要目标上有足够特征点，如果目标表面没有足够特征点而又不能安装发光体靶标，就需要在锥形目标表面设计某种靶标图案，利用靶标上的图案所形成的特征点为位姿测量提供特征信息。靶标图案的设计对位姿测量的精度有重要影响。参见张庆君、胡修林、叶斌等的《基于双目视觉的航天器间相对位置和姿态的测量方法》，宇航学报，2008年，29卷（1）：156-161页；孙先逵、秦岚，《一种新型非接触位姿检测***研究》，光电工程，2007年34(1)：50-54页。

现有技术中，常见的靶标为棋盘格靶标，该靶标由黑白相间的方格排列而成，方格的顶点为靶标的特征点。例如中国专利文献CN100449459C的“一种单摄像机虚拟鼠标***的校准方法”：在靶标平面上布置黑白相间的棋盘格，黑方块与白方块公有的顶点称为格点。选取靶面上格点为特征点。其他常见的编码靶标可参见：ARToolKit marker system（http://www.hitl.washington.edu/ar toolkit/）、ARTag marker system（2010年7月IEEE Transactions on Pattern Analysis and Machine Intelligence，第1317到第1324页中的“Designing Highly Reliable Fiducial Markers”），这两种靶标均由方块构成，方块的顶点为靶标的特征点，方块内部为靶标的识别编码。这些靶标易于检测和识别，应用很广泛。

但是，棋盘格靶标没有设计编码，不能实现特征点的自动识别，而且，上述这些靶标都是在平面上设计和使用的平面靶标，很少有专门针对锥形目标设计的靶标。平面靶标，例如棋盘格靶标，在锥形目标上使用时会产生变形，导致特征点定位不准确；另外，平面靶标也不能充分利用锥形目标的顶点等特殊形状特征。若将平面靶标应用在锥形目标上，得到位姿测量结果不准确。因此要实现锥形目标位姿视频测量，需要针对锥形目标特殊的形状结构来设计靶标。

发明内容

为了解决现有技术中存在的上述缺陷，解决平面靶标无法适应锥形目标曲面结构的问题以及提高锥形目标的位姿测量精度，本发明的目的是：在锥形目标的表面设计一种靶标图案，并提供一种锥形目标双目视频位姿测量方法。

本发明解决其技术问题所采取的技术方案是：

一种锥形目标的双目视频位姿测量方法，以及应用于双目视频位姿测量的锥形目标的靶标图案，该测量方法包括以下步骤：

两个摄像机设置在一个基座上，朝向要进行位姿测量的锥形目标，并使得锥形目标位于两摄像机的公共视场中。两摄像机光轴平行使两摄像机的公共视场最大。

将摄像机采集到的图像数据通过图像采集卡发送给计算机，解算锥形目标的位姿参数。

所述锥形目标的表面设置靶标图案，使靶标图案在两摄像机的公共视场中有3个以上公共特征点。

锥形目标上的靶标图案设置为，以锥形目标的圆锥形顶点为中心，将圆锥表面等分成六个子扇区，将所述六个子扇区间隔设置为两种对比度较大的颜色。将锥形目标的顶端区域设置为与所述六个子扇区颜色有较大对比度的第三种颜色。

在所述六个子扇区内分别设置一定数量的编码区域，所述编码区域的颜色与其所在子扇区的颜色有较大对比度。

所述编码区域基本为四边形，编码区域的侧边平行于锥形目标母线；编码区域的上下边平行于锥形目标横截面的截面圆；所述编码区域的侧边与其所在子扇区的侧边相隔离；所述编码区域下边与锥形目标的底面圆相隔离。

同一子扇区内的各个编码区域相隔离。

相邻子扇区内的编码区域数量或者编码区域的面积比设置为不同。

根据所述的锥形目标的靶标图案中子扇区颜色、子扇区内编码区域的数量及同一子扇区内不同编码区域的面积比，为所述的每个子扇区构造唯一的识别编码。

将所述锥形目标的顶点、所述六个子扇区的侧边与锥形目标底面圆的交点作为测量的主特征点。

将所述的六个子扇区内的各个四边形编码区域的顶点作为测量的辅助特征点。

解算所述锥形目标位姿时，首先从两摄像机的图像中确定所述锥形目标的靶标图案的公共子扇区，提取所述公共子扇区中特征点的图像坐标，再利用三角测量原理求出所述的特征点在摄像机坐标系下的三维坐标，解算出锥形目标的位姿。

所述的两摄像机的基线为0.15米～0.35米；摄像机的分辨率像素为768×576以上，视场角为25°～45°，等效焦距为43.456mm～81.193mm。

本发明的方法还可包括，根据摄像机与锥形目标之间的相对距离，将测量分为10米以内的近距离测量和10米以上的远距离测量两种模式：在近距离测量模式中，提取主特征点和辅助特征点来进行位姿解算；在远距离测量模式中，仅利用主特征点来进行位姿解算；两种模式位姿解算公式相同。

由于靶标图案设计在锥形目标的圆锥面上，所以安放摄像机时应保证摄像机能够拍摄到锥形目标的侧面。如果锥形目标的位姿变化幅度较大（如翻转）则需要两套以上的双目视频位姿测量***，以保证摄像机始终能够拍摄到锥形目标的侧面。

由于采用了上述的技术方案，本发明的有益效果是：通过采用本发明的靶标图案，将锥形目标的表面分成六个子扇区，在本发明的双目视频位姿测量方法中，当锥形目标顶端朝向摄像机时，至少会有一个完整的子扇区出现在两摄像机的公共视场内。由于每个子扇区都含有3个主特征点及4个以上辅助特征点，保证了靶标上始终有足够数量（≥3个）的特征点，靶标图案的主特征点分布位置为最优，使靶标成像对锥形目标位姿变化的响应最强，靶标选取顶点为特征点并依据锥面弧形结构设计靶标，充分适应了锥形目标的形状特征。在设计子扇区编码时，考虑了子扇区扇形颜色和包含区域数量等因素，使得子扇区更容易识别。

通过以下结合附图以举例方式对本发明的实施方式进行详细的描述后，本发明的其他特征、特点和优点将会更加的明显。

附图说明

图1是本发明的一种锥形目标的双目视频位姿测量***的构成示意图。

图2是本发明的一种双目视频位姿测量方法的锥形目标上的子扇区划分图（顶视图）。

图3是本发明的一种双目视频位姿测量方法的锥形目标上的子扇区在图像平面上投影成扇形（60°）示意图。

图4是本发明的一种双目视频位姿测量方法的锥形目标上的每个扇形（60°）内设计3个点（O、A、B）示意图。

图5是本发明的一种双目视频位姿测量方法的锥形目标上的每个扇形（60°）内设计4个点（O、A、B、H）示意图。

图6是本发明的一种双目视频位姿测量方法的锥形目标上的靶标图案。

图7是本发明的一种双目视频位姿测量方法的锥形目标上的靶标图案的特征点分布图。

图8是本发明的一种锥形目标的双目视频位姿测量方法中，当锥形目标的中轴线与摄像机图像平面平行时，两摄像机中锥形目标的靶标图案成像示意图。

图9是本发明的一种锥形目标的双目视频位姿测量方法中坐标系的定义方式示意图。

具体实施方式

下面结合附图和一个典型的具体实施方式对本发明的一种锥形目标的双目视频位姿测量方法，以及锥形目标的靶标图案做详细说明。

根据本发明的对锥形目标进行测量的***中，包括两个摄像机，设置有本发明特定的靶标图案的锥形目标，图像采集卡，计算机等构成。其中由摄像机采集到的图像数据通过图像采集卡发送给计算机，再经位姿测量程序解算出锥形目标的位姿参数。

一种锥形目标的双目视频位姿测量方法，包括以下主要的步骤：两个摄像机设置在一个基座上，朝向锥形目标。两摄像机光轴平行使两摄像机的公共视场最大，使锥形目标位于两摄像机的公共视场中。在锥形目标上设计一种靶标图案，专门用于该目标的位姿测量。将摄像机采集到的图像数据通过图像采集卡发送给计算机，提取该锥形目标上的靶标图案的特征点，得到所述特征点的图像坐标，再求得其在摄像机坐标系下的三维坐标，解算出锥形目标的位姿参数。

锥形目标上的靶标图案设置为，以锥形目标的圆锥形顶点为中心，将圆锥表面等分成六个子扇区，子扇区间隔设置为两种对比度较大的颜色；将锥形目标的顶端区域设置为第三种颜色；在所述六个子扇区内分别设置一定数量的编码区域；为每个子扇区构造唯一的识别编码进行特征点的识别。

参见图1，是本发明的锥形目标的双目视频位姿测量***的示意图。本发明的双目视频位姿测量方法主要包括两个摄像机1，2和一个具有特定的靶标图案的锥形目标3，两个摄像机1，2设置在一个基座上，两摄像机光轴平行使两摄像机的公共视场最大。两摄像机朝向锥形目标3，使得锥形目标3位于两摄像机1，2的公共视场中。

所述的两摄像机的基线为0.2米，两摄像机的光轴平行且水平高度相同；摄像机的分辨率像素为768×576，视场角为30°，等效焦距为67.177mm。

由于靶标图案设计在锥形目标3的侧面，所以安放摄像机时应保证摄像机能够拍摄到锥形目标3的侧面。如果锥形目标3的位姿变化幅度较大（如翻转）则需要两套以上的双目视频位姿测量***，以保证摄像机始终能够拍摄到锥形目标3的侧面。

以下对锥形目标上靶标图案的特征点的位置设计进行详细描述。

参见图2，是本发明的一种锥形目标的双目视频位姿测量方法的锥形目标上的子扇区划分图（顶视图）。

解算锥形目标位姿参数需要目标在两摄像机的成像中有3个以上公共特征点。这里的“公共特征点”意思是指不同图像中的特征点，对应实际空间中相同的点。参见图2，将锥形目标侧面划分6个子扇区，以确保在锥形目标运动时，两摄像机视场中始终有一个以上完整的子扇区；以子扇区为靶标设计的基本单元，在每个子扇区内设计3个以上特征点。

由于特征点的位置分布对***的位姿测量有重要影响，需要进行特征点位置设计。特征点在图像平面上的投影点间距越大，姿态角的分辨能力就越强，根据这个原理设计特征点在锥面上的分布位置。由于特征点在图像平面上的投影点间距与目标相对于摄像机姿态有关，为了方便分析，选择锥形目标中轴线垂直于摄像机图像平面时的姿态设计靶标。在该姿态下进行靶标设计，使特征点的投影点间距最大。

参见图3，当锥形目标中轴线垂直于摄像机图像平面时，锥形目标在图像平面上投影成圆形，子扇区投影成顶角为60°的扇形。

在子扇区内设计3个以上特征点并使其图像平面投影点的间距最大。先考虑设计3个特征点，问题转化为：在顶角为60°的扇形内设计3个点使它们之间的最小间距最大（最小间距对靶标图案的姿态角分辨能力具有决定性影响）。

参见图4，表示了本发明的锥形目标上的每个扇形（60°）内设计3个点（O、A、B）示意图。

这三个特征点应位于O、A、B处，最小间距为锥形目标的底面圆半径R，证明：

图4中△OAB为等边三角形，边长为R。EF和CD为扇形内的任意线段，作辅助线AG，使AG//EF，连接AB、OC。根据大角对大边的性质有

∠AGB>60°>∠ABG,∠ODC>60°>∠DOC （1）

|AB|=R>|AG|>|EF|,|OC|=R>|CD| （2）

由式（2）可知线段端点若不在O、A、B三点处，任意两点的间距始终小于R。因此应将3个特征点设计在O、A、B三处。证毕。

添加第4个特征点使其到O、A、B三点的最小间距最大。

参见图5，是本发明的锥形目标上每个扇形（60°）内设计4个点（O、A、B、H）示意图。

其中，第4个特征点H应位于△OAB的中心，证明：

图5中对于△OAB内的任意点I，假设线段AI、BI、OI中AI最短，有

|AK|=|AH|cos∠HAK,|AJ|=|AI|cos∠IAJ （3）

∠HAK>∠IAJ,|AK|>|AJ| （4）

所以|AH|>|AI|，所以第4个特征点H应位于△OAB的中心。证毕。

$\left|\mathrm{AH}\right|=\frac{\sqrt{3}}{3}R\≈0.58R---\left(5\right)$

式（5）中，加入H后，投影点的间距比只有3个特征点时的间距明显减小。增加特征点虽然为位姿解算提供了冗余信息，但却在很大程度上降低了图像对姿态角变化的分辨力，影响了位姿测量精度。考虑到位姿测量精度对***的重要性，在每个子扇区内只设计3个主特征点。

以下结合附图6～8，对锥形目标上靶标图案的设计进行详细描述。

参见图6，是根据本发明的一个具体实施方式的锥形目标上的靶标图案。

根据计算得出的特征点最佳数量及最佳分布位置设计靶标图案。锥形目标3上的靶标图案设置为，以锥形目标3的圆锥形顶点为中心，将圆锥表面等分成六个子扇区，将所述六个子扇区间隔设置为两种对比度较大的黑色和灰色，黑色和灰色两种颜色的子扇区间隔排列。将锥形目标的顶端区域设置为与所述六个子扇区颜色有较大对比度的白色，防止相同颜色的子扇区相连通。

如图6所示的具体实施方式中，六个子扇区内分别有1个或者2个编码区域5。同一子扇区内的各个编码区域5相隔离。

编码区域5大致呈梯形，梯形编码区域5的侧边平行于锥形目标母线；编码区域的上下边平行于锥形目标横截面的截面圆；编码区域5的侧边与其所在子扇区的侧边相隔离；编码区域5的上下边与锥形目标的底面圆相隔离。

子扇区4和子扇区9关于圆锥相对称，子扇区4和子扇区9内各涂有一个编码区域5；子扇区6和子扇区10关于圆锥相对称，子扇区6和子扇区10内各涂有两个编码区域，两编码区域面积比为2:1；子扇区8和子扇区11关于圆锥相对称，子扇区8和子扇区11内也涂有两个编码区域，两编码区域的面积比为1:1。关于圆锥相对称的子扇区，例如子扇区6和10，子扇区4和9内的编码区域可以是对称的，或者面积分别相等，但不对称并不会影响标记效果，因此也可以是不对称的。

编码区域的颜色与所在子扇区颜色不同：灰色子扇区内的编码区域为黑色，黑色子扇区内编码区域为灰色。

本领域技术人员应当认识到，靶标图案的颜色不局限于本具体实施方式的所示的黑、白、灰三种颜色，只要是相差别较大的三种颜色都可以达到本发明的效果。例如红色与白色、蓝色，或者红色与黑色、黄色，或者其它差别较大的三种颜色的组合等。

此外，靶标图案的大扇形的分块数、大小比例可以改变，这种改变以及靶标图案颜色的变化，均与本发明的技术方案有相同的构思，因而在本专利的保护范围之内。

参见图6，各编码区域的侧边沿锥形目标母线，编码区域的上、下弧线为沿锥形目标横截面的圆弧，编码区域与其它区域不连通。以扇形4为例，编码区域的侧边13平行于锥形目标的母线，而编码区域的弧线边缘12为沿锥形目标截面圆的圆弧。编码区域的侧边13与子扇区侧边相隔一段距离，同一子扇区的两个编码区域之间也相隔一段距离，编码区域的下弧线边缘12与锥形目标底面圆的圆弧之间也相隔一段距离。

参见图7，是本发明的锥形目标上的靶标图案的特征点分布图。

由于线的交点定位精度高、抗干扰性好，利用线交点来生成特征点。图7中子扇区内的特征点有：锥形目标的顶点15共1个、锥形目标底面圆与子扇区侧边14交点16共2个、编码区域的顶点17共4个或8个（根据子扇区内的编码区域数量不同，参见图6所示，子扇区4和子扇区9的交点共4个，子扇区6，8和子扇区10，11的交点共8个）。如图7中所示，顶点15和两个底面交点16处于特征点的最佳分布位置，它们生成的图像点对锥形目标姿态角变化的分辨能力最强，这三个属于主特征点。此外，编码区域的顶点17则属于辅助特征点。图7所示的灰色子扇区中，共有2个编码区域，每个编码区域有4个顶点，因此在图示的灰色子扇区中共8个辅助特征点。

以上所有特征点均由直线和曲线相交而成。

下面对本具体实施方式的子扇区编码设计进行描述。

为了识别子扇区，利用子扇区颜色、编码区域数量及编码区域面积比为每个子扇区构造一个唯一的三位二进制识别编码。识别编码为一个三位二进制码，编码第一位用于表示子扇区的两种不同的颜色，编码第二位用于表示子扇区内编码区域的数量，编码第三位用于表示编码区域的面积比。识别特征点时先利用识别编码识别子扇区，然后再根据特征点在子扇区内具***置识别特征点。本具体实施方式的各子扇区编码见下表。

表中：D₂代表子扇区颜色，灰色记为1，黑色记为0。

D₁代表子扇区内编码区域数量，含一个编码区域记为1，含两个编码区域记为0。

D₀代表编码区域面积比，若为2:1则记为1，1:1记为0，只含一个编码区域时记为1。

利用编码识别特征点的步骤为：

（1）首先利用编码识别子扇区；

（2）再根据特征点在子扇区内位置识别特征点。

以上的描述中，各子扇区4，6，8，9，10，11可分别定义为第一，第二，…，第六子扇区。灰色也可定义为第一颜色，黑色定义为第二颜色，而锥形目标顶端的白色定义为第三颜色。“公共子扇区”是指在不同图像中，对应同一实际空间子扇区的成像区域。

参见图8，当锥形目标的中轴线与摄像机图像平面平行时，两摄像机所能拍摄到的锥形目标侧面的公共面积相对于其它姿态最小。在该姿态下两摄像机的图像中有一个公共的子扇区10，子扇区10上有3个主特征点、8个辅助特征点，能够解算出锥形目标位姿。

下面以图8的靶标图像为例，结合本发明测量方法的靶标图案，对解算锥形目标位姿的过程进行详细描述。下面结合本发明的靶标图案进行锥形目标的位姿解算，其中解算原理和公式是本领域的公知技术。解算锥形目标位姿时，首先从两摄像机图像中找到公共子扇区，提取公共子扇区上主特征点和辅助特征点的图像坐标，并利用三角测量原理求出这些特征点在摄像机坐标系下的三维坐标，以解算出锥形目标的位姿。

图8中，左右两幅图分别是两摄像拍摄到的锥形目标图像。两幅图像中有公共的子扇区10，即子扇区为灰色，其中有两个黑色的编码区域5，两编码区域面积比为2:1。

图8中，子扇区10内有3个公共的主特征点、8个公共的辅助特征点。

参见图9，表示是本发明的测量方法中测量***各个坐标系定义方式。为了避免繁琐未画出每个特征点。这11个特征点在以下3个坐标系中的坐标分别为：

（1）锥形目标坐标系O_w-x_wy_wz_w。设公共子扇区10上第i个特征点在此坐标系上的坐标为q_i＝(x_i ^w，y_i ^w，z_i ^w)^T，i＝1...11；

（2）摄像机1坐标系O_c1-x_c1y_c1z_c1，摄像机2坐标系O_c2-X_c2Y_c2Z_c2。设公共子扇区10上第i个特征点在两标系上的坐标分别为 $p_i^{c2}={({x_i}^{c2},{y_i}^{c2},{z_i}^{c2})}^T,i=1\·\·\·11;$

（3）摄像机1图像的像素坐标系O-uv和摄像机2的图像像素坐标系O'-u'v'，以图像平面左上角为原点，u、v轴分别沿图像的横轴和纵轴。设公共子扇区10上第i个特征点在两坐标系上的坐标为分别为(u_i,v_i)^T,i=1...11和(u'_i,v'_i)^T,i=1...11。

提取所述锥形目标的靶标图案的主特征点和辅助特征点的图像坐标后，进行锥形目标的位姿解算：首先求出这些特征点在摄像机坐标系下的三维坐标；联合这些特征点在摄像机坐标系中三维坐标和在锥形目标坐标系中三维坐标建立超定方程组，再求解该超定方程组即得到锥形目标的位姿参数。上述三维坐标、建立超定方程组以及求解，均为本领域的公知技术，带入特征点即可求解。

提取本发明的锥形目标的靶标图案特征点的图像坐标，步骤如下：

（1）获取锥形目标成像区域：摄像机在测量过程中保持不动，在测量开始前分别用两摄像机拍摄一幅不含锥形目标的背景图像，用包含目标的测量图像减去背景图像可获得差值图像，在差值图像中灰度值不为零的区域即为锥形目标的成像区域；

（2）获取子扇区区域：利用阈值分割法将锥形目标中完整的子扇区分割出来，选择两个灰度阈值T1、T2对锥形目标成像区域进行分割，其中T1较小，用于分割黑色子扇区，T2较大，用于分割灰色子扇区，对分割后的图像进行八连通区域分析，将包含像素数量最多的连通区域作为子扇区的图像区域；

（3）拟合顶点坐标：利用先膨胀再减去膨胀前图像的形态学方法提取该连通区域的轮廓，利用Hough变换法从轮廓中提取直线，这里的直线是指子扇区侧边14，求两直线交点可得到顶点坐标；（其中，Hough变换法属于本领域的公知技术。）

（4）拟合底面交点坐标：从在连通区域的轮廓中分离曲线点并拟合椭圆曲线，这里的曲线点是指子扇区的弧形底边上的点，然后分别求上述两直线与该椭圆曲线的交点，即可获得底面交点坐标；（其中，拟合曲线的方法属于本领域的公知技术。）

以上步骤（3）（4）可获得主特征点的坐标；

（5）求辅助特征点坐标：分析子扇区内部的连通区域，可得到编码区域，分离编码区域的侧边并拟合直线，分离编码区域的上下弧线边缘并拟合曲线，求直线和曲线的交点即可获得辅助特征点的坐标。通过以上步骤得到所有特征点在两摄像机图像中的坐标；

（6）子扇区识别：根据子扇区的灰度值、及其包含编码区域的数量及编码区域面积比得到该子扇区的编码，根据编码识别子扇区，再根据特征点在子扇区内的位置识别特征点。

以上记载详细描述了本发明的从图像中确定子扇区并提取特征点的方法步骤，本领域技术人员根据以上的描述中可以明了并实施本发明的相关方案。

完成提取图像坐标后，利用三角测量原理计算特征点在摄像机坐标系下的坐标：从摄像机1坐标系和摄像机2坐标系到像素坐标系O-uv投影矩阵分别为M₁,M₂：

$\begin{array}{c}{M}_1=K_{c1}\left[\begin{array}{cc}I& 0\end{array}\right]\\ M_2=K_{c2}\left[\begin{array}{cc}{R}_{c2}& T_{c2}\end{array}\right]\end{array}---\left(6\right)$

式（6）中K_c1,K_c2为摄像机的内参数矩阵，R_c2和T_c2为摄像机2相对于摄像机1O_c1-X_c1Y_c1Z_c1的旋转矩阵和平移向量。已知公共子扇区10第i个特征点在像素坐标系O-uv中的齐次坐标为(u_i,v_i,1)^T,i=1...11，该特征点在摄像机1坐标系O_c1-x_c1y_c1z_c1中的齐次坐标为根据透视投影原理得到两坐标系的关系为

$\mathrm{\λ}\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=M_1\left[\begin{array}{c}{x}_i^{c1}\\ y_i^{c1}\\ z_i^{c1}\end{array}\right]---\left(7\right)$

同理可得到与对应像素坐标(u_i,v_i,1)^T,i=1...11的关系。令M₁₁、M₁₂、M₁₃为M₁的行向量，M₂₁、M₂₂、M₂₃为M₂的行向量。联立式（6）（7）可得到

$\left[\begin{array}{c}{M}_{13}{u}_i-M_{11}\\ M_{13}{v}_i-M_{12}\\ M_{23}{u}_i^{\′}-M_{21}\\ M_{23}{v}_i^{\′}-M_{22}\end{array}\right]p_i^{c1}=0---\left(8\right)$

式（8）中有四个方程和三个未知数，可以通过最小二乘原理求解出 的值。在得到特征点在摄像机1坐标系O_c1-x_c1y_c1z_c1下的坐标后，再对位姿参数进行求解。

在两摄像机图像中，公共子扇区10上有11个公共的主、辅特征点，通过式（8）得到这些特征点在摄像机1坐标系O_c1-x_c1y_c1z_c1下的坐标由于靶标设计参数均已知，可得特征点在锥形目标O_w-x_wy_wz_w上的坐标Q={q_i}，(i=1...11)。

这两个坐标系关系为

P=sRQ+T （9）

式（9）中s为比例系数；T为锥形目标的位置向量，它是一个三维向量，表示目标位置；R为锥形目标的旋转矩阵，它是一个3×3的单位正交矩阵，表示目标姿态。

如果是理想的测量数据，则能直接求解式（9）得到s、R、T，即锥形目标的位姿。但是实际数据总会存在误差下面用四元数法求解锥形目标的位姿参数。

公共的子扇区10上这11个特征点的质心在摄像机1坐标系和锥形目标坐标系中的坐标分别为

$\begin{array}{cc}\stackrel{\‾}{p}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{p}_i^{c1},& \stackrel{\‾}{q}=\frac{1}{n}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{q}_i---\left(10\right)\end{array}$

特征点在以质心为原点的坐标系下的新坐标为

$\begin{array}{cc}{p}_i^{\′}=p_i^{c1}-\stackrel{\‾}{p},& q_i^{\′}=q_i-\stackrel{\‾}{q}---\left(11\right)\end{array}$

设 $M=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{p^{\′}}_i{{q^{\′}}_i}^T=\left[\begin{array}{ccc}{S}_{\mathrm{xx}}& S_{\mathrm{xy}}& S_{\mathrm{xz}}\\ S_{\mathrm{yx}}& S_{\mathrm{yy}}& S_{\mathrm{yz}}\\ S_{\mathrm{zx}}& S_{\mathrm{zy}}& S_{\mathrm{zz}}\end{array}\right]$

$N=\left[\begin{array}{cccc}{S}_{\mathrm{xx}}+S_{\mathrm{yy}}+S_{\mathrm{zz}}& S_{\mathrm{yz}}-S_{\mathrm{yz}}& S_{\mathrm{zx}}-S_{\mathrm{xz}}& S_{\mathrm{xy}}-S_{\mathrm{yx}}\\ S_{\mathrm{yz}}-S_{\mathrm{yz}}& S_{\mathrm{xx}}-S_{\mathrm{yy}}-S_{\mathrm{zz}}& S_{\mathrm{xy}}+S_{\mathrm{yx}}& S_{\mathrm{zx}}+S_{\mathrm{xz}}\\ S_{\mathrm{zx}}-S_{\mathrm{xz}}& S_{\mathrm{xy}}+S_{\mathrm{yx}}& -S_{\mathrm{xx}}+S_{\mathrm{yy}}-S_{\mathrm{zz}}& S_{\mathrm{yz}}+S_{\mathrm{zy}}\\ S_{\mathrm{xy}}-S_{\mathrm{yx}}& S_{\mathrm{zx}}+S_{\mathrm{xz}}& S_{\mathrm{yz}}+S_{\mathrm{zy}}& -S_{\mathrm{xx}}-S_{\mathrm{yy}}+S_{\mathrm{zz}}\end{array}\right]$

旋转矩阵R所对应的四元数是N的最大特征值所对应的特征向量。令则其表示的旋转矩阵为：

$R=\left[\begin{array}{ccc}{r_0}^2+{r_1}^2-{r_2}^2-{r_3}^2& 2(r_1{r}_2-r_0{r}_3)& 2(r_1{r}_3+r_0{r}_2)\\ 2(r_1{r}_2+r_0{r}_3)& {r_0}^2-{r_1}^2+{r_2}^2-{r_3}^2& 2(r_2{r}_3-r_0{r}_1)\\ 2(r_1{r}_3-r_0{r}_2)& 2(r_2{r}_3+r_0{r}_1)& {r_0}^2-{r_1}^2-{r_2}^2+{r_3}^2\end{array}\right]---\left(12\right)$

得到R后，容易解得：

$s=\frac{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{p^{\′}}_i\·R{q^{\′}}_i}{\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{\left|\right|{p^{\′}}_i\left|\right|}^2}---\left(13\right)$

$T=\stackrel{\‾}{p}+\mathrm{sR}\stackrel{\‾}{q}---\left(14\right)$

由此，就得到锥形目标相对于摄像机1的相对姿态R，相对位置T，得到本发明的锥形目标的位姿。在得到该初始位姿之后，利用光束平差法对初始结果进行优化能得到更准确的解。光束平差法是本领域公知的技术，这里不再详细描述。

应当认识到，以上描述只是本发明的一个特定实施例，本发明并不仅仅局限于以上图示或描述的特定的结构，权利要求将覆盖本发明的实质精神及范围内的所有变化方案。

Claims (7)

1.一种锥形目标的双目视频位姿测量方法，包括以下步骤：

两个摄像机设置在一个基座上，朝向要进行位姿测量的锥形目标，并使得锥形目标位于两摄像机的公共视场中；两摄像机光轴平行使两摄像机的公共视场最大；

所述的两摄像机的基线为0.15米～0.35米；摄像机的分辨率像素为768×576以上，视场角为25°～45°，等效焦距为43.456mm～81.193mm；

将摄像机采集到的图像数据通过图像采集卡发送给计算机，解算锥形目标的位姿参数；

所述锥形目标的表面设置靶标图案，使靶标图案在两摄像机的公共视场中有3个以上公共特征点；

锥形目标上的靶标图案设置为，以锥形目标的圆锥形顶点为中心，将圆锥表面等分成六个子扇区，将所述六个子扇区间隔设置为两种对比度较大的颜色；将锥形目标的顶端区域设置为与所述六个子扇区颜色有较大对比度的第三种颜色；

在所述六个子扇区内分别设置一定数量的编码区域，所述编码区域的颜色与其所在子扇区的颜色有较大对比度；

所述编码区域基本为四边形，编码区域的侧边平行于锥形目标母线；编码区域的上、下边沿锥形目标横截面的截面圆；所述编码区域的侧边与其所在子扇区的侧边相隔离；所述编码区域下边与锥形目标的底面圆相隔离；

同一子扇区内的各个编码区域相隔离；

相邻子扇区内的编码区域数量或者编码区域的面积比设置为不同；

根据所述的锥形目标的靶标图案中子扇区颜色、子扇区内编码区域的数量及同一子扇区内不同编码区域的面积比，给所述的每个子扇区构造唯一的识别编码；

将所述锥形目标的顶点、所述六个子扇区的侧边与锥形目标底面圆的交点作为测量的主特征点；

将所述的六个子扇区内的各个四边形编码区域的顶点作为测量的辅助特征点；

解算所述锥形目标位姿时，首先从两摄像机的图像中确定所述锥形目标的靶标图案的公共子扇区，提取所述公共子扇区中特征点的图像坐标，再利用三角测量原理求出所述的特征点在摄像机坐标系下的三维坐标，解算出锥形目标的位姿。

2.根据权利要求1所述的一种锥形目标的双目视频位姿测量方法，其特征在于，根据摄像机与锥形目标之间的相对距离，将测量分为10米以内的近距离测量和10米以上的远距离测量两种模式：在近距离测量模式中，提取主特征点和辅助特征点来进行位姿解算；在远距离测量模式中，仅利用主特征点来进行位姿解算；两种模式位姿解算公式相同。

3.根据权利要求1或2所述的一种锥形目标的双目视频位姿测量方法，其特征在于，为每个子扇区构造一个唯一的三位二进制识别编码，编码第一位用于表示子扇区的两种不同的颜色，编码第二位用于表示子扇区内编码区域的数量，编码第三位用于表示编码区域的面积比；识别特征点时先利用识别编码识别子扇区，然后再根据特征点在子扇区内具***置识别特征点。

4.根据权利要求3所述的一种锥形目标的双目视频位姿测量方法，其特征在于，提取所述锥形目标的靶标图案特征点的图像坐标的步骤如下：

(1)获取锥形目标成像区域：摄像机在测量过程中保持不动，先分别用两摄像机拍摄一幅不含锥形目标的背景图像，再用包含目标的测量图像减去背景图像可获得差值图像，差值图像中灰度值不为零的区域即为锥形目标的成像区域；

(2)获取子扇区区域：利用阈值分割法将锥形目标成像区域中完整的子扇区分割出来，选择两个灰度阈值T1、T2对锥形目标成像区域进行分割，其中T1较小，用于分割黑色子扇区，T2较大，用于分割白色子扇区，对分割后的图像进行八连通区域分析，将包含像素数量最多的连通区域作为子扇区区域；

(3)拟合顶点坐标：利用先膨胀再减去膨胀前图像的形态学方法提取该连通区域的轮廓，利用Hough变换法从轮廓中提取子扇区侧边上的直线，两直线求交点即可得到顶点坐标；

(4)拟合底面交点坐标：去掉轮廓中子扇区侧边对应的点，用剩余的点拟合椭圆曲线，分别求直线与该曲线的交点，即可获得底面交点坐标；

以上步骤(3)(4)可获得主特征点的坐标；

(5)求辅助特征点坐标：分析子扇区内部的连通区域，可得到编码区域；分离编码区域的侧边并拟合直线，分离编码区域的上下弧线边缘并拟合曲线，求直线和曲线的交点即可获得辅助特征点的坐标；

通过以上步骤得到所有特征点在两摄像机图像中的坐标；

(6)子扇区识别：根据子扇区的灰度值、及其包含编码区域的数量及编码区域面积比得到该子扇区的编码，根据编码识别子扇区，再根据特征点在子扇区内的位置识别特征点。

5.根据权利要求1或4所述的一种锥形目标的双目视频位姿测量方法，其特征在于，所述的两摄像机的基线为0.2米，两摄像机的光轴平行且水平高度相同；摄像机的分辨率像素为768×576，视场角为30°，等效焦距为67.177mm。

6.一种双目视频位姿测量方法中使用的锥形目标的靶标图案，其特征在于：

所述锥形目标的表面设置靶标图案，使靶标图案在两摄像机的公共视场中有3个以上公共特征点；

锥形目标上的靶标图案设置为，以锥形目标的圆锥形顶点为中心，将圆锥表面等分成六个子扇区，将所述六个子扇区间隔设置为两种对比度较大的颜色；将锥形目标的顶端区域设置为与所述六个子扇区颜色有较大对比度的第三种颜色；

在所述六个子扇区内分别设置一定数量的编码区域，所述编码区域的颜色与其所在子扇区的颜色有较大对比度；

所述编码区域基本为四边形，编码区域的侧边平行于锥形目标母线；编码区域的上下边沿锥形目标横截面的截面圆；所述编码区域的侧边与其所在子扇区的侧边相隔离；所述编码区域下边与锥形目标的底面圆相隔离；

同一子扇区内的各个编码区域相隔离；

相邻子扇区内的编码区域数量或者编码区域的面积比设置为不同。

7.根据权利要求6所述的一种双目视频位姿测量方法中使用的锥形目标的靶标图案，其特征在于，所述锥形目标的靶标图案中，六个子扇区间隔设置为对比度较大的黑色和灰色，锥形目标的顶端区域设置为白色；在黑色子扇区中，编码区域设置为灰色；在灰色子扇区中，编码区域设置为黑色。