We are IntechOpen, the world s leading publisher of Open Access books Built by scientists, for scientists. International authors and editors

Transcription

1 We are IntechOpen, the worl s leaing publisher of Open Access books Built by scientists, for scientists 4, , M Open access books available International authors an eitors Downloas Our authors are among the 154 Countries elivere to TOP 1% most cite scientists 12.2% Contributors from top 500 universities Selection of our books inexe in the Book Citation Inex in Web of Science Core Collection (BKCI) Intereste in publishing with us? Contact book.epartment@intechopen.com Numbers isplaye above are base on latest ata collecte. For more information visit

2 17 Homography-Base Control of Nonholonomic Mobile Robots: a Digital Approach Anrea Usai an Paolo Di Giamberarino University of Rome La Sapienza Italy 1. Introuction Why oes one nee to waste time eriving a iscrete-time moel from a continuous-time one? The answer is: it epens on how fast the system ynamics is. The classical approach, especially for nonlinear systems, is to evelop a continuous-time control law an then realize the controller by means of a computer implementation. Such an approach can lea to poor controller performance ue to the intrinsic system approximation: the sampling perio, at which the controller commans are upate, fixes the sampling perio at which the system ynamics is observe. All the works one in the fiel of visual servoing, eal with continuous time systems (see, for example, in the case of mobile robots: Chen et al., 2006; Lopez-Nicolas et al., 2006; Mariottini et al., 2006). Image acquisition, elaboration an the nonlinear control law computation are very time consuming tasks. So it is not uncommon that the system is controlle with a sampling rate of 0.5Hz. Is it slow? It obviously epens. To face the rawbacks of poor system approximation, the trivial solution is slowing own the system ynamics slowing own the controls variation rate. This is not always possible, but when the kinematic moel is consiere, the system ynamics epens only on the velocity impose by the controller (there is no rift in the moel) an so it is quite easy to limit the effect of a poor system approximation. In general, taking into account the iscrete-time nature of the controlle system can lea to better close-loop system performances. This means that the control law evaluate irectly in the iscrete-time omain can better aress the iscrete-time evolution of the controlle system. This is well unerstoo in the linear case an it is also true for (at least) a particular case of nonlinear systems, the ones that amits a finite or an exact sample representation (Monaco an Norman-Cyrot, 2001). In fact, the possibility of an exact iscretization of the continuous-time moel is necessary to lea to the performances improvement previously iscusse, since in this case no approximations are performe in the conversion continuous time - iscrete time. The commonly use approximations can lea to a iscrete-time moel with a behavior that iverges from the continuous-time one. In Section 2, the system moel is presente. From this moel a icrete-time erivation is shown in Section 3. In Section 4, the esign of a multirate igital control is then escribe

3 328 Frontiers in Robotics, Automation an Control an iscusse, showing that exact solutions are obtaine. An improve control (an planning) strategy for the iscrete-time system is introuce an applie. Simulation results are reporte to put in evience the effectiveness of the propose approach. 2. The Camera-Cycle Moel In this section the kinematic moel of the system compose of a mobile robot (an unicycle) an a camera, use as a feeback sensor to close the control loop, is presente. To erive the mobile robot state, the relationship involving the image space projections of points that lie on the floor plane, taken from two ifferent camera poses, are use. Such a relationship is calle homography. A complete presentation of such projective relations an their properties is shown in (R. Hartley, 2003). Fig view geometry inuce by the mobile robot. 2.1 The Geometric Moel With reference to Figure 1, the relationship between the coorinates of the point P in the two frames is P R0 t 0 P = (1) It is an affine relation that becomes a linear one in the homogeneous coorinate system. If the point P belongs to a plane in the 3D space with normal versor n an istance from the origin, it hols that 0 T 0 T P np n = 0 = 1 (2) 1 note that > 0, since the interest is in the planes observe by a camera an so they on t pass trough the optical center (that is the camera coorinate system origin).

4 Homography-Base Control of Nonholonomic Mobile Robots: A Digital Approach 329 Combining the Equation 1 an the right term of 2, the following relation hols T 0 0 P = R0 0 = tn P HP (3) The two frame systems in Figure 1 represent the robot frame after a certain movement on a planar floor. Choosing the unicycle-like moel for the mobile robot, the matrix H become 1 cosθ sinθ ( Xcosθ + Ysinθ) 1 H = sinθ cosθ ( Xsinθ Ycosθ + ) (4) is the homography inuce by the floor plane uring a robot movement between two allowable poses. Note that [X,Y,θ] T is the state vector of the mobile robot with reference to the first coorinate system. 2.2 The Kinematic Moel Taking four entries of matrix H such that h = cosθ h = sinθ hx = cosθ + ysinθ h = xsinθ + ycosθ (5) an noting that, for the sake of simplicity, the istance has been chosen equal to one, since it is just a scale factor that can be taken into account in the sequel, the kinematic unicycle moel is Xv & = cosθ Yv & = sinθ θ& = ω (6) where v an ω are, respectively, the linear an angular velocity control of the unicycle. Differentiating the system in Equation 5 with respect to the time an combining it with the system in Equation 6, one obtains

5 330 Frontiers in Robotics, Automation an Control h& 1 = h2ω hh & 2 = 1ω hh & 3 = 4ω+ vh = 4ω+ h& = hω 4 3 v (7) that is the kinematic moel of the homography inuce by the mobile robot movement. 3. From Continuous to Discrete Time Moel In the first part of this section, it will be presente how to erive a the iscrete-time system moel from the continuous-time one. Afterwars, a control (an planning) strategy for the iscrete-time system is introuce an applie. Simulations will be presente to prove the presente strategy effectiveness. 3.1 The General Case With reference to Figure 1, the relationship between the coorinates of the point P in the two frames is m ( ) ugx ( ) x& fx (8) = + i= 1 ii with f, g 1,..., g m : M R n, analytical vector fiels. To erive a iscrete-time system from the previous one, suppose to keep constant the controls u 1,..., u m, by means of a zero orer holer, for t є [kt, (k + 1)T ) an k є N. Suppose that the system output is sample (an acquire) every T secons, too. The whole system compose by a z.o.h, the system an the sampler is equivalent to a iscrete-time system. Following (Monaco an Norman-Cyrot, 1985; Monaco an Norman-Cyrot, 2001), it is possible to characterize the iscrete-time system erive by a continuous-time nonlinear system. Sampling the system in Equation 8 with a sampling time T, the iscrete-time ynamics becomes m xk ( + 1) = xk ( ) + T f + ukg ( ) + i i i= 1 xk ( ) 2 T + L m ( I) +... = 2 f + ui( kg ) i i= 1 T = Fxkuk 2 ( ( ), ( )) (9) where L(.) enotes the Lie erivative. It is possible to see that, this series is locally convergent choosing an appropriate T. See (Monaco an Norman-Cyrot, 1985) for etails. The problem here is the analytical expression of F T (x(k),u(k)). In general, it oesn t exist. Otherwise, if from the series of Equation 9 it is possible to erive an analytical expression for

6 Homography-Base Control of Nonholonomic Mobile Robots: A Digital Approach 331 its limit function, the system of Equation 8 is sai to be exactly iscretizable an its limit function is calle an exact sample representation of 8. If, better, the series results to be finite, in the sense that all the terms from a certain inex on goes to zero, a finite sample representation is obtaine. Finite sample representations are transforme, uner coorinates changes, into exact sample ones ((Monaco an Norman-Cyrot, 2001)). As obvious, finite iscretizability is not coorinate free, while exact is. Note that the existence of an exact sample representation correspons to analytical integrability. A nonholonomic system as the one of Equation 8, can be transforme into a chaine form system by means of a coorinate change. This leas to a useful property for iscretization pointe out in (Monaco an Norman- Cyrot, 1992). In fact it can be seen that a quite large class of nonholonomic systems amit exact sample moels (polynomial state equations). Among them, one fins the chaine form systems which can be associate to many mechanical systems by means of state feebacks an coorinates changes. So, in general, to be able to get a finite iscretization of the nonlinear system of Equation 8, one nees to fin a coorinates change (if it exists) that allows to erive an exact sample moel from the nonlinear one. 3.2 The Camera-Cycle Case Suppose the controlle inputs are piecewise constant, such that vt () = vk ω() t = ωk [,( 1) ) tktkt + (10) where k = 0, 1,.. an T is the sampling perio. Since the controls are constant, it is possible to integrate the system in Equation 7 in a linear fashion. It yiels to hk 1( + 1) hk 1( ) A( k) hk 2( 1) = ω hk 2( ) + hk 3( + 1) hk 3( ) T = A( ωk) + B( ω ) v hk 4( + 1) hk 4( ) k k (11) where cosωkt sinωkt A( ωk ) = sinωkt cosωkt ( ωkt ) ( ωt ) sin / ωk B( ωk ) = ( cos k 1 )/ ωk (12)

7 332 Frontiers in Robotics, Automation an Control If one consiers the angular velocity input as a time-varying parameter, the system in Equation 11 become a linear time-varying system. Such a property allows an easy way to compute the evolution of the system. Precisely, its evolution becomes hk 1 1( ) k h1(0) A( ωki 1) hk 2( ) = i = 0 h2(0) hk ( ) h (0) hk k 1 k 2 k 1 3 T 3 T = A( ωki 1) + A( ωki ) B( ωj ) vj 4( ) i= 0 h4(0) j = 0 i= j+ 1 (13) where the sequences {v k } an {ω k } are the control inputs. This structure will be useful in the sequel for the control law computation. 4. Controlling a Discrete-Time Nonholonomic System Interestingly, ifficult continuous control problems may benefit of a preliminary sampling proceure of the ynamics, so approaching the problem in the iscrete time omain instea of in the continuous one. Starting from a iscrete-time system representation, it is possible to compute a control strategy that solves steering problems of nonholonomic systems. In (Monaco an Norman- Cyrot, 1992) it has been propose to use a multirate igital control for solving nonholonomic control problems, an in several works its effectiveness has been shown (for example (Chelouah et al., 1993; Di Giamberarino et al., 1996a; Di Giamberarino et al., 1996b; Di Giamberarino, 2001)). 4.1 Camera-cycle Multirate Control The system uner stuy is the one in Equation 11 an the form of its state evolution in Equation 13. The problem to face is to steer the system from the initial state h 0 = [1, 0, 0, 0] T (obviously correspons to the origin of the configuration space of the unicycle) to a esire state h, using a multirate controller. If r is the number of sampling perios chosen, setting the angular velocity constant over all the motion, one gets for the state evolution hr 1() rc 1 A ( ) hr 2() = ω 0 hr () = A ( ω) B( ω) v + A ( ω) B( ω) v B( ω) v hr 3 r 1 c c r 2 c c c 0 1 r 1 4() (14) At this point, given a esire state, one just nee to compute the controls. The angular velocity c ω is firstly calculate such that: h 1 1 rc A ( ω) = h2 0 (15)

8 Homography-Base Control of Nonholonomic Mobile Robots: A Digital Approach 333 Once c ω is chosen, the linear velocity values v 0,..., v r-1 can be calculate solving the linear system v0 h v 3 ( ω) ( ω) ( ω) ( ω)... ( ω) = h =... 4 r v 1 r 1 c c r 2 c c c 1 A B A B B RV (16) which is easily erive from the secon two equations of 14. Note that, for steering from the initial state to any other state configuration, at least r = 2 steps are neee, except for the configuration that present the same orientation of the initial one. More precisely, it can be seen that if this occurs, the angular velocity c ω is equal to zero or Π/T an the matrix R in Equation 16 become singular. Exactly that matrix shows the reachability space of the iscrete-time system: if c ω {0, Π/T } then the vector B is rotate r-times an the whole configuration space is spanne. Furthermore, if 2 is the minimum multirate orer to guarantee the reachability of any point of the configuration, one can choose a multirate orer such that r > 2 an the further egrees of freeom in the controls can be use to accomplish the task obtaining a smoother trajectory or avoiing some obstacles, for instance. Note that it can be achieve solving a quaratic programming problem as 1 T min VV Σ +Γ V (17) vv 2 where Σ an Γ are two weighting matrixes, such that the robot reaches the esire pose, granting some optimal objectives. Other constraints can be easily ae to take account of further mobile robot movements requirements. 4.2 Closing the Loop with the Planning Strategy Let [ θ, h 3, h 4 ] T be the esire system state an mark the actual one with the subscript k. Note that, in the control law evelopment, the orientation of the mobile robot is use, instea of the first two components of the system of Equation 11. Summarize the algorithm steps as 0. Set r k =r. 1. Choose ( k) ω = θ θ c rt k 2. Compute the control sequence V as minvv T V such that h 3 RV = h4 with the same notation of Equation 16.

9 334 Frontiers in Robotics, Automation an Control 3. If r k >2 then r k = r k +1 an go to step 1. Otherwise, the algorithm ens. The choice of the cost function shown leas to a planne path length minimization. If the orientation error of the point 1 is equal to zero, it nees to be perturbe in orer to guarantee some solution amissibility to the programming problem of point 2. Furthermore, since the kinematic controlle moel erives irectly from an homography, it is possible use the homographies compositional property to easily upate the esire pose, from the actual one, at every control computation step. Exactly, since H = H H... H... H (18) r 1 k 1 0 r 1 r 2 k 1 0 it is possible to easily upate the esire pose as neee for the close-loop control strategy. A Simulate path is presente in Figures 3: the ieal simulate steer execution (Figure 2) is perturbe by the presence of some aitive noise in the controls. This simulate the effect of some non ieal controller behavior (wheel slipping, actuators ynamics,...). The constraine quaratic problems involve in the controls computation are solve using an implementation of the algorithm presente in (Coleman an Li, 1996). 4.3 Setting Up the Trajectory Planning The angular velocity efines the span of the system configuration space by means of the vectors in the matrix R of the Equaion 16: varying it uring the planning can lea to a better functional minimization. Moreover, it is neee to settle the ω choice in case of no orientation error. First of all, we nee ω is equal to zero at the beginning of the planne path an at the en of it. Consier the function max 2 π ω0( k) = ω0 sin k k {0,1,..., r} r (19) it starts from zero an softly goes to 1 up to return, as softly as before, to 0. For using this function as angular velocity control we have to fin its maximum value to take to zero the orientation error. It can be one setting π θ = max r ω sin k (20) = r k 0 In orer to face the null orientation error case, intuitively, the mobile robot shoul first point the esire pose an then compensate for the esire orientation. Take a look at the function in Equation 19 with the ouble frequency max 2 2π + ω 0 sin k k {0,1,..., r \ 2} r ω p( k) = π max 2 2 ω0 sin k k { r \ 2 + 1,..., r} r (21)

10 Homography-Base Control of Nonholonomic Mobile Robots: A Digital Approach 335 where with the symbol \ it is enote the integer ivision operation. Note that integral of the function in the previous Equation is zero, in the interval of interest, as the final orientation isplacement, ue to its contribution. After half interval can be foun the maximum velocity neee to cancel the pointing angle isplacement as in Equation 20. The angular velocity is chosen as ω( k) = ωo( k) + ωp( k) (22) Since we want to get the shortest path, from the planning strategy, we minimize V T V. Furthermore, we woul like to minimize the variations between the planne controls v o,..., v r 1, in orer to have a smoother behavior of the controlle robot movement. It is possible to accomplish to these specifications, solving the quaratic programming problem 1 T 2 min γ ( 1 ) Σ + r VV k v k v (23) vv k = 1 such that RV = b v < 1 k = 0,..., r 1 k v = v = 0 0 r 1 (24) where the γ is a minimization parameter an the matrix R an the vector b are the same of Equation 16. Note that it is a quaratic programming problem since the secon aen of the cost function of Equation 23 can be expresse as T V V (25) The last two constraints of Equation 24 are ae to make explicit that a pose to pose trajectory is planne. Note that the planne trajectory shown in Figure 4 it is shorter than the corresponing one of Figure 2. This happens because the programming problem referre to the trajectory of Figure 4 is more constraine than the other one: varying ω leas to a better suboptimal solution. As expecte. 4.4 Trajectory Tracking: a Multirate Digital Approach Once the trajectory is planne, it is necessary to introuce a tracking technique to execute it. Classical continuous approaches can be obviously applie: for instance, in Usai & Di

11 336 Frontiers in Robotics, Automation an Control Giamberarino (2006), we use a linear controller to track a multirate planne trajectory, again for a visual servoing problem. In the preceing sections, it has been introuce a close-loop control strategy, iterating the previously iscusse planning technique. It will be now iscusse an extension of this approach to erive a igital trajectory tracker. Trivially, if we want to follow a chosen trajectory we can re-apply the multirate planning strategy to take the system from the actual point to a next intermeiary one. The incessant esire pose upate constraints the system to follow the previously planne path. More precisely, every m steps it is possible to re-solve a quaratic programming problem similar to the one solve for the planning problem but, this time, the esire pose is the one planne τ steps ahea the actual one. Summarizing the escribe tracking algorithm, we have 0. Set r an plan the trajectory. Set K=0, m k =0. 1. Execute the planne controls at step k an set k=k+1 an m k = m k If k=r, the algorithm stops. 3. If m k <m come back to point If k (r-τ), upate the ω computing θ 1 θ ω( + 1) = ω( ) + k k i i i = k,..., k + m mt An, subsequently, the sequence v minimizing m T m m m m T m m m 2 min( V) ( V) + γ ( RV b) ( RV b) + γ ( vk+ v k+ ) m V 5. Set m k =0 an go to point Note that the programming problem has been relaxe in orer to have a smoother tracking behaviour. For the same reason it is ae a term regaring the esire pose velocity. Furthermore, the two parameters m an τ influence how the trajectory is tracke. For instance, if τ = 2 the error in the trajectory execution is less than the one obtaine with larger values of τ. On the other han, large values of τ allow many egrees of freeom for the programming problem solution. This etermines the eformation of the previously planne trajectory, putting in the programming problem new constraints. For example, these constraints woul be useful to let the robot avoi other unexpecte obstacles that come out uring robot movement. An interesting implementation coul be the one in which the parameter τ varies when some event occurs (a new obstacle etecte by sensors), allowing the path eformation to a new feasible one. With regar to the parameter m (remark that m < τ), since it influences how many times the control is compute uring the tracking, it epens on the computer on which the controller will be implemente. τ τ 5. Conclusion In this chapter, a kinematic moel for a system compose by a mobile robot an a camera, has been presente. Since such a moel is exactly iscretizable, it has been possible to propose a multirate igital control strategy able to steer the system to a esire pose in an exact way.

12 Homography-Base Control of Nonholonomic Mobile Robots: A Digital Approach 337 As can be seen from the trajectory obtaine in the simulations of Figures 2 an 3, there are large ifferences between the ieal path execution an the perturbe one. It happens because the iterate planning strategy has is no memory of the previously planne path. The controls are just constraine to take the system to the esire state. There is no control on how get there. In a real implementation it is avisable to have a certain egree of preictability of the robot behavior uring its movement an the respect to some optimal criteria (short paths, smooth movements, obstacles avoiance,...), too. This is why it has been chosen to present a separate planning phase an a subsequent the tracking phase. The effectiveness of the control scheme aopte has been verifie by simulations an presente in Figures References Chelouah, A., Di Giamberarino, P., Monaco, S., an Norman-Cyrot, D. (1993). Digital control of nonholonomic systems two case stuies. In Decision an Control, 1993., Proceeings of the 32n IEEE Conference on, pages vol.3. Chen, J., Dixon, W., Dawson, M., an McIntyre, M. (2006). Homography-base visual servo tracking control of a wheele mobile robot. Robotics, IEEE Transactions on [see also Robotics an Automation, IEEE Transactions on], 22(2): Coleman, T. an Li, Y. (1996). A reflective newton metho for minimizing a quaratic function subject to bouns on some of the variables. SIAM Journal on Optimization, 6(4): Di Giamberarino, P. (2001). Control of nonlinear riftless ynamics: continuous solutions from iscrete time esign. In Decision an Control, Proceeings of the 40th IEEE Conference on, volume 2, pages vol.2. Di Giamberarino, P., Grassini, F., Monaco, S., an Norman-Cyrot, D. (1996a). Piecewise continuous control for a car-like robot: implementation an experimental results. In Decision an Control, 1996., Proceeings of the 35th IEEE, volume 3, pages vol.3. Di Giamberarino, P., Monaco, S., an Norman-Cyrot, D. (1996b). Digital control through Finite feeback iscretizability. In Robotics an Automation, Proceeings., 1996 IEEE International Conference on, volume 4, pages vol.4. Lopez-Nicolas, G., Sagues, C., Guerrero, J., Kragic, D., an Jensfelt, P. (2006). Nonholonomic epipolar visual servoing. In Robotics an Automation, ICRA Proceeings 2006 IEEE International Conference on, pages Mariottini, G., Prattichizzo, D., an Oriolo, G. (2006). Image-base visual servoing for nonholonomic mobile robots with central cataioptric camera. In Robotics an Automation, ICRA Proceeings 2006 IEEE International Conference on, pages Monaco, S. an Norman-Cyrot, D. (1985). On the sampling of a linear analytic control system. In Decision an Control, 1985., Proceeings of the 24th IEEE Conference on, pages pp Monaco, S. an Norman-Cyrot, D. (1992). An introuction to motion planning uner multirate igital control. In Decision an Control, 1992., Proceeings of the 31 st IEEE Conference on, pages vol.2.

13 338 Frontiers in Robotics, Automation an Control Monaco, S. an Norman-Cyrot, D. (2001). Issues on nonlinear igital control. European Journal of Control, 7(2-3). R. Hartley, A. Z. (2003). Multiple View Geometry in Computer Vision. Number ISBN: Cambrige University Press. Usai, A., Di Giamberarino, P. (2006). A multirate igital controller for non holonomic mobile robot pose regulation via visual feeback. WSEAS Transactions on Systems, 5, Fig. 2. Multirate control simulation ( = 1m). Ieal (no noise) path execution.

14 Homography-Base Control of Nonholonomic Mobile Robots: A Digital Approach 339 Fig. 3. Multirate control simulation (re-iterate planning, = 1m). Aitive ranom noise on controls (gaussian with st.ev. 0.5 an 0.05, for v an ω respectively).

15 340 Frontiers in Robotics, Automation an Control Fig. 4. Multirate control simulation (planning + tracking, = 1m). Aitive ranom noise on controls (gaussian with st.ev. 0.5 an 0.05, for v an ω respectively).

16 Frontiers in Robotics, Automation an Control Eite by Alexaner Zemliak ISBN Har cover, 450 pages Publisher InTech Publishe online 01, October, 2008 Publishe in print eition October, 2008 This book inclues 23 chapters introucing basic research, avance evelopments an applications. The book covers topics such us moeling an practical realization of robotic control for ifferent applications, researching of the problems of stability an robustness, automation in algorithm an program evelopments with application in speech signal processing an linguistic research, system's applie control, computations, an control theory application in mechanics an electronics. How to reference In orer to correctly reference this scholarly work, feel free to copy an paste the following: Anrea Usai an Paolo Di Giamberarino (2008). Homography-Base Control of Nonholonomic Mobile Robots: a Digital Approach, Frontiers in Robotics, Automation an Control, Alexaner Zemliak (E.), ISBN: , InTech, Available from: a_igital_approach InTech Europe University Campus STeP Ri Slavka Krautzeka 83/A Rijeka, Croatia Phone: +385 (51) Fax: +385 (51) InTech China Unit 405, Office Block, Hotel Equatorial Shanghai No.65, Yan An Roa (West), Shanghai, , China Phone: Fax:

17 2008 The Author(s). Licensee IntechOpen. This chapter is istribute uner the terms of the Creative Commons Attribution-NonCommercial- ShareAlike-3.0 License, which permits use, istribution an reprouction for non-commercial purposes, provie the original is properly cite an erivative works builing on this content are istribute uner the same license.