一种基于双目视觉的带电作业机器人目标跟踪方法与流程

本发明属于电力技术领域，具体涉及一种基于双目视觉的带电作业机器人目标跟踪方法。

背景技术：

随着机器人技术的蓬勃发展，机器人在现代生产生活中的地位越来越重要。将带电作业机器人引入到电力产业中，代替人工进行电力维护检修工作，可以有效避免带电作业时人员伤亡事故的发生，并且能极大提高电力维护检修的作业效率。

采用机器人进行带电作业，作业人员可利用视觉系统采集并反馈的工作环境信息，远程监控机器人，完成带电作业。其中，目标跟踪用于将目标与背景区分出来，从而实现目标的精确定位。然而，在带电作业现场，环境较为复杂，设备器具较多，且这些设备器具颜色单一，不易与背景环境区分，这些因素造成了目标跟踪的困难。目前常用的跟踪方法例如基于均值偏移的目标跟踪方法一般采用颜色概率密度分布作为目标描述，在带电作业环境颜色特征不明显的情况下对目标的识别度不高，当场景中的光照变化时，目标的颜色分布也会发生变化，从而导致模型的不稳定和目标跟踪的失败；且这种方法对于目标尺度变化的适应性较差，当目标在图像中的尺度发生变化时，目标跟踪无法正常进行。

技术实现要素：

本发明提出一种基于双目视觉的带电作业机器人目标跟踪方法，可以提高带电作业机器人作业过程中对目标的识别率。

为了解决上述技术问题，本发明提出一种基于双目视觉的带电作业机器人目标跟踪方法，步骤如下：

步骤1，采集双目摄像头包括目标在内的当前帧图像；

步骤2，判断双目摄像头中左摄像头图像中是否存在标记好的目标区域，若是，则直接执行步骤3；若否，则标记并初始化目标，然后执行步骤3；所述初始化目标是指，获取目标中心点在左摄像头图像中的位置以及目标在左相机图像中的尺度信息；

步骤3，采用颜色纹理融合特征描述目标区域，具体为：

步骤3-1，选取目标hsv模型中的h分量作为目标的颜色特征，并对该特征分量进行降维处理；

步骤3-2，将双目左摄像头图像灰度化，采用加入抗扰因子的具有灰度尺度和旋转不变等价lbp纹理模型作为目标的纹理特征分量如下式所示，

其中，函数p为以左摄像头图像任意像素点nc为中心，半径为r个像素的环形邻域上的像素点个数，gc为像素点nc的灰度值，gp为像素点nc环形邻域上第p个像素点的灰度值，p∈p，a为抗扰因子，riu2代表了旋转不变等价模式，u(lbpp,r)是用来度量用二进制数表示的lbp值在空间上0与1模式的变换次数；

步骤3-3，由步骤3-1获取的目标颜色特征分量和步骤3-2获取的目标纹理特征分量计算目标颜色纹理融合特征直方图；

步骤4，采用基于均值偏移的目标跟踪方法对左摄像头图像的目标区域进行跟踪，获得目标在左摄像头当前帧图像中的位置和尺度信息；在所述基于均值偏移的目标跟踪方法中，采用颜色纹理融合特征直方图作为目标模型的特征直方图；

步骤5，采用模板匹配的方法获取目标在右摄像头图像中的位置，并根据双目立体视觉三维测量原理计算目标在世界坐标系下的坐标；

步骤6，获取双目摄像头下一帧图像；

步骤7，利用左摄像头当前帧图像中目标的尺度信息，采用基于成像原理和移动约束的尺度估计方法对目标在左摄像头图像中的尺度进行估计，对左摄像头下一帧图像中需要跟踪的目标区域进行尺度矫正。

进一步，步骤5中，所述模板匹配方法采用归一化相关系数法，归一化相关系数表示为：

其中，

式中，t为模板图像，i为待匹配图像，m、n为模板图像的宽和高，t(x′,y′)为模板图像中坐标为(x′,y′)的像素值，t(x″,y″)为模板图像中(x″,y″)的像素值，i(x+x′,y+y′)为待匹配图像坐标为(x+x′,y+y′)的像素值，i(x+x″,y+y″)为待匹配图像坐标为(x+x″,y+y″)的像素值，t′(x′,y′)、i′(x+x′,y+y′)为中间计算量，r(x,y)为待匹配图像中以(x,y)为左上角点坐标并与模板图像同样大小的待匹配区域的归一化相关系数，以此作为待匹配区域的匹配度。

进一步，步骤7中，对左摄像头下一帧图像中需要跟踪的目标区域进行尺度矫正的方法为：

步骤7-1，建立左摄像头图像像素坐标系o-uv、左摄像头图像物理坐标系o-xy，左摄像头坐标系oc-xcyczc，运用几何关系运算获取目标尺寸与其在左摄像头当前帧图像中尺度的关系，关系公式如下：

式中，zc为步骤5中计算得到的目标在左摄像头坐标系下的坐标，f为摄像头焦距，w、h分别为目标实际宽和高，wl、hl为目标在左摄像头图像中的宽和高，对于wl、hl有：其中，px、py分别为目标在左摄像头图像中宽和高占的像素个数，dx和dy分别是每个像素在u轴和v轴方向上的物理长度，dx和dy由标定得出；

步骤7-2，使用下式对下一帧图像中的目标尺度进行估计：

其中，vz为双目摄像头在左摄像头坐标系zc轴上相对目标的瞬时速度，dt为取帧时间间隔，为目标在左摄像头下一帧图像中的宽和高的估计值，对于和分别有：

步骤7-3，根据下式计算获得下一帧图像中目标宽和高所占像素个数的估计和以和作为左摄像头下一帧图像中目标尺度的估计值，对下一帧跟踪的目标范围进行矫正，

作为一个实施例，使用上述方法的带电作业机器人包括绝缘斗臂车，搭载在绝缘斗臂车上的机器人平台，安装在机器人平台上的机械臂，括数据采集系统以及数据处理和控制系统；所述数据采集系统包括设置在机器人平台上的摄像机，摄像机用于采集机械臂作业场景图像，并将所述作业场景图像发送给数据处理和控制系统；所述数据处理和控制系统根据所述作业场景图像生成3d虚拟作业场景或者规划出机械臂空间路径。

进一步，所述数据处理和控制系统包括第一工控机、第二工控机，第二工控机内置图像处理器和带电作业动作序列库；所述带电作业动作序列库中预先存储有各项带电作业对应的动作序列数据；所述摄像机采集的作业场景图像发送给第二工控机，图像处理器对作业场景图像进行处理后获的机械臂与作业对象之间的相对位置关系，第二工控机所述相对位置关系以及具体带电作业所对应的动作序列规划机械臂的空间路径，并将所述机械臂的空间路径数据发送给第一工控机；第一工控机根据所述机械臂的空间路径控制机械臂动作。

进一步，所述绝缘斗臂车上设置有控制室，所述数据处理和控制系统包括第一工控机、第二工控机、显示屏和主操作手，第二工控机内置图像处理器，显示屏和主操作手位于控制室内；主操作手与机械臂为主从操作关系，通过改变主操作手的姿态控制机械臂运动；所述摄像机采集的作业场景图像发送给第二工控机，图像处理器对作业场景图像进行处理后获的3d虚拟作业场景，并送显示器显示。

本发明与现有技术相比，其显著优点在于：

(1)本发明采用颜色纹理融合描述子对目标进行描述，对颜色特征不明显的带电作业目标设备零件，如螺栓等，具有较好的跟踪性能，并对光照变化具有较强的鲁棒性；

(2)本发明采用基于成像原理和移动约束的尺度估计方法对目标尺度进行估计，能有效解决目标尺度变化较大时目标跟踪丢失的问题。

附图说明

图1为本发明带电作业机器人的整体结构示意图；

图2为本发明中绝缘斗臂车的系统组成框图；

图3为本发明中机器人平台的结构示意图；

图4为本发明中机械臂的结构示意图。

图5为本发明基于双目视觉的带电作业机器人目标跟踪流程图。

图6为双目立体视觉三维测量原理示意图。

具体实施方式

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

结合附图，带电作业机器人包括绝缘斗臂车1、控制室2、伸缩臂3、机器人平台4。其中，绝缘斗臂车1上架设控制室2和伸缩臂3，伸缩臂3末端连接机器人平台4，机器人平台4与控制室2之间采用光纤以太网通信或者无线网络通信。

绝缘斗臂车1可供操作人员驾驶，从而将机器人平台4运输到作业现场。绝缘斗臂车1上装有支撑腿，支撑腿可以展开，从而将绝缘斗臂车1与地面稳固支撑。绝缘斗臂车1上装有发电机，从而给控制室2及伸缩臂3供电。

伸缩臂3设有沿伸缩方向的驱动装置，操作人员可以通过控制驱动装置，从而将机器人平台4升降到作业高度。该伸缩臂3由绝缘材料制成，用于实现机器人平台4与控制室2的绝缘。在本发明中，伸缩臂3可有由剪叉式升降机构或其他机构代替。

作为一种实施方式，控制室2中设置有第二工控机、显示屏、第一主操作手、第二主操作手、辅助主操作手以及通信模块等。

作为一种实施方式，机器人平台4包括绝缘子46、第一机械臂43、第二机械臂44、辅助机械臂42、第一工控机48、双目摄像头45、全景摄像头41、深度摄像头410、蓄电池49、专用工具箱47、通信模块。

机器人平台4的绝缘子46用于支撑第一机械臂43、第二机械臂44、辅助机械臂42，将这三个机械臂的外壳与机器人平台4绝缘。

蓄电池49为第一工控机48、第一机械臂43、第二机械臂44、辅助机械臂42、全景摄像头41、双目摄像头45、深度摄像头410、通信模块供电。

作为一种实施方式，双目摄像头45一共有三个，分别安装在第一机械臂43、第二机械臂44和辅助机械臂42的腕关节437上，负责采集作业场景的图像数据，并将图像数据发送给第二工控机。双目摄像头45由两个光轴平行的工业相机组成，平行光轴之间的距离固定。

深度摄像头410安装在机器人平台4正对作业场景的侧面，负责采集作业场景的景深数据，将景深数据发送给第二工控机。

全景摄像头41通过支架安装在机器人平台4的上方，负责采集作业场景的全景图像数据，将图像数据发送给第二工控机，并显示在显示器上，作业人员可以通过全景图像监控作业场景。

专用工具箱47是放置抓具、扳手等作业工具的场所。机械臂末端安装有工具快换装置。机械臂根据作业任务的类型到专用工具箱47中使用工具快换装置获取作业工具。

控制室2中第一主操作手、第二主操作手以及辅助主操作手是一种用于人工远程操作机械臂的操作装置，他们与第一机械臂43、第二机械臂44和辅助机械臂42构成主从操作关系。机械臂和主操作手具有相同的结构，只是主操作手尺寸规格比机械臂小，以便于操作人员操作。机械臂和主操作手拥有六个关节，每个关节都有光电编码器采集角度数据，各主操作手的微型控制器通过串口将六个关节的角度数据发送给第二工控机。

作为本发明一个实施例，所述机械臂为六自由度机构，包括基座431，旋转轴方向与基座平面垂直的腰关节432，与腰关节432连接的肩关节433，与肩关节433连接的大臂434，与大臂434连接的肘关节435，与肘关节435连接的小臂436，与小臂436连接的腕关节437，腕关节437由三个旋转关节组成，分别为腕俯仰关节、腕摇摆关节和腕旋转关节；所述六自由度机构中各个关节均具有相应的正交旋转编码器31和伺服驱动电机，正交旋转编码器31用于采集各个关节的角度数据，伺服驱动电机用于控制各关节的运动；第一工控机根据所述机械臂的空间路径解算出各关节的运动角度，控制伺服驱动电机按照所述运动角度控制机械臂各关节运动。

作为一种实施方式，机器人平台4与控制室2之间的数据传输通过光纤有线传输，或者使用无线网络传输。机器人平台4上的通信模块是光纤收发器，光纤收发器用于实现光纤中的光信号与双绞线中的电信号的相互转换，从而在通信上实现机器人平台4与控制室2的电气隔离。控制室2中的通信模块是光纤收发器，光纤收发器用于实现光纤中的光信号与双绞线中的电信号的相互转换，从而在通信上实现机器人平台4与控制室2的电气隔离。

作为一种实施方式，第二工控机可以完成以下任务：

建立动作序列库。预先将各项带电作业任务分解为作用序列，组成动作序列库，存储在第二工控机中，用于机械臂路径规划。

建立作业对象模型库。预先制作各项带电作业任务所涉及的作业对象的三维模型和目标识别模型，例如，根据电力塔杆、电线、耐张绝缘子、隔离刀闸、避雷器等器件实物，制作三维模型和目标识别模型，用于带电作业机器人自动识别作业对象，构建作业场景三维虚拟场景。

建立机械臂和专用工具模型库。预先制作机械臂和专用工具的三维模型和目标识别模型，例如，扳手等，用于带电作业机器人自动构建作业场景三维虚拟场景，规划机械臂空间路径。

获取图像数据。获取全景图像、深度图像和双目图像的数据信息。

根据图像数据识别和跟踪作业目标。

获取主操作手的角度、角速度和角加速度数据，获取机械臂的角度、角速度和角加速度数据。

对相关图像数据进行处理和计算，获取机械臂位置，获取作业对象的位置，获取机械臂与作业对象之间的相对位置，并根据相对位置和作业任务规划机械臂的空间路径。

根据图像数据构建作业对象三维场景，根据机械臂角度信息和作业对象三维场景获得机械臂与作业对象的相对位置，并根据相对位置和作业任务规划机械臂的空间路径。

对相关图像数据进行处理和计算，构建3d虚拟作业场景，送显示器显示，操作人员根据3d虚拟作业场景监控作业过程。与全景图像相比，3d虚拟作业场景综合和深度图像信息和双目图像信息，对机器臂与作业对象之间、机械臂之间、作业对象与作业环境之间的相对位置的判断更精确，且不会存在视觉死角。因此，操作人员通过3d虚拟作业场景进行作业监控，操作精度更高，可以防止碰撞发生，提高了安全性。同时，3d虚拟作业场景显示在控制室2中的显示器上，远离机械臂作业现场，提高了人作业人员的人身安全。

作为一种实施方式，第一工控机可以完成以下任务：

根据第二工控机发送的主操作手各关节的角度信息，控制机械臂各关节的运动。

获取第二工控机发送的机械臂的空间路径数据，根据作业任务的动作序列，解算出机械臂各关节的角度数据运动量，并控制机械臂各关节运动。

本发明中，第一机械臂和第二机械臂相互配合，可以模仿人的两个手的作业顺序完成带电作业。考虑到灵活性，可以再增加一个强壮的辅助机械臂，此时，辅助机械臂专司器件夹持等力道大的动作，第一机械臂和第二机械臂则进行相关业务操作。

根据第二工控机和第一工控机完成的不同任务的组合，本发明带电作业机器人既可以由作业人员进行远程摇操作以完成带电作业，又可以进行自主带电作业。在进行带电作业之前，作业人员先通过观察全景图像，将机器人平台4移动至作业对象附近。

如果选择人工远程摇操作，则由第二工控机根据数目图像和深度图像构建3d虚拟作业场景并送显示器显示，作业人员通过3d虚拟作业场景监控操作过程，通过主操作手控制机械臂的动作，以完成带电作业。在此过程中，作业人员改变主操作手姿态后，主操作手中各关节的光电编码器采集各关节角度，各主操作手的微型控制器通过串口将各关节的角度数据发送给第二工控机。第二工控机将主操作手各关节的角度数据作为机械臂各关节角度的期望值发送给第一工控机，第一工控机根据角度期望值通过伺服电机控制机械臂各关节的运动，已完成带电作业。

如果选择自主作业，则由第二工控机根据数目图像和深度图像计算获取作业对象和机械臂之间的相对位置关系，然后依据作业任务所对应的动作序列进行机械臂空间路径规划，并将空间路径发送给第一工控机，第一工控机解算出机械臂各关节需要转动的角度数据作为机械臂各关节角度的期望值，通过伺服电机控制机械臂各关节的运动，已完成带电作业。

结合附图，带电作业机器人在进行各种作业任务操作过程中，首先要做的就是对作业目标进行识别。本发明对作业目标进行识别的方法包括以下步骤：

步骤1，采集双目摄像头包括目标在内的图像第一帧；

步骤2，判断双目摄像头中左摄像头图像中是否存在标记好的目标区域，若是，则直接执行步骤3；若否，则标记并初始化目标，然后执行步骤3；

所述初始化目标是指，获取目标中心点在左摄像头图像中的位置以及目标在左相机图像中的尺度信息；

步骤3，采用颜色纹理融合特征描述目标区域，具体步骤如下：

步骤3-1，选取目标hsv模型中的h(色调)分量作为目标的颜色特征，并对该特征分量进行降维处理，使颜色特征分量范围为0～35；

步骤3-2，将双目左摄像头图像灰度化，采用加入抗扰因子的具有灰度尺度和旋转不变等价lbp纹理模型作为目标的纹理特征分量：

以左摄像头图像任意像素点nc为中心，在半径为r(r取1个像素)的环形邻域上，有p(p＝8)个像素点均匀分布，gc为像素点nc的灰度值，gp为像素点nc环形邻域上第p个像素点的灰度值，p∈p，则加入抗扰因子a的具有灰度尺度和旋转不变等价lbp纹理模型的表达式如下所示：

其中，

函数

中，上标riu2代表了旋转不变等价模式，它表示lbp数是旋转不变的且在空间上0与1变换次数少于2，u(lbpp,r)是用来度量用二进制数表示的lbp值在空间上0与1模式的变换次数。抗扰因子a使模型能有效区分图像中的平坦区域。至此，这种方法获得的lbp模式种类为5种。

步骤3-3，由步骤3-1获取的目标颜色特征分量和步骤3-2获取的目标纹理特征分量计算目标颜色纹理融合特征直方图，该特征直方图的维数为36×5；

步骤4，采用基于均值偏移的目标跟踪方法对左摄像头图像的目标区域进行跟踪，获得目标在左摄像头当前帧图像中的位置和尺度信息；

在所述基于均值偏移的目标跟踪方法中，采用颜色纹理融合特征直方图代替颜色直方图，将其作为目标模型的特征直方图；

步骤5，采用模板匹配的方法获取目标在右摄像头图像中的位置，并根据双目立体视觉三维测量原理计算目标在世界坐标系下的坐标；

所述模板匹配方法采用归一化相关系数法，归一化相关系数表示为：

其中，

式中，t为模板图像，i为待匹配图像，m、n为模板图像的宽和高，t(x′,y′)为模板图像中坐标为(x′,y′)的像素值，t(x″,y″)为模板图像中(x″,y″)的像素值，i(x+x′,y+y′)为待匹配图像坐标为(x+x′,y+y′)的像素值，i(x+x″,y+y″)为待匹配图像坐标为(x+x″,y+y″)的像素值，t′(x′,y′)、i′(x+x′,y+y′)为中间计算量，r(x,y)为待匹配图像中以(x,y)为左上角点坐标并与模板图像同样大小的待匹配区域的归一化相关系数，以此作为待匹配区域的匹配度。

归一化相关系数法对光照变化不敏感，利用该方法进行匹配将会降低光照变化对匹配结果的影响；

如图2所示，所述双目立体视觉三维测量原理为：

通过目标跟踪和目标匹配可获得目标中心在左、右图像平面上的图像物理坐标分别为(xl,yl)，(xr,yr)，设目标在左、右摄像头坐标系下的坐标分别为p(xc,yc,zc)，

p(xc-b,yc,zc)。基线距离b是两相机的投影中心连线的距离。由于左右像平面行对准，则目标中心点在左右图像的图像物理纵坐标相同，即yl＝yr＝y，由三角几何关系可得：

视差d＝xl-xr，根据视差原理，计算得到目标中心点p以左摄像头坐标系原点为世界坐标系原点的三维坐标：

式中，f为摄像头焦距；

步骤6，获取双目摄像头下一帧图像；

步骤7，利用左摄像头当前帧图像中目标的尺度信息，采用基于成像原理和移动约束的尺度估计方法对目标在左摄像头图像中的尺度进行估计，对左摄像头下一帧图像中需要跟踪的目标区域进行尺度矫正。该方法的原理为：

步骤7-1，如图3所示，o-uv为左摄像头图像像素坐标系，o-xy为左摄像头图像物理坐标系，oc-xcyczc为左摄像头坐标系。运用几何关系运算获取目标尺寸与其在左摄像头当前帧图像中尺度的关系，公式如下：

式中，zc为步骤5中计算得到的目标在左摄像头坐标系下的坐标，f为摄像头焦距，w、h分别为目标实际宽和高，wl、hl为目标在左摄像头图像中的宽和高，对于wl、hl有：

其中，px、py分别为目标在左摄像头图像中宽和高占的像素个数(当前帧图像中目标的尺度信息)，dx和dy分别是每个像素在u轴和v轴方向上的物理长度，dx和dy由标定得出；

步骤7-2，考虑机械臂带动双目摄像头移动，则需获取双目摄像头在zc轴(左摄像头坐标系)上相对目标的瞬时速度，这可由机械臂腕部瞬时相对速度代替，记为vz，取帧时间间隔记为dt，则对下一帧的目标尺度估计有：

其中，为目标在左摄像头下一帧图像中的宽和高的估计值。对于和分别有：

步骤7-3，计算可得下一帧的目标宽和高的所占像素个数估计和为：

以和作为左摄像头下一帧图像中目标尺度的估计值，对下一帧跟踪的目标范围进行矫正；

步骤8，判断是否跟踪结束，若是，结束跟踪，若否，返回步骤二，继续跟踪目标。