Trajectory Planning for Autonomous High-Speed Overtaking in Structured Environments using Robust MPC

—Automated vehicles are increasingly getting main-streamed and this has pushed development of systems for autonomous manoeuvring (e.g., lane-change, merge, overtake, etc.) to the forefront. A novel framework for situational awareness and trajectory planning to perform autonomous overtaking in high-speed structured environments (e.g., highway, motorway) is presented in this paper. A combination of a potential ﬁeld like function and reachability sets of a vehicle are used to identify safe zones on a road that the vehicle can navigate towards. These safe zones are provided to a tube-based robust model predictive controller as reference to generate feasible trajectories for combined lateral and longitudinal motion of a vehicle. The strengths of the proposed framework are: ( i ) it is free from non-convex collision avoidance constraints, ( ii ) it ensures feasibility of trajectory even if decelerating or accelerating while performing lateral motion, and ( iii ) it is real-time implementable. The ability of the proposed framework to plan feasible trajectories for high-speed overtaking is validated in a high-ﬁdelity IPG CarMaker and Simulink co-simulation environment.


I. INTRODUCTION
The initial waves of autonomous driving cars are plying on public roads and successfully providing features such as lane-keeping, distance maintenance, lane departure, cruising, etc.Such systems have helped in improving safety on highways, occupant comfort while reducing driver workload simultaneously [1].However, human intervention or input is still required while performing more challenging, but equally common manoeuvres (e.g., lane-change, merge, overtake etc.).Overtaking represents a template for such complex manoeuvres as it (i) combines lateral and longitudinal motion of an overtaking vehicle (subject vehicle) while avoiding collisions with a slower moving obstacle vehicle (lead vehicle), and (ii) includes sub-manoeuvres i.e., lane-change, lane-keeping, and another lane change back to the original lane in a sequential manner [2] (see Fig. 1).Hence, the development of autonomous overtaking systems is under great focus since it unlocks the potential to perform a host of different manoeuvres and pushes the capabilities of autonomous vehicle further towards the overall goal of complete end-to-end autonomy.
The inherently intricate structure of overtaking stems from its dependence on a large number of factors such as road condition, weather, traffic condition, type of overtaking vehicle, type of overtaken vehicle, relative velocity, legislation, culture, etc. [3].Furthermore, each overtaking manoeuvre is unique in terms of duration of the manoeuvre, relative velocity between vehicles, distance travelled, etc. [4]- [8] thus making classification and standardisation difficult.Moreover, safely performing an overtaking manoeuvre requires accurate information of road and lane availability, lead vehicle trajectory, lead vehicle driving intentions, road conditions, etc.There are a variety of diverse ways proposed in literature for planning safe trajectories to perform an autonomous overtaking manoeuvre by treating it as a moving-obstacle avoidance problem.Incremental search based algorithms and sampling based trajectory planning methods such as 'Rapidly exploring Random Trees' (RRT) have been proposed for planning safe trajectories for autonomous overtaking [9]- [13].Even though algorithms incorporating basic vehicle kinematics within a RRT search algorithm have been proposed, the planned trajectories can be jerky which could lead to reduced occupant comfort.If accurate knowledge of road and surrounding obstacles is available, potential field based techniques are shown to be successful at generating collision free trajectories for avoiding stationary or moving obstacles [9], [14].However, while guaranteeing collision free trajectories, potential field based methods do not incorporate vehicle dynamics and hence cannot ensure feasibility of the planned trajectory [15]- [17].Model Predictive Control (MPC) helps address these shortcomings with its ability to formulate vehicle dynamics and collision avoidance constraints as a finite-horizon constrained optimisation problem.However, collision avoidance constraints for trajectory planning are generally non-convex which limits the feasibility and uniqueness of the solution of the optimisation problem.Researchers rely on techniques such as convexification [18], change of reference frame [19]- [21], create approximate linear collision avoidance constraints [22], [23], and shared control [24], [25] to address the issue.In [26] the concept of motion primitives is included within an MPC framework to plan collision avoidance trajectories.However, since these motion primitives were computed offline and accessed via a look-up table, only a subset of all feasible trajectories were considered for motion planning.In [2] overtaking trajectories were generated by directing the vehicle along virtual target points located at safe distances around the lead vehicle thus reformulating trajectory planning into a navigation problem.A similar approach inspired from missile guidance systems called Rendezvous Guidance was used to plan a trajectory for an overtaking manoeuvre [27], [28].However, in all these techniques the subject vehicle (SV) has been modelled as a point mass with no dynamics and hence these methods are unsuitable for high-speed trajectory planning of autonomous vehicles.For the brevity of the paper, interested readers are directed towards [29] for more details related to trajectory planning for autonomous overtaking.
In this paper, extracting the relevant benefits of each approach described in the literature, we propose a mathematical framework of potential field like functions and MPC for performing an autonomous high-speed overtaking manoeuvre.The framework is composed by three components (i) an artificial potential field, (ii) a target generation block, and (iii) a trajectory generation block.This paper is an extension of our previous work in [30] and builds upon the framework by (i) using a tube-based robust MPC technique to plan feasible trajectories over a larger range of vehicle velocities, (ii) development of collision avoidance constraints based on lateral position and velocity of the subject vehicle, and (iii) numerically validating the entire framework in IPG CarMaker-Simulink co-simulation environment.The potential field is used to map the surrounding region of the subject vehicle.Contrary to typical potential field approaches where an obstacle's position has been used to identify high-risk zones, the method in this paper combines an obstacle's position, orientation and relative velocity to create a map of safe zones surrounding the subject vehicle.At every sampling instant, the target generation block identifies the safest point of the road which is compatible with the dynamics of the subject vehicle and computes the reference state set point (e.g., velocity, lateral position, and heading angle) to be tracked.To achieve this aim of reaching the reference, the target generation block combines the safe zones in the potential field with the vehicle dynamics capability of the subject vehicle which are captured through the reachable set of the subject vehicle from its current state.Finally, the trajectory generation block uses a MPC strategy to generate feasible trajectories and steer the vehicle to the required reference (target) states.The robust tube based MPC approach in [31], [32] is used to solve the reference tracking problem.The dynamics of the lateral and yaw motion of a vehicle have a nonlinear relation with the longitudinal velocity.The robust tube based MPC formulation allows this nonlinearity to be modelled as an additive disturbance which allows the controller to plan feasible lateral motion (lanechange) trajectories over a large range of longitudinal velocities.Moreover, the robust MPC method proposed in [31], [32] guarantees (i) closed-loop stability, and (ii) persistent feasibility of the optimisation problem which is desirable for any model predictive control formulation [33].Additionally, a novel technique of designing collision avoidance constraints as a function of the longitudinal velocity and lateral position of the vehicle is presented.This technique differs from the ones in literature since the constraint design does not depend on the longitudinal position thus allowing the designers the possibility of reducing the state dimension of the system.This is beneficial as removing a state from the system model helps in reducing the dimension of the parameters space which helps in bringing down the memory and computational requirements for solving the optimisation problem.Hence, this paper represents practical use of the theory on the robust MPC presented in [31], [32] to design admissible, safe, and collision free trajectories for autonomous vehicles.The effectiveness of the entire framework for high speed autonomous overtaking is validated in a co-simulation platform where high-fidelity vehicle dynamics are simulated in IPG-CarMaker while the trajectory planning method with the MPC is implemented in MATLAB/Simulink.
The paper is structured as follows: Section II introduces the basic symbols and mathematical definitions used in the paper.The mathematical formulation of relevant vehicle dynamics and vehicle model structure to be used for controller design is discussed in Section III.In Section IV, the robust MPC approach in [31], [32] is briefly overviewed to give to the reader the fundamental details of this algorithm which has been used for trajectory planning.In Section V, the situation awareness system for the vehicle using potential field like functions is presented, while Section VI is dedicated to the design of the target generation block.The design of trajectory planning based on the MPC method in [31], [32] along with the design of the collision avoidance constraints is covered in Section VII.The effectiveness of the framework to support high speed overtaking is numerically shown in Section VIII.Finally, the concluding remarks are presented in Section IX.

II. MATHEMATICAL NOTATIONS AND DEFINITIONS
For a symmetric matrix M and vector x, ||x|| M denotes the weighted norm given by ||x|| M = √ x T M x.Given two sets U and V, such that U ⊆ R n and V ⊆ R n , the Minkowski sum is defined by For a system with states x ∈ X ⊆ R nx and inputs u ∈ U ⊆ R nu , whose dynamics are: where f (•, •) is the state function (linear or non-linear), R (t * ; x 0 ) denotes the reachable set at the time instant t * when the initial state is x(0) = x 0 and it is defined as with u(•) ∈ U being an admissible input in the time range [0, t * ] and x(t * ; x 0 , u(•)) is the solution of (1) with initial condition x 0 and input u(•) [34].
For solving the overtaking problem through the combined use of MPC and potential field, in addition to a coordinate inertial-frame (I-frame), three additional coordinate frames are exploited, i.e., vehicle-frame (V-Frame), obstacle-frame (Oframe), and road-frame (R-frame).The V-frame is located in the centre of gravity of the subject vehicle and follows the Roll-Pitch-Yaw (RPY) convention [35].Similarly, the O-frame is located at the centre of gravity of the lead vehicle and follows the RPY convention while the R-frame is a moving coordinate frame located at the projection of the origin of Vframe onto the innermost (rightmost) edge of the road with x-axis in the direction of the travel.A generic point on the road is denoted as p = (ξ, η), p r = (ξ r , η r ), p v = (ξ v , η v ), or p o = (ξ o , η o ) when expressed in the inertial, road, vehicle, or obstacle frame, respectively.The coordinate frames are depicted in Fig. 2 where w lane [m] is the width of the lane while shadow area denotes a rectangle moving along the roadframe with vertices V = {V 1 , V 2 , V 3 , V 4 }.The potential field is computed online within this region for situational awareness and thus the values of {V 1 , V 2 , V 3 , V 4 } are chosen in a range relevant for high-speed overtaking [5], [29], [36].Finally, T i j with i, j ∈ {I,V,R,O}, denotes the linear transformation from i-frame to the j-frame.Notice that, this transformation can be applied to either individual vectors or sets.When applied to a generic set ∆ ⊂ R 2 , T i j (∆) denotes the following set T i j (∆) {T i j (z)} z∈∆ .

III. CONTROL ORIENTED VEHICLE MODEL
A wide variety of vehicle models have been developed by researchers to study the dynamics of a vehicle and controller design for various applications [37].A comprehensive survey of vehicle model for trajectory planning in [15] list out the relevant vehicle models for this task.Moreover, the review paper for trajectory planning for autonomous high speed overtaking demonstrates that compared to point mass vehicle models, single track vehicle models (i.e., bicycle models) provide a suitable compromise between model order and model accuracy [29].A nonlinear kinematic vehicle model assumes no slip between tyre and road is found to be suitable for trajectory planning for highway driving when lateral acceleration is within bounds (|a y | ≤ 0.4 g) [38], [39] (see Fig. 3).Furthermore, since normal driving on the highway involves small steering inputs, small angles approximation for the side-slip angle and steering angle are often assumed [40], [41].Under this assumption of small angles approximation the vehicle bicycle model is: where ξ and η are the longitudinal and lateral displacement of the centre of gravity in the I-frame, ψ is the inertial heading angle, v is the velocity of the vehicle, l f is the distance of front axle from centre of gravity, and l r is the distance of the rear axle from the centre of gravity.The control inputs are longitudinal acceleration a x and front steering angle δ f .The two aspects that stand-out based on the system dynamics in (3) are: (i) nonlinearity in the system, and (ii) close dependence of longitudinal velocity on the lateral and yaw dynamics of the vehicle.To simplify the design of path planning, system in (3) might be linearised around a nominal longitudinal speed.However, the resulting lateral and yaw predictions of such linear system are valid only when the longitudinal speed does not deviate with respect to the nominal one.Hence, as a vehicle is expected to accelerate (and possibly decelerate) while performing the lane change and passing sub-manoeuvres of the overtaking manoeuvre, linearising this system around a nominal velocity might lead to inaccuracies in lateral and yaw predictions leading to unfeasible and/or unsafe trajectory generation.To tackle nonlinear vehicle dynamics systematically, authors have proposed (i) maintain constant vehicle longitudinal velocity during the lane change [2], (ii) design non-linear controllers [2], and (iii) successive linearisation [42].In this paper, model ( 3) is used for computing the reachability sets of a vehicle to identify safe driving zones, while for the generation of the vehicle trajectory toward a target point, model ( 3) is rewritten as a linear time invariant (LTI) system subjected to an additive bounded disturbance.This is achieved by denoting x a [ξ, η, ψ, v] T ∈ X a ⊆ R 4 as the system state and u [a x , δ f ] T ∈ U ⊆ R 2 the system input with X a and U being state and input convex constraint sets, respectively, system (3) can be recast as a linear parameter varying (LPV) system where v ∈ Proj v (X a ).System ( 4) is discretised with a sampling time t s to obtain the linear parameter varying discrete system shown below.
The pair ) can take values from the convex set P defined as with J ∈ {1, 2, • • • , J}, see [43] (chapter 3).Accordingly the dynamics of the LPV system (6) can be rewritten as a nominal LTI system subjected to an additive disturbance, i.e., where the pair (A dm , B dm ) is obtained by the expression below [43].
Moreover, the disturbance w a is defined as and thus is bound by the set W defined as It is noted that the structure of model ( 8) enables the use the robust tube-based MPC which is briefly revised in the following section.

IV. CONTROL FORMULATION
This section provides an overview of the robust MPC approach proposed in [31], [32].Compared to the classical MPC formulation [33], the advantage of the control method in [31], [32] is its ability to steer the state of a constrained system toward any set-point (i.e.desired target steady state) whether it belongs to the terminal set or not.The method guarantees the asymptotic convergence of the system state to any admissible target steady state.Furthermore, if the target steady state is not admissible, the control strategy in [31], [32] steers the system to the closest admissible steady state.Moreover, the optimisation problem to solve at each sampling time is a quadratic programming problem, which allows explicit implementation of the method, thus facilitating its deployment in real time.Given a discrete linear time-invariant system with states x ∈ X ⊆ R nx , inputs u ∈ U ⊆ R nu , outputs y ∈ Y ⊆ R ny , and bounded process disturbance w ∈ W ⊆ R nx , where X , U and W are known bounded convex sets, a discrete time state-space system is given by where the matrices A, B, C, and D are constant and it is assumed that the pair (A, B) is controllable.The control objective is to stabilize system (12) and steer it in the neighbourhood of a reference set-point despite the disturbance while keeping the system state and control input within the required set constraints (i.e., X and U, respectively) The solution proposed in [31], [32] leverages a nominal system of the plant in (12) defined as where x, ū, and ȳ are the state, input and output of the nominal model, respectively.The idea in [31], [32] to solve the constrained control problem for the uncertain system (12) is to use an MPC approach to steer the nominal model (13) towards the desired set point but with modified state and input set constraints, denoted as X , and Ū, respectively.The set constrains for the nominal model are selected such that if the closed-loop solution of the nominal system satisfies where K ∈ R nx×nu so that A K = A + BK is Hurwitz, and Z is a robust positively invariant set [44] for the system e(k + 1) = A K e(k) + w, with e (x − x), such that In [31], [32] it was proven that if X and Ū are non-empty sets they contain the steady state set-points and control inputs that can be robustly imposed to system ( 12) when e(0) = x(0) − x(0) ∈ Z, under the control action It is noted that, given a target steady state x ∈ R nx , the control action ū is generated by using a receding horizon technique to steer system (13) to an admissible steady-state ρ ss = (x ss , ūss ) ∈ X × Ū, such that xss is as close as possible to x.Moreover, the subspace of steady-states and inputs of system ( 13) have a linear representation of the form where θ ∈ R n θ is a parameter vector that characterises the subspace of steady-states and inputs and M θ is a matrix of suitable dimensions (see [31], [32] for further details).Furthermore, by denoting N as the prediction horizon, the control action ū at the time instant k is computed by solving the following optimisation problem parametrised in x p = x(k) and x. min where Ūi = {ū(0), ū(1), • • • , ū(N − 1)} is the vector of stacked inputs, Xi = {x(1), x(2), • • • , x(N )} the vector of stacked predicted states, and Φ and Ψ are the prediction matrices of appropriate dimensions constructed based on the the nominal system dynamics described in (13) resulting in a prediction model and the terminal set X t is chosen as with K Ω ∈ R nx+n θ being a constant matrix such that the eigenvalues of A + BK Ω lie within the unit circle, L = [K Ω , I nu ] M θ , and the cost function are positive definite, and P ∈ R nx×nx is a positive definite matrix solving the Lyapunov equation It is noteworthy that in the optimisation problem (18), the initial state of the nominal system x(0) = x is also a decision variable selected such that x p − x ∈ Z, which guarantees the evolution of the system (12) in X × U for any w ∈ W (see [31], [32] for further details).Therefore, the solution of the optimisation problem ( 18) yields an optimal initial state x * (x p , x) and an optimal input sequence } along with a parametrised steady-state θ * (x p , x).The net control action applied on the plant is given as

Remarks
• x, ūi , and θ are the decision variables of the optimisation problem (18), while x p and x are its parameters • The terms of the cost function under the summation represent the penalty for deviating from the steady-state and input, the second term penalises the deviation of the terminal state from the steady-state, and the final term penalises the deviation of the artificial state from the reference state • As the optimisation problem ( 18) can be expressed as a quadratic programming problem, it can be converted to an explicit MPC form to reduce online computations [45] • System constraint handling capabilities and closed loop asymptotic stability and feasibility of the proposed controller are proven in [31] • The minimal robust invariant set Z can be computed using the recursive algorithm proposed in [44] V. LOCAL RISK MAP In this paper, it is assumed that the vehicles (subject vehicles and other traffic vehicles) are travelling on a oneway straight road of infinite length.At highway speeds, in addition to maintaining approximately a lane-width's distance with each vehicle in the lateral direction, vehicles also maintain safety distances of ≈ 50 m to the vehicle in front and behind [5].Therefore, an overtaking manoeuvre is expected to maintain these distances while performing the lane-change manoeuvres that mark the start and end of an overtaking manoeuvre resulting in the need for a subject vehicle to have accurate situational awareness of the surrounding obstacles in this range to plan safe trajectories.The authors in [46], [47] mentioned that embedding driving rules and collision avoidance constraints within a multi-objective optimisation problem results in a control laws with large computation requirements.On the other hand, a potential field like function for environmental risk detection can be shaped in such a way that it guides towards desired driving behaviour.In this paper the surrounding environment is described through the use of a potential field where several road elements (i.e., road limits, road markers, and other road users) are considered for shaping the potential function so as to include driving rules and guide the subject vehicle through safe road regions.The net potential function is generated by combining several potential functions where the design of each function is intended to incorporate one or more driving rule(s).The road potential function (U road ) is designed to keep the subject vehicle away from the road limits, the lane potential function (U lane ) is used for lanekeeping, the lane velocity potential function (U vel ) is designed such that the subject vehicle occupies the innermost (slowest) lane when more than one lane is available, and the car potential function (U car ) is designed such that a subject vehicle either maintains a safe distance to the lead vehicle or if the other lane is available, moves to a faster lane.Similar to the approach presented in [48], a net potential function (U r ) is generated by superimposing these individual potential functions to create local risk maps that can be used for autonomous overtaking in a human-like manner.The construction of the individual potential functions is discussed below.

A. Lane Velocity Potential
Different lanes on a road have an implicit velocity associated with them, i.e., the velocity progressively increases from inner (right-most) to outer (left-most) lane.Thus, if one assumes that higher-speeds represent higher-risk, each lane of the road can be appropriated a certain potential to describe its risk.This is achieved by a simple gain-based function as shown below.
where γ is a gain factor, v lane,i is the nominal velocity of the i th lane, and U vel,i is the potential due to lane-velocity of the i th lane.

B. Road Potential
The road potential [48] is designed such that the boundaries of the road have the highest (∞) potential and the centre of the road has the lowest potential.A function often used in robotics for perception is used here to describe the road potential and is given below.
where ζ is a scaling factor and η r,b is the y-coordinate of the b th road edge, b ∈ {1, 2}.

C. Lane Potential
A lane potential function [48] creates a virtual barrier between lanes to direct the subject vehicle towards the lane centre.A Gaussian function shown below is used to achieve this desired behaviour.
Where η l,i is the y-coordinate of the i th lane division, σ and A lane are scaling factors, and U lane,i is the potential due to lane boundary of the i th lane.

D. Car Potential
A technique inspired by [48] is used to embed lead vehicle position, orientation, and velocity within the potential function as an obstacle vehicle.By modelling the lead vehicle as a rectangular area, virtual triangular wedges, also denoted as buffer zones, are appended to the front and rear of the lead vehicle which act as safety margins.The location (x, y coordinate) of triangle's vertex behind the lead-vehicle is calculated based on the velocity of the subject vehicle and the headway time h t while the location of the triangle's vertex in front of the lead-vehicle is calculated based on the velocity of the lead vehicle and the headway time h t .By denoting Γ lv as the set of coordinates in the R-frame containing the obstacle vehicle and the two triangular wedges, a Yukawa function is used to describe the potential due to an obstacle vehicle as given below.
where α is a Yukawa scaling factor, A car is the Yukawa amplitude [49], and K d is the Euclidean distance to the nearest coordinate of the obstacle given as where b 0 represents the set of points lying within the obstacle.These individual potentials are superimposed to obtain an overall risk map in the surrounding of the vehicle given by the expression below.
Where U lane = . By assigning a threshold limit U safe , the safe regions of the road surrounding the subject vehicle are expressed in the inertial frame using the set Thus, equation ( 30) provides a set of safe regions and the subject vehicle needs to plan trajectories that keep it within this region set thereby reducing risk.Moreover, since the net potential field depends on the states of the subject vehicle (longitudinal position, lateral position, and longitudinal velocity) and the lead vehicle (longitudinal position, lateral position, and longitudinal velocity), it updates at each time instant to provide an accurate environmental representation for a subject vehicle.However, the set (30) does not consider vehicle dynamics of the subject vehicle, thus some regions of the road with satisfactory potential may not be reachable in practice.The method designed for selecting reference points in the set of safe regions which are compatible with the dynamics of the subject vehicle is detailed in the next section.

VI. SELECTION OF THE TARGET POINT
In this section, the method designed for selecting reference points in the set of safe regions which are compatible with the dynamics of the subject vehicle is detailed.In ideal highway cruising conditions, a vehicle is expected to traverse along at a constant desired longitudinal velocity v des while maintaining its lane position.While travelling on a straight road, these dynamics of the system from (3) can be described by ẋa = [v des , 0, 0, 0] T .However, in real world scenarios, a vehicle is unable to maintain constant longitudinal velocity and lane position (due to traffic, route, etc.) and has to perform different manoeuvres such as lane-change, merge, etc.These manoeuvres can be thought of as transitions from one set of states to another set of states within the set X ac = {x ∈ X a : ψ = 0}.In such ideal scenarios the objective of the subject vehicle is to adjust its trajectory to avoid obstacles while ensuring that the vehicle's speed is maintained within the range v ∈ Proj v (X a ).Starting from an initial position p 0 = (ξ 0 , η 0 ) and travelling at v des , using admissible control actions from the set {(a x , δ f ) : a x ≤ 0, (a x , δ f ) ∈ U }, the set R total ⊂ R 4 of the vehicle configurations (states) reachable without exceeding the desired velocity v des in the time interval [0, t * ] of the system can be computed using ( 2) and the vehicle model (3).The set of points on the road that are reachable R ⊂ R 2 form a subset of R total and is expressed as

Remarks
• The velocity v des corresponds to the maximum velocity of the SV as desired by the occupants and it is upper bound by the legal speed-limit of the road.• Thus, from a given initial position p 0 , the subject vehicle can theoretically reach all points lying within the set R without exceeding the maximum desired velocity v des .It is noteworthy that the set of admissible control actions mentioned above is a subset of U and is used only for computing the reachable set, the trajectory planning algorithm will have the entire set U at its disposal for generating feasible trajectories.From ( 30), (31) the safe zones surrounding the subject vehicle which are reachable with respect to the current vehicle state and vehicle dynamics is Then, the reference target coordinates p = ( ξ, η) are chosen from R with the aim to maximise the distance travelled by the subject vehicle in the time interval [0, t * ], i.e.

p = arg max
The longitudinal distance from ξ 0 to ξ can be traversed by the SV by travelling with a uniform longitudinal velocity calculated using the equation below.
A vehicle with the ability to closely match or follow the reference velocity computed above will enhance its ability of get closer to the reference position p.Thus, if the initial velocity v 0 of the vehicle is not equal to the target velocity v, the trajectory planner should come up with a suitable acceleration profile to accelerate/decelerate the vehicle to achieve the target velocity v.Moreover, since the subject vehicle is assumed to be travelling on a straight road, the target heading angle of the subject vehicle remains It is noteworthy that in case the subject vehicle is travelling on a curved road, target heading angle ψ can be obtained from the road orientation at the given coordinate ( ξ, η) stored in the vehicle's mapping functionality.Thus, stacking the reference targets for each state the target state vector xa = [ ξ, η, ψ, v] T for the system is obtained.It is noted that, the set of reachable lateral and longitudinal coordinates for subject vehicle in the vehicle frame is VII. TRAJECTORY GENERATION The target states xa which are generated using the approach in the Section VI at each time step result in piecewise references (e.g., if a lane-change is required, η will change from the centre of one lane to another).The robust MPC approach overviewed in Section IV is used in the proposed framework to plan trajectories for directing the vehicle from its current state x a (0) = [ξ 0 , η 0 , ψ 0 , v 0 ] T to a (safe) target state xa = [ ξ, η, ψ, v] T in an admissible way (i.e. by considering vehicle dynamics, state constraints, and input constraints).As the dynamics of the state ξ of system (4) depends only on v, it is possible to further simplify the system for the trajectory generation.The reduced order system for trajectory generation is where x = [η, ψ, v] T is the system state, u = [a x , δ f ] T is the input, w is the disturbance vector composed by the last three entries of the w a -term in (10), and the system and input matrices A and B are obtained by extracting the appropriate rows and columns of A dm and B dm in (8), respectively.The state and input constraints polyhedrons X and U are where x min , x max ∈ R 3 and u min , u max ∈ R 2 are constant vectors.It is noted that the boundedness of X and U and the structure w a -term in (10) imply that the w-term in (8) belongs to a bounded polyhedron set denoted as W [43]. From (37) and (38), it follows that the vehicle dynamics of interest for the overtaking manoeuvre match the hypothesis required for the application of the robust MPC in Section IV, which is therefore used for the generation of a feasible path to steer the vehicle toward x = [η, ψ, v] T belonging to the safe reacheble set (32), where η, ψ and v are defined in the section above.

A. Collision Avoidance Constraints
The basic tools that are used to construct the potential field for situational awarness can also be used to obtain collision avoidance constraints that can be added to the optimisation problem in (18).An example demonstrating how the collision avoidance constraints can be designed while approaching a lead vehicle is explained using Fig. 4 as an exemplar.While designing the potential field in Section V-D, the equation a ca ξ + b ca η + c ca = 0 is one of the hyperplanes that is used to construct the bounds of the unsafe region around the lead vehicle (Γ lv ) .However, the utility of this hyperplane is expanded by using it to divide the given road segment into two zones; (i) safe zone represented by a ca ξ + b ca η + c ca > 0, and (ii) unsafe zone represented by a ca ξ + b ca η + c ca < 0, see Fig. 4.
Thus, for a subject vehicle located at (ξ 0 , η 0 ), an MPC based trajectory planner can ensure collision-free motion if constraints are designed that limits all planned trajectories to stay within the safe zone.This is the crux of the various collision avoidance constraints that are described in literature [19], [50].However, as discussed in the section above, in this paper a reduced order system that does not have longitudinal position ξ as one of its states is used by the MPC for planning trajectories.This gives rise to the need of expressing the collision avoidance constraints using the states from the reduced order system i.e., η and v.
1) Constraint I: If (ξ 0 , η 0 ) represent the current location of the SV in global coordinates and in the context of MPC are known values then the satisfaction of the following constraint equation guarantees that initially the subject vehicle is within the safe zone.
2) Constraint II: Similarly for a given nominal initial state x(0) = (η, ψ, v), which is a part of the decision variable of the problem in (18), the equation below ensures that the nominal initial position of the SV is also within the safe zone.
3) Constraint III: Finally, it is important to ensure that the trajectory obtained by solving the optimisation problem in (18) guarantees that the SV stays within the safe zone throughout the prediction horizon.From (3a), ( 4) it is evident that the evolution of the longitudinal position ξ is a function of the longitudinal velocity of a vehicle v. Thus, along a given prediction horizon N , the predicted nominal longitudinal position ξ can be estimated using the initial longitudinal position ξ 0 and the predicted nominal velocity v using the equation below.
The expression above is utilized to create N different constraints that fulfill the collision avoidance criterion along the entire prediction horizon.The generalized constraint equation that is used to create the N different constraint equations is given below.
where the predicted nominal velocity v(i) and predicted lateral position η(i) can be obtained from the prediction model in (19).Therefore, the set of (N + 2) equations obtained from ( 39), (40) and from different values of j in (42) represent collision avoidance constraints that are expressed solely as a function of two states namely lateral position and longitudinal velocity.These inequalities representing the collision avoidance constraints can be supplemented to the constraints of the optimisation problem in (18) to ensure that the planned trajectory is collision free along the entire prediction horizon.It is noteworthy that the technique for design of the collision avoidance constraints described above can be easily adopted to situations where (i) the SV needs to perform the lanechange while completing an overtaking manoeuvre and/or (ii) when there are multiple hyperplanes representing collisionavoidance constraints for more than one traffic member.
At each discrete time instant k, problem in (18) with additional constraints (39), (40), and ( 42) is solved by setting the target state and the initial state as x = [η, ψ, v] T and x p = x(k) respectively.The optimal trajectory x * = [ξ * , η * , ψ * , v * ] T is generated by simulating the vehicle model (3) with the optimal inputs u * = [a * x , δ * f ] T from the solution of MPC problem (18) and then passed to a trajectory tracking controller as reference signals.The following algorithm (depicted in the closed loop structure in Fig. 5) summarises all the steps required for performing a safe overtaking manoeuvre in the proposed framework, see Algorithm 1.

VIII. NUMERICAL RESULTS
In this section, a results obtained from a closed-loop simulations are used to evaluate the ability of the proposed framework for planning trajectories for a high-speed overtaking manoeuvre.The scenario used is as follows: both the subject vehicle and the lead vehicle are travelling on a two-lane oneway road of infinite length at longitudinal velocity v and v LV , respectively.The dimensions of the road, lane-limits and lead vehicle's states are available to the subject vehicle ondemand through for example a V2X communication link.Each lane of the highway is assumed to have a nominal desired velocity which is provided to the subject vehicle by the route planner while the decision to perform an overtaking Algorithm 1 Trajectory Planning 1: initialize: 2: R V ← bank of reachable sets in V-frame 3: U safe ← upper bound of risk potential 4: procedure GENERATETRAJECTORY 5: top: G ← safe regions of the road as (30) 13: R safe ← G ∩ R as (32) 14: xa ← generateT argetStates(R safe ) as ( 33)- (35) 15: x a ← getCurrentStateV ector(•) 16: getCollisionAvoidanceConstraints(•) as ( 39)- (42) 17: u * ← solveRobustM P C(x, x) as (23) 18: x * ← applyOptimalInput(u(k)) as (3) 19: if user request change in v des then 20: goto top.goto loop.manoeuvre and availability of the faster lane is verified by the decision making block of the SV [51], [52].The design parameters, state, and input set constraints in Table I are used to set up the scenario and controller within an integrated Simulink and IPG CarMaker co-simulation platform.It is noteworthy that, the constraints for the inputs were designed by considering the steering and longitudinal acceleration applied by an inbuilt IPG CarMaker controller for several smooth highspeed overtaking manoeuvres in CarMaker.Furthermore, the control weights were chosen so as to ensure that the generated trajectory was similar to the one obtained via IPG CarMaker's default lane-change trajectory.Alternatively, other techniques can be used to tune the control weights and a comprehensive review of such techniques is available in [53], [54].

Remarks
• As discussed in the section above, the optimal trajectory generated by the trajectory planner acts as reference signal for a lower level trajectory tracking controller, see Fig. 5.The trajectory tracking controller is responsible for actuating the steering, accelerator/brakes to follow the reference trajectory as closely as possible while handling system non-linearities and disturbances.In this paper, the optimal velocity v * obtained from the robust MPC is passed on to a longitudinal tracking controller as a reference signal.The longitudinal tracking controller is sensitive to the powertrain delays and factors them in while computing an appropriate longitudinal acceleration signal for the SV.On the other hand, the lateral tracking is performed by an adaptive controller that uses x * as its reference to compute appropriate steering action [?].In addition to tracking the reference trajectory as closely as possible, these lower level controllers can handle system delays, tire nonlinearities, road surface variations, etc.However, the task of longitudinal and lateral tracking can also be performed by the multitude of techniques available in literature but is beyond the scope of this paper (the reader is referred to [29], [55]- [58] for possible alternative techniques).• The entire co-simulation was run on a laptop machine with an Intel i7-6820HQ processor, 16GB RAM running Microsoft Windows 7 64-bit, and MATLAB 2012b 32bit.The average time required at each time step for the optimisation routine was 0.0077 s with a standard deviation of 0.0011 s.

A. Robust positive invariant set and MPC implementation
The robust positive invariant set Z for the error dynamics and the nominal control law in (15), ( 16) is calculated using the algorithm in [44].The algorithm in [44] provides an iterative technique based on the supporting function of polytopic sets to calculate the outer approximation of a minimal robust positively invariant set for a discrete-time linear time-invariant system.Equation (15) suggests that the structure of the set Z has a dependence on (i) size of set W, and (ii) the matrix A K .Since, the set W is fixed by the vehicle geometric constraints and chosen longitudinal velocity range (see Table I), the only degree of freedom available for designing the set Z is via the design of a Hurwitz matrix A K by choosing an appropriate controller K to ensure stable error dynamics.The tradeoff for the design of the nominal controller with fixed gain K (or equivalently the design of the matrix) are twofold; (i) to constraint the error set Z to a reasonable size such that the deviation between nominal system and actual system is reduced and (ii) to ensure that the input set Ū for the MPC is as large as possible, thus enlarging the decision space for the MPC to compute smooth control inputs.Furthermore, it was noted that A K -matrices with eigenvalues close to the origin of the complex plane might result in an empty Ū, while if the eigenvalues of A K are close to the unit circle, the set Z might become so large that X is an empty set, thus both extreme cases make the MPC problem in ( 18) ill-posed.For this application, this trade-off was met by selecting the dominant eigenvalue λ of A K for the lateral and yaw dynamics such that Z is a bounded set and Ū is as large as possible.
Fig. 6 provides a visual representation for this behaviour where the plot on the left depicts the disturbance set W and the robust positively invariant set Z for a given controller, whereas the plot on the right depicts the net input set U and the constrained input set Ū.It is noteworthy that only a projection of the disturbance and error sets onto the lateral and yaw dimension of the system is plotted since the disturbance for the system exists only along these dimensions, see (10).Furthermore, by increasing the dominant eigenvalue beyond λ = 0.72 results in a large Z that renders X = φ.Likewise, the plots show that the error set grows along the lateral position dimension as the eigenvalue changes whereas the absolute limits along the heading angle dimension are close to constant.However, even for the error set Z obtained with the larger eigenvalue, the magnitude of the error limits in lateral position is a small fraction of the actual limits in lateral position while allowing a large Ū making it a suitable choice for being used to solve the MPC problem in (18).

B. Simulation Results
A simulation environment is initialised with the subject vehicle behind the lead vehicle and the initialisation parameters given in Table I.The simulation is then allowed to run and the proposed framework performs three primary tasks; (i) surrounding risk zone detection, (ii) safe target identification, and (iii) trajectory generation at each sampling time.Some details for each task output as well as overall simulation results are discussed below.Figs.7a and 7b shows the snapshot of the output of the local risk map and target point selection at the time instant t = 14 s during the overtaking when the subject vehicle has detected the lead vehicle as it is performing the first lane change of the overtaking manoeuvre.Fig. 7a provides a 3D-view of the entire potential function computed as in (29) and the local minima at the centre of each lane for guiding a subject vehicle can be seen along with the trapezoidal field created by a lead vehicle (it is noted that in Figs.7a and 7b large values of the potential field are truncated for the sake of readability of the figure).Significantly, the potential field approach can be expanded to accommodate more lanes, additional traffic members, and/or more complex road geometries.Furthermore, the computation of potential fields is based on simple mathematical operations and hence addition of traffic participants, more lanes, etc. will not result in any significant computation overhead.Similarly, the design of collision avoidance constraints relies of basic mathemat-Fig.6. Error polyhedron and resulting tightened input set obtained by changing magnitude of eigenvalue ical operations and thus collision avoidance constraints for each traffic participant can be generated without any major computation overhead.However, the design of potential fields for different road types is not the primary focus of this body of work and hence not discussed in greater detail.Fig. 7b depicts the level curves of the potential field for the same time instant in the R-frame along with the reachable set of the subject vehicle and identified target on the road computed as in (33), which also represents the output of the block Target Id.In Fig. 7b, the lead vehicle is depicted as red rectangle and the buffer zones (as triangular appendages), where the potential field rapidly increases to prevent the subject vehicle from getting too close to the lead vehicle during the different phases of an overtaking manoeuvre, can be easily observed.As the region Γ lv (unsafe region) surrounding the lead vehicle moves in the R-frame with speed v LV −v, at each time step the local risk map of the safe reachable region and the reference targets change accordingly.Fig. 8 shows some of the target references selected by the subject vehicle for safely overtaking.The reference points, dynamically generated, are used by the Trajectory Generation block in Fig. 5 the generation of the trajectory as described in Section VII.The results from the entire simulation are depicted in Fig. 9.The trajectories of the subject and lead vehicles as well as the relevant states and inputs of the subject vehicle are shown in the inertial frame in Fig. 9.Moreover, Gaussian noise is added to the lead vehicle's velocity in an attempt to (i) introduce sensor imperfections, (ii) wireless network packet loss, and (iii) lack of accurate knowledge of lead-vehicle states.Introducing this noisy signal in the potential field calculation in (27) will help in understanding if the proposed technique is robust against the random variations in lead-vehicle states.The top plot shows the actual path followed by the subject and lead vehicles and the trajectory of the overtaking manoeuvre for the subject vehicle can be observed.Moreover, since the subject vehicle is travelling with a higher longitudinal velocity, it covers a larger portion of the road segment in the given time.The bottom four plots of Fig. 9 show the states and input of the subject vehicle evolving over time.The key aspect about the overtaking manoeuvre is that the overtaking manoeuvre is initiated close to 10 s and one can observe the longitudinal velocity of the vehicle increasing while the first lane change manoeuvre is being performed.The reverse behaviour (i.e., decreasing velocity while performing the lane change) is visible after 20 s.This is reminiscent of a real-world overtaking manoeuvre where a vehicle may accelerate or decelerate while performing the lane change manoeuvre(s) thus demonstrating the efficacy of the proposed controller.The noisy data from the lead vehicle's velocity also does not have any impact on the trajectory planning process as both the states and input signals are devoid of high-frequency oscillation.Another key aspect is that the two states of SV, (i) longitudinal velocity and (ii) heading-angle show smooth evolution without any highfrequency oscillation during either of the lane-changes.The longitudinal acceleration profile is obtained via the tracking controller discussed above and it also does not demonstrate any high-frequency oscillations.However, it is designed using basic frequency-based techniques and is not tuned to minimize the jerk but if required this controller can be swapped with any preferred control technique available in literature.Similarly, the steering action for the lateral motion demonstrates smooth evolution with no high-frequency oscillation.Moreover, just as in the case of the longitudinal tracking controller, if necessary another controller for the steering action can be utilized with the proposed trajectory planning framework.Also, as expected the MPC controller respects all the system and input constraints which is evident from the plots in Fig. 9.
To show the need of the robust MPC to tackle variations of the longitudinal vehicle speed while performing the overtaking manoeuvre, we compare the performance of the proposed framework when the robust MPC in the Trajectory Generation Block in Fig. 5 is replaced by the MPC strategy for disturbance free LTI systems proposed in [30], referred in the rest of the paper as nominal MPC.The LTI vehicle model for the design of the nominal MPC is obtained from system (13) based on (9), while the remaining vehicle parameters for the control tuning are set to those listed in Table I.It is noted that, despite the fact that nominal MPC is effective for overtaking with fixed speed (see [30] for further details), its Fig. 9. Simulation Results: SV and LV trajectories, longitudinal velocity, heading angle, longitudinal acceleration, and steering angle for a high-speed overtaking manoeuvre.Note: (--) are the system constraints to generate suitable trajectories for the overtaking manoeuvre with varying longitudinal velocity.The trajectories suffer from overshoot and also takes the subject vehicle very close to the lead vehicle during the initial lane change.Both these factors make the nominal MPC based technique unsuitable for planning overtaking trajectories with variable velocity.On the other hand, the robust MPC based trajectory generates very little overshoot and also maintain the safety margins to the lead vehicle during all three sub-manoeuvres.Furthermore, due to its ability to generate consistent and uniform trajectories for lane change while accelerating and decelerating, the controller proposed in this paper appeals to a wider application set (lane-change, merging into traffic, etc.).It is noted that in the proposed approach, the parameters of the MPC strategy (i.e., Q, R, P, T, and N ) can be tuned for adjusting the aggressiveness of a manoeuvre.Additionally, at each time step the optimisation problem underlying the robust MPC techniques is always feasible according to Theorem 2 in [31].IX.CONCLUSION In this paper, a modular control framework for autonomous high-speed overtaking was presented with (i) Local Risk Map generation, (ii) safe target identification, and (iii) trajectory planning being the different modules of the system.In this framework the onus of situational awareness lies with the local risk map and safe zone identification sub-systems and the onus of feasible and collision-free trajectory generation lies with the MPC controller.This modular design allows the framework to avoid non-convex constraints allowing for an MPC formulation that can be solved using commonly available optimisation solvers.Moreover, a robust tube based MPC technique with the nonlinearities in lateral and yaw dynamics due to variation in longitudinal velocity being modelled as additive disturbances has been used.Additionally, a novel technique for designing collision avoidance constraints based only on lateral position and longitudinal velocity of the subject vehicle was presented.This allows the trajectory planning controller to generate feasible and safe trajectories with admissible inputs even while performing lateral manoeuvres with changing longitudinal velocity.Numerical results in a Simulink/IPG CarMaker cosimulation environment demonstrated that the algorithm is able to fulfil the safety considerations for high speed overtaking manoeuvre and generate trajectories which are also compatible with the vehicle dynamics and safety considerations.Furthermore, comparing the results of the technique proposed to a normal MPC demonstrated the added benefits of the robust based approach.As future work the proposed framework will be extended to (i) more challenging overtaking scenarios with multiple traffic participants, external disturbances, etc. and (ii) other manoeuvres under different road geometries.

N lanes i=1 U
lane,i and U vel = N lanes i=1 U vel,i with N lanes being the number of lanes.To facilitate trajectory planning the potential field is studied in the inertial frame through the use of the function U (p) acaξ + bcaη + cca = 0

Fig. 4 .
Fig. 4. Schematic to explain identification of collision avoidance zone.Note: SV -blue rectangle, LV and surrounding unsafe region -red polygon, target coordinate -magenta cross, safe zone -green polygon

6 :
v des ← desired longitudinal velocity from user 7: R total ← reachable set for given v des as(2) 8:R ← projection of R total in I-frame as(31) 9: loop: 10:U r ← net potential field in R-frame as(29) 11:U ← net potential field in I-frame12:

Fig. 8 .
Fig. 8. Snapshot of simulation demonstrating: reference targets (×) for different configurations of subject vehicle ( ) and lead vehicle (♦) while driving on a highway.Note: solid lines (-) are the road boundaries and dashed line (--) is the lane marking