吳文海，姚 波，于蘭英，王國(guó)志

絕緣子水沖洗機(jī)器人視覺伺服仿真

吳文海，姚 波，于蘭英，王國(guó)志

（西南交通大學(xué)機(jī)械工程學(xué)院，成都610031）

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

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

0 引言

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

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

1 圖像雅克比矩陣計(jì)算

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

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

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

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

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

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

圖3 攝像機(jī)成像平面坐標(biāo)Fig.3 Plane coordinate of the camera imaging

將u和v帶入式（5），并對(duì)u和v求導(dǎo)，最后變換成如下形式：

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

2 圖像深度計(jì)算

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

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

圖4 攝像機(jī)測(cè)距模型Fig.4 Ranging model of the camera

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

式中：v0為光軸在像平面投影點(diǎn)的縱坐標(biāo)；v（p）為P點(diǎn)在像平面的縱坐標(biāo)。

3 視覺伺服仿真

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

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

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

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

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

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

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

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

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

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

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

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

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

圖8 仿真過程中特征點(diǎn)運(yùn)動(dòng)軌跡Fig.8 The movement tracks of feature point in the simulation

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

圖9 攝像機(jī)運(yùn)動(dòng)軌跡Fig.9 The movement tracks of camera

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

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

4 結(jié)語(yǔ)

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

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

[2]雷洋.保留圖像特征點(diǎn)的機(jī)械手視覺伺服控制[D].杭州：浙江大學(xué)，2014.

[3]楊延西，劉丁，閆振杰.圖像反饋機(jī)器人視覺伺服系統(tǒng)仿真[J].系統(tǒng)仿真學(xué)報(bào)，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ī)器人視覺伺服控制及應(yīng)用研究的現(xiàn)狀[J].太原科技大學(xué)學(xué)報(bào)，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]田凱.基于單目視覺的視覺伺服與位姿估計(jì)[D].武漢：華中科技大學(xué)，2009.

[8]徐德，譚民.機(jī)器人視覺測(cè)量與控制[M].北京：國(guó)防工業(yè)出版社，2011.

[9]姚波，于蘭英，吳文海，等.變電所絕緣子水沖洗機(jī)器人設(shè)計(jì)[J].機(jī)械設(shè)計(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]張廣軍.機(jī)器視覺[M].北京：科學(xué)出版社，2005.

[11]于乃功，黃燦，林佳.基于單目視覺的機(jī)器人目標(biāo)定位測(cè)距方法研究[J].計(jì)算機(jī)測(cè)量與控制，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)奎.基于機(jī)械手的視覺伺服控制及其應(yīng)用研究[D].合肥：中國(guó)科學(xué)技術(shù)大學(xué)，2006.

[13]于蘭英，姚波，吳文海，等.一種基于多特征的絕緣子識(shí)別方法[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]尚俊霞.復(fù)合絕緣子憎水性檢測(cè)與等級(jí)判斷的研究[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—）男，博士，講師，主要從事機(jī)電液一體化技術(shù)的研究。