Path Planning and Steering Control for an Automatic Perpendicular Parking Assist System

Transcription

1 Pat Panning and Steering Contro for an Automati Perendiuar Parking Assist System Pamen Petrov, Fawzi Nasasibi, Member, IEEE, and Moamed Marouf Abstrat Tis aer onsiders te erendiuar reverse arking robem of front wee steering veies. Reationsis between te widts of te arking aise and te arking ae, as we as te arameters and initia osition of te veie for anning a oision-free reverse erendiuar arking in one maneuver are first resented. Two tyes of steering ontroers (bang-bang and saturated tan-tye ontroers) for straigtine traking are roosed and evauated. It is demonstrated tat te saturated ontroer, wi is ontinuous, aieves aso quik steering avoiding attering and an be suessfuy used in soving arking robems. Simuation resuts and first exerimenta tests onfirm te effetiveness of te roosed ontro seme. I. INTRODUCTION Te erendiuar arking is te most effiient and eonomia sine it aommodates te most veies er inear meter [], and is eseiay effetive in ong term arking areas. Due to te seia onstraint environments, mu attention and driving exeriene is needed to ontro te veie, and tis arking maneuver may be a diffiut task. For tis reason, automated oeration attrats signifiant attention from resear view oint, as we, and from te automobie industry. One of te diffiuties in aieving automati arking is te narrow oerating ae for oisionfree motion of te veie during te arking maneuver, and anning of otima trajetories is often used in te aiations. In [], an otima stoing agoritm was designed for arking using an aroa ombining an ouany grid wit anning otima trajetories for oision avoidane. Te geometry of te erfet arae arking maneuver is resented in [3]. In [4], a ratia reverse arking maneuver anner is given. A trajetory anning metod based on forward at generation and bakward traking agoritm, eseiay suitabe for bakward arking situations is reorted in [5]. A ar arking ontro using trajetory traking ontroer is resented in [6]. In [7], a saturated feedbak ontro for an automated arae arking assist system is reorted. In reent years, automati arking systems ave been aso deveoed by severa automobie manufaturers [9, ]. In tis aer, we fous on geometri oision-free at anning, and feedbak steering ontro for erendiuar reverse arking in one maneuver. Geometri at anning based on admissibe iruar ars witin te avaiabe arking P. Petrov is wit te Fauty of Meania Engineering, Tenia University of Sofia, Sofia, Bugaria, (e-mai: etrov@ tu-sofia.bg). F. Nasasibi is wit te Robotis & Inteigent Transortation Systems (RITS), INRIA - Roquenourt, 7853 Roquenourt, Frane, (e-mai: fawzi.nasasibi@inria.fr). M. Marouf is wit te Robotis & Inteigent Transortation Systems (RITS), INRIA - Roquenourt, 7853 Roquenourt, Frane, (e-mai: moamed.marouf@inria.fr). sot is resented in order to steer te veie in te diretion of te arking ae in one maneuver. Two steering ontroers (bang-bang and saturated tan-tye) for at traking are roosed and evauated. Te rest of te aer is organized as foows: In Setion II, geometri onsiderations for anning erendiuar reverse arking in one maneuver are resented. In Setion III, two feedbak steering ontroers are roosed. Simuation resuts and first exerimenta tests are reorted in Setion IV. Setion V onudes te aer. II. GEOMETRIC CONSIDERATIONS FOR COLLISION-FREE PERPENDICULAR PARKING IN ONE MANEUVER A. Veie Mode In tis aer, a retanguar mode of a front-wee assenger veie is assumed. Te veie arameters wi affet te arking maneuver, as we as te arameter vaues used in te simuations, are resented in Tabe I. TABLE I. VEHICLE PARAMETERS Veie arameters Notation Vaue Longitudina veie base.6m Wee base b.8m Distane between te front axe and te front bumer.94m Distane between te rear axe and te rear bumer.74m Maximum steering ange α max π/6rad B. Coision-Free Pat Panning wit a Constant Turning Radius Te geometry of te reverse erendiuar arking in one oision free maneuver is sown in Fig.. In te erendiuar arking senario onsidered in tis aer, te veie starts to move bakward from an initia osition in te arking aise, wit onstant steering ange α, wi may be smaer tan te maximum steering ange ( α α max ), and as to enter in te arking ae (osition ) witout oiding wit te boundary of arking ot L and boundaries, and 3 of arking ot L. In osition te orientation of te veie is arae wit reset to te arking ae. After tat, te veie ontinues to move bakward in a straigt ine into te arking ae unti it reaes te fina osition 3 (Fig. ). Assuming a iruar motion of te veie (wit turning radius ), wit enter O (Fig. ). Te radius is auated from te formua =. () tanα

2 Te boundaries of te turning at during te erendiuar arking are determined by te dimensions of te traes (iruar ars) formed by te eft orner of te front bumer B wit radius r B, te eft orner of te rear bumer B 4 wit radius r B4, and te end of te rear wee axe C, resetivey, as sown in Fig.. Sine te veie exeutes a ane rotation, te trajetories of tese oints form ars of onentri ires. Parking aise y G 3 F 3 x Parking ot L A D r B 4 B 3 Figure. Geometry of te oision-free erendiuar arking maneuver From te ΔOC B, aying te Pytagorean Teorem, we obtain an exression for te radius r B of te iruar ar traed by te eft orner of te front bumer B in terms of te veie arameters,, b, and te turning radius, as foows b ( ) rb = OB = () From te ΔOC B 4, we determine te radius r B4, of te iruar ar traed by te eft orner of te rear bumer B 4 b 4 4 rb = OB = + +. (3) We assign an inertia frame Fxy attaed to te arking ae, were te enter F is aed in te midde between te borders of te arking ae, wi as its y-axis aigned wit te boundary of arking ot L, as sown in Fig.. Let O denotes te enter of rotation of te veie (te Instantaneous Center of Rotation (ICR)) wen it starts te arking maneuver wit onstant steering ange α. Deending on te sign of x-oordinate of ICR (oint O) wit reset to te Fxy frame, i.e., te offset s (Fig. ), different formuas an be derived in order to determine te required widt of te O C P r B4 C Parking ot L r B s B B b arking ae and te widt of te arking aise (te orridor) as funtions of s in order to ensure oision-free erendiuar arking in one maneuver. We onsider rigt turning of te ar in te foowing two ases: Te ICR O beongs to te interva: s [ ( b/), ] Te ower vaue of te interva orresonds to te ase wen te rigt side of te veie B B 3 (Fig.) ies on te boundary ine of arking ot L. In order to avoid oision between te eft orner B of te front bumer wit te boundary of L (Fig. ), using (), we obtain an exression for te widt of te arking aise, as foows = r B s = b. (4) ( + ) + + s Te funtion = f(s) defined by (4) is inear in s, ositive and monotoniay inreasing in te above-mentioned osed interva for s. Terefore, it takes its minimum and maximum vaues at te ends of tis interva. To avoid a oision between te rigt oint C of te rear axe wit te vertex A of obstae L, from te ΔOAD, aying te Pytagorean Teorem, te distane OD (Fig. ) is auated as foows b = s. (5) OD In order to avoid a oision between te eft orner B 4 of te rear bumer wit te edge 3 of te arking ae, using (3) and (5), te foowing exression for te widt of te arking sae is obtained = rb 4 OD = + b b + s. (6) Te funtion = f(s) defined by (6) is ontinuous on te osed interva of s mentioned above. Tis funtion is differentiabe on te oen interva s ( ( b/), ), and its derivative is given by s = s b s. (7) < Terefore, te funtion = f(s) is strity dereasing on te osed interva [-( b/), ]. Te maximum and minimum vaues of an be found by reaing in (6) te boundary vaues of te interva: s = - ( b/) and s =. Te ICR O beongs to te interva: s [, ] Te uer bound orresonds to te ase wen te rear bumer ies on te Fy-axis at te instant wen te orientation of te veie is arae to te arking ae.

3 In order to avoid a oision between te eft orner B of te front bumer wit te boundary of L, using (), we obtain an exression for te widt of te arking aise b = rb + s = ( + ) s. (8) Again, te funtion = f(s) defined by (8) is inear in s, ositive and monotoniay inreasing in te abovementioned ose interva of s. Terefore, it takes its minimum and maximum vaues at te ends of tis interva. To avoid a oision between te eft orner B 4 of te rear bumer wit te edge 3 of te arking ae, and between te rigt oint C of te rear veie axe wit te vertex A of obstae L, we obtain te foowing exression for = b b + + s. (9) Te funtion = f(s) defined by (9), is ontinuous on te osed interva of s [, ]. Tis funtion is differentiabe on te oen interva s, and te derivative is s = ( ) s b + + s. () < Terefore te funtion is strity dereasing on te osed interva s [,. Te maximum and minimum vaues of ] an be found by reaing te imit vaues s = and s = of te interva, resetivey, in te exression (). It soud be noted tat for s =, te two funtions defined by (6) and (9) take te same maximum vaue. For s =, te funtion = f(s) takes minimum vaue, wi is exaty te widt b of te veie. From a ratia oint of view, it is imortant to determine te starting ositions of te veie for arking witout oision in one maneuver in te ase wen te widts and of te arking aise and te arking sae, resetivey, are seified in advaned. Suose tat te widts of te arking aise and te arking ae are set as = d and = d, resetivey, and aso tat d < r B. In tis ase, from () and (4), it foows tat s = d r. () max B From (3) and (6), we obtain a formua for te minimum vaue of s as foows ( r ) b s = min. () B4 d Simuation resuts were erformed to iustrate te reationsis between te widts and of te arking aise and te arking sae, resetivey, as funtions of te offset s in te interva [-( b/), ] by using arameters of te test veie (Tabe I) wit α = α max, ( = min ). Te vaues of and, ( d and d ), were osen as foows: d = 6m and d =.4m. As seen from Fig., te funtion = f(s) (te soid bue ine) dereases in te interva and onverges to b=.8m (te red dotted ine), wi is exaty te engt of te wee base of te veie. Meanwie, te gra intersets te orizonta ine for te assigned vaue of d =.4m (te bue dotted ine) at s = - s min = -.9m, wi is te minimum vaue of s obtained from () for oision-free arking. In order to ark te veie in one maneuver for s = - s min = -.9m, from (8), te required minimum widt of te arking aise is obtained to be = 4.55m wi is ess tan te seified vaue of d = 6m. Te funtion = f(s) (te green soid ine) inreases ineary in te interva and te gra intersets te orizonta ine for te assigned vaue of d = 6m (te green dotted ine) at s = - s max = -.46m, wi is te maximum vaue of s, obtained from (). For s = - s max = -.46m, from (6), te required minimum widt of te arking ae as to be =.88m, wi is ess tan te assigned vaue of d =.4m. Terefore, given seified vaues = d = 6m and = d =.4m for te arking aise and te arking sae, resetivey, for oision-free arking, te offset s an take vaues in te interva [- s min, - s max ] = [-.9m, -.46m], were te boundary vaues are determined by () and (), resetivey. [m], [m] d = 6m min =.8m = f(s), = f(s) = f(s) = f(s) - s min = -.9m d =.4m - s max = -.46m s[m] Figure. Coision-free interva for s Te distanes between te ar and te boundaries of te arking sae and r (Fig. ), wen te veie is arae to te arking sae, are determined as foows r b b s, (3) = d r = b. (4) From te simuations, for s = - s min = -.9m, te obtained vaues of r and are r =.55m and =.5m.

4 From a ratia view oint, it is better to ark te ar symmetriay wit reset to te boundaries of te arking ae, sine it is not very wide. For tis end, we auate te minimum vaue of te offset s = s m, in order to ark te veie symmetriay in te enter of te arking sae (Fig. 3). We set as s d b : = r = =. (5) From te ΔOAD (Fig. 3), te distane OD is determined d x Parking aise Parking ot L P(x.y) b s m OD =. (6) Sine te turning radius an be exressed as y F T A D r O - s max - s m b = + r + OD, (7) and substituting r from (3) and OD from (6) into (7), we arrive to an exression for s m, as foows 3 G Parking ot L d Figure 3. Geometry of oision-free erendiuar arking b d s m =. (8) Te new offset - s m is bigger tan tose given by () (- s m > - s min ). In te simuation resuts, - s m = -.44m > -.9m. In genera, it must be eked weter te new offset - s m is smaer tan - s max given by (). If it is te ase, te ar an ark symmetriay witout oision in reverse wen s is at east s = - s m. In tis ase, owever, te boundary 3 of te arking ae wi not be tangent to te ar of ire traed by oint B 4 of te eft orner of te rear bumer; neverteess, oint A (vertex A of obstae L) wi ie again on te ar of ire traed by oint C of te rear veie axe. Terefore, given seified dimensions of te arking aise and arking ae = d and = d, resetivey, te offset s an take vaues in te osed interva - s [- s m, - s max ], were - s m and - s max are determined from formuas (8) and (), resetivey, (Fig.3). Hene, in order to erform reverse erendiuar arking in one maneuver and to ae te veie symmetriay in te arking ae, te starting osition, i.e., te referene oint P of te veie as to be on any one of te ars of ires wit radius of enter O(x O, y O ), were x O [- s m, - s max ] and y O = -, wit reset to an inertia frame Fxy attaed to te arking ae. Te initia orientation as to be tangent to te ar (Fig. 3). Te referene at of te arking maneuver onsists of two arts. Te first one is a iruar ar wit enter O onneting te staring osition of te veie and te tangent oint T between te ar and te x-axis of Fxy. At tat oint, te ar wi be arae to te arking ae. Te seond art of te referene at is a straigt ine aong te y- axis of te oordinate frame Fxy between oint T and te goa osition G of te arking ae, were oint G ies on te x-axis of Fxy, (Fig. 3). III. STEERING CONTROL For a ow seed motion, wi is te ase of te arking maneuver, we assume tat te wees of te veie ro witout siding, and te veoity vetors are in te diretion of te orientation of te wees. We onsider a simified (biye mode) of te veie, were te front and rear wees are reaed by two virtua wees, aed at te ongitudina axis of te veie. An inertia oordinate system is attaed to te arking ae (Fig. 3). Te oordinates of te referene oint P in Fxy are denoted by (x P, y P ). Te orientation of te veie θ is defined as an ange between te x-axis of Fxy and te ongitudina veie base. Te front wee steering ange is denoted by α. Te equations of motion of te veie in te ane ave te form [7] x& P = v y& P = v & vp θ = P P osθ sinθ, (9) tanα were v P is te veoity of oint P. We onsider a ratia stabiization of te veie in te arking ae. Our aroa is based on ontroing te motion of te veie aong a straigt ine (te x-axis of Fxy) assing troug te goa oint G (Fig.3) in te arking ae and aigned wit te orientation of te ae wit veoity of te ar, wi is deendent of te distane between te veie and te goa osition [7]. Sine te referene at for te first art of te arking maneuver is a iruar ar, first a bang-bang ontroer is roosed, were te front wee steering ange is onstrained by magnitude and takes ony two onstant vaues. As a onsequene, te veie trajetories reresent iruar

5 ars. However, in ratie, due to te disontinuity of te ontro aw, an undesirabe beavior of te system (attering) wi our wen te osition of te veie is in te viinity of te traking ine, and te orientation error is aso sma. In order to avoid te attering, a saturated ontro based on yerboi tangent funtion is aso roosed, wi is onstrained by magnitude, but te ontro funtion is ontinuous. A. Bang-Bang Contro In tis aer, we roose a bang-bang ontro in te ase wen te veie is moving bakward, (v P = - v P < ). Te veie as to trak a straigt ine wi oinides wit te x- axis of oordinate frame Fxy. Te design of te ontro ow is based on te seond and tird equations of (9). Te steering ange of te front wees is onstraint and takes vaues ± α. For brevity of exosition, we wi resent te fina form of te bang-bang ontro. Te ontro design roedure for bakward driving of te veie is simiar to tose resented in [8], but te form is sigty different, sine te veie veoity as negative sign. Te bang-bang ontroer for bakward driving as te form u u = u were if or if or θ θ > sin sin tanα θ θ = sin sin and tanα θ θ < sin sin tanα θ θ = sin sin and tanα >, () y < tanα u =. () B. Saturated Contro In order to avoid attering in te system wen a ure bang-bang ontro is used, we roose a differentiabe saturation in te form of yerboi tangent (tan(.)) onstraint. Tis funtion is bounded by ±. Aso tan(x) if x, and tan(x) < if x <. Tan(x) is ose to te signum funtion, wen in tan(k t x) te gain K t is arge, as sown in Fig. 4. tan(x) x Figure 4. Te funtion tan(k t x) for K t =,3, and We roose te foowing feedbak bounded steering ontroer P were u is given by (), [ u tan( K v) ] α = a tan t, () and K t, K and a are ositive onstants. v = K( θ a ), (3) IV. SIMULATION AND EXPERIMENTAL RESULTS Simuation resuts using MATLAB are resented to iustrate te effetiveness of te roosed steering ontroers for erendiuar reverse arking in one maneuver. Te arameters of te veie are given in Tabe I. For te simuations, te onstant steering ange of te front wees was osen to be α = α max = π/6rad. Using (), for te minimum turning radius is obtained te vaue of =4.5m. Te arking aise d was 6m wide, wie te widt of te arking ae d was.4m. Te initia oordinates of te veie referene oint P wit reset to te inertia frame Fxy attaed to te arking ae were (x P (), y P ()) = (3.5m, -4.5m). In tis ase, te offset s is equa to s = -m and beongs to te interva [- s m, - s max ] = [-.44m, -.46m] for symmetri arking in one maneuver. Te initia orientation of te veie was osen to be θ() = -π/rad. Te initia oordinates of te veie referene oint P wit reset to an inertia frame Gxy wit enter aed in te goa osition G of te veie in te arking ae (Fig. 3), and wi as its x-axis aigned wit te x-axis of Fxy are (x P (), y P ()) = (7.5m, -4.5m). Te maximum vaue of te veie veoity was osen to be v P =.3m/s. Te vaues of te saturated tan-tye ontroer were K t = 8, K = 5.85, a =.7. Starting from identia initia onditions, te anar ats of te veie using bang-bang ontro and saturated (tantye) ontro are resented in Fig. 5. As seen from te simuation, te veie trajetories are quite simiar. Tis resut sows tat te saturated ontro an be used instead of bang-bang ontro in order to steer te veie into te arking ae aording to te geometria onsiderations for oision-free reverse erendiuar arking in one maneuver resented in Setion II. y[m] Veie at in te ane (a) x[m] y[m] Veie at in te ane (b) x[m] Figure 5. Perendiuar arking: Panar ats of te veie using bangbang ontro (a) and saturated ontro (b). Evoution in time of te front-wee steering ange by using bang-bang ontro and saturated ontro is resented in Fig. 6. Te simuation resuts sow te advantage of te saturated ontro: te attering ourring using bang-bangontro, wen te osition of te veie is in te viinity of te traking ine, and te orientation error is aso sma, is avoided.

6 afa[rad] Steering ange "afa" t[s] (a) afa[rad] Steering ange "afa" (b) t[s] ontroers) for straigt-ine traking ave been roosed and evauated. It was demonstrated tat, te saturated tan-tye ontroer, wi is ontinuous, was abe to aieve aso quik steering avoiding attering and an be suessfuy used in soving arking robems. Simuation resuts and te first exeriments wit a test veie onfirm te effetiveness of te roosed ontro seme. Figure 6. Perendiuar arking: Evoution in time of te front-wee steering ange using bang-bang ontro (a) and saturated ontro (b) An animation of te erendiuar reverse arking in one maneuver using saturated tan-tye steering ontro is sown in Fig y[m] x[m] 5 6 Figure 7. Perendiuar reverse arking using saturated ontro Te saturated tan-tye ontroer as been imemented on an exerimenta automati eetri veie CyCab and initia tests of erendiuar reverse arking as been initiaized (Fig. 8). In te first tests, ony information from te enoders mounted on te wees were used for determining te osition of te veie wit reset to an inertia frame attaed to te goa osition into te arking ae. Te dimensions of te CyCab are: =.m; b =.m; = =.35m; α = α max = π/6rad and = min =.8m. Te assigned vaues for te arking aise and te arking ae were osen to be d = 3m and d = m, resetivey. For symmetri arking into te arking ae, aording to (8) and () te offset s an take vaues in te osed interva - s [- s m, - s max ] = [-.m, -.95m]. For te exeriment sown in Fig. 8, te initia oordinates of te veie wit reset to Gxy wit enter aed at te goa osition in te arking ae were aroximatey (x P (), y P ()) = (3m, -.m). Te first exeriments onfirm te effetiveness of te roosed ontroer. V. CONCLUSION In tis aer, te robem of erendiuar reverse arking of front wee steering veies was onsidered. Geometri onsiderations for oision-free erendiuar arking in one reverse maneuver were first resented, were te sae of te veie and te arking environment were exressed as oygons. Reationsis between te widts of te arking aise and arking ae, as we as te arameters and te initia osition of te veie ave been given, in order to an a oision-free maneuver, in te ase, wen te ar as to be symmetriay ositioned into te arking ae. Two tyes of steering ontroers (bang-bang and saturated 7 Figure 8. Automati erendiuar arking of a CyCab veie REFERENCES [] USAF - LANDSCAPEDESIGN GUIDE, avaiabe at: tt:// mtu.edu/ubiations/7/parkingdesignconsiderations.df. [] B. Gutjar and M. Wering, Automati oision avoiding during arking maneuver an otima ontro aroa, In Pro. 4 IEEE Inte.. V. Symosium, 4, [3] S. Bakburn, Te geometry of erfet arking, Avaiabe at: tt://ersona.ru.a.uk/ua/58/erfet_arking.df. [4] C. Pradaier, S. Vaussier and P. Corke, Pat anning for arking assistane system : Imementation and exerimantation, In Pro. Austr. Conf. Rob. Automation, 5. [5] J. Moon, I. Bae, J. Ca, and S. Kim, A trajetory anning metod based on forward at generation and bakward traking agoritm for automati arking systems, In Pro. IEEE Int. Conf. Inte. Trans. Systems, 4, [6] K. Lee, D. Kim, W. Cung, H. Cang, and P. Yoon, Car arking ontro using a trajetory traking ontroer, In Pro SICE_ICASE Int. J. Conferene, 6, [7] P. Petrov and F. Nasasibi, Saturated feedbak ontro for an automated arae arking assist system, In Pro. IEEE Conf. Contr. Autom. Rob. Vision, 4, [8] P. Petrov, C. Boussard, S. Ammoun, and F. Nasasibi, A ybrid ontro for automati doking of eetri veies for rearging, In Pro. IEEE Int. Conf. Rob. Automation,, [9] Avaabe at: tts:// [] Avaabe at: tts:// 8