一种带电作业机器人作业环境3D重构方法与流程

技术特征：

1.一种带电作业机器人作业环境3D重构方法，其特征在于，利用双目摄像头确定单个元器件和机械手工具的位姿参数，将获得的位姿参数导入带电作业场景的模型中，生成带电作业现场实时监控场景并显示在监控设备上。

2.如权利要求1所述带电作业机器人作业环境3D重构方法，其特征在于，利用双目摄像头，从两个视点同时观察同一场景获取的目标图像，通过三角测量原理，计算图像像素间的视差来解算特征点在测量坐标系下的三维坐标，根据物体上对应特征点在测量坐标系下空间坐标以及物体坐标系下空间坐标，求解目标的位置姿态参数。

3.如权利要求2所述带电作业机器人作业环境3D重构方法，其特征在于，求解目标的位置姿态参数的具体过程为：

第一步：建立坐标系：

建立物体坐标系O_O-x_oy_oz_o，设目标上第i个特征点在物体坐标系上的坐标为q_i＝(x_wi,y_wi,z_wi)^T；

建立测量坐标系，将第一个摄像头的摄像头坐标系为测量坐标系，目标上第i个特征点在测量坐标系上的坐标为p_i＝(x_ci,y_ci,z_ci)^T；

建立两个摄像机所采集的像素坐标系o-uv和o′-u′v′，o-uv为左摄像机像素坐标，o′-u′v′为右摄像机像素坐标，以成像平面左上角为原点，u、v轴分别平行于像平面坐标系的x轴和y轴。目标第i个特征点在两双目图像上的投影在其像素坐标系下的坐标分别为(u,v)^T和(u′,v′)^T；

第二步：计算特征点在测量坐标系中的坐标：

双目摄像机中两摄像机的投影矩阵分别为M₁和M₂，

M₁＝K[I 0]

M₂＝K[R_C T_C]

其中，K为摄像机的内参数矩阵，R_C和T_C为右摄像头相对于左摄像头的旋转矩阵和平移向量。

所求该特征点在测量坐标系下的齐次坐标用下式解出，

$\left[\begin{array}{c}{M}_{13}{u}_i-M_{11}\\ M_{13}{v}_i-M_{12}\\ M_{23}{u}_i-M_{21}\\ M_{23}{v}_i-M_{22}\end{array}\right]{\tilde{P}}_i=0$

M₁₁、M₁₂、M₁₃为投影矩阵M₁的行向量，M₂₁、M₂₂、M₂₃为投影矩阵M₂的行向量，设左摄像头采集图像上第i个特征点在像素坐标系下的齐次坐标为(u₁,v₁,1)^T；

第三步：求解旋转矩阵N：一个四元数包含一个标量分量和一个3D向量分量，的最大值是旋转矩阵N的最大特征值，为其对应的特征向量，N的求解公式如下，式中的S为M的分块矩阵；p为特征点在测量坐标系下的三维坐标，q为特征点在物体坐标系下的三维坐标；

假设n个特征点的质心在测量坐标系和物体坐标系中的坐标分别为和

$\begin{array}{cc}\stackrel{\‾}{p}=\frac{1}{n}\underset{i=1}{\overset{n}{\Σ}}{p}_i& \stackrel{\‾}{q}=\frac{1}{n}\underset{i=1}{\overset{n}{\Σ}}{q}_i\end{array}$

则可以得到分别以质心为原点的坐标系下的新坐标

$\begin{array}{cc}{p}_i^{\′}=p_i-\stackrel{\‾}{p}& q_i^{\′}=q_i-\stackrel{\‾}{q}\end{array}$

$N=\left[\begin{array}{cccc}{S}_{xx}+S_{yy}+S_{zz}& S_{yz}-S_{yz}& S_{zx}-S_{xz}& S_{xy}-S_{yx}\\ S_{yz}+S_{zz}& S_{xx}+S_{yy}-S_{xz}& S_{xy}+S_{yz}& S_{zx}+S_{xz}\\ S_{zx}-S_{xz}& S_{xy}+S_{yx}& -S_{xx}+S_{yy}-S_{zz}& S_{yz}+S_{zy}\\ S_{xy}-S_{yx}& S_{zx}+S_{xz}& S_{yz}+S_{zy}& -S_{xx}-S_{yy}+S_{zz}\end{array}\right]$

$\begin{array}{c}M=\underset{i=1}{\overset{n}{\Σ}}{p}_i^{\′}{q}_i^{\′T}=p^{\′T}{q}^{\′}\\ =\left[\begin{array}{ccc}{S}_{xx}& S_{xy}& S_{xz}\\ S_{yx}& S_{yy}& S_{yz}\\ S_{zx}& S_{zy}& S_{zz}\end{array}\right]\end{array}$

第四步：计算出目标对象的位移向量：

通过公式计算出位移向量由，位移向量即确定了相关的位姿参数。