高压带电作业双臂机器人系统设计

王 康，刘 亮，李慧杰，余志森，龙 飞，蒋 鑫，陈双叶，白劲松，田悦谷

(1.华北电力科学研究院有限责任公司，北京 100045；2.北京工业大学人工智能与自动化学院，北京 100124)

0 引言

为保障国民经济和社会持续发展，提高配电网络的供电能力和供电可靠性，对高压配电网带电作业检修提出了更高的要求。一方面要求电力部门提高检修效率，减少电力故障带来的损失，另一方面也要努力降低电力检修成本、减轻检修人员的劳动强度，更重要是要保障检修人员的生命安全，防止高压、高空、高劳动强度带来的伤亡事故[1-3]。因此如何利用科技手段提高高压配电线路带电作业的自动化水平、减轻带电作业劳动强度、保障操作人员的人生安全就成为了行业研究热点[7-15]。

针对10 kV配电网带电作业的实际需求，设计开发了双臂六轴机器人系统。该机器人系统采用协作型六轴机械臂进行扩展设计，在6轴机械臂末端扩展绝缘臂和作业工具来实现带电作业，如断、接引流线、更换绝缘子。带电作业机器人包含机械臂、绝缘扩展臂、作业工具、快接装置、视觉定位系统、遥操作控制盘，带电作业机器人结合了现代的机器人技术、传感器技术、视觉图像处理与定位技术，通讯技术以及人工智能技术，是一个集多学科、多领域技术的交叉融合的综合智能系统。

机器人系统采用wifi和LoRa无线通讯网络，通过遥操作控制盘，可以实现对六轴机械臂和专用作业工具的操作控制，可以代替绝大部分人工带电作业，大大减轻作业人员的劳动强度，同时也有利于保障作业人员的人生安全。

1 双臂六轴机器人系统组成

高压带电作业双臂机器人系统结构如图1所示，由左、右两台六轴机械臂、深度相机、视觉定位系统、路由器、机器人控制器以及机械臂末端专用作业工具构成。机械臂末端专用作业工具采用快接方式进行换装，不同作业采用不同的专用工具，专用作业工具和机器人本体之间采用600 mm长高压绝缘杆连接；两台六轴机械臂和视觉定位系统通过RJ45连接到路由器，并一起安装在可升降的移动平台上，机器人控制器通过WIFI接入到路由器构成的局域网，深度相机通过USB连接到视觉算法计算机，由视觉算法计算机完成目标识别和视觉定位。在该局域网中，两台六轴机械臂，视觉定位系统均作为TCP SERVER，分别具有不同的IP地址和端口号，机器人控制器作为TCP CLIENT，可随时和服务器端建立连接，实现对双臂六轴机器人的操作控制。

图1 高压带电作业双臂机器人

1.1 六自由度协作型机械臂

双臂六轴机器人系统采用6自由度协作型机械臂，包括左机械臂、右机械臂以及机械臂安装底座，如图2所示。

图2 六轴协作型机械臂

双臂机器人底座安装在可移动式升降机作业平台上，最大升降高度12米。六自由度协作型机械臂模仿人的手臂，共有6个旋转关节，每个关节表示一个自由度，机械臂关节包括2个肩部，一个肘部和3个腕部，每个关节之间都采用臂管连接，控制各个关节转动，就可以控制机械臂末端移动到不同的位姿。机械臂大臂长615 mm，小臂572.5 mm，腕部116.5 mm，臂展1 533 mm。机械臂工作空间，是一个半径为1 304 mm的球体，基座正上方和正下方的圆柱形空间是奇异点区。奇异点是机器人因配置或者轨迹的命令速度导致关节速度超过安全限制而失去自由度的位置。在对机器人进行控制时，应避免出现奇异点，因为奇异点的出现会导致机械臂突然或快速移动，或者中断程序导致其过早停止。6轴机械臂不能连续旋转，只能在指定的范围内工作，除关节3的旋转角度在负168o至正168o，其余关节均可在负360o至正360o范围内工作。

Denavit-Hartenberg参数或D-H参数是表示机器人正向运动学模型的传统方式，用于在给定机器人各关节角度的情况下，在基座坐标系中表示工具中心点的坐标位置，机械臂D-H参数模型如表1所示。

表1 D-H参数模型

机械臂的正解函数可以通过JSON协议获得：

{" jsonrpc "："2.0"，" method "：" positiveKinematic "，"params "：{" targetPos "：

targetPos }，"id"：id}，通过正解函数，根据机械臂关节角信息得到对应的位姿信息，参数 targetpos为目标关节角度信息，机械臂返回参数为六轴位姿信息。假设机器人ip地址为：192.168.1.200，则通过SOCKET编程获取机械臂正解的示例如下：

设置机械臂的ip地址

r o b o t _ i p =” 1 9 2. 1 6 8. 1. 2 0 0 ”

建立和机械臂的通讯连接

conSuc, s o c k = c o n n e c t E T C o n t r o l l e r ( r o b o t _ i p )

获取机械臂当前的位置信息

i f ( conSuc ) ： suc, r e s u l t, i d = sendCMD ( sock, ” g e t R o b o t P o s ” )

向机械臂发出JSON指令,等待机械臂返回由正解函数得到的6轴位置信息。

suc, r e s u l t, i d =sendCMD ( sock, ” p o s i t i v e K i n e m a t i c ”, { ” t a r g e t P o s ” ： r e s u l t} )

获得机械臂的逆解函数的JSON协议：

{" jsonrpc "："2.0"," method "：" inverseKinematic ","params "：{" targetPose "：

targetPose },"id"：id},逆解函数根据位姿信息得到对应的机械臂关节角度信息,targetpose为目标位姿信息,机械臂响应返回关节角度信息。逆解函数通讯示例如下：

指定机械臂ip地址

r o b o t _ i p =” 1 9 2. 1 6 8. 1. 2 0 0 ”

和机械臂建立通讯连接

conSuc, s o c k = c o n n e c t E T C o n t r o l l e r ( r o b o t _ i p )

获取机器人当前位姿信息

i f ( conSuc ) ：

suc, r e s u l t, i d = sendCMD ( sock, ” g e t R o b o t P o s e ” )

向机器人发送JSON协议指令,得到机械臂由逆解函数得到的关节角信息。

suc, r e s u l t, i d =sendCMD ( sock, ” i n v e r s e K i n e m a t i c ”, { ” t a r g e t P o s e ” ： r e s u l t} )

也可以通过JSON协议{" jsonrpc "："2.0"，" method "：" inverseKinematic "，"params "：{" targetPose "：targetPose，" referencePos "：referencePos }，"id"：id} 得到带参考点的位置逆解，可以根据位姿信息得到对应的机械臂关节角信息，参数targetPose为目标位姿信息，referencePos为逆解参考点关节角信息。示例代码如下：

设置机器人IP 地址

r o b o t _ i p =” 1 9 2. 1 6 8. 1. 2 0 0 ”

指定参考点

P000 =[ 0, ?90, 90, ?90, 90, 0, 0, 0 ]

和机械臂建立通讯连接

conSuc, s o c k = c o n n e c t E T C o n t r o l l e r ( r o b o t _ i p )

获取机器人当前位姿信息

i f ( conSuc ) ：

suc,result,i d=sendCMD(sock,”getRobotPose”)

得到带参考点位置逆解

suc, r e s u l t, i d =sendCMD ( sock, ” i n v e r s e K i n e m a t i c ”, { ” t a r g e t P o s e ” ： r e s u l t, ” r e f e r e n c e P o s ” ： P000 } )

机械臂从起始位置运动到目标位需要将其离散成多个中间目标位置，决定中间离散点和离散点之间以何种轨迹运动的方法称为插补方法。常用的插补算法有空间直线插补算法和空间圆弧插补算法，插补算法按插补方式又有脉冲增量插补和数据采样插补[25]。采用直线插补算法可以控制机器人末端工具的端点移动路径为直线，而机器人手腕则是根据插补算法得到的插补位置自动变化；圆弧插补可以控制机器人末端工具按指定的圆弧轨迹运动，在圆弧轨迹的运动过程中，机器人腕关节的运动控制由圆弧插补算法确定。

机械臂提供了多个IO接口以及网络通讯接口，利用机械臂提供的IO接口，可根据应用需求扩展机械臂末端的操作工具，通过网络通讯接口，采用SOCKET编程，可实现对机械臂的运动规划和姿态控制。

软件设计上，通过SOCKET通讯向机械臂以太网端口发送指定格式的json字符串来实现相应的功能，采取客户端/服务器模型，客户端主动发送json字符串，服务器端接收json串并解析，根据解析结果返回给客户端应答json串。json通讯协议如下。

客户端发送json字符串：

{“jsonrpc”：“2.0”,“method”：方法名称,“params”：参数,“id”： id }

机械臂控制器收json字符串,并解析,若结果正常,则向客户端回复json串：

{“jsonrpc”： “2.0”, “result”： 结果, “id”： id }

若结果错误,则回复json串：

{“jsonrpc”： “2.0”, “error”： {“code”： 错误代码, “message”： 出错信息}, “id”： id }

发送json 字符串时的id 和接收结果时的id 一致。比如要设置机械臂负载重心,只需要向机械臂发送json字符串：

课堂提问比较常见，而要想把课堂提问的有效作用充分发挥出来，却是一件非常有技术含量的事情。作为教师来说，都应该明白这个道理，如果整节课教师一直在讲解而无视学生的状态，那么，这节课的教学效果肯定不好。因为课堂教学应是师生间的双边活动，讲究的是以教师为主导，以学生为主体，学生才是课堂的主人。

{“jsonrpc”： “2.0”, “method” ： “ cmd_set_payload”, “params”：{ “tool_num” ： tool_num, “m” ：m, “point” ： point}, “id” ：id}

其中：tool_num：工具号,范围：0～7,m：负载重量,单位Kg,范围：0～12,point：重心,x,y,z,单位毫米,范围：-5 000～5 000,返回值：成功true,失败false。

1.2 深度相机与视觉定位

深度相机采用Intel Realsense D415，由1个RGB相机，2个红外线机和1个红外投影仪组成，其中红外投影仪负责发射红外结构光，红外相机负责接收物体表面反射回来的结构光图像，根据三角测距原理计算得到物体表面的深度信息。D415可测量0.3～3 m之间的物体，不受环境光影响，测量精度可达mm级。视觉定位系统根据深度相机采集到的物体表面的深度信息和RGB相机得到像素坐标信息，通过坐标旋转、平移和转换，将像素坐标和深度信息转换为机器人坐标，得到目标物体的三维信息，采用聚类和图像分割算法[11-12]，实现机器人的视觉目标检测与定位。

1.3 双臂机器人控制器

机器人控制器主要实现对机器人系统的现场操作控制，要求操作时使用方便，具有友好的人机交互界面。图3是双臂机器人控制器结构框图，由工业平板电脑和机械键盘构成，工业平板电脑主要用于参数显示、设置、通讯协议转换以及机械臂控制指令解析，机械键盘主要用于操作控制，它们之间可以采用USB或者无线wifi连接。机械键盘采用ARM嵌入式处理器设计，由键盘、指示灯接口电路、USB接口电路，wifi无线通讯接口电路构成，主要实现键盘扫描、组合按键编码、操作控制、数据帧构造与发送，用户使用机械键盘，可以实现对机器人的操作控制。

图3 双臂机器人控制器结构框图

键盘软件设计中定义了不同的功能键和组合键的按键编码，只要相应的功能键和组合键按下，处理器就将相应的按键编码按照通讯协议规范发送给工业平板电脑做进一步的任务处理，按键编码定义在0x00～0x43区域内；如：机械臂选择功能键，按键码是0x03，长按该功能键5秒以上，转换选择要操作的机械臂，并有相应的指示灯显示所选择的机械臂；组合按键码0x11，当选择1号位作业工具时，长按工具功能键和加号键，处理器就会产生抓取1号位作业工具的按键编码0x11，并根据操作者的具体操作和机械臂的运行状态形成一帧通讯数据，并将该帧数据通过USB或无线通讯接口发送给工业平板电脑，工业平板电脑上的控制软件通过wifi控制机械臂执行相应的动作。如通讯数据帧：

0x55,0xaa,0x00,0x01,0x01,0x01,0x01,0x01, 0x11,0xaa,0x55

1.4 双臂机器人无线通讯网络

为保障机器人系统和检修人员的生命安全，机器人本体和带电高压线缆之间采用高压绝缘杆隔离。在2台6轴机械臂末端扩展了长600 mm绝缘杆快接头，可以根据操作人员操作指令快接和更换作业工具，完成特定带电作业。绝缘杆快接头和快接作业工具是根据特定带电作业需求专门设计的。国内配电网带电作业主要是带电断、接引流线，带电更换绝缘子[16-18]。目前设计的快接作业工具有：自动剥皮器，引线锁紧装置，电动扳手，电动线钳，电动夹具。由于快接作业工具和机械臂本体之间采用绝缘杆隔离，机器人控制器只能通过无线方式和快接作业工具连接，结构图4所示。

图4 快接作业工具

机器人控制器通过WIFI和机械臂连接，机械臂RS485接口和LoRa主站模连接，LoRa主站模块采用定点发送的方式和LoRa快接作业工具进行无线通讯[20-21]，从而可实现机器人控制器对快接作业工具的通讯控制，操作快接作业工具完成特定的作业任务。

2 双臂机器人运动规划与软件设计

2.1 双臂机器人运动规划

双臂机器人系统安装在可移动升降平台上，可根据作业需求和现场环境移动到指定工作位置，具体作业实施则依靠机械臂及快接作业工具来完成的。机械臂作业过程中，共有6种工作状态S0～S5。S0状态是机械臂锁定状态，也是机器人最初开始作业和作业结束返回的状态，S1状态是机器人复位状态，该种状态是机器人从锁定状态解除到准备就绪，可以开始工作的状态，S2 状态是机器人从复位状态到取完快接作业工具以后的一种预备状态，S3状态是快接完作业工具到达预设作业点的状态，S4是目标作业点状态，在该状态下，作业工具已处于就绪状态，机器人系统给作业工具发送操作指令来完成具体作业，如剥皮、夹紧、拧紧螺丝等，S5是机械臂作业结束状态，在此状态下，机械臂需要回到S2状态卸掉作业工具，再回到S1复位状态。在S1复位状态，可控制机械臂更换其它作业工具，实施其它作业任务，也可以回到S0锁定状态，结束作业任务。机械臂运动规划状态机如图5所示。

图5 机械臂运动规划状态机

通讯方面，机器人控制器采用client/server方式连接到机械臂，左右2台机械臂作为服务器端，它们有各自的IP地址和服务器端口号，机器人控制器作为客户端。开机后机械臂处于S0锁定状态，通过键盘复位操作，机械臂运行到S1复位状态，在S1复位状态下，可以根据不同的任务要求，机器人通过快接方式快接不同的专用作业工具，并根据实际作业点和预定的路径规划控制机械臂工具末端到达目标工作点位，通过LoRa无线通讯控制专用工具执行相应动作，比如螺丝拧紧装置自动紧固螺丝、剥皮器自动剥皮等。当前作业工具任务完成后，机械臂重新回到S2状态，自动卸载掉作业工具，回到S1复位状态，若需要进行下一道工序，则可以再次快接其它作业工具进行作业，若已经完成任务，则机械臂自动回到S0锁定状态。

2.2 双臂机器人控制器软件设计

双臂机器人控制器通过解析键盘发送过来的数据帧或触摸屏操作按钮，经分析计算，构造JSON格式控制命令，采用SOCKET方式发送给机械臂，机械臂收到命令后执行相应动作，同时将机械臂运动状态信息返回给机器人控制器。双臂机器人软件设计包括2部分内容：工业平板电脑上运行的机器人系统控制软件和嵌入式机械键盘软件。嵌入式机械键盘软件主要负责机械键盘的键值获取，按键组合、工作状态获取，并最终构造键盘数据帧，通过USB或wifi发送给工业平板，工业平板对收到数据帧进行解析，控制机器人进行操作。键盘通讯数据帧协议如表2所示。

表2 键盘数据帧协议

定义0x55，0xaa为键盘通讯数据帧帧头的高字节和低字节，0xaa，0x55为帧尾的高字节和低字节，定义通讯变量：键盘锁状态，当前选择的工具，当前选择的预备位，当前选定的机械臂轴，当前选择的激光打点舵机，当前选择的机械臂，当前的按键操作码。机器人控制器软件设计流程图如图6所示。收到键盘发来的数据帧后，首先判断数据帧的帧头和帧尾是否正确，若不正确，给出数据帧校验错误提示，然后退出本次接收流程；若数据校验无误，进一步判断键盘锁定状态，若键盘锁，则不响应此次的键盘操作，退出流程；若键盘没有锁，则取出通讯变量，保存当前要操作的机械臂、机械臂轴、当前操作的工具号，作业预备位、舵机号以及本次按键操作码。控制器对收到的按键操作码进行判断，若等于0，则表示此次接收到的数据帧是心跳包，无实际按键操作，退出此次流程；若不等于0，则查找机器人操作码表，向对应机械臂发送SOCKET控制命令，控制机械臂执行相应动作。

图6 机器人控制软件流程图

键盘软件设计流程图如图7所示。CPU每隔100 ms扫描1次键盘，检测是否有按键按下，若没有按键按下则构造键盘心跳包，并通过wifi或USB发送给机器人控制器，然后结束本次扫描。若有键按下，读取键值判断是否是功能键按下，若是功能键按下则设置功能键变量，构造键盘数据帧并通过wifi或USB发送给控制器；若不是功能键按下，则判断是否是选择键和组合键按下，若不是则不做处理，退出键盘扫描；若是组合键按下，则查表得到机械臂操作码并构造数据帧，通过wifi或USB发送给机器人控制器，完成此次键盘扫描。

图7 键盘服务程序流程图

图8 机器人系统实验现场

图9 机器人系统功能测试

3 调试与试验

对高压带电作业双臂机器人系统进行了综合试验。

在实验现场，测试了工业平板和左右两台6轴机械臂的网络通讯功能，通过SOCKET通讯，实现了对6轴机械臂的运动规划与姿态控制功能；测试双臂机器人控制器的按键操作、网络通讯功能，通过键盘操作，可以实现对机器人6轴机械臂的操作控制；测试了LoRa主站模块和LoRa工具模块，LoRa主站模块通过RS485和机械臂通讯，通过LoRa定点方式和工具模块通讯，最终通过脚本变量可以实现全部工具模块的通讯控制。

在某培训实验基地10 kV配电线路上，对带电作业机器人系统进行了实验验证，能够完成断、接引流线、更换绝缘子等作业任务。

图10为机器人10 kV绝缘导线剥皮实验，导线标称截面为1×120 mm2，绝缘标称厚度3.4 mm，电缆计算外径21.3 mm。机器人从接收到剥皮指令开始到剥皮完成，整个过程用时3分20秒。

图10 机器人电缆线剥皮实验

图11为机器人接引流线实验，整个过程用时2分50秒。

图11 机器人接引流线实验

机器人系统现场实验运行情况表明，带电作业机器人系统操作方便、性能稳定，依靠LoRa无线通讯，不仅实现了前端操作工具和机器人本体的高压绝缘隔离，而且实现了作业工具的操作控制，可代替人工完成相应的作业任务。

4 结束语

高压带电作业机器人系统主体由12米高液压升降平台，左右两台6轴机械臂构成。设计开发了双臂机器人控制器硬件、软件和机械臂运动路径规划方法，通过机械键盘或触摸屏可以实现对2台机械臂的有效控制；采用LoRa通讯和组网方式，对6轴机械臂进行了末端扩展，实现机器人控制器对任意专用工具模块进行装卸；设计开发了LoRa主站模块和LoRa专用工具模块，采用LoRa定点扫描通讯方式，实现了各工具模块和机器人控制器的相互通讯；通过现场试验，验证了机器人系统的各项设计功能。