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

本发明属于电力技术领域，具体涉及一种带电作业机器人作业环境3D重构方法。

背景技术：

目前，我国高压带电作业，主要采用绝缘手套作业法，要求操作人员攀爬高压铁塔或借助绝缘斗臂车进行不停电作业。但人工带电作业稍有不慎就容易发生人身伤亡事故，给家庭和社会带来严重的损失。

目前，国内已研发的一款带电作业机器人，仍然需要操作人员在绝缘斗内随机器人升至线路附近，没有从根本上解决操作人员的生命安全问题。并且，该作业机器人完全由操作人员控制，不能自主完成带电作业，较传统的绝缘手套作业法反而效率更低。

技术实现要素：

本发明提出一种带电作业机器人作业环境3D重构方法，在三维空间中对障碍物建模，并应用3D重构技术在监控显示器显示中，再现机器人作业的真实场景，以便操作人员发出相应的指令。

为了解决上述技术问题，本发明提供一种带电作业机器人作业环境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为右摄像头相对于左摄像头的旋转矩阵和平移向量。

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

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

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

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

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

第四步：计算出目标对象的位移向量：通过公式计算出位移向量由，位移向量即确定了相关的位姿参数。

本发明与现有技术相比，其显著优点在于，本发明中，操作人员通过主从遥操作方式，将带电作业机器人平台送至自主作业工位。机器人到达自主作业工位后，将根据操作人员下达的任务指令，完成带电作业任务。在作业机械手自主作业时，必须避免碰到障碍物，否则可能会引起线路损坏或断电。为使系统自主作业时具有避障功能，需要在三维空间中对障碍物建模，并应用3D重构技术在监控显示器显示中，再现机器人作业的真实场景，以便操作人员发出相应的指令。

附图说明

图1是带电作业场景结构示意图。

图2是位姿计算流程图。

图3是作业环境重构流程图。

图4是带电作业机器人系统示意图。

图5是机器人平台示意图。

具体实施方式

容易理解，依据本发明的技术方案，在不变更本发明的实质精神的情况下，本领域的一般技术人员可以想象出本发明带电作业机器人作业环境3D重构方法的多种实施方式。因此，以下具体实施方式和附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限制或限定。

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

具体包括如下步骤：

一、场景构建

所构建场景包括天空远景、电线杆塔、机械臂(包括第一机械臂43、第二机械臂44、辅助机械臂42)、电线、单个元器件和机械手专用工具。其中，机械臂包括第一机械臂43、第二机械臂44和辅助机械臂42。单个元器件包括绝缘子、跌落式熔断器以及隔离刀闸等。机械手专用工具包括电动扳手、自动破螺母工具、夹持器以及自动剥皮器等等。

基于系统双机械臂、辅助机械手、电线杆塔、单个元器件，电线等的实体装置按比例建立三维模型。

机械臂建模。建模过程中釆用测绘技术或机械臂的结构参数，先建立机械臂的等比例的三维模型。由于机械臂机械结构复杂，为了保证仿真的有效性，先对机械臂各个构件单独建立三维模型，然后利用三维软件的装配功能对机械臂各个构件按按照自由度进行构件装配。

单个元器件建模。需要先根据单个元器件详细的尺寸参数，建立一个平面图，然后再通过各种特征的建立和修改，构造其三维模型。

电线杆塔建模。通过单个元器件的建模，当杆塔的各部分组成元器件模型均建立后，根据三维场景中模型的全部可见面，去除被物体遮挡面和不可见面，将杆塔上的电力元器件装配成一个整体。

天空远景建模。天空背景通过实地拍摄，由摄像机传入计算机，运用Photoshop软件进行修正，生成纹理，再转换成渲染引擎软件可用的格式。采用半圆球法给天空建模，用加盖一个笼罩整个地形的半球面作为“顶”，加上适当光照效果，产生强烈的纵深感。

在给所有部件建模之后，布局带电作业场景时依据单个元器件、电线杆塔、天空远景、机械臂之间的机械连接关系，需对照部件实物及其机械手册，获取模型尺寸的比例关系，需对作业场景和带电作业机器人系统，进行多个角度的全景拍摄，通过对这些图像的分析组成线路杆塔的各种设备和机械手各机械部件的标准参数。

在渲染引擎里集成各个部件后，要给场景进行加工修饰，准确匹配各个部件在虚拟光源照射下所产生的实体亮度、阴影强度。

机械臂模型的渲染驱动：接收第一工控机48发送给机械臂的各个电机的控制信号，依据机械臂模型的物理性质以及机电特性，模拟出机械臂的运动轨迹并显示于监控画面。

连续拍摄第一机械臂43、第二机械臂44、辅助机械臂42双目图片，对包围待识别物体的特征点的区域进行特征点形态开闭运算边缘检测提取，与事先准备好的特征点模板进行匹配，匹配度最高的即为特征点，将特征点中心坐标输入对象三维模型，作为相机坐标系下的坐标。

利用双目摄像头45，从两个视点同时观察同一场景获取的目标图像，通过三角测量原理，计算图像像素间的视差来解算特征点在测量坐标系下的三维坐标，根据物体上对应特征点在测量坐标系下空间坐标以及物体坐标系下空间坐标，求解目标的位置姿态参数。下面是具体的计算过程

第一步：建立坐标系：

建立物体坐标系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为右摄像头相对于左摄像头的旋转矩阵和平移向量。

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

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

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

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

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

第四步：计算出目标对象的位移向量：位移向量由公式可以算出，位移向量即确定了相关的位姿参数，其中s的求解办法如下，