Hierarchical control for trajectory generation and tracking via active front steering

A new hierarchical model predictive controller for autonomous vehicle steering control is presented. The controller generates a path of shortest distance by determining lateral coordinates on a longitudinal grid, while respecting road bounds. This path is then parameterized by arc length before being optimized to restrict the normal acceleration values along the trajectory’s arc length. The optimized trajectory is then tracked using a nonlinear model predictive control scheme using a bicycle plant model to calculate an optimal steering angle for the tires. The proposed controller is evaluated in simulation during a double-lane-change maneuver, where it generates and tracks a reference trajectory while observing the road bound-aries and acceleration limits. Its performance is compared to a controller without path optimization, along with another that uses a smooth, predetermined, reference path instead of creating its own initial reference. It is shown that the proposed controller improves the tracking compared to a controller without path optimization, with a four-times reduction in average lateral tracking error. The average lateral acceleration is also reduced by 6%. The controller also maintains the tracking performance of a controller that uses a smooth reference path, while showing a much greater ﬂexibility due to its ability to create its own initial reference path rather than having to follow a predeter-mined trajectory.


Introduction
Autonomous driving technology has become an increasingly popular research topic [1], in both academic and industrial environments. Efforts have primarily focused on * Corresponding author. environment detection, path planning and vehicle control. For control of the lateral response of the vehicle, research has focused both on conventional methods of lateral control such as Active Front Steering (AFS), either as a driver aid [2], or as an autonomous implementation, along with new techniques made possible using additional actuators. Direct Yaw-moment Control (DYC) [3] via either differential braking or active torque distribution to manipulate the longitudinal force of individual tires, is one such example of a control technique that is able to achieve more control authority by utilizing longitudinal tire forces, in a manner that is not achievable by human drivers.
To successfully implement autonomous vehicle behavior such as AFS, a control methodology which can handle complex, time-varying constraints and is adaptable to varying system parameters and passenger preferences in the problem definition is required. Model Predictive Control (MPC) is one such method [4,5], as the use of optimization allows constraints to be actively considered in the design of the controller. MPC has many applications in automotive contexts, see [6] for examples. It has been shown to be effective at controlling Multi-Agent Systems [7], highlighting its utility in autonomous driving, where a fleet of vehicles communicate with one another to control and optimize their collective behavior.
In particular, nonlinear MPC [8,9] allows for the use of higher-fidelity plant models, that give a more accurate portrayal of vehicle behavior. It allows for the inclusion of nonlinear constraints into the control problem, allowing for the diverse types of constraints present in autonomous driving, such as acceleration and road constraints, to be formulated explicitly into the problem formulation. This provides for the intuitive inclusion and modification of constraints, allowing for the control designer to change the dynamic performance to suit the personal preferences of passengers or specific behaviors of the autonomous vehicle.
It has always been a concern however, that due to the nature of MPC solving an optimization routine online and inthe-loop, that it is computationally expensive and prohibits its application to controllers needing operating frequencies of one cycle per second (Hz) and greater. From a computational complexity point of view, the value of the preview horizon is particularly important [10], so it is beneficial to use as small a value as possible, yet the preview horizon has a significant effect on both controller performance and stability.
A way of reducing the preview horizon while maintaining both stability and performance is therefore vital. Hierarchical multilayer control [11] is such a method and has been used in several scenarios related to vehicle dynamics and autonomous driving functionality [12,13]. The upper level controls the slower dynamics of the system in consideration; specifying a trajectory for the lower levels to follow by using a large value for the preview horizon. The lower level uses a higher frequency to control the faster acting dynamics within the system allowing it to calculate the optimal control action over a shorter timescale.
Utilizing different prediction horizon lengths provides some stability to the control problem. The high-level control has an awareness of potential events long before they come into consideration by the low-level controller. This allows for the upper level reference generator to provide a revised trajectory, which mitigates difficult scenarios, such as sudden step changes to road bounds, for the lower level to track. This is done before the lower level controller, which incorporates dynamics that are more complex, must optimize the control variables to traverse a difficult scenario.
There are several reasons why a hierarchical framework is an attractive control methodology for complex tasks such as path planning and active steering control; it allows for the logical splitting of the control task into separate, discrete modules. Additionally, the hierarchical control framework is analogous to those that exist to model human driver behavior [14,15]. The ability to split the different control tasks, so that they can be addressed separately, is also beneficial as it simplifies the control approach.
In [12] a two-tier control framework is presented consisting of path optimization, where a reference trajectory for the vehicle's lateral displacement and yaw angle, generated via trigonometric functions, is optimized to ensure that the paths normal acceleration does not exceed a specified limit. This optimal path is then passed to a nonlinear model predictive controller, which calculates an optimal steering angle to track the target lateral displacement and yaw angle. This controller was evaluated on a simulation vehicle in a low friction environment during a double-lane-change maneuver. The controller did not constrain the lateral displacement, which limited its application to highway environments. It also required a pre-determined reference trajectory to be provided, which limits it to pre-existing scenarios.
In this paper, a Hierarchical Model Predictive Control (HMPC) framework is presented that extends the model presented in [12] with two key aspects of functionality, during constant speed scenarios. A third control module is added, termed the Path Generation Module (PGM), at the start of the optimization scheme, to generate an initial path without the need to specify one via the composition of trigonometric functions. The path is then parameterized by arc length so that it can be addressed in the time domain.
This combination of path generation, path optimization and vehicle control was chosen as it is analogous to the model proposed by Michon [16,17] for a driver's model of the driving task. In this model, driving is split into high-level strategic planning, more immediate tactical driving and the low-level control of the vehicle.
Road bounds are also included in the control problem to ensure that the vehicle is still within the safe sections of the road. In this paper, it is assumed that the upper and lower lateral bounds are supplied to and utilized by each layer of the hierarchical controller, after being detected and determined via an appropriate method, such as via LIDAR or a computer vision system (see, for example, [18,19]).
The second stage is the Path Optimization Module (POM) then constrains the normal acceleration of the trajectory, via path curvature, as in [12]. To accommodate the nature of the PGM, the POM calculates a yaw angle for a single steered axle vehicle to track. When calculating the yaw angle, it penalizes excessively large values of yaw angle which helps with stability in certain scenarios, such as highway driving.
The final stage of the controller uses a nonlinear vehicle model to track this reference trajectory by manipulating the steer angle of the front tires. Evaluation of the controller is completed using a simulation model of an autonomous vehicle completing a double-lane-change maneuver in MAT-LAB [20] at two longitudinal speeds.
The paper is structured as follows. In Section 2 the vehicle models used by the controller and for the simulated vehicle are presented. Section 3 details the controller methodology and implementation used in this paper. Finally, Section 4 describes the driving maneuver used to evaluate the controller performance, along with the simulation results.

Vehicle model
A five state nonlinear bicycle model is utilized in the simulations. The lateral and yaw acceleration of the vehicle is modelled along with the lateral and longitudinal displacement in a global reference frame for coordinate tracking. It is assumed for the analysis that the vehicle is travelling at a constant longitudinal speed u. Because of this, the normal force acting upon the tires is modelled as the vehicle's static weight distribution. Due to the tire model's nonlinear dependence on normal load, the vehicle model assumes two tires at each axle, each with half the axle load acting upon it. Therefore, the state vector of the system is z z z = ( v, ω z , ψ, X, Y ) T . The lateral velocity of the vehicle in its local reference frame is denoted as v, while ψ is the yaw angle of the vehicle defined as the rotation angle between the local and global longitudinal axes. Its derivative is ω z , the vehicle's yaw rate. X and Y are the global coordinates of the vehicle's center of mass. The system of differential equations that describe the vehicle's motion is denoted by: where u u u is the control input vector, which for this implementation is simply the steer angle of the front tires, δ. An emboldened function denotes that it returns a vector value; otherwise, the function returns a scalar value.
The lateral and yaw acceleration of the vehicle body is given by the following equations: The force terms in (2), calculated for a single tire on an axle, are given by referring the forces calculated by the Magic Tire Formula (MTF) [21] to the body-fixed coordinate frame. For the front tire the terms are referred from the tires coordinate frame to the body fixed coordinate frame, see Figure 1, where the tire-fixed cornering term, F cf , is obtained by the MTF. As there is no steer angle acting upon the rear tires, F yr = F cr . The MTF requires the calculation of the lateral slip angle, α, which is given for the front and rear tires as: (4) The global longitudinal and lateral coordinates are computed This vehicle model outlined above is used in Section 3.4 as the prediction model, where the integration in time is performed using the forward Euler method with a fixed timestep. The parameter values used in the model are shown in Table 1.

Tire transience
The tire forces calculated via the MTF are steady-state approximations that assume a tire's slip angle is instantly reached. If the steer angle of the tire varies rapidly, such as in an active steering application using a piecewise constant input, then this assumption is not representative of the behavior. A first order tire transience is added to the vehicle model to characterize the lag in the tire force: where α s is the static tire slip, as calculated via (4) and α is now the apparent slip angle, used to calculate the lateral tire force in the MTF. σ is the relaxation length of the tire. This augmented system of equations, f f f tt (z z z tt , u u u), is used as the simulation model, with the state vector z z z extended to include the apparent slip angle for both the front and rear axles.

Control methodology
The proposed hierarchical structure splits the control task into three separate modules composed of: path generation, path optimization and vehicle control modules (see the schematic in Figure 2). The first module generates a static path as a first approximation for the vehicle to track. This path is then taken by the trajectory optimization routine in which the path is altered to ensure that specified lateral acceleration constraints are not exceeded. The final module is responsible for vehicle control, where it tracks the optimised reference path and tracks them by optimising the steer angle of the vehicle's front axle.
The system is designed with the assumption that each control module has a different call rate, allowing for the path generation module to have a longer computation time due to the much larger preview horizon than that used by the path planner and trajectory optimization routines. For the current analysis, the computational delay is omitted to simplify the analysis, and because this delay is highly dependent on the hardware on which the controller is implemented.
There are existing techniques, however, to study the effect on performance and stability of a time delay [22] and how to account for it [8]. The lack of a computational delay allows the presented controller to react more quickly to changes in the environment, making it possible to take corrective action more quickly as well as reducing the prediction model error. For the results presented, this assumption has negligible impact as there are no sudden changes in the simulated environment.

Hierarchical structure
The individual modules of the controller have different control frequencies. Therefore the hierarchical control framework, which is responsible for coordinating its modules, must call the individual controllers at different time steps. Call ratios are defined, which determine how many times a subordinate module is called for a single call to its parent module. As there are three controllers in three separate tiers of the hierarchy, there are two call ratios, ρ 1 which determines how many times the path optimization module is called for single call to the path planning module, and ρ 2 determines how many times the vehicle controller is called for every trajectory optimization call: where ∆T is the call period, with the subscripts 'pg', 'po', 'vc', denoting the Path Generation, Path Optimization and Vehicle Control modules, respectively.

Choosing the preview horizons
One key design choice, critical for both the performance and stability of the hierarchical system, is the preview horizon value chosen for each of the three control modules [8]. For both performance and stability, values as large as possible are desirable. However, increasing the preview horizon increases the computational complexity dramatically [10].
Therefore, there is a compromise to be made when choosing the preview horizon and no analytical method for determining it. Instead, rules-of-thumb are employed along with manual tuning of values based upon a given scenario [23].
Several conditions must be met that guide the choice of preview horizons for the hierarchical framework. The most important one is to ensure that the reference trajectory provided to a subordinate controller is at least as long as that of the parent controller: where T pg , T po , T vc represent how far the path generation, path optimization and vehicle control modules predict into the future respectively. The next factor to consider is that the PGM works in the spatial domain, so one can write the preview horizon constraint between the path generation and optimization modules as: where these values denote the final displacement values over a single given prediction. The problem is that the arc length generated by the PGM is not fixed, as the lateral coordinates are the optimization variables. It is assumed that for the driving scenarios considered, the average lateral step size, ∆Ȳ , chosen by the POM is much smaller than the fixed longitudinal step size, ∆X (∆Ỹ ∆X). A value, ε > 0, is given so that when using the following expression: for an approximation of the arc length of the PGM path, Eq. (10) holds. ∆X is chosen to be the distance travelled by the vehicle over the fixed time-step ∆t, used by both the POM and VCM: so that the PGM is capable of reacting to different road environments and the displacement values s pg , s po are both func-tions of the vehicles longitudinal speed, as well as ensuring, for sensible values of ε, Eq. (9) holds. For the relationship between the POM and VCM the constraint N po ≥ N vc ensures that the second inequality in Eq. (8) holds, as they both utilize the same fixed time step. The values are then chosen to provide stability to the system.

Geometric path generation
The PGM creates the initial reference path for the POM by generating a set of N pg Y -coordinate values, denoted Y Y Y pg , based upon a longitudinal grid with equal step size, ∆X, creating a set of N pg Cartesian coordinate pairs (X X X pg ,Y Y Y pg ). At each X-coordinate the road limits are defined by an upper and lower bound resulting in two vectors, Y Y Y ub and Y Y Y lb , that represent bound constraints on the absolute lateral displacement. The controller must therefore determine a coordinate between these limits at each X-coordinate.
The PGM is designed to generate a spatial path which minimizes the total distance travelled between the initial and final X-coordinates. This assumes that if a vehicle was travelling along a path of low or zero curvature, which as an example, is true for motorway driving, then travelling in a straight line is the optimal behavior, and a lane change is only made to avoid a slower moving vehicle ahead.
To accomplish this, the optimization routine calculates N pg lateral steps ∆Y Y Y , which represent the lateral step between adjacent Y -coordinates. The absolute lateral position, Y Y Y pg , is then calculated componentwise as: WhereỸ 0 is the initial measured lateral position of the vehicle at the start of the optimization problem. In this paper, the value is taken from the state vector z z z of the simulation model. The bound constraints included in the optimization routine have a safety margin, Y sm , included when determining the optimization bounds to account for vehicle width, which reduces the apparent feasible space available to the path planner.
The resulting optimization problem, which finds the optimal reference path by choosing the vector of lateral stepsizes ∆Y, at time t = i can be written as: The optimal initial reference path generated by the PGM is then calculated by Eq. (12), using the vector ∆Y Y Y * that represent the optimal solution to the optimization problem. This problem is of Quadratic Programming (QP) form with inequality constraints, for which there are fast solution meth-ods for both large (sparse) and small optimization problems [24].

Path parameterization
In the POM, the X X X pg and Y Y Y pg variables that make up the geometric path are addressed separately. In order to do this, the PGM module parameterizes the path coordinates so that they are functions of the path's arc length.
The continuous path that is assumed to connect coordinate pairs, p, is approximated via straight-line segments, with p i denoting the (X, Y ) coordinates at the i-th time-step. From that, the displacement between the points p i and p i−1 , denoted ∆s i is given by: where ∆X is the fixed step-size utilized in the PGM module and ∆Y i is the lateral step-size between p i and p i−1 which can be taken from the solution of the optimization problem defined in Eq. (13). The vector containing the absolute displacement values from the initial point of the path up to the point at the preview horizon can be calculated elementwise as: This set of displacement values, which are monotonically increasing, can now be used to parametrize the X X X pg and Y Y Y pg coordinate pairs, which are denoted X X X pg (s s s) and Y Y Y pg (s s s).
So that the PGM coordinates can be tracked in the time domain, a time-based interpolation vector, denotedŝ s s, is required. As this paper considers constant longitudinal speed scenarios and, assuming that the lateral velocity is of a much smaller magnitude, this interpolation vector are calculated elementwise as: where ∆t is the time-step of the VCM. The resulting vector allows for the interpolation of the vector Y Y Y pg at any time-step up until N po usingŝ s s as the interpolation vector. Linear interpolation is then used to create the initial reference trajectory for the path optimization.

Path optimization
The POM attempts to match the parametrized X X X pg (s s s) and Y Y Y pg (s s s), while considering kinematic and dynamic factors affecting the reference path generation problem. It does this by assuming that the vehicle is a point mass travelling along the reference spatial path. The path coordinates from the PGM are modified so that the acceleration component perpendicular to the optimized path does not exceed specified limits. Along with a reference displacement, the path optimization module also determines a yaw reference profile for the vehicle control module to track. Following [25], where the optimal yaw angle is tangential to the reference path, the yaw reference is calculated as: Where ∆X and ∆Y are the finite differences of the X and Y values. A penalty to the yaw magnitude is included in the POM to encourage the module to return a path that is smooth and does not have excessively large deviations. This can occur due to the PGM returning a path that consists of straight lines between changes in road bounds.
The normal acceleration is calculated as the centripetal acceleration of a point mass travelling along a path with curvature, κ, at a tangential velocity, u (see Figure 3). An approximation of the path curvature is calculated using finite difference approximations for the first and second derivatives of the X and Y coordinates with respect to time. The curvature at the jth time-step is given by: where ∆X, ∆Y and ∆ 2 X, ∆ 2 Y denote the first and second backward finite differences for the X(t) and Y (t) coordinates, which are calculated by backward finite differences. Using the backwards finite difference method, it is possible to account for the data points that are lost via the finite difference method, with data from the previous time-step. As path curvature is the inverse of radius of curvature, the vehicle's lateral acceleration is given as the centripetal acceleration of the point mass travelling along the arc length: a n,j = κ j · u 2 (19) So far, the model outlined for the POM is not constrained by inertia effects and so is free to vary the path displacement as quickly as possible so long as inertia and displacement constraints are obeyed. This can result in a path that is not realizable by the VCM controller. To restrict the optimal path to a set that is more easily tracked by the VCM, the step-size in lateral acceleration is constrained in the POM problem formulation via limiting the step-size between normal acceleration values calculated at each time-step. The optimization problem solved at t = j can be formulated as: −ā n ≤a a a n ≤ā n , −∆a n ≤ a n,j − a n,j−1 ≤ ∆a n , Where Z Z Z po = z z z po,t+1 , . . . , z z z po,t+N po is the sequence of reference trajectories generated by the POM to optimize the cost function Eq. (20), with z z z T po,j = (X j ,Y j , ψ j ) and z z z r j are the corresponding reference values provided by the PGM. x x x 2 M M M , as seen in Eq. (20), denotes the square of the weighted matrix norm x x x T M M Mx x x. Equation (20b) constrains the re-planned coordinates to ensure that the distance between two adjacent coordinates is equal to the distance travelled by the point mass model when moving at the fixed longitudinal speed, u, over the interval of a single time-step, ∆t.
To calculate the lower and upper bound terms in Eq. (20c), linear interpolation is used. The optimized coordinates X X X po are used as the interpolation vector to interpolate the upper and lower bounds based upon the original fixed X X X pg grid generated by the path planning module.
Equations (20f) and (20g) specify the additional data variables that must be provided to the optimization routine for the calculation of path curvature. The variables with overbars denote that they are taken from the vehicle plant itself.

Vehicle control
The task of the Vehicle Control Module (VCM) is to calculate an optimal control sequence for the vehicle plant when called at time k. The VCM attempts to track the reference coordinates supplied to it by manipulating the internal plant model's steering angle, up to preview horizon N vc . The plant model that is utilized is taken as the vehicle model in Section 2. The continuous time model is discretized using the forward Euler method: The lateral acceleration is constrained, as was the normal acceleration of the point mass model. The lateral acceleration is given by: In the optimization routine, the lateral acceleration is calculated from the state and rate vectors in Eq. (21) at every time step up to the preview horizon. The optimization scheme constrains these predicted values between upper and lower bounds. Therefore the VCM solves the following optimization problem at t = k: Where u = u u u t , . . . , u u u t+N vc−1 is the sequence of optimal control vector inputs applied to the system, which for this application is the steer angle of the front tires. Like in Eq. (20c), interpolation is used in Eq.. (23g) to obtain the upper and lower bounds at each predicted (X vc,k ,Y vc,k ) point.

Summary of the control tasks
To conclude the control methodology section, a summary of the steps taken by the hierarchical controller is given below: 1. The geometric path generation module generates a set of N pg Y Y Y pg coordinates based on a uniformly spaced vector of X X X pg coordinates. 2. The path is then parametrized based upon arc length. 3. The set of N pg coordinate pairs is passed to the replanning module. The controller then returns a reference trajectory of length N po for the vehicle control module. The re-planning module is called ρ 1 times for every path planner call. 4. For each call to the vehicle control module, it selects the relevant trajectory of length N vc from the re-planner trajectory. It tracks this reference trajectory by optimizing the steer angle of the nonlinear bicycle model front wheels. The path planning controller is called ρ 2 times for every trajectory re-planner call.

Results
The vehicle controller is evaluated in simulation, performing a double-lane-change maneuver. The course consists of three straight line sections, with the middle section offset by a distance of 3.5 m (see Figure 4). The longitudinal distance is 150 m, with the individual section length's shown in Table 2.
To demonstrate the performance of the proposed hierarchical controller, it is compared against two other controllers derived from it. For ease, the controllers shall be referred to as controller A, controller B and controller C, with specifications as follows: The purpose of controller B is to demonstrate that the POM performs a valuable function and that splitting the control task into separate control layers is a worthwhile design goal, even when there is a path provided by. Conversely, controller C's purpose is to demonstrate that the performance of controller A is comparable to that of a specifically crafted, continuously smooth reference trajectory, while being completely flexible to the driving scenario. Table 3 shows the preview horizon and time period for the three control modules. The values chosen for the module time periods results in the call ratios ρ 1 = 2 and ρ 2 = 5. The PGM preview horizon was chosen to comfortably cover the majority of the scenario in a single optimisation call. This is possible as it is a QP problem and so the execution time of the controller is not a limiting factor. The values for the POM and VCM were chosen to give stability to the controller   Table 4 shows the weighting values used by the three control variants, chosen to give stability and adequate performance based on controller, A determined during a trial-run of the lane change scenario. Tables 5 and 6 show the values used as constraints in the POM and VCM respectively. Figure 5(a) shows all sets of the Y-coordinates generated over the course of the maneuver; representing stage 1 of the control scheme. As would be expected, the PGM generates straight line paths from the location of the vehicle when the PGM is called and between the apexes of the constraint     Figure 5(b) shows the effect the POM has on the PGMs initial path. There are twice as many reference trajectories in Figure 5(b) than Figure 5(a), matching the call ratio between the two modules. The POM smooths out the PGM path around the apexes, where the road bounds change.

Controller A simulation results
At 14 m/s (31 mph) the controller can closely track the references provided to it. The effect of solving the path generation problem from the vehicle's current location can be observed in Figure 5(a), showing the primary benefit of the hierarchical controller; the feasible reference path is always in the close neighborhood of the vehicle. Figure 5(c) shows the displacement of the simulation vehicle and it can be observed that the simulation vehicle sucessfully navigates the course at 14 m/s. Here the effect of including bound constraints on the global lateral coordinates can be observed. In [12], the path optimization minimizes the vehicles lateral acceleration by reducing the maximum Y value, i.e. straightening the reference path. In this scenario, it is not possible to reduce the maximum lateral coordinate, so instead, the POM increases the maximum lateral coordinate value to ensure bound constraints, and acceleration constraints are satisfied.
There are small constraint violations that occur on several of the course apexes. These occur due to the way the lateral coordinate constraints are enforced. In the interval between two discrete time-steps, there is no guarantee that the vehicle will not violate the road bounds. It is therefore necessary when choosing the lateral safety margin to factor in the constraint violation that can occur between VCM time-steps, along with the vehicles width. Figure 6 shows the yaw profile of the simulation vehicle along with the reference profile made up of the most up-to-date value from the last POM call. The yaw profile is tracked closely by vehicle, and at the end of the run, the penalty on yaw deviation in the vehicle control module successfully damps the vehicle trajectory to a straight line at the end of the simulation run. Figure 7 shows the lateral acceleration of the vehicle and the corresponding steering input used to track the coordinates. Figure 7(a), which shows the lateral acceleration, shows the inclusion of a penalty on lateral acceleration in the optimization cost function. The acceleration bounds im-

High-speed run
The simulation manoeuvre is repeated, at a higher longitudinal speed of 20 m/s, to show that the proposed controller maintains its performance at higher speeds. Figure 8(a) shows that the lateral displacement of controller A at 20 m/s, with the road and controller bounds, along with the reference trajectory. The vehicles displacement looks similar, although there is an overshoot as the vehicle returns to the initial lane. This overshoot occurs to decrease the path curvature, reducing the lateral acceleration. Table 7 shows that at higher speeds, there is a slight degradation in both maximum and RMS lateral tracking errors, reflecting the increase in difficulty of tracking at higher speeds. Figure 8(b) shows that the hierarchical controller maintains good yaw tracking at the higher longitudinal speed. Although the RMS tracking error is almost double the value that it is at 14 m/s, the maximum tracking error is only increased by 7% and the absolute value of the RMS error is still more than adequate for automotive scenarios.
In Figure 8(c), it can be seen that the lateral acceleration of the simulation vehicle exceeds the limits imposed on the hierarchical controller, with a maximum acceleration of 0.37 g occurring during the simulation run. This excessive acceleration can be attributed to both the tire transience included in the simulation model, and the more accurate numerical integration scheme used in the simulation compared to the MPC prediction model. This constraint violation is not predicted by the MPC controller and the simulation models response appears to suggest that the rapidly switched steering input has a significant effect on the common assumption that steady state tire force values are appropriate in control synthesis and evaluation of discrete-time steering controllers. Table 7 shows the 67% increase in RMS lateral acceleration. Despite the fact that this is an average measure of lateral acceleration, including the final two seconds during which it is very low, this 67% increase is more than four times smaller than the increase in centripetal acceleration that might be naively expected due to the increase in longitudinal velocity, suggesting that the constraints effectively change the dynamic behavior of the vehicle.

Comparison of controller performance
To provide a reference to the performance of controller A, the simulation vehicle is guided through the double-lanechange maneuver is done again with two derivative controllers. Controller B does not perform the path optimization routine, and instead supplies parameterized coordinates from the initial PGM path for the VCM to track. The purpose of this controller is to demonstrate the performance benefit of a hierarchical architecture containing path optimization, both from a tracking point of view, along with respect to constraint satisfaction.
Controller C replaces the PGM of controller A with a smooth reference trajectory, based on the longitudinal displacement of the vehicle. The lateral displacement reference is given by where and α = 1.4, ∆X 1 = 20, ∆X 2 = 20, ∆Y 1 = 4, ∆Y 2 = 4.25, X s1 = 24 and Y s2 = 71. 25. These values were chosen by visually fitting a curve to the feasible road space. Figure 8(d), shows that the lateral displacement of the vehicle when steered by controller B is similar to that of controller A (Figure 5(c)), although Table 8 details the degradation in tracking performance measured via both RMS and maximum deviation. There is a rather significant deviation in the maximum lateral tracking error, being three and a half times greater than controller A at 20 m/s. Part of the reason for this is that as the POM is inactive, the reference path to the VCM controller is only updated once every second compared to the two times a second that the occurs in controllers A and C due to the POM. Figure 8(e) shows in greater clarity the effect of deactivating the POM; as the POM takes no action to minimize the second derivatives of their reference trajectory. This results in large steps in the yaw trajectory, which is not physically realizable, resulting in the comparatively poor yaw tracking performance. As with controller A, the lateral acceleration of the simulation vehicle exceeds the acceleration limit imposed by the controller, particularly when the vehicle begins to return to the initial lane. Figure 8(g) shows the lateral tracking performance of controller C. As would be expected, the displacement of the simulation vehicle, when tracking a smooth reference, is smooth. Controller C produces a smooth yaw profile, with good tracking performance. Table 8 shows that the trajectory tracking performance is actually worse than controller A at 20 m/s (see Table 7). This is partly due to nature of the reference trajectory. For controller C, the reference trajectory is a function of longitudinal displacement only, and therefore any tracking errors accumulate over time. For controller A, this is not an issue as both the PGM and POM generate reference trajectories from the vehicles location. This means that the reference trajectory is generated so that it can be well tracked.
An argument is made by Wang and Boyd in [10], which is that with MPC optimization is used to find good control, rather than solving the optimization problem accurately. Therefore, if MATLAB's optimization solver 'fmincon' does not find an optimal control sequence to solve the MPC optimization problem to within the specified solution tolerances, the suboptimal solution is still applied to the system as otherwise. As can be observed by the results, this still results in adequate tracking performance, and the lateral acceleration constraints, along with the control input constraints are always observed.

Conclusions
A hierarchical model predictive controller for an autonomous double lane change maneuver has been presented. (i) Lateral acceleration Using simple, fundamental information about the driving scenario, the controller generates optimal steering values that allowed the simulation vehicle to navigate the double-lanechange course. The controller achieves this by generating a reference trajectory in two stages: first generating a feasible geometric reference path between road bounds; this path is then passed to the path optimization module, which optimizes it to ensure that the path curvature does not exceed values that would cause high lateral acceleration values.
Once an optimal path has been calculated, it is passed to the final stage of the controller. In this, a nonlinear MPC scheme with a nonlinear model bicycle model as the system plant is used to calculate the optimal front steer-angle. The effectiveness of the controller was investigated via simulation during a double-lane-change maneuver, and the effectiveness of the POM was shown with controller A exhibiting a greater than four-times reduction in the root mean square lateral tracking error compared to controller B at 20 m/s. The performance was also shown to be comparable to a controller using a smooth initial reference trajectory, showing that the flexibility of the PGM does not compromise dynamic performance. The proposed controller is an example of the flexibility of this extended hierarchical control framework. The chosen implementations of the individual modules can be changed independently of one another, enabling greater flexibility in the range of applications for this framework.
Future work for this proposed framework includes terminal constraint sets in addition to adequate preview horizons to provide system stability. Investigating the capabilities of the controller during different driving scenarios should also be studied; particularly if the framework is extended to allow for a varying longitudinal velocity.