Integrated nonlinear model predictive control for automated driving

This work presents a Nonlinear Model Predictive Control (NMPC) scheme to perform evasive maneuvers and avoid rear-end collisions. Rear-end collisions are among the most common road fatalities. To reduce the risk of collision, it is necessary for the controller to react as quickly as possible and exploit the full vehicle maneuverability (i.e., combined control of longitudinal and lateral dynamics). The proposed design relies on the simultaneous use of steering and braking actions to track the desired reference path and avoid collisions with the preceding vehicle. A planar vehicle model was used to describe the vehicle dynamics. In addition, the dynamics of the brake system were included in the NMPC prediction model. Furthermore, the controller incorporates constraints to ensure vehicle stability and account for actuator limitations. In this respect, the constraints were defined on Kamm circle and Ideal Brake Torque Distribution (IBD) logic for optimal tire force and brake torque distribution. To evaluate the design, the performance of the proposed NMPC was compared with two "more classical" MPC designs that rely on: (i) a linear bicycle model, and (ii) a nonlinear bicycle model. The performance of these three controller designs was evaluated in simulation (using a high-fidelity vehicle simulator) via relevant KPIs, such as reference tracking Root Mean Square (RMS) error, controller’s rise/settling time, and Distance to Collision (i.e., the lateral distance by which collision was avoided safely). Different single-lane-change maneuvers were tested and the behavior of the controllers was evaluated in the presence of lateral wind disturbances, road friction variation, and maneuver aggressiveness.


Introduction
The U.S. National Highway Traffic Safety Administration (NHTSA) reported that the number of fatal crashes in 2016 increased by 5.6% with a toll of 37,461 deaths (National Highway Traffic Safety Administration, 2017). The European Road Safety Observatory reported similar numbers for the EU (European Road Safety Observatory, 2018). In the attempt to reduce the number of fatalities on the road, the automotive industry started equipping the vehicles with active vehicle safety technologies, such as Antilock Brake System (ABS), Vehicle Stability Control (VSC) and traction control. These technologies halved the number of fatalities from 20,774 in year 2007to 11,990 in year 2016(European Road Safety Observatory, 2018. But, the NHTSA reported that rear-end collisions are the main cause of road fatalities (accounting for more than 30% of all the road fatalities) (Insurance Information Institute, 0000; National Highway Traffic Safety Administration, 2016). A rearend crash occurs when the difference in relative speeds between the subject vehicle (SV) and lead vehicle (LV) in front causes a collision. Rear-end crashes are extremely common in both urban and highway focuses on the last component of the EDS design, that is, the design of a tailored control strategy for the rear-end collision scenario. The design of such a controller is an active research area. Extensive reviews on various control strategies-such as PID control, Sliding Mode Control (SMC), Linear Quadratic Regulator (LQR), Nonlinear backstepping control, etc.-were presented in Ackermann, Bechtloff, and Isermann (2015), Aripina, Sam, Kumeresan, Ismail, and Kemao (2014), Choi et al. (2011), Mokhiamar and Abe (2004), Shah (2015), Soudbakhsh and Eskandarian (2010) and Zhu, Shyrokau, Boulkroune, van Aalst, and Happee (2018).
According to Ackermann et al. (2015), Choi et al. (2011) and Mokhiamar and Abe (2004), an effective control design in the context of EDS should: 1. Involve both steering and VSC via Differential Braking (DB). 2. Optimally distribute the steering and brake control actions to improve the overall vehicle performance. 3. Handle tire nonlinearities during highly dynamic situations (e.g., during an evasive steering maneuver).
Hence, an integrated (i.e., involving both steering and DB), optimal, and nonlinear control design should be able to handle an evasive maneuver successfully. PID and SMC control are not optimal in nature. While LQR does provide optimal control, it only works for unconstrained optimization problem which is a limitation for vehicle control as the vehicle dynamics are always bounded within the designed operating range. MPC on the other hand covers all the three conclusions made under one control design and becomes the most suitable control algorithm for vehicle control. Since MPC is an optimal control technique and is based on the designed prediction model, it can accommodate vehicle nonlinearities and Multi-Input-Multi-Output (MIMO) models in its design. Therefore our goal is to design an EDS controller that relies on NMPC to accommodate all control objectives above. By relying on an augmented nonlinear planar vehicle model as prediction model, the proposed NMPC design allows to simultaneously control the lateral and longitudinal vehicle dynamics via steering and braking, while taking actuator dynamics into account. The proposed solution has brake actuator dynamics modeled inside its prediction which allows direct control of the wheels and does not require any additional control allocation scheme (which would be nontrivial to implement). In addition, NMPC allows to directly account for tire saturation limitations and actuator limits in the constraint formulation. Most of the literature in the area of MPC for evasive maneuvers focuses on lateral control at constant longitudinal speed and relies on simplified vehicle models (e.g., the bicycle model) (Beal & Gerdes, 2009Choi, Kang, & Lee, 2012;Keviczky, Falcone, Asgari, & Hrovat, 2006). The main reasons for this choice is that more complex vehicle and tire models are challenging to implement in real-time framework. Using a dynamic bicycle model as prediction model, however, limits the controller to exploit DB. This is because one requires control of the left and right wheels for DB to work. But in bicycle model, both the front and rear tires are lumped together respectively as one tire each as a result of which the effect of DB is not well captured in the dynamics of bicycle model. From the vehicle dynamics perspective, DB plays a fundamental role to ensure safety during evasive maneuvers. Compared to the aforementioned controllers, the proposed design exploit the benefits of DB by controlling each of the wheels directly and allows to control longitudinal and lateral dynamics. This is achieved by modeling the brake actuator dynamics inside the prediction model with the planar vehicle model to have an overall optimal control strategy and removing the need of conventional control allocation schemes, making it a unique MPC-based controller design that allows direct control of the vehicle's wheels.
The designed planar vehicle-based integrated NMPC control was validated in several different scenarios, ranging from highly dynamic single-lane-change evasive maneuvers (to replicate scenarios in which rear-end collisions occur if not properly handled) to normal lane change maneuvers. The controller was tested at varying vehicle velocities. Its performance was also validated in the presence of external disturbances such as lateral wind and parameter uncertainty via varying the road friction coefficient. The integrated NMPC control design is not limited to EDS and can be used as a controller for automated driving for highway and urban driving environments, provided that a path-planning algorithm provides a suitable trajectory for the proposed controller to follow (for example, the vulnerable-road-users-aware path-planning algorithm proposed in Ferranti et al., 2019). Finally, using specific Key Performance Indicators (KPIs), the proposed integrated nonlinear MPC design was compared with the following baselines: (i) an MPC design that uses a linear bicycle model as prediction model (referred to as the Linear MPC design), and (ii) an MPC design that uses a nonlinear bicycle model as prediction model (referred to as the Nonlinear MPC design). In all test cases, the proposed design outperforms the baseline controllers.
The authors in Falcone, Borrelli, Asgari, Tseng, and Hrovat (2007) formulated the NMPC problem for a double-lane-change maneuver using a bicycle model as system model with the steering wheel angle as control command. The designed NMPC worked successfully at speed of 7 m/s but failed to stabilize the vehicle at 10 m/s. The authors of Falcone et al. (2007) concluded that integrated control of steering and braking can improve the performance of the controller. The same research team then designed a NMPC based control with 1s as prediction horizon to optimize combination of braking and steering for obstacle avoidance via double-lane-change maneuver (Falcone et al., 2008). They used a 10 DoF planar vehicle model as prediction model (first six DoF being the vehicle's longitudinal and lateral velocity, heading angle and yaw rate, and the vehicle's global position coordinates in both longitudinal and lateral direction. The four wheel's dynamics considered individually are the remaining four DoF) and used a Pacejka model to model the tire characteristics. The control action was the front steering angle and each wheel's brake torque values. The controller successfully passed the test at 14 m/s. The controller, however cannot be applied for real-time applications because it took around 15 min to complete a 12 s simulation. In addition, high amount of oscillations in the steering angle were observed due to improper tuning because of increased number of model parameters.
The authors in Jalali et al. (2017) designed an integrated Linear MPC (LMPC) control using Active Front Steering (AFS) and DB for lateral stability of the vehicle. The authors use bicycle model as prediction model with a prediction horizon of 0.3 s. The controller provides assistance control of ±10 deg on the road wheel angle, satisfying the side-slip angle constraint to ensure vehicle stability at all times. The controller, however, is not subjected to robustness tests, such as wind disturbance or parameter uncertainty.
Similarly, the authors in Choi and Choi (2016) designed MPC controller via an extended bicycle model that utilized AFS and DB for vehicle stability. In their work, the prediction model encapsulated the lagged characteristics of actuator dynamics and tire forces, both modeled as a first-order lag system. By calculating the control action as steering wheel angle and yaw moment correction , another optimization problem was solved to get the optimal tire forces, thereby increasing the overall computational time and loss of performance. By solving two optimization problems, the idea of having one integrated controller for vehicle control was lost.
The authors in Barbarisi et al. (2009) designed a Vehicle Dynamics Control using linear time-varying MPC with sampling time and prediction horizon as 0.25 s and 5 steps, respectively. They assumed constant longitudinal dynamics with control on lateral vehicle motion only. While the controller was able to pass the standard ISO 19365:2016 Sine with Dwell test, the controller showed oscillatory behavior when working close to the constraint boundaries.
Lastly, the authors in Yi et al. (2016) designed two MPC controllers for collision avoidance control using steering and braking combined as control action. They used a nonlinear bicycle model as prediction model for the NMPC controller design and for the linear MPC design, they linearized the bicycle model around the operating point along with approximation of the nonlinear constraints into linear form. But instead of calculating brake torques, their MPC control action is the commanded longitudinal acceleration which is then used in another logic to calculate required brake torques. They tested their design on a singlelane-change maneuver at 70 km/h with target lateral displacement 2 m. They set the sampling time and prediction horizon at 0.06 s and 25 steps, respectively. While the collision was avoided, it was seen with both the control strategies that an overshoot of about 35% was achieved in lateral position tracking, leading to poor tracking performance. Also, the NMPC designed was not real-time feasible with mean computation time between 4 to 8 s reported.
Compared to the previous approaches, the proposed integrated NMPC control design relies on a single controller to compute the control action for the four wheels, while taking into account the vehicle limitations. The integrated NMPC design solves the optimization problem online and in real-time. In addition, the actuator dynamics was modeled in the prediction model to account for their reaction time and have more accurate predictions. Furthermore, a kinematic reference path 1 (i.e., the reference is generated based on a kinematic description of the vehicle, without taking into account dynamics) as reference trajectory was used in all the simulations to further assess the controller's robustness towards tracking imprecise (from the dynamic point of view) reference values. Lastly, this approach in its modeling involves the use of dynamic constraints aimed at maximizing the vehicle stability by minimizing the vehicle body slip angle and body slip angle rate. Hence, the control design accounts for g-g diagram 2 and Kamm circle 3 constraints by design to ensure that the vehicle and the four tires operate in the stable working regime of motion. In addition, compared to the state of the art, the integrated NMPC control design considers IBD constraints for ideal distribution of brake torques.
The robustness of the proposed integrated NMPC design was extensively tested to various disturbances and uncertainties such as lateral wind and road friction variation. The maneuver's nature during these tests was kept evasive at all times to test controller's robustness under aggressive and nonlinear vehicle dynamics conditions. For all the tested scenarios, the integrated NMPC design was successful to avoid rear-end collisions at all times thereby guarantying vehicle stability. All these simulations were tested on a high-fidelity vehicle simulator provided by Toyota and validated by field tests.

Paper structure
The paper is structured as follows. Section 2 introduces preliminary concepts used in the paper, such as the designed maneuver, the reference trajectory, and model predictive control. Section 3 details the integrated NMPC formulation. Section 4 details the two benchmark controllers used for performance comparison. Section 5 presents the high-fidelity vehicle simulator. Section 6 explains the KPIs to quantitatively assess the controllers performance. This section also gives an overview of all the scenarios covered to analyze the performance better. Section 7 presents the results for each maneuver performed. It also details the KPIs values for every maneuver and provides an comparative performance analysis between the three controllers designed in this research. Finally, Section 8 concludes the paper and provides recommendations for future work.

Single lane change maneuver
To evaluate the designed controller, a representation of a rear-end collision avoidance maneuver is required. To the best of our knowledge, however, no standard maneuvers for evasive action are available in the literature (when referred to SAE, NHTSA, ISO and Euro NCAP, respectively). Nevertheless, the NHTSA report (Lee, Llaneras, Klauer, & Sudweeks, 2007) provides certain key insights concerning real-life scenarios in which rear-end collisions occur. By collecting the crash data of 100 different cars, the NHTSA highlighted that collisions frequently happen on a straight road with no junctions while driving at constant speed. Hence, based on the NHTSA conclusions, the single-lane-change maneuver was used shown in Fig. 1 and parameter values provided by Ford Motors Research were selected to represent an evasive scenario (Zegelaar, 2017). The values are ref = 2.5 m and ref = 30 m, that is, the maneuver begins when the subject vehicle is exactly 30 m away from lead vehicle and has to laterally traverse 2.5 m to avoid the rear-end collision. To ensure that the designed maneuver is indeed aggressive and nonlinear in nature, the vehicle speeds were selected to perform the maneuver according to the following metrics: where Time to Collision (TTC), Time to Brake (TTB) and Time to Steer (TTS). Here, refers to the subject vehicle's longitudinal speed, max is the subject vehicle's maximum longitudinal acceleration (taken as 0.8 μg) and max is the subject vehicle's maximum lateral acceleration (taken as 0.6 μg), with being the friction coefficient. The acceleration values mentioned above were chosen based on the baseline values provided by NHTSA's definition of a Near-Crash which states that circumstances involving vehicle braking greater than 0.5 g or steering input leading to lateral acceleration greater than 0.4 g to avoid a crash constitutes a rapid maneuver scenario (Lee et al., 2007). Therefore it was ensured that the designed maneuver is evasive at all times. Eq.
(1) was evaluated at different vehicle speeds and ensured that the inequality TTS ≤ TTC ≤ TTB is satisfied at all times to design realistic test scenarios. Table 1 reports the speed range for each value of . The small values of TTC highlights that the maneuver is aggressive and captures the real-life evasive situations. The single-lane-change maneuver should be translated into a path the vehicle can follow in terms of lateral position, heading angle, and yaw rate. Hence, the lateral reference position is approximated by using a Sigmoid curve (Choi et al., 2011) defined as a function of vehicle's longitudinal position as follows: In addition, the reference heading angle ref and reference yaw ratė ref are defined as follows: For the readability of the paper, Appendix A details the quantities associated with the definition of the reference signals.

Model predictive control
Model Predictive Control (MPC) solves a constrained optimization problem online to compute the optimal sequence of control commands over a finite time window, called prediction horizon. The problem is formulated based on (i) the available plant measurements, (ii) the plant-prediction model, (iii) control objectives, and (iv) plant/actuator limitations. Only the first control command of this sequence is applied to the plant in closed loop in the receding-horizon fashion. The prediction model captures the plant dynamics and gives controller the ability to predict the behavior of plant. The prediction model, as this work also shows, is fundamental for the performance of the controller.
A general formulation of MPC controller is given by where is the cost function to be minimized for optimal control action . and ref are the states and the reference values at prediction instant ( = 0, … , ), respectively. Function is the prediction model that captures the plant's dynamics. init is the current state measurement from the plant and updated online at every sampling instant. Finally, function comprises of all the constraints defined on the states and control action with being the bound value. The constraints can be either convex or nonconvex.
There are several toolboxes that can be used to solve Problem (4). In this work the ACADO Toolkit (Quirynen, Vukov, Zanon, & Diehl, 2014) has been used. ACADO tackles nonlinear optimal control problems and multi-objective optimal control problems efficiently. In ACADO, the optimal control problem (OCP) is discretized. ACADO relies on Sequential Quadratic Programming (SQP). The SQP algorithm linearizes the discretized nonlinear control problem to convert it into a Quadratic Programming (QP) problem (Vukov, Domahidi, Ferreau, Morari, & Diehl, 2013). Using condensing techniques, the state variables are eliminated (i.e., the overall number of optimization variables is reduced). Multiple shooting method was preferred over single shooting method as it is more robust for nonlinear systems, such as the vehicle dynamics in the non-linear handling area. Once the optimization problem has been linearized, an Active Set Method was used to solve the resulting quadratic programming problem. Levenberg-Marquardt Gauss Newton based hessian approximation method was used, and, for active set method, open-source C++ software qpOASES (Ferreau, Kirches, Potschka, Bock, & Diehl, 2014) was selected. In addition, ACADO implements the Real Time Iteration (RTI), which involves performing a single SQP iteration per sampling time, for a more efficient solution calculation (Vukov et al., 2013).
In the current study, to mitigate the effects of the unpredictable computation times, the maximum amount of iterations of the solver was fixed. If the optimizer reaches the maximum number of iterations without converging to an optimal solution, then previously computed feasible prediction (shifted one step forward) is used. This previously computed prediction is used to warm-start the optimizer. Since the vehicle dynamics (plant model) are continuous in nature, sudden jumps in its dynamics are less likely. This gives a higher chance that the next optimal solution is close to the previous one. Both modifications are common practice in practical MPC implementations.

NMPC controller design
This section describes the proposed integrated NMPC formulation. First, the various dynamic couplings in the vehicle model are discussed that should be well captured in the prediction model to improve the performance of the controller. Then, the planar vehicle model used as prediction model in the proposed NMPC design is discussed. Finally, the control objectives and the constraints are described.

Vehicle dynamics coupling
Designing an integrated control is a non-trivial problem due to the strong couplings in the vehicle dynamics as explained below. Based on the knowledge of vehicle dynamics and its associated coupling effect, it is therefore essential for developing the prediction model of the MPC controller. According to Attia, Orjuela, and Basset (2012) and Lim and Hedrick (1999), the following longitudinal and lateral couplings arise in case of vehicle motion: Kinematic and dynamic coupling. This coupling arises due to the effect of wheel steering on longitudinal dynamics of the vehicle by changing the tire lateral forces. The tire longitudinal forces on the other hand affects both the lateral dynamics and yaw motion of vehicle and subsequently the rate change of lateral position is a function of longitudinal velocity. Thus both dynamics are always coupled as reflected in (B.1)-(B.6) (detailed in Appendix B.1).
Tire-road coupling. This coupling arises due to the application of lateral and longitudinal forces by the tire. This coupling is reflected in (17). The equations used for the lateral forces are reported in Appendix B.2. To model the longitudinal forces, the single corner model was used as shown in Fig. 3. The equations used for the longitudinal forces are reported in Appendix B.3.
Load transfer phenomenon. This coupling arises because of the load transfer during longitudinal and lateral accelerations. This coupling is reflected in (22a)-(23e) (detailed in Section 3.3).

Prediction model
The designed prediction model for integrated NMPC accounts for these coupling effects and captures the behavior of vehicle in both linear and nonlinear regime of motion. A planar vehicle model was selected as prediction model of the integrated NMPC design, because it allows to preserve the effect of the couplings.
Following modeling simplifications were made, which are mostly made when using a planar vehicle model: • Roll, pitch, and vertical motion are not modeled.
• Suspension and compliance effects are not considered. • Ackermann geometry is not considered (i.e., the front left and front right wheel turn by same amount). • Since the actuator delay is small for both the front and rear calipers, it has been ignored in the prediction model formulation.
where ( ) ∈ R 15 , ( ) ∈ R 5 , and ∶ R 15 × 5 → R 15 . To improve the readability of the paper, Appendix B.1 details ( , are the rate of change of road wheel angle and brake torque rate for each wheel, respectively. The control action is applied to the vehicle's steering system as steering wheel velocity (SWV) by multiplying the control action with steering ratio st .
The considered maneuver is aggressive and nonlinear in nature, therefore the model needs to capture the tire nonlinearities. Hence, instead of giving a fixed value to the cornering stiffness , a Dugoff tire model was used to capture the tire nonlinear behavior. Appendix B.4 details the model formulation. The fitted nonlinear cornering stiffness non is given by: where is the longitudinal slip and ( ) is a function of the longitudinal slip (its definition is reported in (B.17), Appendix B.4). This allows the prediction model to preserve the overall tire behavior as well as the tire dynamics for an accurate and superior control. Remark 1. The designed prediction model (refer for details to (B.8)-(B.9) in Appendix B) captures the brake actuator's dynamics. Hence, the controller can understand how and at what rate will the brake pressure build up in each wheel's caliper, allowing accordingly to calculate the control action, that is, the four brake torque rates ij . In addition, by including the actuator dynamics, the proposed strategy does not require a separate controller design for control allocation, making it an integrated approach and theoretically easy to implement as a hardware on actual vehicle. Instead of only calculating the upper-level control action, the designed integrated NMPC controller can now directly calculate the control action that needs to be applied to the wheels. In this way, one controller can take care of all the two classical control layers, that are, vehicle level and control allocation. This integrated controller design is only possible with MPC due to its modular design concept.
Remark 2. The brake torques ij act represent the actual brake torques applied to the wheels (i.e., the values after the brake actuator dynamics). Brake torques ij cal represent the values calculated before the effect of actuator dynamics. By providing the actuator dynamics, the controller can compensate for possible performance losses to compute the final brake torque value ij act .

Constraints
States and actuator limitations should be taken into account by design for a realistic control and to eliminate actuator failure. In addition, the controller should ensure that, if a feasible solution of the MPC problem exists, this will keep the vehicle in the stability envelope, that is, the vehicle dynamics need to be constrained to a region in which control of the vehicle can be ensure at all times. The MPC framework allows to incorporate these stability-envelope and actuator limits in the form of constraints in the design phase of the controller. Hence, to account for physical limitation and to define a handling envelope, the following constraints (19 in total) were defined that should be satisfied along the length of the prediction horizon of the NMPC controller: Constraint (7) limits the vehicle's speed. To ensure vehicle stability, constraints (8)- (9) limit both the vehicle sideslip angle and the sideslip angle gradienṫ. Based on the concept of stable -̇reference region by He, Crolla, Levesley, and Manning (2006) and the evaluations on the same phase plane by European Council Service Framework Programme (0000) and Shyrokau, Wang, Savitski, Hoepping, and Ivanov (2015), it was concluded that a bound of 5 deg for and a bound of 25 deg/s foṙwere reasonable to define a stable region for vehicle motion. The constraints defined ensure that the vehicle remains within this stable region at all times and does not spin away. The vehicle sideslip angle can be in terms of vehicle states so that the constraint is dynamic in nature and is always satisfied along the entire prediction horizon using Eq. (19).
Since, the bounds in constraints (8)

=
(20) =̇ (21) Constraints (10)-(11) limits the steering wheel angle and steering wheel rate, respectively (note that by using the steering ratio st they have been written in the form of road wheel angle to directly bound the state). Constraint (12) represents the g-g diagram constraint representing the working limit of the vehicle. Since the controller is integrated in nature and can control both lateral and longitudinal dynamics, therefore accordingly the working envelope is defined. Constraints (13)-(16) define the brake actuator limits in terms of maximum brake torque and rates. Constraint (17) represents the four Kamm circle constraints (one for each tire). These constraints prevent/minimize the effect of tire saturation. By making assumptions that the sprung and unsprung masses are lumped as total mass , the roll angle is small and the dynamic terms of roll and pitch motion are ignored, that is, only the contribution from the static terms are taken in modeling, the normal load (which is the right hand side of the bound) on each tire ij , respectively, was defined in equations below: where front , and ℎ = ℎ cg − ( ℎ rf + ℎ rr ) −1 . Finally, Constraint (18) defines the front to rear brake torque distribution ratio based on the parabolic curve (right-hand side of Constraint (18) according to Breuer & Bill, 2008) for an ideal brake torque distribution. In straight-line driving, when a vehicle brakes, it pitches forward, increasing the normal load of the front tires. Therefore the ability of the front tires to generate brake force increases as compared to rear ones. Hence due to vehicle design, usually in a straight-line driving, the front tires brake more than the rear tires. Since this was not modeled in the prediction model, constraint (18), which is only activated during straight-line driving, captures the IBD behavior well and is defined with in denominator equal to 0.001 to ensure mathematical infeasibility is avoided.

Cost function
The cost function incorporates the control objectives of the NMPC design. It is designed to keep the tracking error between process output and given reference as small as possible and at the same time, minimize the control action along the prediction horizon. Based on Barbarisi et al.  , heading angle and yaw rate, respectively. In addition, the cost penalizes to control the magnitude of the Steering Wheel Angle (SWA) at higher speeds (high SWA and SWV may lead to vehicle spinning out). Furthermore, the cost penalizes the brake torques to ensure that minimum control action energy is utilized to perform the maneuver. Lastly, the cost penalizes all the five control actions to ensure that the entire maneuver can be performed at minimum control values. This ensures that the control energy cost is minimized, reducing the actuator wear and improving its service life as well.

Controller tuning
The proposed design involves the selection of several tuning parameters. This section details and motivates the design choices.
The sampling time of the controller to 0.035 s has been chosen motivated by the cycle update time of all the other ECU's of the passenger car. This ensures that at each sample, the controller has adequate information of all the reference signals and vehicle's states to solve the optimization problem. Furthermore, the prediction horizon of the proposed controller is set to 30 steps (i.e., 1.05 s) to maximize its performance (according to the assessment criteria described in Section 6.2) while ensuring computational feasibility (i.e., real-time performance).
The remaining tuning parameters are the weight matrices ⪰ 0, ≻ 0, and ⪰ 0 in (24). Tuning these matrices is nontrivial, given the multiple objectives the controller needs to optimize. Tuning is based on a trade-off to achieve small tracking error balancing the use of the control actions. Compared to other control techniques (such as rulebased controllers), tuning of these weighting matrices is based on the dynamics and kinematics of the vehicle. For example, giving a high penalty to the control action means that the actuators should stay as close as possible to zero, which is unrealistic in the scenarios considered in this work.
The initial tuning was refined using heuristics and simulations. One parameter at the time was varied and the KPIs were evaluated. In addition, given that the maneuver is performed for various speeds and for various values of , the tuning was performed at different speeds and for each value of . By doing so, following patterns were identified which were used to schedule the weights in the designed cost function: • Increasing the terminal position tracking tuning weight lead to corner cutting.
• Increasing value of yaw rate tuning weighṫimproved the overall reference tracking performance. • Reducing tuning weight of wheel angle improved tracking performance.
• For a fixed value with increasing maneuver speeds, the tracking was improved by increasing the weights of road wheel angle and wheel velocity ( anḋ), and by reducing the weight of lateral position .

Benchmark controller design
This section describes the benchmark controllers designed to compare it against the proposed integrated NMPC control design. Compared to the proposed controller, these designs rely on a simplified prediction model, that is a bicycle model represented in Fig. 4. This representation is based on the same assumptions made in the planar vehicle based MPC control (Section 3.2) with addition of one more assumption that no longitudinal or lateral load transfer is considered.
Compared to the planar vehicle model, the bicycle model has seven states, that are, , , , , , and and only a control action, that is, the rate of change of the wheel steering angle . 4 Both benchmark controllers have the same cost function and constraints (detailed in Section 4.1. In the following, additional details about the two benchmark controllers are provided.

Linear MPC
This controller involves the use of a linear bicycle model as prediction model. This representation requires the additional following assumptions: • Small angle approximation, that is, sin ≈ , cos ≈ 1.
• Constant longitudinal velocity, that is, = 0. • Linear tire model is used to capture the tire dynamics: The dynamics are detailed in Appendix C. In addition, compared to the proposed design, this design has only six constraints to ensure vehicle stability and bounded control action within feasible actuator range. Specifically, five constraints are similar to constraints (7)-(11), define for the planar vehicle model. The last constraint bounds the lateral acceleration of the vehicle as follows: The cost function associated with the linear bicycle model is similar (with a reduced set of state and control objectives) to the one presented in Section 3.4. Finally, was set to 50 steps to predict 1.75 s in future to improve the control performance. The horizon is longer with respect to the one selected using the planar vehicle model because the optimizer has to solve a smaller problem with less constraints and decision variables. While, with a shorter horizon the two baseline controllers provided substandard tracking performance, with a longer horizon the integrated design failed to meet the real-time requirements. Hence, to have a fair comparison among the controllers, different horizon lengths were selected for the integrated and baseline controllers. While a shorter horizon (due to real-time constraints) for the integrated design could be seen as a limitation, its performance is still very good thanks to its ability to use a more accurate model to generate predictions.

Nonlinear MPC
This controller relies on a nonlinear bicycle model to capture the vehicle's nonlinearities while performing the maneuver. In addition, compared to the previous design and similar to the proposed design, this controller relies on the Dugoff tire model to capture the tire nonlinearities. The states and control commands are the same of the linear bicycle model, but they are non-linearly coupled. Appendix D details on the nonlinear bicycle model. The constraints, cost function, and horizon length are those detailed for the linear bicycle model.

Vehicle simulator
The three controllers were tested and compared on an IPG CarMaker-based simulation platform using a high-fidelity Toyota vehicle model. The model has been parametrized based on mass-inertia parameters obtained from vehicle inertia measuring facility, suspension kinematics and compliance obtained by measurement on a Kinematics & Compliance test rig for wheel suspension characterization, and finally, validated by field tests on the proving ground. A high-fidelity 3-DoF steering model with column-based electric power steering logic was used as the steering actuator model. This steering system was validated with full-vehicle testing and it is implemented in the Toyota's high-end driving simulator (Damian, Shyrokau, Ocariz, & Akutain, 2019). To simulate tire dynamics, the Delft-Tyre 6.2 was used in combination with a detailed tire property file identified from bench testing (pure and combined slip, transient dynamics).
The brakes considered in this research are floating point disk brakes with conventional HAB system. The nonlinear HAB brake dynamic model derived from real-life vehicle data is the following (according to Zhou, Lu, & Peng, 2010):  The parameters for the front axle are = 0.06 s, = 0.12 s and = 230 bar/s. For the rear axle, the parameters are = 0.02 s, = 0.05 s and = 550 bar/s. The maximum pressure max that the brakes can achieve is taken as 160 bar. To convert the brake pressure to brake torque, the following relationship was used according to Limpert (1999): ij act = 2 act ij wc ij ij ij ij , ij = (fl, fr, rl, rr) The brake hysteresis effect was neglected as it is assumed that brake hysteresis has a minor influence on the brake performance for a new hydraulic disk brake mechanism (Shyrokau, Wang, Augsburg, & Ivanov, 2013).

Maneuver scenarios
The designed single lane change maneuver was performed under a variety of conditions to check the controller capabilities and robustness in different scenarios. This paper presents the results for the most relevant scenarios.

Set 1 -varying velocity
These scenarios involve variations in vehicle speeds at a constant road friction coefficient. Table 2 summarizes these scenarios. Note that the speed range was selected based on the results of Section 2.1 (Table 1).

Set 2 -varying friction coefficient
These scenarios involve variations in values of for a given speed. Table 3 summarizes these scenarios.

Set 3 -varying lateral wind velocity
These scenarios involve variation of external lateral wind speeds for a fixed value of and . Table 4 summarizes these scenarios. Note the wind is modeled as constant perturbation to flow only in direction South, directly opposing the vehicle as it turns left (towards direction North) according to the defined maneuver.

Set 4 -varying maneuver's aggressiveness
These scenarios highlight the ability of the controller to handle various dynamic maneuvers ranging from evasive actions to normal single-lane changes. In these scenarios, the parameter 2 in the sigmoid curve decreases gradually. By doing so, the slope of the trajectory was gradually reduced, making the reference trajectory less aggressive. Table 5 summarizes these scenarios.

Assessment criteria
To assess the performance of the three controllers for all the scenarios defined in Section 6.1, following KPIs were defined.
The first KPIs selected were Overshoot ( ), Settling Time ( ), and Rise Time ( ). These are typically used to assess the performance of a controller to step reference signals (which is a close approximation of the reference trajectory described in Section 2.1 and graphically shown in Fig. 6a). To further assess the tracking performance of the controller, the RMS of the tracking errors over the horizon length of the controllers was considered, that is:  Fig. 5). This KPI represents the lateral distance between the left-rear corner of LV and right-front corner of SV. The DTC is a safety-based KPI and gives an idea of the safety margin the controller can produce.
For a good control performance, the DTC should be as high as possible and all other KPIs should be as small as possible. This will ensure collision avoidance, well tracked trajectories, and quick stabilization of the vehicle post lane change.

Simulation results
The controllers were tested in the scenarios described in Section 6.1. This section presents one specific case in more detail (with comparison with the benchmark controllers), that is the scenario = 90 km/h and = 0.9. In addition, this section shows the KPI results for all the scenarios using the proposed design. Furthermore, the section shows how the proposed controller handles constraints by design and is real-time feasible.
Comparison with the benchmark controllers. Fig. 6 compares the three control strategies with respect to the reference signals. The dashed-blue lines represent the reference signals, the red, yellow, and purple lines represent the proposed controller, the linear MPC design, and nonlinear MPC design, respectively. The first plot of Fig. 6 shows that the proposed integrated NMPC control approach significantly reduces the overshoot compared to the linear MPC design (33% overshoot). In addition, the proposed design provides better tracking performance ( RMS = 7.39% and DTC = 0.41 m) compared to nonlinear MPC design ( RMS = 11.25% and DTC = 0.26 m). The improved tracking performance is due to the more detailed prediction model and integrated control action of steering and braking. In this respect, Fig. 7 highlights how the proposed approach provides the necessary steering action. Recall that the singlelane change maneuver starts with a left turn, followed by a right turn, and concludes with a straight-line drive. During the first turn, as the figure shows, the left brakes brake while the right brakes are kept at zero. This gives the required additional yaw moment for better tracking. During the second turn, the controller provides the control action to steer the steering wheel clockwise (i.e., negative SWA value). Simultaneously, the controller also reduces the left brakes and increases the right brakes to get desired yaw moment for tracking the reference values. Finally, in the last phase of straight-line driving, the SWA goes to zero. At the same time, the right and left brake values are modulated to ensure the vehicle remains stable and aligned straight. Once done, the brake torques also go to zero to conclude the maneuver. The most important KPI for collision avoidance is the DTC value. This is because the top priority in case of evasive action is collision avoidance which is directly represented by DTC. A positive and non-zero value ensures that collision was avoided successfully. The higher the DTC values are, the higher the safety margins are. Figs. 8a and 9a presents the DTC results for scenario sets 1 and 2 (Section 6.1). As the figures show, the designed integrated NMPC control outperforms the benchmark control strategies providing the highest DTC values. Also, both the benchmark controllers fail to avoid the collision at 100 km/h (DTC value is zero) whereas integrated NMPC controller avoids the collision successfully. It is to be noted that DTC values are meaningful when looked along with trajectory tracking overshoot values. A higher overshoot value may result in high DTC value. In principle this reflects that collision was safely avoided but it does not highlight that trajectory tracking was poor. Therefore, for both sets 1 and 2, the percentage overshoot figures have also been plotted in Figs. 8b and 9b. It can be seen that linear MPC gives slightly higher DTC than nonlinear MPC. But linear MPC also gives a very high overshoot value as compared to nonlinear MPC. Therefore, with a marginal difference in DTC value and negligible overshoot observed, the performance of nonlinear MPC is overall better than linear MPC. And integrated NMPC not only gives highest DTC value but also gives close to zero overshoot value, proving that it indeed performs the best of all.
Lateral wind. Table 6 summarizes the KPI values for scenario set 3 (i.e., lateral wind offset scenario) using the proposed controller. For wind speeds up to 70 km/h, the controller is able to avoid the collision successfully. The table reports an additional parameter, namely off , that measures the offset distance between reference trajectory and the vehicle trajectory at the end of the maneuver. With such high wind speeds, the proposed controller returns a maximum offset value of 0.12 m, judged smaller than existing references (≈ 0.5 m). This highlights the effectiveness and robustness of the proposed control scheme.
Maneuver's aggressiveness. Table 7 summarizes the KPI values for scenario set 4 (i.e., varying maneuver's aggressiveness) using the proposed controller. The table reports the maximum lateral acceleration generated by the maneuver, that is max , highlighting that for varying dynamic scenarios, the controller is working efficiently. These results show that the proposed design, thanks to its integrated ability to simultaneously steer and brake, provides both lateral and longitudinal control and avoids the collision in all scenarios successfully.  (18) is active, the lane change scenario with pre-braking maneuver was considered. Two seconds before the subject vehicle is 30 m away from lead vehicle, the subject vehicle will brake and decelerate. After this, the single lane change maneuver is performed. This was done as the IBD constraint is only activated during straight-line pre-braking maneuver. Fig. 10 shows the brake torques. Due to this brake distribution constraint, front brake torques are more than the rear.

Constraint satisfaction.
The controller is real-time feasible during the maneuver. Fig. 11 shows the calculation times for the number of calls for each considered scenario (5-6 simulations per each scenario according to the Tables 2-5). In particular, it should be noted that the computation time increases when the reference signal changes. This behavior is typical of the optimizer used to solve the nonlinear control problem. The computation time tends to increase with the number of active constraints. However, the computation time is still within the real-time constraint highlighted by the dashed-red line. The maximum number of iterations of the solver is empirically fixed to 5 (nX + nU + nC) with nX, nU and nC representing total number of states of the prediction model, total number of control action and total number of constraints respectively. The value is based on the diagnostic flags of the solver during initial tests. The solver was warm-started with the prediction computed at the previous time step (shifted by one in time). This helps mitigate the effect of unpredictable computation times. In addition as a backup, if the solver fails to find a feasible solution within the fixed number of iterations, the last feasible solution is applied to the vehicle in closed loop.   The proposed design is able to deal with constraints even when the maneuver is performed in the nonlinear regime of motion, as Figs. 12 and 13 show. The figures depict the g-g diagram and Kamm circle values, respectively, for = 90 km/h and = 0.9. It can be seen that the values are within the defined stable envelope and at the same time, while preserving tracking performance. It can be seen in   i.e. towards negative longitudinal force direction as the maneuver is being performed. This is because the test vehicle used in the simulation is a Front Wheel Drive (FWD) car. Even though all the four wheels are braking (as seen in Fig. 7), the drive torque from the engine is being transferred to the front wheels as a result of which the overall tire longitudinal forces in the front tires are mostly in the positive region of Kamm circle. But during the maneuver, the rear tires brakes as well and the overall tire longitudinal force becomes negative as a result of which the rear tire's longitudinal force are mostly in the negative side of Kamm circle. Since the RR tire brakes the most among the two rear wheels, the longitudinal force shift towards the negative half is more as compared to other wheels. Nevertheless, it can be seen that the lateral force ratio is very high and close to the limits, suggesting that the controller is able to control the nonlinearities of the maneuver effectively at all times.

Conclusion and future work
The goal of this work is to design an integrated nonlinear MPC controller to provide effective vehicle control in both linear and nonlinear regime of motion and to reduce the high number of accidents caused in a rear-end collision scenario. In this scenario, it is important to show the controller's ability to guarantee vehicle stability and passenger safety for various conditions. Also in this scenario, it is of foremost importance  to take into account that a vehicle's motion is always coupled in both the lateral and longitudinal direction. Conventional hierarchical control strategies implemented on vehicles, often consider the design of lateral and longitudinal control separately, making the simplified assumptions. In contrast to the classical approaches, we proposed an integrated control strategy based on nonlinear model predictive control (NMPC) taking into account the coupling between lateral and longitudinal control by design. It was shown how this strategy is able to effectively control the vehicle when the maneuver is in the nonlinear range of motion and in the presence of lateral wind. The controller did not show any oscillatory behavior and overshoots while tracking the desired reference trajectory for the various conditions tested. We compared the proposed design with two benchmark controllers based on model predictive control. Compared to the proposed MPC controller design, the other two baseline MPC control approaches rely on simplifying assumptions on the prediction model (linear and nonlinear bicycle model) and definition of the constraints. Our integrated NMPC design outperformed the other two control strategies in all considered scenarios. Furthermore, the designed strategy showed robustness to external disturbances and parameter uncertainty, while being real-time feasible.
The recommendations for future work are (i) to include the tire slip dynamics inside the prediction model to control the phenomenon of wheel locking and the ABS activation while braking; (ii) better trajectory generation methods can be used to make the reference values more realistic and practical to follow; (iii) incorporation of tire model inside the controller's prediction model can make the MPC more robust to parameter variation and improve the dynamic capabilities of the controller to capture vehicle dynamics.

B.3. Longitudinal forces
The mathematical equation that describes the wheel dynamics in y-axis in the single corner model described below (note that the rolling resistance moment has been neglected here): w ij̇i j = ij − ij act − ij eff ij , ij = (fl, fr, rl, rr) (B.13) Neglecting wheel dynamics, the term ( The wheel slip ij for each wheel is given by: The Dugoff model based tire longitudinal and lateral force calculation is used in the nonlinear bicycle model based MPC design. By capturing the tire nonlinearity during the maneuver, the overshoot (as seen in the case of linear bicycle model based MPC) was reduced substantially, thereby improving the overall performance.