Object. frame. Sensor. C(t) frame. y Mo. Mp t. Sensor frame at equilibrium (Absolute frame)

Transcription

1 Position base visual servoing using a non-linear approach Philippe Martinet an Jean Gallice LSME, Université Blaise Pascal, UMR 662 u CNRS, 6377 ubi ere cee, France martinet@lasmea.univ-bpclermont.fr bstract Vision base control has retaine attention of many authors uring the last few years. We first have been intereste in Image Base Visual Servoing approach an recentlywehavefocuse our attention in Position Base Visual Servoing approach. In this paper, our goal is to stuy how we can introuce 3D visual features in a close robot control loop. We consier a camera mounte on the en effector of the manipulator robot to estimate the pose of the target object. The require positioning task is to reach a specific pose between the sensor frame an a target object frame. Knowing the target object moel, we can localize the object in the 3D visual sensor frame an estimate the pose between the camera an the target object at each iteration. To perform the visual servoing task, we use a non-linear state feeback. We propose a new eact moel for parametrization of the pose (position an the orientation of the frame object in the sensor frame). The main avantage of this approach is that camera translation an camera rotation are separately controlle ue to use of a particular choice of frames. Convergence an stability have been prove theoretically, an the tests in simulation an on our eperimental site show goo behaviour using this type of approach. Introuction Sanerson an Weiss in [5] introuce an important classification of visual servo structures base on two criteria: space of control an presence of joint feeback. So in this classification we istinguish two main approaches: ffl Position Base Control : image features are etracte from the image an a moel of the scene an the target is use to etermine the pose of the target with respect to the frame attache to the camera. ffl Image Base Control : in image base control, pose estimation is omitte, an the control law is irectly epresse in the sensor space (image space). bibliographical review on Visual Servoing has been compile in etail by P. Corke in[4]. tutorial on Visual Servo Control was organise by G. Hager et al [8] in 996. These surveys of the state of the art in the fiel of visual servoing show that Image Base Control has been retaine as an alternative to the Position Base Control approach. Generally, many authors consier that the Image Base Control approach is the better of the two, accoring to the criteria of camera calibration, han-eye calibration, robot moelling,scene an target moelling,an also regaring the processing time require to compute the sensor signal. It is clear that the Image Base Control approach oes not nee precise calibration an moelling, because of the close loop efine in the sensor space. Much work,e.g. [,2,7,9,,2], has been one on the camera sensor an the 2D space. The notion of Task Function introuce by Samson et al in [4],can be use to elaborate a control law in the sensor space. ccoring to this concept,martinet et al in [] introuce the notion of a 3D visual sensor which elivers a 3D sensor signal by monocular vision at vieo rate. Recent progress in pose estimation, localization an 3D moelling [5, 6] shows that it is not unrealistic to introuce 3D visual information into a close loop control. Using this assumption, we can synthesize control laws using this kin of information as we o irectly with the camera sensor. nother way to obtain the estimation of the pose is to compute a Kalman filter using several visual features, as escribe in [6]. In this approach,the control law is efine to reach a particular pose between sensor an object frames with a PD controller. In fact, little work has been one using a 3D sensor signal. However, we can remark that precise calibration an moelling are really useful only where

2 the task to be achieve is epresse in Cartesian space. If the 3D reference signal is learne by means of the 3D sensory apparatus an pose estimation algorithm (use in real conitions), as in the Image Base pproach, we obtain the same goo results in the 3D sensor space. In the first part of this paper,we evelop the moel of the close loop system using a non-linear state feeback, an analyse the problem of stability an convergence. In the secon part, we present results obtaine with our eperimental robotic platform. We use a specific object compose of four illuminate points in real eperimentation. In conclusion, we present some prospects for evelopments in visual servoing. is the homogeneous transformation matri between an absolute frame attache to the scene, an the sensor frame compute at each iteration R c t ffl M is the homogeneous transformation matri between the sensor frame compute at each iteration R c t,an the object frame R o ffl M p t Without loss of generality, we can consier that the absolute frame is equivalent (R = R Λ c) to the sensor frame at the equilibrium situation (see figure ). The consequences are a simplifie representation of the state moel. If we measure the pose of the object M p t in the sensor frame by monocular vision at each iteration,we can euce the matri M by the following relation: 2 Moelling the system with the state space formalism 2. Principle M = M o :M pt () The task to be performe is a positioning task (Pose control) of the en effector frame (sensor frame when the sensor is embee) relative to the absolute frame attache to the scene. z Object frame O y Mo z Mp t y Sensor frame at equilibrium (bsolute frame) Sensor frame y M V C(t) Ω z Figure : Different frames use in moelling In the following, we consier a scene with a 3D object an a wrist 3D sensory apparatus mounte on the en effector of the robot. We efine three homogeneous transformation matrices as follows: is the homogeneous transformation matri between an absolute frame R attache to the scene,an the object frame R o ffl M o The pose parameters of the sensor frame in the absolute frame can be epresse as a rigi transformation matri M as follows: R M = 3 (2) where R represents the orientation part of the pose, an the position part. We use the eponential representation for the epression of the rotation. So we write: R = ep(s( (t))) (3) where S( (t)) is the antisymmetric matri associate with the orientation vector (t): (t) =k (t)k:u(t) (4) (k (t)k is the norm of the vector (t) an u(t) isthe associate unitary vector). Deriving relation 2,we obtain: t R t (5) 3 t M = an after eveloping,we have: an t =[V (C)] = R:V (6) t R = R:S(Ω) (7) with the following notations:

3 ffl V the translation velocity of the camera epresse in the sensor frame R c t ffl S(Ω) the antisymmetric matri associate with the rotation velocity Ω of the camera epresse in the sensor frame R c t In orer to transform relations 6 an 7 using the state space formalism, we have to efine the state vector X. We choose a 6-imension vector X = ( T ;y T ) T ( T represents the tranpose of ),where ffl is the position of the sensor frame R c t epresse in the absolute frame R ffl y is a function of the orientation of the sensor frame R c t relative to the absolute frame R We now have to introuce the efinition of the variable y. Developing the eponential representation (Eq. 3) of R with the Rorigues formulae [3],we can shown that: R T R =2:sin(k (t)k):s(u(t)) (8) an efine the vector y as: S(y) = 2 (RT R) =sin(k (t)k):s(u(t)) (9) Then,using Eq. 4 we obtain: y = sin(k (t)k) : (t) () k (t)k Using this representation,we can epress the state equation of the system: where t X = B(X) :U () ffl U = (V T ; Ω T ) T represents the kinematic screw (the control vector) ffl B(X) = R O 3 O 3 with = (trace(r):i 2 3 R T ) O 3 I 3 The state equation of the system is then linear with regar to the control vector U, an non-linear with regar to the state vector. Controllability ofthe system is obtaine if the control matri B(X) is full rank. In our case, this conition is always realize ecept in the singular case k k = ß + kß. 2 In the conitions use in all theoretical evelopment ( k k < ß ), the inverse 2 of the control matri B(X) can be compute,an its epression is the following: B (X) = R T O 3 O 3 (2) Developing an using the matri inversion lemma, we obtain the epression of the matri efine by: ( k k )u:ut cos( 2 ) cos(k k) 2.2 Stability analysis k k cos( 2 ) :R T ( 2 : (t)) We now procee to analyse the stability of the control law an then iscuss the problems which may be encountere when using a Pose estimation algorithm from image features. To control the system,we choose a non-linear state feeback which linearizes the close loop system. In this case,we have: ρ U = B (X):K:X t X = K:X (3) In these conitions, to stabilize the system it is sufficient to choose the control gain matri K as a iagonal matri with positive values. The close loop system behaves as a set of ecouple integrators,an each component of the state vector has an eponential ecrease. To estimate the pose parameters for 3D objects by monocular vision,many methos are propose in the litterature. Some methos give close form solutions of the inverse perspective problem aresse, others use iterative processes to reach the solution. The problem of unicity for the solution is often omitte, an the authors use spatio-temporal filters to etract the right solution. t the present time,it is clear that we are not able to emonstrate that pose measurement is stable an always converges towars the right solution. However,some authors have aresse this kin of problem an some results are known. We

4 fin similar problems when using the Image Base pproach as presente by F. Chaumette in [3]. In our application we use the DeMenthon algorithm [6] an we choose the best matching using a spatiotemporal filter. So far,we have not encountere any problems,but this is not a theoretical proof. 3 Eperimental results To valiate this approach,we wrote a simulator in Matlab an use our eperimental robotic platform. In this paper, we present onlya small sample of our eperimental results. We use a regular tetraheron object in the form of four les as shown in figure 2 P P3 Ra P Xo o Ro P2 Yo 3D Object Zo 3D Sensor c Virtual link Zc Yc Rct Figure 2: Object an sensor in the scene. The low level image processing consists of a simple barycenter computation for each projection of the les. From these four etecte points,the DeMenthon algorithm [6] is use to locate the moelle object an thus the control law can be compute. The sampling perio is twice the image acquisition time,i.e. 8 ms. We use a Cartesian robot with si egrees of freeom, the camera being embee on the en effector (see Figure 3). ll software was written in C language an we use the VWorks real time system environment. We measure the pose of the object M p t in the sensor frame by monocular vision at each iteration, an we euce the matri M (Pose of the sensor in the absolute frame) with equation Eq. (). t the equilibrium situation, the object pose in the sensor frame is represente by the matri M o,an is efine by the programmer using the Cartesian position an the roll,pitch an yaw angles. We have use various initial an final situations between the sensor frame an the object frame an a iagonal control gain Xc Figure 3: Overview of the robotic platform matri K =:5:I 6. For this paper,we choose to move back an forwar accoring to the following conitions: Initial situations (m, ffi ) Final situations (m, ffi ) (; ; :5; ; ; ) (; ; :2; 5; 5; 3) (; ; :2; 5; 5; 3) (; ; :5; ; ; ) For both eperiments, in the initial situation the 3D sensor give us the following components ( T () ;yt ()) of the state vector: T (m) () yt () (:73; :49; :733) (:75; :33; :454) (:33; :35; :69) (:78; :36; :453) Figure 4 shows the sensor trajectory in Cartesian space uring the first eperiment. The trajectory is obtaine by reconstruction using the joint measurement ateach iteration Z ais (m) Y ais (m) Sensor frame trajectory (,y,z view) Nonlinear feeback - K iagonal Rectilinear trajectory X ais (m) Figure 4: Sensor frame trajectory (scale in m) The positioning task is well performe, an we can observe that the trajectory of the camera in

5 the reference frame R Λ c approimates to a straight line. This result is ue to splitting between the control of position an the control of orientation, which represents one of the main avantages of this control law Position (t) (m) Orientation y(t) Position (t) (m) Orientation y(t) Figure 8: Position an Orientation y Translation velocities (mm/s) 5 Rotation velocities (egre/s) Figure 5: Position an Orientation y Translation velocities (mm/s) 2 5 Rotation velocities (egre/s) Figure 9: Control vector (V ; Ω) Figure 6: Control vector (V ; Ω) In Figure 5, the position an orientation of the object are presente. The behavior is like a first orer system. We obtain eponential ecay of the sensor signals an convergence at aroun iterations. Figure 6 represents the evolution of the control vector. notice a little isturbance on the curves, ue to the sensitivity of the 3D sensor. This isturbance is ue to the ifficulty of etracting the Z position an the orientation y of the sensor frame in a ynamic sequence, an the sensitivity is greater when the eperiment involves forwar motion. To improve the results of the control laws we think that one solution is to use a filter on the pose parameters, as Wilson proposes in [6]. In the secon eperiment, from the final position obtaine in the first eperiment we return to the first initial position. Figure 7 shows the corresponing sensor trajectory in Cartesian space Z ais (m) Y ais (m) Sensor frame trajectory (,y,z view) Nonlinear feeback - K iagonal Rectilinear trajectory X ais (m) Figure 7: Sensor frame trajectory Figures 8 an 9 show the eponential ecay of the sensor signal an control signal respectively. We 4 Conclusion Many people are intereste in visual servoing. Up to now,image Base visual servoing has principally been consiere. In this paper, we show thata 3D visual sensor etracting 3D features at vieo rate can be use (twice vieo rate in our implementation). We have evelope theoretically an original moel for the Pose parameters which simplifies the control synthesis. We have shown the controllability an stability of this control law, an foun the optimal control law which allows the sensor frame to follow a straight line uring servoing. Results seem to be satisfactory in regar to primitive etraction (image le barycenter computation) an pose estimation (DeMenthon algorithm) algorithms. The istinguishing characteristic of this kin of metho appears in the simplicity of the formalism. The control law epens only on the esire an

6 current situations of the observe object. Then,from one application to another, only the pose estimation algorithm has to be moifie. We are now stuying waysto introuce a constraint into the control law to be sure that the object is always in the camera fiel uring servoing. Further evelopment shoul focus on the evaluation of the robustness of the control law with regar to noise estimation in pose etraction, moelling errors, an particularly han-eye calibration error. References [] P. K. llen an. Timcenko an B. Yoshimi an P. Michelman "utomate tracking an grasping of a moving object with a han-eye system", IEEE Transactions on Robotics an utomation, Vol. 9(2),pp ,993. [2] Espiau B., F. Chaumette, P Rives, " new approach to visual servoing in robotics", IEEE Transactions on Robotics an utomation, vol. 8(3),pp ,June 992. [3] F. Chaumette,"Potentiel problems of stability an convergence in image base an position-base visual servoing", The confluence of Vision an Control,LNCIS series,springer Verlag,998. [4] Corke P.,"Visual control of robot manipulators - review", in "Visual Servoing", Hashimoto K., Worl Scientific,pp. -3,993. [5] S. Christy an R. Horau,"Iterative pose computation from lines corresponences", to appear in Computer Vision an Image Unerstaning, caemic Press INC., 998 [6] Dementhon D.F., L.S. Davis, "Moel-Base Object Pose in 25 Lines of Coe", International Journal of Computer Vision, vol. 5(-2),pp. 23-4,June 995. [7] Feema J.T. an O.R. Mitchell, "Visionguie servoing with feature-base trajectory generation", IEEE Transactions on Robotics an utomation, vol. 5(5),pp. 69-7,October 989. [8] Hager G.D., S. Hutchinson, P. Corke, "Tutorial on Visual Servo Control", IEEE International Conference on Robotics an utomation,minneapolis,minnesota,us,22-28 pril,996. [9] Kharaoui D., G. Motyl, P. Martinet, J. Gallice, F. Chaumette, "Visual Servoing in Robotics Scheme Using a Camera/Laser-Stripe Sensor", IEEE Transactions on Robotics an utomation, vol. 2(5),pp ,October 996. [] Martinet P., F. Berry, J. Gallice. "Use of first erivative of geometric features in Visual Servoing", Proceeings of the IEEE International Conference on Robotics an utomation,minneapolis,minnesota,us,vol.4, pp ,pril 996. [] Martinet P., D. Kharaoui, J. Gallice. "Vision Base Control Law using 3D Visual Features", Worl utomation Congress,Montpellier,France, Vol. 3,pp ,May 996. [2] Papanikolopoulos N., P.K. Khosla, T. Kanae, "Visual tracking of a moving target by a camera mounte on a robot: combination of control an vision", IEEE Transactions on Robotics an utomation, vol. 9(),pp. 4-35,February 993. [3] Rorigues O. "Des lois géométriques qui régissent les éplacements 'un syst eme solie ans l'espace, et e la variation es cooronnées provenant e ces éplacements consiérés inépenamment es causes qui peuvent les prouire", Journal e Mathématiques pures et appliquées, Tome 5, pp.38-44,84. [4] Samson C., M. Le Borgne, B. Espiau. "Robot Control : The Task Function pproach", Ofor University Press,99. [5] Sanerson.C.,L.E. Weiss. "Image-base visual servo control using relational graph error signals", Proceeings of the IEEE International Conference on Robotics an utomation,pp ,98. [6] Wilson W.J., C. C. Williams Hulls, G.S. Bell. "Relative En-Effector Control Using Cartesian Position Base Visual Servoing", IEEE Transactions on Robotics an utomation, vol. 2(5),pp ,October 996.