Automatic Parallel Parkinkg Maneuver with Geometric Continuous-Curvature and Trajectory Regenation

Transcription

1 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) Selected and Extended Papers rom 1th International Symposium on dvanced Vehicle Control (VEC'14) Research Paper utomatic Parallel Parkinkg Maneuver with Geometric Continuous-Curvature and Trajectory Regenation Hélène Vorobieva 1) icoleta Minoiu-Enache ) Sébastien Glaser 3) Saïd Mammar 4) 1)-) Renault 1 avenue du Gol, Guyancourt, France ( hvorobieva@gmail.com and nicoleta.minoiu-enache@renault.com)) 3) IFSTTR, LIVIC 77 rue des Chantiers Versailles, France ( sebastien.glaser@isttar.r) 4) IISC, Université d Evry Val d Essonne 9105 Evry, France ( said.mammar@ibisc.univ-evry.r) Received on March 17, 015 Presented at the VEC 14 on September 5, 014 STRCT: This paper presents a trajectory generation method or the automatic parallel parking. In the irst place, a continuouscurvature path satisying geometric constraints is generated. During the automated tracking o the trajectory, ollowing errors might appear. Then, i necessary, a new trajectory is generated to correct these errors. The regeneration method presents the same advantages as or the initial path generation (continuous curvature) and allows correcting the deviation o the vehicle. Moreover, the complete unctional architecture including the path generation is presented in this article to illustrate the execution o the parking maneuver. KEY WORDS: electronics and control, autonomous driving system, parking assist system / path planning [E1] 1. Introduction Since parking spots have become very narrow in big cities, maneuvering the vehicle can be diicult and stressul even or experimented drivers. Thereore, automatic parking can increase the comort and the saety o the driver. In this contribution, we consider the path generation or the automatic parallel parking. The methods to generate the trajectory or the parking problem can be divided into two groups: geometric methods with circle arcs and continuous-curvature methods. The geometric methods involve easy geometrical equations and create trajectories based on admissible collision-ree circular arcs, which lead the vehicle in the parking. Some methods have only been presented or the parking in one maneuver as in (1) and (). Other methods allow the parking in several maneuvers as in (3), where a orbidden zone has been deined and the car can park traveling outside it. nother approach is based on retrieving a vehicle rom the parking spot and reversing this procedure. This approach is particularly instinctive and adapted to the parking problems (4) and (5). However, the curvature o path based on circle arcs is discontinuous: the vehicle has to stop to reorient its ront wheels. This is particularly demanding or the steering column, induces a aster wear o the tires and leads to an undesirable delay. Continuous-curvature methods overcome these problems. Some o these methods are based on two phases path planning. First, a collision-ree path is created and then subdivided to create an admissible path. For example, (6) uses clothoidal curves or this purpose. However, these methods oten need multiple optimizations o the created path when applied to the parking problem and a possible high computational cost. Moreover, the optimal solution is not always guaranteed. To reduce the complexity and create a path adapted to the parking problem with low computational cost, we recently have combined the simplicity o the geometric path planning with the advantages o a continuous- curvature path by using clothoids (7). The resulted curve answers the constraints o admissibility or a car-like vehicle and describes very well its behavior. Thus, generally, a simple open-loop control provides a satisactory path ollowing. However, experiments showed that or parking spots where multiple maneuvers are necessary, the accumulation o the errors with respect to the desired pose might induce signiicant deviations and prevent the vehicle rom parking correctly. For these cases the trajectory regeneration can be a solution in order to correct the deviations. In the next section, we present a brie state o the art o the sensors and the actuators and explain why with these technologies open loop parking is not always satisactory. In the section 3, the path planning method with continuous curvature is presented. In the sections 4, the control o the steering angle and the control o the longitudinal speed are outlined. The section 5 details the regeneration o the path and gives a simulation example. In the section 6, we present the ull sequence o the parking maneuver with the regeneration o the trajectory. Finally, the section 7 is devoted to the conclusions.. Why Trajectory Regeneration is ecessary.1. State o the art o the sensors or the parking For the autonomous parking the vehicle needs to know the size o the parking, the position o the obstacles and its own pose on the road (exteroceptive inormation). It also should be able to measure the travelled distance when it parks (proprioceptive inormation). Modern cars are oten equipped with an S (nti-lock raking System), which measures the speed o the wheels at each time step. Moreover, the steering angle is also generally available via a steering angle sensor or by using an EPS (Electric Power Steering) internal estimator. This inormation can be used to calculate the travelled distance, as in (10). However, the vehicle has also to be equipped with other sensors to have inormation about its environment and to correct the drit o the odometry. There are a lot o sensors which provide exteroceptive inormation, but we will list only some o them more appropriate or a mass-produced autonomous parking system. Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 107

2 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) The ultrasound sensors are very popular or the parking problem (11) (1) or their simplicity o use and low price. However, they have a blind zone (some centimeters), which prevent them to detect very close obstacles. Moreover, lane markings cannot be detected and that is why only spots deined by surrounding obstacles can be detected. Cameras present the advantage to detect lane marking and with stereovision (or with a mono-camera when the vehicle is moving) it is also possible to calculate the distance to the obstacles. However, diiculties can be encountered or the inormation proceeding and in low light conditions. Modern cars are also oten equipped with anti-collision systems which use radars or LIDRs. These sensors could be used or the parking problem only or the beginning o the maneuver as they cannot detect close obstacles (or example, or Continental the anti-collision systems don t work below 1 m (13) ). good solution to minimize the errors is to have dierent sort o sensors and proceed to a usion o the inormation, or example with a camera and 16 ultrasound sensors as in (14)... State o the art o the actuators or the parking The control o the actuators to achieve the desired steering angle and vehicle speed is a diicult task or low parking speeds. Let consider irst the steering column and the steering actuation. This is carried out mainly by controlling the electric machine installed on the steering column or on the rack and used or the driver assistance. The steering angle error is transormed into a steering torque or the electric machine. For low speeds and high angles, the nonlinear behavior o the steering system and o the tires is diicult to neglect and the steering control law has to deal with it. Moreover, the limitation o the steering dynamics imposed or the drivers saety when a driver is in the car has to be taken into consideration as in (16). The automatic longitudinal control o the vehicle or low speeds is carried out today mainly or vehicles with thermal engine and automatic transmission or or electric vehicles. For a thermal engine one diiculty is to manage very low throttle inputs or low accelerations that are comortable or the passengers. This operation is easier or an electric engine. Either the type o engine, a problem arises when controlling the brake, since the small brake dierentials are also hard to obtain. Moreover, designing an appropriate switching between the throttle and the brake remains challenging. Studies o dierent control approaches or automatic speed control at low speeds are given in (18) and (19). Moreover the lateral and longitudinal vehicle control can be impacted by the tires deormation. The hypothesis o rolling without slipping might not be completely respected on all suraces, especially or the driven wheels. comparison o the kinematic vehicle model with a dynamic vehicle model or a low speed trajectory tracking maneuver is given in (17)..3. Trajectory regeneration necessity Having the pose o the vehicle and its environment, it is possible to generate a path and the associated control signals or the automatic parking. However, as previously shown, the use o dierent sensors, even with a data usion, cannot provide a perect detection o the environment and a perect pose o the vehicle. The resulting detection errors could be corrected during the maneuvers i the environment is better detected when the coniguration o the vehicle changes. However, the initial trajectory might not be valid anymore. Moreover, the environment can change during the maneuver (apparition or disappearance o an obstacle, or instance). In that case, the initially generated path is no more valid. In addition, as previously outlined, the actuators control is challenging or low speeds and this leads to deviations rom the initially generated parking path. To solve these problems, two solutions exist. The irst one is to have a closed loop system or each time step that ensures a good trajectory tracking. This approach does not solve however the problem o changing environment. The second approach is to regenerate the path at some key points o the trajectory. In the sequel, we present our path planning method, enhanced by the regeneration method to correct the trajectory deviations. 3. Initial Path Planning 3.1. Geometric path planning with circle arcs The parking maneuver is a low-speed movement. Consequently, the ckerman steering is considered with the our wheels rolling, without slipping around the instantaneous center o rotation. Dierent turning radiuses are calculated. For example, with E being respectively the center o the rear track, R E =a/tanδ. The other radiuses can be deduced geometrically. The notations used or the vehicle are presented in Fig. 1 and Fig. and we denote R min =R Emin the radius associated with the maximal steering. First a geometric path is created with the reversed method (5). Then the trajectory will be processed to obtain a continuous curvature path. The vehicle is considered in the parking spot, and a retrieving path composed by circle arcs representing orward and backward moves o the vehicle is deined and reversed. orward move means steering at maximum toward the outside o the spot and then moving orward until approaching the ront obstacle. backward move means steering at maximum toward the inside o the spot and then moving backward until approaching the rear or lated obstacle. When a orward move allows the vehicle to retrieve without collision, the last two circle arcs connecting the real initial pose o the vehicle are calculated (Fig. ). 3.. Clothoid turns In this section, we briely describe the general properties o clothoids applied to the path planning or vehicles (8). clothoid is a curve whose curvature κ=1/r varies linearly with its arc length L and that is deined by its parameter with ²=RL. Without loss o generality, consider the start coniguration o the vehicle q i =(x i,y i,ψ i,δ i )=(0,0,0,0). vehicle moving with constant positive longitudinal and steering velocity is describing a clothoid whose parameter depends on these velocities. The coniguration or any position q o the vehicle at a distance L rom the initial coniguration is then (Fig. 3): q x L L (1) C, y S, ² /(R²), 1/ R where C and S are the Fresnel integrals: x x and () C ( x) cos u² du S ( x) sin u² du 0 0 The center o the circular arc C, located at a distance R rom a coniguration q, in the direction normal to ψ is: xc xr Rsin, yc yr Rcos (3) In addition, we deine the radius R 1 and the angle μ between the orientation o q i and the tangent to the circle o center C and radius R 1 : R arctan x / y (4) 1 xc ² yc ², C C Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 108

3 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) Fig. 1 Vehicle in global (x,y,ψ)-coordinates (let) and in a parking spot (right) Fig. Example o geometric parking in 3 maneuvers into a sequence CC. Three cases are classically considered (8) (see Fig. 5 or the cases and ). Case ) α tot α clotho : We use the sequence CC( min,l min, α tot -α clotho ). Case ) α clotho α tot μ: We use the sequence CC( new,l new,0), with: new cos C R1 ²sin( )² / sin S / Lnew new (6) Where ( ) mod end start is the delection o the turn. Case C) α tot <μ: In this case, it is no more possible to use the method o the case. evertheless, a let steering is still possible. clothoid o parameter min and o length L casec is calculated to satisy the ollowing criteria: α tot = α clothocasec, and at the end position it is no more possible to go orward (or backward, depending on the case) without a collision. s this calculus includes parametric curves with Fresnel integrals, an approached solution is considered. Finally, the sequence CC( min,l casec,0) is used. The last step is to calculate the starting point o the irst circle arc. Indeed, this starting point is dierent with respect to the starting point calculated with the geometric method based on circle arcs. To minimize lateral oset, the vehicle has to go backward or orward without steering rom its initial position E init until it arrives at a point E correct on a circle o radius R 1 connected with a tangential point to the second circle (Fig. 4). (5) 4. Control Steering and Speed Signals Fig. 3 Clothoid turns Then we deine a clothoidal sequence CC(,L,θ): 1) clothoid o parameter, length L, initial null curvature and inal curvature equal to 1/R; ) optional circle arc o radius R and angle θ; 3) clothoid o parameter, length L, initial curvature equal to 1/R and inal null curvature. To make the most o the maximum steering angles o the vehicle, we want the radius R 1 to be as small as possible. The minimum time to steer rom null steering to maximum steering is t min =δ max /v δ, where v δ is the maximum desired steering velocity. The minimum associated length o the path is then deined as L min =v longi t min, where v longi is the maximum desired longitudinal velocity. The parameter o the used clothoid is then min ²=R min L min and R 1 is calculated using Eq Let s also call α clotho the angle ormed by a clothoid started with a null curvature and inished with a curvature 1/R min. Open-loop control signals o the steering angle δ and the longitudinal velocity v are generated in real time in unction o the traveled distance d. The speed control purpose is that the vehicle accelerates to a desired speed and then stays at this speed to inally decelerate and stop when the vehicle has traveled the total distance o a orward move or a backward move. This control signal could be a trapezoid signal. The steering control signal or each sequence CC(,L,θ) is: k arctan( a / Rd1) ( d) k arctan( a / Rmin ) k arctan( a / Rd ) d d L L d 0, L, L L L arc arc, L With k=±1 the let or right steering, L arc = θr min, L CC =L+L arc, R d1 =²/d, R d =²/(L CC -d). CC (7) 3.3. From circle arcs to a continuous-curvature path s previously mentioned, the irst step o the parking path generation is to create a geometric path composed o circle arcs as in (5). In order to prepare the transormation o the generated path in a continuous curvature path, the radius R 1 is used instead o the usual radius R min. nother modiication concerns C the instantaneous center o rotation. In the geometrical methods, it is located on the same line with the rear track. Here, in order to use clothoids, the orientation o the vehicle situated on the circles o the geometric path always presents an angle μ with the tangent to the circle in E, so C is transormed by the rotation o center E and angle μ (Fig. 4). These two modiications eectuated, the second step is to transorm each circle arc o angle α tot o the geometric method Fig. 4 Path or a parking in one maneuver with the geometric method using the angle μ and radius R 1 Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 109

4 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) Fig. 5 Evolution rom a circle arc to an or sequence 5.1. General Strategy 5. Regeneration o the Trajectory Dierent methods exist to ensure that the parking is correct and can be divided into two groups: methods which use reerence trajectories and these which doesn t use one. The irst step or the methods which use reerence trajectories is to create a trajectory. Then, robust control laws, e.g. (0), ensure that the trajectory is well executed. However, these methods need to calculate explicitly the (x,y)-points o the trajectory, which would need a lot o computational cost or our method, and the sensors need to measure precisely the pose o the vehicle at each moment, what is not always possible. Moreover, i the environment changes, all the planniication has to be redone entirely. Some methods don t use any reerece trajectory. For example, (1) uses uzzy logic to create the control laws knowing at each moment the pose o the vehicle. However, these methods also need to have good sensors enough to give precise inormation o the pose o the vehicle at each moment. Our strategy or the correction o the trajectory brings novelty with respect to the state o the art. s we have shown in (15), big deviations rom the initial path planning are observable only ater 4 maneuvers (a maneuver is a sequence in the same direction). Consequently, we consider that our control method is precise enough at least or one maneuver and it does not need to be corrected at every time step. Moreover, an emergency stop can prevent the vehicle rom collision i the deviation is dangerous, thus we don t need to know the precise pose o the vehicle at each moment. s we will show, our regeneration method need the precise pose o the vehicle only when it stops. Our path planning method and associated control signal being simple and eicient, it would be thereore interesting to have the same advantages or the regenerated path. The method we present relies on the generation o a trajectory between two poses. Each time the vehicle stops (end o a backward or orward maneuver or emergency stop due e.g. to an obstacle being too near), the position o the vehicle is estimated and compared to the calculated position o the path planning. I the error makes unacceptable the next maneuvers, the regeneration o the path occurs. However, i the current pose o the vehicle is acceptable or a inal pose, the parking maneuver is considered as inished and no urther maneuver is perormed. The method to regenerate the path has to respond to the same constraints as the initial path planning: that means to satisy the geometric constraints, to have a low computational cost and, i possible, a continuous-curvature. n interesting solution is to use the reversed geometrical method as or the initial path planning. In that case, there is no need to re-calculate the trajectory (as the method o retrieving rom the parking spot never changes) or all the maneuvers except or the one that the vehicle should have done immediately ater the stop. The problem consists thus in inding a trajectory rom the real pose o the vehicle (position and orientation), to the next stop pose calculated during the initial path planning. For example in Fig. 6, or a parking in three maneuvers, the algorithm generates a trajectory characterized by the irst real position (1r), a second, third and ourth wished positions (s, 3s, 4s). I at the end o the irst maneuver the real position o the vehicle is r instead o s, the problem is to generate a path between r and 3s. The path between 3s and 4s already exists. In the section 5, the pose o the vehicle is represented as an arrow, which starts at the point E o the vehicle and whose direction represents the orientation o the vehicle. 5.. Continuous-curvature solution i-elementary paths In order to keep simple control signals and admissibility o the trajectory, clothoids are used. In (9), bi-elementary paths composed by two sequences CC (without the optional arc o circle) are presented. They can link any two poses o the vehicle thanks to an intermediary pose, symmetric to both the initial and inal poses o the vehicle (Fig. 7). To create a bi-elementary path between two poses o the vehicle q =( x,y,φ ) and q =( x,y,φ ), a new rame R is deined, it is centered in the middle o the segment [] and its x- axis is oriented along the vector. In this rame, the coordinates o q and q are respectively (-r/, 0, α) and (r/, 0, α ), with α=φ -θ, α =φ -θ and θ is the orientation o R in the global rame (Fig. 8). The results o (9) show that it is possible to ind a CC sequence (without the optional arc o circle) between two poses q and q only i theses poses are symmetric. This means that the orientations o these poses are symmetric with respect to the line joining their positions. Formally, the coordinates o q and q must veriy: ( x x)sin ( y y)cos (8) In practice, two poses are rarely symmetric, so an intermediary pose q I, symmetric to both q and q, is needed to link q and q by CC sequences. The authors o (9) show that the set o such poses has a constant curvature κ=sin(β)/r, with β=(α -α)/. Consequently, this set is a circle o center C and radius R=1/κ i κ 0 or a line otherwise (Fig. 9). This set is deined by (s reers to the length along the set o positions): Fig. 6 Examples or the regeneration o the trajectory Fig. 7 Examples o bi-elementary paths Fig. 8 Frames involved in bi-elementary paths Fig. 9 Conigurations symmetric to q and q Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 110

5 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) Remark: the angle between the tangent o the curve in respectively and, and the poses q and q is γ= α+α /. The angle between the tangent to the curve in I and the pose q I, is γ. q (, s) I and i κ=0: y xi (0, s) s qi (0, s) yi (0, s) 0 I (0, s) I x (, s) sin( s) / (, s) (cos( ) cos( s)) / (, s) s ( ') / I I (9) (10) Having two symmetric poses, it is possible to create a CC(,L,0) sequence taking the ollowing parameters (9) : 1/ (11) L / (1) with 4 cos( ) C ( / ) sin( ) S ( / ) r 5... Strategy When the regeneration o the trajectory has to be done between two poses q and q, the method presented above is applied to ind a trajectory composed by one CC sequence (i the poses are symmetric) or by a bi-elementary path (i the poses are not symmetric). Once this trajectory is created, we veriy that it is admissible and collision ree. I it is admissible and collision ree, the created trajectory is kept. Otherwise, i it is possible to use curvature discontinuous paths, the solution presented in 5.3. is implemented. I it is not allowed, the solution presented in 5.4. can be used. In the sequel, we study the conditions o admissibility or the created trajectory. I the poses are symmetric, the CC sequence is created using the Eq.(11) and Eq.(1). It is admissible i the needed steering does not exceed the maximal steering o the vehicle, which means that ²/L R min. I the poses are not symmetric, there exists a set o intermediary poses q I. The diiculty resides on inding an admissible intermediary pose q I, or which I ²/L I R min and I ²/L I R min. Due to the high computational cost o Fresnel integrals involved in the creation o the CC sequences, all the set cannot be analyzed. The geometric study o the unctions give the ollowing results (see Fig. 10). Let P be the intersection point between the line containing q, the circle o center C and radius R. I the point I goes rom to, when it goes close to the point P, the steering needed or the irst circle arc decreases (which mean that I ²/L I decreases). I P is not situated between and, when the point I goes rom to, the steering needed or the irst circle arc increases. Moreover, wherever the point P is situated, when the point I goes rom to, the steering needed or the second circle arc decreases (which mean that I ²/L I decreases). These variations allow us to proceed by dichotomy to ind an admissible pose q I. I P is situated between and, the dichotomy is done on the circle arc between and P. Otherwise, it is done on the circle arc between and. The conditions to stop the algorithm o dichotomy are:. 1) n admissible pose q I is ound. ) pose q I such as I ²/L I <R min and I ²/L I <R min is ound. In that case, giving the geometric study o the variations o the unctions, no admissible pose q I exists. 3) The dichotomy exceeds a given number o iterations. In that case, we consider that no admissible pose q I exists Solution based on circle arcs s it was done in the previous section with clothoids, two poses q and q can be linked by a circle i the poses are symmetric or by two circles thanks to an intermediate pose q I i the poses q and q are not symmetric. The set o poses q I is deined by the equations (9) and (10) and is a line or a circle o center C and o radius R as deined in the previous section. I such a created path is admissible and collision ree, the trajectory is used. Otherwise, the solution presented in 5.4 is implemented. In the sequel, we study the conditions o admissibility or the created trajectory. I the poses q and q are symmetric, the circle arc linking these poses is easily deined: its center is the intersection o the normals to the poses q and q. I the poses q and q are not symmetric, a subset o intermediate poses q I has to be ound (with I and I ), such as R R min and R R min, with R and R the radiuses o respectively the circle linking q and q I and the circle linking q I and q. To ind this subset, a new rame centered in C with the x-axis oriented along the vector Cis deined (Fig. 11). In this new rame, the coordinates o the points, and I are given in unction o the angle λ=ĉi and we have: x Rcos y Rsin xi R cos I yi Rsin R x R 0 y Rsin cos (13) with λ =acos(1-r²/(r²)), λ =0 and λє] λ, λ [ and r deined as in the section The equation o the normal to the poses q, q and q I are then respectively: D D D I x ( ) y x ( ) y x ( ) y D DI D D DI D R R R R cos( ) cos( ) sin( ) sin( ) cos( ) cos( ) sin( ) sin( ) R cos( ) cos( ) R sin( ) sin( ) where the parameters τ, τ, τ are real numbers. (14) Fig. 10 Variation o the maximal curvatures o the CC sequences in unction o the intermediary pose Fig. 11 Solution based on two circle arcs Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 111

6 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) In the sequel, as all the ormulas or the calculations or respectively irst the points and I and then the points and I are o the same shape, the ormulas are written in unction o, where is to be replaced respectively by or. The intersection C I o D and D I is calculated. The point C I represents the center o the circle o radius R and joining the points and I. y writing D ( ) D ( ), the point C I and the values sin and are ound. The value is: cos cos sin I (15) cos sin with tan( ). The problem is to ind all the radiuses R such as R R min and we have R R. Consequently, to solve the problem it is suicient to study the variations o R in unction o λ and to solve the equations (16) and (17): R Rmin 0 (16) R Rmin 0 (17) Proceeding by implications, two solutions are ound or Eq. (16): g 1 g1² e1 ² 1² (18) e1 tan e1 1 with: e1 Rsin( ) cos( ) 1 R Rmin cos( ) Rmin sin( ) g1 R Rmin sin( ) Rmin cos( ) The unique solution is then ound by testing the two values o λ. With the same method, the Eq. (17) is solved and the unique solution is to ind by testing the two solutions o the implications: g g ² e ² ² e tan (19) e with: e Rsin( ) cos( ) R Rmin cos( ) Rmin sin( ) g R Rmin sin( ) Rmin cos( ) The study o the variations o the unctions R (λ) and R (λ) gives the ollowing--g results in, with, 0, : - R (λ) increases i zero or one solution o the equations (16) and (17) is in this range. It increases and then decreases i both the solutions o the equations (16) and (17) are in this range. - R (λ) decreases in this range. Thanks to the study o the unctions variations R (λ) and R (λ), and, the solution o the Eq. (16) and (17), we ound a range, 1 (it may be null or open i 1 or ) in which R and R are admissible or the vehicle. I this range is empty, the solution described in the next section is used. I it is not empty, the λ or the pose q I can be chosen, with one o the radiuses R or R being the biggest possible. regeneration o the trajectory has to be perormed or the next maneuver. The orward or backward move has to be done with as much steering as possible Simulation In this section, we illustrate a situation when the regeneration is needed with Matlab simulation results. To consider that a parking is correct, there is no need or the vehicle to achieve the exactly planned pose. We assume that a inal pose is correct as long as the vehicle is entirely inside the spot. The initial conditions and the parameters o the vehicle or the simulation are presented in the Table 1. The Fig. 1 presents a case o a parking in 3 maneuvers with a perect ollowing o the initially planned path. To simulate perturbations during a real parking, we calculate the perect trajectory and the perect inal pose or each maneuver and then add random perturbations to this inal pose. The Fig. 13 and Fig.14 present parking with the same initiall random perturbation. In the Fig. 13, there is no regeneration o the trajectory. Consequently, at the end o each maneuver, even i the inal pose is not the whished one, the vehicle continues to execute the control signals as it was initially planned, which leads to an incorrect parking. In the Fig. 14, the regeneration o the trajectory is used, which allows the vehicle to adapt the path without collisions and to achieve a correct inal pose. Table 1 Parameters o the vehicle and initial conditions Parameters Value Initial Conditions Value a 588 mm Length parking spot 6,31 m b 1511 mm Width parking spot,3 m d ront 839 mm x init 7,5 m d rear 657 mm y init 4 m d r =d l 130 mm φ init 0 δ max 33 Fig. 1 Perect parking in 3 maneuvers 5.4. Solution with a orward or backward move I the previous methods do not provide any admissible and collision ree-path, a simple orward or backward move with maximum steering is perormed until the vehicle reaches the ront or back obstacle. ter this maneuver, as the vehicle doesn t have the pose calculated by the path planning, the routine o the Fig. 13 Parking with perturbations without regeneration Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 11

7 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) Finally, the supervisor has to calculate the pose o the vehicle and o the parking spot in a local map rom the inormation provided by the units. It also has to make decisions about the methods or the path calculations or the regeneration i the spot is big enough to initiate a parking. It has to decide when to stop the maneuvers (end or emergency stop) and ensure the global diagnostic o the system. 6.. Proceedings o the parking maneuver In this section we describe the proceedings o the automatic parking. The pseudo-code or this algorithm is in the Fig. 16. Fig. 14 Parking with perturbations with regeneration 6. Vehicle Parking Maneuver with Trajectory Regeneration The previous sections presented the path planning and regeneration methods based on the environment data and the geometry o the vehicle. Moreover, control speed and steering signals have been presented. To be integrated into the vehicle as a parking system, these modules have to be linked in a more complete unctional architecture that will be presented in this section. Moreover, a supervisor has to make decisions about the maneuver, oversee its proceedings and provide to the modules the data they need Global unctional architecture In this section we reer to the Fig. 15. The parking system has 4 modules: the supervisor, the perception o the vehicle in the external world, the path planning and the control signals. The module o the vehicle in the external world gives to the supervisor the inormation about the size o the parking spot, its distance rom the vehicle and the obstacles. Moreover, the calculations o the travelled distance are also done in this module thanks to the proprioceptive sensors, or example with the speed o each wheel. The module o path planning calculates the initial path with the parameters imposed by the supervisor, or example i the maneuvers have to begin by a orward or a backward move, the choice o the method and o the curves. This module also calculates the regeneration o the trajectory when needed. The module o the control signals calculates the speed and steering signals at each time step and insures that these signals are executed by the actuators First step o the parking maneuver The irst step is to ind a suitable spot or the parking maneuver and might be restarted until such a spot is ound Then, when a spot is detected by the vehicle in the world module, all the inormation about this spot is sent to the supervisor. The supervisor determines the size o the spot and decides to keep this spot i its size is at least equal to a predetermined minimal size. I the spot is not kept, another spot has to be searched. I the spot is considered, the supervisor determines the pose o the vehicle with an optional data usion. Then, it chooses the methods and options or the initial path planning and selects the inormation to give to the path planning module, which generates an initial path and sends to the supervisor the parameters o the trajectory. terwards, the supervisor analyses these data (number o maneuvers, eventual collisions with the environment, overtaking on the lane) and decides to inally keep or not the spot and proceed to the parking maneuvers. I the spot is not kept, the irst step o the parking maneuver has to be restarted. Otherwise, the second step o the parking maneuver is carried out. While suitable spot == alse Vehicle in the world : environment Supervisor : size o spot I size o spot > minimal size Supervisor : pose o vehicle Supervisor : choice o methods Path planning : intial path Supervisor : decides i the initial path is suitable I initial path suitable reak While parking end == alse While speed >0 OR (speed == 0 D distance to travel or the current maneuver < travelled distance D emergency == alse) Vehicle in the world : obstacles, odometry, speed Supervisor : emergency? Control signals : new speed and steering Vehicle in the world : environment, pose Supervisor : size o spot Supervisor : end o parking? I parking end == alse Supervisor : regeneration? I regeneration needed Supervisor : choice o methods Path planning : new path Fig. 16 lgorithm o the parking maneuver Fig. 15 Global unctional architecture Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 113

8 Hélène Vorobieva et al./international Journal o utomotive Engineering 7 (016) Second step o the parking maneuver This step considers each maneuver one by one and consequently, it has to be restarted until the supervisor decides to end the parking. First, a loop is done until the vehicle stops or until the vehicle is stopped but the distance which had to be travelled or the considered maneuver has not been travelled yet and no emergency stop was imposed. During this loop, the module o the vehicle in the world gives to the supervisor, at each time step, the inormation about the proximity o the obstacles and the travelled distance. I the supervisor considers that the obstacles are too close, an emergency stop is triggered and a null speed signal is sent to the control module. Otherwise, the parameters needed or the calculations o the speed and steering signals are sent to the control module. When the vehicle stops because o an emergency stop or because the distance to travel or the considered maneuver is inished, the vehicle in the world module sends to the supervisor the inormation about the spot and the obstacles. Then, the supervisor determines again the size o the spot and the pose o the vehicle. terwards, the supervisor decides i the parking is ended. Two ends are possible: the vehicle is considered as well parked or the parking maneuvers have to be given up (or example i the sensors show a new obstacle which prevents a collision-ree parking). I the parking is not ended, the supervisor tests i a regeneration o the trajectory is needed. I positive, the supervisor sends to the path planning module all the needed data. Then the regeneration is calculated and the path planning module sends to the supervisor the new parameters o the path. The second step o the parking maneuver is then restarted until the parking is ended. 7. Conclusion lateral parking based on a continuous curvature trajectory in several maneuvers is presented. The trajectory generation starts with a geometric path based on circle arcs that is ormer transormed thanks to the clothoid curves to gain a continuous curvature path. n algorithm or the regeneration o the trajectory or the automatic parallel parking is urther proposed, or instance in case o important deviation or obstacles. The initial path generation and the regeneration are included in a global unctional architecture o the automatic parking. ext oreseen steps are to link this trajectory generation algorithms to the perception algorithms and to embed the calculus in a real control unit. Reerences (1). Gupta and R. Divekar, utonomous parallel parking methodology or ckerman conigured vehicles, in Proc. o Int. Con. on Control, Communication and Power Engineering, Chennai, India, July 010. () T. Inoue, M. Q. Dao, and K.-Z. Liu, Development o an autoparking system with physical limitation, in Proc. o SICE nnuel Con., Sapporo, Japan, ugust 004. (3) K. Jiang, D. Z. Zhang, and L. D. Seneviratne, parallel parking system or a car-like robot with sensor guidance, in Proc. o the Institution o Mechanical Engineers, Part C: Journal o Mechanical Engineering Science, (4) S. Choi, et al. Easy path planning and robust control or automatic parallel parking in tiny spots, in Proc. o IFC World Congress, Milano, Italy, ugust-september 011. (5) H. Vorobieva, et al., Geometric path planning or automatic parallel parking in tiny spots, in Proc. 13th IFC Symp. Control in Transportation Systems, Soia, ulgaria, September 01. (6). Mueller, J. Deutscher, and S. Grodde, Continuous curvature trajectory design and eedorward control or parking a car, in IEEE Transactions on Control Systems Technology, Vol. 15, o. 3, pp (007) (7) H. Vorobieva, et al., Geometric continuous- curvature path planning or automatic parallel parking, in Proc. o IEEE ICSC, Evry, France, pril 013. (8) T. Fraichard and. Scheuer, From Reeds and Shepp s to continuouscurvature paths, in IEEE Trans. Robotics, Vol. 0, o. 6, pp (004) (9). Scheuer and T. Fraichard, Collision-Free and Continuous- Curvature Path Planning or Car-Like Robots, in Proc. o IEEE ICR, lbuquerque, M, US, pril (10) P. onniait, P. ouron, P. Crubillé, and D. Meizel, Data usion o our abs sensors and gps or an enhanced localization o car-like vehicles, in Proc. o IEEE Int. Con on Robotics and utomation, Seoul, Korea, May 001. (11)K. Jiang, D. Z. Zhang, and L. D. Seneviratne, parallel parking system or a carlike robot with sensor guidance, Proc. o the Institution o Mechanical Engineers, Part C : Journal o Mechanical Engineering Science, (1) T.-H. Hsu, J.-F. Liu, P.-. Yu, W.-S. Lee, and J.-S. Hsu, Development o an automatic parking system or vehicle, in Proc. o IEEE Vehicle Power and Propulsion Con., Harbin, China, 008. (13) (01) SRR 0 /- /-C /-1 Short Range Radar. Continental. llemagne. [Online]. vailable : wnload/srr0x_datasheet_en.pd (ccess Date ). ccess date/web site only. (14) Y.-W. Ryu, S.-Y. Oh, and S.-Y. Kim, Robust automatic parking without odometry using an evolutionary uzzy logic controller, Int. Journal o Control, utomation, and Systems, Vol. 6, o. 3, pp ( 008) (15) H. Vorobieva, S. Glaser,. Minoiu-Enache, and S. Mammar, utomatic parallel parking with geometric continuous-curvature path planning, in Proc. IEEE Intelligent Vehicles Symp., Dearborn, MI, US, June 014. (16) D. Kim,. Song, Lateral Vehicle Control or Semi- utonomous Valet Parking with Consideration o ctuator Dynamics, Int. Journal o Control Science and Engineering, Vol., o. 6, pp (01) (17). Minoiu Enache, Trajectory Tracking Control by LMIbased pproach or Car-like Robots, in Proc. o 9 th Int. Con. on Inormatics in Control, utomation and Robotics, Vol., Rome, Itlay, pp (01) (18) V. Milanés, et al., Low-Speed Longitudinal Controllers or Mass-Produced Cars: Comparative Study, IEEE Transactions on Industrial Electronics, Vol. 59, o. 1, pp (01) (19) P. Ioannou and Z. u, Throttle nd rake Control Systems For utomatic Vehicle Following, Journal o Intelligent Transportation Systems, Vol.1, issue 4, pp (1994) (0) O. midi, Integrated mobile robot control, Robotic Insitiute, Carnegie Mellon University, Pittsburgh, Pa, US, Tech. Rep. Tech. report cmu-ri-tr-90-17, 1990 (1) H. Miyata, M. Ohki, Y. Yokouchi, and M. Ohkita, Control o the autonomous mobile robot dream-1 or a parallel parking, Mathematics and Computers in Simulation, Vol. 41, o. 1- (1996) Copyright 016 Society o utomotive Engineers o Japan, Inc. ll rights reserved 114