面向高压带电开关柜的视觉控制机械臂作业系统设计

毛成林，于瑞强，边 疆

(1.东南大学仪器科学与工程学院，江苏 南京 210096)(2.亿嘉和科技股份有限公司，江苏 南京 210012)

带电作业进行检修对于维护电力系统的稳定运行具有重要意义[1]。电力行业一直在推进机器人自动化作业，以降低人工操作的风险和时间成本，提高处理问题的速度。随着智慧电网的大力发展，带电作业机器人受到了广泛的关注和重点研究[2]。室内环境下的变电站/配电站高压带电开关柜的定期检修和紧急故障排除是一种常见的应用场景，柜体上设置有需要按压、旋转等操作的设备， 因此可使用视觉技术构建一个视觉控制系统来定位操作目标，通过安装机械臂和末端执行器来执行操作。

在视觉控制领域，前人已经进行了较多探索。Vahrenkamp 等[3]先估计出目标物体的位姿,然后利用仿人机械臂完成抓取操作任务；刘理想[4]搭建了 Eye-In-Hand 机器人抓取平台，通过深度相机获取信息，利用深度神经网络实现目标的抓取位姿估计；Zuo等[5]借助一个额外的摄像头，使用深度卷积神经网络实时估计机械臂的三维姿态，控制机械臂到达空间中任意给定三维坐标的位置。然而这些任务通常对绝对定位精度要求不高，而在电力应用场景存在直径为1～2 mm小按钮等类型的设备，对绝对定位精度要求较高。

当前大部分工业机器人基于视觉的操作策略都是基于目标与机械臂相对位置固定这一前提，而在电力应用场景中，机械臂装载在可移动的底盘上，由于底盘定位精度的限制，机械臂位置相对于操作目标有一定的误差。另外，针对按钮、旋钮等设备还需要进行垂直于目标所在平面的操作，为了最大程度减少工具的出力，保护工具和设备本身不损坏，需要对机械臂操作时的姿态进行约束。

现阶段广泛使用的估计操作平面的方法是视觉检测二维码方法，ArUco marker是其中一种代表性方法[6]，其可以估计出相机成像平面和二维码所在平面之间的姿态变换。然而这一方法需要粘贴辅助标识。随着Kinect等高性能深度相机的出现，人们开始对深度相机获取的目标三维信息进行分析以估计平面。一种基于最小二乘法的分割平面算法[7]对噪声具有一定的抵抗能力，但在实验中也存在0°～1.7°的测量误差，且不具有自动分割平面的能力。点云计算库(point cloud library,PCL)中部署了一种基于随机采样一致性(RANSAC)的分割平面算法[8]，可以分割多个平面，但只有在存在唯一一个平面时精度较高，有多个平面时结果精度会降低，计算速度也会下降。

本文针对电力特种行业应用场景设计了一套对高压带电开关柜进行操作的视觉控制机械臂操作系统。所提系统安装在实体的室内带电作业机器人上，并在国内多处变电站、所进行试运行，多次实验和实机运行结果证明了系统的有效性。

1 视觉控制确定操作位姿

本文设计的系统计算出操作对象所处的柜体平面，分割出对象目标，从而确定目标位姿和机械臂位姿，以解决上述提到的需要垂直于目标所在平面操作且对绝对定位精度要求较高的问题。因为柜体平面离机械臂原点较近，所以构建了一个“眼在手上”的视觉控制系统，相对于“眼在手外”的方式具有更高的灵活性。系统工作流程如下：1)深度相机获取RGB-D图像(RGB图像+DepthMap，深度图像)；2)从RGB-D图像自动分割柜体平面并计算平面；3)预处理设备图像，获取目标类别和位姿；4)计算末端执行器垂直对准目标所需位姿。

1.1 深度相机拍照

机械臂的载体(通常为运动底盘)运动到柜体正前方，机械臂基坐标系y轴平行于柜体平面，x轴垂直于柜体平面，z轴垂直于地面向上。由于运动误差，因此x，y轴与柜体平面存在5°以内的误差。地面可能不平整，使得z轴和竖直方向存在2°的误差。机械臂运动并调整到指定的位姿后利用深度相机采集目标图像，由于上述运动误差,因此相机的光轴也存在和底盘相同的误差。此外相机与柜体平面之间要保持一定的距离，以保证采集图像时能覆盖到柜体平面大多数区域。

1.2 目标区域分割

由于需要操作的目标设备都位于电力开关柜的柜体平面上，因此首先进行柜体平面的拟合计算，然后分割出所有设备进行识别。

深度相机抓取目标RGB-D图像上的像素和RGB图像上的像素一一对应，每一个像素对应一个相机坐标系下的空间三维坐标(xi,yi,zi)。柜体平面区域通常有相同的颜色，而且具有最大的连通区域外接矩。基于此目标区域算法流程如下：

1)将RGB图像转换到HSV颜色空间，进行聚类粗分割。聚类使用K-Means++算法[9]，由于受到光照的影响，因此分类不够精确。

2)对每一类颜色进行形态学处理，计算连通区域，找出连通区域中外接矩最大的一个。

3)在选出的连通区域内随机选出3个点使用RANSAC算法[10]拟合平面方程：

Ax+By+Cz+D=0

(1)

式中：(x,y,z)为空间三维坐标；A,B,C,D为三维平面方程系数，平面法向量为[A,B,C]，且|[A,B,C]T|=1。计算连通区域里所有点到该平面距离的和distance：

distance=∑(Axi+Byi+Czi+D)2

(2)

4)重复步骤3)若干次(在实践中通常为1 500次以上)，然后选取最小distance所对应的平面方程。

5)计算整个图像对应的全部点到计算出的平面的距离，小于2 mm的点认为都属于柜体平面，其余的点被认为属于分割出的设备。分割出的像素点属于同一个连通区域的被合并。 RANSAC算法相当于对平面进行了校准。

1.3 设备识别，获取目标相机坐标系位置

分割出操作目标后，需要对目标进行识别，以确定下一步的操作位姿和执行动作。为提升视觉识别目标的精准程度，基于深度学习目标检测网络进行识别，并对网络的输入提出了一种基于相机位姿变换的预处理方法。

1.3.1校准设备图像

由于1.1节中拍摄的深度图像不是完全平行于柜体平面的，因此对分割出的图像区域(记为P1)进行预处理。假设把相机调整成光轴垂直于柜体平面，则拍摄的图像平行于柜体平面，此时相当于对相机坐标系进行刚体变换，这种变换由一个旋转加上一个平移组成。只考虑旋转，将图像区域投影到平行于柜体的平面上：

P2=KRK-1P1

(3)

(4)

1.3.2深度学习识别图像，获取中心点坐标

1.4 计算机械臂法兰调整位姿

在1.2节中获得了柜体平面方程，由于对柜体表面设备进行的操作都是垂直于柜体平面的，也即得知了操作目标在相机坐标系下的空间姿态，在1.3节中获得了操作目标在相机坐标系下的空间位置，因此通过相机和机械臂坐标系的转换关系可获得操作目标在机械臂坐标系下的空间位姿。依此调整机械臂法兰的位姿，使得末端工具可以垂直于操作平面。

1.4.1从柜体平面法向量计算相机和柜体平面之间的姿态变换关系

将相机和柜体平面之间的相对姿态转换为相机光轴和柜体平面法向量之间的相对变换关系来表达，当相机光轴垂直于柜体平面时，相机坐标系下平面法向量为[0,0,1]。记三维空间中单位向量v1=[A,B,C]，v2=[0,0,1]。因为只考虑姿态的变换，所以只需计算两个三维向量之间的旋转。从v1到v2之间的旋转可以用一个旋转轴u和一个旋转角θ来描述，即围绕轴u旋转了角度θ，则根据向量内外积定义：

u=v1×v2

(5)

cosθ=v1·v2

(6)

利用三角函数公式：

(7)

(8)

1.4.2调节机械臂末端使工装垂直对准目标

通过机械臂手眼标定和TCP标定预先确定深度相机坐标系和机械臂坐标系的转换关系。在机械臂坐标系下记工具的位姿为gbt，记相机的位姿为gbc，法兰(机械臂末端)的位姿为gbe，法兰到工具的位姿变换矩阵为get，法兰到相机的位姿变换矩阵为gec，存在如下关系：

图1 计算相机光轴到柜体平面法向量的旋转

(9)

式中：Rbt,Rbc,Rbe分别为工具、深度相机、机械臂末端法兰在机械臂坐标系下的姿态；Ret,Rec分别为机械臂末端法兰到工具的姿态变换矩阵、机械臂末端法兰到深度相机的姿态变换矩阵；pbt,pbc,pbe分别为工具、深度相机、机械臂末端法兰在机械臂坐标系下的位置；pet,pec分别为机械臂末端法兰到工具的位置变换矩阵、机械臂末端法兰到深度相机的位置变换矩阵。

则机械臂末端需要调整的旋转变换Rchange为：

(10)

在机械臂坐标系下，目标的位姿gbo为：

(11)

式中：I为单位矩阵。

将目标位置pbo记为：

pbo=Rbc×pco+pbc

(12)

则将目标和工具重合需要调整的齐次变换矩阵为：

(13)

式中：pchange为需要调整的位置向量，大小为3×1。

(14)

(15)

式中：gchange为工具垂直于柜体平面方向对准目标需要调整的齐次变换矩阵。则机械臂法兰调整后的位姿为gbe×gchange，利用机械臂逆运动学可反解出机械臂各个关节的关节角，对机械臂运动进行控制。之后工具只需要在自身坐标系z轴方向上前进pchange[2]即可完成操作。

2 机器人作业实验

本文设计的系统已安装在亿嘉和科技股份有限公司研发的“羚羊”室内带电作业机器人(https://www.yijiahe.com/robot-d200)上，如图2所示，该机器人面向的作业场景是变/配电站等站所内的高压开关柜，需要执行的操作包括旋转柜体上的远方-就地转换开关、按压带电显示器按钮等。整个机器人系统安装在运动底盘上，可以运动到柜体正前方。机械臂采用国产李群六轴机械臂，在机械臂末端安装有自研的末端执行器，在末端执行器上安装Intel realsense d435i深度相机，通过深度相机准确进行目标的定位与识别，然后驱动机械臂运动。在末端执行器上安装有夹爪(对应旋钮)、按压杆(对应按钮)等操作装置。

图2 “羚羊”机器人外观示意图

2.1 基于计算平面法向量自动分割柜体平面的测试实验

为测试1.4节算法的精度,设计了一种测量装置，如图3所示，深度相机被安装在一个旋转精度为0.1°的转盘上(转盘上刻度为-180°～180°，从上往下看逆时针为负)，对一个平板进行拍摄，距离为30 cm。设置水平旋转量为-6°，测量平面法向量y方向(水平旋转)的偏移角度。共进行了130次测量实验，统计得出了测量结果计算的法向量的误差为0°～0.6°(图4，0.6°为图中最大值-5.4°与最小值-6.0°的差值)。只调节y轴是因为测量工具的设计制造不便，所以这里用y轴发生变化后产生的最大误差来代表3个轴。图5展示了一例采用1.2节算法获得的柜体平面自动分割结果。图6展示了一例采用1.4节算法获得的调节末端执行器垂直于柜体平面的调整结果。

图3 深度相机计算平面法向量测量工具示意图

图4 水平旋转量为-6°时测量结果计算的法向量误差

2.2 任务整体成功率测试

“羚羊”机器人在4种不同类型的电力开关柜(北辰、华电、天灵、许继的KYN28A-12型开关柜)上进行了测试，作业对象包括2种旋钮(远方-就地转换开关、断路器开关)和3种按钮(复归开关、带电显示器按钮、继电保护装置)，对每种作业对象进行了250次操作(图7)，成功率达到99.24%，整个系统的绝对定位精度小于1 mm。

图5 柜体平面自动分割结果

图6 调整末端执行器垂直于柜体平面

亿嘉和科技股份有限公司同时和多家电力公司合作，先后将“羚羊”机器人部署在南京市国泰变电站、扬州市花园变电站，苏州市运河变电站、徐州市绿地变电站、宿迁市隆锦变电站、淮安市文府变电站、北京市通州区田府变电站等变电站，支持变电站无人运维。

图7 “羚羊”室内带电作业机器人操作现场

3 结束语

本文设计了一套面向高压带电开关柜的视觉控制机械臂作业系统，其创新点为基于RGB-D数据结合颜色空间聚类分割和RANSAC算法计算柜体平面法向量；基于RGB-D数据结合柜体平面法向量对柜体表面设备的定位识别进行图像预处理；基于RGB-D数据结合柜体平面法向量计算进行操作。安装了本文系统的亿嘉和科技股份有限公司“羚羊”机器人在4种不同类型的电力开关柜体上的测试结果显示，系统的绝对定位精度小于1 mm。本文系统在“羚羊”机器人上的成功应用，表明其具有较高的工业实用价值，将进一步促进带电作业机器人功能的完善。