[发明专利]一种基于视觉的空间定位方法

权利要求书

1.一种基于视觉的空间定位方法，包括如下步骤：

通过标定，获取两个相机的内参数矩阵；

在某一固定点和需定位位置设置不同id的方形标签；

使用两个相机同时拍摄固定点和需定位位置的方形标签，并获取各方形标签的坐标信息，根据坐标信息和内参数矩阵求取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角；其中，相机的位置为任意位置，只要满足两个相机均能拍摄到固定点和需要定位的位置上的方形标签即可；

根据两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角，求得每一个方形标签中心点的以固定点为原点的三维世界坐标完成定位；

其中，通过标定，获取两个相机的内参数矩阵包括：

通过相机拍摄采集具有棋盘格的标定板的若干标定图片；

提取每一张标定图片中棋盘格上的内角点信息，得到每一个内角点的2D像素坐标及以左上角第一个内角点为原点的3d世界坐标并据此计算该相机的内参数矩阵K；

根据下式计算该相机的内参数矩阵：

其中，K表示相机的内参矩阵，f_x和f_y分别表示坐标系中x轴和y轴方向的尺度因子，s表示两个坐标系的畸变系数，(u₀，v₀)是主点坐标，R是一个3×3的旋转矩阵，t是1×3的平移向量，(X_w，Y_w，Z_w)和(u，v)是某一点的3d/2d坐标；

各方形标签的坐标信息包括，各方形标签的id和各方形标签的四个顶点的像素坐标；

所述使用两个相机同时拍摄固定点和需定位位置的方形标签，并获取各方形标签的坐标信息，包括：

使用两个相机在不同位置同时拍摄设置有方形标签位置的图像，再求取图像的梯度图像，提取梯度图像中的直线，检测方形角点，得到若干方形区域及其关键的角点；

将所述方形区域和标签库中的方形标签进行逐个匹配对比，从而确定各个方形标签在图像中的位置，并得到每一个方形标签的id，根据角点信息得到每一个方形标签的四个顶点在像平面坐标系的像素坐标，从而得到各个标签中心点P的像素坐标P(u，v)；

取固定点处的方形标签中心点为原点O_w(0，0，0)，以四个顶点世界坐标(-n，-n，0)，(n，-n，0)，(n，n，0)和(-n，n，0)作为固定点处标签的四个顶点3D坐标以及对应的2D像素坐标，n为方形标签的边长；

所述根据坐标信息和内参数矩阵求取两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角，包括：

将四个顶点3D坐标以及对应的2D像素坐标代入如下的像素坐标系和世界坐标系转换式，

求得相机的旋转矩阵R和平移向量T；世界坐标系原点O_w在相机坐标系中相应的投影点o_w的坐标为(T₁₁，T₁₂，T₁₃)；

根据旋转矩阵R，由如下的计算式分别依次求出相机坐标系旋转至与世界坐标系完全平行的三个旋转角θ_z，θy和θ_x，

θ_z＝atan2(R₂₁，R₁₁)

θ_x＝atan2(R₃₂，R₃₃)；

根据前述求得的o_w(T₁₁，T₁₂，T₁₃)和三个旋转角θ_z，θ_y和θ_x，将原始相机坐标系依次绕z轴旋转θ_z，绕y轴旋转θ_y，绕x轴旋转θ_x得到与世界坐标系平行的坐标系，将o_w绕z轴旋转-θ_z，绕y轴旋转-θ_y，绕x轴旋转-θ_x得到o_w2(x₂，y₂，z₂)，使相机O_c在世界坐标系的坐标为(-x₂，-y₂，-z₂)；

其中，根据两个相机在以固定点为原点的三维世界坐标和相机坐标系到三维世界坐标系的三个旋转角，求得每一个方形标签中心点的以固定点为原点的三维世界坐标完成定位包括：

通过如下计算式分别求出需定位位置P在两个相机坐标系对应的投影点三维坐标P_c1(x_pc1，y_pc1，z_pc1)和P_c2(z_pc2，y_pc2，z_pc2)，

x_c＝(u-u₀)·F/f_x

y_c＝(v-v₀)·F/f_y

Z_c＝F；

将两个相机坐标系分别依次经过三次旋转得到与世界坐标系平行的坐标系，分别将P_c1和P_c2依次经过三次反向旋转得到相对相机的三维坐标P_c1R(x_pw1，y_pw1，z_pw1)和P_c2R(x_pw2，y_pw2，z_pw2)；

求得两个相机的世界坐标O_c1(x_ow1，y_ow1，z_ow1)和O_c2(x_ow2，y_ow2，z_ow2)，P1和P₂的世界坐标：P1(x_pw1+x_ow1，y_pw1+y_ow1，z_pw1+z_ow1)和P₂(x_pw2+x_ow2，y_pw2+y_ow2，z_pw2+z_ow2)，得到两条直线O_c1P₁和O_c2P₂；

分别将O_c1和P₁坐标、O_c2和P₂坐标代入如下计算式，

求得直线O_c1P₁、O_c2P₂的方程；

以两条直线O_c1P₁和O_c2P₂的交点确定需定位位置P。

2.如权利要求1所述的基于视觉的空间定位方法，其特征在于，还包括消除误差，包括：当两条直线O_c1P₁和O_c2P₂的交点不存在，则求取两条直线O_c1P₁和O_c2P₂的距离最近点的中点，作为需定位位置P的三维世界坐标。