吴文海，姚 波，于兰英，王国志

绝缘子水冲洗机器人视觉伺服仿真

吴文海，姚 波，于兰英，王国志

（西南交通大学机械工程学院，成都610031）

视觉伺服研究是绝缘子水冲洗机器人研究的重要内容，其研究成果用于机器人自动瞄准绝缘子。水冲洗机器人采用基于图像的视觉伺服控制方案，对冲洗机器人的图像雅克比矩阵进行计算，设计基于机器人位置参数和绝缘子高度信息获取图像深度的方法，最后基于Robotics Tool⁃box和Visual Servoing Toolbox工具箱对冲洗机器人视觉控制方案进行建模、仿真，结果表明，基于图像的机器人视觉伺服系统能够实现对绝缘子瞄准功能。

视觉伺服控制；图像雅克比矩阵；水冲洗机器人

0 引言

视觉伺服是通过获取机器人与目标位置的视觉图像信息，来控制机器人末端执行器的运动[1]。主要包括：基于位置的视觉伺服控制和基于图像的视觉伺服控制，它们的差别主要在于控制率中输入信号的不同，前者通过图像信息和机器人几何模型计算机器人执行器与目标位置的误差，以此在笛卡尔空间坐标控制机器人运动到期望位置，后者直接利用图像信息误差控制机器人，可以克服摄像机标定误差。该水冲洗机器人工作场所为铁路变电所，环境复杂，不适宜进行场景三维重构，因此采用基于图像的视觉伺服控制方案[2]。

基于图像的视觉伺服控制系统涉及摄像机成像理论、图像雅克比矩阵，机器人运动学、控制理论等学科[3]，是在摄像机透视成像模型的基础上，通过图像雅克比矩阵来实现的。而图像雅克比矩阵的建立需要对图像的深度信息进行估计、计算。由于机器人系统非线性和视觉信息计算的复杂性[4]，本文采用专门的视觉仿真软件进行研究，Robotics Toolbox[5]和 Visual Servoing Toolbox[6]是应用最广的视觉伺服控制工具箱，通过建立基于图像的水冲洗机器人的视觉伺服系统，验证该系统的有效性。

1 图像雅克比矩阵计算

对于手眼机器人视觉系统，从图像二维平面空间到机器人关节空间的雅克比矩阵称为图像雅克比矩阵，是由图像二维平面空间到摄像机三维运动空间微分运动的雅克比矩阵和摄像机三维运动空间到机器人关节转动空间的雅克比矩阵的乘积构成[7-8]。求解冲洗机器人的图像雅克比矩阵，需先建立机器人的系统数学模型，绝缘子水冲洗机器人（见图1）为二自由度机器人，通过伺服电机驱动水枪绕关节进行水平面和垂直面旋转[9]，CCD摄像机固定在水枪末端，用于绝缘子图像摄取。对水冲洗机器人系统建立机器人坐标、摄像机坐标变换关系。图2是根据机器人结构建立的坐标变换关系图。设机器人水枪水平旋转角为θ1，俯仰角为θ2，将图中的O点作为基坐标参考点，基座标系用X0，Y0，Z0表示，摄像机坐标系用Xc，Yc，Zc表示，水枪末端坐标系用Xe，Ye，Ze表示。Dx，Dy，Dz分别代表摄像机坐标系相对于水枪末端坐标系x轴，y轴，z轴上的偏移量，l表示水枪的长度。

图1 冲洗机器人结构图Fig.1 The structure of the washing robot

图2 机器人机构坐标变换图Fig.2 The coordinate transformation diagram of the robot mechanism

用0Te表示机器人水枪末端坐标系相对于机器人基座标系的变化矩阵，用eTc表示摄像机坐标系相对于水枪末端坐标系的变换矩阵，则机器人摄像机坐标系与基座标系之间变换矩阵如下：

空间目标点P在摄像机坐标系中的其次坐标为，在基座标系中的其次坐标为，两个坐标系之间的变换关系为对 方 程 求 导 后 ，可 得，因为P点在基座标下位置不变，所以，即，为方便计算，本文假定摄像机固定在水枪末端坐标系原点，即忽略Dx，Dy，Dz的大小，可得：

式（5）描述了冲洗机器人水平旋转角θ1、俯仰角θ2与目标物体在摄像机坐标系中坐标（Xc，Yc，Zc）的变化关系。图3所示为摄像机成像平面坐标，又根据摄像机成像原理，可知点P成像平面坐标为（u，v），且u=fXc/(dxZc)+u0，v=fYc/(dyZc)+v0，其中f为摄像机焦距，f/dx表示图像u轴方向的转换因子或归一化焦距，f/dy表示图像v轴方向的转换因子或归一化焦距，成像平面中的光轴交点g在图像坐标系中的坐标为（u0，v0）

图3 摄像机成像平面坐标Fig.3 Plane coordinate of the camera imaging

将u和v带入式（5），并对u和v求导，最后变换成如下形式：

J是关于参数θ1、θ2和Zc的矩阵，即冲洗机器人的图像雅克比矩阵，描述了机器人转角θ1、θ2的变化与图像特征变化之间的关系。由于变换过程太过复杂，本节没有继续计算矩阵J中各参数值。

2 图像深度计算

实现对机器人进行图像伺服控制，需要对机器人的图像雅克比矩阵进行实时更新，而图像雅克比矩阵中包含参数Zc，表示特征点相对于摄像机坐标系的深度，因此必须对图像的深度信息Zc进行实时估计[10]。目前单目视觉求解深度信息主要采用对应点标定法，根据特定点在不同坐标系中的坐标求解深度信息，一般适用于空间位置固定摄像机[11]，由于变电所内绝缘子种类较多，剖面面积不统一，再加上绝缘子图像处理过程中，绝缘子图像面积会发生变化，也排除以摄像机俯仰角、绝缘子剖面面积、绝缘子成像面积为依据进行定位的测距方法[12]，由于变电所绝缘子安装高度一定，最后本文决定采用以机器人自身高度、绝缘子高度、摄像机俯仰角、焦距等参数为依据对目标物体进行定位的几何测距方法[13-14]，可以减少外界环境影响带来的误差。

当冲洗机器人定位出绝缘子时，摄像头会与地面成固定角度，摄像机测距模型如图4所示为测距侧视图。

图4 摄像机测距模型Fig.4 Ranging model of the camera

YU为绝缘子平面，O点为摄像机光心，Zc轴为摄像机光轴，与绝缘子平面相交于点G，I点为O点在绝缘子平面上投影点，EF为摄像机拍摄空间，ef为EF对应的图像平面，O0为图像坐标原点，假定摄像机的垂直视角为2β，摄像机的俯仰角为θ2，目标点P在空间内坐标（Xw，Yw，Zw），机器人水枪安装高度为h，水枪长度为l，P点在Zc轴上的深度距离为ON，P点至光心得水平距离为OI，根据几何原理可得：

式中：v0为光轴在像平面投影点的纵坐标；v（p）为P点在像平面的纵坐标。

3 视觉伺服仿真

本节结合Visual Servoing Toolbox和Robotics Toolbox对机器人视觉伺服系统进行联合仿真，其中Visual Servoing Toolbox可以模拟摄像机视场，并在视场中生成特征点，最终建立图像特征点与摄像机运动之间的关系，但缺少与机器人结合。而Ro⁃botics Toolbox工具箱包含了大量功能丰富的函数，可用于机器人运动学、动力学、轨迹规划等研究，可以弥补Visual Servoing Toolbox的不足。

冲洗机器人基于图像的伺服控制仿真模型以Visual Servoing Toolbox的IBVS模型为基础，并加入Robotics Toolbox的ikine（逆运动学）模块，把摄像机图像特征与机器人运动联系起来。其中ikine（逆运动学）的求解是已知机器人末端执行器的位姿，逆计算出机器人各个运动关节的角度值[15-16]。

Robotics Toolbox中机器人三维模型构建命令：

图5为Robotics Toolbox构建的初始状态下的机器人机构模型，机器人初始姿态θ1为5°，θ2为-25.7°。

图5 绝缘子冲洗模型Fig.5 Washing model of the insulator

Visual Servoing Toolbox仿真实验前，需对视觉伺服系统进行参数设定，包括摄像机的内外参数、目标物体在视场中的初始位置和期望位置等。本文选取长为0.575 m，宽为0.125 m的长方形模拟绝缘子，4个顶点分别为4个特征点，摄像机固定于机器人末端执行器，摄像机内参数ax=1 000，ay=1 000，u0=256，v0=256，图像平面分辨率为512×512，摄像机模型为理想的透视模型，不考虑镜头畸变，绝缘子与机器人水平距离为3 m，绝缘子底部高度为2.832 m，最后，得到图5的联合机器人模型构建的整个仿真图像。

假定初始状态时，摄像机位置坐标到机器人基座标的变换矩阵根据式（2）-式（4）进行计算，最后得到的机器人初始状态拍摄的绝缘子图像见图6，O点为图像平面的中心点，P点为绝缘子底部中点。

图6 初始状态绝缘子图像Fig.6 The intial state image of the insulator

当水枪瞄准绝缘子并准备冲洗时，O点应与P点重合。图7所示为在Visual Servoing Toolbox环境下得到理想状态下，水枪瞄准绝缘子时4个特征点图像的分布状态，实际控制过程中，在得到理想特征点与初始特征点误差之后，还不能进行伺服控制，必须对图像深度信息进行实时估算。由于本文为仿真状况，深度信息直接给出为3 m，但实际过程中并不能提前知道深度信息，需由初始图像信息计算出深度信息。

图7 理想状态下绝缘子图像Fig.7 The ideal state image of the insulator

绝缘子初始图像6中，绝缘子底部中点P的图像坐标为（339，181），为验证上节中计算图像深度方法的正确性，笔者将基于摄像机仿真模型进行深度逆计算，根据式（7），计算过程如下：

仿真图像（见图5）中机器人与绝缘子水平距离为3 m与计算结果相近，考虑计算误差，笔者采用的图像深度计算方法切实可行，能够计算出绝缘子图像的深度信息。

当知道特征点误差和图像深度信息之后，可进行基于图像的伺服瞄准，图8所示为基于IBVS模型仿真过程中4个特征点1、2、3、4的图像运动轨迹，摄像机刚开始运动时，特征点位置变化较大，随着特征点与理想点误差减少，特征点位置变化变小，最终达到理想特征点位置，绝缘子区域运动至图像中心。

图8 仿真过程中特征点运动轨迹Fig.8 The movement tracks of feature point in the simulation

将机器人末端的运动轨迹与摄像机运动轨迹重合（见图9），通过将此轨迹导入Robotics Toolbox的ikine（逆运动学）函数模块可以求得机器人关节的变化过程，图10所示为冲洗机器人关节角度变化情况，其中关节角速度变化可以作为机器人的控制输入，θ1为冲洗机器人关节水平旋转角，θ2为俯仰旋转角。

图9 摄像机运动轨迹Fig.9 The movement tracks of camera

图10 机器人关节角度变化Fig.10 Joint angle change of the robot

机器人运动从初始状态的θ1为5°、θ2为-25.7°，最终变化至理想状态θ1为0°、θ2为-30°，角速度变化从初始的剧烈变化最终趋于平稳，表明控制过程趋于收敛。仿真系统趋于稳定所需的时间可以通过调节系统增益的大小进行控制。

4 结语

本次仿真实验是基于图像的伺服控制方式，通过求解图像雅克比矩阵，建立机器人运动与图像特征点之间的关系。验证了视觉伺服过程中图像深度计算方法，并得到图像特征点的变化与机器人关节变化之间的关系，结果证明基于图像的伺服控制方式可以实现水冲洗机器人自动瞄准绝缘子。

[1]CORKE P I.Visual control of robots:high-performance vi⁃sual servoing[M].New York:Wiley，1996.

[2]雷洋.保留图像特征点的机械手视觉伺服控制[D].杭州：浙江大学，2014.

[3]杨延西，刘丁，闫振杰.图像反馈机器人视觉伺服系统仿真[J].系统仿真学报，2003，15（12）：1737-1740.YANG Yanxi，LIU Ding，YAN Zhenjie.Simulation of im⁃age feedback robot manipulator visual servo system[J].Journal of System Simulation，2003，15（12）:1737-1740.

[4]黎志刚，段锁林，赵建英，等.机器人视觉伺服控制及应用研究的现状[J].太原科技大学学报，2007，28（1）：24-31.LI Zhigang，DUAN Suolin，ZHAO Jianying，et al.The present status of visual servo-control and application study on robot[J].Journal of Taiyuan University of Science and Technology，2007，28（1）:24-31.

[5]The Visual Servoing Toolbox[DB]，2003.http://vstoolbox.sourceforge.net/.

[6]Corke Peter I.Robotics Toolbox for Matlab[DB]，http://pe⁃tercorke.com/Robotics_Toolbox.htm.

[7]田凯.基于单目视觉的视觉伺服与位姿估计[D].武汉：华中科技大学，2009.

[8]徐德，谭民.机器人视觉测量与控制[M].北京：国防工业出版社，2011.

[9]姚波，于兰英，吴文海，等.变电所绝缘子水冲洗机器人设计[J].机械设计与制造工程，2015，44（8）：65-68.YAO Bo，YU Lanying，WU Wenhai，et al.The design of water washing robot for insulator[J].Machine Design and Manufacturing Engineering，2015，44（8）:65-68.

[10]张广军.机器视觉[M].北京：科学出版社，2005.

[11]于乃功，黄灿，林佳.基于单目视觉的机器人目标定位测距方法研究[J].计算机测量与控制，2012，20（10）：2654-2656.YU Naigong，HUANG Can，LIN Jia.Target location and ranging based on monocular vision robot[J].Computer Mea⁃surement&Control，2012，20（10）:2654-2656.

[12]邱联奎.基于机械手的视觉伺服控制及其应用研究[D].合肥：中国科学技术大学，2006.

[13]于兰英，姚波，吴文海，等.一种基于多特征的绝缘子识别方法[J].电瓷避雷器，2016（3）：79-83.YU Lanying，YAO Bo，WU Wenhai，et al.Insulator iden⁃tification method based on multi-feature[J].Insulators and Surge Arresters，2016（03）:79-83.

[14]尚俊霞.复合绝缘子憎水性检测与等级判断的研究[J].电瓷避雷器，2016（3）：79-83.SHANG Junxia.Insulator identification method based on multi-feature[J].Insulators and Surge Arresters，2016（3）:79-83.

[15]MANOCHA D，CANNY J F.Efficient inverse kinematics for general 6R manipulators[J].Robotics and Automation，IEEE Transactions on，1994，10（5）:648-657.

[16]PAUL R P，SHIMANO B.Kinematic control equations for simple manipulators[C].Decision and Control including the 17th Symposium on Adaptive Processes，1978 IEEE Conference on 1978.

Insulator Water Washing Robot Visual Servo Simulation

WU Wenhai，YAO Bo，YU Lanying，WANG Guozhi
（College of Mechanical Engineering，Southwest JiaoTong University，Chengdu 610031，China）

Visual servoing research is an important content of insulator water washing robotics re⁃search，and the research results can be used for robot automatic aiming insulator.Image based visual ser⁃vo control scheme is used on the water washing robot，and the flushing image jacobian matrix of the robot is calculated.The calculation of the image depth is based on the robot location parameters and the height information of insulator.The visual control system of washing robot is modeled and simulated based on Robotics Toolbox and Visual Servoing Toolbox，the results show that the robotic Visual servo system based on image could aim insulator automatically.

visual servo control；image jacobin matrix；water washing robot

10.16188/j.isa.1003-8337.2017.06.035

2016-09-01

吴文海（1979—）男，博士，讲师，主要从事机电液一体化技术的研究。