本发明涉及数字图像处理和计算机视觉领域，特别是涉及一种准确定位空间中任意点的世界坐标的方法和系统，属于空间定位技术，尤其适用于机器人定位和工业自动化生产。
背景技术：
近几年，人工智能得到了迅速的发展，甚至成为世界各国竞争科技强国的重要因素。计算机视觉技术作为人工智能领域的重要分支，得到了广大研究者的关注。基于视觉的空间定位技术也被应用于大量的机器视觉系统。空间定位技术应用广泛，包括机器人定位，工业自动化生产，监控，工程测绘以及军事航空航天等领域。比如在机器人抓取物体时，需要通过准确定位物体从而抓取。在多自由度机械手工作时，是否能够精确定位各个关节点位置对安全生产有重要意义。在监控领域，空间定位技术可以准确监控物体是否发生移位。
很多研究者提出了空间定位方法。中国科学院自动化研究所机器人视觉研究组发明了cvsuite软件，实现了图像的特征点提取，匹配，相机的标定和最后三维显示的功能，运用起来方便，但运行速度缓慢，影响实时定位效果且精度也不够高。还有近几年特别火热的视觉slam(即时定位与地图构建)技术，已经成功应用到许多机器人当中，但它操作较为复杂，且对摄像头要求较高。
技术实现要素：
本发明主要目的是提供一种基于方形标签和视觉的空间定位方法，能够解决当前空间定位方法速度缓慢，精度不高，操作复杂，设备要求高等问题。
为解决上述技术问题，本发明采用的一个技术方案是：
一种基于视觉的空间定位方法，包括如下步骤：
通过标定，获取两个相机的内参数矩阵；
在某一固定点和需定位位置设置不同id的方形标签；
使用两个相机同时拍摄拍摄固定点和需定位位置的方形标签，并获取各方形标签的坐标信息，根据坐标信息和内参数矩阵求取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角；
根据两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角，求得每一个方形标签中心点的以固定点为原点的三维世界坐标完成定位。
进一步地，通过标定，获取两个相机的内参数矩阵包括：
通过相机拍摄采集具有棋盘格的标定板的若干标定图片；
提取每一张标定图片中棋盘格上的内角点信息，得到每一个内角点的2d像素坐标及以左上角第一个内角点为原点的3d世界坐标并据此计算该相机的内参数矩阵k。
进一步地，根据下式计算该相机的内参数矩阵：
其中，k表示相机的内参矩阵，fx和fy分别表示坐标系中x轴和y轴方向的尺度因子，s表示两个坐标系的畸变系数，(u0，v0)是主点坐标，r是一个3×3的旋转矩阵，t是1×3的平移向量，(xw，yw，zw)和(u，v)是某一点的3d/2d坐标。
进一步地，各方形标签的坐标信息包括，各方形标签的id和各方形标签的四个顶点的像素坐标。
进一步地，使用两个相机同时拍摄拍摄固定点和需定位位置的方形标签，并获取各方形标签的坐标信息包括：
使用两个相机在不同位置同时拍摄设置有方形标签位置的图像，再求取图像的梯度图像，提取梯度图像中的直线，检测方形角点，得到若干方形区域及其关键的角点；
将所述方形区域和标签库中的方形标签进行逐个匹配对比，从而确定各个方形标签在图像中的位置，并得到每一个方形标签的id，根据角点信息得到每一个方形标签的四个顶点在像平面坐标系的像素坐标，从而得到各个标签中心点p的像素坐标p(u，v)；
取固定点处的方形标签中心点为原点ow(0，0，0)，以四个顶点世界坐标(-n，-n，0)，(n，-n，0)，(n，n，0)和(-n，n，0)作为固定点处标签的四个顶点3d坐标以及对应的2d像素坐标，n为方形标签的边长。
进一步地，根据坐标信息和内参数矩阵求取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角包括：
将四个顶点3d坐标以及对应的2d像素坐标代入如下的像素坐标系和世界坐标系转换式，
求得相机的旋转矩阵r和平移向量t；世界坐标系原点ow在相机坐标系中相应的投影点ow的坐标为(t1，t2，t3)；
根据旋转矩阵r，由如下的计算式分别依次求出相机坐标系旋转至与世界坐标系完全平行的三个旋转角θz，θy和θx，
θz＝atan2(r21，r11)
θx＝atan2(r32，r33)。
进一步地，还包括：根据前述求得的ow(t11，t12，t13)和三个旋转角θz，θy和θx，，将原始相机坐标系依次绕z轴旋转θz，绕y轴旋转θy，绕x轴旋转θx得到与世界坐标系平行的坐标系，将ow绕z轴旋转-θz，绕y轴旋转-θy，绕x轴旋转-θx得到ow2(x2，y2，z2),，使相机oc在世界坐标系的坐标为(-x2，-y2，-z2)。
进一步地，根据两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角，求得每一个方形标签中心点的以固定点为原点的三维世界坐标完成定位包括：
通过如下计算式分别求出需定位位置p在两个相机坐标系对应的投影点三维坐标p1(xpc1，ypc1，zpc1)和p2(xpc2，ypc2，zpc2)，
xc＝(u-u0)·f/fx
yc＝(v-v0)·f/fy
zc＝f；
将两个相机坐标系分别依次经过三次旋转得到与世界坐标系平行的坐标系，分别将pc1和pc2依次经过三次反向旋转得到相对相机的三维坐标p1(xpw1，ypw1，zpw1)和p2(xpw2，ypw2，zpw2)；
求得两个相机的世界坐标oc1(xow1，yow1，zow1)和oc2(xow2，yow2，zow2),p1和p2的世界坐标：p1(xpw1+xow1，ypw1+yow1，zpw1+zow1)和p2(xpw2+xow2，ypw2+yow2，zpw2+zow2)，得到两条直线oc1p1和oc2p2；
分别将oc1和p1坐标、oc2和p2坐标代入如下计算式，
求得直线oc1p1、oc2p2的方程；
以两条直线oc1p1和oc2p2的交点确定需定位位置p。
进一步地，还包括消除误差，包括：当两条直线oc1p1和oc2p2的交点不存在，则求取两条直线oc1p1和oc2p2的距离最近点的中点，作为需定位位置p的三维世界坐标。
通过采取上述技术方案，本发明仅需要两个普通的相机和准备好的方形标签就能完成空间精确定位，对相机的要求极低，从而使整个方案实现的成本低，成效高。本发明提出的基于方形标签的空间定位方法过程中，相机标定过程简单易实现，标定准确度高，所需设备简单，算法复杂度低且定位精度高可以广泛地用于机器人定位系统，工业自动化实时的生产和视频监控等领域。
附图说明
图1是本发明一实施例中基于视觉的空间定位方法的流程示意图；
图2是本发明一实施例中标定两个相机的流程示意图；
图3是本发明一实施例中求取所有标签的中心点三维世界坐标的流程示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。
参考图1及图3，在一实施例中，提供的基于视觉的空间定位方法，具体步骤如下：
1)对两个相机进行标定，分别获得两个相机的内参数矩阵(畸变系数在内参数矩阵内)；
2)在某一固定点和需要定位的位置设置不同id的方形标签；较为常见地，采用贴设方式设置方形标签。
上述的方形标签是类似于二维码的图案，每一个标签对应于唯一的id号和图案排列，编写程序可以识别这个标签并得到相应的id。
3)通过准确识别两个相机拍得的图像获取每一个标签的id和四个顶点的像素坐标，将这些坐标信息和步骤1)求得的内参数矩阵代入相关公式求取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角；其中，相机位置可在任意位置，只要满足两个相机均能拍到固定点和需要定位的位置上的标签即可。
4)根据两幅图像和两个相机的三个旋转角求得每一个标签中心点的以固定点为原点的三维世界坐标完成定位。
进一步，两个相机均采用步骤1)的方法进行相机标定，参考图2，上述步骤1)中的相机标定方法具体包含以下步骤：
1-1)准备好棋盘格标定纸，棋盘格分布均匀，每个方形格边长固定，将标定纸贴在平面板上制成标定板，让标定板在不同位置，不同角度不同姿态下用相机进行拍摄15张左右标定图片；
1-2)对每一张标定图片提取棋盘格上的内角点信息，得到每一个内角点的2d像素坐标，和以左上角第一个内角点为原点的3d世界坐标；
1-3)根据相机成像的原理，公式(1)中k表示相机的内参矩阵，其中fx和fy分别表示x轴和y轴方向的尺度因子，s表示两个坐标系的畸变系数，(u0，v0)是主点坐标，r是一个3×3的旋转矩阵，t是1×3的平移向量，(xw，yw，zw)和(u，v)是某一点的3d/2d坐标。由于标定板在一个平面上，令zw＝0，r只有r1和r2即可，r1和r2是标准正交的，将步骤1-2)得到的3d/2d坐标组合代入，得到相机的内参矩阵k。
进一步，两个相机均采用步骤3)的方法获取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角，上述步骤3)具体包含以下步骤：
3-1)相机在不同位置同时拍摄图像，再求图像的梯度图像，提取梯度图像中的直线，检测方形角点。最后得到一些方形区域及其关键的角点，这些正方形区域和标签库的标签进行逐个匹配对比，从而确定各个标签在图像中的位置，并得到每一个标签的id，根据角点信息得到每一个方形标签的四个顶点在像平面坐标系的像素坐标，从而得到各个标签中心点p的像素坐标p(u，v)；
3-2)每一个方形标签的边长为n毫米，取固定点处的方形标签中心点为原点ow(0，0，0)，其余四个顶点世界坐标依次是(-n，-n，0)，(n，-n，0)，(n，n，0)和(-n，n，0)，这样就有了固定点处标签的四个顶点3d坐标以及对应的2d像素坐标。
3-3)将步骤3-2)得到的四组3d/2d坐标代入像素坐标系和世界坐标系转换公式如公式(2)，求得相机的旋转矩阵r和平移向量t；世界坐标系原点ow在相机坐标系中相应的投影点ow的坐标为(t1，t2，t3)；
其中fx、fy，u0、v0，和s在步骤1)已经求得，r是一个3×3的正交矩阵，t是3×1的平移向量，(xw，yw，zw）和(u，v)是某一点的3d/2d坐标。
3-4)根据步骤3-3)得到的旋转矩阵r，由公式(3)分别依次求出相机坐标系旋转至与世界坐标系完全平行的三个旋转角θz，θy和θx。
θz＝atan2(r21,r11)
θx＝atan2(r32，r33)公式(3)
公式(3)中θ＝atan2(x,y)表示当x的绝对值比y的绝对值大时，取θ＝arctan(y/x),反之则θ＝arctan(x/y)；
3-5)根据步骤3-3)中求得的ow(t11，t12，t13)和步骤3-4)求得相机的三个旋转角，将原始相机坐标系依次绕z轴旋转θz，绕y轴旋转θy，绕x轴旋转θx得到与世界坐标系平行的坐标系，为保证相机原点oc到ow的向量依然是指向ow,需要将ow绕z轴旋转-θz，绕y轴旋转-θy，绕x轴旋转-θx得到ow2(x2，y2，z2),因此相机oc在世界坐标系的坐标为(-x2，-y2，-z2)；
进一步，对所有的标签中心点p均通过步骤4)进行定位，步骤4)具体包含以下步骤：
4-1)根据公式(4)分别求出待求点p在两个相机坐标系对应的投影点三维坐标p1(xpc1，ypc1，zpc1)和p2(xpc2，ypc2，zpc2)。和步骤3-5)一样，将两个相机坐标系分别依次经过三次旋转得到与世界坐标系平行的坐标系，分别将pc1和pc2依次经过三次反向旋转得到相对相机的三维坐标p1(xpw1，ypw1，zpw1)和p2(xpw2，ypw2，zpw2)；
xc＝(u-u0)·f/fx
yc＝(v-v0)·f/fy
zc＝f公式(4)
4-2)通过步骤3-5)分别可求得两个相机的世界坐标oc1(xow1，yow1，zow1)和oc2(xow2，yow2，zow2),那么可求得p1和p2的世界坐标：p1(xpw1+xow1，ypw1+yow1，zpw1+zow1)和p2(xpw2+xow2，ypw2+yow2，zpw2+zow2)从而得到两条直线oc1p1和oc2p2；
4-3)分别将oc1和p1坐标代入公式(5)求得直线oc1p1的方程，将oc2和p2坐标代入公式(5)求得直线oc2p2的方程；
4-4)两条直线oc1p1和oc2p2的交点即为待求点p,但由于误差的存在，通过步骤4-3)求出来的两条直线方程求解交点时可能会出现没有交点的情况，采取求两条直线oc1p1和oc2p2的距离最近点的中点当作是p点的三维世界坐标。
步骤4-3)已经求出两条直线的方程，通过立体几何的方法就可求出两条直线距离最近时两条直线上的最近点坐标，两个最近点的中点即为p点坐标从而完成定位。
以六自由度机器臂关节点定位应用场景为例，具体实施方案包含以下步骤：
1)首先对选取的两个相机进行标定，分别获得两个相机的内参数矩阵和畸变系数；
2)在机械臂所在的平台的平面上和需要定位的各个机械臂关节点贴上不同id的方形标签；
3)在机械臂的周围布置两个相机，尽量保证整个机械臂在两个相机的视野范围内。同时按下相机开始拍摄视频，期间可以设置机械臂运动。
4)下面选取步骤3)中拍得的两个视频中某一帧图像为例，定位出这一时各个机械臂的位置。具体步骤如下：
4_1)准确识别两个相机拍得的图像获取每一个标签的id和四个顶点的像素坐标，将这些坐标信息和步骤1)求得的内参数矩阵代入相关公式求取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角。
4_2)根据两幅图像和两个相机的三个旋转角求得每一个标签中心点的以固定点为原点的三维世界坐标完成定位。
需要说明的是，本发明提供的技术方案，需要两个相机共同拍到同一个方形标签才能实现定位，其中，“相机”就普通的网络摄像头等数字相机就可以，或者其他能够实现拍照功能的图像采集装置。
显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。