# Optimal Control of Overtaking Maneuver for Intelligent Vehicles

Authors: Balázs Németh; Péter Gáspár; Tamás Hegedűs
Journal: Journal of Advanced Transportation
 (2018)

---

## Abstract

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 strategy is able to guarantee the safe motion of the vehicles and handle the interactions with the other traffic participants.


---

## Body

## 1. 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 coordinatedSeveral 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]. Real-time 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.


### 1.1. Architecture of the Robust Overtaking Control Strategy

The overtaking control has been composed of a hierarchical architecture with three different layers, as found in Figure1. 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 probability-based 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:Figure 1
Architecture of the control system.(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 motionYmin,Ymax(b) Based on the constraints the actual reference signalsyref,ψref are computed together with the desired longitudinal velocity vx(c) Finally, the robust vehicle control computes the actual steering angleδ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 Section2. 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.


## 1.1. Architecture of the Robust Overtaking Control Strategy

The overtaking control has been composed of a hierarchical architecture with three different layers, as found in Figure1. 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 probability-based 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:Figure 1
Architecture of the control system.(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 motionYmin,Ymax(b) Based on the constraints the actual reference signalsyref,ψref are computed together with the desired longitudinal velocity vx(c) Finally, the robust vehicle control computes the actual steering angleδ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 Section2. 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.


## 2. 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 velocityvx and acceleration ax 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 vx and ax 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.


### 2.1. Processing of Preceding Vehicle Signals

The preceding vehicle is considered to be driven by a human, whose velocity selection is determined by a reference velocityvx,ref. The value of vx,ref 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 ev=vx-vx,ref. 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 ev and the longitudinal acceleration ax is illustrated in Figure 2. In this simplified illustrative scenario vx,ref is considered as the speed limit on the course of the vehicle. It is shown that most of the data are around ev=0, ax=0, which means that the vehicle is generally cruising close to vx,ref. 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 ev<0 and ax>0. The purpose of the analysis is to derive the probabilities of the different driving motions of the preceding vehicle.Figure 2
Data on 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 planeev,ax. 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(1)Jcl=∑i=1K∑j=1Nxj-ciwhere K represents the number of clusters, xj contains [ev,ax] object data, N is the number of the objects, and ci∈Ci is the centre of the cluster Ci. 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 Jcl drops significantly. In our case the elbow method proposes K=3 clusters; see Figure 3. Result of the clustering is shown in Figure 4, where the clusters are marked with different colours.Figure 3
Selection of cluster number.Figure 4
Clustered data on preceding vehicle.Results of the clustering show that theev, ax 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:(2)fix=12πn/2∑i0.5e-0.5x-μiT∑ix-μi,where i is related to the cluster number, μi represents the mean value within a cluster, ∑i is the covariance matrix, and x=ev,axT contains the value of the elements in the current cluster. In the estimation the μi and ∑i values must be computed for all fi functions of all the K clusters. The overall probability density function f(x) is derived from each fi(x) such as(3)fx=1K∑i=1Kfix,which guarantees the criterion for probability density functions ∫ev∫axf(x)dx=1. Result of the method for our case is illustrated in Figure 5.Figure 5
Probability density function of preceding vehicle.Based on the proposed algorithm the preceding vehicle data are transformed into a probability density function, which depends on bothev and ax. In the following the function f(x) is used to predict the future position of the vehicle considering the probability of driver actuation.


### 2.2. Prediction of Vehicle Position

The future position of the vehicle is predicted based on the longitudinal kinematic equations(4a)vxti+1=vxti+∫titi+1axtdt,(4b)sti+1=sti+∫titi+1vxtdt,where vx is the longitudinal velocity and s is the displacement. In the estimation the relations (4a) and (4b) are transformed into a discrete form with the time step T=ti+1-ti. Thus, in the following the discrete longitudinal velocity vx,i and displacement si are used, where i represents the step index. The purpose of the prediction is to predict s1…sn values based on the probability density function f(x). For each prediction step i, slices of the function f(x) are generated as f(x)|ev(i); see, e.g., Figure 6.Figure 6
Slices of probability density function at different velocities.The computations ofvx,i and si require the value of ax,i, which is generated from the gridding of f(x)|ev(i). The aim of the computation is to determine the probability density function of the vehicle position f(si), depending on si for each time step. The computation is based on the relations of (4a) and (4b), in which the probability density function of f(x)|ev(i) is used. The computation of v1 and s1 for i=1 requires the deterministic initial velocity v0 and position s0, which can be measured.Due tof(x)|ev(i) the speed and positions v1, s1 are also represented by probability density functions f(s1),f(v1). Therefore, in the computation of v2,s2 for i=2 the previous velocity and position of i=1 are considered with their probability density functions. As an illustration, Figure 7 shows an example for i=1…3 predictions. Here the initial velocity vx,0=140km/h, the initial position s0=0, and the sampling time 1sec are selected. The illustration shows that for i=1 the probability density function of s1 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 i=3 the probability density function of s3 has high values in a broad range, which means that the forthcoming position of the vehicle has higher uncertainty.Figure 7
Illustration of thesi probability density function in the predictions.In the prediction of the vehicle position the probability density function has fundamental importance. The probabilityP(si,-<si<si,+) that the vehicle in the prediction step i is between the positions si,-…si,+ is computed as follows:(5)Psi,-<si<si,+=∫si,-si,+fsidsiA safe maneuver to avoid a collision requires a guarantee that the probability of the vehicle in the critical position range si,-<si<si,+ is smaller than a predefined pmax value. If P(si,-<si<si,+)>pmax, the position si,-<si<si,+ 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 pmax are taken into consideration. Note that if pmax 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 dataev, ax of the closest vehicle in the opposite lane are available, the prediction of the motion can be performed.


## 2.1. Processing of Preceding Vehicle Signals

The preceding vehicle is considered to be driven by a human, whose velocity selection is determined by a reference velocityvx,ref. The value of vx,ref 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 ev=vx-vx,ref. 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 ev and the longitudinal acceleration ax is illustrated in Figure 2. In this simplified illustrative scenario vx,ref is considered as the speed limit on the course of the vehicle. It is shown that most of the data are around ev=0, ax=0, which means that the vehicle is generally cruising close to vx,ref. 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 ev<0 and ax>0. The purpose of the analysis is to derive the probabilities of the different driving motions of the preceding vehicle.Figure 2
Data on 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 planeev,ax. 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(1)Jcl=∑i=1K∑j=1Nxj-ciwhere K represents the number of clusters, xj contains [ev,ax] object data, N is the number of the objects, and ci∈Ci is the centre of the cluster Ci. 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 Jcl drops significantly. In our case the elbow method proposes K=3 clusters; see Figure 3. Result of the clustering is shown in Figure 4, where the clusters are marked with different colours.Figure 3
Selection of cluster number.Figure 4
Clustered data on preceding vehicle.Results of the clustering show that theev, ax 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:(2)fix=12πn/2∑i0.5e-0.5x-μiT∑ix-μi,where i is related to the cluster number, μi represents the mean value within a cluster, ∑i is the covariance matrix, and x=ev,axT contains the value of the elements in the current cluster. In the estimation the μi and ∑i values must be computed for all fi functions of all the K clusters. The overall probability density function f(x) is derived from each fi(x) such as(3)fx=1K∑i=1Kfix,which guarantees the criterion for probability density functions ∫ev∫axf(x)dx=1. Result of the method for our case is illustrated in Figure 5.Figure 5
Probability density function of preceding vehicle.Based on the proposed algorithm the preceding vehicle data are transformed into a probability density function, which depends on bothev and ax. In the following the function f(x) is used to predict the future position of the vehicle considering the probability of driver actuation.


## 2.2. Prediction of Vehicle Position

The future position of the vehicle is predicted based on the longitudinal kinematic equations(4a)vxti+1=vxti+∫titi+1axtdt,(4b)sti+1=sti+∫titi+1vxtdt,where vx is the longitudinal velocity and s is the displacement. In the estimation the relations (4a) and (4b) are transformed into a discrete form with the time step T=ti+1-ti. Thus, in the following the discrete longitudinal velocity vx,i and displacement si are used, where i represents the step index. The purpose of the prediction is to predict s1…sn values based on the probability density function f(x). For each prediction step i, slices of the function f(x) are generated as f(x)|ev(i); see, e.g., Figure 6.Figure 6
Slices of probability density function at different velocities.The computations ofvx,i and si require the value of ax,i, which is generated from the gridding of f(x)|ev(i). The aim of the computation is to determine the probability density function of the vehicle position f(si), depending on si for each time step. The computation is based on the relations of (4a) and (4b), in which the probability density function of f(x)|ev(i) is used. The computation of v1 and s1 for i=1 requires the deterministic initial velocity v0 and position s0, which can be measured.Due tof(x)|ev(i) the speed and positions v1, s1 are also represented by probability density functions f(s1),f(v1). Therefore, in the computation of v2,s2 for i=2 the previous velocity and position of i=1 are considered with their probability density functions. As an illustration, Figure 7 shows an example for i=1…3 predictions. Here the initial velocity vx,0=140km/h, the initial position s0=0, and the sampling time 1sec are selected. The illustration shows that for i=1 the probability density function of s1 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 i=3 the probability density function of s3 has high values in a broad range, which means that the forthcoming position of the vehicle has higher uncertainty.Figure 7
Illustration of thesi probability density function in the predictions.In the prediction of the vehicle position the probability density function has fundamental importance. The probabilityP(si,-<si<si,+) that the vehicle in the prediction step i is between the positions si,-…si,+ is computed as follows:(5)Psi,-<si<si,+=∫si,-si,+fsidsiA safe maneuver to avoid a collision requires a guarantee that the probability of the vehicle in the critical position range si,-<si<si,+ is smaller than a predefined pmax value. If P(si,-<si<si,+)>pmax, the position si,-<si<si,+ 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 pmax are taken into consideration. Note that if pmax 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 dataev, ax of the closest vehicle in the opposite lane are available, the prediction of the motion can be performed.


## 3. 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.


### 3.1. Modelling of Trajectory Tracking

The lateral motion of the vehicle is formulated based on the kinematic model of the vehicle, such as(6a)dytdt=vxsin⁡ψt,(6b)dψtdt=vxDtan⁡δt,where y is the lateral displacement, vx is the longitudinal velocity in the vehicle coordinate system, ψ is the yaw angle, δ is the front steering angle of the front wheels, and D is the distance between the front and the rear axle. Translate the motion equation to space domain by making vx=ds(t)/dt and assume that vx is a continuous function in the rearrangement of (6a) and (6b):(7a)dysds=sin⁡ψs(7b)dψsds=δsD.The term δ(s)/D in (7a) and (7b) represents the curvature κ(s) of the path; see also [23].To guarantee a smooth trajectory for the vehicle, the curvatureκ(s) is constrained so as to generate a clothoid form; see Figure 8. It leads to the following formulation [23]:(8)κi+1=κi+ciLi,where Li=Tvx,i is the distance between two section points and ci is the ratio of the clothoid section.Figure 8
Piecewise linear formulation of the curvature.The motion equations (7a) and (7b) are transformed into a discrete form such as(9a)yi+1=yi+Lisin⁡ψ(9b)ψi+1=ψi+κi+1Li=ψi+κiLi+Li22ciFor 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 xi+1=Axi+Bui in the following way:(10)yi+1ψi+1κi+1=1Li001Li001yiψiκi+0Li22Liciwhere the system depends on the values of ci. The purpose of the trajectory computation is to determine these values.


### 3.2. Optimization of Trajectory Tracking

In the trajectory design the lateral position of the vehicleyi+1 must be controlled. It is necessary to guarantee that yi+1 tends to the reference trajectory yi+1,ref 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 lengthLn ahead of the vehicle. The lateral position error yi+1-yi+1,ref in the horizon is described in the following way:(11)YC=yi+1yi+2⋮yi+n-yi+1,refyi+2,ref⋮yi+n,ref=CACA2⋮CAnxi-yi+1,refyi+2,ref⋮yi+n,ref+CB0⋯0CABCB⋯0⋮⋮⋱⋮CAn-1BCAn-2B⋯CBcici+1⋮ci+n-1=A-R+BCwhere 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:(12)JC=12YTCQYC+CTRC,where Q and R are weighting matrices. The role of Q is to minimize the tracking error, while the role of R is to minimize the ratios of the clothoid sections. Substituting (11) into the function (12), the function is transformed as(13)JC=12A-R+BCTQA-R+BC+CTRC=12AT-RT+CTBTQA-R+BC+CTRC=12ATQA-ATQR+ATQBC-RTQA+RTQR-RTQBC+CTBTQA-CTBTQR+CTBTQBC+CTRC=12CTBTQB+2RC+12ATQB-RTQBC+CT12BTQA-BTQR+ε=12CTϕC+βTC+CTγ+εwhere(14a)ϕ=BTQB+2R,(14b)βT=12ATQB-RTQB,(14c)γ=12BTQA-BTQR,(14d)ε=12ATQA-ATQR-RTQA+RTQR.In (13) ε contains all the constant components. Since ε is independent of the effect of C on J(C), it can be canceled from the further optimization problem.Through the minimization of the cost functionJ(C) the tracking of the reference trajectory yi+1,ref 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 yi+1. Practically, yi+1min and yi+1max 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 yi+j≥yi+jmin, j∈{1,n}. The inequalities are transformed into a matrix representation:(15)A+BC≥Ymin,where Ymin=yi+1minyi+2min⋯yi+nminT. Thus, the constraint on the clothoid ratios C is formed as(16)A-Ymin≥-BC.Similarly, the constraints of the maximum values of yi+1 are derived as(17)Ymax-A≥BC,where Ymax=yi+1maxyi+2max⋯yi+nmaxT. The minimum Ymin and maximum Ymax 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 Ymax, with which the safe cruising of the ego vehicle is guaranteed against collisions with the vehicles from the opposite lane. The selections of Ymin and Ymax are illustrated in Figure 9, where ysafety describes a safety zone of the vehicles.Figure 9
Determination ofYmin and Ymax.Finally, from (13), (16), and (17) the constrained trajectory optimization problem is formed as(18)minui…ui+n-112CTϕC+βTC+CTγsuch that the following constraints are also guaranteed:(19a)A-Ymin≥-BC(19b)Ymax-A≥BC(19c)C∈Cwhere 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 Ln. The current ratio ci can be computed online during the cruising of the vehicle.


### 3.3. 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 roadyi+1,ref must be modified according to the parameters of the requested lane. Moreover, in a lane changing maneuver the constraints Ymin and Ymax 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.


## 3.1. Modelling of Trajectory Tracking

The lateral motion of the vehicle is formulated based on the kinematic model of the vehicle, such as(6a)dytdt=vxsin⁡ψt,(6b)dψtdt=vxDtan⁡δt,where y is the lateral displacement, vx is the longitudinal velocity in the vehicle coordinate system, ψ is the yaw angle, δ is the front steering angle of the front wheels, and D is the distance between the front and the rear axle. Translate the motion equation to space domain by making vx=ds(t)/dt and assume that vx is a continuous function in the rearrangement of (6a) and (6b):(7a)dysds=sin⁡ψs(7b)dψsds=δsD.The term δ(s)/D in (7a) and (7b) represents the curvature κ(s) of the path; see also [23].To guarantee a smooth trajectory for the vehicle, the curvatureκ(s) is constrained so as to generate a clothoid form; see Figure 8. It leads to the following formulation [23]:(8)κi+1=κi+ciLi,where Li=Tvx,i is the distance between two section points and ci is the ratio of the clothoid section.Figure 8
Piecewise linear formulation of the curvature.The motion equations (7a) and (7b) are transformed into a discrete form such as(9a)yi+1=yi+Lisin⁡ψ(9b)ψi+1=ψi+κi+1Li=ψi+κiLi+Li22ciFor 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 xi+1=Axi+Bui in the following way:(10)yi+1ψi+1κi+1=1Li001Li001yiψiκi+0Li22Liciwhere the system depends on the values of ci. The purpose of the trajectory computation is to determine these values.


## 3.2. Optimization of Trajectory Tracking

In the trajectory design the lateral position of the vehicleyi+1 must be controlled. It is necessary to guarantee that yi+1 tends to the reference trajectory yi+1,ref 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 lengthLn ahead of the vehicle. The lateral position error yi+1-yi+1,ref in the horizon is described in the following way:(11)YC=yi+1yi+2⋮yi+n-yi+1,refyi+2,ref⋮yi+n,ref=CACA2⋮CAnxi-yi+1,refyi+2,ref⋮yi+n,ref+CB0⋯0CABCB⋯0⋮⋮⋱⋮CAn-1BCAn-2B⋯CBcici+1⋮ci+n-1=A-R+BCwhere 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:(12)JC=12YTCQYC+CTRC,where Q and R are weighting matrices. The role of Q is to minimize the tracking error, while the role of R is to minimize the ratios of the clothoid sections. Substituting (11) into the function (12), the function is transformed as(13)JC=12A-R+BCTQA-R+BC+CTRC=12AT-RT+CTBTQA-R+BC+CTRC=12ATQA-ATQR+ATQBC-RTQA+RTQR-RTQBC+CTBTQA-CTBTQR+CTBTQBC+CTRC=12CTBTQB+2RC+12ATQB-RTQBC+CT12BTQA-BTQR+ε=12CTϕC+βTC+CTγ+εwhere(14a)ϕ=BTQB+2R,(14b)βT=12ATQB-RTQB,(14c)γ=12BTQA-BTQR,(14d)ε=12ATQA-ATQR-RTQA+RTQR.In (13) ε contains all the constant components. Since ε is independent of the effect of C on J(C), it can be canceled from the further optimization problem.Through the minimization of the cost functionJ(C) the tracking of the reference trajectory yi+1,ref 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 yi+1. Practically, yi+1min and yi+1max 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 yi+j≥yi+jmin, j∈{1,n}. The inequalities are transformed into a matrix representation:(15)A+BC≥Ymin,where Ymin=yi+1minyi+2min⋯yi+nminT. Thus, the constraint on the clothoid ratios C is formed as(16)A-Ymin≥-BC.Similarly, the constraints of the maximum values of yi+1 are derived as(17)Ymax-A≥BC,where Ymax=yi+1maxyi+2max⋯yi+nmaxT. The minimum Ymin and maximum Ymax 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 Ymax, with which the safe cruising of the ego vehicle is guaranteed against collisions with the vehicles from the opposite lane. The selections of Ymin and Ymax are illustrated in Figure 9, where ysafety describes a safety zone of the vehicles.Figure 9
Determination ofYmin and Ymax.Finally, from (13), (16), and (17) the constrained trajectory optimization problem is formed as(18)minui…ui+n-112CTϕC+βTC+CTγsuch that the following constraints are also guaranteed:(19a)A-Ymin≥-BC(19b)Ymax-A≥BC(19c)C∈Cwhere 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 Ln. The current ratio ci can be computed online during the cruising of the vehicle.


## 3.3. 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 roadyi+1,ref must be modified according to the parameters of the requested lane. Moreover, in a lane changing maneuver the constraints Ymin and Ymax 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.


## 4. 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 ui…ui+n-1, which are equal to the ratios of the clothoid sections ci…ci+n-1. In the case of the tracking control the vehicle must follow the reference path yref=yi+1=y1 and the reference heading angle ψref=ψi+1=ψ1. The reference signals are computed from (10) such as(20a)yref=y0+L0ψ0(20b)ψref=ψ0+L0κ0+L022c0where y0,ψ0 are related to y(t),ψ(t) in discrete time, while L0 is the length of the forthcoming segment and κ0 is the result in the previous trajectory computation step. The parameter c0 is the first element among the results of the optimization u0.After the computation of the reference signals the tracking performances together with the control performance are defined as(21a)z1=yref-ytz1→min,(21b)z2=ψref-ψtz2→min,(21c)z3=δtz3→min.The performance vector is as follows: z=z1z2z3T.The model of the vehicle is described by the dynamical bicycle model; see [27]:(22a)my¨=Cfδ-ψ˙lf+y˙vx+Crψ˙lr-y˙vx(22b)Jψ¨=Cflfδ-ψ˙lf+y˙vx-Crlrψ˙lr-y˙vxwhere J is the yaw inertia of the vehicle, m is the vehicle mass, Cf,Cr are the cornering stiffness coefficients, and lf,lr are geometric parameters. The signal y˙ is the lateral velocity and ψ˙ is the yaw rate. The longitudinal velocity vx 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 x=ψ˙y˙ψyT, the control input is the steering angle u=δ, and the measured output vector is ym=ψyT. The state equation and the performances as well as the measurements are written as(23a)x˙=Aρx+Bu(23b)z=C1x,(23c)ym=C2x+Du,where ρ=vx 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 Figure10. 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 Ww,1,Ww,2 are related to the sensor characteristics on the lateral error and yaw error measurement. These weights are related to the sensor dynamics. Wz,1,Wz,2, and Wz,3 are the weights for the performances, which represent the minimization of z1,z2, and z3. The weights are formed as Wz,i=Ai/Tis+1 where Ai,Ti are design parameters. These parameters influence the balance between the performances z1,z2, and z3. Moreover, the role of the weight Wr,i is to consider the dynamics of the reference signals yref,ψref. The blocks also represent the dynamics of their variations.Figure 10
Closed-loop interconnection structure.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 ρ∈FP; 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 FP. The induced L2 norm of parameter-dependent stable LPV systems with zero initial conditions is defined as(24)infKsupϱ∈FPsupw2≠0,w∈L2z2w2,where w is the disturbance and z is the performance. The result of the optimization is the steering angle δ.


## 5. 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 atT=0.1s with n=20 the prediction points. The simulation is performed using the high-complexity vehicle dynamic software CarSim.The simulation is illustrated in Figure11. 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 Ymin and Ymax (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 yi+1,ref is set as shown in Figure 11(e). Then the velocity of the vehicle increases; see Figure 11(f).Figure 11
Simulation scenario.
(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 velocityThe numerical values of the lateral position and the velocity are illustrated in Figure12. The increase and decrease of the lateral positions at 100m and 200m, 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 20m, which has been kept by the ego vehicle. The initial velocity is 110km/h, which must be reduced to keep the velocity of the green preceding vehicle (90km/h); see Figure 12(b). At 500m 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.Figure 12
Simulation results.
(a)
Trajectory of the vehicle (b)
Velocity profile


## 6. 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.



---

*Source: 2195760-2018-12-31.xml*