2-D Visual Servoing for MARCO

Transcription

1 2-D Visual Seroing for MARCO Teresa A. Vidal C. Institut de Robòtica i Informàtica Industrial Uniersitat Politèctica de Catalunya - CSIC Llorens i Artigas 4-6, Edifici U, 2a pl. Barcelona 82, Spain tidal@iri.upc.es IRI-DT-3-2. September 23. Abstract This report presents an image-based isual seroing for our mobile platform called MARCO. The kinematic model of MARCO has been obtained in this work. An implementation has been deeloped using camera calibration parameters. Simulations and experimental results are presented here. Introduction The use of isual information in a feedback loop is an attractie solution for the position and motion control of autonomous mobile robots eoling in dynamic enironments. Visual sero systems typically use one or two camera configurations: end-effector mounted (eye-in hand), or fixedintheworkspace. Existing eye-in hand approaches are typically classified in two different categories: position based and image based control systems. In a position-based control system, the input is computed in the three-dimensional (3-D) Cartesian space. The pose of the target with respect to the coordinate system of the camera is estimated from image features corresponding to the perspectie projection of the target in the image. In this approach knowledge of a perfect geometric model of the object is necessary. Contrary, for an image-based control system, the input is computed in a 2-D image space (2-D Visual Seroing) [2]. The problem with 2-D isual seroing is in terms of image regulation, without an explicit 3-D reconstruction of the scene. A clear disadantage of this kind of control system is that conergence is theoretically ensured only in a small region around the desired position. To oercome such difficulty a more recent approach has been deeloped [5], the 2-/2-D isual seroing. This control system is based on the estimation of the camera displacement (the rotation and the scaled translation of the camera) between the current and desired iews of an object. The work presented in this report is based on 2-D isual seroing for a wheeled mobile robot with an on-board camera mounted on a pan and tilt neck. The kinematic screw

2 between the camera and the scene is obtained by an image control law. Howeer, it is still necessary to obtain the kinematic model of the robot and head in order to compute the linear and angular elocities of the mobile platform and the angular elocities of the pan and tilt. This technical report is organized as follows, section two presents the kinematics of the complete mobile platform MARCO. In section three the perspectie projection and way to model the image motion is deried. The feedback control law is presented in section four. Some simulations are shown in section fie. In section six the technical features of the robot are described, along with a brief description of the application deeloped for the experiments results and conclusions are also presented. 2 Kinematic model of MARCO Our mobile platform MARCO can be schematically represented, from a mechanical point of iew, as a kinematic chain of rigid bodies (links) connected by joints. One end of the chain is constrained to the base, while an end effector (in this case the camera) is mounted to the other end. The resulting motion of the structures is obtained by composition of the elementary motions of each link with respect to the preious one. Using Denait-Hartenberg conention it is easy to deduce the direct kinematics function of the complete chain. The construction of the direct kinematics is deried from the position and orientation change of the frames attached to the link (see Figure.). The change of coordinates of the points in the camera coordinate frame with respect to the world coordinate frame is MW c = MW r Mr pan MpanM tilt tilt. c We next describe each of these homogenous transformation matrix. 2. Robot Coordinate Frame (S r ) - World Coordinate Frame (S w ) The homogenous transformation matrix (HTM) for the Robot Coordinate Frame to the World Frame is computed by, cos θ sin θ X r M r sin θ W = cos θ Y r r where X r and Y r are the position of the center of the robot axle with respect to the World Coordinate Frame, and θ is the angle between X r and X W about axis Z W. 2.2 Pan Coordinate Frame (S pan ) - Robot Coordinate Frame (S r ) The HTM from the Pan to the Robot is, 2

3 C ω 2 Y pt Z pt C S c C S p,t X pt Z W Z r ω Y r Yw S r S W X r Xw Figure : Mobile Robot MARCO and its Frame Coordinates. M pan r = cos θ 2 sin θ 2 DX sin θ 2 cosθ 2 h where DX and h are shown in 2. The pan angle is θ Tilt Coordinate Frame (S pan )- Pan Coordinate Frame (S tilt ) where θ 3 is the tilt angle. M tilt pan = cos θ 3 sinθ 3 sin θ 3 cos θ Tilt Coordinate Frame (S tilt )-Camera Coordinate Frame (S C ) It is necessary to find the HTM from the camera to the tilt Mtilt c to obtain the relationship between camera and the World Frame. 3

4 <DX=-> <h=8> <h=35> <a=7> Figure 2: 4

5 b Mtilt c a = h b,a, and h are the coordinates of the S c center of projection in the camera frame (see 2). These parameters can be obtained by calibration of the neck-eye system. 2.5 Camera Coordinate Frame (S C ) - Fixed Coordinate Frame (S w ) The full Transformation Matrix between the Camera Frame and the World Coordinate System is, sin (θ + θ 2 ) sin θ 3 cos (θ + θ 2 )cosθ 3 cos (θ + θ 2 ) MW c cos (θ = + θ 2 ) sin θ 3 sin (θ + θ 2 )cosθ 3 sin (θ + θ 2 ) cos θ 3 sin θ 3 X r + DX cos θ a sin (θ + θ 2 )+b cos θ 3 cos (θ + θ 2 )+h sin θ 3 cos (θ + θ 2 ) Y r + DX sin θ + a cos (θ + θ 2 )+b cos θ 3 sin (θ + θ 2 )+h sin θ 3 sin (θ + θ 2 ) h + r b sin θ 3 + h cos θ Mobile Robot Kinematics Most wheeled mobile robots hae the non-holonomic property that the number of degrees of freedom of the input, linear and angular ω elocities, is less than that of the configuration (position (X r,y r ) and orientation θ ). This relationship can be expressed as 2.7 Camera Velocity Ẋ r Ẏ r = θ cos θ µ sin θ ω The origin of the Camera Frame is located at point Oc and the origin of the Robot Frame is located at Or Now its elocity can be decomposed in rotational and translational. The following camera elocity is expressed in terms of the robot frame. () V Oc = V Or + Ω Sr/S W OrOc + V Oc/Sr Ω Sc/S W = Ω Sr/S W + Ω Sc/S r 5

6 2.8 Relationship between Kinematic Matrix and Robot The pan and tilt mount ector elocity in terms of the World Frame S W is µ T tilt W V Stilt /S W = t with T tilt W = T tilt W t X r + DX cos θ Y r + DX sin θ r + h Ẋ r θ DX sin θ = Ẏ r + θ DX cos θ (2) Substituting the equation in 9, we obtain cos θ DX sin θ sin θ DX cos θ ω V Stilt /S W = {z } ω 2 J Let Ω Stilt /S W be the rotation ector of the pan and tilt mount with respect to the reference frame S W, that is obtained as Ω Stilt /S W = + θ + θ 2 θ 3 = Rewriting the last equation in terms of the robot inputs, ω Ω Stilt /S W = {z } ω 2 J Ω θ 3 θ + θ 2 ThecoordinateframesS c and S tilt are linked rigidly, so we can apply the kinematics equation of the rigid body: V c/sw = V Stilt /S W + OcOt Ω Stilt /S W where OcOt is the distance between the origin of the Tilt Frame and the origin of the Camera Frame referred to the World Coordinate System. The angular elocity is, Ω c/sw = Ω Sc/S W = Ω Stilt /S W 6

7 The translation between the camera and the tilt is Ttilt c, changing the reference to the World Frame results OcOt = RW tilt Ttilt c then Defining the matrix, where V c/sw = J ω ω 2 Rtilt W T c tilt J Ω a 3 a 2 = a 3 a a 2 a ω ω 2. (3) a = a sin (θ + θ 2 )+b cos θ 3 cos (θ + θ 2 )+h sin θ 3 cos (θ + θ 2 ) a 2 = a cos (θ + θ 2 )+b cos θ 3 sin (θ + θ 2 )+h sin θ 3 sin (θ + θ 2 ) a 3 = b sin θ 3 + h cos θ 3 and Ttilt c = T a a 2 a 3,wecanrewrite3as R tilt m Ttilt c ω J Ω ω 2 = J ω Ω ω 2 Because of 4 the Jacobian can be obtained as follows µ (J J J = Ω ), or cos θ a 2 DX sin θ a 2 a 3 sin θ a + DX cos θ a a J = The relationship between the robot and camera elocities (expressed in the World Coordinate Frame) is shown by the following expression. V x V y V z ω x ω y ω z J Ω ω = J ω 2 7 (4)

8 Changing the last expression in terms of the camera frame, we hae Ω (Sc) S c/s W V (Sc) c/s W = Ω (Sc) S tilt /S W =(R c tilt ) R tilt (S W V W ) =(R c tilt ) R tilt W c/s W Ω (S W ) S tilt /S W Extending the last two equations: V (S c) c/s W =(R c tilt ) R tilt W J ω ω 2 Rtilt W Ttilt c J ω Ω ω 2 Ω (S c) S c/s W =(R tilt c ) RW tilt ω J Ω ω 2 the camera elocity in the camera reference frame stays as follow, T (Sc) c = Ã V (S c) c/s W Ω (Sc) S c/s W = J (Sc) ω ω 2! where and (R c W ) = Ã J (Sc) = (R tilt c ) Rm tilt! (J J Ω ) J Ω (R c tilt ) R tilt m (cos θ 3 )cos(θ + θ 2 )(cosθ 3 )sin(θ + θ 2 ) sin θ 3 sin (θ + θ 2 ) cos(θ + θ 2 ) (sin θ 3 )cos(θ + θ 2 )(sinθ 3 )sin(θ + θ 2 ) cosθ 3 sin θ 3 cos (θ + θ 2 ) sinθ 3 sin (θ + θ 2 ) cosθ 3 cos θ 3 cos (θ + θ 2 ) cos θ 3 sin (θ + θ 2 )sinθ 3 sin (θ + θ 2 ) cos (θ + θ 2 ) Computing the Jacobian of our robot in terms of the camera reference frame, we hae: J (S c) = J J 2 J 3 J 4 8

9 J = J 2 = J 3 = J 4 = cos θ 2 sin θ 3 cos θ 2 cos θ 3 sin θ 2 a sin θ 3 + DX sin θ 2 sin θ 3 a cos θ 3 DX cos θ 3 sin θ 2 DX cos θ 2 b cos θ 3 h sin θ 3 cos θ 3 sin θ 3 a sin θ 3 a cos θ 3 b cos θ 3 h sin θ 3 cos θ 3 sin θ 3 b cos (θ + θ 2 )+a cos θ 3 sin (θ + θ 2 ) h cos (θ + θ 2 )+a sin θ 3 sin (θ + θ 2 ) b sin θ 3 sin (θ + θ 2 )+h cos θ 3 sin (θ + θ 2 ) sin θ 3 sin (θ + θ 2 ) cos θ 3 sin (θ + θ 2 ) cos (θ + θ 2 ) The Jacobian of the robot is the relationship between camera elocities and robot elocities. 3 Camera Kinematics The camera model used is the full perspectie pin-hole model. The corresponding transformation from camera frame to the image plane is through perspectie projection Let us consider a camera with its related frame S c moing with respect to a reference frame S W as in Figure 3. The camera interacts with a part of the enironment with an associated frame at the image plane. The coordinates of a point on the image plane are u u α u α = x z = y z 9

10 P(x,y,z) u P(u,) Z Optical Axis c(u, ) S c Image Plane Camera Figure 3: where α u,α,u are intrinsic parameters of the camera. These parameters are obtained with camera calibration [6]. Image-based isual seroing is based on the selection in the image of a set s of isual features that has to reach a desired alue s. Usually, s is composed of the image coordinates of seeral points belonging to the considered target. It is well known that the image Jacobian L T s plays a crucial role in the design of the possible control laws. This Jacobian relates the isual features elocity with the camera elocity [2]. ṡ = L T s T (Rc) c (5) where: - ṡ is the isual features elocity ector or the apparent motion between image pixels dim(m ); - L T s is the Image Jacobian or the interaction matrix dim(m 6) and has the information of the isual features s; - T c (Sc) is the the elocity ector of dim(6 ) and represents the camera motion with respect to the scene. The elements T c = T V c/sr Ω Sc/Sr are the translation and rotation elocities respectiely, expressed in camera frame. Using a classical perspectie projection model with unit focal length, and if U and V coordinates of image points are selected in s, two successie rows of L T s are gien by: µ U V The depth z is used as z = z d. µ = U UV ( + U 2 ) V z z V ( + V 2 ) UV U z z T (Rc) c

11 S*(t) Image Control T c =-λl (s-s*) T c Robot + - s(t) z=zd Extraction of Image Features Camera Figure 4: Block Diagram of the 2-D Visual Seroing 4 Image- Based Visual Seroing Defining an error function as the difference between the current and desired images of the feature points e = s s, the control law is defined as T c (Sc) = λ L T s e where s is the desired position of the isual features. In our particular case is the position of each point of the pattern. The closed loop system stays in the following form, ṡ = L T s T(Sc) c = λl T s L T s (s s ) then if L T s L T s >k s s k the system conerges to the desired alues. Knowing T c (Sc), it is possible to obtain the robot elocities, as described in the robot kinematics section, with ω ω 2 = J T (S c) c. Considering the asthepseudo-ineerse.thesamedimensionasa T so that AXA = A, XAX = X and AX and XA are Hermitian.

12 Figure 5: Image Space Trajectory. In our case the interaction matrix has the following form, U z U z V ( + U 2) V V ( + V z z 2) U V U U2 U z z 2 V 2 ( + U2 2 ) V 2 L T s = V 2 ( + V 2 z z 2 ) U 2 V 2 U 2 U z 3 U z 3 V 3 ( + U3 2 ) V 3 V 3 ( + V 2 z z 3 ) U 3V 3 U 3 U z 4 U z 4 V 4 ( + U4 2) V 4 V 4 ( + V 2 z z 4 ) U 4 V 4 U 4 Figure 4 shows a block diagram of the 2-D isual seroing approach. 5 Simulations The simulations presented here were deeloped in SIMULINK and the parameters are coincident with the real system. Figures 5, 6, 7 and 8 show some of the results obtained when applying image -based isual seroing to a robot like ours. It is desirable not to saturate the actuators een in simulations, in Figure 7 it is shown how the elocities of the robot are reasonable for the real robot. Figure 8 shows the error tending exponentially to zero. 6 Implementations and Experiments A 2-D Visual Seroing implementation has been deeloped for the mobile robot MARCO. The application was created in Visual C++ using the Matrox Imaging Library for the segmentation, features extraction and image interpretation. Acquisition is made with a 2

13 Figure 6: Workspace Trajectory..4.3 w w2 w Figure 7: Robot Velocities. 3

14 Figure 8: Errors (s-s*). PXC2 frame grabber. The camera used is a SONY EVI-37DG with standard format PAL RGB. The robot MARCO has a mobile platform PIONEER 2.. Image processing blocks are shown in Figure 9. Saphira is the natie operating software of the PIONEER Mobile Robots. Saphira can be thought of as haing two architectures, one built on top of the other. The system architecture is an integrated set of routines for communicating and controlling the robot from a host computer. On top of the system routines is a robot control architecture, that is a design for controlling the mobile robot that addresses many of the problems inoled in naigation, from low-leel issues such as planning and object recognition [9]. ThePXC2acquiresanimageanditisstoredinmemoryaddress,afterthatitisconerted to a MIL image in order to be processed. The sequence followed is shown in Figure 9. - Binarization (threshold ); - Filtering (noise remoal); - Region Segmentation (blobs) by the area and compacity; - Getting blobs; - Extraction of the center of each blob; - Ordering of blobs. In Figure a segmented image is shown, with the corresponding blobs found and ordered. The Visual C++ application shows at the screen the linear and angular instant elocities of the robot, the image space errors, the coordinates in millimeters of each center blobs, the frames per second of a refresh image and the segmented image as Figure shows. The sample time is about 6 ms, in consequence it is difficult to stabilize the system. Sometimes the pattern gets out of the image and it becomes impossible to recoer control, then a null command of elocities is sent to the robot. As it is mentioned before for this implementation, the depth is not calculated, instead we used the desired depth. A disadantage of this image-based isual seroing is the impossible trajectories for the 4

15 -BW Adquisition -Binarizing -Filtering Region Segmentation Features Extraction -area -compacity Interpretation -blob position -center Figure 9: Image Procesing Block Diagram. Figure : Segmented Image. 5

16 Figure : 2D Visual Seroing Aplication. 6

17 robot that the control law can compute. That is the reason why the 2 /2 Visual Seroing was deeloped, but it becomes necessary to compute the depth. References [] Cadenat V., R. Swain., P. Spuères, M. Dey (999), A Controller to Perform a Visually Guided Tracking Task in Cluttered Eniroment.Proc. of the IEEE/RSJ Intern. Conference on Intelligent Robots and Systems, pp [2] Chaumette F., Ries P., Espiau B. The Task Function Approach Applied to Vision- Based Control. Proc. of the IEEE International Conference on Robotics & Automation (ICRA), pp [3] Swain-Oropeza R. Contrôle de tâches référencées ision pour la naigation d un robot mobile en milieu structuré. L Institut National Polytechnique de Toulouse PhD Thesis,, 999. [4] Hutchinson Seth, Hager G. D., Corke P.I. A tutorial on Visual Sero Control. IEEE Transactions on Robotics and Automation, Vol. 2, No 5, October 996, pp [5] Malis, E.. Contributions à la modélisation et à la commande en asserissement isuel. Uniersité de Rennes I PhD Thesis [6] Andrade, J. Camera Calibration. Technical Report IRI- DT 2/2, Juny 2. [7] Sciaicco L., Siciliano B. Modelling and Control Robot Manipulators. Chapters on Kinematics and Differential Kinematics and Statitcs.Springer-Verlag 2. [8] Zhang H., Ostrwski J.P. Visual Motion Planning for Mobile Robots. IEEE Transactions on Robotics and Automation, Vol. 8, No2, April 22. [9] Konolige K. SAPHIRA Sofware Manual. Version