Optimal Control of Overtaking Maneuver for Intelligent Vehicles

. In the paper a hierarchical overtaking strategy, which is a driver assistance function or rather an autonomous function in electric/autonomous vehicles, is proposed. The solution uses speed and acceleration signals from the surrounding vehicles. These signals are processed with clustering methods in order to achieve probability density functions and predict their expected motion. The strategy includes several additional layers, such as decision making concerning the maneuver, the computation of the required trajectory, and the tracking control of the vehicle. Trajectory generation is formed as an optimization task, which is able to include the prediction model of the surrounding vehicles in the constraints. A robust Linear Parameter Varying (LPV) control design method is proposed to guarantee the tracking of the computed reference.The proposed strategyis able to guarantee the safe motion of the vehicles and handle the interactions with the other traffic participants.


Introduction and Motivation
Overtaking and lane changing maneuvers are critical on roads due to various types of human errors.The necessary distance required by the maneuver must be estimated accurately and the vehicle should return to the lane as fast as possible and the maneuver must be safe regarding the other participants in the traffic.
The appropriate handling of overtaking maneuvers is also difficult for several reasons: (i) The motion of the vehicle ahead must be monitored and predicted (ii) The motions of vehicles in the environment, especially vehicles coming from the opposite lane and traveling in the return lane, must be monitored and predicted (iii) The overtaking maneuver must be designed (iv) Before the maneuver the decision concerning the necessary overtaking must be made (v) Several factors, such as lateral/longitudinal acceleration and jerks in relation to comfort, must be considered (vi) steering, braking, and driving must be coordinated Several different control approaches in the field of overtaking maneuvers of vehicles have been developed.An optimal control design of the overtaking trajectory using polynomial equations to minimize the lateral jerk was proposed by [1].The model predictive control (MPC) method for path planning together with collision avoidance with the application of overtaking was found in [2].In [3] it has been shown that the nonconvex optimal control problem of the vehicle positioning can be transformed to a convex quadratic program, which can be solved efficiently.A stochastic model predictive control, in which the velocities of the surrounding vehicles are considered, is designed [4].A mixed integer programming method, which uses a low number of variables, was presented by [5].Another solution to reduce the numerical difficulties was to use linguistic variables.For example, a two-level fuzzy logic control was proposed by [6] and the application of the Q-learning method was demonstrated by [7].The fuzzy logic with the visual system, differential GPS, and inertial measurements was integrated by [8].Realtime measurement from the overtaken vehicle to generate the vehicle trajectory was used in [9].A nonlinear adaptive controller design using third-order reference trajectory for overtaking scenarios was proposed by [10].
The prediction of vehicle motions is strongly linked to the overtaking in the field of autonomous vehicles; see, e.g., [11].With the precise estimation of the environment with vehicles, pedestrians, and cyclists the safe operations of the autonomous systems can be significantly enhanced.Several methods have also been designed to predict the motion of human-driven vehicles; see, e.g., [12].Probabilistic approaches based on the dynamic Bayesian network and Markov chain models were found in [13,14].A method which used the past similarities in vehicle motion between the current vehicle and predefined number of test vehicles was proposed by [15].The factor of driver aggression and the motion of vehicles in unorganized traffic were considered in the motion planning of an overtaking maneuver in [16].
In this paper an overtaking strategy of our own vehicle is developed.In the following our vehicle will be referred to as ego vehicle.The overtaking strategy is formed in a hierarchical structure with different layers.The core of the autonomous strategy is the motion prediction of the preceding vehicle and surrounding vehicles (e.g., in the opposite lane).Speed and acceleration signals are used to generate probability density functions, which are built in a constrained optimization structure of the trajectory generation.The result of the calculation is a clothoid trajectory, which enhances smooth driving and traveling comfort.Moreover, the paper proposes a strategy, with which further vehicle motions can be considered, e.g., following and overtaking the preceding vehicles.The tracking of the designed trajectory is based on the robust LPV vehicle control, which has the important role in guaranteeing safety in all scenarios, such as overtaking, lane changing, and following the preceding vehicle.

Architecture of the Robust Overtaking Control Strategy.
The overtaking control has been composed of a hierarchical architecture with three different layers, as found in Figure 1.It contains the layer of the surrounding vehicle motion estimation, the trajectory optimization with an overtaking decision logic, and the vehicle control, which generates the steering angle.The advantage of the hierarchical design is the variation in the design method of the layers.The different tasks of the layers require different approaches; e.g., the estimation is based on clustering and probabilitybased methods, and the trajectory design is based on a constrained optimization, while the vehicle control should guarantee robustness.Therefore, it is sufficient to define the interfaces and the layers can be designed independently.In the architecture the interfaces of the layers are the following: (a) The inputs of the surrounding vehicle estimation block are the longitudinal velocity and the acceleration signals of the ego and the surrounding vehicles, while the outputs are the lateral constraints in the vehicle motion   ,    In the rest of the paper the layers of the automated overtaking strategy are presented.The estimation of the surrounding, especially the preceding vehicle motion, which includes the processing of vehicle data and the prediction of its position, is presented in Section 2. The trajectory design is formed in a constrained optimization problem, which is based on a clothoid path of the vehicle; see Section 3. It also shows the decision logic of the overtaking and lane changing maneuvers.The robust vehicle control based on the generated reference signals is presented in Section 4. The operation of the control system is illustrated through the CarSim simulation environment in Section 5. Finally, Section 6 summarizes the contributions of the paper.

Estimation of the Motion of Surrounding Vehicles
Safe overtaking and lane changing maneuvers require information about the motion of the preceding vehicle.In this paper its motion is estimated using the current longitudinal velocity V  and acceleration   signals.It is assumed that the motion of the preceding vehicle in the future will be similar to what it has been so far.Moreover, it is also assumed that the required signals V  and   are available for the ego vehicles.
The proposed estimation has two main steps.First, the preceding vehicle signals are processed through a clustering procedure and the probability density function is generated.Second, the future position of the vehicle is predicted based on the calculated density functions.

Processing of Preceding Vehicle Signals.
The preceding vehicle is considered to be driven by a human, whose velocity selection is determined by a reference velocity V , .The value of V , can depend on several factors, e.g., the speed limitation, the quality of the road surface, the curvature and the number of lanes on the road, and the traffic situation.
In the method the velocity of the human-driven vehicle is compared to the reference velocity, and the difference is computed as  V = V  − V , .If several measurements are available from a longer time period, the required signals of the preceding vehicle can be estimated.As an example the relationship between the speed differences  V and the longitudinal acceleration   is illustrated in Figure 2. In this simplified illustrative scenario V , is considered as the speed limit on the course of the vehicle.It is shown that most of the data are around  V = 0,   = 0, which means that the vehicle is generally cruising close to V , .However, the signals have a variance; e.g., if the velocity changes the vehicle is accelerated to reach the speed limit again, which leads to  V < 0 and   > 0. The purpose of the analysis is to derive the probabilities of the different driving motions of the preceding vehicle.
The generation of probability density functions requires the clustering of data.In this vehicle dynamic examination clustering means that the collected data are ordered in groups, depending on their locations in the plane  V ,   .Numerous algorithms for this problem have been developed in recent decades; see, e.g., partitioning methods [17], hierarchical algorithms [18], and density based methods [19].In this paper a fast and efficient improved k-means method is used as presented and analyzed in [20].
The purpose of the algorithm is to minimize the Euclidean distance between the objects and the centre of the selected cluster, such as where  represents the number of clusters,  is the centre of the cluster   .The candidate clusters must be nonempty and nonoverlapping.
In the estimation process the number of the clusters significantly influences the results.Several methods have been developed for the determination of the cluster number; see, e.g., [21].Below the elbow method is applied.In this case it is necessary to find the cluster number with which the value of the cost function   drops significantly.In our case the elbow method proposes  = 3 clusters; see Figure 3. Result of the clustering is shown in Figure 4, where the clusters are marked with different colours.
Results of the clustering show that the  V ,   data have variance inside the clusters.Thus, in what follows probability density functions are fitted to the data of each cluster.The function requires a two-dimensional Gaussian form:  where  is related to the cluster number,   represents the mean value within a cluster, ∑  is the covariance matrix, and  = [ V ,   ]  contains the value of the elements in the current cluster.In the estimation the   and ∑  values must be computed for all   functions of all the  clusters.The overall probability density function () is derived from each   () such as which guarantees the criterion for probability density functions Result of the method for our case is illustrated in Figure 5.
Based on the proposed algorithm the preceding vehicle data are transformed into a probability density function, which depends on both  V and   .In the following the function () is used to predict the future position of the vehicle considering the probability of driver actuation.

Prediction of Vehicle Position.
The future position of the vehicle is predicted based on the longitudinal kinematic equations where V  is the longitudinal velocity and  is the displacement.
In the estimation the relations (4a) and (4b) are transformed into a discrete form with the time step  =  +1 −   .Thus, in the following the discrete longitudinal velocity V , and displacement   are used, where  represents the step index.The purpose of the prediction is to predict  1 . . .  values based on the probability density function ().For each prediction step , slices of the function () are generated as ()|  V () ; see, e.g., Figure 6.The computations of V , and   require the value of  , , which is generated from the gridding of ()|  V () .The aim of the computation is to determine the probability density function of the vehicle position (  ), depending on   for each time step.The computation is based on the relations of (4a) and (4b), in which the probability density function of ()|  V () is used.The computation of V 1 and  1 for  = 1 requires the deterministic initial velocity V 0 and position  0 , which can be measured.Due to ()|  V () the speed and positions V 1 ,  1 are also represented by probability density functions ( 1 ), (V 1 ).Therefore, in the computation of V 2 ,  2 for  = 2 the previous velocity and position of  = 1 are considered with their probability density functions.As an illustration, Figure 7 shows an example for  = 1 . . . 3 predictions.Here the initial velocity V ,0 = 140 /ℎ, the initial position  0 = 0, and the sampling time 1  are selected.The illustration shows that for  = 1 the probability density function of  1 has high values in a small range, which means that the position of the vehicle can be estimated with high probability.However, e.g., for  = 3 the probability density function of  3 has high values in a broad range, which means that the forthcoming position of the vehicle has higher uncertainty.
In the prediction of the vehicle position the probability density function has fundamental importance.The probability ( ,− <   <  ,+ ) that the vehicle in the prediction step  is between the positions  ,− . . . ,+ is computed as follows: A safe maneuver to avoid a collision requires a guarantee that the probability of the vehicle in the critical position range  ,− <   <  ,+ is smaller than a predefined   value.
If ( ,− <   <  ,+ ) >   , the position  ,− <   <  ,+ must not be reached by the ego vehicle.Consequently, based on the safe maneuver the vehicle must perform the overtaking or decelerate to the speed of the preceding vehicle.Thus, the trajectory of the ego vehicle must be designed in such a way that both the estimation of the preceding vehicle motion and the maximum probability level   are taken into consideration.Note that if   is reduced, the safety of the maneuver increases, but the avoidable regions also increase.It can lead to a conservative maneuver, which requires too long traveling in the opposite lane to overtake the preceding vehicle.Finally, it is necessary to mention that the proposed estimation method is also used for the estimation of the motions of the vehicles in the opposite lane.If the data  V ,   of the closest vehicle in the opposite lane are available, the prediction of the motion can be performed.

Formulation of Trajectory Design
In the overtaking and lane changing strategy the result of the estimation is used for the computation of the vehicle trajectory.In the design two criteria are considered.First, the results of the estimation must be incorporated in the trajectory design to guarantee safe cruising.Second, the generated trajectory must guarantee a comfortable maneuver.It means that the motion of the vehicle must be smooth, which is achieved by applying a clothoid trajectory.The advantages of the clothoid trajectory were presented in [22,23].In the following an optimal trajectory design method which guarantees both requirements is proposed.

Modelling of Trajectory
Tracking.The lateral motion of the vehicle is formulated based on the kinematic model of the vehicle, such as where  is the lateral displacement, V  is the longitudinal velocity in the vehicle coordinate system,  is the yaw angle,  is the front steering angle of the front wheels, and  is the distance between the front and the rear axle.Translate the motion equation to space domain by making V  = ()/ and assume that V  is a continuous function in the rearrangement of (6a) and (6b): The term ()/ in (7a) and (7b) represents the curvature () of the path; see also [23].
To guarantee a smooth trajectory for the vehicle, the curvature () is constrained so as to generate a clothoid form; see Figure 8.It leads to the following formulation [23]: where   = V , is the distance between two section points and   is the ratio of the clothoid section.
The motion equations (7a) and (7b) are transformed into a discrete form such as For small yaw angle differences from the path the assumption sin  ≈  is acceptable.Using ( 8), (9a), and (9b) the system is transformed into a state-space representation  +1 =   +   in the following way: ]   (10) where the system depends on the values of   .The purpose of the trajectory computation is to determine these values.

Optimization of Trajectory Tracking.
In the trajectory design the lateral position of the vehicle  +1 must be controlled.It is necessary to guarantee that  +1 tends to the reference trajectory  +1, as accurately as possible in order to avoid the preceding vehicle and keep the required lanes during the overtaking maneuver.This task leads to a constrained tracking optimization problem, which is formed in the following way.The trajectory design of the overtaking maneuver is formed in a finite horizon length   ahead of the vehicle.The lateral position error  +1 −  +1, in the horizon is described in the following way: where A contains the current states of the system, R is built by the reference values, B is built by the state matrices, and C is built by the ratios of clothoid sections.In the tracking problem it is necessary to minimize the following function: where  and  are weighting matrices.The role of  is to minimize the tracking error, while the role of  is to minimize the ratios of the clothoid sections.Substituting (11) into the function ( 12), the function is transformed as where In ( 13)  contains all the constant components.Since  is independent of the effect of C on (C), it can be canceled from the further optimization problem.Through the minimization of the cost function (C) the tracking of the reference trajectory  +1, can be guaranteed.In order to increase the safety of the maneuver both collision avoidance and lane keeping are built into the design.Therefore, constraints are incorporated into the trajectory optimization problem.The constraints are related to both the minimum and the maximum values of  +1 .Practically,   +1 and   +1 are determined by the lanes, which guarantees lane keeping.However, in the overtaking maneuver the constraints must be modified with the current position of the preceding vehicle.The constraints of the minimum values of the positions are formed as  + ≥   + ,  ∈ {1, }.The inequalities are transformed into a matrix representation:  where . Thus, the constraint on the clothoid ratios C is formed as Similarly, the constraints of the maximum values of  +1 are derived as where .The minimum   and maximum   values are determined by the edges of the lanes and the surrounding vehicles, especially the preceding vehicle and the vehicle in the opposite lane.If a vehicle in the opposite lane performs a lane change maneuver, it can limit the lateral motion of the ego vehicle.This information is also incorporated in   , with which the safe cruising of the ego vehicle is guaranteed against collisions with the vehicles from the opposite lane.The selections of   and   are illustrated in Figure 9, where   describes a safety zone of the vehicles.Finally, from ( 13), (16), and (17) the constrained trajectory optimization problem is formed as min such that the following constraints are also guaranteed: where C contains the achievable clothoid ratios.The optimization problem can be solved using standard quadratic programming methods, e.g., [24,25].The solution of ( 18) leads to a series of clothoid ratios on the horizon   .The current ratio   can be computed online during the cruising of the vehicle.

Decision Strategy of Overtaking and Lane
Changing.The overtaking and lane changing strategy is based on the constrained trajectory optimization method (18).Although the constraints and the achievable clothoid ratios are considered, the calculated tracking must be modified for safety reasons.Before the maneuver the ego vehicle reaches a preceding vehicle which is traveling at a slower speed.The vehicle must follow the preceding vehicle.If the preceding vehicle accelerates or decelerates the ego vehicle must strictly track the velocity within the speed limit.Meanwhile, it calculates a safe trajectory for the overtaking and lane changing.The optimization method (18) results in a clothoid curve, which guarantees the cruising of the ego vehicle between the minimum and the maximum limits.
However, if there is a follower vehicle in the inner lane which is traveling at a higher velocity, a conflict with the ego vehicle may occur during the overtaking maneuver.Moreover, if there is a vehicle in the opposite lane, a conflict with the ego vehicle in the maneuver may also occur.These maneuvers are considered unfeasible.Infeasibility means that it is impossible to find an appropriate trajectory which guarantees both the minimization (18) and the constraints (19a), (19b), and (19c).In this case the ego vehicle must follow the preceding vehicle and modify the longitudinal dynamics.
When the traffic situation changes and the optimization becomes feasible, the overtaking maneuver can be performed.The maneuver is realized through the actuation of steering and driving/braking systems; see, e.g., [26].
Note that the proposed method can be used not only for overtaking, but also for simple lane changing maneuvers, e.g., changing the route in a highway intersection.In this scenario the reference signal of the road  +1, must be modified according to the parameters of the requested lane.Moreover, in a lane changing maneuver the constraints   and   must be modified for the new lane.Furthermore, the proposed optimization (18) and decision problem can be formed as a MPC task, in which the steering intervention can also be incorporated.However, the entire control system must guarantee the robustness of the system, which can lead to high complexity in the computation of the vehicle control.In the paper a robust LPV-based control design method is proposed to guarantee the tracking of the generated trajectory and the robustness of the controlled system.

Robust LPV Control for Autonomous Overtaking
The goal of the robust LPV control is to guarantee the tracking of the trajectory, which has been generated by optimization (18) together with the decision algorithm.The results of the optimization are   . . . +−1 , which are equal to the ratios of the clothoid sections   . . . +−1 .In the case of the tracking control the vehicle must follow the reference path   =  +1 =  1 and the reference heading angle   =  +1 =  1 .
The reference signals are computed from (10) such as where  0 ,  0 are related to (), () in discrete time, while  0 is the length of the forthcoming segment and  0 is the result in the previous trajectory computation step.The parameter  0 is the first element among the results of the optimization  0 .After the computation of the reference signals the tracking performances together with the control performance are defined as The performance vector is as follows: The model of the vehicle is described by the dynamical bicycle model; see [27]: where  is the yaw inertia of the vehicle,  is the vehicle mass,   ,   are the cornering stiffness coefficients, and   ,   are geometric parameters.The signal ẏ is the lateral velocity and ψ is the yaw rate.The longitudinal velocity V  may vary along the route of the vehicle; therefore it is chosen as a scheduling variable.Equations ( 22a) and (22b) can be transformed into a state-space representation, where the state vector is  = [ ψ ẏ  ]  , the control input is the steering angle  = , and the measured output vector is   = [ ]  .The state equation and the performances as well as the measurements are written as ẋ =  ()  +  (23a) where  = V  is selected as a scheduling variable.Its value is modified through the decision strategy.The control design is based on a weighting strategy, which is formulated through a closed-loop interconnection structure; see Figure 10.The interconnection structure contains several weighting functions, whose roles are to guarantee the trade-off between the performances and to scale the signals.The weights  ,1 ,  ,2 are related to the sensor characteristics on the lateral error and yaw error measurement.These weights are related to the sensor dynamics. ,1 ,  ,2 , and  ,3 are the weights for the performances, which represent the minimization of  1 ,  2 , and  3 .The weights are formed as  , =   /(   + 1) where   ,   are design parameters.These parameters influence the balance between the performances  1 ,  2 , and  3 .Moreover, the role of the weight  , is to consider the dynamics of the reference signals   ,   .The blocks also represent the dynamics of their variations.
The design of the control is based on robust LPV methods.The advantage of these methods is that the controller meets stability and performance demands by using affine parameterized Lyapunov functions in the entire operational interval, since the controller is able to adapt to the current operational conditions; see [28,29].The solution of an LPV problem is governed by the set of infinite dimensional LMIs being satisfied for all  ∈ F P ; thus it is a convex problem.In  practice, this problem is set up by gridding the parameter space and solving the set of LMIs that hold on the subset of F P .The induced L 2 norm of parameter-dependent stable LPV systems with zero initial conditions is defined as where  is the disturbance and  is the performance.The result of the optimization is the steering angle .

Simulation Results
To illustrate the efficiency of the proposed method a complex simulation scenario is presented.The traffic scenario contains four vehicles, such as the ego vehicle, two preceding vehicles, and another vehicle in the opposite lane.In the setting of the optimal trajectory computation the sample time is set at  = 0.1 with  = 20 the prediction points.The simulation is performed using the high-complexity vehicle dynamic software CarSim.
The simulation is illustrated in Figure 11.At the beginning of the simulation the ego vehicle (blue) reaches the preceding vehicle (white) due to the significant difference in the velocities; see Figure 11(a).Since the overtaking maneuver is feasible, it is carried out as Figure 11(b) shows.Then the ego vehicle reaches the next preceding vehicle (green), which also has lower velocity; see Figure 11(c).The ego vehicle calculates the overtaking trajectory based on the optimization task (18) and the constraints   and   (19a), (19b), and (19c).But there is a vehicle in the opposite lane (red), which makes the overtaking maneuver unfeasible.Thus, the ego vehicle must follow the preceding vehicle, which results in the reduction of its velocity; see Figure 11(d).Finally, the ego vehicle performs a lane changing in order to modify its route at an exit ramp.In this case a new reference signal  +1, is set as shown in Figure 11(e).Then the velocity of the vehicle increases; see Figure 11(f).
The numerical values of the lateral position and the velocity are illustrated in Figure 12.The increase and decrease of the lateral positions at 100  and 200 , respectively, illustrate the overtaking of the first slower vehicle (white) and then the tracking of the second preceding vehicle (green) due to the third vehicle (red) coming from the opposite lane; see Figure 12(a).In the proposed scenario the safe distance between the vehicles is 20, which has been kept by the ego vehicle.The initial velocity is 110 /ℎ, which must be reduced to keep the velocity of the green preceding vehicle (90/ℎ); see Figure 12(b).At 500  the lane change is carried out, which results in the changes of the lateral position and the increases in the velocity compared to the original value.

Conclusions
In the paper an overtaking and a lane changing strategy have been proposed.The expected motion of the preceding vehicle is predicted by using clustering methods and probability density functions.The trajectory design is formed as a constrained optimization problem, in which the traffic in the lanes of the road is considered.In the decision making concerning overtaking the motions of the surrounding vehicles such as the follower vehicle with higher velocity and vehicles in the opposite lane is also incorporated.The robust parameter varying tracking control design of the lateral dynamics has been presented.The proposed strategy is able to guarantee the safe motion of the vehicles and handle the interactions with the other traffic participants.The results have been illustrated through simulation examples.

Figure 1 :
Figure 1: Architecture of the control system.
is the number of the objects, and   ∈

Figure 5 :
Figure 5: Probability density function of preceding vehicle.

Figure 6 :Figure 7 :
Figure 6: Slices of probability density function at different velocities.

Figure 8 :
Figure 8: Piecewise linear formulation of the curvature.
(a) Start of overtaking (b) End of overtaking (c) Approaching the preceding vehicle (d) Tracking the preceding vehicle (e) Change of lane (f) Cruising with its own reference velocity