絕緣子水沖洗機器人視覺伺服仿真

吳文海，姚 波，于蘭英，王國志

絕緣子水沖洗機器人視覺伺服仿真

吳文海，姚 波，于蘭英，王國志

（西南交通大學機械工程學院，成都610031）

視覺伺服研究是絕緣子水沖洗機器人研究的重要內(nèi)容，其研究成果用于機器人自動瞄準絕緣子。水沖洗機器人采用基于圖像的視覺伺服控制方案，對沖洗機器人的圖像雅克比矩陣進行計算，設(shè)計基于機器人位置參數(shù)和絕緣子高度信息獲取圖像深度的方法，最后基于Robotics Tool?box和Visual Servoing Toolbox工具箱對沖洗機器人視覺控制方案進行建模、仿真，結(jié)果表明，基于圖像的機器人視覺伺服系統(tǒng)能夠?qū)崿F(xiàn)對絕緣子瞄準功能。

視覺伺服控制；圖像雅克比矩陣；水沖洗機器人

0 引言

視覺伺服是通過獲取機器人與目標位置的視覺圖像信息，來控制機器人末端執(zhí)行器的運動[1]。主要包括：基于位置的視覺伺服控制和基于圖像的視覺伺服控制，它們的差別主要在于控制率中輸入信號的不同，前者通過圖像信息和機器人幾何模型計算機器人執(zhí)行器與目標位置的誤差，以此在笛卡爾空間坐標控制機器人運動到期望位置，后者直接利用圖像信息誤差控制機器人，可以克服攝像機標定誤差。該水沖洗機器人工作場所為鐵路變電所，環(huán)境復雜，不適宜進行場景三維重構(gòu)，因此采用基于圖像的視覺伺服控制方案[2]。

基于圖像的視覺伺服控制系統(tǒng)涉及攝像機成像理論、圖像雅克比矩陣，機器人運動學、控制理論等學科[3]，是在攝像機透視成像模型的基礎(chǔ)上，通過圖像雅克比矩陣來實現(xiàn)的。而圖像雅克比矩陣的建立需要對圖像的深度信息進行估計、計算。由于機器人系統(tǒng)非線性和視覺信息計算的復雜性[4]，本文采用專門的視覺仿真軟件進行研究，Robotics Toolbox[5]和 Visual Servoing Toolbox[6]是應(yīng)用最廣的視覺伺服控制工具箱，通過建立基于圖像的水沖洗機器人的視覺伺服系統(tǒng)，驗證該系統(tǒng)的有效性。

1 圖像雅克比矩陣計算

對于手眼機器人視覺系統(tǒng)，從圖像二維平面空間到機器人關(guān)節(jié)空間的雅克比矩陣稱為圖像雅克比矩陣，是由圖像二維平面空間到攝像機三維運動空間微分運動的雅克比矩陣和攝像機三維運動空間到機器人關(guān)節(jié)轉(zhuǎn)動空間的雅克比矩陣的乘積構(gòu)成[7-8]。求解沖洗機器人的圖像雅克比矩陣，需先建立機器人的系統(tǒng)數(shù)學模型，絕緣子水沖洗機器人（見圖1）為二自由度機器人，通過伺服電機驅(qū)動水槍繞關(guān)節(jié)進行水平面和垂直面旋轉(zhuǎn)[9]，CCD攝像機固定在水槍末端，用于絕緣子圖像攝取。對水沖洗機器人系統(tǒng)建立機器人坐標、攝像機坐標變換關(guān)系。圖2是根據(jù)機器人結(jié)構(gòu)建立的坐標變換關(guān)系圖。設(shè)機器人水槍水平旋轉(zhuǎn)角為θ1，俯仰角為θ2，將圖中的O點作為基坐標參考點，基座標系用X0，Y0，Z0表示，攝像機坐標系用Xc，Yc，Zc表示，水槍末端坐標系用Xe，Ye，Ze表示。Dx，Dy，Dz分別代表攝像機坐標系相對于水槍末端坐標系x軸，y軸，z軸上的偏移量，l表示水槍的長度。

圖1 沖洗機器人結(jié)構(gòu)圖Fig.1 The structure of the washing robot

圖2 機器人機構(gòu)坐標變換圖Fig.2 The coordinate transformation diagram of the robot mechanism

用0Te表示機器人水槍末端坐標系相對于機器人基座標系的變化矩陣，用eTc表示攝像機坐標系相對于水槍末端坐標系的變換矩陣，則機器人攝像機坐標系與基座標系之間變換矩陣如下：

空間目標點P在攝像機坐標系中的其次坐標為，在基座標系中的其次坐標為，兩個坐標系之間的變換關(guān)系為對 方 程 求 導 后 ，可 得，因為P點在基座標下位置不變，所以，即，為方便計算，本文假定攝像機固定在水槍末端坐標系原點，即忽略Dx，Dy，Dz的大小，可得：

式（5）描述了沖洗機器人水平旋轉(zhuǎn)角θ1、俯仰角θ2與目標物體在攝像機坐標系中坐標（Xc，Yc，Zc）的變化關(guān)系。圖3所示為攝像機成像平面坐標，又根據(jù)攝像機成像原理，可知點P成像平面坐標為（u，v），且u=fXc/(dxZc)+u0，v=fYc/(dyZc)+v0，其中f為攝像機焦距，f/dx表示圖像u軸方向的轉(zhuǎn)換因子或歸一化焦距，f/dy表示圖像v軸方向的轉(zhuǎn)換因子或歸一化焦距，成像平面中的光軸交點g在圖像坐標系中的坐標為（u0，v0）

圖3 攝像機成像平面坐標Fig.3 Plane coordinate of the camera imaging

將u和v帶入式（5），并對u和v求導，最后變換成如下形式：

J是關(guān)于參數(shù)θ1、θ2和Zc的矩陣，即沖洗機器人的圖像雅克比矩陣，描述了機器人轉(zhuǎn)角θ1、θ2的變化與圖像特征變化之間的關(guān)系。由于變換過程太過復雜，本節(jié)沒有繼續(xù)計算矩陣J中各參數(shù)值。

2 圖像深度計算

實現(xiàn)對機器人進行圖像伺服控制，需要對機器人的圖像雅克比矩陣進行實時更新，而圖像雅克比矩陣中包含參數(shù)Zc，表示特征點相對于攝像機坐標系的深度，因此必須對圖像的深度信息Zc進行實時估計[10]。目前單目視覺求解深度信息主要采用對應(yīng)點標定法，根據(jù)特定點在不同坐標系中的坐標求解深度信息，一般適用于空間位置固定攝像機[11]，由于變電所內(nèi)絕緣子種類較多，剖面面積不統(tǒng)一，再加上絕緣子圖像處理過程中，絕緣子圖像面積會發(fā)生變化，也排除以攝像機俯仰角、絕緣子剖面面積、絕緣子成像面積為依據(jù)進行定位的測距方法[12]，由于變電所絕緣子安裝高度一定，最后本文決定采用以機器人自身高度、絕緣子高度、攝像機俯仰角、焦距等參數(shù)為依據(jù)對目標物體進行定位的幾何測距方法[13-14]，可以減少外界環(huán)境影響帶來的誤差。

當沖洗機器人定位出絕緣子時，攝像頭會與地面成固定角度，攝像機測距模型如圖4所示為測距側(cè)視圖。

圖4 攝像機測距模型Fig.4 Ranging model of the camera

YU為絕緣子平面，O點為攝像機光心，Zc軸為攝像機光軸，與絕緣子平面相交于點G，I點為O點在絕緣子平面上投影點，EF為攝像機拍攝空間，ef為EF對應(yīng)的圖像平面，O0為圖像坐標原點，假定攝像機的垂直視角為2β，攝像機的俯仰角為θ2，目標點P在空間內(nèi)坐標（Xw，Yw，Zw），機器人水槍安裝高度為h，水槍長度為l，P點在Zc軸上的深度距離為ON，P點至光心得水平距離為OI，根據(jù)幾何原理可得：

式中：v0為光軸在像平面投影點的縱坐標；v（p）為P點在像平面的縱坐標。

3 視覺伺服仿真

本節(jié)結(jié)合Visual Servoing Toolbox和Robotics Toolbox對機器人視覺伺服系統(tǒng)進行聯(lián)合仿真，其中Visual Servoing Toolbox可以模擬攝像機視場，并在視場中生成特征點，最終建立圖像特征點與攝像機運動之間的關(guān)系，但缺少與機器人結(jié)合。而Ro?botics Toolbox工具箱包含了大量功能豐富的函數(shù)，可用于機器人運動學、動力學、軌跡規(guī)劃等研究，可以彌補Visual Servoing Toolbox的不足。

沖洗機器人基于圖像的伺服控制仿真模型以Visual Servoing Toolbox的IBVS模型為基礎(chǔ)，并加入Robotics Toolbox的ikine（逆運動學）模塊，把攝像機圖像特征與機器人運動聯(lián)系起來。其中ikine（逆運動學）的求解是已知機器人末端執(zhí)行器的位姿，逆計算出機器人各個運動關(guān)節(jié)的角度值[15-16]。

Robotics Toolbox中機器人三維模型構(gòu)建命令：

圖5為Robotics Toolbox構(gòu)建的初始狀態(tài)下的機器人機構(gòu)模型，機器人初始姿態(tài)θ1為5°，θ2為-25.7°。

圖5 絕緣子沖洗模型Fig.5 Washing model of the insulator

Visual Servoing Toolbox仿真實驗前，需對視覺伺服系統(tǒng)進行參數(shù)設(shè)定，包括攝像機的內(nèi)外參數(shù)、目標物體在視場中的初始位置和期望位置等。本文選取長為0.575 m，寬為0.125 m的長方形模擬絕緣子，4個頂點分別為4個特征點，攝像機固定于機器人末端執(zhí)行器，攝像機內(nèi)參數(shù)ax=1 000，ay=1 000，u0=256，v0=256，圖像平面分辨率為512×512，攝像機模型為理想的透視模型，不考慮鏡頭畸變，絕緣子與機器人水平距離為3 m，絕緣子底部高度為2.832 m，最后，得到圖5的聯(lián)合機器人模型構(gòu)建的整個仿真圖像。

假定初始狀態(tài)時，攝像機位置坐標到機器人基座標的變換矩陣根據(jù)式（2）-式（4）進行計算，最后得到的機器人初始狀態(tài)拍攝的絕緣子圖像見圖6，O點為圖像平面的中心點，P點為絕緣子底部中點。

圖6 初始狀態(tài)絕緣子圖像Fig.6 The intial state image of the insulator

當水槍瞄準絕緣子并準備沖洗時，O點應(yīng)與P點重合。圖7所示為在Visual Servoing Toolbox環(huán)境下得到理想狀態(tài)下，水槍瞄準絕緣子時4個特征點圖像的分布狀態(tài)，實際控制過程中，在得到理想特征點與初始特征點誤差之后，還不能進行伺服控制，必須對圖像深度信息進行實時估算。由于本文為仿真狀況，深度信息直接給出為3 m，但實際過程中并不能提前知道深度信息，需由初始圖像信息計算出深度信息。

圖7 理想狀態(tài)下絕緣子圖像Fig.7 The ideal state image of the insulator

絕緣子初始圖像6中，絕緣子底部中點P的圖像坐標為（339，181），為驗證上節(jié)中計算圖像深度方法的正確性，筆者將基于攝像機仿真模型進行深度逆計算，根據(jù)式（7），計算過程如下：

仿真圖像（見圖5）中機器人與絕緣子水平距離為3 m與計算結(jié)果相近，考慮計算誤差，筆者采用的圖像深度計算方法切實可行，能夠計算出絕緣子圖像的深度信息。

當知道特征點誤差和圖像深度信息之后，可進行基于圖像的伺服瞄準，圖8所示為基于IBVS模型仿真過程中4個特征點1、2、3、4的圖像運動軌跡，攝像機剛開始運動時，特征點位置變化較大，隨著特征點與理想點誤差減少，特征點位置變化變小，最終達到理想特征點位置，絕緣子區(qū)域運動至圖像中心。

圖8 仿真過程中特征點運動軌跡Fig.8 The movement tracks of feature point in the simulation

將機器人末端的運動軌跡與攝像機運動軌跡重合（見圖9），通過將此軌跡導入Robotics Toolbox的ikine（逆運動學）函數(shù)模塊可以求得機器人關(guān)節(jié)的變化過程，圖10所示為沖洗機器人關(guān)節(jié)角度變化情況，其中關(guān)節(jié)角速度變化可以作為機器人的控制輸入，θ1為沖洗機器人關(guān)節(jié)水平旋轉(zhuǎn)角，θ2為俯仰旋轉(zhuǎn)角。

圖9 攝像機運動軌跡Fig.9 The movement tracks of camera

圖10 機器人關(guān)節(jié)角度變化Fig.10 Joint angle change of the robot

機器人運動從初始狀態(tài)的θ1為5°、θ2為-25.7°，最終變化至理想狀態(tài)θ1為0°、θ2為-30°，角速度變化從初始的劇烈變化最終趨于平穩(wěn)，表明控制過程趨于收斂。仿真系統(tǒng)趨于穩(wěn)定所需的時間可以通過調(diào)節(jié)系統(tǒng)增益的大小進行控制。

4 結(jié)語

本次仿真實驗是基于圖像的伺服控制方式，通過求解圖像雅克比矩陣，建立機器人運動與圖像特征點之間的關(guān)系。驗證了視覺伺服過程中圖像深度計算方法，并得到圖像特征點的變化與機器人關(guān)節(jié)變化之間的關(guān)系，結(jié)果證明基于圖像的伺服控制方式可以實現(xiàn)水沖洗機器人自動瞄準絕緣子。

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

[2]雷洋.保留圖像特征點的機械手視覺伺服控制[D].杭州：浙江大學，2014.

[3]楊延西，劉丁，閆振杰.圖像反饋機器人視覺伺服系統(tǒng)仿真[J].系統(tǒng)仿真學報，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]黎志剛，段鎖林，趙建英，等.機器人視覺伺服控制及應(yīng)用研究的現(xiàn)狀[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].北京：國防工業(yè)出版社，2011.

[9]姚波，于蘭英，吳文海，等.變電所絕緣子水沖洗機器人設(shè)計[J].機械設(shè)計與制造工程，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]邱聯(lián)奎.基于機械手的視覺伺服控制及其應(yīng)用研究[D].合肥：中國科學技術(shù)大學，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—）男，博士，講師，主要從事機電液一體化技術(shù)的研究。