A new fuzzy visual servoing with application to robot manipulator

Transcription

1 2005 American Control Conference June 8-10, Portlan, OR, USA FrA09.4 A new fuzzy visual servoing with application to robot manipulator Marco A. Moreno-Armenariz, Wen Yu Abstract Many stereo vision algorithms reuire the images which are very similar in appearance, an the istance between the two cameras shoul be small enough. Normal visual servoing of robot manipulator nees both image information an joint velocities. In this paper we make two moifications to overcome these problems: 1) a new simple stereo vision moel is propose, 2) a fuzzy controller base on only visual information is implemente to a robot manipulator. We successfully apply our new algorithms to real-time visual servoing of a inustrial robot. The stereo moel propose in this paper can also be extene to other applications such as, mobile robots, 3D reconstruction, etc. I. INTRODUCTION The basic iea of stereo computer vision is to reconstruct objects using the information of two well-selecte sensors. These sensors are separate by a constant istance, which permits to know the epth of the object. In general, recognition of 3D object reuires two or more appropriately efine 2D images. For human being, corresponing points in the visual cortex in the process are calle binocular stereopsis. Years of research have shown that etermining stereo corresponences by computer is not an easy problem [6]. Many methos have been propose for stereo vision, such as structure analysis from motion [10][12], stereo corresponence calculation [9][6][4] an shape reconstruction [11]. These methos can be escribe as four-steps process: 1) use visual algorithms to fin features in an image, 2) fin the same features in the other camera, 3) stuy the ifferences of the same point between the two cameras, 4) calculate the actual position of the feature point by the parameters of the cameras. For real applications, many methos are subject to one or more limitations. It is not easy to fin a irect relation in the images of two cameras. A trae-off between efficiency an accuracy is often necessary, such that a satisfie accuracy can be achieve while large computation is avoie. Many current methos are successful when the images are very similar in appearance, as in the case of human vision. This reuires the two cameras have to be put as close as possible, an the objects shoul also be close to the cameras. When the istance between the cameras becomes large, surfaces in the images exhibits ifferent egrees of foreshortening, there are large isparities in their locations in the two images. All of these make it ifficult for computers to etermine correct stereo corresponences. In this paper we This work was supporte by CONACyT uner Grant 38505A. Marco A. Moreno-Armenariz is with Escuela e Ingeníeria, Coorinación e Investigación, Universia La Salle, Benjamin Franklin 47 Col.Conesa, Mexico D.F, 06140, Mexico. Wen Yu is with the Departamento e Control Automatico, CINVESTAV-IPN, Av.IPN 2508, México D.F., 07360, México yuw@ctrl.cinvestav.mx /05/$ AACC 3688 present a new visual algorithm, which is an exten of the famous Weak-Perspective camera moel [13]. It can process the images uickly an efficiently, an can realize autocalibration. The use of a vision system to complete the feeback control for a robotic system is referre as visual servoing [7]. It has the potential to provie a low-cost an a lowmaintenance automation solution for unstructure inustries an environments. Visual servoing is a rapily maturing approach for robot manipulator control that is base on the visual perception of a robot an a work piece location. It is well known that most of inustrial manipulators are euippe with the simplest proportional an erivative (PD) controller. For camera base visual servoing, PD-like control is also very popular [8][3][5], h = [ ] i where an are the positions of en-effector an target in the images, an are real an esire joint velocities, an are positive matrices. This PD controller reuires measurements of both images an joints velocity. It is very important to realize visual servoing with only vision information. In this paper, we propose a novel fuzzy controller base on our stereo vision algorithm. Several experiments show excellent behavior of the stereo vision algorithm an the fuzzy control, even when a large amount of salt an pepper noise are presente. This research is base on the following assumptions: 1) The visual marks attache to the robot manipulator are completely visible. 2) The target point is static an it is insie the visual space. 3) There exists at least one joint position < 3 (the robot is 6 DOF, 3 links are correspone to rigi robot manipulator, the other 3 links are on the robot s han), such that the en-effector of the robot is able to reach the target point. < 3 Assumption 1 limits the motion of the robot. This means that the first egree of freeom of the robot coul be to rotate ±30 (see Fig. 1). Assumption 2 means that the vision algorithm propose in this paper is suitable for fixe points. Assumption 3 gives a possibility to locate only the position (without orientation) of the en-effector of the robot an to esign a fuzzy visual servoing. II. STEREO VISION PLATFORM AND VISION ALGORITHM The fuzzy control system base on computer vision is shown in Fig.1. The main parts are: A Robot Manipulator, CRS Robotics A465. A pair of analog cameras, Pulnix TM72-EX. A pair of lenses, Cosmicar C815B. A frame grabber, NI A personal computer with all the algorithms presente in this paper. An articular controller, CRS Robotics C550C.

2 will be represente in the resize image with at least 3 pixels. The finalsizeoftheimageis91 68 pixels. Make a segmentation of the visual marks which are obtaine by the resize process. After resizing the image we have a list of pixels (its coorinates) that represents the three visual marks. By the mechanical behavior of the robot, we know that the visual marks of the robot never overlap. An example of our visual algorithms are presente in Fig.2. The first column of table presents the coorinates of the pixels obtaine by the resize process. The secon column shows the coorinates that are separate in three objects by the segmentation process. The thir one is the centroi for the three visual points.4) Fin the centroi of the visual Fig. 1. Structure of Computer Vision System. In orer to reuce noise influence, the scene backgroun is set to be black an only three white marks are put on the robot. These marks are attache to three main parts of the robot: base, elbow an en-effector. The mark of eneffector has a black circle insie it. We know the initial position (reay position) of the en-effector of the robot. The other two marks are use as references. The goal of vision algorithm is to etect the three white marks from images, an obtain the centrois of the marks such that we can use these values in optical stereo vision moel which will be explaine in the next section. We use following four steps to process the images: 1) Binaries the image. We establish an appropriate threshol value to binaries the image. The PCI-1408 evice can automatically execute this process. 2) Apply a resize process to the binary image. Since the visual marks (circles) have a iameter of =26pixels, we only nee to search for objects which have this size. Any smaller objects will be consiere as noise. However thewholesizeofthebinaryimageis pixels, there are too much information to be processe continuously. In orer to work effectively, we resize the binary image by the following factor: = sin (45 ) (1) 25 We just work with the resize image consierably reucing the amount of information to be compute. To obtain the resize value we mae a benchmark test which can calculate execution time via -values. is change from 1 to 25 ( =1means that no resize is applie, binary image is pixels). We foun that the best value is =7 because this value has a better balance between the execution time an the amount of pixels that represents the visual marks. So every object in the binary image Binary image & Noise Resize & Segmentation O 2 O 3 O C 2 C 3 C 1 Centroi Resize (x,y) Segmentation (x,y) Centroi (x,y) O C O C O C Fig. 2. Example of appling the visual algorithms to obtain the centroi ofthethreewhitemarks. marks. Now we present an algorithm to relate an label each centroi of the marks (note that one of the visual marks has a black circle insie). At reay position, the centrois are shown in Table 1. Table 1. LIST OF CENTROIDS TO BE LABELED Object Centroi Object Object Object If there are black pixels insie the mark it is eneffector. The separation of the other two marks is mae by comparison of -values of them, the bigger one is labele as elbow an the smaller one is base. For the Table 1, we label the marks as in Table 2. Table 2. CENTROIDS RELATED WITH A PART OF THE ROBOT

3 Label Centroi En-effector Elbow Base With above algorithm the computer can acuire uniue corresponence between the visual mark an the robot s arm. A minimization reuirement of the visual algorithm is the robot an its work space shoul appear complete in the images. We use a pair of lens, Cosmicar C815b. The focal length is 85. A camera with this kin of lens at two meters to the scene can capture images in working space as This istance satisfies the minimum reuirement of the inustrial robot we use. However, such big working space prouces a istortion (name raial istortion). Optical stereo vision system is base on the centroi of the white marks. It s very important to eliminate the raial istortion. We use the algorithm presente in [13], 0 = (2) where is a correcte raius an 0 is the istortion raius. In our experiment we obtain that 2 = an 4 = The coorinates without istortion are obtaine by = = (3) where ( 0 0 ) are the coorinates of the centroi of the marks in the istortion image, ( ) are the new coorinates without istortion, ( ) are the coorinates of the center of the image. This section solves the first part of the stereo corresponence problem [13], the secon part of this problem is to etermine the position of en-effector, which will be solve by the optical stereo vision moel in the next section. Now we iscuss some concepts of stereo vision. An optical camera moel is shown in Fig.3A relation between Real Object h Fig. 3. the istance an height is Object lens f F Ocular lens h a CCD Sony Variables efine for the optical moel. 0 = 0 1 = (4) where an are efineinfig.3, 0. In real application, an 0 are obtaine by = = 0 Since the units of an 0 are centimeters an pixels, we have to use a factor as = With this factor we obtain = an 0 = We first introuce a simple stereo moel, the two cameras are mounte in the same plane with the istance of.the position of the two cameras in worl frame may be efine as [ 2 0 0] an [ 2 0 0] Image planes are in front of each camera with a istance We assume that -axis of the camera is the optical axis. Now the problem is how to use the image information ( ) an ( ) to the corresponing worl frame ( ). Usingso-calle pinhole lens moel [6], when 6= an the two camera axes are not parallel = + (6) 3690 Usually there not exit an such that [ ]= [ ], but we may fin ( ) such that [ ] [ ] 2 is minimize, i.e., (5) 2 = k + k (7) Since the relation (7) between the worl frame an the images of the two cameras is not an analytical expression. An approximation approach, name visual space metho can be use [4], = 1 sin = 1 sin (8) = 1 sin tan ( ) The error between [ ] an [ ] is efine as [ ] Now we iscuss our new stereo vision moel, it is another approximation an is simpler than [4]. This moel is use for locating the en-effector of robot using visual information, it can calculate the 3D position of robot by the centrois of the object an the rotate angles of the cameras. The top view of the robot is show in Fig.4. Here is the istance between the 1 mark an the 2 mark. The full geometric moel is shown in Fig.5, perceive that the optical camera moel is incorporate (the efinitions of 0 an 00 can be foun in Fig.3). The position of the en-effector is obtaine by the following proceure: 1) Calculate the angles 1 an 1 for the left an right cameras with respect to x-axis of the robot, see Fig.4). The values of these angles can be use while the cameras are fixe. The visual system is setup such that the xz-plane of the robot an the uv-plane of the camera are parallel when the base an the elbow visual marks are aligne, see Fig.4. To o it, we use the following steps: (a) Set the robot in Reay Position (see Fig.1). (b) Rotate the 1 angle (joint 1) of the robot by 025 (see Fig. 1) an capture an image

4 Fig. 6. Elbow visual mark ( 1 ) an en-effector mark ( 2 ) The istance between these points: left camera, right camera. Fig. 4. '' Distances between the cameras an the robot. ' θ 1a f l e 1 a 1 a a f 2 a 2 c Fig. 5. Full geometric iagram use to obtain the istances between the cameras an the robot. with the right camera. (c) Apply our visual algorithms to locate the centroi of the three visual marks an store it. () Compare the values of the which coorinate the visual marks of base an elbow, if the values are the same (meaning aligne) go to step (e), if not return to (b). (e) Stop to move the robot an rea the angle 1 (joint 1) from the controller. The value of this angle is store as 1 an correspons to the angle of the right camera. (f) Repeat the proceure using the left camera an store the value as 1. 2) Select elbow an en-effector visual marks ( 1 2 ) an calculate the centroi coorinates ( ) an ( ) for both points in each image. Calculate the istance between the two points in the left an right images an set the istance as an respectively, see Fig.6. b 1 e 2 b 2 b b f 1 f r θ 1b ' '' ) From Fig.5 we obtain the next group of euations ( ³ tan 1 = tan = ( 0 ) 2 +( 2 1 ) 2 2 = ( 1 ) 2 +() 2 (2 1 cos ( 1 )) ³ 2 = 1 2 ( 1 ) 3 = =180 ( 1 1 ³ = tan 3 = tan = ( 2 ) 2 +() 2 (2 2 cos ( 3 )) 2 = ( 0 ) 2 +( 2 3 ) 2 ³ 1 = = 1 1 ( 3 ) 6 = = = ) The istances are obtaine as: left camera 1 = (2) ( 2 ) right camera 2 = (1) ( 2 ) 2 = ( 5 +1) ( 2 ) 1 = (2+ 5 ) ( 2 ) where 1 is istance between the left borer of the image an visual mark 1 in left camera, 3 is istance between the left borer of the image an visual mark 1 in right camera, 2 is half of the with of the images capture by the cameras (320 pixels), is istance between the visual marks 1 an 2 ( cm), is angle between the -line an the -axis of the robot (41620 ). After the calibration of the Reay position, the robot is free to move, conseuently the centroi coorinates ( ) an ( ) of the visual mark 2 are unknown, therefore the istance is also unknown (see Fig.4). In this part our main goal is obtain the position (coorinates ( )) of the visual mark 2 (en-effector). To obtain the value of z- axis, we assume that both cameras are mounte at the same height an istance to the robot to ensure this conition, (we use an aitional program to move the cameras until this conition is fulfille) we establish the next relation (9) = = (10) To obtain the coorinates we use an analog geometric moel as shown in Fig.5, the ifference now is we reuire to

5 calculate the istance 0 an the angle 0 as shown in Fig.4. Using the same iea we obtain the coorinates = 0 cos ( 0 ) = 0 sin ( 0 ) Remark 1: Compare with the normal 3D moel (8), the avantages of our moel are: General use, using just three visual marks in the scene we can obtain the position of an unknown point. Distance between the cameras an the scene, it is possible to change the istance between the cameras an the robot while the experiment is carrie on, our stereo moel can recalculate them automatically as in (8). Useful for applications, our new stereo moel oes not reuire a fixe istance between the two cameras or a vergence angle as in (8). The calculation euations are very simple, it can be use in ifferent kins of applications in inustry. III. FUZZY VISUAL SERVOING The common use of this kin of inustrial robot manipulator is first to save the trajectory of the en-effector in the articular controller, then the robot can move by the interior PID controller. The fuzzy visual servoing propose in this section can give this robot more flexibility, because the "eye" of robot can see an unlimite number of points an sen them to the articular controller automatically. In Section II, we propose the vision algorithm an stereo vision moel, which can generate a 3D position of robot s en-effector. In this section, we will use fuzzy control techniue to esign a fuzzy controller to move the inustrial robot to a esire position. Since this fuzzy control is base on the visual information, we calle it fuzzy visual servoing. In orer to simplify the experiment, we only control the three egrees of the 6 DOF robot, 1 2 an 3 in Fig.4. We esign three fuzzy controllers for each angle. The fuzzy controller will generate an angular change comman an sen it to the articular controller by the special instruction, name RAPL-II. Selection of state an control variables. Since the robot A465 an its lower level controller C550C reuire angular comman, we efine the joint angular error an its erivation as state variable = = (11) where =123 is the actual value of the i-angle, is the target value of the i-angle, is the actual value of the i-error, is the previous value of the i- error. These variables are inputs to each fuzzy moule, the output (control variable) is also angular value. Since the visual information is position of the en-effector, we use the robot kinematics to transform the position information to angular. Now we can calculate the three angles by inverse kinematics moel of the robot from the worl frame ( ) which is obtaine from 1 2 an 3.Weuse pseuocoe metho [1] to calculate inverse kinematics. The pseuocoe is as follows: 3692 ( 0 =0 1) Calculate 1 = ³ tan 1 6= 0 2) Calculate an 4 1 = = =cos 1 ( 1 ) 2 =tan 1 3 =cos 1 ( 2 ) where = = p ) Calculate 2 = = B. Creation of the fuzzy rules The fuzzy sets are shown in Fig.7, where 1) Error () fuzzy label LN1 N1 Z1 P1 LP1 2) Derivation of the error () fuzzy label LN2 N2 Z2 P2 LP2 statement Large Negative value Negative value Zero value Positive value Large Positive value statement Large Negative value Negative value Zero value Positive value Large Positive value 3) Output variable, angular value fuzzy label LN3 N3 Z3 P3 LP3 statement Large Negative value Negative value Zero value Positive value Large Positive value Although the range of the control variable is very ifficult to ajust, we can construct our fuzzy rules by the robotics prior information. The corresponing fuzzy rules are obtaine, the normal form as [14] is shown in Fig.8. C. Implementation of fuzzy visual servoing an experimental results The fuzzy visual servoing is applie in following steps: 1) The user set up coorinates for the en-effector of the robot an the target. 2) The stereo vision platform obtains visual information of the en-effector an the target. 3) Using the inverse kinematics moel of the robot, we transform the positions of the en-effector an the target into their corresponing angles. 4) Apply our fuzzy visual servoing to move the robot to the target. 5) If the input variable is in esire accuracy for all fuzzy moules, the proceure is stoppe, otherwise go to step 2. The complete control time for fuzzy visual servoing is about 15 which inclues 10 articular controller, 20 vision system an 1 fuzzy controller. The real-time experimental results are shown in Fig.9. We check 6 ifferent ranges for the fuzzy output variable () in this experiment. They are: 1) We set the

6 µ(e) 1.0 GN1 N1 C1 P1 GP e µ( e) GN2 N2 C2 P2 GP e µ(γ) 1.0 GN3 N3 C3 P3 GP3 0.5 Fig. 9. Exprimental Results to use fuzzy visual servoing in the robot. Fig. 7. values Fuzzy sets for the variables an with its final tuning Fig. 8. LN1 N1 Z1 P1 LP1 γ LP3 LP3 LP3 Z3 Z3 LP3 P3 P3 P3 Z3 LP3 P3 Z3 N3 LN3 P3 Z3 N3 N3 LN3 Z3 Z3 LN3 LN3 LN3 LN2 N2 Z2 P2 LP2 Fuzzy matrix fille with IF-THEN rules. value range in [ ] proucing instability, it is shown in Fig.9.a. 2) We use the value range in [ ] the en-effector cannot reach the esire object, it is shown in Fig. 9.b. 3) We select the value range in [ ] the robot can reach the target in 23 secons, however some unwante oscillations appear, it is shown in Fig.9.c. 4) We use the value range in [ ] the robot can reach the target in 23 secons, the the fuzzy controller generates a smooth trajectory, but it is slow, it is shown in Fig.9.. 5) We purpose the value range in [ ] the robot can reach the target in 18 secons, nevertheless there exist some oscillations, it is shown in Fig.9.e. 6) We choose the value range in [ ] the robot can reach the final position in 6 secons without oscillations. extene to other applications such as, mobile robots, 3D reconstruction, etc. REFERENCES [1] T.H. Cormen, Introuction to Algorithms, 2n Eition, MIT Press, [2] J. Denavit an R.S. Hartenberg, A Kinematic Notation for Lower- Pair Mechanisms Base on Matrices, Journal of Applie Mechanics, 1955, [3] B.Espiau, F.Chaumette an O.Rives, A new approach to visual servoing in robotics, IEEE Trans. on Robotics an Automation, Vol.8, No.3, , 1992 [4] E. Grosso, G. Metta, A. Oera an G. Sanini, Robust visual servoing in 3-D reaching tasks, IEEE Trans. on Robotics an Automation, Vol.12, No.5, , [5] K.Hashimoto an H.Kimura, Visual Servoing with Nonlinear Observer, Proc. IEEE Int. Conf. Robotics an Automation, , [6] R.M. Haralick an L.G. Shapiro, Computer an Robot Vision II, Aison-Wesley Pub. Co., [7] S.Hutchison, G.D.Hager an P.Corke, A tutorial on visual servo control, IEEE Trans. on Robotics an Automation,Vol.12,No.5, , [8] R.Kelly, Robust asymptotically stable visual servoing of planar robots, IEEE Trans. on Robotics an Automation, Vol.12, No.5, , [9] J.E.W. Mayhew an J.P Frisby, 3D Moel Recognition from Stereoscopic Cues, MIT Press, [10] S.M Seitz an C.R Dyer, Complete Scene Structure from Four Point Corresponences, Proc. 5th Int. Conf on Computer Vision, Cambrige MA, , [11] R. Szeliski, From Image to Moel (an beyon): A Personal Restrospective, Vision Interface 97, Kelowna, B.C, May 21, [12] C.J Taylor an D.J. Kriegman, Structure an Motion from Line Segments in Multiple Images, IEEE Trans. on Pattern Analysis, Machine Intelligence, 17(11), [13] E. Trucco an A. Verri, Introuctory Techniues for 3-D Computer Vision, Prentice Hall, 26-28, [14] L.X.Wang, Aaptive Fuzzy Systems an Control, Englewoo Cliffs NJ: Prentice-Hall, IV. CONCLUSION The main contributions of this paper are, 1) we evelope an implement a new optical stereo vision moel, 2) we successfully applie fuzzy visual servoing controller to control an inustrial robot. Our stereo moel can also be 3693