CN104504675A - 一种主动视觉定位方法 - Google Patents

Abstract

本发明公开了一种主动视觉定位方法，对合作目标进行适应性设计，使合作目标的形状尺寸和正方形之间的相对位置更有利于合作目标的检测和识别：将三个方块边缘距离大于方块的边长，有效提高角点分类与排序的成功率；以红色图案为背板和三个方形的黄色图案为合作标志共同组成，使得两者更容易区分；居于中间偏上的方形标志偏大一些，这样使得合作标志具有很好的方向性，其余两个方形图案分别居于中间方形图案两侧，在图像处理部分，可根据方形图案的轮廓大小对三个方形图案进行有效辨识；将三个方形的比例设计为4:3，提高检率；本发明采用角点补偿算法对确定缺点的位置，提高角点提取准确率；同时为用户定位提供更多的特征点，由此提高定位精度。

Description

一种主动视觉定位方法

技术领域

本发明涉及视觉导航技术领域，尤其涉及一种主动视觉定位方法。

背景技术

随着用户对导航定位需求的提高，由于卫星信号受到遮挡，传统方式使用GPS在信号遮挡扽该区域定位误差在30米以上，无法有效告知用户当前的位置和相应的导航信息。

视觉合作目标定位方式的主要思想是：首先设计合作目标，然后通过用户视觉传感器获取当前包含合作目标图像，根据设计的合作目标特征利用图像处理技术，得到合作目标特征信息，即角点坐标，最后利用用户坐标系、相机坐标系、像素坐标系以及合作目标的角点坐标之间的相互转换关系计算出用户所需的导航位置和姿态。

2013年西北工业大学睢志佳等人设计了一款三个正方形的合作目标，该合作目标利用图像颜色信息，给合作目标角点检测提供了辅助。该合作目标在检测时拥有一定得优势，但是该合作目标在角点提取时，利用角点与合作目标信息相关进行角点优化，没有考虑角点与边缘、角点与颜色区域的相关关系，容易出现角点检测失败、图像分割以及角点排序错误等问题。

发明内容

有鉴于此，本发明提供了一种主动视觉定位方法，能够实现用户在场景中GPS缺失环境下的高精度定位。

为了解决上述技术问题，本发明是这样实现的：

本发明的一种主动视觉定位方法，包括如下步骤：

步骤1、制作合作目标，并将合作目标设置在场景中；

其中，所述合作目标以红色图案为背板，背板上设置三个黄色正方形作为合作标志；所述背板采用RGB颜色灰度值为(255，0，0)，三个正方形RGB颜色灰度值为(255，255，0)；其中第一正方形和第二正方形以第三正方形的其中一条对角线为对称轴对称分布；所述第三正方形的尺寸大于第一和第二正方形；所述第三正方形和第一正方形及第二正方形的边长比为4:3；三个正方形边界的最小距离大于最小正方形的边长；

步骤2、利用用户图像采集设备获取场景中的合作目标图像，并提取合作目标中的合作标志的角点，获得角点在所述合作目标图像上设置的图像坐标系中的坐标；

步骤3、根据合作标志角点在合作目标坐标系中的坐标和步骤2得到的合作标志角点在图像坐标系中的坐标，得到合作目标坐标系与图像坐标系之间的转换关系；然后再基于已知的图像坐标系与用户图像采集设备坐标系的坐标转换关系，得到用户图像采集设备坐标系与合作目标坐标系的坐标转换关系；最后，基于用户图像设备坐标系与车辆坐标系的坐标转换关系以及合作目标坐标系与地理坐标系的坐标转换关系，得到车辆在地理坐标系中坐标和姿态，由此完成对车辆在场景中的定位。

较佳的，所述背板尺寸为80厘米*120厘米，所述第三正方形的边长为32厘米，第一和第二正方形的边长为24厘米，第一和第二正方形中心距离第三正方形中心距离大于35厘米；所述第三正方形的所述对称轴与所述背板宽度方向中心线重合。

较佳的，所述步骤2包括如下具体步骤：

步骤A0、控制用户图像采集设备实时采集场景中的实景图像，并判断每帧图像中是否含有合作目标：如果有，将含有合作目标的图像定义为图像A，执 行步骤A1；如果没有，返回步骤A0；

步骤A1、提取所述图像A中每个像素点的R、G、B三通道灰度值信息；并将满足如下条件的像素点赋值为255，不满足如下条件的像素点赋值为0，对每个像素点按照上述方法赋值后，再对图像A进行二值化，得到图像A*；

条件为：针对每一个像素点，R通道灰度值大于50，R通道灰度值大于1.5倍G通道灰度值，并且R通道灰度值大于1.3倍B通道灰度值；

步骤A2、对图像A*进行形态学开操作，去除细小的区域；然后再对图像A*进行边缘提取，并进行边缘补偿，得到图像A**；

步骤A3、检测图像A**中所有的边缘序列，进行边缘跟踪，寻找图像A**中的最长边缘，并提取最长边缘上所有像素点中在x轴和y轴的最大值和最小值：最大值xmax,ymax以及最小值xmin,ymin；

步骤A4、在图像A中，依次连接坐标点(xmin,ymin)、(xmin,ymax)、(xmax,ymax)和(xmax,ymin)，得到的区域作为合作目标区域，区域中的图像定义为图像T1；

步骤A5、对图像A进行灰度化，在灰度化后图像A中，依次连接坐标点(xmin,ymin)、(xmin,ymax)、(xmax,ymax)和(xmax,ymin)，得到的区域作为角点检测区域，区域中的图像定义为图像T2；

步骤A6、对于图像T1中的每个像素点，将满足R通道灰度值大于50，G通道灰度值大于50，并且G通道大于1.5倍B通道灰度值的像素点赋值为255，否则赋值为0，通过二值化得到图像T1*；

步骤A7：对图像T1*进行形态学开操作，去除图像T1*中细小的区域，且对T1*进行边缘提取，并进行边缘补偿，得到图像T1**；

步骤A8、对图像T1**进行边缘跟踪，寻找图像T1**中边缘长度最长的前三段边缘作为合作目标中方形的边缘，删除其余边缘；

步骤A9、对图像T2利用Harris角点提取算法进行角点提取，如果提取的角点位于所述合作目标中方形的边缘上，则将满足该条件的角点作为的合作目标角点；

步骤A10、利用Kmeans算法将A9中得到的合作目标角点分为3类，如果存在一类角点个数小于3个的情况，返回步骤A10，继续分类；如果存在一类角点个数等于3个的情况，进行角点补偿，使得每一类都具有4个角点；

步骤A11、根据12个角点的相对位置关系，将每一类角点中的4个角点与所述合作目标的3个正方形的顶点互相对应，得到合作目标角点在图像坐标系中的坐标。

较佳的，所述步骤A0中判断实时图像中是否含有合作目标的具体方法为：

步骤D1：提取当前图像中每个像素的R、G、B三通道信息；

步骤D2:将满足R通道灰度值大于50，R通道灰度值大于1.5倍G通道灰度值，并且R通道灰度值大于1.3倍B通道灰度值的像素点赋值为255，否则赋值为0，然后对当前图像进行二值化；

步骤D3：计算二值化后的图像中灰度值为255的像素点个数，如果像素点个数大于全图所有像素点个数的20％，则认为该当前图像存在合作目标，否则认为合作目标不存在。

较佳的，所述步骤A10中，当存在一类角点个数等于3个的情况时，所述角点补偿方法如下：

步骤B1、针对某一类角点，分别用x1、x2和x3表示3个角点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]   (3) 

分别用y1、y2和y3表示3个角点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]   (4) 

步骤B2、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该类角点在x轴方向存在缺点，执行步骤B3；若y轴方向之差最小，则表明该类角点在y轴方向存在缺点，执行步骤B4；

步骤B3、在x轴方向补充缺点；

SB31、首先判断x轴方向第几个差值最小：

如果第1个差值ΔX1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔX2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔX3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SB32、然后分别计算缺点相对的角点x轴坐标与另外两个角点x轴坐标差值，如果两个差值均小于0，则缺点的x轴坐标大于自身相对角点的x轴坐标；如果两个差值均大于0，则缺点的x轴坐标小于自身相对角点的x轴坐标；

SB33、找到差值ΔX1、差值ΔX2和差值ΔX3中最大的两个差值，并求两个差值的均值

当缺点的x轴坐标大于自身相对角点的x轴坐标，缺点的x轴坐标等于自身相对角点x轴坐标加两倍的差值均值

当缺点的x轴坐标小于自身相对角点的x轴坐标，缺点的x轴坐标等于自身相对角点x轴坐标减两倍的差值均值

所述缺点的y轴坐标等于自身相对角点的y轴坐标；

步骤B4、在y轴方向补充缺点：

SB41、判断y轴方向第几个差值最小：

如果第一个差值ΔY1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔY2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔY3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SB42、然后分别计算缺点相对的角点y轴坐标与另外两个角点y轴坐标差值，如果两个差值均小于0，则缺点的y轴坐标大于自身相对角点的y轴坐标；如果两个差值均大于0，则缺点的y轴坐标小于自身相对角点的y轴坐标；

SB43、找到差值ΔY1、差值ΔY2和差值ΔY3中最大的两个差值，并求两个差值的均值

当缺点位于其相对角点的+y轴方向，缺点的y轴坐标等于其相对角点y轴坐标加两倍的差值均值

当缺点位于其相对角点的-y轴方向，缺点的y轴坐标等于其相对角点y轴坐标减两倍的差值均值

所述缺点的x轴坐标等于自身相对角点的x轴坐标；

由此得到缺点的坐标，实现角点补偿。

较佳的，所述步骤A10中，当存在一类角点个数等于3个的情况时，所述角点补偿方法如下：

步骤C1、针对某一类角点，分别用x1、x2和x3表示3个角点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]   (3) 

分别用y1、y2和y3表示3个角点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]   (4) 

步骤C2、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该类角点在x轴方向存在缺点，执行步骤C3；若y轴方向之差最小，则表明该类角点在y轴方向存在缺点，执行步骤C4；

步骤C3、在x轴方向补充缺点；

SC31、首先判断x轴方向第几个差值最小：

如果第1个差值ΔX1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔX2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔX3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SC32、然后分别计算缺点相对的角点x轴坐标与另外两个角点x轴坐标差值，如果两个差值均小于0，则缺点的x轴坐标大于自身相对角点的x轴坐标；如果两个差值均大于0，则缺点的x轴坐标小于自身相对角点的x轴坐标；

步骤C4、在y轴方向补充缺点：

SC41、判断y轴方向第几个差值最小：

如果第一个差值ΔY1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔY2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔY3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SC42、然后分别计算缺点相对的角点y轴坐标与另外两个角点y轴坐标差值， 如果两个差值均小于0，则缺点的y轴坐标大于自身相对角点的y轴坐标；如果两个差值均大于0，则缺点的y轴坐标小于自身相对角点的y轴坐标；

步骤C5、根据步骤C3或步骤C4确定的缺点与相对角点的相对位置关系，求解如下公式，得到坐标P的最优值：

$\underset{P}{\arg \min }\underset{i,j\∈\left\{\mathrm{1,2}\right\},i\≠j}{\mathrm{\Σ}}{\left|\right|(P-P_i)\·(P_j-P_0)\left|\right|}_F^2$

s.t||P-P₃||₂＝||P₀-P₃||₂且

其中，P₀表示与缺点相对角点p₀坐标，P₁和P₂分别为某类角点中除缺点和角点p₀的另外两个角点的坐标；P₃表示坐标P₁和坐标P₂的连线中心点坐标；表示正整数； 

将坐标P的最优值作为缺点的坐标，实现角点补偿。

较佳的，当场景中在不同位置设置多个合作目标时，在每个合作目标的背板上设置标识，以此区分不同的合作目标。

较佳的，在每个合作目标的背板上设置多个朝向不同的三角形作为标识，所述三角形为绿色图案，RGB颜色灰度值为(0,255,0)；

对合作目标的标识的识别方法如下：

步骤E1、识别三角形的个数；

步骤E2、针对步骤E1中得到的每一个三角形，分别用x1、x2和x3表示三角形3个顶点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]

分别用y1、y2和y3表示三角形3个顶点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]

步骤E3、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x 轴方向之差最小，则表明该三角形朝向x轴正方向或负方向，执行步骤E4；若y轴方向之差最小，则表明该三角形朝向y轴正方向或负方向，执行步骤E5；

步骤E4、若三角形朝向x轴正方向或负方向；

找到两个x轴差值最小的两个三角形顶点，然后判断第三个顶点与前两个顶点的x坐标差值，如果两个差值均小于0，则三角形朝向x轴负方向；如果两个差值均大于0，则三角形朝向x轴正方向；

步骤E5、三角形朝向y轴正方向或负方向：

找到两个y轴差值最小的两个三角形顶点，然后判断第三个顶点与前两个顶点的y坐标差值，如果两个差值均小于0，则三角形朝向y轴负方向；如果两个差值均大于0，则三角形朝向y轴正方向；

步骤E6、对每个三角形均执行步骤E1至E5，得到所有三角形的朝向。

较佳的，在每个合作目标的背板上设置不同个数的三角形，用于识别不同的合作目标。

较佳的，所述三角形个数的识别方法如下：

步骤F1、设置三角形为绿色图案，RGB颜色灰度值分别为(0,255,0)；针对所述步骤A5中的图像T2，遍历每一个像素点，将满足G通道灰度值大于100、G通道大于1.5倍B通道灰度值并且G通道大于1.5倍R通道灰度值像素点点赋值为255，否则赋值为0，通过二值化得到图像T3*；

步骤F2、对图像T3*进行形态学开操作，去除T3*中细小的区域，且对T3*进行边缘提取，并进行边缘补偿，得到图像T3**；

步骤F3、对图像T3**进行边缘跟踪，跟踪边缘个数即为合作目标中三角形个数。

本发明具有如下有益效果：

(1)本发明以视觉导航为使用背景，对合作目标进行适应性设计，使合作 目标的形状尺寸和正方形之间的相对位置更有利于合作目标的检测和识别：①、将三个方块边缘距离大于方块的边长，有效提高角点分类与排序的成功率；②以红色图案为背板和三个方形的黄色图案为合作标志共同组成，使得两者更容易区分；③居于中间偏上的方形标志偏大一些，这样使得合作标志具有很好的方向性，其余两个方形图案分别居于中间方形图案两侧，在图像处理部分，可根据方形图案的轮廓大小对三个方形图案进行有效辨识；④将三个方形的比例设计为4:3，提高检率；

(2)本发明采用角点补偿算法对确定缺点的位置，提高角点提取准确率；同时为用户定位提供更多的特征点，由此提高定位精度；

(3)在合作目标中设置标识，以区分不同的合作目标，由此可在场景中设置多个合作目标，为车辆定位提供更多的选择以及更精确的定位。

附图说明

图1为本发明的合作目标的示意图。

图2为本发明获得的图像A；

图3为本发明中对图像A进行二值化得到的图像A*；

图4为本发明的图像A*边缘提取后结果；

图5(a)为本发明的T1区域图像；图5(b)为T2区域图像；

图6为本发明的图像T1*边缘提取结果；

图7为本发明的T2角点检测结果；

图8为本发明的T2角点分类结果；

图9为本发明排序后的角点图像；

图10为本发明的X方向缺点的两种情况示意图；

图11为本发明的Y方向缺点的两种情况示意图；

图12为本发明的视觉合作目标定位示意图；

图13为本发明的定位方法流程图；

图14为本发明的车载合作目标定位***坐标系转换关系图。

图15为本发明的车载合作目标的角点排序示意图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明的一种用于地下场景车辆的高精度定位方法，如图13所示，包括如下步骤：

步骤1、制作合作目标，并将合作目标设置在场景中；

如图1所示，本发明中的合作目标考虑到了易于识别的特点，设计的合作目标使用分层结构的思想，以红色图案为背板和三个方形的黄色图案为合作标志共同组成，背板采用RGB颜色灰度值分别为(255，0，0)，三个正方形RGB颜色灰度值分别为(255，255，0)。在三个正方形的图案的设计上也有所不同，居于中间偏上的正方形标志偏大一些，这样使得合作标志具有很好的方向性，其余两个正方形图案分别居于中间正方形图案两侧，在图像处理部分，可根据方形图案的轮廓大小对三个方形图案进行有效辨识，同时三个方向之间的距离大于每一个方形的半径，有利于实现角点的位置提取和排序。

三个正方形图案还有一定的比例关系，根据实验分析，大小比4:3时是最优比例，如果此比例过大，小的正方形图案有时会被认为是杂点而无法检测到；若此比例过小，则在不同角度和不同距离进行拍摄时，由于图像畸变，导致正方形图案的轮廓发生改变，无法区分大小正方形图案。

考虑到通用场景入口大小为2米到2.5米，场景内柱子宽度75厘米到100厘米，合作目标设计为背板大小80厘米*120厘米，大的正方形32厘米*32厘 米，大的正方形位于合作目标y中心轴线，小的正方形24厘米*24厘米，位于大正方形两边，距离大正方形中心距离大于35厘米。

步骤2、如图12所示，利用用户图像采集设备获取场景中的合作目标图像，并根据现有技术的方法提取合作目标中的合作标志的角点，获得角点在合作目标图像坐标系中的坐标；

步骤3、根据步骤2得到的合作标志的角点在合作目标图像坐标系中的坐标，得到车辆在地理坐标系中坐标，由此完成对车辆在场景中的定位。

其中，步骤2包括如下具体步骤：

步骤A0、控制用户图像采集设备实时采集场景中的实景图像，如图2所示，并判断每帧图像中是否含有合作目标：如果有，将含有合作目标的图像定义为图像A，执行步骤A1；如果没有，返回步骤A0；

其中，判断实时图像中是否含有合作目标的具体方法为：

步骤D1：提取当前图像中每个像素的R、G、B三通道信息；

步骤D2:将满足R通道灰度值大于50，R通道灰度值大于1.5倍G通道灰度值，并且R通道灰度值大于1.3倍B通道灰度值的像素点赋值为255，否则赋值为0，然后对当前图像进行二值化；

步骤D3：计算二值化后的图像中灰度值为255的像素点个数，如果像素点个数大于占全图像素点个数的20％，则认为该当前图像存在合作目标，否则认为合作目标不存在。

步骤A1、提取所述图像A中每个像素点的R、G、B三通道灰度值信息；并将满足如下条件的像素点赋值为255，不满足如下条件的像素点赋值为0：

条件为：R通道灰度值大于50，R通道灰度值大于1.5倍G通道灰度值，并且R通道灰度值大于1.3倍B通道灰度值，即：

R＞50并且R＞1.5G并且R＞1.3B   (1)；

遍历图像A中的所有像素点，并对每个像素点按照上述方法赋值后，再对图像A进行二值化，得到图像A*，如图3所示；

步骤A2、对图像A*进行形态学开操作，去除细小的区域；然后再对图像A*进行边缘提取，并对提取进行边缘补偿，得到图像A**，如图4所示；

步骤A3、检测图像A**中所有的边缘序列，进行边缘跟踪，寻找图像A**中的最长边缘，并提取最长边缘上所有像素点中在x轴和y轴的最大值和上最小值：最大值xmax,ymax以及最小值xmin,ymin；

步骤A4、在图像A中，依次连接坐标点(xmin,ymin)、(xmin,ymax)、(xmax,ymax)和(xmax,ymin)，得到的区域作为合作目标区域，区域中的图像定义为图像T1，如图5(a)所示；

步骤A5、对图像A进行灰度化，在灰度化后图像A中，依次连接坐标点(xmin,ymin)、(xmin,ymax)、(xmax,ymax)和(xmax,ymin)，得到的区域作为角点检测区域，区域中的图像定义为图像T2，如图5(b)所示；

步骤A6、对于图像T1中的每个像素点，将满足R通道灰度值大于50，G通道灰度值大于50，并且G通道大于1.5倍B通道灰度值的像素点赋值为255，否则赋值为0，通过二值化得到图像T1*；

步骤A7：对图像T1*进行形态学开操作，去除图像T1*中细小的区域，并且对T1*进行边缘提取，并进行边缘补偿，得到图像T1**；

步骤A8、对图像T1**进行边缘跟踪，寻找图像T1**中边缘长度最长的前三段边缘作为合作目标中方形的边缘，删除其余边缘，如图6所示；

步骤A9、对图像T2利用Harris角点提取算法进行角点提取；如果提取的角点位于所述合作目标中方形的边缘上，则将满足该条件的角点作为的合作目 标角点，如图7所示；

步骤A10、利用kmeans算法将A9中得到的合作目标角点分为3类，如果存在一类角点个数小于3个的情况，返回步骤A10，继续分类；如果存在一类角点个数等于3个的情况，进行角点补偿，使得每一类都具有4个角点，如图8所示；为了方便辨别角点，可以对角点进行排序，如图9所示，选取A13中x最小的角点作为第一类点，对应角点序号按顺时针排序为1，2，3，4；选取三类中y最小的角点作为第二类点，对应角点序号按顺时针排序为5，6，7，8；选取三类中y最大的角点作为第三类点，对应角点序号按顺时针排序为9，10，11，12；

步骤A11、根据12个角点的相对位置关系，将每一类角点中的4个角点与所述合作目标的3个正方形的顶点互相对应，得到合作目标角点在图像坐标系中的坐标，具体方法为：

如图15所示，选取合作目标角点中在x轴坐标最小的角点a与第三正方形的一个顶点对应，将与角点a同类的其它三个角点分别与第三正方形的另外三个顶点分别对应；选取合作目标角点中在y轴最小的角点b与第一正方形的一个顶点对应，将与角点b同类的其它三个角点分别与第一正方形的另外三个顶点分别对应；选取合作目标角点中在y轴最大的角点c与第二正方形的一个顶点对应，将与角点c同类的其它三个角点分别与第二正方形的另外三个顶点分别对应。

步骤A12、根据步骤A11得到的合作目标的角点在图像坐标系中坐标，得到车辆当前的位置和姿态。

其中，在步骤A10中，当存在一类角点个数等于3个的情况时，本发明提供了两种角点补偿方法，具体如下：

角点补偿方法一包括：

步骤B1、针对某一类角点，分别用x1、x2和x3表示3个角点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]   (3) 

分别用y1、y2和y3表示3个角点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]   (4) 

步骤B2、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该类角点在x轴方向存在缺点，执行步骤B3；若y轴方向之差最小，则表明该类角点在y轴方向存在缺点，执行步骤B4；

步骤B3、在x轴方向补充缺点，如图10所示；

SB31、首先判断x轴方向第几个差值最小：

如果第1个差值ΔX1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔX2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔX3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SB32、然后分别计算缺点相对的角点x轴坐标与另外两个角点x轴坐标差值，如果两个差值均小于0，则缺点的x轴坐标大于自身相对角点的x轴坐标，即缺点位于其相对角点的+x轴方向；如果两个差值均大于0，则缺点的x轴坐标小于自身相对角点的x轴坐标，即缺点位于其相对角点的-x轴方向；

SB33、找到差值ΔX1、差值ΔX2和差值ΔX3中最大的两个差值，并求两个差值的均值

当缺点的x轴坐标小于自身相对角点的x轴坐标，缺点的x轴坐标等于其相对角点x轴坐标加两倍的差值均值

当缺点位于其相对角点的-x轴方向，缺点的x轴坐标等于其相对角点x轴坐标减两倍的差值均值

所述缺点的y轴坐标等于与其相对角点的y轴坐标；

步骤B4、在y轴方向补充缺点，如图11所示：

SB41、判断y轴方向第几个差值最小：

如果第一个差值ΔY1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔY2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔY3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SB42、然后分别计算缺点相对的角点y轴坐标与另外两个角点y轴坐标差值，如果两个差值均小于0，则缺点的y轴坐标大于自身相对角点的y轴坐标，即缺点位于其相对角点的+y轴方向；如果两个差值均大于0，则缺点的y轴坐标小于自身相对角点的y轴坐标，即缺点位于其相对角点的-y轴方向；

SB43、找到差值ΔY1、差值ΔY2和差值ΔY3中最大的两个差值，并求两个差值的均值

当缺点位于其相对角点的+y轴方向，缺点的y轴坐标等于其相对角点y轴坐标加两倍的差值均值

当缺点位于其相对角点的-y轴方向，缺点的y轴坐标等于其相对角点y轴 坐标减两倍的差值均值

所述缺点的x轴坐标等于与其相对角点的x轴坐标；

由此得到缺点的坐标，实现角点补偿。

角点补偿方法二如下：

步骤C1、针对某一类角点，分别用x1、x2和x3表示3个角点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]   (3) 

分别用y1、y2和y3表示3个角点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]   (4) 

步骤C2、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该类角点在x轴方向存在缺点，执行步骤C3；若y轴方向之差最小，则表明该类角点在y轴方向存在缺点，执行步骤C4；

步骤C3、在x轴方向补充缺点；

SC31、首先判断x轴方向第几个差值最小：

如果第1个差值ΔX1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔX2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔX3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SC32、然后分别计算缺点相对的角点x轴坐标与另外两个角点x轴坐标差值，如果两个差值均小于0，则缺点的x轴坐标大于自身相对角点的x轴坐标， 即缺点位于其相对角点的+x轴方向；如果两个差值均大于0，则缺点的x轴坐标小于自身相对角点的x轴坐标，即缺点位于其相对角点的-x轴方向；

步骤C4、在y轴方向补充缺点：

SC41、判断y轴方向第几个差值最小：

如果第一个差值ΔY1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔY2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔY3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SC42、然后分别计算缺点相对的角点y轴坐标与另外两个角点y轴坐标差值，如果两个差值均小于0，则缺点的y轴坐标大于自身相对角点的y轴坐标，即缺点位于其相对角点的+y轴方向；如果两个差值均大于0，则则缺点的y轴坐标小于自身相对角点的y轴坐标，即缺点位于其相对角点的-y轴方向；

步骤C5、根据步骤C3或步骤C4确定的缺点与相对角点的相对位置关系，求解如下公式，得到缺点的坐标P的最优值：

$\underset{P}{\arg \min }\underset{i,j\∈\left\{\mathrm{1,2}\right\},i\≠j}{\mathrm{\Σ}}{\left|\right|(P-P_i)\·(P_j-P_0)\left|\right|}_F^2$

s.t||P-P₃||₂＝||P₀-P₃||₂且

其中，P₀表示与缺点相对角点位置，P₁和P₂分别为某类角点中除缺点P和角点P₀的另外两个角点；P₃表示角点P₁和角点P₂的连线中心点；表示正整数。 

将坐标P的最优值作为缺点的坐标，实现角点补偿。

另外，当场景中在不同位置设置多个合作目标时，车辆无法自动识别其拍 摄的是哪个合作目标，则无法完成自动定位。本发明为解决该问题，在每个合作目标的背板上设置不同个数的三角形，或者在每个背板上设置多个朝向不同的三角形，以此作为区分合作目标的标志；如图1中，背板的左上角，三角形为绿色图案，RGB颜色灰度值分别为(0,255,0)；

当在合作目标上设置不同个数的三角形作为合作目标标志时，对合作目标标志的识别方法如下：

步骤F1、针对所述步骤A5中的角点检测区域T2，遍历每一个像素点，将满足G通道灰度值大于100、G通道大于1.5倍B通道灰度值并且G通道大于1.5倍R通道灰度值像素点点赋值为255，否则赋值为0，通过二值化得到图像T3*；

步骤F2、对图像T3*进行形态学开操作，去除T3*中细小的区域，对T3*进行边缘提取，并进行边缘补偿，得到图像T3**；

步骤F3、对图像T3**进行边缘跟踪，跟踪边缘个数即为合作目标中三角形个数；

当在合作目标上设置多个朝向不同的三角形作为合作目标标志时，对合作目标标志的识别方法如下：

步骤E1、执行步骤F1至F3，识别出三角形的个数；

步骤E2、针对步骤E1中得到的每一个三角形，分别用x1、x2和x3表示三角形3个顶点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]

分别用y1、y2和y3表示三角形3个顶点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]

步骤E3、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该三角形朝向x轴正方向或负方向，执行步骤E4；若y轴方向之差最小，则表明该三角形朝向y轴正方向或负方向，执行步骤E5；

步骤E4、若三角形朝向x轴正方向或负方向；

找到两个x轴差值最小的两个三角形顶点，然后判断第三个顶点与前两个顶点的x坐标差值，如果两个差值均小于0，则三角形朝向x轴负方向；如果两个差值均大于0，则三角形朝向x轴正方向；

步骤E5、三角形朝向y轴正方向或负方向：

找到两个y轴差值最小的两个三角形顶点，然后判断第三个顶点与前两个顶点的y坐标差值，如果两个差值均小于0，则三角形朝向y轴负方向；如果两个差值均大于0，则三角形朝向y轴正方向；

步骤E6、对每个三角形均执行步骤E1至E5，得到所有三角形的朝向。

其中，步骤3中，如图14所示，得到合作标志的角点在合作目标图像坐标系中的坐标后，根据已知的合作标志角点在合作目标中的坐标(事先已标定)，可得到合作目标与合作目标图像坐标系之间的转换关系；再依次得到合作目标坐标系与摄像机坐标系的转换关系、摄像机坐标系与合作目标坐标系之间的转换关系以及合作目标坐标系与车辆坐标系之间的关系，最后根据合作目标坐标系在地理坐标系中的位置，确定车辆在地理坐标系中的位置。具体的坐标系转换过程如下：

首先定义上述各坐标系：

它是相对于地球表面不动的一种坐标系，原点取自合作目标大方形图案顶点，Ox_g轴位于水平面内，平行于合作目标背板短边且指向前；Oy_g轴位于水平面内，平行于合作目标背板长边且指向右；Oz_g轴垂直于Ox_gy_g平面并指向上，坐 标系符合右手规则。

2、用户坐标系Oxyz

固定在用户体上的一个坐标系，原点O取在用户的重心。x轴与用户纵轴一致，指向用户前方，且处于用户对称平面内；z轴于用户对称平面并指向右方；Oy轴在用户对称平面内且垂直于Ox轴，指向下。

3、摄像机(或用户图像采集设备)坐标系.

摄像机坐标系的原点为摄像机光心，Oz_c轴与摄像机光轴重合，且取拍摄方向为正向，Ox_c、Oy_c轴与图像物理坐标系X、Y轴平行。

4、图像坐标系(u,v)和(X,Y)

图像坐标系分为图像像素坐标系(u,v)和图像物理坐标系(X,Y)，分别定义为：

(i)图像像素坐标系(u,v)：以图像左上角为原点以像素为坐标单位的直角坐标系，u、v分别表示该像素在数字图像中的列数与行数。

(ii)图像物理坐标系(X,Y)：图像物理坐标系是以光栅与像平面的焦点为原点，以毫米为单位的直角坐标系。其中X、Y轴分别与像素坐标系的u、v轴平行。

各坐标系之间的转换关系如下：

1)、车辆坐标系与合作目标坐标系之间的关系：

由于摄像机与车辆固连，车辆坐标系与合作目标坐标系之间的夹角就是摄像机的安装姿态角，又称欧拉角。

(a)俯仰角β：轴Oy与平面Ox_gy_g之间的夹角；

(b)偏航角(方位角)γ：轴Oy在水平面Ox_gy_g上的投影与合作目标纵轴Oy_g 之间的夹角；

(c)滚转角(倾斜角)α：对称面绕轴Oy转过的角度，右滚为正。

2)、合作目标坐标系与摄像机坐标系之间的转换关系

合作目标坐标系中的点到摄像机坐标系的变换可以用一个正交旋转矩阵R和一个平移矩阵T来表示为： $\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R\left[\begin{array}{c}{x}_g\\ y_g\\ z_g\end{array}\right]+T---\left(6\right)$

其中， $R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\α\γ}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}-\cos \mathrm{\β\γ}\sin \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\β}\sin \mathrm{\γ}\\ \cos \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\sin \mathrm{\γ}-\sin \mathrm{\β}\cos \mathrm{\γ}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right],$ T＝[t_x,t_y,t_z]是合作目标坐标系原点在摄像机坐标系中的坐标。

3)图像坐标系与摄像机坐标系之间的转换关系

摄像机坐标系中的物点p在图像物理坐标系中的像点P坐标为：

X＝fx_c/z_c   (7)

Y＝fy_c/z_c   (8)

由上式的图像物理坐标系进一步转化为图像像素坐标系：

u＝-X/d_x+u₀＝-f_xX+u₀＝-f_xfx_c/z_c+u₀   (9) 

v＝Y/d_x+v₀＝f_yY+v₀＝f_yfy_c/z_c+v₀   (10) 

其中f为摄像机焦距，(u₀,v₀)为图像物理坐标系原点在图像像素坐标系中的坐标值，f_x、f_y分别为x_i、y_i方向上的采样频率，即单位长度上的像素个数；f_x、f_y、u₀和v₀四个参数只与摄像机内部结构有关。

4)合作目标坐标系与图像坐标系之间的转换关系

将式(8)、(9)分别带入式(6)、(7)可以得到合作目标坐标系与图像坐标系之间的转换关系：

$\frac{X}{f}=\frac{u_0-u}{f_{x}f}=\frac{r_{11}{x}_g+r_{12}{y}_g+r_{13}{z}_g+t_x}{r_{31}{x}_g+r_{32}{y}_g+r_{33}{z}_g+t_z}---\left(11\right)$

$\frac{Y}{f}=\frac{v_0-v}{f_{y}f}=\frac{r_{21}{x}_g+r_{22}{y}_g+r_{23}{z}_g+t_y}{r_{31}{x}_g+r_{32}{y}_g+r_{33}{z}_g+t_z}---\left(12\right)$

以上两式也称共线方程，必须要求物点、光心和像点这三点在同一直线上，它表示了空间物点坐标(x_g,y_g,z_g)和光心坐标T及光轴角度R和对应像点(u,v)之间的数学关系。根据共线方程，在摄像机内部参数确定的条件下，利用若干个已知物点和相应像点坐标，就可以求解出摄像机在合作目标坐标系中的空间位置和姿态。

假设合作目标中的第i角点在合作目标坐标系下的坐标为  $q_i={\left[\begin{array}{cccc}{x}_g^i& y_g^i& z_g^i& 1\end{array}\right]}^T,$ 该点在像素坐标系下的坐标为p_i＝[u_i v_i 1]^T，则两者关系表示为：

λ_ip_i＝A[R T]q_i   (13)

其中， $A=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right],$ f_x,f_y分别表示摄像机的等效焦距，即f_x＝f/dx,f_y＝f/dy，f是摄像机焦距，u₀,v₀为摄像机坐标系原点在像素坐标系下的坐标。旋转矩阵表示如下：

$\begin{array}{c}R=\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]={\left[r_{\mathrm{ij}}\right]}_{3\×3}\\ =\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}-\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\β}\sin \mathrm{\γ}\\ \cos \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\mathrm{co\; s}\mathrm{\β}\cos \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\β}\sin \mathrm{\γ}-\sin \mathrm{\β}\cos \mathrm{\γ}\\ -\sin \mathrm{\α}& \cos \mathrm{\α}\sin \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]\end{array}---\left(14\right)$

T＝[t_x t_y t_z]^T是平移矩阵，λ_i是未知尺度：

${\mathrm{\λ}}_i=e_3^T\left[\begin{array}{cc}R& T\end{array}\right]q_i---\left(15\right)$

其中 $e_3^T{\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T,$ 将联立得到：

$(A^{-1}{p}_i{e}_3^T-I)\left[\begin{array}{cc}R& T\end{array}\right]q_i=0---\left(16\right)$

从设计的合作目标图案上可知，q_i是已知量。在图像每一帧中，角点检测算 法提取特征点p_i，而特征点标定算法把每一个p_i和其对应的q_i联系起来。根据上面的数据就可以得到摄像机的位姿参数，其中R是从摄像头到合作目标的转动量，T是从摄像头到合作目标的平移量。

由于合作目标是处于大地平面上，所以又有所以可得： 

$\left[\begin{array}{cccccccccccc}-x_g& 0& \frac{u_i-u0}{f_x}{x}_g& -y_g& 0& \frac{u_i-u0}{f_x}{y}_g& -\mathrm{zg}& 0& \frac{u_i-u0}{f_x}{z}_g& -1& 0& \frac{u_i-u0}{f_x}\\ 0& -x_g& \frac{v_i-v0}{f_y}{x}_g& 0& -\mathrm{yg}& \frac{v_i-v0}{f_y}{y}_g& 0& -\mathrm{zg}& \frac{v_i-v0}{f_y}{z}_g& 0& -1& \frac{v_i-v0}{f_y}\end{array}\right]\left[\begin{array}{c}{r}_{11}\\ r_{21}\\ r_{31}\\ r_{12}\\ r_{22}\\ r_{32}\\ r_{13}\\ r_{23}\\ r_{33}\\ t_x\\ t_y\\ t_z\end{array}\right]=0---\left(17\right)$

简记为F[r₁ ^T r₂ ^T T^T]^T＝0，其中 $F={\left[\begin{array}{ccc}{F_1}^T& \·\·\·& F_n^T\end{array}\right]}^T,$ n是角点个数。

$F_i=\left[\begin{array}{ccccccccc}{x}_g^i& 0& -x_g^i{u}_i& y_g^i& 0& -y_g^i{u}_i& 1& 0& u_i\\ 0& x_g^i& -x_g^i{v}_i& 0& y_g^i& -y_g^i{v}_i& 0& 1& v_i\end{array}\right]---\left(18\right)$

根据标准奇异值分解(SVD)技术来计算对应于F最小奇异值的奇异矢量 

通过和的单位范数来计算平移矢量的T大小，

可以分两步利用估算得到的和来解出旋转矩阵R。第一步，通过计算方程 $\left[\begin{array}{ccc}{\tilde{r}}_1& {\tilde{r}}_2& 0\end{array}\right]={\mathrm{U\ΣV}}^T$ 的SVD，若则若则 Q＝diag(1,1,-1)。

最终求出车辆相对于合作目标的位置参数：

Η＝[x_g y_g z_g]^T＝-R^-1T   (19) 

姿态参数： $\left\{\begin{array}{c}\mathrm{\α}=-\arcsin r_{31}\\ \mathrm{\β}=\arctan (r_{32}/r_{33})\\ \mathrm{\γ}=\arctan (r_{21}/r_{11})\end{array}\right.---\left(20\right).$

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种主动视觉定位方法，其特征在于，包括如下步骤：

步骤1、制作合作目标，并将合作目标设置在场景中；

其中，所述合作目标以红色图案为背板，背板上设置三个黄色正方形作为合作标志；所述背板采用RGB颜色灰度值为(255，0，0)，三个正方形RGB颜色灰度值为(255，255，0)；其中第一正方形和第二正方形以第三正方形的其中一条对角线为对称轴对称分布；所述第三正方形的尺寸大于第一和第二正方形；所述第三正方形和第一正方形及第二正方形的边长比为4:3；三个正方形边界的最小距离大于最小正方形的边长；

步骤2、利用用户图像采集设备获取场景中的合作目标图像，并提取合作目标中的合作标志的角点，获得角点在所述合作目标图像上设置的图像坐标系中的坐标；

步骤3、根据合作标志角点在合作目标坐标系中的坐标和步骤2得到的合作标志角点在图像坐标系中的坐标，得到合作目标坐标系与图像坐标系之间的转换关系；然后再基于已知的图像坐标系与用户图像采集设备坐标系的坐标转换关系，得到用户图像采集设备坐标系与合作目标坐标系的坐标转换关系；最后，基于用户图像设备坐标系与车辆坐标系的坐标转换关系以及合作目标坐标系与地理坐标系的坐标转换关系，得到车辆在地理坐标系中坐标和姿态，由此完成对车辆在场景中的定位。

2.如权利要求1所述的一种主动视觉定位方法，其特征在于，所述背板尺寸为80厘米*120厘米，所述第三正方形的边长为32厘米，第一和第二正方形的边长为24厘米，第一和第二正方形中心距离第三正方形中心距离大于35厘米；所述第三正方形的所述对称轴与所述背板宽度方向中心线重合。

3.如权利要求1所述的一种主动视觉定位方法，其特征在于，所述步骤2包括如下具体步骤：

步骤A0、控制用户图像采集设备实时采集场景中的实景图像，并判断每帧图像中是否含有合作目标：如果有，将含有合作目标的图像定义为图像A，执行步骤A1；如果没有，返回步骤A0；

步骤A1、提取所述图像A中每个像素点的R、G、B三通道灰度值信息；并将满足如下条件的像素点赋值为255，不满足如下条件的像素点赋值为0，对每个像素点按照上述方法赋值后，再对图像A进行二值化，得到图像A*；

条件为：针对每一个像素点，R通道灰度值大于50，R通道灰度值大于1.5倍G通道灰度值，并且R通道灰度值大于1.3倍B通道灰度值；

步骤A2、对图像A*进行形态学开操作，去除细小的区域；然后再对图像A*进行边缘提取，并进行边缘补偿，得到图像A**；

步骤A3、检测图像A**中所有的边缘序列，进行边缘跟踪，寻找图像A**中的最长边缘，并提取最长边缘上所有像素点中在x轴和y轴的最大值和最小值：最大值xmax,ymax以及最小值xmin,ymin；

步骤A4、在图像A中，依次连接坐标点(xmin,ymin)、(xmin,ymax)、(xmax,ymax)和(xmax,ymin)，得到的区域作为合作目标区域，区域中的图像定义为图像T1；

步骤A5、对图像A进行灰度化，在灰度化后图像A中，依次连接坐标点(xmin,ymin)、(xmin,ymax)、(xmax,ymax)和(xmax,ymin)，得到的区域作为角点检测区域，区域中的图像定义为图像T2；

步骤A6、对于图像T1中的每个像素点，将满足R通道灰度值大于50，G通道灰度值大于50，并且G通道大于1.5倍B通道灰度值的像素点赋值为255，否则赋值为0，通过二值化得到图像T1*；

步骤A7：对图像T1*进行形态学开操作，去除图像T1*中细小的区域，且对T1*进行边缘提取，并进行边缘补偿，得到图像T1**；

步骤A8、对图像T1**进行边缘跟踪，寻找图像T1**中边缘长度最长的前三段边缘作为合作目标中方形的边缘，删除其余边缘；

步骤A9、对图像T2利用Harris角点提取算法进行角点提取，如果提取的角点位于所述合作目标中方形的边缘上，则将满足该条件的角点作为的合作目标角点；

步骤A10、利用Kmeans算法将A9中得到的合作目标角点分为3类，如果存在一类角点个数小于3个的情况，返回步骤A10，继续分类；如果存在一类角点个数等于3个的情况，进行角点补偿，使得每一类都具有4个角点；

步骤A11、根据12个角点的相对位置关系，将每一类角点中的4个角点与所述合作目标的3个正方形的顶点互相对应，得到合作目标角点在图像坐标系中的坐标。

4.如权利要求3所述的一种主动视觉定位方法，其特征在于，所述步骤A0中判断实时图像中是否含有合作目标的具体方法为：

步骤D1：提取当前图像中每个像素的R、G、B三通道信息；

步骤D2:将满足R通道灰度值大于50，R通道灰度值大于1.5倍G通道灰度值，并且R通道灰度值大于1.3倍B通道灰度值的像素点赋值为255，否则赋值为0，然后对当前图像进行二值化；

步骤D3：计算二值化后的图像中灰度值为255的像素点个数，如果像素点个数大于全图所有像素点个数的20％，则认为该当前图像存在合作目标，否则认为合作目标不存在。

5.如权利要求3所述的一种主动视觉定位方法，其特征在于，所述步骤A10中，当存在一类角点个数等于3个的情况时，所述角点补偿方法如下：

步骤B1、针对某一类角点，分别用x1、x2和x3表示3个角点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]   (3)

分别用y1、y2和y3表示3个角点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]   (4)

步骤B2、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该类角点在x轴方向存在缺点，执行步骤B3；若y轴方向之差最小，则表明该类角点在y轴方向存在缺点，执行步骤B4；

步骤B3、在x轴方向补充缺点；

SB31、首先判断x轴方向第几个差值最小：

如果第1个差值ΔX1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔX2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔX3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SB32、然后分别计算缺点相对的角点x轴坐标与另外两个角点x轴坐标差值，如果两个差值均小于0，则缺点的x轴坐标大于自身相对角点的x轴坐标；如果两个差值均大于0，则缺点的x轴坐标小于自身相对角点的x轴坐标；

SB33、找到差值ΔX1、差值ΔX2和差值ΔX3中最大的两个差值，并求两个差值的均值：

当缺点的x轴坐标大于自身相对角点的x轴坐标，缺点的x轴坐标等于自身相对角点x轴坐标加两倍的差值均值；

当缺点的x轴坐标小于自身相对角点的x轴坐标，缺点的x轴坐标等于自身相对角点x轴坐标减两倍的差值均值；

所述缺点的y轴坐标等于自身相对角点的y轴坐标；

步骤B4、在y轴方向补充缺点：

SB41、判断y轴方向第几个差值最小：

如果第一个差值ΔY1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔY2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔY3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SB42、然后分别计算缺点相对的角点y轴坐标与另外两个角点y轴坐标差值，如果两个差值均小于0，则缺点的y轴坐标大于自身相对角点的y轴坐标；如果两个差值均大于0，则缺点的y轴坐标小于自身相对角点的y轴坐标；

SB43、找到差值ΔY1、差值ΔY2和差值ΔY3中最大的两个差值，并求两个差值的均值：

当缺点位于其相对角点的+y轴方向，缺点的y轴坐标等于其相对角点y轴坐标加两倍的差值均值；

当缺点位于其相对角点的-y轴方向，缺点的y轴坐标等于其相对角点y轴坐标减两倍的差值均值；

所述缺点的x轴坐标等于自身相对角点的x轴坐标；

由此得到缺点的坐标，实现角点补偿。

6.如权利要求3所述的一种主动视觉定位方法，其特征在于，所述步骤A10中，当存在一类角点个数等于3个的情况时，所述角点补偿方法如下：

步骤C1、针对某一类角点，分别用x1、x2和x3表示3个角点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]   (3)

分别用y1、y2和y3表示3个角点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]   (4)

步骤C2、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该类角点在x轴方向存在缺点，执行步骤C3；若y轴方向之差最小，则表明该类角点在y轴方向存在缺点，执行步骤C4；

步骤C3、在x轴方向补充缺点；

SC31、首先判断x轴方向第几个差值最小：

如果第1个差值ΔX1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔX2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔX3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SC32、然后分别计算缺点相对的角点x轴坐标与另外两个角点x轴坐标差值，如果两个差值均小于0，则缺点的x轴坐标大于自身相对角点的x轴坐标；如果两个差值均大于0，则缺点的x轴坐标小于自身相对角点的x轴坐标；

步骤C4、在y轴方向补充缺点：

SC41、判断y轴方向第几个差值最小：

如果第一个差值ΔY1最小，则在角点(x2,y2)和角点(x1,y1)连线另一侧且与角点(x3,y3)相对的位置为缺点位置；

如果第二个差值ΔY2最小，则在角点(x3,y3)和角点(x1,y1)连线另一侧且与角点(x2,y2)相对的位置为缺点位置；

如果第三个差值ΔY3最小，则在角点(x2,y2)和角点(x3,y3)连线另一侧且与角点(x1,y1)相对的位置为缺点位置；

SC42、然后分别计算缺点相对的角点y轴坐标与另外两个角点y轴坐标差值，如果两个差值均小于0，则缺点的y轴坐标大于自身相对角点的y轴坐标；如果两个差值均大于0，则缺点的y轴坐标小于自身相对角点的y轴坐标；

步骤C5、根据步骤C3或步骤C4确定的缺点与相对角点的相对位置关系，求解如下公式，得到坐标P的最优值：

$\underset{P}{\arg \min }\underset{i,j\∈\left\{\mathrm{1,2}\right\},i\≡j}{\mathrm{\Σ}}{\left|\right|(P-P_i)\·(P_j-P_0)\left|\right|}_F^2$

s.t||P-P₃||₂＝||P₀-P₃||₂且

其中，P₀表示与缺点相对角点p₀坐标，P₁和P₂分别为某类角点中除缺点和角点p₀的另外两个角点的坐标；P₃表示坐标P₁和坐标P₂的连线中心点坐标；表示正整数；

将坐标P的最优值作为缺点的坐标，实现角点补偿。

7.如权利要求3所述的一种主动视觉定位方法，其特征在于，当场景中在不同位置设置多个合作目标时，在每个合作目标的背板上设置标识，以此区分不同的合作目标。

8.如权利要求7所述的一种主动视觉定位方法，其特征在于，在每个合作目标的背板上设置多个朝向不同的三角形作为标识，所述三角形为绿色图案，RGB颜色灰度值为(0,255,0)；

对合作目标的标识的识别方法如下：

步骤E1、识别三角形的个数；

步骤E2、针对步骤E1中得到的每一个三角形，分别用x1、x2和x3表示三角形3个顶点在x轴方向坐标，并且两两求差，即:

[ΔX1＝x1-x2 ΔX2＝x1-x3 ΔX3＝x2-x3]

分别用y1、y2和y3表示三角形3个顶点在y轴方向坐标，并且两两求差，即:

[ΔY1＝y1-y2 ΔY2＝y1-y3 ΔY3＝y2-y3]

步骤E3、分别找到x轴方向之差和y轴方向之差的最小值，并判断：若x轴方向之差最小，则表明该三角形朝向x轴正方向或负方向，执行步骤E4；若y轴方向之差最小，则表明该三角形朝向y轴正方向或负方向，执行步骤E5；

步骤E4、若三角形朝向x轴正方向或负方向；

找到两个x轴差值最小的两个三角形顶点，然后判断第三个顶点与前两个顶点的x坐标差值，如果两个差值均小于0，则三角形朝向x轴负方向；如果两个差值均大于0，则三角形朝向x轴正方向；

步骤E5、三角形朝向y轴正方向或负方向：

找到两个y轴差值最小的两个三角形顶点，然后判断第三个顶点与前两个顶点的y坐标差值，如果两个差值均小于0，则三角形朝向y轴负方向；如果两个差值均大于0，则三角形朝向y轴正方向；

步骤E6、对每个三角形均执行步骤E1至E5，得到所有三角形的朝向。

9.如权利要求7所述的一种主动视觉定位方法，其特征在于，在每个合作目标的背板上设置不同个数的三角形，用于识别不同的合作目标。

10.如权利要求8或9所述的一种主动视觉定位方法，其特征在于，所述三角形个数的识别方法如下：

步骤F1、设置三角形为绿色图案，RGB颜色灰度值分别为(0,255,0)；针对所述步骤A5中的图像T2，遍历每一个像素点，将满足G通道灰度值大于100、G通道大于1.5倍B通道灰度值并且G通道大于1.5倍R通道灰度值像素点点赋值为255，否则赋值为0，通过二值化得到图像T3*；

步骤F2、对图像T3*进行形态学开操作，去除T3*中细小的区域，且对T3*进行边缘提取，并进行边缘补偿，得到图像T3**；

步骤F3、对图像T3**进行边缘跟踪，跟踪边缘个数即为合作目标中三角形个数。