Strojniški vestnik - Journal of Mechanical Engineering 51(2005)11, 724-736 UDK - UDC 007.52:658.52.011.56 Strokovni članek - Speciality paper (1.04) Vodenje robota na temelju zaznaval sile in računalniškega vida Robot Control Based on Force and Vision Sensors Leon Lahajnar - Leon Žlajpah V prispevku je predstavljen postopek za sledenje krivulji. Med sledenjem robot vzdržuje stik s podlago z zahtevano silo. Metoda temelji na zbiranju in analizi vidne informacije ter merjenju sil z ustreznimi zaznavali. Tako pridobljeni podatki se vključijo v algoritem, tako da se zagotovi želeno delovanje sistema. S postopkom je mogoče izboljšati robotske sisteme v industrijskem okolju. © 2005 Strojniški vestnik. Vse pravice pridržane. (Ključne besede: sistemi robotski, vodenje robotov, zaznavalne sile, vid računalniški) In this paper a robot-control algorithm for tracking a curve on a surface is presented. During the tracking the robot maintains contact with the surface at a predefined force. The method is based on visual information provided by a camera mounted on the robot’s end effector, and the measured force acquired from a force sensor. The obtained data are analyzed and the required information is employed in the control algorithm to ensure satisfactory operation. The proposed solution gives new enhancement opportunities for industrial robot-based applications. © 2005 Journal of Mechanical Engineering. All rights reserved. (Keywords: robotic systems, robotic control, force sensors, computer vision systems) 0 UVOD 0 INTRODUCTION V industrijskih uporabah robotskih sistemov se pogosto pojavi zahteva, da je orodje nameščeno na robotsko roko, v stiku s podlago in sledi določeni krivulji. Primeri take uporabe so nanos lepil ali tesnilnih mas na spojne krivulje površin, poliranje posameznih delov površin ali brušenje spojev med površinami in podobno. Hkrati s sledenjem neki krivulji po površini je v nekaterih primerih uporabe pomembno tudi razpoznavanje same površine po krivulji. Tako je v primerih uporabe mogoče med samim opravilom odkriti različne napake v obliki površin, ki so lahko posledica napake v izdelavi posameznih izdelkov ali vgrajenih komponent v izdelke. V preteklosti so se mnogi avtorji ukvarjali z opisano tematiko in so se problemom posvečali na različne načine. V prvih fazah poskusov vodenja robotov z optičnimi zaznavali so bili ti v večini primerov zasnovani na kalibriranih sistemih, ki so delovali na temelju izgradnje 3D informacije iz stereo ali podobnih sistemov za zajem slik, kjer so bile kamere za zajem večinoma nepremično nameščene, ali vodene In industrial applications there are frequent demands to maintain contact between the tool mounted on the robotic hand and the processed surface. Examples of such demands are the operations where a glue or a sealant needs to be carried to the surface, where the polishing of a surface has to be performed or where a contact between surfaces has to be brushed. For such robotic applications it is also interesting to be able to detect surface defects, or to identify the wrong types of assembled parts. This can be solved by identifying the shape of the surface along the tracked curve. In the past, several researchers have worked in this field. The first tests of robot control with visual sensors were based on calibrated systems. 3D information about the environment was acquired by stereo or similar visual systems. Cameras were fixed or controlled by special camera-control manipulators. Such systems have some drawbacks that limits their practical use, e.g., high calculation demands and the occlusions of observed scenes in the case of fixed 724 Strojniški vestnik - Journal of Mechanical Engineering 51(2005)11, 724-736 s posebnimi rokami za vodenje kamer. Stereo sistem ima velike računske zahteve, poleg tega pa se posebej v primeru stalno nameščenih kamer pojavijo problemi zakrivanja opazovanih predmetov. Za zmanjšanje računske zahtevnosti so bile opravljene določene poenostavitve na podlagi analize stereo slik med približevanjem robotske roke predmetu. V zadnjih letih so se na področju robotike in vgradnje robotskega vida v sisteme pojavili sistemi z vizualnim zaznavalom, nameščenim na vrh robotske roke. S to postavitvijo se je mogoče izogniti problemu zakrivanja, poveča pa se natančnost zaradi ožjega področja zanimanja [1]. Problem robotskih sistemov, opremljenih zgolj z vizualnimi zaznavali, so optične lastnosti, ki jih morajo za robustno delovanje 3D algoritmov vsebovati opazovani predmeti. Z namenom,da se zagotovi stalen stik s podlago in hkrati omogoči sledenje potem po površinah, so se razvili sistemi, ki vsebujejo kombinacijo vizualnih zaznaval in zaznaval sile, ki merijo silo, s katero robotska roka pritiska na površino. Zaznavalo sile je lahko nameščeno različno. Namestitev zaznavala sile na vrhu orodja se uporablja predvsem za natančno sledenje obrisov [2], vendar taka razporedba onemogoči namestitev orodja, zato se pogosteje gradijo sisteme z zaznavalom sile v zapestju robota. Kombinacije teh zaznaval je za sledenje krivulji na neznani podlagi z nespremenljivo silo v smeri orodja uporabil Xiao [3]. Opisani način ne omogoča določitve nagiba površine. Sistem poišče točke na ravnini in predpostavi, da je krivulja med sosednjima točkama premica. Kamero, nameščeno na vrh robota, sta uporabila Baeten ([4] in [5]) za predikcijsko sledenje obrisa ravnega predmeta in Malis [6] pri natančni namestitvi orodja. Vsi navedeni primeri vsebujejo zgolj eno s silo vodeno smer, tako da ne pridobijo informacije o usmeritvi orodja na podlago. V prispevku je opisana metoda, ki omogoča sledenje krivulji in zaznavanje usmeritve in temelji na hitrostno vodenem robotu. Sistem poskrbi za sledenje krivulji, zarisani na površini, tako da vrh orodja, nameščenega na robotski roki, zagotavlja stalen stik s površino, ne glede na njeno obliko. Sistem na vsaki točki poti določi normalo na površino in razpozna točko površine glede na celotni koordinatni sistem. Sistem je zasnovan na zaznavalu sile in podaja informacijo o silah, ki delujejo na orodje v trirazseDnem prostoru in na slikovnem zaznavalu, ki preslika 3D informacije v dvorazsežni slikovni cameras. To reduce the calculation demands some simplifications based on image analyses were introduced during the approach to the observed objects. In recent years robotic systems with the image sensor mounted on the end effector appeared. With this configuration the problems of occlusion can be avoided and a higher accuracy, because of a more focused area of interest, can be achieved [1]. The main problem of robotic systems based only on visual sensors are the optical characteristics of the observed objects required in order to achieve robust behavior of the 3D extraction algorithms. To establish and maintain a constant contact with the surface and simultaneously to track the desired trajectory, systems consisting of visual and force sensors were developed. Force sensors can be mounted in various ways. If the sensor is attached to the top of the tool the system is able to attain precise contour tracking [2]. Such a configuration disables the installation of a tool, therefore wrist-mounted force sensors are used in many cases. The combination of the described sensors for curve tracking on an unknown surface with constant force in the direction of the tool was presented by Xiao [3]. With this approach it was not possible to identify the surface orientation. In the experiment some simplifications were made, and then the system was only capable of moving from the starting point to the end point in a straight line. A camera mounted on the top of the manipulator was used by Beaten ([4] and [5]) for predictive, planar contour tracking and by Malis [6] for the precise placement of the tool. In all these examples the force was controlled in only one direction. Consequently, no information about the orientation between the tool and the surface can be derived, and hence no identification of the surface orientation can be made. With the method presented in this paper, a velocity-controlled manipulator tracks a curve drawn on an uncalibrated surface and perceives its orientation. The system maintains constant contact with the surface and tracks the curve regardless of the dynamics of the surface or the curve. With the proposed method, in each position of the curve the surface normal is determined and the tool is placed perpendicular to the surface. Our system consists of a force sensor and a vision system. The force sensor provides six dimensional information about the force and torque acting on the tool, while the camera transforms 3D Vodenje robota - Robot Control 725 Strojniški vestnik - Journal of Mechanical Engineering 51(2005)11, 724-736 prostor. Slikovno zaznavalo je v našem primeru nameščeno na zapestje robotske roke, kar preprečuje zakrivanje, hkrati pa ni ovira za izbiro ustreznega orodja. V nadaljevanju prispevka je opisana metoda zbiranja informacije o silah in slikovne informacije, postopek združitve informacij, pridobljenih iz obeh zaznaval, delovni prostor robota. Nazadnje je opisana uporaba modela hibridnega vodenja. Sistem je bil v praksi zgrajen iz gradnikov, ki so opisani v poglavju Eksperimentalni sistem. Prispevek se konča s sklepnim poglavjem, v katerem so opisane naše ugotovitve in podane možnosti izboljšave sistema. Namen našega sistema je sledenje krivuljam na neznani površini in ob tem ohranjati orodje, usmerjeno pravokotno na podlago. Zadano nalogo je mogoče izvesti s kombinacijo zaznaval različnih veličin. Sledenje krivulji je izvedeno z računalniškim vidom, medtem ko se orodje površini prilagaja na podlagi informacij, pridobljenih iz zaznavala sile. Združevanje različnih informacij, pridobljenih iz zaznaval, ki obsegajo različne veličine in so ob tem še prostorsko odmaknjeni, je treba predstaviti kot celoto in jo uporabiti za učinkovito vodenje robota. Združitev podatkov, zbranih z merilnikom dotika in slikovnim zaznavalom, je zaradi njegovih lastnosti primerno izvajati v delovnem prostoru. Ta prostor omogoča preslikavo sil in leg sledene krivulje, zajete s slikovnim zaznavalom, v različne koordinatne sisteme. S tem opisom okolja se vzpostavi pravokotnost, ki omogoča neodvisno, hibridno vodenje na podlagi slikovne informacije in meritve sil. 1.1 Delovni prostor Povezavo med silami in lego v delovnem prostoru je prvi definiral Mason [7]. Delovni prostor je postavljen v delovno točko orodja. Vse naloge v delovnem kartezičnem koordinatnem sistemu (KS) se razstavijo na tri premike v smereh x, y, z in tri zavrtitve okrog osi x, y, z. S kombinacijo izvajanja teh podnalog se izvedejo zelo raznolike robotske naloge. Predstavitev delovnega prostora omogoča tudi preslikavo sil in vrtilnih navorov iz zaznaval v delovni prostor. Z uporabo predstavitve usmeritve z vzponom, odklonom in nagibom (VON - RPY), ki je najpogosteje uporabljena pri vodenju robotov, se information of the scene into the image space. The camera mounted on the hand of the robotic manipulator reduces the possibility of occlusion and also gives the opportunity to use a fastened-on end-effector. In this paper the method of the force and vision sensors data acquisition and data analysis is described. The extracted data are integrated into a proposed task frame. The proposed method was validated by different tests that were carried out on a testbed described in the section called Experimental system. In the Conclusion the findings and some challenging future improvements of our system are discussed. The main goal of our work is to track a curve on an unknown surface. Additionally, the tool has to be oriented perpendicular to the surface. The given task can be executed using the data obtained by sensors of different modalities. The curve tracking is based on a vision system, while the tool adaptation to the surface is controlled by a force sensor. Sensor information, which represents the different modalities and is retrieved in different positions, has to be combined in its entirety to be used for efficient robot control. The integration of the data captured by the force and the vision sensors is reasonable to be done in the task frame. The task frame formalism enables the transformation of forces and the position of the tracked curve captured by the vision sensor, between different coordinate systems (CSs). Within the task frame the orthogonality is established. This enables hybrid control based on visual information and force measurement. 1.1 Task frame The linkage between the force and the position in the task frame was proposed by Mason [7]. The task frame is positioned on the top of the tool. All tasks in the task frame’s Cartesian coordinate system (CS) are divided into three translations in the direction of the coordinate axes and the three rotations around the coordinate axes. Through a combination of the defined basic motions all the robotic tasks can be executed. In the task frame formalism the forces and torques can be transformed from one coordinate system (CS) to another. If the orientation is represented as roll, pitch, yaw (RPY) (which is most commonly used in manipulator control) then the 1 METODE IN POSTOPKI 1 METHODS 726 Lahajnar L. - Žlajpah L. Strojniški vestnik - Journal of Mechanical Engineering 51(2005)11, 724-736 sile (F) in vrtilni navori (t) na posamezne koordinatne sisteme preslikajo takole: forces and torques between different CSs are transformed in the following way: tF = RPY(stjx,stjy,st jz)sF tt r = RPY(stjx,stjy,t j z) ((tspx,tspy,stpz)T x sF+st r) (1), (2), kjer oznaka (.) označuje KS, iz katerega poteka preslikava, (.) pa KS, v katerega se sile prenesejo. V našem delu opazujemo štiri KS. Prvi je pritrjen na vrh orodja in ga označimo s predpono (.), drugi predstavlja kamero (.), tretjega pa določa zaznavalo sile (.). Vse dogajanje je predstavljeno v osnovnem KS robota. KS orodja in merilnika sil in vrtilnih navorov sta enako usmerjena in premaknjena v smeri osi z za razdaljo L. Razdalja L je zaradi podajnega orodja odvisna od sile, ki deluje v smeri orodja in jo določimo posredno prek F: where (.) marks the CS from which the data is processed, while (.) represents the destination CS. In our work four CSs are observed. The first one t(.)is attached to the top of the tool, the second one c(.) represents the vision sensor, the third one s(.) defines the CS of the force sensor. All the occurrences are presented in base CS of the manipulator. The CS of the tool and the CS of the force sensor have the same orientation and are translated for L in the z direction. Because of the compliant tool the distance L is subjected to the contact force F hence, the force L is measured indirectly through force sFz: L=L +ms Fz (3), kjer sta L0 razdalja med izhodiščema KS in KS, ko na orodje ne deluje nobena sila dotika, m pa koeficient podajnosti orodja. Medtem ko so medsebojne usmeritve med KS, KS in KS stalne, pa se njihova lega glede na osnovni KS spreminja. Preslikavo med KS zaznaval in bazičnim KS določimo na podlagi kinematike robota. where L0 is the distance between tCS and sCS, when there is no contact force on the tool, and m is the coefficient of the tool compliance. The orientations between tCS, cCS and sCS are fixed. However, their position in the base CS is changing. The transformation between these CSs and the base CS is obtained from the direct kinematics of the robot. Sl. 1. Postavitev koordinatnih sistemov Fig. 1. Placement of coordinate systems Vodenje robota - Robot Control 727 Strojniški vestnik - Journal of Mechanical Engineering 51(2005)11, 724-736 1.2 Merilnik sile in vrtilnega navora 1.2 Force sensor Merilnik sile in vrtilnega navora je nameščen v zapestju robota in izveden tako, da kot rezultat vrača izmerjene vrednosti sil in vrtilnih navorov v smereh pravokotnih osi x, y, z. Izmerjena sila F r je sestavljena iz sil dotika Fco, pospeška robota F in težnosti F : The force sensor is mounted on the wrist of the manipulator. We measure the forces and torques in the orthogonal directions x, y, z. The measured forces consist of the contact Fco , inertia Fa and rr gravity Fr forces: rr rr F =F +F +F (4). Za namen vodenja je pomembna samo informacija o sili stika, zato je treba iz meritev odstraniti vpliv teže in vztrajnosti. Za izravnavo vztrajnosti je treba poznati pospeške orodja. V večini primerov pospeškov ni mogoče meriti. Ker so pospeški majhni v primerjavi s silami, s katerimi delujemo na obdelovano površino, lahko F v našem primeru zanemarimo. Vpliv teže orodja izravnamo v fazi kalibracije robotskega sistema na način kakor ga je predlagal Omrčen [8]. Pri tem postopku na podlagi meritev sil in vrtilnih navorov v treh različnih znanih usmeritvah izračunamo maso in težišče orodja. Usmeritev orodja pridobimo v vsaki točki iz zavrtitvene matrike robota. Iz tako pridobljenih podatkov izračunamo F in jo odštejemo od izmerjene Fr. Sile, zaznane z zaznavalom sile, se z enačbama (1) in (2) preslikajo na vrh orodja. V nadaljevanju se oznaka F zaradi poenostavitve zapisa nanaša le na sile, ki se vzpostavijo zaradi stika orodja s podlago. Tako obdelani podatki so uporabljeni za identifikacijo površine. V statičnih razmerah sila podlage deluje na orodje v smeri normale na podlago. In the control we need only the information about the contact. Therefore, we have to eliminate the contribution of the gravity and the inertia forces from the measured data. If the exact acceleration of the tool were to be known, inertia could be compensated. However, in our case F is small compared to Fco and therefore, it can be neglected, i.e., the gravity influence was compensated in the calibration phase of the robotic system as proposed by Omrčen [6]. On the basis of force and torque measurements in three known different orientations of the tool weight and the centre of mass of the tool can be calculated. The orientation of the tool is known in each point (from the rotation matrix of the robot). F can be calculated and subtracted from the measured Fr. Using Equations (1) and (2) the forces measured with the force sensor are transformed to the top of the tool. For simplicity we denote in the following the contact forces between the tool and the surface as F. The processed data are used for the surface identification. In static conditions the force of the surface acts on the tool in the direction normal to the surface. Sl. 2. Sila dotika, delujoča na vrh orodja Fig. 2. Contact force on the top of the tool 728 Lahajnar L. - Žlajpah L. Strojniški vestnik - Journal of Mechanical Engineering 51(2005)11, 724-736 Iz komponent sile F izračunamo nagibna kota From the components of force F the inclina- glede na površino (sl. 2): tion angles to the surface are calculated (Fig. 2): 9>x=arctan Fx ) (5) m =arctan F ) (6). Kadar tipalo drsi po površini, se v sili F izmeri When the tool is sliding over the surface a tudi sila trenja, ki deluje v nasprotni smeri gibanja. frictional force in the opposite direction of the motion Silo trenja je mogoče oceniti v primeru znanega is present. The friction force can be calculated only koeficienta trenja in ravnih površin, kar pa v praksi if the friction coefficients are known and the surface ni najbolj pogosto. V primeru neupoštevanja sile is flat. As in practice this is usually not the case, the trenja se pri drsenju pojavi določen kot napake pri influence of the friction is neglected and nagibu orodja glede na normalo površine. To napako consequently some error is introduced to the zmanjšamo z uporabo orodij z majhnim koeficientom inclination angles. To minimize these errors we use trenja. tools with low friction coefficients. 1.3 Slikovno zaznavalo 1.3 Vision sensor Za sledenje na površini izrisane poti uporabimo na zapestje nameščenno slikovno zaznavalo (kamera). Zajeto sliko je treba obdelati. Da bi sistem omogočal kar najbolj robustno delovanje, je zajem slikovne informacije zastavljen na barvni kameri, v barvnem prostoru “barvni odtenek -nasičenost - vrednost” (HSV). Slika je razčlenjena na podlagi barvne sestavine H in S. V prvi fazi se v področju zanimanja, predstavljenem v pravokotniku s polno črto (sl. 3a), na podlagi barvne informacije določi področje slike, ki pripada orodju. Iz tega področja se izračuna koordinate vrha orodja P. Zaradi znane medsebojne lege orodja, glede na kamero, se pri analizi slike poišče le vrh podajnega orodja, katerega dolžina se v odvisnosti od sile, s katero deluje na podlago, lahko spreminja. Potek krivulje se poišče s Canny-evim postopkom iskanja robov na sestavini barvnega prostora H (sl. 3b). Krivulja na sliki ima dva robova. Za točke krivulje so upoštevane zgolj točke srednjih vrednosti robov. Ob tem se preveri še, ali je odtenek barve med robovoma podoben barvi, ki je v fazi kalibracije določena kot barva krivulje. Na podlagi segmentiranih podatkov se po metodi najmanjših kvadratov približa potek krivulje s kvadratnim polinomom v smeri premika orodja (sl. 3c). V višini vrha orodja j se iz približne krivulje določi tangento in izračuna razdaljo med orodjem in tangento e (sl. 3d). Po tangenti se določi kot