[发明专利]一种基于双目立体视觉的排爆机器人空间定位方法

权利要求书

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

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

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

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

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

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

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

xn→=Xc/ZcYc/Zc=xnyn---(1)]]>

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

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

xd→=xdyd=(1+kc1r2+kc2r4)xn→+dx→---(2)]]>

dx→=2kc3xnyn+kc4(r2+2xn2)kc3(r2+2yn2)+2kc4xnyn---(3)]]>

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

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

u=1dx(x+αcy)+u0]]>   (4)

v=ydy+v0]]>

ud=fxxd+fxαcyd+u0vd=fyyd+v0---(5)]]>

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

(12)的求解过程：

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

首先求得

xd=(ud-u0-αc(vd-v0)/fy)/fxyd=(vd-v0)/fy---(6)]]>

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

xn(k+1)=(xd-2*kc3*xn(k)*yn(k)-kc4*(r2(k)+2*xn(k)*xn(k)))/(1+kc1*r2(k)+kc2*r2(k)*r2(k))yn(k+1)=(yd-kc3*(r2(k)+2*yn(k)*yn(k))-2*kc4*xn(k)*yn(k))/(1+kc1*r2(k)+kc2*r2(k)*r2(k))---(7)]]>

其中：rn(k)=(xn(k))2+(yn(k))2.]]>