D Pérez-Morales

O Kermorgant

S Domínguez-Quijada

P Martinet

Multi-Sensor-Based Predictive Control

For Autonomous Parking David Pérez-Morales, Olivier Kermorgant, Salvador Domínguez-Quijada, and Philippe Martinet

Abstract-This paper formalizes, under a single common Multi-Sensor-Based Predictive Control framework, five different types of parking maneuvers: perpendicular, diagonal for both forward and backward motions and parallel for backward motions. Since, from a practical point of view, forward parallel parking is usually not advisable, it is not addressed in this work. By moving the effort from motion planning to control, the parking tasks can be completely defined solely from the detected empty parking spots. Additionally, the classical compromise between completeness and computational efficiency when compared to exploration-based path planning techniques is eliminated. The results of a few individual cases are presented and compared against a state of the art path planning approach to illustrate the behavior and performance of the proposed framework as well as results from exhaustive simulations to assess its convergence. As shown in the convergence analyses, the presented approach allows to park from virtually any sensible initial pose. Finally, real experimentation using a robotized Renault ZOE shows the validity and robustness in the convergence domain of the presented approach.

Index Terms-Intelligent vehicles, sensor-based control, model predictive control, motion control, intelligent parking system.

I. INTRODUCTION

A. Motivation and related work E VEN for experienced drivers, parking can be a difficult task, especially in big cities where the parking spots are often very narrow. The search for an increase in comfort and safety when parking has led to a quite extensive literature [START_REF] Wang | Automatic parking of vehicles: A review of literatures[END_REF], having explored many different approaches to automate this bothersome task.

Despite the fact that the automobile industry has already started to roll out some commercial parking assistants able to actively control acceleration , braking and steering [START_REF] Song | Analysis and Review of State-of-the-Art Automatic Parking Assist System[END_REF], the research interest in the topic remains strong.

Path planning approaches for automated vehicles have been heavily investigated in recent years [START_REF] Gonzalez | A Review of Motion Planning Techniques for Automated Vehicles[END_REF]. Among the different planning techniques relevant for parking applications it is possible to distinguish between geometric approaches, with either constant turning radius [START_REF] Petrov | Path Planning and Steering control for an Automatic Perpendicular Parking Assist System[END_REF], using saturated feedback controllers [START_REF] Petrov | Saturated Feedback Control for an Automated Parallel Parking Assist System[END_REF], or continuous-curvature planning using clothoids [START_REF] Vorobieva | Geometric Continuous-Curvature Path Planning for Automatic Parallel Parking[END_REF], [START_REF] Yi | Smooth path planning for autonomous parking system[END_REF]. Recent developments on sampling based planners using hybrid curvature steering functions based on clothoids [START_REF] Banzhaf | Hybrid curvature steer: A novel extend function for samplingbased nonholonomic motion planning in tight environments[END_REF] and using continuous curvature rate steering functions based on cubic spirals [START_REF] Banzhaf | From G 2 to G 3 Continuity: Continuous Curvature Rate Steering Functions for Sampling-Based Nonholonomic Motion Planning[END_REF] have been reported as well. Heuristic techniques [START_REF] Chen | Path planning with orientationaware space exploration guided heuristic search for autonomous parking and maneuvering[END_REF] and combination of sampling based planner with machine learning algorithms to guide the planner [START_REF] Banzhaf | Learning to Predict Ego-Vehicle Poses for Sampling-Based Nonholonomic Motion Planning[END_REF] can also be found in the literature, as well as optimizationbased refinement of the planned trajectories [START_REF] Zhang | Autonomous parking using optimization-based collision avoidance[END_REF], [START_REF] Zhang | Optimization-based collision avoidance[END_REF]. It is worth noting that parking maneuvers with forward motions are seldom considered, with [START_REF] Vorobieva | Automatic parallel parking in tiny spots: Path planning and control[END_REF] for the parallel parking case and [START_REF] Min | A control system for autonomous vehicle valet parking[END_REF] for the perpendicular case being some of the few works on this regard.

A well-known drawback of path planning is that it assumes that the whole environment is known beforehand. Also, the planned path must be tracked perfectly which might be difficult because of localization uncertainties (e.g. underground parking lots without any special infrastructure) or accumulated differences between the planned path and the performed one [START_REF] Vorobieva | Geometric Continuous-Curvature Path Planning for Automatic Parallel Parking[END_REF], [START_REF] Yi | Smooth path planning for autonomous parking system[END_REF]. In such situations, it might be necessary to replan the whole path which is not computionally efficient.

An interesting alternative is the use of a sensor-based control approach. It has been proven to be valid for navigation [START_REF] De Lima | Sensor-Based Control with Digital Maps Association for Global Navigation: A Real Application for Autonomous Vehicles[END_REF], dynamic obstacle avoidance [START_REF] Kang | Dynamic obstacles avoidance based on image-based dynamic window approach for humanvehicle interaction[END_REF] and for parking applications [START_REF] Perez-Morales | Laser-Based Control Law for Autonomous Parallel and Perpendicular Parking[END_REF]. It should be noted that an important limitation of a purely sensor-based control approach considering only a straightforward parking task (like in [START_REF] Perez-Morales | Laser-Based Control Law for Autonomous Parallel and Perpendicular Parking[END_REF]) is the possibility of getting trapped in local minima -i.e. if the car is not able to park in one maneuver from the initial pose then the parking maneuver won't be successful.

B. Reasoning and contribution

A natural goal for a human driver when parking would be to try to make the vehicle's longitudinal axis to be collinear to the main axis of the parking spot (i.e. to be centered lateral-wise) and finish the maneuver at a certain distance from either the rear or front boundary of the parking spot (respectively for backward or forward parking maneuvers) while avoiding collision with surrounding obstacles during the whole maneuver. However, depending of various factors (e.g. initial pose with respect to the parking spot, size of the vehicle, surrounding obstacles, etc.), often multiple maneuvers are required to park successfully. In such situations it would be necessary to perform motions that go against the final goal (i.e drive the vehicle away from the desired parked pose) but that in the end will allow to have a successful parking maneuver.

Assuming that the vehicle is capable of perceiving surrounding free parking spots, we will show that it is possible to park using a Multi-Sensor-Based Predictive Control (MSBPC) approach by minimizing the distance of a certain set of sensor features to its corresponding desired value while avoiding collision by imposing certain constraints on another set of sensor features along a given prediction horizon. The cost function should account for the main task that will actually park the vehicle and for the auxiliary task that drives the car away from the parking spot when needed. By exploiting the multi-sensor-based modeling, it is possible to express these two tasks by means of two (one for each task) pairs of 2D lines that can be easily extracted from the free parking spot. We highlight that since the presented approach is based on the features perceived at each time instant and a certain desired fixed value for each feature, no localization is inherently required for it to be stable in spite of the prediction step considered.

The contribution of this paper is the formalization of parking operations under a common MSBPC framework allowing the vehicle to park autonomously into perpendicular and diagonal parking spots with both forward and backward motions and into parallel ones with backward motions. Parking into parallel spots with forward motions was not considered since it is not an advisable maneuver in general, especially when it cannot be done in just one shot. By extending our previous work [START_REF] Perez-Morales | Laser-Based Control Law for Autonomous Parallel and Perpendicular Parking[END_REF], now considering an additional auxiliary subtask and a predictive approach, the presented technique is capable of performing multiple maneuvers (if necessary) in order to park successfully in constrained workspaces. The auxiliary subtask is key since it allows to account for the potential motions that go essentially against the final goal (i.e drive the vehicle away from the parking spot) but that in the end allow to park successfully.

C. Content of the paper

In the next section the kinematic model of the vehicle and the multi-sensor modeling are presented. Section III describes the interaction model allowing to formalize the parking tasks and the constraints for collision avoidance. Afterwards, the controller is presented in Section IV. The obtained results are presented in Section V-A: a few cases in simulation environments are presented and compared against a state of the art path planning approach, together with exhaustive simulations assessing the convergence performance of the presented approach. Additionally, Section V-B shows real experimentation with the car passing over a speed bump during the parking maneuver. Finally, some conclusions are given in Section VI

For the sake of clarity, for the equations and explanations presented in this work, the parking spot is considered to be always on the right side of the vehicle to park at the beginning of the maneuver II. MODELING AND NOTATION Given that parking maneuvers are low-speed motions, a kinematic model can be considered as accurate enough.

A. Car-like robot model and notation

Considering the well-known kinematic model of a car with rear-wheel driving [START_REF] De Luca | Feedback control of a nonholonomic car-like robot[END_REF], the vehicle's twist is defined by the following column vector (elements separated by a semicolon):

v m = [v; θ], (1)
where v and θ are, respectively the longitudinal (along x m ) and rotational velocities expressed in the moving base frame F m . Additionally, one can link the steering angle δ to θ using the following equation:

θ = v tan δ l wb . (2)
Therefore, it is possible to consider as control input of the robotized vehicle the following expression:

v r = [v; δ] (3)
Finally, the turning radius ρ m around the instantaneous center of rotation (ICR) can be defined as:

ρ m = l wb tan δ . (4)
It should be noted that, thanks to the multi-sensor-based formalism considered (introduced in the next subsection), our closed-loop control law does not need to have any knowledge about the Cartesian pose of the vehicle (x,y,θ).

The vehicle used for experimentation and simulation, represented by its bounding rectangle in Fig. 1a, is a Renault ZOE (Fig. 1b). Its relevant dimensional parameters are presented in Table I. Our approach relies on multi-sensor-based control [START_REF] Kermorgant | Dealing with constraints in sensorbased robot control[END_REF] in order to easily design the control law. In this framework, a robot is assumed to carry several sensors S i that give information about the environment. In this Section we recall the key concepts of this approach.

Each sensor is associated with a vector of sensor features s i of dimension d i . Typically, s i can be the raw sensor signal or any output of a processing algorithm. For instance, the features corresponding to a camera can be the full image or a parametrization of primitives of interest [START_REF] Chaumette | Visual servo control, part I : Basic Approaches[END_REF]. In practice, the choice of sensor features depends on the considered task and desired behavior of the system.

In a static environment, the sensor feature derivative can be expressed as follows:

ṡi = L i v i = L i T i v m (5)
where L i is the interaction matrix [START_REF] Chaumette | Visual servo control, part I : Basic Approaches[END_REF] of s i and T i is the screw transformation matrix that expresses the sensor twist v i (which is expressed in its corresponding frame F i ) with respect to the robot twist v m , expressed in the mobile frame F m which is used as control frame. Classically, sensor-based control considers full free-flying 3D motions (i.e. dim(L i ) = d i × 6 and dim(T i ) = 6 × 6). As we will see in the next Section, we rely on a simpler, planar model to express the motions of the sensors.

Sensor-based control is about designing a control law which induces the desired behavior in the feature space. This behavior can be twofold:

• Task features, simply denoted as s, are associated to some desired value s * . The control should in this case have s converge to s * . • Constrained features, now denoted as c, are associated to some bounds (upper, lower or both). The control should thus ensures that c is always within its bounds. We now detail how this approach is adapted to the planar case for autonomous parking.

C. Multi-sensor for autonomous parking

In order to perform autonomous parking, we rely on the perception of several points P a and lines L j that correspond to the vertices and edges of the quadrilateral defined by a given parking spot (or can be straightforwardly obtained from them). Figure 2 shows the convention to number the points and lines of interest. Additional points, for instance corresponding to other obstacles, can be easily considered with the same formalism.

The presented technique does not impose any specific type of sensor as long as the information related to the parking spot (i.e. its vertices and edges) to which the car should park can be extracted. For example, one could approximate a parking spot from the free space between two already parked cars [START_REF] Perez-Morales | Laser-Based Control Law for Autonomous Parallel and Perpendicular Parking[END_REF] or, if only one car (or none) was already parked, one could instead use the markings on the ground perceived by a vision system. If a precise enough map and localization system are available, another option would be to generate virtual sensory data from the relative pose between the vehicle and the known parking spots. Finally, one could use a combination of different sensor sources to get a more robust estimation of the parking spot and surrounding obstacles.

Regardless of the real sensory system of the autonomous vehicle, the extracted points and lines are expressed as if they were observed by several virtual sensors S i . As shown in Fig. 1c, these sensors are placed at convenient frames F i in order to simplify the sensor features definitions and their interaction matrices. Changing the reference frames of points and lines is a trivial operation that allows easily expressing the task and constrained features.

In our approach, sensors S 1 and S 2 are laterally centered and mainly used for the parking task. Sensors S 3 to S 6 are placed on the corners of the car's bounding rectangle (taking into account the side mirrors) with the purpose of collision avoidance with surrounding obstacles. S 7 and S 8 are used to prevent hitting the curb on parallel parking maneuvers thus are placed on the right side corners (with respect to the vehicle) of the dashed red rectangle whose width and length are equal to w b and l rw + l fw respectively.

We rely on a planar model hence all sensor frames are aligned with the vehicle frame F m and only differ from a position offset (x i , y i ). As a consequence, (5) is expressed with lower dimension dim(L i ) = d i × 3 and T i is defined as:

T i =    1 -y i 0 x i 0 1    (6)
In this planar case, T i thus expresses the induced planar twist of a sensor (of dimension 3), when a control v m (1) is applied to the vehicle. The interest of virtual sensors is first to make the modeling and control independant from the actual way the vehicle perceives the surroundings. It also makes easier to design the task and the constraints. For instance, a feature related to the parking task is to align the main axis L 1 with the x-axis of F 1 . Similarly, if the car is parking into perpendicular spot with a backward motion (Fig. 2a), the risk of collision with the obstacle on the left is the highest for the car's rear left corner. This distance is easily expressed by how sensor S 6 sees line L 3 .

In the next Section we introduce the interaction model of the considered points and lines features.

III. INTERACTION MODEL

In this Section we express how lines and point features are parameterized, and derived their interaction matrix. We also detail which features are used to describe the parking task and the collision avoidance constraints.

A. Line parametrization

We denote i P j and i L j the coordinates of P j and L j expressed in F i . A line i L j passing through two distinct 3D points i P 1 and i P 2 can be represented using Plücker coordinates as a couple of 3-vectors [START_REF] Přibyl | Camera Pose Estimation from Lines using Plücker Coordinates[END_REF], expressed in F i :

i u j = [ i X 2 ; i Y 2 ; i Z 2 ] -[ i X 1 ; i Y 1 ; i Z 1 ] = 0 i r j = [ i X 1 ; i Y 1 ; i Z 1 ] × [ i X 2 ; i Y 2 ; i Z 2 ] (7)
This work relies on normalized coordinates [START_REF] Andreff | Visual Servoing from Lines[END_REF] expressed as:

i u j = i u j /|| i u j || i h j = i r j /|| i u j || (8)
From the planar world assumption (Z = 0), only 3 components in [START_REF] Banzhaf | Hybrid curvature steer: A novel extend function for samplingbased nonholonomic motion planning in tight environments[END_REF] are non-null. They are:

• The first two components of i u j , that correspond to the cosine and sine of the line orientation, • The last component of i h j , that is the signed distance from the origin to the line. In the sequel, a line is thus parameterized from these 3 values denoted as:

i l j = i u j,1 ; i u j,2 ; i h j , (9)
The corresponding interaction matrix L i,j can be expressed as [START_REF] Andreff | Visual Servoing from Lines[END_REF]:

L i,j =    0 0 i u j,2 0 0 -i u j,1 -i u j,2 i u j,1 0    (10)

B. Parking spots models

As it can be seen in Figs. 2a-2c, points P 1 to P 4 correspond to the corners of the parking spot while P 5 and P 6 are, respectively, the midpoints between (P 1 , P 4 ) and (P 2 , P 3 ). Line L 1 is placed along the main axis of the parking spot while lines L 2 to L 4 are placed around the edges of the parking spot, leaving one side open from where the vehicle to park can enter the spot (denoted by L 5 ). Line L 2 corresponds to the rear and front boundary of the parking spot, respectively, for backward and forward maneuvers. Lines L 3 and L 4 correspond to the lateral boundaries on the non-parallel cases. For the parallel case, L 3 corresponds to the right side lateral boundary while L 4 corresponds to the front boundary of the parking spot. Additionally, L off 1 , L off 4 and L off 5 are simply offsets of, respectively, L 1 , L 4 and L 5 away from the parking spot.

The pair of points through which each line passes are defined in Table II.

C. Task sensor features

The set of task sensor features s corresponding to the positioning is composed of two opposing tasks subsets: the main task s p (used to actually drive the car into the parking spot) and the auxiliary task s a (used to drive the car away from the parking spot). Both s p and s a are composed from two line features:

• s p is composed with lines L 1 and L 2 , that describe the pose of the parking spot • s a is composed with lines L off 5 and L off 1 (parallel maneuvers) or L off 4 (non-parallel), that describe where to go when stuck during parking Table III lists the frames where these lines are expressed, depending on the parking configuration.

The next Section details the constrained sensor features, used to model the surrounding obstacles.

D. Constrained sensor features

Let us consider Fig. 3 as an example of a parking environment in the vicinity of a straight road with the road being labeled as transitable area. On the lower section of the figure one can see a row of parking spots. Among these parking

Maneuver type

Main task Auxiliary task spots, the chosen one to park the vehicle is denoted by a green rectangle and its boundaries are denoted by yellow lines leaving one side open from where the vehicle can enter the spot. The sections colored in red denote forbidden zones, i.e. areas that the vehicle should never go into. These forbidden zones may be comprised of other parking spots which may or may not be already occupied (like those on the sides of the selected parking spot and on the top section of the figure) as well as walls, sidewalks, bushes, etc., and thus not actually being part of the road anymore. Given that for parking scenarios one can often expect to have static obstacles in the red regions of Fig. 3, it is possible to avoid collision by constraining certain sensor features such that the vehicle is able to move only inside the transitable area and the chosen parking spot. Assuming a straight road, the necessary constraints can be defined considering only sensor features related to the parking spot as shown in this section.

Backward non-parallel sp = [ 2 l 1 ; 2 l 2 ] sa = [ 1 l 1 off ; 1 l 5 off ] Forward non-parallel sp = [ 1 l 1 ; 1 l 2 ] sa = [ 2 l 1 off ; 2 l 5 off ] Backward parallel sp = [ 2 l 1 ; 2 l 2 ] sa = [ 1 l 4 off ; 1 l 5 off ]
To define the constraints, three other types of features are considered in addition to i L j : the coordinates of the entry corner points P a of the parking spot, and signed differences of distances to the ICR. With the ICR denoted C, the distances of interest are denoted as:

     ρ lat = |ρ m |- w ve 2 Lateral distance to the ICR ρ i = S i C Distance from S i to the ICR ρ Pa = P a C
Distance from P a to the ICR [START_REF] Banzhaf | Learning to Predict Ego-Vehicle Poses for Sampling-Based Nonholonomic Motion Planning[END_REF] Similarly, the differences of interest are defined as:

ρ lat,a = ρ Pa -ρ lat ρ i,a = ρ i -ρ Pa ( 12
)
Figure 4 shows the geometrical interpretation [START_REF] Banzhaf | Learning to Predict Ego-Vehicle Poses for Sampling-Based Nonholonomic Motion Planning[END_REF] and [START_REF] Zhang | Autonomous parking using optimization-based collision avoidance[END_REF]. The interaction matrices related to ( 11) and ( 12) can be easily computed from the classical 3D interaction matrix of a

point ( i X, i Y ) expressed in F i [21]: L i X = -1 0 i Y L i Y = 0 -1 -i X ( 13
)
The set of constrained sensor features used for collision avoidance is denoted as c. Unlike s, due to the nature of each type of parking maneuver, the definition of the set of sensor features c required for collision avoidance is different for each parking case although, in general, constraints on i h j and X a are used to avoid going outside the boundaries of the parking spot or into the forbidden areas (Fig. 3), constraints on [START_REF] Zhang | Autonomous parking using optimization-based collision avoidance[END_REF] are used to keep the vehicle's path away from the obstacles and constraints on Y a are used to keep a certain minimum lateral distance to point P a .

The exact definition of the set of constrained sensor features c is now given case by case.

Backward non-parallel case: Considering that for this type of maneuvers the rear side of the vehicle has to enter first into the parking spot, the majority of the constrained sensor features should be observed by the sensors placed at the rear corners of the vehicle, thus c is defined as follows:

c = [c 3 ; c 4 ; c 5 ; c 6 ] (dim. 14) ( 14
)
with the ten constrained features perceived by the two rear sensors being:

c 3 = [ 3 h 2 ; 3 h 4 ; 3 h 5 ; 3 X 2 ; 3 Y 2 ; ρ lat,2 ], (15a)
c 6 = [ 6 h 2 ; 6 h 3 ; 6 h 5 ; 6 X 3 ], (15b)
and the four ones perceived by the two front sensors being:

c 4 = [ 4 h 4 ; 4 h 5 ], (16a)
c 5 = [ 5 h 3 ; 5 h 5 ]. (16b)
Forward non-parallel case: Analogously to the precedent case, given that now the front side of the vehicle should enter first into the parking spot, the majority of the constrained sensor features should be observed by the sensors placed at the front corners of the vehicle, thus c is defined as follows:

c = [c 3 ; c 4 ; c 5 ; c 6 ] (dim. 13) (17)
with the features perceived by the rear sensors being:

c 3 = 3 h 5 , (18a)
c 6 = 6 h 5 , (18b)
and those perceived by the front sensors being:

c 4 = [ 4 h 2 ; 4 h 3 ; 4 h 5 ; 4 X 3 ; 4 Y 3 ], (19a)
c 5 = [ 5 h 2 ; 5 h 4 ; 5 h 5 ; 5 X 2 ; 5 Y 2 ; ρ 5,2 ]. (19b)
Backward parallel case: Considering that for this case the vehicle is likely to maneuver while being mostly inside of the parking spot, it should be expected to have more constrained features than for the two previous cases. Additionally, even if the rear side of the vehicle should enter first into the parking spot, the greater risk of collision is on its right side thus the majority of the constrained sensor features should be observed by the sensors placed at the right corners of the car. Moreover, given that this type of parking maneuvers are typically performed on streets rather than in parking lots, the boundary i L 3 would often coincide with a curb. Therefore, c is defined as follows:

c = [c 3 ; c 4 ; c 5 ; c 6 ; c 7 ; c 8 ] (dim. 18) (20)
with the features perceived by the sensors placed on the right side corners being:

c 3 = [ 3 h 2 ; 3 h 5 ; 3 X 3 ; 3 Y 3 ; ρ lat,3 ; 3 X 4 ; 3 Y 4 ], (21a)
c 4 = [ 4 h 4 ; 4 h 5 ; 4 X 3 ; 4 Y 3 ; ρ 4,3 ], (21b)
those perceived from the left side corners being:

c 5 = [ 5 h 4 ; 5 h 5 ], (22a)
c 6 = [ 6 h 2 ; 6 h 5 ], (22b)
and the ones used to avoid hitting the curb being:

c 7 = 7 h 3 , (23a)
c 8 = 8 h 3 .
(23b) Constraints deactivation: It should be noted that some constraints must be deactivated under certain conditions in order to be able to park successfully. As an example, considering a forward parallel parking maneuver, the constrained features 4 Y 3 and 3 d lat3 used to keep i p 3 at a certain distance on the right side of the car should only be active as long as 3 p 3 is not behind the vehicle, otherwise they would prevent a successful parking maneuver since at the desired pose i p 3 should be on the left side of the car. The constraints deactivation conditions used to obtain the results presented in this work are detailed in Appendix A.

IV. CONTROL

The presented MSBPC approach is based on the Visual Predictive Control (VPC) described in [START_REF] Allibert | Predictive Control for Constrained Image-Based Visual Servoing[END_REF] with some modifications to impose an exponential decay of error related to the main task e p = s ps * p , where the superscript * indicates the desired value. Furthermore, considerations have been made to deal with the nonholonomic constraints inherent to car-like robots in addition to some other constraints (most of them unilateral). 5. Control structure [START_REF] Allibert | Predictive Control for Constrained Image-Based Visual Servoing[END_REF] The control structure is based on the internal-model-control (IMC) structure [START_REF] Morari | Robust Process Control[END_REF] (Fig. 5). This allows compensating for modeling error and disturbances which also helps the optimization (MPC) block to converge faster. The System block contains the robotized vehicle system and sensors whose input is the control variable v r and outputs s, c are the current values of the sensor features. The reference s * is the desired value of the task sensor features s while c ± are the constraints to be imposed on c. The error signals ε s , ε c represent all the modeling errors and disturbances between the current features (s, c) and the values that were predicted from the model (s mp , c mp ):

A. Structure - + s * (n) c ± * (n) - + s d (n) c ± (n) Optimization System v r (n) Model + - s mp (n) c mp (n) s mp (n) c mp (n) ε s (n) ε c (n) s(n) c(n) Fig.
ε s (n) = s(n) -s mp (n) (24a) ε c (n) = c(n) -c mp (n) ( 24b
)
where n is the current time.

The optimization algorithm minimizes the difference between the desired value s d and the predicted model output s mp . According to Fig. 5: [START_REF] Morari | Robust Process Control[END_REF] from where it is possible to deduce

s d (n) = s * (n) -ε(n) = s * (n) -(s(n) -s mp (n)),
s d (n) -s mp (n) = s * (n) -s(n), (26)
therefore, to track s * by s is equivalent to track s d by s mp .

To predict the behavior of s mp and c mp over a finite prediction horizon N p , the interaction model described in Sec. III is used. The difference between s d and s mp is used to define a cost function J to be minimized with respect to a control sequence ṽr over N p . As with any classical MPC strategy, once a solution to J has been found, the first component v r (n) of the optimal control sequence is applied to the vehicle at each iteration.

B. Constraint handling

Model-predictive-control strategies are designed to explicitly take into account arbitrary constraints in the control-law design. As such, in this subsection we present the imposed constraints.

The longitudinal velocity v and steering angle δ are bounded by its maximum values as follows:

|v| < v max , (27a) |δ| < δ max , (27b)
where v max is an adaptive saturation value imposing a deceleration profile based on the velocity profile shown in [START_REF] Petrov | Saturated Feedback Control for an Automated Parallel Parking Assist System[END_REF] as the vehicle approaches the final pose. Furthermore, to avoid large changes in the control signals at the current iteration n that may cause uncomfortable sensations for the passengers or surrounding witnesses and, to consider to some extent the dynamic limitations of the vehicle, the following constraints are considered:

| v| ≤ vmax |v| ≤ vmax      | δ| ≤ δmax | δ| ≤ δmax | ... δ | ≤ ... δ max (28)
The sensor features c considered for collision avoidance are constrained as follows:

c -≤ c mp ≤ c + ( 29
)
where the superscripts -and + , respectively, indicate the low and high sides of the constraints to be imposed. The numerical values of this constraints can be found in Tables VII -IX that shows most of the features are constrained only on one side. A constraint domain C can be defined for v r = [v; δ] (3):

C = {v r , C(v r ) ≤ 0} (30)
where C(v r ) gathers all inequalities from ( 28) and [START_REF] Pérez-Morales | Multi-Sensor-Based Predictive Control for Autonomous Backward Perpendicular and Diagonal Parking[END_REF].

C. Mathematical formulation

The MSBPC approach can be written in discrete time as follows:

min

J(v r ) ṽr ∈ C (31)
with

J(v r ) = n+Np j=n+1 [s d (j) -s mp (j)] T Q(j) [s d (j) -s mp (j)] + v m (j -1) T R(j -1) v m (j -1) (32)
and ṽr = {v r (n), v r (n+1), . . . , v r (n+N c ), . . . , v r (n+N p -1)} (33) where (s mp (j), c mp (j)) are predicted from the first-order model [START_REF] Petrov | Saturated Feedback Control for an Automated Parallel Parking Assist System[END_REF].

It should be noted that, from v r (n+N c ) to v r (n+N p -1), the control input is constant and is equal to v r (n+N c ), where N c is the control horizon.

Due to the constrained nonlinear optimization, proving stability for non-linear model predictive controllers is rather difficult [START_REF] Findeisen | An Introduction to Nonlinear Model Predictive Control[END_REF], particularly for a finite N p . As for sensor-based techniques, only local asymptotic stability can be demonstrated [START_REF] Chaumette | Visual servo control, part I : Basic Approaches[END_REF] when sensor data is kept in the sensor space. Nonetheless, it has been shown that the local asymptotic stability property is kept for the optimal MSBPC considered [START_REF] Allibert | Visual Servoing via Nonlinear Predictive Control[END_REF].

Additionally, it has been shown that a local model based on the interaction matrix (such as the one we consider) is enough to predict the evolution of the sensor features and thus allows performing sensor-based tasks successfully while satisfying the considered constraints [START_REF] Allibert | Predictive Control for Constrained Image-Based Visual Servoing[END_REF], [START_REF] Allibert | Visual Servoing via Nonlinear Predictive Control[END_REF]. We illustrate the performance of our approach in Sec. V.

1) Parking strategy: Considering the definition of i L 1 and i L 2 (Sec. III-B), a sensible choice would be for i L * 1 to be collinear with the vehicle's longitudinal axis (x m -axis) and i L * 2 to be parallel to y m -axis at a safe distance from either the rear or front boundary of the vehicle for, respectively, backward and forward maneuvers.

Moreover, since the objective of the auxiliary task is to pull the vehicle away from the parking spot, the desired values a L off * q and a L off * 5 are chosen to be collinear with the x m -axis. The conflicting goals for the orientations of these features are exploited by adapting their influence in function of the current sensor features' values. The consistency of the complete task is ensured by means of the weighting approach introduced in the next subsubsection. Nonetheless, the actual desired values for the offset lines may vary (but remain constant during the parking maneuver) depending on the characteristics of the vehicle (e.g. l wb , δ max ), collision-avoidance constraints (e.g. size of the parking spot, type of maneuver) and weighting strategy, although in general they should be defined such that the vehicle reaches a favorable unparked pose.

Furthermore, using only the features introduced in Sec. III-B, it is possible to reconstruct what the desired parked/unparked poses of the vehicle would be and thus what desired value the sensor features would have. To this goal, one can simply define some auxiliary parked/unparked frames with respect to the parking spot and express the desired lines and points in the aforementioned frames.

The interaction matrix L p for the features observed by S p is computed by a 2nd order approximation [START_REF] Thari | On the efficient second order minimization and image-based visual servoing[END_REF] of the form:

L = L L + L * L 2 (34)
where

L L = [L i,1 ; L i,2
] and L * L is equal to the value of L L at the desired pose. As it has been shown in our previous work [START_REF] Perez-Morales | Laser-Based Control Law for Autonomous Parallel and Perpendicular Parking[END_REF], [START_REF] Pérez-Morales | Multi-Sensor-Based Predictive Control for Autonomous Backward Perpendicular and Diagonal Parking[END_REF], the use of this type of interaction matrix (for the parking task) in the control architecture induces a rather interesting and useful behavior: whenever there is a large error in orientation and positioning (regarding i L 1 for the latter) the vehicle steers away from the parking spot initially giving itself more room to afterwards steer into the parking spot. Since this behavior is not as interesting for the auxiliary task and for simplicity reasons, the interaction matrix L a for the features observed by S a is computed at each iteration and is defined by [START_REF] Chen | Path planning with orientationaware space exploration guided heuristic search for autonomous parking and maneuvering[END_REF].

2) Weighting strategy: As mentioned in Sec.III-C, s p contains the features used to actually drive the car into the parking spot, thus the minimization of the error e p = s ps * p is what drives the vehicle towards a parked pose, typically with a backward motion for backward maneuvers and forward ones for the forward case, while e a = s as * a would be normally minimized with opposite directions of motion. This enables automatic multi-maneuver, that is online switching of the moving direction, without any need for planning or replanning. In order to automatically maneuver the vehicle with the appropriate direction of motion, the influence of each sensor feature is regulated by means of the weighting matrix Q, which remains constant along the prediction horizon. It is defined as:

Q = Q p W p 0 6×6 0 6×6 Q a W a (35)
where:

• Q p is the global adaptive gain, taking into account the speed of the vehicle with regards to the vicinity of the constraints • Q a is the auxiliary adaptive gain, allowing the unparking maneuver • W p = diag(w 1 , . . . , w 6 ) balances the importance of the 6 parking features s p • W a = diag(w 7 , . . . , w 12 ) balances the importance of the 6 unparking features s a The auxiliary gain Q a activates the unparking maneuver. As such, it is defined as:

Q a = 0 if p l 1 -p l * 1 < L1 and Q p > 0 1 -Q p otherwise ( 36
)
where L1 is a small positive scalar value that serves to nullify Q a (and consequently the influence of s a ) when the vehicle is almost collinear to p L 1 (parking spot main axis). The values of Q p and w i∈ [START_REF] Wang | Automatic parking of vehicles: A review of literatures[END_REF][START_REF] Zhang | Autonomous parking using optimization-based collision avoidance[END_REF] are are computed from a combination of the same elementary function denoted w in the sequel.

Due to the different nature of each of the considered cases, the exact definition of (Q p , w i∈ [START_REF] Wang | Automatic parking of vehicles: A review of literatures[END_REF][START_REF] Zhang | Autonomous parking using optimization-based collision avoidance[END_REF] ) varies slightly depending on the type of maneuver although their structure remains similar. We now detail the rationale behind the adaptive weighting for the non-parallel backward case. Appendix B exposes the shape of the w function and goes further into numerical implementation for all parking configurations. Let us consider a boolean variable fwd that serves as a hint of whether the vehicle should move forward or not. If the vehicle is almost collinear to p L 1 (i.e. p l 1 -p l * 1 < L1 ) or near the desired parked pose (i.e. e p < p ), fwd is set to false. The vehicle is considered to be at (or in the vicinity of) a local minimum if e p ≥ p (i.e. non-null error) while |v| is below a given threshold (i.e. null output velocity). In this case, fwd is flipped and the control automatically switches to the opposite maneuver.

The procedure for computing the weight multiplier Q p at each iteration depends on fwd as shown in Algorithm 1. The idea is to drive the vehicle typically with a backward motion if it starts to move while being on the right side of i L 1 and with a forward motion if the vehicle starts on the left side of said line or if a local minimum had been detected when moving in reverse, generally not driving farther away than i L off 1 . Regarding W p , its role is to prioritize the error in position by letting the orientation (mostly) free for the most part of the maneuver and, as the current orientation approaches to the desired one, smoothly change the priority from position to orientation. As such, w 3 and w 6 can be set to constant values while the remaining diagonal elements of W p can be defined as:

w i = w(e 6 ) ∀i ∈ {1, 2, 4, 5}. (37)
As for W a , its role is to induce small corrective motions if the vehicle is relatively close to be collinear to the main axis of the parking spot and otherwise try to drive the vehicle away from the parking spot towards a more suitable unparked position as a sort of prepositioning/maneuver-restarting mechanism. Therefore, the diagonal elements of W a are given by: w 7 = w 8 = w( 5 h 5 , 6 h 5 , max (w(e 10 ), w(e 11 ))) (38d)

w( m h 1 ) w( 1 h 1 , max (w(e 7 ), w(e 8 ))), (38a)
3) Imposing an exponential decay of the error: The inclusion of the term related to v m as part of the cost function J allows to impose an exponential decay on the error which additionally improves the stability of the controller. Nevertheless, this behavior (particularly regarding the longitudinal velocity) is mostly only desired for the main parking task. For this purpose, the influence of v and θ are regulated by means of the weighted matrix R, which remains constant along the prediction horizon. It is defined as:

R = λ v Q p 0 0 1 . ( 39
)
where λ v is a constant gain that serves to regulate how fast the vehicle decelerates when approaching the desired parked pose.

V. RESULTS

For the results shown in this section, the parameters in Table IV are corresponds to the maximum steering angle of the real vehicle while the rest of the parameters required to solve (31) were determined by empirical testing, nevertheless some guidelines on how to tune them can be given:

• The maximum longitudinal velocity v max and the maximum values of the constraints (28) should be large enough so that the vehicle can park in a reasonable amount of time (without a feeling of sluggishness) but not so large that the passengers and surrounding witnesses feel unease during the maneuver. • A larger control horizon N c allows the system to maneuver the vehicle more freely at the expense of a larger computation effort.

• N p should be large enough so that a collision-free motion can be guaranteed but small enough to be able to meet the computational time requirements. Additionally, it is known that if N p tends to infinity, the control problem becomes an optimal control and thus closed-loop stability is ensured [START_REF] Findeisen | An Introduction to Nonlinear Model Predictive Control[END_REF]. • The threshold value L1 used to determine whether or not Q a should be equal to zero has influence on the total number of maneuvers required to park and on the convergence of the controller. In general, a smaller value of L1 enforces a smaller final error at the expense of a potential increase on the number of maneuvers required to park.

A. Simulation results

The simulations carried out in MATLAB use the fmincon solver with a Sequential Quadratic Programming (SQP) algorithm. Longitudinal velocity gain λv 0.1

1) Individual cases -MATLAB simulations:

To illustrate the performance of the proposed approach, a few examples of each type of parking situation are shown below. The initial position is marked with a colored (yellow and light green in the shown cases) square whose color depends on the final value of ||e p || as the associated colorbar indicates. The initial and final bounding rectangles of the vehicle are colored in magenta and cyan respectively.

In addition to the parking maneuver evolution, the evolution of the different control-related signals can be seen in Fig. 6. Thanks to the constraints related to the controls signals, they evolve smoothly in general (Fig. 6c). From the constraints values defined in VII, it can bee seen that the (active) constraints imposed on c are satisfied at each time instant (Fig. 6f) ensuring a collision-free maneuver. It can be seen how the main task error (e p = s ps * p ) (Fig. 6b) is minimized as the vehicle moves backwards while e a (= s as * a ) (Fig. 6d) is minimized with motions in the opposite direction. Additionally, one can notice that when Q p is larger than Q a (Fig. 6e), the vehicle is moving backwards and when Q a is larger the opposite occurs. Regarding the main task weights, it can be seen how the elements related to the orientation have no influence during the most part of the maneuver and only when the vehicle is close to the desired pose the priority changes from position to orientation. As for the auxiliary task weights, it can be seen how a small influence of s 9 pulls the vehicle out of the parking spot while the weights related to s 7 and s 8 try to keep the vehicle parallel to main axis of the parking spot.

Two more backward perpendicular cases can be seen in Fig. 7. Fig. 7a shows a case where parking with a single maneuver is possible while Fig. 7b presents a rather challenging initial pose from where the vehicle has to perform four maneuvers in order to park. For the three backward perpendicular cases shown, the final value of ||e p || is rather small (< 0.0082).

The initial poses of the cases presented in Figs. 8a and 8b are, respectively, the same as those in Figs. 6a and7b. It can be seen how, as expected, when considering diagonal parking spots (Fig. 8) instead of perpendicular ones the complexity of the maneuvers is reduced. The case shown in Fig. 8a still requires only one maneuver but now there are much larger portions of the maneuver of mostly straight motions. As for the case presented in Fig. 8b, the number of maneuvers was reduced to only two compared to four for the perpendicular case.

As most experienced drivers know, parking maneuvers with forward motions are usually more challenging than their backward motions counterparts thus a potential increase on the number of required maneuvers should not be surprising. The two non-parallel forward cases shown in Fig. 9 share the same initial pose. On the one hand, for the diagonal case (Fig. 9a) parking with a single maneuver is possible with a final ||e 1 || = 0.0125. On the other hand, for the perpendicular case shown in Fig. 9b the vehicle has to perform five maneuvers (four changes of direction), achieving a final ||e 1 || = 0.0345. Even if the final error of the forward perpendicular case is considerably larger than the previous cases (around three times larger), it remains at a reasonable value, especially for such a challenging case.

Finally two backward parallel cases are shown in Fig. 10. In Fig. 10a, the vehicle starts the parking maneuver by moving forward in order to preposition itself (similarly to Fig. 7b) and then performs three more motions leading to a total of four maneuvers to park with a final ||e 2 || = 0.0182. For Fig. 10b no prepositioning is required and thus the car is able to park with only three maneuvers and a final ||e 2 || = 0.0197.

2) Comparison against a state of the art path planning approach: To put in perspective the performance of the proposed MSBPC approach, it is compared against a state of the art path planning approach [8] using a sampling-based motion planner and a Hybrid Curvature (HC) steer function. To implement said approach, the steering functions package published by the authors was interfaced with the Open Motion Planning Library (OMPL) [START_REF] Sucan | The Open Motion Planning Library[END_REF] using its RRT * implementation for the planner. To account for the different planner (BiRRT * in [START_REF] Banzhaf | Hybrid curvature steer: A novel extend function for samplingbased nonholonomic motion planning in tight environments[END_REF] and RRT * in our implementation) the maximum planning time was set to 20 s instead of 6 s as presented in [START_REF] Banzhaf | Hybrid curvature steer: A novel extend function for samplingbased nonholonomic motion planning in tight environments[END_REF]. It was chosen for the HC paths to have zero curvature at the beginning and at the end (denoted as HC 00 ). The path planning results shown below were obtained on an AMD Ryzen 7 2700X at 4.15 GHz. The initial poses of the cases presented in Figs. 11, 13 and 12 are, respectively, the same as in Figs. 6a, 7a and 7b. All of them where tested in analogous simulated environments. Two different outcomes for each initial pose of the planned cases are shown: a desirable one (Figs. 11a, 12a and13a) and an undesirable one (Figs. 11b, 12b and 13b). As one could expect, due to the planner's randomness, the planned paths can be completely different each time the planning is performed. For this reason, achieving repeatable parking maneuvers with RRT-based planners is not straightforward, especially for short planning times.

As shown in Fig. 11 the planned path could be rather good (Fig. 11a), leading to a visibly shorter path length than our MSBPC approch (Fig. 6a) but could also be completely undesirable (Fig. 11b). When considering the case where parking in a single maneuver is possible for our approach (Fig. 7a), one can notice that the desirable planned path (Fig. 12) also performs only one maneuver. In this case it is harder to tell which one is better just by visual inspection. Finally, for the most challenging initial pose (of the presented ones) for our approach (Fig. 7b), the desirable planned path (Fig. 13) shows less maneuvers (4 vs 3) by starting with a backwards motion instead of a forward one.

One can see that path planning could yield shorter paths than our approach with the drawback of these good outcomes not being straightforwardly repeatable. Additionally, these comparisons do not account for sensor noise, modelplant mismatches, localization and path-tracking errors, etc. Furthermore, since our MSBPC approach does not plan any path and instead the controller runs online, there is (virtually) no inherent delay to be able to start a parking maneuver.

3) Exhaustive simulations: To assess the convergence of the proposed scheme, several exhaustive simulations were conducted for the different parking cases. Due to paper length constraints, for all the shown cases (Fig. 14), the initial orientation of the vehicle is equal to zero (θ T =0 = 0°). Cases with different initial orientations (ranging from -30°to 30°w ith a step of 5°) can be found in the attached video 1 .

Since the exhaustive simulations are an aggregation of the results obtained from several simulations (like those shown in Figs. 6a,7a-10b), each figure consists of a parking spot (represented by a green rectangle) adapted to each case, forbidden zones represented in gray and a scatter plot of the initial position of the vehicle, whose color depends (like in Sec. V-A1) on the final value of ||e p ||. As denoted by the colorbar, yellow/light green values of each scatter plot are inside the region of attraction (ROA), the green/blue values can be interpreted as the boundaries of the ROA and the dark blue ones represent the initial positions that are outside of the ROA.

It can be clearly seen that, thanks to the capability of the MSBPC approach of performing automatically multiple maneuvers, the car is able to park from virtually any initial position in the analysis window with no points out of the ROA.

As expected, the most challenging type of parking maneuver is the forward perpendicular case, often ending the maneuver with a final main task error 0.03 < ||e p || < 0.05 as opposed to ||e p || < 0.015 for the other non-parallel maneuvers. Backward parallel maneuvers are the second most challenging case often yielding larger final ||e p || values than the less challenging diagonal (any direction) and backward perpendicular cases but not to the same extent as the forward perpendicular case. It should be noted that the lateral (associated to s 3 ) error is responsible for the larger (but still rather satisfactory) final ||e p || values. Such behavior is reasonable considering that the lateral motion of the vehicle can't be controlled directly. Nonetheless, the final lateral error may be improved by increasing its associated weight at the expense of a potential increase on the number of maneuvers required for the parking task.

B. Real experimentation

Real experimentation was conducted for the backward perpendicular parking case (Fig. 15a) with the controller running at 10 Hz. In this case the controller is implemented in C++ using the solver NLopt [START_REF] Johnson | The NLopt nonlinear-optimization package[END_REF] with a Sequential Least Squares Programming (SLSQP) algorithm [START_REF] Kraft | A Software Package for Sequential Quadratic Programming[END_REF]. The whole software architecture (and not only the parking controller) runs on a dual core Intel Core i5-3610ME.

To address the robustness concerns that might arise from the considered planar world assumption, it was chosen to make the vehicle pass over a speed bump during the parking maneuver (Fig. 15a). The estimated minimum and maximum values of roll and pitch (in degrees) were, respectively, [-2.57, 0.61] and [-1.36, 3.51].

It can be clearly seen that, in spite of the speed bump (which in addition to perturb the 3D orientation of the vehicle, disturbs as well the response of the linear velocity), the vehicle manages to park successfully (Figs. 15a,15c) while satisfying the constraints during the whole maneuver (Fig. 15e) thus validating the results obtained from the corresponding convergence analysis. The final ||e 2 || achieved was 0.0499 which if reconstructed translates to errors of approximately 0.11 cm laterally, -4.97 cm longitudinally and 0.18°in orientation.

The linear velocity setpoint and response can be seen in Fig. 15b. One can notice that the setpoint is not perfectly followed by the low-level controller, especially when the car passes over a speed bump. This behavior occurs because the longitudinal velocity parameters had been identified for higher velocities (up to 130 km/h), showing an unperfect behavior for very low velocities (such as our application). Even if this behavior is not related to the presented technique, it serves to show the robustness of our approach against model-plant mismatches.

VI. CONCLUSIONS

As an extension of our previous work [START_REF] Pérez-Morales | Multi-Sensor-Based Predictive Control for Autonomous Backward Perpendicular and Diagonal Parking[END_REF], here we have formalized under a single common framework five different parking cases: parking into perpendicular and diagonal parking spots with both forward and backward motions and into parallel ones with backward motions.

The task definition is essentially the same for every case: try to make the vehicle's longitudinal axis to be collinear to the main axis of the parking spot (i.e. to be centered lateralwise) and finish the maneuver at a certain distance from either the rear or front boundary of the parking spot; having just to consider slight adaptations on the interaction model for the different cases.

Furthermore, by moving the effort from the motion planning to the control, the need of having knowledge about the free and occupied space of the whole environment beforehand is eliminated as well as the classical compromise between completeness and computational efficiency when compared to exploration-based path planning techniques.

Thanks to the convergence analyses performed, it has been shown that the presented MSBPC approach is able to sucessfully park the vehicle from virtually any initial pose, normally achieving rather small final ||e p || values.

The results obtained from real experimentation validate the conducted convergence analysis as well as the robustness and effectiveness of the presented approach since, even in the presence of a speed bump that considerably disturbs the 3D orientation of the vehicle and its longitudinal velocity, the car parked successfully with a satisfactory final ||e 2 || = 0.0499 while satisfying the constraints during the whole maneuver.

Future work will focus on simplifying the weighting strategy and adapting it with online learning. Moving obstacle avoidance, that is theoretically addressed in this work, will also be investigated with real experiments.

APPENDIX A CONDITIONS AND NUMERICAL VALUES FOR CONSTRAINTS

DEACTIVATION

The constraints deactivation conditions used to obtain the results presented in this work are now detailed.

APPENDIX B WEIGHTING FUNCTIONS NUMERICAL IMPLEMENTATION

In this appendix we detail the implementation of the weights. They are all computed from a sigmoid-like weighting function w(s), illustrated in Fig. 16

  Fig. 1. (a) Kinematic model diagram for a car-like rear-wheel driving robot. (b) Robotized Renault ZOE used for real experimentation. (c) Multi-sensor model

Fig. 2 .

 2 Fig. 2. Parking spot models for backward (a) and forward (b) non-parallel types and for (c) parallel ones.

Fig. 3 .

 3 Fig. 3. Example of a parking environment. The green rectangle denotes the parking spot into which the car should park. Red areas are considered forbidden zones, as such the vehicle should never go into them. Furthermore, it is considered that parking maneuvers can only start inside the transitable area and if no portion of the vehicle is inside any of the forbidden zones.

Fig. 4 .

 4 Fig. 4. Differences of radii: ρ lat,2 from lateral distance and P 2 (blue) and ρ 5,3 from S 5 and P 3 (orange). All the radii are defined with regards to the ICR C.

w 9 =

 9 (w(s 3 ) + max (w(e 7 ), w(e 8 ))) w(e 9 ), (38b) w 10 = w 11 = w(s 9 , 1 h 1 ) + w(e 3 ) + (1w( 5 h 5 )), (38c) w 12 = (1w(e 9 )) w( m h 1 ) + w( 5 h 5 ) (1w(e 3 )) (1max (w(e 7 ), w(e 8 ))) w( 3 h 5 ) + 1w( 3 h 5 , 5 h 5 , 6 h 5 ) max (w(e 7 ), w(e 8 )) .

Fig. 6 .

 6 Fig. 6. Constrained ⊥ backward parking maneuver signals: (a) performed maneuver, (c) control signals, (e) weighting-related signals, (b) main and (d) auxiliary task errors and (d) constrained sensor signals. . Initial pose = (8 m, 6 m, 0°)

Fig. 7 .Fig. 8 .

 78 Fig. 7. Constrained ⊥ backward parking maneuvers. (a) Initial pose = m, 7.5 m, -5°), (b) Initial pose = (0 m, 5.1 m, 0°)

Fig. 9 .

 9 Fig. 9. Constrained non-parallel forward parking maneuvers: (a) diagonal and (b) perpendicular. Initial pose = (-8 m, 6.5 m, 0°)

Fig. 11 .Fig. 12 .

 1112 Fig. 11. RRT * path planning results using HC 00 steering function. Initial pose = (8m, 6m, 0°)

Fig. 13 .

 13 Fig. 13. RRT * path planning results using HC 00 -RS steering function. Initial pose = (0m, 5.1m, 0°)

Fig. 14 .Fig. 15 .

 1415 Fig. 14. Exhaustive simulations for different types of parking maneuvers with a sampling step of 20cm for the initial position. For non-parallel cases: spot length = 5m and width = 2.7m. For (e): spot length = 5.6m and width = 2m.

3 h 5 ≥ d 1 3 h 4 > d 1 and 3 h 3 < -d 1 3 X 2 ≥ lve +d 1 4 h 5 > d 1 or 4 h 4 < 0 or 3 h 5 < 0 or 3 Y 2 < -d 1 or 6 Y 2 > d 1 3 X 2 ≤ -d 1 3 X 2 < 3 Y 2 ≤ -d 2 3 h 3 4 h 4 ≥ d 1 4 h 5 > d 1 or 4 h 4 < 0 4 h 5 ≥ d 1 4 h 4 > d 1 and 4 h 3 < -d 1 and 3 h 5 < 4 h 5 5 h 3 ≤ -d 1 3 h 3 > 0 and 3 h 5 > d 1 5 h 5 ,

 43125453262123232235043553315 -2vmax or 3 Y 2 < -d 1 or 3 h 3 > 0 or ( 3 h 5 ≤ d 1 and v ≤ 0) > 0 or v ≤ 0 and 3 h 5 ≤ d 1 or ρ lat,2 ≥ -d 2 or 3 Y 2 > 0 or 6 h 4 < 0 6 h 5 ≤ w rd -6 h 3 ≤ -d 1 3 Y 3 < -d 3 or ( 6 X 3 < 0 and 6 Y 3 < d 3 ) or ( 6 X 3 > 0 and 3 Y 3 < 0) 6 X 3 ≤ -d 1 3 Y 3 < -d 1 or 6 Y 3 > d 1 or 3 h 3 > 0 and 3 Y 3 < -d 1

h 4 > 0 and 3 h 3 < 0 and 3 h 5 > 4 h 5 4 h 2 , 5 h 2 ≤ d 4 - 4 h 5 ≤ d 1 4 h 4 > d 1 and 4 h 3 < -d 1 4 h 3 ≥ -d 2 4 X 3 > 2vmax or 4 Y 2 4 Y 3 > 0 or 4 h 5 > 0 4 X 3 ≤ d 2 4 Y 3 < 0 or 5 Y 3 > d 1 or 4 X 3 > 2vmax or 3 X 3 < -d 1 4 Y 3 ≥ -d 1 4 X 3 > 2vmax or 3 h 4 > d 1 or 3 h 5 < 4 h 5 ρ 5 , 2 ≤ d 3 v 5 5 Y 2 ≥ -wve-d 1 4 h 4 > 0 or 5 X 2 >

 35524454313434434343534333134345552352452 ≤ 0 or δ ≥ 0 or 4 Y 3 ≥ -d 3 or ρ - 5,2 ≤ d 3 or 5 h 5 ≤ 0 or 5 h 1 ≥ 0 or 5 h 5 ≥ 1 4 ρ - 5 -3 3 vmax or m h 3 ≥ 0 5 h 4 ≤ d 1 ρ 5,2 ≥ d 3 or 5 h 5 > 0 or 5 X 2 > 0 5 h 5 , 6 h 5 ≤ w rd -5 X 2 ≤ d 2 5 Y 2 > d 1 or 4 Y 2 < -d 1 or 5 X 2 > 2vmax or 6 X 2 < -d 1 or 3 h 3 < 0 and 3 h 5 < d 1 and 3 h 5 < 4 h 2vmax or 6 X 2 < -d 1

  .

Fig. 16 .

 16 Fig.[START_REF] De Lima | Sensor-Based Control with Digital Maps Association for Global Navigation: A Real Application for Autonomous Vehicles[END_REF]. Generic weighting function w(s). This function smoothly maps a feature s to [0, 1] according to lower and upper thresholds.

TABLE II PAIR

 II OF POINTS THROUGH WHICH EACH LINE PASSES

	Line	Non-parallel	Parallel (backward)
	L 1

(P 5 , P 6 ) (P 5 , P 6 ) L 2 (P 1 , P 4 ) (P 1 , P 4 ) L 3 (P 3 , P 4 ) (P 1 , P 2 ) L 4 (P 1 , P 2 ) (P 2 , P 3 ) L 5 (P 2 , P 3 ) (P 3 , P 4 )

TABLE III TASK

 III SENSOR FEATURES DEFINITION

  Algorithm 1: Q p computation at each iteration for backward non-parallel case Update sensor features s and c; fwd = true if car is moving forward; if p l 1 -p l * 1 < L1 or e p < p then fwd = false; if local minimum detected then fwd = !fwd; if fwd == false then Q p = w(e 3 ); / * Q p → 0 if the car is on the left of p L 1 * / else Q p = w(e 9 ) w( 3 h 5 , 5 h 5 , 6 h 5 ); / * Q p → 1 if the car is far from the transitable area limits and is (or is approaching to be) on the right side of a L off

	1	* /

  Table V lists the car and environment-dependant parameters while Table VI details the tuning parameters with their values and semantics.

  ≤ -d 2 δ ≥ 0 or v ≥ -v th or v < 0 and 3 X 2 > |x 3 | or 3 h 5 > 3 4 ρ + m or | 5 h 4 | < 3 4 ρ + m and 3 h 5 > 3 4 ρ + m 3 h 2 , 6 h 2 ≥ d 4 -3 h 4 ≥ d 3 3 Y 2 ≥ 0 or 6 Y 3 ≤ 0 or 3 X 2 < 0

				TABLE VII
			CONSTRAINTS DEACTIVATION -BACKWARD NON-PARALLEL CASE
			Constraint	Deactivate if
			ρ lat,2
		TABLE V
	CAR AND ENVIRONMENT SYMBOLS
	Symbol	Description
	w rd	Width of the transitable area
	wsp	Width of the parking spot
	ρ -i	Distance S i C for minimum steering angle -δmax
	ρ + i	Distance S i C for maximum steering angle δmax
		TABLE VI
		TUNING SYMBOLS AND VALUES
	Symbol	Semantic	Value
	d 1	lateral margin, entry side	5 cm
	d 2	lateral margin, opposite side	7.5 cm
	d 3	final lateral margin	10 cm
	d 4	front / rear margins	15 cm

TABLE VIII CONSTRAINTS

 VIII DEACTIVATION -FORWARD NON-PARALLEL CASE

	Constraint	Deactivate if
	3 h 5 ≥ d 1	3

https://youtu.be/UO3yGzJnWrY

Manuscript received ... This work was supported by the Mexican National Council for Science and Technology (CONACYT). This paper describes work carried out in the framework of the Valet project, reference ANR-15-CE22-0013-02.

Constraint

Deactivate if

3 Y 3 > 0 or 3 X 3 > |x 3 | and v < 0 or 4 X 3 > 0 or 4 X 3 ≤ d 3 or |δ| > 0 and 3 h 5 > 0 and 3 h 4 < 0

h 4 < 0 and 4 h 2 > 0 and 3 h 5 < 4 h 5 and 4 h 5 > 0 or 4 h 5 < 0

The weighting function is usually even (i.e. s -= -s + and s s-= -s s+ ) and is often used with only one (upper or lower) threshold. In the sequel we thus denote this function as:

for even thresholds w(s, s s + , s + , s s -, s -) for non-even thresholds w + (s, s s + , s + ) for upper thresholds only w -(s, s s -, s -) for lower thresholds only

As exposed in Section III-C, the task features are Plücker coordinates and as such can be:

• Distance-based (e i , i ∈ {3, 6, 9, 12}, . h . ), in this case the thresholds are given in meters. Some of the thresholds are the same as the ones introduced in Table VI. • Orientation-based (e i , i / ∈ {3, 6, 9, 12}), in this case the feature corresponds to the sine/cosine of the actual angle and the thresholds are given in this metric. The numerical values that we use are listed in Table X. Furthermore, the two weights associated to the orientation of a given line are always equal:

In the detailed weights we denote the offset from i L j to i L off j as i o j . The distance h rd = max( 5 h 5 , 6 h 5 )-w rd , that expresses the distance to the other side of the road, is also introduced. We now proceed to give the numerical implementation of the considered weights for the different parking types.

A. Backward non-parallel case

During the parking phase (i.e. fwd = False) then Q p only depends on e 3 = 2 h 1 , that is the centering of the vehicle with regards to the parking spot axis:

Otherwise, it is defined as:

where h max = max -3 h 5 , 5 h 5 -w rd , 6 h 5 -w rd expresses the suitable distance to take into account while unparking.

The weights related to the parking position error are defined as:

The weight related to the parking orientation error associated to 1 L 1 :

the remaining weights are defined as follows:

w 9 = 0.15(0.1 + w + (s 3 , d 2 , d 6 ) + w u1 ) (45) × (0.05 + w + (e 9 , -2v max , 0))

where s ψ = sin ψ indicates the angle of the parking spot with regards to the road (Fig. 2).

B. Forward non-parallel case

Weights related to the position error are defined as:

50) Considering the weights related to the orientation errors associated to 2 L 1 and 2 L 2 respectively: w u1 = w + (max(e 7 , e 8 ), 0, a 2 )

We denote w u2 = 1w u2 the complementary value to w u2 .

It is used to scale the maximum velocity according to how far to alignment the vehicle is. The remaining weights are defined as follows:

C. Backward parallel case

When fwd = False then Q p only depends on e 6 = 2 h 2 , that is the distance to the rear bound of the parking spot:

Otherwise it is defined as:

Weights related to the position error are defined as:

, 0.75, 1), ∀i ∈ {1, 2, 4, 5} The weight related to the orientation error are:

the remaining weights are defined as follows: