Flatness based tracking control of a manoeuvrable vehicle : the Car

Transcription

1 Fatness based tracking contro of a manoeurabe ehice : the Car P. Braut, H. Mounier Département AXIS, Institut d Éectronique Fondamentae, Bâtiment, Uniersité Paris-Sud, 9145 Orsay, FRANCE. fpatrice.braut,hugues.mounierg@ief.u-psud.fr N. Petit, P. Rouchon Centre Automatique et Systèmes, Écoe des Mines de Paris, 6 bd. Saint Miche, 755 Paris, FRANCE. fpetit,rouchong@cas.ensmp.fr Keywords: Fatness, automotie systems, autonomous ehices, tracking contro. Abstract We study the tracking contro of a ery manoeurabe ehice aimed at autonomous tasks. Two modes are used and are shown to be fat; this property is then used to obtain open oop contros. The study is compemented by a stabiization around the desired trajectory. 1 Introduction Fuy automatic driing of persona cars is a far reaching chaenge for the automotie industry. The Car project of the AXIS department in the Institut d Éectronique Fondamentae (IEF 1 ) is aimed at giing concrete contributions in this respect. A prototype of a maneourabe eectric ehice has been designed and named Car. The design of the Car ehice is quite simiar to the CyCab one, which originated at INRIA (see[1] for detais) and is now sod by the Robosoft company (see, e. g., The CyCab is a four whee drie car, with the possibiity of steering independenty the front and rear whees. In the Car configuration, the rear and front steering anges and are mechanicay couped (i.e. f ()). We can thus consider the Car ehice as haing two actuators: a motor steering the ange and another motor for the propusion. The embedded sensors are a steerabe camera (through pan and tit contro), a aser teemeter (aso controed through pan and tit) and one to four actuated utrasonic sensors (the direction and shape of the pattern is controed). This work was partiay supported by the European Commission s Training and Mobiity of Researchers (TMR) Contract # ERBFMRX-CT97137, by the G.D.R. Medicis and by the G.D.R.- P.R.C. Automatique. 1 Institute of the Uniersité Paris-sud. This paper is deoted to the tracking contro probem for a kinematic mode of the Car. We obtain a simpe soution thanks to the fatness property (see [4], [5], [7] for fu detais on the theory). In the second section, the kinematic mode is deried. The fatness property is then recaed in informa terms in section three; it is estabished for the mode and the open oop contros are obtained in section four. Stabiization around the desired trajectories is then studied in the ast section; seera different schemes are deried and discussed. The Mode.1 Deriation of the mode The front and rear steering anges are and ; the orientation ange of the car is (see Figure 1). For reasons that wi x θ ψ P H M Q M ρ M α M Figure 1 : The Car ehice. become cear in section 4, we wi use the foowing construction. Let Q and P be the middes of the front and rear axes. Take two straight ines passing through P (resp. Q) and orthogona to the rear (resp. front) whees; these two ines intersect at a point R. Now take the straight ine pass- R φ y

2 ing trough R and orthogona to the ine (P; Q); eth be the intersection point of this ine and of (P; Q) (see Figure 1). Let now M be any point of the ine (P; Q), M the ange between the ines (M;R) and (H; R) and M the ength of the segment [M;R]. The dynamics of the orientation ange at M is gien by _ M M where M is the speed of the point M. The steering radius is HM M sin( M ) The distance HM may be obtained through thus and PH tan( ) HQ PH tan() tan( ) HQ tan() HM tan( M ) HM HQ tan( M) tan() with HQ + PH where is the ength of the segment [P; Q] (the inter axes distance). Then or which gies HQ (, HQ) tan() HQ HM The radius M is then gien by and tan() tan( ) tan() + tan( ) tan( M ) tan() + tan( ) M cos( M ) (tan() + tan( )) _ M cos( M ) (tan() + tan( )) The dynamics of (x M ;y M ) is gien by the projection of the speed ector M aong the x and y axis: _x M M cos( + M ) _y M M sin( + M ) Finay, the kinematic mode of the ehice written at a point M is: _x M M cos( + M ) _y M M sin( + M ) _ M cos( M ) (tan() + tan( )) (1a) (1b) (1c). The Car Configuration: Various coordinate frames In the present case, we wi consider a static couping between and of the form with f() <for a [; ]. f() ()..1 Mode in Cartesian coordinate frame The kinematic mode of the ehice at point H is ( M for M H): Let us set: _x H H cos() _y H H sin() _ H tan() + tan(f()) F : 7,! tan() + tan(f()) (3a) (3b) (3c) and drop the H subscript here and in the seque (for notationa conenience). The mode then becomes: _x cos() _y sin() _ F () Where the contros are (4a) (4b) (4c) the propusionspeed ; and the steering ange and the oriented curature (t) is expressed as (s(t)) y(t)_x(t), _y(t)x(t) (_x (t) + _y (t)) 3.. Mode in Frenet s coordinate frame F ((t)) A mode that wi be conenient in the seque (see section 5) arises using the arc ength s d of a reference cure C d for H instead of the physica time t. Now take C d as a parametric cure C d (x d (s d );y d (s d )) and a differentiabe function t 7! s d (t) from [;T] into [;L] such that s d (), s d (T )L and _s d () _s d (T ). taking the parametric equations of the trajectory C d. We then set and the corresponding mode is (denoting dds by ): _s d x cos y sin (5a) (5b) (5c)

3 3 Fatness Let us reca, briefy, the meaning of the fatness notion. A noninear system is described by a (finite) set of differentia equations F (z;:::;z (i) ;:::;z ( ) ); 1;:::;N: Broady speaking, the notion of fatness (see [4], [5], [7]) corresponds to the foowing: a noninear system is caed fat if there exists a coection y (y 1 ;:::;y m ) (where m is the number of independent inputs in the system) of functions, caed a fat output, with the foowing three properties: 1. The components of y can be expressed in terms of the system ariabes z iadifferentiareations of thetype for i 1;:::;m. y i P i (z;:::;z (k) ;:::;z ( i) ). The components of y are differentiay independent, i.e. they are not reated by any (non-triia) differentia equation Q(y;:::;y (k) ;:::;y () ): 3. Eery ariabe z i used to describe the system, for instance states or inputs, are directy expressed from y using ony differentiations. In other words, any such z i satisfies a reation of the type z i R(y;:::;y (k) ;:::;y () ): The third property yieds a simpe soution to the probem of tracking the coection of desired trajectories y d (y 1d (t);:::;y md (t)). The second property ensures that the different components of y d (t) can be chosen independenty. 4 Fatness and open oop Tracking 4.1 Fatness of the Car Configuration The mode (4) with two contros is fat and a fat output for this mode is (x; y). Indeed, (4a) + (4b) gies p _x + _y Then, (4b)(4a) gies And, finay, (4c) yieds Arctan F,1 _y _x _! with _ d dt Arctan Thus, one has : _y _x 1 1+ _y _x y _x, _y x _x (y _x, _y x) F,1 (_x + _y ) 3 recaing that F is the foowing function: F : 7,! tan() + tan(f()) y _x, _y x _x + _y Taking a parametric cure C (x(s);y(s)), withs the arc ength, one has (t) _s(t) (t) F,1 [ (s(t))] where _s is the eocity aong the cure C and (s) is the oriented curature. The oriented curature is expressed as (s(t)) y(t)_x(t), _y(t)x(t) (_x (t) + _y (t)) 3 F ((t)) And, for a gien steering ange, the higher F is at, the higher the curature can be, and the more steerabe the ehice is. 4. Open oop Tracking Take a desired trajectory (x d (t);y d (t)). The open oop contros steering the ehice aong these trajectories (assuming a perfect mode and an exact knowedge of the initia conditions) are q d (t) _x Hd (t) + _y Hd (t) (6a) (yd d (t) F,1 (t) _x d (t), _y d (t)x d (t)) (6b) (_x d (t) + _y d (t))3 Now take a parametric cure C (x(s);y(s)) and a differentiabe function t 7! s(t) from [;T] into [;L] such that s(), s(t )L and _s() _s(t ). The open oop contro (t) _s(t) (t) F,1 ((s(t)) (7a) (7b) eads the ehice from (x();y();()) to (x(l);y(l);(l)). 5 Cosed oop Contro 5.1 Contro schemes The preceding contro aws supposed that the mode is perfect and that the initia conditions are known exacty. Since this is neer the case in practice, we add correctie terms to the open oop contros deried aboe in order to hae a stabiization around the desired trajectories.

4 5. First scheme: a simpe PID One of the first ideas that comes to mind is that, when the ehice goes off its reference trajectory C, the whees shoud be steered in order to go back to C again. In other terms, when there is an error on the curature radius, or what is the same, on the curature, an appropriate action on the ange shoud be taken. The most cassica waytodothisistotakeapidcontroeronthiserror. One can aso sow down the ehice if the atter goes quite quicky off its reference trajectory, or if the curature radius becomes quite sma. Set e _s (t) _s(t), _s d (t); where _s d (t) and d (s(t)) stand for q _s d (t) _x d (t) + _y d (t) Since we can set Thus e (t) (s(t)), d (s(t)) d (s(t)) y d(t)_x d (t), _y d (t)x d (t) (_x d (t) + _y d (t))3 F () F ( d) d e F (),F( d) F () e + F ( d ) This eads to the foowing controer: (t) d (t) + c (t) d (t), K (t)e (t), K e (t) + e _s (t) + F ()(t) F( d )(t) +F ( c )(t) Z t t e _s ( )d (8) F( d )(t) +(K, )e (t) + _e (t) + Z t e ( )d (9) t where the term K (t)e (t) accounts for wide braking anges. 5.3 Second scheme: endogenous dynamic feedback Foowing the dynamica extension agorithm, we differentiate the fat outputs unti the contros and appear. The mode being _x cos _y sin _ F() through the first deriatie of x, wefind. Through the second deriatie of y: y _ sin + _ cos _ sin + F () cos we see appearing. We want to derie a dynamic feedback such that the resuting system (with the new contros u 1 and u )is We then set x u 1 y u u 1 x _ cos, F () sin u y _ sin + F () cos which can be rewritten as u1 u cos sin, sin _ F() 1 A And we obtain the foowing dynamica controer (by inersion of the preceding formuae) F () (u cos, u 1 sin ) _ u 1 cos + u sin (1a) (1b) (1c) This controer has sti a singuarity in, which can be soed through a time scaing, by taking the parametric equations of the trajectory C.Wethentake(see (5)) The controer F () _s d (u cos, u 1 sin ) u 1 cos + u sin can then be used to foow a reference trajectory C gien by s 7! (x d (s);y d (s)) We take u 1 x d (s), 1(x, x d (s)), (x, x d (s)) x d (s), 1( cos, x d (s)), (x, x d (s)) u y d (s), 1(y, y d (s)), (y, y d (s)) y d (s), 1( sin, y d (s)), (y, y d (s))

5 Since 1 and can be chosen such that the system in s e x, 1e x, e x e y, 1e y, e y is stabe. Finay, the controer in time t More precisey, the maximum steering ange is mechanicay imited to :34 radonthecar. The figures beow exhibit the difference in steering aues for both ehices foowing the same trajectory. They show a ower aue for the 4-whee steering Car ehice for which the steering aue aways stays beow :34 rad. F() _s (11a) (u cos, u 1 sin ) s(u 1 cos + u sin ) (11b) (11c) can be used to foow a reference trajectory C gien by s 7! (x d (s);y d (s)). Wetake u 1 x d (s(t)), 1 [ cos, x d (s(t))], [x, x d (s(t))] (1a) u y d (s(t)), 1 [ sin, y d (s(t))], [y, y d (s(t))] (1b) piecr b When the ehice is going forward, one must choose 1 and in order to hae eigenaues with negatie rea part. Indeed, we hae an error system of the form sa, and _s >. When the ehice is going backwards, one must choose 1 and in order to hae eigenaues with negatie rea part, since _s < inthiscase. 6 Mechanica couping in Car and steering A simpe and quite conenient choice is f(). Thus, F() tan. The configuration of the Car ehice is f () with 1 < < 1. Thus, for a steering ange [;) F() tan() + tan() > tan() and the Car (with a curature off ()) is more steerabe than a conentiona car (with a curature of tan()). piecr b Figure 3 : Moements of the Car ehice Figure 4 : Corresponding steering ange of Car ecr Figure : High breaking ange trajectory Figure 5 : Moements of a conentiona ehice.

6 >< >: and when h<: (x i,1 ;y j )(x i ;y j i;y (x i ;y j,1 )(x i ;y j i;y + h + (x i;y (x i;y j (15).1..3 If we take an increase of one pixe between two points of the trajectory, we then assume jhj1 and we can compute from (14 and 15) the Lapacian for each point : Figure 6 : Corresponding steering ange of a conentiona ehice. 7 Anti-coision trajectory generation The autonomous motion of a ehice impies obstace aoidance. The method we use is a refinement of the potentia method impements a gradient descent on a potentia function (x; y) as an initia trajectory generation. This method starts with setting the potentia for the characteristic points which are : The initia point of the trajectory where the potentia is set to : (x ;y )1 The fina point where : (x n ;y n ) The obstaces! where : (x!i ;y!i )1 Then the potentia function is computed by an iteratie method. In order to aoid the trajectory computation to be stopped by the presence of oca minima, the function has to be harmonic, as a resut of the mean aue property (see, e.g., [], [1]). In this case the fina point is the end of the trajectory. The Harmonic condition is gien, for a function < n, by the Lapace equation : r (x 1; :::; x n ) i (13) In our two-space procedure, the potentia function (x; y) < is discretized in (x i ;y i ). The Lapacian is first computed from one point to the next by taking a series deeopment of the discretized potentia function at order on both x and y directions. We then write, when h>: 8 >< >: (x i+1 ;y j )(x i ;y j i;y (x i ;y j+1 )(x i ;y j i;y + h + (x (x i;y (14) r (x i ;y j (x i ;y j (x i ;y j 4(x i ;y j )+(x i+1 ;y j )+ (x i,1 ;y j )+(x i ;y j+1 )+(x i ;y j,1 ) According to (13), the trajectory is then iteratiey computed, using the Gauss-Seide method, in order to minimize the Lapacian. One Gauss-Seide iteration consists in repacing eery non-boundary (x i ;y i ) with the aerage of its neighbor s aues taken on two different iterations k and k+1: (k+1) (x i ;y j ) 1 4 (k) (x i+1 ;y j )+ (k+1) (x i,1 ;y j )+ (k) (x i ;y j+1 )+ (k+1) (x i ;y j,1 ) Remark. The appied method is rigourousy aid when the steering ange is not too high. The adaptation to the case of high manoeurabiity is under study. 7.1 Optimization by smoothing spines The trajectory computed by Gauss-Seide iterations exhibits stairs and is not smooth enough to enabe to deduce open oop contros. Furthermore, we woud ike to optimize this trajectory with respect to the steering parameter. In this order, we decimate the noisy trajectory x;y (k) then fiter it with smoothing spines. Such spines proide an aternatie to interpoating spines and are of better hep if we want to get rid of an exact fit on noisy data. In these spines, the interpoation constraint is reaxed and the best fit between the mode s(x) and the noisy data x;y (k) is made by minimization of an error term (the first term). The second term imposes a smoothness constraint on the soution. X kz ( x;y (k), s(k)) + Z +1,1 (s (m) (x)) dx (16) An optima soution can be found by taking a spine of degree n m, 1 [8], [11], [1]. In our case, we set m ; so we make use of cubic spines.

7 Another idea to smooth the data woud be to use waeet denoising techniques. [5] FLIESS M., LÉVINE J., MARTIN P. and ROUCHON P., Fatness and Lie-Bäckund approach to equiaence and fatness of noninear systems, IEEE Trans. Automat. Contr., to appear. [6] GUILLAUME D. and ROUCHON P., Obseration and contro of a simpified car. In Proc. IFAC Motion Contro 98, Grenobe, 1998 [7] MARTIN P., MURRAY R.M. androuchonp., Fat Systems, in Penary Lectures and Mini-Courses, ECC 97, G. Bastin and M. Geers, Eds., pp , Brusses, [8] REINSCH C.H., Smoothing by spine functions, Numer. Math., 1, 1967, pp [9] ROUCHON P. and RUDOLPH J., Inariant tracking and stabiization: probem formuation and exampes, in D. Aeyes, F. Lamnabhi-Lagarrigue, and A. an der Schaft (Eds.), Stabiity and Stabiization of Noninear Systems, Lecture Notes in Contro and Information Sciences 46, Chapter 14, pp , [1] RUDIN W. Rea and Compex Anaysis, McGraw-Hi, New York, [11] SCHOENBERG I.J., Spine functions and the probem of graduation, Proc. Nat. Acad. Sci., 5, 1964, pp [1] UNSER M., Spines: a perfect fit for signa/image processing, IEEE Signa Processing Magazine, to appear. Figure 7 : Square obstace and the generated trajectory. 8 Concusion The fatness and open oop tracking for manoeurabe ehices has been deried. Two cosed oop schemes hae been inestigated ; the ery nice scheme of inariant quasi-static state feedback [9] wi soon be appied to our case, as we as an obserer reying upon anaogous ideas [6]. Other trajectory generation schemes are under study. References [1] BAILLE G., GARNIER P., MATHIEU H. and PISSARD- GIBOLLET R., Le Cycab de INRIA Rhône-Apes, INRIA tech. report n 9, [], CONNOLLY C.I. and GRUPEN R.A., On the appication of Harmonic Functions to Robotics, Journa of Robotic Systems, 1, 1993, pp [3] DELALEAU E. and RUDOLPH J., Contro of fat systems by quasi-static feedback of generaized states. Internat. J. Contro, 71, 1998, pp [4] FLIESS M., LÉVINE J., MARTIN P. and ROUCHON P., Fatness and defect of non-inear systems: introductory theory and appications, Internat. J. Contro, 61, 1995, pp