CN106493708B - 一种基于双机械臂和辅助臂的带电作业机器人控制系统 - Google Patents

Abstract

本发明提出一种基于双机械臂和辅助臂的带电作业机器人控制系统，第一机械臂、第二机械臂和辅助机械臂上均搭载有双目摄像头；所述双目摄像头用于采集机械臂作业场景图像，并将所述作业场景图像发送给数据处理和控制系统；所述数据处理和控制系统根据所述作业场景图像规划出机械臂空间路径，具体方法为：将机械臂末端位置与作业目标位置转换到同一参考坐标系下，获取械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差；根据所述坐标之差使用运动学逆解计算方法解算出机械臂每个关节的角度期望值。本发明基于双目视觉结合坐标变换的方法自主式地控制机械臂完成配电线路带电作业的任务，减轻作业人员的劳动强度，提高了安全性。

Description

一种基于双机械臂和辅助臂的带电作业机器人控制系统

技术领域

本发明属于电力技术领域，具体涉及一种基于双机械臂和辅助臂的带电作业机器人控制系统。

背景技术

带电作业是在高压电气设备上不停电进行检查、维修及部件更换的一种特殊的工程技术。长期以来,我国普遍釆用的是人工带电作业或者主从操作的作业方式。对于作业对象是10KV及以下的高压架空线路,这些作业方式要求作业人员长时间处在高空、高压、强电磁场的环境中,极易发生人身伤亡的事故。

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

面向这些较为复杂且危险的作业任务，需要自主作业机器人具有很高的智能，带电作业安全可靠，这对整个控制系统的设计提出了很高的要求。为使作业机械手具有一定的自主作业能力，需具备多种关键技术，使其能够安全可靠地自主完成带电作业任务。

发明内容

本发明提出一种基于双机械臂和辅助臂的带电作业机器人控制系统，基于双目视觉结合坐标变换的方法自主式地控制机械臂完成配电线路带电作业的任务，减轻作业人员的劳动强度，提高了安全性。

为了解决上述技术问题，本发明提供一种基于双机械臂和辅助臂的带电作业机器人控制系统，包括绝缘斗臂车，搭载在绝缘斗臂车上的机器人平台，安装在机器人平台上的机械臂，还包括数据处理和控制系统；所述机械臂包括第一机械臂、第二机械臂和辅助机械臂，所述摄像机包括双目摄像头，所述第一机械臂、第二机械臂和辅助机械臂上均搭载有双目摄像头；所述双目摄像头用于采集机械臂作业场景图像，并将所述作业场景图像发送给数据处理和控制系统；所述数据处理和控制系统根据所述作业场景图像规划出机械臂空间路径，具体方法为：将机械臂末端位置与作业目标位置转换到同一参考坐标系下，获取械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差；根据所述坐标之差使用运动学逆解计算方法解算出机械臂每个关节的角度期望值。

进一步，使用D-H参数法以机械臂基座的中心点为原点建立基坐标系，将机械臂末端位置与作业目标位置均转换到基坐标系下；

将机械臂末端位置转换到基坐标系下的过程为：

1-1：在机械臂的每一个关节处建立连杆坐标系，并在机械臂末端执行工器具处建立工具坐标系；以双目摄像头连线的中垂点为原点建立双目坐标系；

1-2：由机械臂各关节处的编码器采集每个关节当前的角度数据并发送给第二工控机，第二工控机根据关节角度确定机械臂基座到末端执行工器具间的总连杆变换矩阵；

1-3：根据所述总连杆变换矩阵获得机械臂末端在基坐标系下的坐标。

将作业目标的位置转换到基坐标系下的过程为：

2-1：通过模板匹配的方法获取作业目标在双目摄像机左、右图像中的位置，并根据立体视觉三维测量原理计算获得作业目标在双目坐标系下的坐标；

2-2：将作业目标在双目坐标系下的坐标左乘平移坐标变换矩阵得到目标在机械臂腕旋转关节坐标系下的坐标，再左乘腕旋转关节坐标系到基坐标系的连杆变换矩阵得到作业目标在基坐标系下的坐标。

进一步，所述数据处理和控制系统包括第一工控机、第二工控机，第二工控机内置图像处理器和带电作业动作序列库，所述带电作业动作序列库中预先存储有各项带电作业对应的动作序列数据；所述摄像机采集的作业场景图像发送给第二工控机，图像处理器对作业场景图像进行处理后获的械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差，第二工控机根据械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差以及具体带电作业所对应的动作序列解算出机械臂每个关节的角度期望值，并将所述角度期望值数据发送给第一工控机；第一工控机根据所述角度期望值控制机械臂动作。

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

进一步，所述机械臂或者主操作手为六自由度机构，包括基座，旋转轴方向与基座平面垂直的腰关节，与腰关节连接的肩关节，与肩关节连接的大臂，与大臂连接的肘关节，与肘关节连接的小臂，与小臂连接的腕关节，腕关节由三个旋转关节组成，分别为腕俯仰关节、腕摇摆关节和腕旋转关节；所述六自由度机构中各个关节均具有相应的正交旋转编码器和伺服驱动电机，正交旋转编码器用于采集各个关节的角度数据，伺服驱动电机用于控制各关节的运动；第一工控机根据机械臂各关节角度的期望值，通过控制伺服驱动电机控制按机械臂各关节运动。

进一步，各机械臂间之间分配有优先级，第一机械臂设置为第一优先级，第二机械臂设置为第二优先级，辅助机械臂的优先级在其需要抓取执行工器具时设置为第一优先级，在不需要抓取执行工器具时，设置为第三优先级，优先级最高的机械臂在执行任务过程中不受约束，优先级较低的机械臂在执行任务过程中主动规避与优先级最高的机械臂发生避碰。

本发明与现有技术相比，其显著优点在于，该控制系统是基于双目视觉结合坐标变换的方法自主式地控制机械臂完成配电线路带电作业的任务，减轻作业人员的劳动强度，提高了安全性。另外，相较于常规的双机械臂带电作业人增加了辅助作业机械臂，它可以与双机械臂进行协同作业，完成测量和监测的任务，并且机械臂之间可以共享图像信息。通过双机械臂与辅助机械臂的协同作业，能够完成一些以往较为难完成的作业任务，如：更换跌落式熔断器，更换隔离刀闸等等，提高了作业的可靠性与便捷性。

附图说明

图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的腕旋转关节439上，负责采集作业场景的图像数据，并将图像数据发送给第二工控机。双目摄像头45由两个光轴平行的工业相机组成，平行光轴之间的距离固定。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

带电作业机器人控制系统

上述基于双机械臂(第一机械臂和第二机械臂)和辅助机械臂的带电作业机器人中，双机械臂是该带电作业机器人的主体操作部件，作业时需要直接接触带电线路等带电体，并通过工控机发出的运动指令来完成相应动作，应能够实现速度、位置控制。且第一机械臂和第二机械臂具有良好的协作能力。第一机械臂和第二机械臂末端需配有符合国标的机械接口，根据带电作业任务的不同，可以更换不同的专用作业工器具。利用这些专用工器具，带电作业机械臂可完成拆装螺母、拆接导线等作业。

机器臂采用电驱动，具有体型灵巧，自重低，持重相对高，安装尺寸小、关节的控制驱动一体化及编程开放性好的特点。机械臂关节处安装有光电编码器测量关节转角。使用的主机械臂与辅助机械臂均为6个自由度，由基座、腰关节、肩关节、肘关节、腕关节组成，各关节之间通过连杆连接，其中：腕关节有三个自由度，其他各有一个自由度。本实施例使用的六自由度主机械臂如图4所示。以第一机械臂43为例，六自由度机械臂由基座431、腰关节432、肩关节433、大臂434、肘关节435、小臂436、腕关节、夹爪接口4310组成，腕关节又包括三个关节，即腕俯仰关节437、腕摇摆关节438、腕旋转关节439。因此，腕关节有三个自由度，其他各关节各有一个自由度。机器人本身是由挤压铝管和关节组成的手臂。基座431是机器人的安装位置，机器人的另一端与从机械臂专用工具箱47中取出工具相连。通过协调各个关节的运动，机器人可随意移动工具进行作业。夹爪接口4310用于连接机械臂专用夹爪，夹爪用于抓起或连接机械臂专用工具箱47内的末端工器具。

第一机械臂和第二机械臂为主要的作业执行器，进行装卸螺栓螺母、接搭引线等操作；辅助机械臂与主机械臂同构，具有更大的尺寸，主要用于协作双机械臂进行抓取、固定和移动隔离变压器、跌落式熔断器等室外输配电设备以及夹持引线等等作用，提高作业可靠性；

三个机械臂各有一个伺服控制器，使其可以进行位置、速度控制。机械臂之间可以进行一定的协调作业，保证在作业时可通过最优路径进行自主作业，避免发生相互碰撞或碰撞障碍物如带电引线、配电设备等，确保机器人执行作业任务的可靠性、安全性和稳定性；

带电作业机器人采用两台工业控制计算机(第一工控机和第二个工控机)进行信息处理和系统控制，具有大量可扩展外设接口，分别与每个机械臂控制器通过以太网接口或串口控制。其中，第一工控机主要用于机械臂控制，第二工控机用于图像处理，机械臂运动学计算，坐标系变换处理等过程。

在开始作业前需要为带电作业机器人系统建立模型和生成动作序列，具体包括如下工作：

1、通过主从遥操作使机械臂移动到目标作业区域附近，确保作业对象的目标在双目视觉的识别范围内；

2、使用D-H参数法以机械臂基座的中心点为原点建立基坐标系，在机械臂的每一个关节处建立连杆坐标系，并在专用工具处建立工具坐标系。

基坐标系为参考坐标系，从基座开始变换到肩关节，然后到肘关节，最后到末端的腕关节。A_i为连杆坐标系i-1到连杆坐标系i之间的连杆变换矩阵。

其中，θ_i,α_i,a_i和d_i分别为D-H模型参数中的关节角、扭转角、连杆长度和连杆偏移量。

3、以双目摄像机中两相机连线的中垂点为原点建立双目坐标系，为方便进行平移坐标变换到腕旋转关节坐标系，令双目坐标系的三个轴的方向与腕旋转关节坐标系的三个轴的方向相同；

4、通过人工选择或双目视觉识别作业任务。根据带电作业机器人自主作业方法生成动作序列，它包含机械臂自主作业时每一步的路径及动作顺序；

机械臂的自主作业控制步骤及如下：

步骤一、确定机械臂末端当前的位姿

1-1：使用D-H参数法以机械臂基座的中心点为原点建立基坐标系，在机械臂的每一个关节处建立连杆坐标系，并在机械臂末端执行工器具处建立工具坐标系；以双目摄像头连线的中垂点为原点建立双目坐标系；

1-2：由机械臂各关节处的编码器采集每个关节当前的角度数据并发送给第二工控机，第二工控机根据关节角度确定机械臂基座到末端执行工器具间的总连杆变换矩阵，根据当前所连接的末端专用作业工器具的长度获取工器具末端在工具坐标系下的坐标

设T_t为基座到机械臂工具坐标系的变换矩阵，A_t为工具坐标系相对于腕旋转关节坐标系下的坐标变换矩阵。因此，机械臂基座到末端末端执行工器具间的总连杆变换矩阵为:

其中为旋转变换矩阵，与机械臂的关节角度θ有关。其中，n为法向矢量，o为方向矢量，a为接近矢量。总连杆变换矩阵右上方的3*1矩阵为平移变换矩阵，即为当前机械臂工具坐标系原点在基坐标系下的坐标。计算右上方的3*1的矩阵P_t即为机械臂末端在基坐标系下的坐标。

步骤二、获取作业目标的位置

2-1：通过模板匹配的方法获取作业目标在左、右摄像机图像中的位置，并根据立体视觉三维测量原理计算获得目标在双目坐标系下的坐标x_m，y_m，z_m为由双目视觉通过三维测量图像处理后得到的目标作业位置在双目坐标系下的坐标。

2-2：将目标在双目坐标系下的坐标左乘平移坐标变换矩阵A_m得到目标在机械臂腕旋转关节坐标系下的坐标，再左乘腕旋转关节坐标系到基坐标系的连杆变换矩阵T₆得到目标在基坐标系下的坐标。

因此，双目坐标系相对于基坐标系的坐标变换矩阵T_m为：T_m＝T₆A_m，其中A_m为双目坐标系相对于腕旋转关节坐标系的坐标变换矩阵，由双目相机在机械臂上的安装位置决定。

计算其中，T_m，分别由前述计算得到，为左边的两个矩阵相乘获得。R_m为3*3的旋转变换矩阵。右上角的矩阵P_m为3*1的平移变换矩阵，即为机械臂工具坐标系原点在基坐标系下的坐标；

步骤三、进行机械臂空间路径规划

3-1：第二工控机计算机械臂末端位置与作业目标位置在同一基坐标系下的坐标之差；

3-2：第二工控机利用机械臂的运动学逆解计算方法进行运动轨迹规划。即通过比较以下两式的办法解得机械臂每个关节角位移θ_i的所有解；

根据机械臂工作情况，通过结合虚拟现实技术的模拟避障要求求得θ_i的最优唯一解。

3-3、第二工控机将解得的关节位移角度发送给第一工控机，第一工控机将角度信号发送给机械臂内部的伺服控制器，控制机械臂各关节运动至目标作业点；

3-4、到达目标作业点后，第一工控机继续根据动作序列控制末端作业工器具完成作业动作。

步骤四、在完成动作后返回步骤一继续移动到下一位置。若完成所有动作指令，机械臂复位到初始位置。

本发明中的带电作业机器人采用双机械臂加辅助臂协同工作的方式，增加了灵活性和容错率的同时提高了空间复杂程度和发生冲突的几率。其协同工作方法主要体现在：

1、各机械臂间的防碰撞方式：采用结合虚拟现实技术的优先级自主式避碰方式。

首先，为机械臂之间分配优先级：定义第一机械臂为主机械臂，优先级高，第二机械臂为副机械臂，优先级低。辅助机械臂的优先级在其需要进行抓取操作时设置为第一优先级，在不需要进行抓取，只完成测量与监测任务时为最低的优先级。优先级最高的机械臂在执行任务过程中不受约束，优先级较低的机械臂执行避碰操作。

在执行任务的过程中，根据机械臂当前位姿的测量原理结合虚拟现实技术计算并构建出每个机械臂在三维虚拟场景中的模型，计算机械臂之间的最短距离并找出最短距离对应的点的位置，由该点的位置确定需要重新调整的关节。由于虚拟现实场景对于实际过程中存在延迟，并且机械臂在作业运动时存在初速度，因此需设定安全距离。如该距离小于设定的安全距离下，则对优先级较低机械臂重新选择运动学逆解，并在虚拟现实场景中进行验证是否最短距离已大于安全距离，直至两个机械臂之间的最短距离都大于安全距离，并且三个机械臂之间距离都满足避碰的安全距离要求。从而实现机械臂之间的避障控制。

2、盲区测量：在实际测量中，双机械臂末端的双目摄像机需要识别并跟踪目标作业点的位置，但由于其尺寸及动作的局限性，在双目视觉测量过程中存在盲区。因此在双机械臂双目视觉对于处在盲区的作业目标不能识别到的情况下，利用辅助机械臂更大更灵活的优势，通过主从操纵辅助机械臂上的双目摄像机对作业目标的位置进行测量，从而利用获取目标作业位置的方法得到目标位置在辅助机械臂基坐标系下的位置信息。

由于双机械臂以及辅助机械臂的基座是相对固定于机器人平台上的，因此可以通过坐标变换将目标在辅助机械臂基坐标下的坐标转化为双机械臂中任一基坐标系下的坐标，从而协助双机械臂对目标进行识别与跟踪，实现信息共享。

3、作业监测：在双机械臂执行主要的操作任务时，辅助机械臂可完成带电作业任务完成情况的监测，通过将反馈数据发送给工控机进行处理，判断工作任务是否执行完毕。

当双机械臂在执行装卸螺栓螺母的任务时，由于机械臂内部的力反馈传感器的灵敏度不高，在执行该工作时无法通过力反馈判断螺栓的安装情况。由于双机械臂上的双目摄像机在作业过程中无法直接测量螺栓与螺母之间的间隙，因此需要控制辅助机械臂移动到相应位置上采集螺栓与螺母之间的间隙距离，发送给工控机判断螺栓是否被拧紧，形成反馈控制回路，从而完成了作业工作的监测任务。

Claims (5)

1.一种基于双机械臂和辅助臂的带电作业机器人控制系统，包括绝缘斗臂车，搭载在绝缘斗臂车上的机器人平台，安装在机器人平台上的机械臂，其特征在于，还包括数据处理和控制系统；所述机械臂包括第一机械臂、第二机械臂和辅助机械臂，摄像机包括双目摄像头，所述第一机械臂、第二机械臂和辅助机械臂上均搭载有双目摄像头；所述双目摄像头用于采集机械臂作业场景图像，并将所述作业场景图像发送给数据处理和控制系统；所述数据处理和控制系统根据所述作业场景图像规划出机械臂空间路径，具体方法为：将机械臂末端位置与作业目标位置转换到同一参考坐标系下，获取械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差；根据所述坐标之差使用运动学逆解计算方法解算出机械臂每个关节的角度期望值；

使用D-H参数法以机械臂基座的中心点为原点建立基坐标系，将机械臂末端位置与作业目标位置均转换到基坐标系下；

将机械臂末端位置转换到基坐标系下的过程为：

1-1：在机械臂的每一个关节处建立连杆坐标系，并在机械臂末端执行工器具处建立工具坐标系；以双目摄像头连线的中垂点为原点建立双目坐标系；

1-2：由机械臂各关节处的编码器采集每个关节当前的角度数据并发送给第二工控机，第二工控机根据关节角度确定机械臂基座到末端执行工器具间的总连杆变换矩阵；

1-3：根据所述总连杆变换矩阵获得机械臂末端在基坐标系下的坐标；

将作业目标的位置转换到基坐标系下的过程为：

2-1：通过模板匹配的方法获取作业目标在双目摄像机左、右图像中的位置，并根据立体视觉三维测量原理计算获得作业目标在双目坐标系下的坐标；

2-2：将作业目标在双目坐标系下的坐标左乘平移坐标变换矩阵得到目标在机械臂腕旋转关节坐标系下的坐标，再左乘腕旋转关节坐标系到基坐标系的连杆变换矩阵得到作业目标在基坐标系下的坐标。

2.如权利要求1基于双机械臂和辅助臂的带电作业机器人控制系统，其特征在于，所述数据处理和控制系统包括第一工控机、第二工控机，第二工控机内置图像处理器和带电作业动作序列库，

所述带电作业动作序列库中预先存储有各项带电作业对应的动作序列数据；

所述摄像机采集的作业场景图像发送给第二工控机，图像处理器对作业场景图像进行处理后获的机械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差，第二工控机根据机械臂末端位置和作业目标位置在同一参考坐标系下的坐标之差以及具体带电作业所对应的动作序列数据解算出机械臂每个关节的角度期望值，并将所述角度期望值数据发送给第一工控机；

第一工控机根据所述角度期望值控制机械臂动作。

3.如权利要求1基于双机械臂和辅助臂的带电作业机器人控制系统，其特征在于，所述绝缘斗臂车上设置有控制室，所述数据处理和控制系统包括第一工控机、第二工控机、显示屏和主操作手，第二工控机内置图像处理器，显示屏和主操作手位于控制室内；主操作手与机械臂为主从操作关系，通过改变主操作手的姿态控制机械臂运动；所述摄像机采集的作业场景图像发送给第二工控机，图像处理器对作业场景图像进行处理后获的3D虚拟作业场景，并送显示器显示。

4.如权利要求1至3所述任意一基于双机械臂和辅助臂的带电作业机器人控制系统，其特征在于，所述机械臂或者主操作手为六自由度机构，包括基座，旋转轴方向与基座平面垂直的腰关节，与腰关节连接的肩关节，与肩关节连接的大臂，与大臂连接的肘关节，与肘关节连接的小臂，与小臂连接的腕关节，腕关节由三个旋转关节组成，分别为腕俯仰关节、腕摇摆关节和腕旋转关节；

所述六自由度机构中各个关节均具有相应的正交旋转编码器和伺服驱动电机，正交旋转编码器用于采集各个关节的角度数据，伺服驱动电机用于控制各关节的运动；

第一工控机根据机械臂各关节角度的期望值，通过控制伺服驱动电机控制机械臂各关节运动。

5.如权利要求1至3所述任意一基于双机械臂和辅助臂的带电作业机器人控制系统，其特征在于，各机械臂间之间分配有优先级，第一机械臂设置为第一优先级，第二机械臂设置为第二优先级，辅助机械臂的优先级在其需要抓取执行工器具时设置为第一优先级，在不需要抓取执行工器具时，设置为第三优先级，优先级最高的机械臂在执行任务过程中不受约束，优先级较低的机械臂在执行任务过程中主动规避与优先级最高的机械臂发生避碰。