CN104680528A - 一种基于双目立体视觉的排爆机器人空间定位方法 - Google Patents

Abstract

本发明涉及一种基于双目立体视觉的排爆机器人空间定位方法，其包括：(1)根据实际的图像像素坐标结合摄像机内参数及畸变参数求出理想投影点归一化坐标；(2)根据理想投影点的归一化坐标和摄像机外参数获取左右投影点分别与摄像机光心形成的两射线的方程；(3)计算所述两射线的公垂线；(4)计算公垂线的长度并将公垂线的中点作为目标点的三维坐标。本发明提出的基于双目立体视觉的排爆机器人空间定位方法，能够提高排爆机器人对目标点三维坐标的求取速度和定位精度，有效地解决了目前排爆机器人对目标点三维坐标的求取速度和定位精度较低的技术问题。

Description

一种基于双目立体视觉的排爆机器人空间定位方法

技术领域

本发明涉及机器视觉领域，具体涉及一种基于双目立体视觉的排爆机器人空间定位方法。

背景技术

目前单目视觉系统排爆机器人采用一台摄像机对目标进行测量，其单目视觉系统对深度信息的恢复能力较弱，多目视觉系统排爆机器人采用多台摄像机对目标进行测量，其多目测量精度较高但是图像传输速度慢，计算量大，测量耗时多。同时由于排爆机器人工作环境的特殊性，图像与数据信息的有线传输不仅增加了排爆的负担，有碍其灵活工作还给其自身安全性带来隐患。且排爆机器人空间定位时，由于摄像机模型的和投影点的近似误差的存在，左右投影点与摄像机光心的射线方程不交于一点，给目标点的三维求距带来了麻烦。因此，现有技术中排爆机器人对目标点三维坐标的求取速度和定位精度都较低。

发明内容

本发明针对现有技术中排爆机器人对目标点三维坐标的求取速度和定位精度较低的技术问题，提出一种基于双目立体视觉的排爆机器人空间定位方法。

本发明提出的一种基于双目立体视觉的排爆机器人空间定位方法，其包括以下步骤(1)至步骤(4)：

(1)根据实际的图像像素坐标结合摄像机内参数及畸变参数求出理想投影点归一化坐标；

(2)根据理想投影点的归一化坐标和摄像机外参数获取左右投影点分别与摄像机光心形成的两射线的方程；

(3)计算所述两射线的公垂线；

(4)计算公垂线的长度并将公垂线的中点作为目标点的三维坐标。

进一步，所述步骤(1)包括步骤(11)和步骤(12)：

(11)令为理想投影点归一化坐标矢量：

$\stackrel{\→}{x_n}=\left[\begin{array}{c}{X}_c/Z_c\\ Y_c/Z_c\end{array}\right]=\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]---\left(1\right)$

其中：X_c、Y_c、Z_c为摄像机光心的三维坐标，x_n、y_n为理想投影点归一化坐标；

令r²＝x_n ²+y_n ²,根据摄像机的非线性模型，考虑径向畸变和切向畸变，实际投影点的归一化坐标矢量可表示如下：

$\stackrel{\→}{x_d}=\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_{c1}{r}^2+k_{c2}{r}^4)\stackrel{\→}{x_n}+\stackrel{\→}{d_x}---\left(2\right)$

$\stackrel{\→}{\mathrm{dx}}=\left[\begin{array}{c}{2k}_{c3}{x}_n{y}_n+k_{c4}(r^2+{{2x}_n}^2)\\ k_{c3}(r^2+{{2y}_n}^2)+{2k}_{c4}{x}_n{y}_n\end{array}\right]---\left(3\right)$

其中：r为光心坐标在图像平面投影到原点的距离，d代表畸变后的坐标，x_d和y_d为图像畸变后的二维坐标；为切向畸变矢量，k_c1，k_c2，k_c3，k_c4为镜头畸变系数；

把式(2)和式(3)代入以下式(4)可得投影点的实际图像像素坐标(u_d,v_d)

$u=\frac{1}{\mathrm{dx}}(x+{\mathrm{\α}}_{c}y)+u_0$

$v=\frac{y}{\mathrm{dy}}+v_0---\left(4\right)$

$\left\{\begin{array}{c}{u}_d=f_x{x}_d+f_x{\mathrm{\α}}_c{y}_d+u_0\\ v_d=f_y{y}_d+v_0\end{array}---\left(5\right)\right.$

其中：u和v为目标点图像像素坐标，dx为一个像素在x方向上的物理尺寸，dy为一个像素在y方向上的物理尺寸，(u₀,v₀)为光心的像素坐标；f_x和f_y为摄像机的焦距，α_c为变形因子，其与u,v像素轴的角度有关；

(12)的求解过程：

根据式(1)至式(5)，根据标定所得出的镜头畸变系数，利用迭代法，可以由(u_d,v_d)求得具体步骤如下:

首先求得

$\left\{\begin{array}{c}{x}_d=(u_d-u_0-{\mathrm{\α}}_c(v_d-v_0)/f_y)/f_x\\ y_d=(v_d-v_0)/f_y\end{array}---\left(6\right)\right.$

由(x_d,y_d)经过如下迭代函数式可得

$\left\{\begin{array}{c}{x}_n^{(k+1)}=(x_d-2*k_{c3}*x_n^{\left(k\right)}*y_n^{\left(k\right)}-k_{c4}*(r^{2\left(k\right)}+2*x_n^{\left(k\right)}*x_n^{\left(k\right)}))/(1+k_{c1}*r^{2\left(k\right)}+k_{c2}*r^{2\left(k\right)}*r^{2\left(k\right)})\\ y_n^{(k+1)}=(y_d-k_{c3}*(r^{2\left(k\right)}+2*y_n^{\left(k\right)}*y_n^{\left(k\right)})-2*k_{c4}*x_n^{\left(k\right)}*y_n^{\left(k\right)})/(1+k_{c1}*r^{2\left(k\right)}+k_{c2}*r^{2\left(k\right)}*r^{2\left(k\right)})\end{array}\right.---\left(7\right)$

其中： $r_n^{\left(k\right)}={\left(x_n^{\left(k\right)}\right)}^2+{\left(y_n^{\left(k\right)}\right)}^2.$

进一步，所述步骤(2)包括步骤(21)和步骤(22)：

(21)令世界坐标系与右摄像机坐标系重合无旋转，则两摄像机的外参数分别为：

[R T]^right＝[I 0]，其中，I为单位矩阵，R为右摄像机坐标系相对于左摄像坐标系的旋转矩阵；T为右摄像机坐标系相对于左摄像坐标系的平移矩阵；

[R T]^left＝[R T]，其中[R T]为右摄像机坐标系相对于左摄像坐标系的旋转和平移矩阵；

(22)设左、右摄像机的光心分别为O_l和O_r，场景中P点在左摄像机坐标系中的像点为p_l，空间点为P_l，空间点P_l坐标为(X_l,Y_l,Z_l)，在右摄像机坐标系中的像点为p_r，空间点为P_r，坐标为(X_r,Y_r,Z_r)，可根据O_l、p_l、O_r、p_r四点的空间位置求所述两射线和

令在世界坐标系下，则各矢量在右摄像机坐系中的定义如下：

进一步，所述步骤(3)计算所述两射线的公垂线具体如下：

其中，为所述两射线与的公垂线。

进一步，所述步骤(4)具体计算步骤如下：

因为为射线与的公垂线，则可得：

其中:⊙代表两向量的点乘；

由于R,T可通过标定求得，可根据式(7)求得,因此在式(11)中Z_l、Z_r为仅有的两个未知量，式(11)有且存在唯一解，至此P_r(X_r,Y_r,Z_r)P_l(X_l,Y_l,Z_l)完全确定，取公垂线的中点最为最终点P的坐标：

$P=\frac{P_r+(R*P_l+T)}{2}---\left(12\right)$

有益效果：相对于现有技术中的单目或多目视觉系统排爆机器人，本发明提出的基于双目立体视觉的排爆机器人空间定位方法，能够提高排爆机器人对目标点三维坐标的求取速度和定位精度，有效地解决了目前排爆机器人对目标点三维坐标的求取速度和定位精度较低的技术问题。

附图说明

图1是用于辅助理解本发明实施例方案的一种基于双目立体视觉的排爆机器人图像采集系统的工作原理示意图。

图2是本发明提出的基于双目立体视觉的排爆机器人空间定位方法实施例的计算原理示意图。

图3是本发明提出的基于双目立体视觉的排爆机器人空间定位方法实施例整体流程图。

图4是发明提出的基于双目立体视觉的排爆机器人空间定位方法实施例详细流程图。

具体实施方式

为了便于本领域技术人员理解，下面将结合附图以及实施例对本发明进行进一步描述。

在介绍本发明提出的基于双目立体视觉的排爆机器人空间定位方法实施例之前，作为辅助理解，以下先介绍一种基于双目立体视觉的排爆机器人图像采集系统。

请参阅图1，所述基于双目立体视觉的排爆机器人图像采集系统，包括左右两个摄像机、无线图像收发器、图像采集卡、服务器、数传电台等。两个摄像机固定安装在排爆机器人机械臂的小臂上作为机器人计算物体目标的“双眼”用来拍摄空间场景图像；摄像机采集的图像通过无线图像传送设备传送到后台服务器，后台接收到图像后，通过安装在计算机中的图像采集卡，便可以将图像送入计算机进行处理，处理结果通过数传电台再传送给下位机PC/104。

所述的无线图像收发器包括图像发送器和图像接收器，图像发送器与两个摄像机相连安装在排爆机器人上，图像发送器将摄像机传来的图像通过无线传输至图像接收器，

所述的图像采集卡安装在服务器内，是将图像接受器传来的模拟图像转换为计算机能识别的数字图像，然后在对应的程序窗口中进行显示。

所述基于双目立体视觉的排爆机器人图像采集系统具有以下优点：1、较单目视觉排爆机器人恢复三维信息能力强；2、较多目排爆机器人处理速度快，定位精度高；3、由于采用了无线传输方式，减轻了排爆机器人的负担，增加了其工作的灵活性，保障了其危险场合工作的安全性。

以下介绍本发明提出的基于双目立体视觉的排爆机器人空间定位方法实施例。

请参阅图2至图4，本发明提出的基于双目立体视觉的排爆机器人空间定位方法实施例包括以下步骤(1)至步骤(4)：

(1)根据实际的图像像素坐标结合摄像机内参数及畸变参数求出理想投影点归一化坐标；

(2)根据理想投影点的归一化坐标和摄像机外参数获取左右投影点分别与摄像机光心形成的两射线的方程；

(3)计算所述两射线的公垂线；

(4)计算公垂线的长度并将公垂线的中点作为目标点的三维坐标。

为了更好地实现本实施例的目的，本实施例对以上步骤(1)至步骤(4)做了具体的算法设计。

具体的，所述步骤(1)包括步骤(11)和步骤(12)：

(11)令为理想投影点归一化坐标矢量：

$\stackrel{\→}{x_n}=\left[\begin{array}{c}{X}_c/Z_c\\ Y_c/Z_c\end{array}\right]=\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]---\left(1\right)$

其中：X_c、Y_c、Z_c为摄像机光心的三维坐标，x_n、y_n为理想投影点归一化坐标；

令r²＝x_n ²+y_n ²,根据摄像机的非线性模型，考虑径向畸变(本实施例中，由于径向畸变较小，因此可以忽略)和切向(即切线方向)畸变，实际投影点的归一化坐标矢量可表示如下：

$\stackrel{\→}{x_d}=\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_{c1}{r}^2+k_{c2}{r}^4)\stackrel{\→}{x_n}+\stackrel{\→}{d_x}---\left(2\right)$

$\stackrel{\→}{\mathrm{dx}}=\left[\begin{array}{c}{2k}_{c3}{x}_n{y}_n+k_{c4}(r^2+{{2x}_n}^2)\\ k_{c3}(r^2+{{2y}_n}^2)+{2k}_{c4}{x}_n{y}_n\end{array}\right]---\left(3\right)$

其中：r为光心坐标在图像平面投影到原点的距离，d代表畸变后的坐标，x_d和y_d为图像畸变后的二维坐标；为切向畸变矢量，k_c1，k_c2，k_c3，k_c4为镜头畸变系数；

把式(2)和式(3)代入以下式(4)可得投影点的实际图像像素坐标(u_d,v_d)

$u=\frac{1}{\mathrm{dx}}(x+{\mathrm{\α}}_{c}y)+u_0$

$v=\frac{y}{\mathrm{dy}}+v_0---\left(4\right)$

$\left\{\begin{array}{c}{u}_d=f_x{x}_d+f_x{\mathrm{\α}}_c{y}_d+u_0\\ v_d=f_y{y}_d+v_0\end{array}---\left(5\right)\right.$

其中：u和v为目标点图像像素坐标，dx为一个像素在x方向上的物理尺寸，dy为一个像素在y方向上的物理尺寸，(u₀,v₀)为光心的像素坐标；f_x和f_y为摄像机的焦距，α_c为变形因子，其与u,v像素轴的角度有关；

(12)的求解过程：

在三维计算的实际操作中，获得的投影点的图像像素坐标是经过畸变后的实际图像像素坐标，我们需要根据实际图像像素坐标得到理想投影点归一化的图像平面坐标矢量，以便于后面的几何求距；

根据式(1)至式(5)，根据标定所得出的镜头畸变系数，利用迭代法，可以由(u_d,v_d)求得具体步骤如下:

首先求得

$\left\{\begin{array}{c}{x}_d=(u_d-u_0-{\mathrm{\α}}_c(v_d-v_0)/f_y)/f_x\\ y_d=(v_d-v_0)/f_y\end{array}---\left(6\right)\right.$

由(x_d,y_d)经过如下迭代函数式可得

$\left\{\begin{array}{c}{x}_n^{(k+1)}=(x_d-2*k_{c3}*x_n^{\left(k\right)}*y_n^{\left(k\right)}-k_{c4}*(r^{2\left(k\right)}+2*x_n^{\left(k\right)}*x_n^{\left(k\right)}))/(1+k_{c1}*r^{2\left(k\right)}+k_{c2}*r^{2\left(k\right)}*r^{2\left(k\right)})\\ y_n^{(k+1)}=(y_d-k_{c3}*(r^{2\left(k\right)}+2*y_n^{\left(k\right)}*y_n^{\left(k\right)})-2*k_{c4}*x_n^{\left(k\right)}*y_n^{\left(k\right)})/(1+k_{c1}*r^{2\left(k\right)}+k_{c2}*r^{2\left(k\right)}*r^{2\left(k\right)})\end{array}\right.---\left(7\right)$

其中： $r_n^{\left(k\right)}={\left(x_n^{\left(k\right)}\right)}^2+{\left(y_n^{\left(k\right)}\right)}^2;$

在进行迭代时，可以设置迭代初值为迭代次数为20次。

具体的，所述步骤(2)包括步骤(21)和步骤(22)：

(21)令世界坐标系与右摄像机坐标系重合无旋转，则两摄像机的外参数分别为：

[R T]^right＝[I 0]，其中，I为单位矩阵，R为右摄像机坐标系相对于左摄像坐标系的旋转矩阵；T为右摄像机坐标系相对于左摄像坐标系的平移矩阵；

[R T]^left＝[R T]，其中[R T]为右摄像机坐标系相对于左摄像坐标系的旋转和平移矩阵；

(22)设左、右摄像机的光心分别为O_l和O_r，场景中P点在左摄像机坐标系中的像点为p_l，空间点为P_l，空间点P_l坐标为(X_l,Y_l,Z_l)，在右摄像机坐标系中的像点为p_r，空间点为P_r，坐标为(X_r,Y_r,Z_r)，可根据O_l、p_l、O_r、p_r四点的空间位置求所述两射线和

令在世界坐标系(即右摄像机坐标系)下，则各矢量在右摄像机坐系中的定义如下：

具体的，所述步骤(3)计算所述两射线的公垂线具体如下：

其中，为所述两射线与的公垂线。

具体的，所述步骤(4)计算步骤如下：

因为为射线与的公垂线，则可得：

其中:⊙代表两向量的点乘；

由于R,T可通过标定求得，可根据式(7)求得,因此在式(11)中Z_l、Z_r为仅有的两个未知量，式(11)有且存在唯一解，至此P_r(X_r,Y_r,Z_r)P_l(X_l,Y_l,Z_l)完全确定，取公垂线的中点最为最终点P的坐标：

$P=\frac{P_r+(R*P_l+T)}{2}---\left(12\right).$

本实施例提出的基于双目立体视觉的排爆机器人空间定位方法，能够提高排爆机器人对目标点三维坐标的求取速度和定位精度，有效地解决了目前排爆机器人对目标点三维坐标的求取速度和定位精度较低的技术问题。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (5)

1.一种基于双目立体视觉的排爆机器人空间定位方法，其特征在于，包括以下步骤(1)至步骤(4)：

(1)根据实际的图像像素坐标结合摄像机内参数及畸变参数求出理想投影点归一化坐标；

(2)根据理想投影点的归一化坐标和摄像机外参数获取左右投影点分别与摄像机光心形成的两射线的方程；

(3)计算所述两射线的公垂线；

(4)计算公垂线的长度并将公垂线的中点作为目标点的三维坐标。

2.根据权利要求1所述的基于双目立体视觉的排爆机器人空间定位方法，其特征在于，所述步骤(1)包括步骤(11)和步骤(12)：

(11)令为理想投影点归一化坐标矢量：

$\stackrel{\→}{x_n}=\left[\begin{array}{c}{X}_c/Z_c\\ Y_c/Z_c\end{array}\right]=\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]---\left(1\right)$

其中：X_c、Y_c、Z_c为摄像机光心的三维坐标，x_n、y_n为理想投影点归一化坐标；

令r²＝x_n ²+y_n ²,根据摄像机的非线性模型，考虑径向畸变和切向畸变，实际投影点的归一化坐标矢量可表示如下：

$\stackrel{\→}{x_d}=\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_{c1}{r}^2+k_{c2}{r}^4)\stackrel{\→}{x_n}+\stackrel{\→}{\mathrm{dx}}---\left(2\right)$

$\stackrel{\→}{\mathrm{dx}}=\left[\begin{array}{c}2k_{c3}{x}_n{y}_n+k_{c4}(r^2+2{x_n}^2)\\ k_{c3}(r^2+2{y_n}^2)+2k_{c4}{x}_n{y}_n\end{array}\right]---\left(3\right)$

其中：r为光心坐标在图像平面投影到原点的距离，d代表畸变后的坐标，x_d和y_d为图像畸变后的二维坐标；为切向畸变矢量，k_c1，k_c2，k_c3，k_c4为镜头畸变系数；

把式(2)和式(3)代入以下式(4)可得投影点的实际图像像素坐标(u_d,v_d)

$u=\frac{1}{\mathrm{dx}}(x+{\mathrm{\α}}_{c}y)+u_0$    (4)

$v=\frac{y}{\mathrm{dy}}+v_0$

$\left\{\begin{array}{c}{u}_d=f_x{x}_d+f_x{\mathrm{\α}}_c{y}_d+u_0\\ v_d=f_y{y}_d+v_0\end{array}\right.---\left(5\right)$

其中：u和v为目标点图像像素坐标，dx为一个像素在x方向上的物理尺寸，dy为一个像素在y方向上的物理尺寸，(u₀,v₀)为光心的像素坐标；f_x和f_y为摄像机的焦距，α_c为变形因子，其与u,v像素轴的角度有关；

(12)的求解过程：

根据式(1)至式(5)，根据标定所得出的镜头畸变系数，利用迭代法，可以由(u_d,v_d)求得具体步骤如下:

首先求得

$\left\{\begin{array}{c}{x}_d=(u_d-u_0-{\mathrm{\α}}_c(v_d-v_0)/f_y)/f_x\\ y_d=(v_d-v_0)/f_y\end{array}\right.---\left(6\right)$

由(x_d,y_d)经过如下迭代函数式可得(x_n y_n)^T

$\left\{\begin{array}{c}{x}_n^{(k+1)}=(x_d-2*k_{c3}*x_n^{\left(k\right)}*y_n^{\left(k\right)}-k_{c4}*(r^{2\left(k\right)}+2*x_n^{\left(k\right)}*x_n^{\left(k\right)}))/(1+k_{c1}*r^{2\left(k\right)}+k_{c2}*r^{2\left(k\right)}*r^{2\left(k\right)})\\ y_n^{(k+1)}=(y_d-k_{c3}*(r^{2\left(k\right)}+2*y_n^{\left(k\right)}*y_n^{\left(k\right)})-2*k_{c4}*x_n^{\left(k\right)}*y_n^{\left(k\right)})/(1+k_{c1}*r^{2\left(k\right)}+k_{c2}*r^{2\left(k\right)}*r^{2\left(k\right)})\end{array}\right.---\left(7\right)$

其中： $r_n^{\left(k\right)}={\left(x_n^{\left(k\right)}\right)}^2+{\left(y_n^{\left(k\right)}\right)}^2.$

3.根据权利要求2所述的基于双目立体视觉的排爆机器人空间定位方法，其特征在于，所述步骤(2)包括步骤(21)和步骤(22)：

(21)令世界坐标系与右摄像机坐标系重合无旋转，则两摄像机的外参数分别为：

[R T]^right＝[I 0]，其中，I为单位矩阵，R为右摄像机坐标系相对于左摄像坐标系的旋转矩阵；T为右摄像机坐标系相对于左摄像坐标系的平移矩阵；

[R T]^left＝[R T]，其中[R T]为右摄像机坐标系相对于左摄像坐标系的旋转和平移矩阵；

(22)设左、右摄像机的光心分别为O_l和O_r，场景中P点在左摄像机坐标系中的像点为p_l，空间点为P_l，空间点P_l坐标为(X_l,Y_l,Z_l)，在右摄像机坐标系中的像点为p_r，空间点为P_r，坐标为(X_r,Y_r,Z_r)，可根据O_l、p_l、O_r、p_r四点的空间位置求所述两射线和

令在世界坐标系下，则各矢量在右摄像机坐系中的定义如下：

4.根据权利要求3所述的基于双目立体视觉的排爆机器人空间定位方法，其特征在于，所述步骤(3)计算所述两射线的公垂线具体如下：

其中，为所述两射线与的公垂线。

5.根据权利要求4所述的基于双目立体视觉的排爆机器人空间定位方法，其特征在于，所述步骤(4)具体计算步骤如下：

因为为射线与的公垂线，则可得：

其中:⊙代表两向量的点乘；

由于R,T可通过标定求得，(X_l/Z_l Y_l/Z_l 1)^T、(X_r/Z_r Y_r/Z_r 1)^T可根据式(7)求得,因此在式(11)中Z_l、Z_r为仅有的两个未知量，式(11)有且存在唯一解，至此P_r(X_r,Y_r,Z_r)P_l(X_l,Y_l,Z_l)完全确定，取公垂线的中点最为最终点P的坐标：

$P=\frac{P_r+(R*P_l+T)}{2}---\left(12\right).$