Image-based Visual Servoing for Nonholonomic Mobile Robots with Central Catadioptric Camera

Transcription

1 Image-base Visual Servoing for Nonholonomic Mobile Robots with Central Cataioptric Camera Gian Luca Mariottini, Domenico rattichizzo Dipartimento i Ingegneria ell Informazione Università i Siena Via Roma, Siena, Ital {gmariottini,prattichizzo}@ii.unisi.it Giuseppe Oriolo Dipartimento i Informatica e Sistemistica Università i Roma La Sapienza Via Euossiana 8, 8 Roma, Ital oriolo@is.uniroma.it Abstract We present an image-base visual servoing strateg for nonholonomic mobile robot equippe with a central cataioptric camera. This kin of vision sensor combines lens an mirrors to enlarge the fiel of view. The propose approach, which eploits the epipolar geometr efine b the current an the esire camera views, oes not nee an knowlege of the -D scene geometr. The control scheme is ivie in two steps. In the first one, the epipoles are use use together with an approimate input-output linearizing feeback to align the robot with the goal. Feature points are then use in the secon translation step to reach the esire. Global asmptotic convergence is proven. Simulation an eperimental results show the effectiveness of the propose control scheme. I. INTRODUCTION This paper presents an image-base visual servoing (IBVS) strateg for riving a nonholonomic mobile robot to a esire (set-point), which is specifie onl through a esire image previousl acquire b an on-boar omniirectional camera. In IBVS the control law is irectl esigne in the image omain, oes not nee an apriorknowlege of the -D structure of the observe scene an is robust with respect to moel uncertainties an isturbances in both camera an robot moels,. Recentl, there has been an increasing interest in the visual control of mobile robots subject to nonholonomic kinematic constraints. A first stu of this kin can be foun in where the authors propose to use a pan-tilt camera, thus aing more egrees of freeom to the vision sensor, to control a mobile robot with the task function approach. In, a piecewise-smooth visual servoing for mobile robots is presente. In both cases, however, a metrical knowlege of the observe scene is neee to guarantee the convergence to the esire, thus limiting the applicabilit to a real contet. However, the previous strategies o not consier the visibilit constraint, i.e., the problem of keeping the relevant features in the camera fiel of view (FOV). In orer to solve this problem some strategies have been investigate, e.g., base on zoom ajustment or switching control. All these strategies are specificall esigne for robotic manipulators without nonholonomic constraints an cannot be easil generalize to take into account nonholonom. A completel ifferent approach to the visibilit constraint problem consists in using panoramic FOV provie b omniirectional cameras, that naturall overcome the visibilit constraint. We will refer to these sensors as cataioptric cameras, because the consist of a coupling between mirror (catoptric element) an conventional cameras with lenses (ioptric element). Recentl, applications of visual servoing using cataioptric cameras are growing in interest. A -D visual servoing with central cataioptric camera observing feature points has been stuie in an etene to the case of observe straight lines in. In is presente a visual servoing strateg for mobile robots equippe with central cataioptric cameras in which is necessar an estimation of the feature height to the plane of motion. All these approaches suffer from the same potential rawback, i.e., the control law is base on the inverse of the image Jacobian an can then became singular for certain s of the mobile robot or of the image feature position. In orer to overcome these problems, we propose an IBVS strateg for set-point global asmptotic stabilization of nonholonomic mobile robots to a target position. The robot is equippe with a central cataioptric camera. We onl assume to know both the image acquire at the target position (esire) an the image at the current position (current) (see Fig. ). The visual servoing uses the epipolar geometr eisting between the current an the esire image 9. This work is base on our previous contributions 7, 8 that are etene to central cataioptric cameras. Our control algorithm consists of two sequential steps. The first compensates the orientation error so as to align the robot to its target an is base on the use of an approimate input-output linearization with the sstem outputs epening on the epipole values. The secon step leas the sstem to the target zeroing the translational isplacement using the istance between corresponing image points. The resulting image-base visual servoing strateg guarantees global asmptotic convergence with eponential rate of the nonholonomic mobile robot to the esire. The main avantages of our approach are here outline: Using the epipoles, we are able to avoi the problems arising from local minima an singularities that arise

2 when using the image Jacobian. Differentl from, we o not estimate an relative camera isplacement, but irectl eploit the kinematics of epipoles in the image plane to esign a global asmptoticall stable control law. No metrical knowlege of the -D scene geometr is necessar, because epipoles can be compute from corresponing feature points in the current an esire view 9,. The visibilit constraint is automaticall satisfie b the aoption of a central cataioptric camera as a vision sensor. The paper is organize as follows. Section II introuces the basics of central cataioptric cameras an their associate epipolar geometr. In Section III, the nonholonomic visual servoing problem is formulate an the -step control strateg is outline. The first step is analze in Section IV, whenever Section V escribes the feature-base control law which implements the secon step. Simulation an eperimental results are presente in Section VI an Section VII, to show the effectiveness of the propose approach. In Section VIII, we provie some concluing remarks highlighting the main contributions of the paper. II. BASIC EIOLAR GEOMETRY FOR CENTRAL CATADIOTRIC CAMERAS Several tpes of cataioptric cameras satisf the single viewpoint constraint, i.e., the whole vision sensor onl measures the light through a single point. This is necessar for the eistence of epipolar geometr an for the generation of geometricall correct images 8. In this case the are referre to as central cataioptric cameras, but we will hereafter refer to them onl as cataioptric cameras, for short. Consier the case in Fig. in which a parabolic mirror is centere at the focus O an an orthographic camera is in front of it. Ever scene point IR is projecte onto the mirror surface at X IR through O. The image point p (piels) is obtaine via orthographic projection of X. Note that a function η : IR IR, epenent on known camera calibration parameters an mirror geometr, can be efine as a map from a mirror point X to its projection p onto the image plane, namel η(x) =p. More etails can be foun in 7. Consier now two panoramic views as in Fig., acquire b the same camera place in O an O, an referre to as current p X O m m z m image plane parabolic mirror Fig.. Imaging moel of a cataioptric camera (e.g. orthographic camera couple with parabolic mirror). π X e ; X ; e e O O ; e ; C C : ; esire view R,t current view Fig.. Basic epipolar geometr setup for cataioptric cameras. Ever epipolar plane contains the baseline an intersects the mirror at the epipolar conic. an esire views, respectivel. Without loss of generalit, we choose the worl reference frame coincient with the one at the esire view. Camera parameters are suppose to be known an consequentl the epipolar geometr can be irectl epresse onto the mirror surface if not otherwise specifie. The segment OO, calle baseline, intersects the two mirror surfaces at the epipoles, namel e an e for the current view, an e an e for the esire view. The epipolar plane π intersects each mirror at the epipolar conics C an C. Each epipolar conic passes through the epipoles. Given a pair of views of a set of scene points i (i =,..., n) there eists a matri E IR, calle the essential matri 9, such that X i T EX i = () for all corresponing mirror points X i an X i (i.e., images of the same point in the two views), obtaine as a backprojection of the corresponing image points p i an p i,b means of the inverse of η. In general, the essential matri is of rank an is efine up to an arbitrar scale. Given at least generic corresponences E can be compute up to a scale factor, without an knowlege of the -D structure of the observe scene 9. In presence of image noise, E can be robustl estimate also b means of well known algorithms, e.g.,,. We are here particularl intereste in retrieving the epipole irection in both views. We will henceforth assume to be able to have the right guess of where is the epipole irectl pointing towar the other view (i.e., e an e in Fig. ). The current an esire epipoles can then be retrieve from the left an right null-spaces of E 9. Remark : Suppose given two views p an p, the current an the esire, of the same -D scene point, respectivel. Suppose, moreover, that onl a translation t =z T occurs between their coorinate frames (i.e., R = I). It is an eas matter to see that, for an translational motion of the camera along t, the epipolar conic C associate with (p, p ) oes not var an p will lie on it. In fact, the normal vector of the epipolar plane oes not var for an scaling λ IR {}, thus keeping constant the shape of C.

3 c θ be the two outputs of the sstem that can be written as a function of the epipoles e =e e e z T an e =e e e z T, epresse in the mirror frame, as follows: = π ATAN { e,e } () c = π ATAN {e,e }. () Fig.. camera. A mobile robot with uniccle kinematics carring a cataioptric III. THE NONHOLONOMIC VISUAL SERVOING ROBLEM The objective of this work is to present an image-base visual servoing strateg that rives a nonholonomic robot towar a esire. The nonholonomic mobile robot consiere in this paper is a uniccle moving on a plane (see Fig. ). Its vector is efine b q =θ T, where, are the Cartesian coorinates (in meters) of the center of the robot in a reference frame {O,, }, being θ (in raians) the orientation with respect to the ais (see Fig. ). The nonholonomic kinematic moel is ẋ = u cos θ ẏ = u sin θ θ = u () where u an u are respectivel the translational an the angular velocit. Without loss of generalit, we suppose that the esire is q =π/ T, which correspons to the robot being centere at the origin an aligne with the positive ais. The cataioptric camera is fie to the robot bo in such a wa that the mirror focus O is in T. In the spirit of the visual servoing approach, it is henceforth assume that the esire camera view (i.e., the view acquire in q ) has been gathere in avance an that the epipoles are estimate in real-time accoring to the techniques escribe in Sect. II. Let us consier the situation in Fig.. Let an O e esire e ; O ; θ current Fig.. The planar geometric setup use to efine the kinematics of outputs, written as a function of the epipoles. The propose image-base visual servoing scheme rives the nonholonomic mobile robot to the esire q in two steps (see Fig. ): ) From the initial q, appl a control law that brings an to zero. As shown in the net section, such a control ma be compute through inputoutput feeback linearization. At the en of this step the camera-robot is in the intermeiate q i, aligne with the esire one (see Fig., left). ) From the intermeiate q i appl another image-base control proucing a translation to q (see Fig., right). Clearl, an are ienticall zero in this phase an cannot be use. However, as will be shown net, this step can be realize on the basis of corresponing points in the images. Roughl speaking, the first step is aime at zeroing the orientation error (an placing the robot along the -ais) while the secon step compensates the translation error. IV. FIRST STE: ZEROING THE EIOLES We here esign a visual feeback for the mobile robot in orer to rive both outputs to zero an realize the first step of our visual control strateg. To this en, after eriving the epipole kinematics, we aopt an approimate input-output linearization approach. For our visual servoing purposes it is paramount to remark that these outputs can be written as a function of the epipoles e an e, as in () an (). From Fig. it ma be also seen that Differentiating () we have: From (), the first erivative of is = + θ π. () ẏ = u +ẏ. () e ẏ =ė e + e ė e + e. (7) From Fig., let ( + ) /, being e = λ an e = λ for an unknown λ IR, an = sin an = cos. Then from (7) it iels ẏ = u cos(θ + ). (8) Now, substituting () in (8) we get: ẏ = u u sin ẏ = u sin e (9) ()

4 q i q i e ; e ; q e e st step q q n step Fig.. The two steps of the propose visual servoing strateg. The nonholonomic robot is first riven to the ais b zeroing both outputs an. A feature-base controller is then use to recover the translation error. The stanar proceure to compute an input-output linearizing control law is to ifferentiate the output functions an invert, if possible, the resulting map (see ). From the epipole ifferential kinematics (9-), the relationship between the control inputs an the output time erivatives is epresse as ẏ ẏ u = E u sin with E = sin Here, we are face with a major ifficult, i.e., the inverse of the ecoupling matri E cannot be compute because the parameter (the istance between the current an the esire robot position) is unknown in the purel image-base control framework we are ealing with. Although this prevents from performing an eact input-output, an approimate input-output linearization strateg can be pursue b setting with u u Ê = = Ê ν ν ˆ sin.. () in which an estimate ˆ of is use an the resulting output erivatives are ˆ ẏ ν = ẏ EÊ = ν ν ν roposition : Let ν ν = k k β/γ ˆ () where k >, k > an β,γ are positive o integers, with β<γ. Also, upate the istance estimate ˆ accoring to ˆ = ˆk β/γ () tan initialize at ˆ >, being the istance from the initial to the esire robot s. Then, for sufficientl small k, the approimatel linearizing control () rives both output coorinates an to zero for an initial conition with eponential convergence rate. roof. First of all note that the close-loop equations uner the propose control law are ẏ = k ( ) ˆ k β/γ () ẏ = ˆ k β/γ () while the eplicit epression for the control inputs in () is: u = ˆk β/γ () sin u = k + k β/γ. (7) The istance evolves accoring to = ẋ+ẏ an using () an being = sin an = cos we get = u cos = ˆk β/γ. (8) tan Comparing (8) with (), it is clear that an ˆ obe to the same ifferential equation. Hence, t ˆ ˆ = + ˆ t + =+ ˆ > t ˆ t

5 uner the assumption ˆ >. As a consequence, the coefficient of β/γ in () is negative an boune below in moulus. This means that is a terminal attractor 7 for, which will converge to zero at a finite time instant t. Then from t on, the ifferential equation () governing reuces to ẏ = k, so that will converge to zero with eponential rate k. As a consequence, it can be seen from (8) that ue to the zeroing of at t, the robot converges at a finite istance after t, as shown b (8). In conclusion, note also that () is never singular because u becomes zero after t. It remains to be shown that the control input in (-7) is not singular. In fact, the linear velocit in (7) has a potential singularit when =. This is not a problem for t t, for which u is ienticall zero, being =. Before t, the namics of in () inclue a perturbation term whose effect can be arbitraril boune b bouning k. Hence, for sufficientl small k, the output can not cross zero uring the transient. The following remarks are in orer at this point. The above control law is purel image-base because it onl relies on the measure epipoles. No knowlege of the robot or an other oometric ata is neee. The particular form of the eponent of in the control law () is essential in guaranteeing its convergenc to zero in finite time, an then that the propose control law is never singular. This kin of control law is also known as a terminal sliing moe. Accoring to the above rop., it is necessar to initialize ˆ at an initial value ˆ >. To this en one ma use an upper boun on erive from the knowlege of the environment where the robot moves. If is zero at the initial instant, then Ê is unefine in (). Then, it is necessar to perform a preliminar maneuver (e.g., a fie rotation) before appling the propose controller, in orer to attain a nonzero. The zero namics (i.e., the resiual namics when the outputs are ienticall zero ) associate with our approimate input-output linearization controller is obtaine from (8) as =. That is, the robot will converge to some point of the ais at a finite istance from its esire position, consistentl with the above proof. V. SECOND STE: FEATURES MATCHING At the en of the first step, both the outputs an are zero an the intermeiate robot q i is aligne with the esire (see Fig., right). We now present the feature-base control law that realizes the secon step of our visual servoing strateg, i.e., moving the robot from q i to q so as to recover the translation error. As the epipolebase controller of the previous section, also the secon step controller works irectl in the camera image plane. The basic iea consists in translating the robot until each feature p i (i =,..., n) in the current image plane matches the corresponing one p i in the esire image plane. A similar approach has been propose in, for pinhole cameras. As shown in Sect. II, if the relative rotation is compensate, then uring a translational motion to the target position along the baseline, all feature points p i converge to p i an are constraine also to lie on the corresponing epipolar conic C i. Then the feature istance between corresponing points, chosen as the arc-length s i (t) between p i an p i onto C i, will be zero when p i = p i, i.e., when the robot reaches the final q. In principle, onl one feature is neee to implement this iea, an therefore we will present the controller with reference to the case n =. The propose metho can be easil etene to inclue a larger number of features, a convenient choice in the case of nois images. roposition : Let the robot velocities uring the secon step be efine as u = k t s i(t)e (9) u = () where k t >. Then the robot converges eponentiall from the intermeiate q i to the origin. roof. We here consier that the rotation isparit with respect to the esire robot-camera has been full compensate uring the first step, i.e., θ i = π/ that correspons to e = e =. Consier the positive efinite Lapunov function V =( + )/. Note that e = λ an e = λ for an unknown positive λ. Being e =(i.e., θ = π/), then the time erivative of V iels to V = ẋ + ẏ = u λ e. Substituting (9) in it, we obtain V = k t s i e that is alwas efinite negative, being s i (t) >. Note that s i(t) actsasa stop conition, because it clearl zeroes when p i = p i. VI. SIMULATION RESULTS The simulation results have been performe using Matlab- Simulink an the Epipolar Geometr Toolbo 9. It is assume that five pairs of corresponing feature points are ientifie in the esire an current image. The camera calibration parameters 9 are f = f = mm. an u = piels an u = piels. The uniccle robot moves uner the action of the propose two-step visual strateg from its, ra t s m intermeiate initial m Fig.. Nominal case. First Step. Outputs behavior. Note how the secon output coorinate reaches zero in finite time; Robot trajector.

6 7 u m/s u ra/s.... eg piels t s t s -9 8 steps 8 steps Fig. 7. Nominal case. First Step. Linear velocit an angular velocit of the mobile robot. Note how the linear velocit goes to zero with. initial q = π/ T to the esire one q =π/ T. First the control law in () is applie with k =., k = an β/γ =7/9. The initial estimate of the robot istance has been set to ˆ = m. As shown in Fig., both the output coorinates an are riven to zero an, as epecte, the first one is zeroe in finite time t = sec. The robot trajector for the first step is reporte in Fig. while the resulting control inputs are shown in Fig. 7-. To guarantee a finite time uration of the first step, a tolerance of 9 m. has been use for (recall that convergence is eponential). The secon step is then eecute uner the action of the control law in (), with k t =. The eponential ecrease of the istance s(t) between the current an the esire feature points along the conic is shown in Fig. 8. The robot trajector in the secon step is reporte in Fig. 8. VII. EXERIMENTAL RESULTS In orer to valiate the here propose visual servoing strateg we present some eperimental results realize in our labs. The vision sensor is a fole cataioptric mirror b Remote Realit screwe on a CCD camera b Lumenera Inc. Such a camera can be moele b an orthographic camera looking at a parabolic mirror moele b the equation z m + a = m + m a where a =. mm. The mobile robot is Fig. 9. Eperimental results. Outputs variables an are controlle to zero b the image-base control law; Distance (in piels) from current to target image feature points measure on the epipolar conic. an ioneer X-DE b ActivMeia, connecte to a notebook enowe with a GHz entium processor an with MB of RAM. Orthographic camera parameters are f =mm., f = mm., u =. an v = 8. piels. A set of n =9corresponing feature points has been tracke an use for epipolar geometr estimation using the robust M-estimator propose in. In orer to obtain a better epipolar geometr estimation, we normalize all feature points as suggeste in. Fig. 9 shows the outputs that are riven to zero uring the first step. Note that, ue to image noise affecting the epipolar geometr estimation, perfect convergence of outputs to zero can not be achieve. However, as epecte, is the first output to reach zero, followe b. After both outputs are below (in moulus) a specifie threshol (τ v =7 eg.), then the secon step starts an the mean istance of s i (t) between corresponing points is use to control the translation (Fig. 9). It can be seen that it rapil ecreases to zero, but ue to image noise, to the non perfect alignment 8 s(t) m 7 m initial initial position target position esire t s m Fig. 8. Nominal case. Secon Step. Eponential ecrease of the istance s between the current an the esire features in the image planes; Robot trajector. Fig.. Eperimental results. The ioneer X-DE robot is equippe with a panoramic camera an correctl moves towar the target position using onl the visual information provie b the current an the esire panoramic images.

7 v piels u piels Fig.. Eperimental results. Feature motion on the image plane superimpose to the esire image, starting from the initial features (circle) to the esire ones (cross). after the first step an to moel uncertainties, we obtaine a final istance of about piels for each of the 9 feature pairs. This correspons in a robot isplacement of about cm with respect to the target position. However it is our plan to improve performances an increase robustness b setting the propose two-step controller in an iterative framework as propose in, for nonholonomic robots. The robot motion uner the action of the propose control law, from the starting position to the target one, is reporte in Fig.. The whole feature motion in the image plane is shown in Fig. an superimpose to the esire image, thus showing the convergence to the target image features. VIII. CONCLUSIONS A novel image-base visual servoing strateg has been presente for nonholonomic mobile robots. The control scheme, which is ivie in two inepenent an sequential steps, rives the robot to a esire specifie through a target view, previousl acquire b the on-boar central cataioptric camera. A ke point is the use of multiple-view epipolar geometr uring the first step in orer to compensate the rotational error an align the current view to the esire one. In particular, an approimate input-output linearizing feeback law is use to cope with the nonholonomic kinematics of the camera-robot sstem. Simulations an eperimental results have been performe in orer to valiate the propose visual servoing algorithm. REFERENCES J.. Barreto, F. Martin, an R. Horau. Visual servoing/tracking using central cataioptric images. In 8th International Smposium on Eperimental Robotics, pages 8 89,. R. Basri, E. Rivlin, an I. Shimshoni. Visual homing: Surfing on the epipoles. International Journal of Computer Vision, ():7 7, 999. R. Benosman an S.B. Kang. anoramic Vision: Sensors, Theor an Applications. Springer Verlag, New York,. D. Burschka an G. Hager. Vision-base control of mobile robots. In IEEE International Conference on Robotics an Automation, pages 77 7, G. Chesi, K. Hashimoto, D. rattichizzo, an A. Vicino. Keeping features in the fiel of view in ee-in-han visual servoing: a switching approach. IEEE Transactions on Robotics, ():98 9,. B. Espiau, F. Chaumette, an. Rives. A new approach to visual servoing in robotics. IEEE Transactions on Robotics an Automation, 8():, C. Geer an K. Daniiliis. A unifing theor for central panoramic sstems. In roc. of Sith European Conference on Computer Vision, pages,. Dublin, Irelan. 8 C. Geer an K. Daniiliis. Mirrors in motion: Epipolar geometr an motion estimation. In 9th IEEE International Conference on Computer Vision, volume, pages 7 77,. 9 R. Hartle an A. Zisserman. Multiple View Geometr in Computer Vision. Cambrige Universit ress,. K. Hashimoto an T. Noritsugu. Visual servoing of nonholonomic cart. In 997 IEEE International Conference on Robotics an Automation, pages 79 7, 997. S.A. Hutchinson, G.D. Hager, an.i. Corke. A tutorial on visual servo control. IEEE Transactions on Robotics an Automation, (): 7, 99. A. Isiori. Nonlinear Control Sstems. Springer, 99.. Lucibello an G. Oriolo. Robust stabilization via iterative state steering with an application to chaine-form sstems. Automatica, 7:7 79,. Q.T. Luong an O.D. Faugeras. The funamental matri: theor, algorithms an stabilit analsis. International Journal of Computer Vision, 7(): 7, 99. E. Malis an S. Benhimane. Vision base control with respect to planar an non-planar objects using a zooming camera. In IEEE International Conference on Avance Robotics, pages 8 89,. E. Malis, F. Chaumette, an S. Bouet. -/-D visual servoing. IEEE Transactions on Robotics an Automation, ():8, G.L. Mariottini, G.Oriolo, an D.rattichizzo. Epipole-base visual servoing for nonholonomic mobile robots. In IEEE International Conference Robotics an Automation, volume, pages 97,. 8 G.L. Mariottini, G. Oriolo, an D. rattichizzo. Image-base visual servoing for nonholonomic mobile robots using epipolar geometr. IEEE Transactions on Robotics,. Submitte. 9 G.L. Mariottini an D. rattichizzo. The Epipolar Geometr Toolbo: multiple view geometr an visual servoing for MATLAB. IEEE Robotics an Automation Magazine,. to appear. Y. Mezouar, H. Haj Abelkaer,. Martinet, an F. Chaumette. Central cataioptric visual servoing from straight lines. In IEEE/RSJ International Conference on Intelligent Robots an Sstems, volume, pages 9,. S.K. Naar. Cataioptric omniirectional camera. In roc. of International Conference on Computer Vision an attern Recognition, pages 8 88, 997. G. Oriolo an M. Venittelli. A framework for the stabilization of general nonholonomic sstems with an application to the plate-ball mechanism. IEEE Transactions on Robotics, (): 7,. R. issar-gibollet an. Rives. Appling visual servoing techniques to control a mobile han-ee sstem. In 99 IEEE International Conference on Robotics an Automation, pages 7, 99. T. Sato an J. Sato. Visual servoing from uncalibrate cameras for uncalibrate robots. Sstems an Computers in Japan, (): 9,. T. Svoboa an T. ajla. Epipolar geometr for central cataioptric cameras. International Journal of Computer Vision, 9(): 7,..H.S. Torr an D.W. Murra. The evelopment an comparison of robust methos for estimating the funamental matri. International Journal of Computer Vision, ():7, M. Zak. Terminal attractors in neural networks. Neural Networks, :9 7, 989.