CN104677340B - 一种基于点特征的单目视觉位姿测量方法 - Google Patents

Abstract

本发明面向实际工程应用，采用合理设计测量系统、充分利用模型约束和冗余特征点互验证等技术手段，提出了一种基于点特征的单目视觉位姿测量方法，可以实现两个物体间相对位置和相对姿态的测量，能够适用于复杂多变的背景和光照条件。本发明包括以下四个步骤：1、以摄像机作为测量传感器，配合特殊的点特征合作标志器组成测量系统；2、利用摄像机摄取包含合作标志器的图像，通过图像处理找到满足标志器模型约束的特征点图像区域组合作为候选标志器；3、利用P3P测量方法实现两个物体间相对位置和相对姿态的计算；4、采用冗余信息互验证方法获得最终测量结果。本发明解决3点特征单目视觉测量的多解问题、计算速度问题和稳定性问题，提高测量精度。

Description

一种基于点特征的单目视觉位姿测量方法

技术领域

本发明属于计算机视觉领域，具体的说涉及一种基于点特征的单目视觉位姿测量方法。

背景技术

随着科学技术的不断发展，两个物体（如飞船、飞机、移动机器人、机械手等）间的相互合作显得越来越重要，越来越多的应用场合需要快速准确地确定两个物体间的三维位姿关系，如空间自主交会对接、机械手的自动抓取操作、目标跟踪等。简单地说，三维位姿测量即测量两个物体坐标系间的相对位置和相对姿态角，分别用三个位置分量（tx,ty,tz）和三个角度分量(ax,ay,az)来表示。为实现三维位姿测量，可以采用超声测量、激光测量、电磁测量、微波测量、GPS测量、视觉测量等。其中视觉测量由于具有被动、非接触、灵活等特点，已成为一个研究热点。常用的视觉三维测量方法包括：双目视觉测量、结构光测量和基于模型的单目视觉测量。其中单目视觉位姿测量更因设备简单、不存在匹配问题、测量精度可以与双目视觉相当而受到高度重视，特别是在空间、水下等对测量设备的重量、体积、功耗具有严格要求的应用场合，单目视觉测量更体现了其优势。

单目视觉由于只有一个摄像机，必须借助于多个特征的组合才能实现测量任务，这些特征的组合构成了合作目标，因此单目视觉位姿测量一般都是基于合作目标的测量。根据所选特征的不同，可分为基于点特征的测量、基于直线特征的测量、基于曲线特征的测量等。由于点特征比其它特征更容易实现，且受光照的影响更小，处理效率更高，在工程上的应用更为普遍。基于点特征的单目视觉测量，至少要有3个特征点，且存在多解问题。为解决多解问题，多采用具有唯一解的4点共面目标，但其测量精度较低。3点目标虽可以达到较高的测量精度，在工程应用中却存在多解、计算复杂等问题。且3点目标的模型约束较为简单，当背景或光照条件复杂时容易造成误识，稳定性较差。

多年来，中国科学院沈阳自动化所结合实际工程应用背景，对基于点特征的单目视觉进行了深入研究，取得了一系列研究成果：周鑫，朱枫发表于《计算机学报》，2003年第12期上的论文“关于P3P问题解的唯一性条件的几点讨论”，从工程应用的角度出发，对P3P问题的多解性进行了分析，得到了当3个控制点构成等腰三角形时，在空间可以找到两个区域，只要摄像机在这些区域内时，可以唯一地求出P3P问题的真实解。王挺等人发表在2006IEEE International Conference on Mechatronics and Automation上的论文“SomeDiscussion on the Conditions of the Unique Solution of P3P Problem”将这样的空间区域扩展到4个，扩大了唯一解的适用范围。朱枫的博士论文《基于模型的单目视觉应用理论研究》对P3P问题的求解方法和鲁棒性进行了深入研究，提出了一种基于几何意义的最优两分迭代法，算法具有收敛快、精度高的特点；得到了在3个特征点构成等腰三角形的条件下，当光轴与合作目标中3个特征点所构成的平面平行时测量精度最高的重要结论。

发明内容

为解决3点目标单目视觉测量的多解问题、计算速度问题和稳定性问题，提高测量精度，本发明面向实际工程应用，采用合理设计测量系统、充分利用模型约束和冗余特征点互验证等技术手段，提出了一种基于点特征的单目视觉位姿测量方法。

本发明为实现上述目的所采用的技术方案是：一种基于点特征的单目视觉位姿测量方法，用于实现两个物体间相对位置和相对姿态的测量，包括以下步骤：

在主动物体上安装摄像机，在被动物体上安装点特征合作标志器；标定出摄像机的图像坐标系与主动物体坐标系间的变换关系，并测量出合作标志器上各特征点在安装点特征合作标志器的被动物体坐标系下的三维坐标；

利用摄像机摄取包含合作标志器的图像，通过图像处理找到满足标志器模型约束的特征点图像区域组合作为候选标志器，并计算出每组候选标志器上各 特征点的图像坐标；

利用P3P测量方法计算主动物体坐标系与被动物体坐标系间相对位置和相对姿态；

采用冗余信息互验证方法获得最终测量结果。

所述点特征合作标志器由一组特征点组成，该组特征点中至少包含两个可计算特征点组合，每个可计算特征点组合包含3个特征点，这三个特征点构成等腰三角形。

所述在主动物体上安装摄像机，在被动物体上安装点特征合作标志器，要求所述合作标志器上的任意一组可计算特征点组合中，其等腰三角形的顶点比底边上的两个点更靠近摄像机。

所述图像处理包括初始捕获、大窗口跟踪和小窗口跟踪三个处理方式；

所述初始捕获方式在整幅图像中搜索目标；

所述大窗口跟踪和小窗口跟踪方式都是在一个限定窗口内搜索目标；

第一次测量为初始捕获，初始捕获成功后进入大窗口跟踪，大窗口跟踪成功后进入小窗口跟踪，只要测量不失败，一直处于小窗口跟踪方式；

小窗口跟踪失败后，先进入大窗口跟踪，大窗口跟踪失败，再进入初始捕获。

所述图像处理包括以下步骤：

采集图像，根据先验知识确定处理窗口；

根据特征点与背景的差异，在处理窗口内进行图像分割，找到所有可能是特征点的连通区域作为特征点候选区域；根据测量系统的实际特点，建立单个特征点的约束，排除不满足单个特征点约束的特征点候选区域；

根据标志器三维模型以及测量过程中6个位姿量的变化范围，建立标志器上各特征点在图像上的相互关系约束，利用这些约束找到满足标志器模型约束的候选特征点图像区域组合作为候选标志器。

所述P3P测量方法包括以下步骤：

根据相机和标志器标定结果计算出各特征点与光心连线间的夹角(θ₁₂,θ₁₃,θ₂₃)、任意两个特征点间的距离(d₁₂,d₁₃,d₂₃)，设各特征点到光心的距离分别为(l₁,l₂,l₃)，则有：

$\left\{\begin{array}{c}{d_{12}}^2={l_1}^2+{l_2}^2-2l_1{l}_2\cos {\mathrm{\θ}}_{12}\\ {d_{13}}^2={l_1}^2+{l_3}^2-2l_1{l}_3\cos {\mathrm{\θ}}_{13}\\ {d_{23}}^2={l_2}^2+{l_3}^2-2l_2{l}_3\cos {\mathrm{\θ}}_{23}\end{array}\right.---\left(1\right)$

该方程采用最优两分迭代法，实现该方程唯一正解的确定；在获得各特征点到光心的距离(l₁,l₂,l₃)后，计算出各特征点在主动物体坐标系下的坐标(x_ai,y_ai,z_ai)；

根据三个特征点分别在主动物体坐标系和被动物体坐标系统下的坐标P_ai=(x_ai,y_ai,z_ai)^T、P_pi=(x_pi,y_pi,z_pi)^T，按照下式求解旋转矩阵R和平移向量T：

P_ai=RP_pi+T （2）

其中的平移向量T即为相对位置，旋转矩阵R可以分解为分别绕X，Y，Z三个轴的旋转角，即姿态角；（2）式求解时，先根据三个特征点在两个坐标系下的三维坐标，分别获得三个单位向量分别在两个坐标系下的表示，进而求出旋转矩阵，再由（2）式求出平移向量。

所述冗余信息互验证方法包括以下步骤：

根据特征点冗余信息验证每个候选标志器的有效性，计算出该候选标志器的测量结果，通过该验证的候选标志器记为有效标志器。

对最终得到的有效标志器测量结果进行比较，得到最终测量结果。

所述验证每个候选标志器的有效性包括以下步骤：

在该候选标志器中，利用全部或高鲁棒性的可计算特征点组合进行验证；

对于选出的任意一组可计算特征点组合，利用P3P测量方法计算出相对位置和姿态；

根据标志器模型可以得到该候选标志器上其余特征点的计算图像坐标，并与实际图像坐标比较得到图像坐标误差值；、

设置一个合理的特征点图像坐标阈值T1，只有所有点的图像坐标误差值都小于阈值时，才说明该组可计算特征点是有效的，得到一组相对位置和姿态。

所述对最终得到的有效标志器测量结果进行比较，得到最终测量结果，具体为：对选出的可计算特征点组合分别进行验证后，统计有效的可计算特征点组数，如果组数为0，则该候选标志器验证结果为无效标志器；否则，验证结果为有效标志器，并将各组相对位置和姿态的均值作为该候选标志器的相对位姿计算结果。

本发明具有以下优点及有益效果：

1.采用基于点特征的合作标志器和单个摄像机实现视觉位姿测量，具有设备简单、测量精度高的特点；

2.通过合理设计合作标志器，以及合作标志器与摄像机间的初始位置关系，不仅保证了测量过程中可以得到唯一的正确解，还能获得较高的测量精度，解决了P3P问题的多解性和鲁棒性问题；

3.通过标志器上冗余特征点的设计，增加目标模型的约束条件；在目标识别和结果验证两个环节，充分利用冗余点信息，提高测量系统的稳定性和测量精度，能够适用于复杂多变的背景和光照条件；

4.在P3P问题求解时，采用收敛速度快的数值解法，提高了求解速度。

附图说明

图1为测量系统安装与坐标系示意图；

图2为合作标志器示意图；

图3为初始位置标志器成像示意图；

图4为靶座安装孔示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明针对3点目标位姿测量在工程应用中遇到的问题，结合前期研究成果，得到了一种解决方案：

1）通过合理设计合作标志器，以及合作标志器与摄像机间的初始位置关系，不仅保证了测量过程中可以得到唯一的正确解，还能获得较高的测量精度，解决了P3P问题的多解性和鲁棒性问题；

2）通过标志器上冗余特征点的设计，增加目标模型的约束条件；在目标识别和结果验证两个环节，充分利用冗余特征点信息，提高测量系统的稳定性和测量精度。

3）在P3P问题求解时，采用收敛速度快的数值解法，提高了求解速度。1.测量系统的设计与标定

基于点特征的单目视觉位姿测量方法可以测量两个物体间的相对位置和姿态，测量系统包括摄像机和合作标志器两部分，测量时，摄像机和合作标志器分别安装在不同的物体上。将安装摄像机的物体称为主动物体，安装合作标志器的物体称为被动物体，两个物体都有各自的坐标系，分别定义为主动物体坐标系Oa和被动物体坐标系O_p。位姿测量即测量出这两个坐标系之间的相对位置和姿态。测量系统中的摄像机和合作标志器也有自己的坐标系，即摄像机坐标系O_c和合作标志器坐标系O_o，当测量系统安装在被测物体上后，摄像机坐标系O_c和主动物体坐标系O_a间的关系以及合作标志器坐标系O_o与被动物体坐标系O_p间的关系就固定了。为简便起见，本发明省略坐标系间的转换，在摄像机标定时，直接建立图像坐标系O_i与主动物体坐标系间的变换关系O_a,在合作标志器标定时，直接测量出标志器上各特征点在被动物体坐标系下的三维坐标。图1给出了测量系统的安装和坐标系示意图。

实际工程应用中，测量系统的设计包括摄像机的设计、合作标志器的设计以及两者初始位置的设计三个部分。其中，摄像机采用黑白摄像机，其它参数的设计取决于实际测量任务的需求。

合作标志器的设计包括标志器上特征点构型和尺寸的设计，由于标志器尺寸取决于实际测量任务的需求，这里提供一种标志器上特征点构型的设计方案。本方案的标志器由7个特征点组成（如图2所示），其中，1-3号特征点在同一 平面上，4-7号特征点在同一平面上，1-3号特征点所在平面与4-7号特征点所在平面平行，且高于该平面。7个特征点可以组成8组可计算特征点组合：（2、7、4）、（2、6、5）、（1、6、4），（3、7、5），（2，6，7），（2，4，5），（1、7、5），（3、6、4），每组可计算特征点组合都包含2个底面上的特征点和一个高出的特征点，满足至少包含2组可计算特征点的要求。为使特征点在图像中更容易被识别，采用LED作主动标志，每个特征点就是一个LED灯，而合作标志器的底面采用黑色不反光表面。这样，摄像机采集的图像中，标志器表面为黑色，特征点为白色圆点，便于特征点的识别。

摄像机和合作标志器间初始位置的设计依据是要保证合作标志器上的任意一组可计算特征点组合中，其等腰三角形的顶点比底边上的两个点更靠近摄像机，因此，设置其初始位置为当两个物体间三个姿态角都为0时，合作标志器的底面与摄像机的像平面平行，1-3号标志点指向相机镜头，且1-3号标志点连线为垂直直线。初始位置时，标志器成像如图3所示。

摄像机的标定采用内参和外参分别标定的方式，先在实验室标定好相机内参数。在任务现场，将摄像机安装到主动物体上后，再标定摄像机的外参数。采用激光跟踪仪测量合作标志器上各标志点在被动物体坐标系下的坐标，为此，在合作标志器上设计3个安装激光跟踪仪靶座的安装孔（如图4中的8、9、10），在实验室，精确标定出合作标志器上7个特征点以及3个靶座安装孔在合作标志器坐标系下的三维坐标。在实验现场，将合作标志器安装在被动物体上以后，利用激光跟踪仪测量出3个靶座安装孔在被动物体坐标系下的三维坐标，并计算出标志器上的7个特征点在被动物体坐标系下的三维坐标。

2.图像处理

图像处理的目的是找到满足标志器模型约束的特征点图像区域组合作为候选标志器，并计算出每个候选标志器上各特征点的图像坐标。

根据是目标搜索窗口的不同，图像处理分为初始捕获、大窗口跟踪和小窗口跟踪三种处理方式。第一次测量时，由于没有任何先验知识，只能在整幅图 像中搜索目标，为初始捕获处理。初始捕获成功后，已知了上一帧图像中特征点的位置，可以在有限的跟踪窗口中搜索目标。由于初始捕获时间较长，初始捕获成功后先进入大窗口跟踪处理，大窗口跟踪成功后，再进入小窗口跟踪处理，只要测量不失败，一直处于小窗口跟踪方式。小窗口跟踪失败后，先进入大窗口跟踪，大窗口跟踪失败，再进入初始捕获方式。

无论是哪种方式，图像处理都包括如下几个步骤：

1）采集图像，根据先验知识确定处理窗口；

2）在处理窗口内进行图像分割，找到所有可能是特征点的连通区域作为特征点候选区域；

3）根据单个特征点的约束，排除不满足单个特征点约束的特征点候选区域；

4）利用标志器上各特征点在图像上的相互关系约束，找到满足标志器模型约束的候选特征点图像区域组合作为候选标志器。

处理窗口分为三种：初始捕获窗口、跟踪大窗口和跟踪小窗口。其中，初始捕获窗口为整幅图像，跟踪大窗口和跟踪小窗口都以上一帧图像中特征点位置为中心，向四周扩展一定的宽度和高度来得到。根据跟踪处理时间、上一帧图像标志器成像的大小，再结合两个物体间的运动速度来确定跟踪小窗口的扩展范围。根据初始捕获时间、上一帧图像标志器成像的大小，再结合两个物体间的运动速度来确定跟踪大窗口的扩展范围。

由于标志点成像灰度值远高于标志器背景灰度值，采用阈值法进行图像分割，利用区域生长法得到处理窗口内所有可能是特征点的连通区域。采用区域的面积、面积/周长²、宽度、高度、灰度分布均匀性等约束作为单个特征点的约束。

根据各特征点在图像上的相互关系约束，寻找候选标志器的过程在初始捕获阶段和跟踪阶段是不同的。跟踪阶段已经知道了上一帧图像中7个特征点的位置，直接以这些位置为中心，根据两物体运动速度确定匹配窗口，在匹配窗口内寻找特征点。初始捕获阶段由于没有先验知识，采用假设检验法寻找候选 标志器。假设检验法包括如下步骤：

1）遍历所有特征点候选区域，如遍历完成转5，否则假设任意两个特征点候选区域为特征点4和6，检查两个区域间距离和位置关系是否满足约束，如果不满足，假设错误，继续下一个区域；

2）如满足约束，根据这两个特征点的位置，寻找特征点1和2，如果找不到，假设错误，返回1；

3）如找到，根据1、4、6或2、4、6计算出一组相对位置和姿态，并根据计算的相对位置和姿态推算出其它特征点的图像坐标，分别在这些图像坐标附近寻找标志点候选区域，如找不到，假设错误，返回1；

4）如所有其余特征点都能找到，即认为找到了一组候选标志器，完成记录后返回1；

5）遍历所有特征点候选区域，如遍历完成转9，否则假设任意两个特征点候选区域为特征点5和7，检查两个区域间距离和位置关系是否满足约束，如果不满足，假设错误，继续下一个区域；

6）如满足约束，根据这两个特征点的位置，寻找特征点2和3，如果找不到，假设错误，返回5；

7）如找到，根据2、5、7或3、5、7计算出一组相对位置和姿态，并根据计算的相对位置和姿态推算出其它特征点的图像坐标，分别在这些图像坐标附近寻找标志点候选区域，如找不到，假设错误，返回5；

8）如所有其余特征点都能找到，即认为找到了一组候选标志器，完成记录后返回5；

9）合并前面得到的所有候选标志器，删除重复的。

3.P3P法相对位姿计算

已知3个特征点的图像坐标(u_i,v_i)（i=1，2，3）和在被动物体坐标系下的三维坐标(x_pi,y_pi,z_pi)（i=1，2，3），计算两个物体间相对位置和姿态时，采用P3P位姿计算方法。包括以下步骤：

1）利用朱枫博士论文《基于模型的单目视觉应用理论研究》中提出的基于几何意义的两分迭代算法，计算出各特征点在主动物体坐标系下的坐标(x_ai,y_ai,z_ai)。

2）根据三个特征点分别在主动物体坐标系和被动物体坐标系统下的坐标(x_ai,y_ai,z_ai)、(x_pi,y_pi,z_pi)，按照（2）式求解旋转矩阵R和平移向量T。

其中的平移向量T即为相对位置，旋转矩阵R可以分解为分别绕X，Y，Z三个轴的旋转角，即姿态角。当旋转顺序为X、Y、Z时,计算公式为（3）式，其中a_x,a_y,a_z分别为围绕x,y,z轴沿逆时针方向的转角。

$\left\{\begin{array}{c}{a}_z=-a\tan 2(r_{10},r_{00})\\ a_y=a\tan 2(r_{20},r_{00}\cos \mathrm{\γ}-r_{10}\sin \mathrm{\γ})\\ a_x=a\tan 2(r_{20}\sin \mathrm{\γ}+r_{12}\cos \mathrm{\γ},r_{01}\sin \mathrm{\γ}+r_{11}\cos \mathrm{\γ})\end{array}\right.---\left(3\right)$

为求解位姿关系，先求旋转矩阵R。（2）式不仅对两个坐标系下的特征点成立，对向量也同样成立。当向量为单位向量时，只有旋转而没有平移，即为（4）式。

${\stackrel{\→}{n}}_{\mathrm{ai}}=R{\stackrel{\→}{n}}_{\mathrm{pi}}---\left(4\right)$

其中和分别为同一单位列向量在两个物体坐标系下的表示。当已知3个特征点时，可以通过任意两个特征点的连线求出两个线性无关的单位列向量 和令为所构成的单位列向量，则这三个列向量之间都是线性无关的，且满足（5）式。

$n_a=\left[{\stackrel{\→}{n}}_{a1}{\stackrel{\→}{n}}_{a2}{\stackrel{\→}{n}}_{a3}\right]=R\left[{\stackrel{\→}{n}}_{p1}{\stackrel{\→}{n}}_{p2}{\stackrel{\→}{n}}_{p3}\right]={\mathrm{Rn}}_p---\left(5\right)$

根据（5）式和（2）式可以求出旋转阵R和平移阵T。

$\left\{\begin{array}{c}R=n_a{n}_p^{-1}\\ T=(\underset{i=\mathrm{1,3}}{\mathrm{\Σ}}{P}_{\mathrm{ai}}-{\mathrm{RP}}_{\mathrm{pi}})/3\end{array}\right.---\left(6\right)$

4.冗余信息互验证

经过图像处理得到的候选标志器是否是真正的标志器，需要进行最终确认，即冗余信息的互验证。图像处理得到的候选标志器可能有多组，因此，冗余信 息的互验证包括两个步骤：

1）根据特征点冗余信息验证单个候选标志器的有效性；

2）对有所有有效标志器的测量结果进行比较，给出最终测量结果。

根据特征点冗余信息验证单个候选标志器的有效性即对每一组候选标志器，利用其上的7个标志点进行测量验证，以确认该候选标志器是否能通过验证，通过验证的为有效标志器，没有通过验证的为由噪声干扰形成的无效标志器。单个候选标志器有效性验证包括如下几个步骤：

1）首先根据实际任务需求，设置一个特征点图像坐标通过阈值T1和特征点图像坐标良好阈值T2，其中T2<T1。

2）对该候选标志器的7个标志点，选择（2、7、4）、（2、6、5）、（1、6、4），（3、7、5）共4组可计算特征点组合进行验证。

3）对于选出的任意一组可计算特征点组合，分别利用P3P测量方法计算出相对位置和姿态，根据标志器模型可以得到该候选标志器上其余4个特征点的计算图像坐标，并与实际图像坐标进行比较得到图像坐标误差值，将图像坐标误差值与T1、T2进行比较，判断该组测量结果的状态：如果所有点的图像坐标误差值都小于阈值T2，则该组测量结果为成功，状态为良好；如果所有点的图像坐标误差都小于T1，但有大于T2的，则该组测量结果成功，状态为通过；只要有一个点的图像坐标误差大于T1，则该组测量结果为失败。

4）对4组组合分别进行验证后，统计测量成功的组数，如果组数为0，则该候选标志器为无效标志器。否则，为有效标志器，将各组相对位置和姿态的均值作为该候选标志器的相对位姿计算结果，并进一步判断该有效标志器的测量结果状态是良好还是通过，只有4组测量结果状态都是良好时，该有效标志器测量结果状态才为良好，否则为通过。

对有所有有效标志器的测量结果进行比较时，首先统计有效标志器的数目，根据数目进行如下判断：

1)有效标志器数目为0，最终测量结果为失败。

2)有效标志器数目为1，最终测量结果为成功，输出该有效标志器的测量位置和姿态。

有效标志器数目大于1，比较各标志器的测量结果状态，如果均为良好或均为通过，由于无法判断哪个标志器是正确的，最终测量结果为失败。如果有1个为良好，其余为通过，则最终测量结果为成功，输出测量结果状态为良好的有效标志器的测量位置和姿态。

Claims (7)

1.一种基于点特征的单目视觉位姿测量方法，其特征在于，用于实现两个物体间相对位置和相对姿态的测量，包括以下步骤：

在主动物体上安装摄像机，在被动物体上安装点特征合作标志器；标定出摄像机的图像坐标系与主动物体坐标系间的变换关系，并测量出合作标志器上各特征点在安装点特征合作标志器的被动物体坐标系下的三维坐标；

利用摄像机摄取包含合作标志器的图像，通过图像处理找到满足标志器模型约束的特征点图像区域组合作为候选标志器，并计算出每组候选标志器上各特征点的图像坐标；

利用P3P测量方法计算主动物体坐标系与被动物体坐标系间相对位置和相对姿态；

采用冗余信息互验证方法获得最终测量结果；

所述冗余信息互验证方法包括以下步骤：

根据特征点冗余信息验证每个候选标志器的有效性，计算出该候选标志器的测量结果，通过该验证的候选标志器记为有效标志器；

对最终得到的有效标志器测量结果进行比较，得到最终测量结果；

所述验证每个候选标志器的有效性包括以下步骤：

在该候选标志器中，利用全部或高鲁棒性的可计算特征点组合进行验证；

对于选出的任意一组可计算特征点组合，利用P3P测量方法计算出相对位置和姿态；

根据标志器模型可以得到该候选标志器上其余特征点的计算图像坐标，并与实际图像坐标比较得到图像坐标误差值；

设置一个合理的特征点图像坐标阈值T1，只有所有点的图像坐标误差值都小于阈值时，才说明该组可计算特征点是有效的，得到一组相对位置和姿态。

2.根据权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述点特征合作标志器由一组特征点组成，该组特征点中至少包含两个可计算特征点组合，每个可计算特征点组合包含3个特征点，这三个特征点构成等腰三角形。

3.根据权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述在主动物体上安装摄像机，在被动物体上安装点特征合作标志器，要求所述合作标志器上的任意一组可计算特征点组合中，其等腰三角形的顶点比底边上的两个点更靠近摄像机。

4.根据权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述图像处理包括初始捕获、大窗口跟踪和小窗口跟踪三个处理方式；

所述初始捕获方式在整幅图像中搜索目标；

所述大窗口跟踪和小窗口跟踪方式都是在一个限定窗口内搜索目标；

第一次测量为初始捕获，初始捕获成功后进入大窗口跟踪，大窗口跟踪成功后进入小窗口跟踪，只要测量不失败，一直处于小窗口跟踪方式；

小窗口跟踪失败后，先进入大窗口跟踪，大窗口跟踪失败，再进入初始捕获。

5.根据权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述图像处理包括以下步骤：

采集图像，根据先验知识确定处理窗口；

根据特征点与背景的差异，在处理窗口内进行图像分割，找到所有可能是特征点的连通区域作为特征点候选区域；根据测量系统的实际特点，建立单个特征点的约束，排除不满足单个特征点约束的特征点候选区域；

根据标志器三维模型以及测量过程中6个位姿量的变化范围，建立标志器上各特征点在图像上的相互关系约束，利用这些约束找到满足标志器模型约束的候选特征点图像区域组合作为候选标志器。

6.根据权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述P3P测量方法包括以下步骤：

根据相机和标志器标定结果计算出各特征点与光心连线间的夹角(θ₁₂,θ₁₃,θ₂₃)、任意两个特征点间的距离(d₁₂,d₁₃,d₂₃)，设各特征点到光心的距离分别为(l₁,l₂,l₃)，则有：

$\left\{\begin{array}{c}{d_{12}}^2={l_1}^2+{l_2}^2-2l_1{l}_2{\mathrm{cos\&theta;}}_{12}\\ {d_{13}}^2={l_1}^2+{l_3}^2-2l_1{l}_3{\mathrm{cos\&theta;}}_{13}\\ {d_{23}}^2={l_2}^2+{l_3}^2-2l_2{l}_3{\mathrm{cos\&theta;}}_{23}\end{array}\right.---\left(1\right)$

该方程采用最优两分迭代法，实现该方程唯一正解的确定；在获得各特征点到光心的距离(l₁,l₂,l₃)后，计算出各特征点在主动物体坐标系下的坐标(x_ai,y_ai,z_ai)；

根据三个特征点分别在主动物体坐标系和被动物体坐标系统下的坐标P_ai＝(x_ai,y_ai,z_ai)^T、P_pi＝(x_pi,y_pi,z_pi)^T，按照下式求解旋转矩阵R和平移向量T：

P_ai＝RP_pi+T (2)

其中的平移向量T即为相对位置，旋转矩阵R可以分解为分别绕X，Y，Z三个轴的旋转角，即姿态角；(2)式求解时，先根据三个特征点在两个坐标系下的三维坐标，分别获得三个单位向量分别在两个坐标系下的表示，进而求出旋转矩阵，再由(2)式求出平移向量。

7.根据权利要求1所述的一种基于点特征的单目视觉位姿测量方法，其特征在于，所述对最终得到的有效标志器测量结果进行比较，得到最终测量结果，具体为：对选出的可计算特征点组合分别进行验证后，统计有效的可计算特征点组数，如果组数为0，则该候选标志器验证结果为无效标志器；否则，验证结果为有效标志器，并将各组相对位置和姿态的均值作为该候选标志器的相对位姿计算结果。