Shared Control Up to the Limits of Vehicle Handling

Despite significant advances in vehicle safety, roadway accidents remain a substantial danger. To bring about safer vehicles, researchers and manufacturers continue to develop new systems to assist drivers in dangerous situations. Current approaches often implement several independent systems, each of which can assist in only specific situations, leaving open critical paths for accidents to occur. We propose a general approach to driver assistance based on nonlinear model predictive control (NMPC). This system can intervene in both lateral and longitudinal commands to keep the vehicle safely within the boundaries of the road, but allows the driver freedom to maneuver the vehicle as they wish when they act safely. This work builds on previous MPC approaches by incorporating a notion of safe operating speed. Properly modulating speed ensures the vehicle's limits are never exceeded, and not reached unless necessary. Experimental results on a full-size steer-, brake-, and throttle-by-wire vehicle, validate the performance of this system. These experiments show that the controller effectively matches the driver's commands when possible but will deviate from those commands when necessary for safe operation.


Shared Control Up to the Limits of Vehicle Handling
John Talbot , Matthew Brown , and J. Christian Gerdes , Member, IEEE Abstract-Despite significant advances in vehicle safety, roadway accidents remain a substantial danger.To bring about safer vehicles, researchers and manufacturers continue to develop new systems to assist drivers in dangerous situations.Current approaches often implement several independent systems, each of which can assist in only specific situations, leaving open critical paths for accidents to occur.We propose a general approach to driver assistance based on nonlinear model predictive control (NMPC).This system can intervene in both lateral and longitudinal commands to keep the vehicle safely within the boundaries of the road, but allows the driver freedom to maneuver the vehicle as they wish when they act safely.This work builds on previous MPC approaches by incorporating a notion of safe operating speed.Properly modulating speed ensures the vehicle's limits are never exceeded, and not reached unless necessary.Experimental results on a full-size steer-, brake-, and throttle-by-wire vehicle, validate the performance of this system.These experiments show that the controller effectively matches the driver's commands when possible but will deviate from those commands when necessary for safe operation.
Index Terms-Driver assistance, model predictive control, shared control, vehicle safety.

I. INTRODUCTION
E VERY year, thousands of Americans lose their lives to motor vehicle crashes [1].Of the many factors that can contribute to a crash, a study conducted by NHTSA showed that human error accounts for 94% of the critical reasons for pre-crash events [2].
To address driver shortcomings, manufacturers develop and deploy many safety systems on consumer vehicles to reduce traffic accidents.Anti-lock Braking Systems (ABS) were introduced in passenger vehicles throughout the 1980's and early 1990's.The system modulates brake pressures to prevent wheel lockup while maintaining the maximum possible brake force.Preventing front wheel lockup also aids the driver by maintaining steering authority of the vehicle.Despite success on the test track, Kahane and Dang found through a long term study that the system had a negligible reduction in fatal roadway crash risk [3].While not pointing definitively to a reason for the inefficacy of ABS, the authors suggest that while ABS reduced fatalities on Fig. 1.X1, the drive-by-wire, student build research vehicle.The drive-bywire capabilities are well suited to driver assistance system testing, allowing the system to override driver commands when necessary for safety.
the road, it increased the risk of the vehicle leaving the roadway and a fatality occurring.
Though not directly effective at reducing fatalities, ABS technology became a fundamental component of Electronic Stability Control (ESC) which has successfully improved vehicle safety.Van Zanten et al. [4] proposed an ESC system that modulates brake pressures not just to prevent wheel lockup, but to ensure the vehicle maintains stable operation in challenging maneuvers.The system infers the driver's intended trajectory from steering wheel and pedal measurements.It then stabilizes the vehicle around this intended trajectory by modulating brake pressure to each wheel as needed to induce a supplementary yaw moment.Adding a yaw moment through braking also improves the steering authority of the driver in an understeering condition.By including a concept of driver intent, ESC performed much more effectively than previous ABS concepts.Webb showed that ESC successfully prevented over 7000 roadway fatalities in the United States between 2011 and 2015 [5].
Modern advanced driver assistance systems (ADASs) significantly expand the ability of a vehicle to keep a driver safe by including a notion of the vehicle's environment.Collision imminent braking systems apply brake pressure without input from the driver to mitigate the effects of a front or rear-end collision [6].Lane keeping assist systems helps drivers avoid drifting into an adjacent lane [7].Taking the vehicle's environment into account marks a great improvement in safety; however, each system utilizes only a specific concept of the environment.This limited scope of the environment allows the system to intervene only in specific situations, leaving open the possibility that no system would have the authority or knowledge to intervene appropriately in a complex emergency scenario.
Ideally, an assistance system could reason more generally about the safety of the vehicle alongside the wishes of the driver.It is important that the vehicle always remains in a safe state, so the system must be able to make significant corrective actions when in an emergency.It must also allow the driver to operate the vehicle unimpeded so long as their actions result in safe operation.These objectives come into tension when the driver acts incorrectly or negligently, so the system must be able to trade-off between them, overriding the driver's actions when necessary.
Many approaches have been proposed for such a system, and one technique, Model Predictive Control (MPC), is popular among them.Borrelli et al. [8] describe MPC as an optimal control technique where an open-loop, finite time, constrained optimization problem is repeatedly solved given the system's sampled states.The first inputs of the resulting optimal trajectory are executed, and after some finite time, the system's states are resampled and the process repeats.MPC typically refers to optimization problems that constrain the problem's decision variables using only linear combinations of those decision variables.See Rawlings' tutorial of MPC for an accessible reference on the topic [9].
MPC offers a convenient way to encode trade-offs between competing objectives like driver matching and safe operation.Beal and Gerdes [10] developed a linear convex MPC to enforce vehicle stability while directly incorporating driver steering commands into the optimization.It effectively stabilized the vehicle, but no environmental information was included.Erlien and Gerdes [11] extended Beal's approach by adding a safe driving corridor constraint to the problem.This constraint enforced that the vehicle's trajectory could not go off the roadway or through obstacles.Guo et al. [12] went further, dynamically varying the incentive to match the driver's steering input based on a metric of hazard.Anderson et al. [13] developed a technique to blend the driver's commands and the output of a MPC path tracking controller based on a heuristic metric of vehicle threat.
Importantly, these architectures only control steering angle and have no way of influencing vehicle speed.While steering control alone works well in many circumstances, effective speed control becomes increasingly important as the vehicle reaches its handling limits.Inagaki et al. [14], showed that increasing speed through a maneuver decreases the size of the stable convergence region of the vehicle's yaw-rate and lateral velocity states.For this reason, production ESC systems limit a driver's ability to accelerate when active and often provide braking specifically to reduce vehicle speed in addition to providing a yaw moment [4].
Because MPC encodes a notion of time through its prediction horizon, it naturally allows for temporal trade-offs such as reducing speed now to avoid saturating cornering forces later.This behavior offers a great advantage in automotive applications; however, the nonlinear interdependence of longitudinal and lateral dynamics creates an obstacle to using a classic linear MPC.Nonlinear Model Predictive Control (NMPC) shares the fundamental idea of MPC, but generalizes the underlying optimization problem to accept nonlinear constraints and objectives.It can therefore incorporate coupled lateral and longitudinal vehicle dynamics directly.Brown and Gerdes [15] showed that NMPC could be used to navigate a challenging double lane change to avoid two obstacles leaving only a small navigable path.In that work, coordinating longitudinal and lateral forces throughout the maneuver was critical to maintaining vehicle stability while avoiding the obstacles as the tires on each axle were operating at their limits.The fullest treatment of applying NMPC for driver assistance comes from Schwarting et al. [16], who developed a framework directly incorporating a driver's steering and acceleration commands, while maintaining a safe trajectory.They proposed a method of dynamically weighting the two main objectives of the system, matching driver commands and tracking a safe reference path, over the NMPC prediction horizon which we adopt in this work.In their formulation, the authors use a maximum speed objective through the NMPC horizon as an approximate progress objective.This contrasts with ESC and other proposed safety systems which look to decrease vehicle velocity in emergency scenarios.While showing promising results in simulation, pushing vehicles nearer their limits can reduces the robustness of the system in some situations and should be avoided when possible.
We propose a NMPC approach to driver assistance that directly incorporates a driver's steering and longitudinal force commands while ensuring the vehicle maintains a safe trajectory through its environment.We contribute a novel progress objective matching a safe reference speed, allowing the system to operate up to the limits of handling when necessary but giving it no incentive to do so normally.We contribute a method to mitigate delays between the driver's commands and the vehicle's actions using their command rates.We contribute an improved objective function for driver matching improving upon similar approaches by eliminating small unnecessary interventions while allowing significant interventions when necessary.
This article is structured as follows.Section II details the dynamic model used to describe the vehicle's behavior.Section III explains the setup of the NMPC problem, in particular a novel formulation for the driver matching and path progress objectives.Section IV provides two sets of simulation results verifying the selected objective parameters and the systems response to different driving behaviors.Experimental results in Section V demonstrate the effectiveness of this controller on a full size vehicle on a closed test course.

II. MODELING
To adequately capture vehicle behavior up to the limits of handling, we employ the planar single-track vehicle model shown in Fig. 2. Three velocity states describe the vehicle's motion with respect to the vehicle body frame: the longitudinal velocity u x , the lateral velocity u y , and the yaw rate r.Three position states locate the vehicle in the curvilinear frame of the reference path: progress along the path s, lateral offset from the path e, and heading offset ΔΨ. κ represents the instantaneous curvature of the path.The steering angle δ and the longitudinal forces at each axle, F xf and F xr act as inputs to the model.F yf and F yr are lateral tire forces as described in Section II-B.The model is parameterized using the vehicle's mass m, the distances from the vehicle's center of mass to it's front and rear axles a and b, respectively, and it's yaw axis moment of inertia I zz .The following six equations describe the behavior of the vehicle:

A. Longitudinal Weight Transfer
During heavy braking and acceleration events, the weight transferred between the front and rear axle significantly changes the normal load at each axle.The change in normal load then affects the force producing capability of that axle's tires as described in Section II-B.A static moment balance sufficiently captures this phenomenon while maintaining model simplicity.The instantaneous normal load on each axle is given by where g is acceleration due to gravity, h cm is the height of the center of mass measured from the ground, and L = a + b is the total wheelbase of the vehicle.

B. Tire Model
The lateral tire forces, F yf and F yr , arise from the slip angles, α f and α r , respectively.The angle from a tire's longitudinal heading vector to its velocity vector defines its slip angle.The front and rear slip angles are given by ( 9) and (10), respectively.
The lateral tire forces relate to the tire slip angles using a modified Fiala brush tire model derived from models proposed by Pacejka [17].This brush tire model describes the lateral force at a given slip angle using two parameters: the road-tire friction coefficient, μ, and the cornering stiffness of the tire C α .The sliding slip angle, α sl characterizes the transition from the unsaturated to the saturated operating region of the tire model.
is the maximum lateral force the tire can produce.
When operating at the limits, the maximum lateral and longitudinal forces of the tire are coupled, limited by the total force generation capacity of the tire.We describe this coupling by derating the maximum lateral force capability of the tire by the commanded longitudinal force as shown by Hindiyeh and Gerdes [18].

III. NMPC PROBLEM FORMULATION
We formulate the proposed ADAS as a NMPC with N = 60 stages of Δt = 50 milliseconds each for a total horizon length of N Δt = 3 seconds.Converting the continuous vehicle model from Section II to a discrete model using trapezoidal integration over the stage duration yields a discrete temporal mapping from current vehicle states and inputs to the vehicle states at the next stage.Two variable changes are implemented between the vehicle model proposed in Section II and the NMPC formulation.First, the front and rear longitudinal force are combined, F x = F xf + F xr , to avoid competing front and rear forces.Secondly, the inputs to the vehicle model δ and F x are treated as states in the NMPC formulation, and their rates of change δ and Ḟx are treated as inputs.This choice of inputs allows the simple inclusion of actuator slew rate constraints and penalties to ensure command feasibility and smoothness.The state vector at stage k of the NMPC horizon The complete NMPC formulation is given by (14a).As is typical in receding horizon controllers, the initial state vector is constrained to the actual measured state of the vehicle and actuators (14b).Each subsequent state is constrained by the discretized dynamics described in Section II (14c).At each time step the actuator position limits (14d) (14e) and slew rate limits (14f) (14g) are constrained by their physical limits.The total longitudinal force is mapped into a front and rear longitudinal force using brake and drive biases at each axle.The biases, χ f for front axle bias, and χ r for rear axle bias are computed using the smooth approximation presented by Laurense et al. [19], adapted for a rear wheel drive vehicle.Finally, the magnitude of the front and rear maximum longitudinal tire force are constrained by their respective friction limits (14j) (14k).As the lateral forces are constrained within the derated brush tire model itself, this completely encodes the friction limits of each of the tires.
x (14h) The total objective J combines J driver (k), the objective to match the driver's commands (18), J path (k), the objective to tracking the reference path (20), J actuator (k), the objective to minimize actuator slew rates (21) J edge , the objective to stay within the road boundaries (16a), and J term , the terminal objective of the NMPC problem (22).λ(k) is a dynamic weighting function between matching the driver and tracking a reference path (17).The following sections detail the individual components of the objective function.

A. Safe Operation
Approximating the vehicle body as a set of circles as presented by Brown and Gerdes [15] yields a computationally inexpensive formulation to find the signed distance from the vehicle to the road boundaries.A set of two circles, one at the front axle and one at the rear approximates the vehicle body in this work.Within the road boundaries, the signed distance is positive and crossing a road boundary yields a negative distance.We define the safe operating region for the vehicle as the region where the distance to both road edges are positive.At each stage, k, while within the limits of the safe operating region, the vehicle incurs no cost.Violating these boundaries incurs a high quadratic cost given by (16a) where d (k) edge is the signed distance to the road boundary at stage k, and W edge is the cost on crossing the road boundary.

B. Time Variable Secondary Objective
While the system must always keep the vehicle safe, to be accepted, it must match the driver's intent as closely as possible.Because the driver may continuously vary their driving inputs, predictions of their behavior lose validity as they are made further into the future.To mitigate this Schwarting proposed trading between two different secondary objectives over the prediction horizon through an exponential weighting function [16].In the near term, predictions of the driver's steering and longitudinal commands are more certain, making matching them directly more reasonable.Later in the horizon as the uncertainty in those predictions grows, tracking a predetermined safe path becomes more appropriate.We employ the same strategy in this work, decreasing the weight on matching the driver's commands exponentially as a function of time through the horizon according to (17) where γ is a tuning parameter: The relative contributions of driver matching and path tracking over the prediction horizon are shown in Fig. 3.

C. Driver Intent
The system obtains the driver's intended steering commands by measuring their hand wheel angle directly.The driver's intended longitudinal commands cannot be measured directly, so the system infers them through position measurements of the throttle and brake pedals.These pedal positions map linearly onto the force generating capabilities of the motor and brakes, respectively.We assume that the current driver steering rate and pedal position remain constant throughout the NMPC horizon to illustrate this system performs well without a complex model of the driver's behavior.This is discussed in greater detail in Section V-B.
Ideally, small, unnecessary deviations from the driver's commands would be eliminated from an ADAS while still allowing for significant intervention when needed.A 1-norm or absolute Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
value cost function eliminates small deviations while allowing large deviations when needed, but is not smooth which can make it difficult for numeric solvers to find a feasible, optimal solution.We use a log cosh cost function as shown in (18) where δ driver is the driver's steering command, F driver is the driver's longitudinal force command, W δ driver is the cost on deviations from the driver's steering commands, and W F driver is the cost on deviations from the driver's longitudinal force commands.This function approximates the 1-norm cost function behavior well, while also being twice-differentiable and smooth.A scaling factor η δ is used to normalize the relatively small value of steering angle in radians and η F x scales the relatively large values of longitudinal force in newtons to kilonewtons.

D. Path Tracking and Safe Progress Objective
As the influence of the driver's current commands diminishes throughout the horizon, the main objective changes to maintaining a safe and continuously feasible trajectory on the roadway.In this region of the NMPC horizon, a progress objective throughout the horizon ensures the horizon continuously recedes, rather than simply stopping the vehicle whenever approaching an environmental boundary.Schwarting's approach relies on a maximum speed objective as a proxy for maximum progress [16].While such an objective performs adequately in many situations, it can unnecessarily incentivize the system to operate the vehicle near its operating limits, thus reducing the robustness of the system.In contrast, the proposed formulation incentivizes matching a predetermined safe speed profile as a progress objective.In this implementation, the maximum speed a point mass could navigate a given corner with the modeled friction limits and road curvature given by (19) determines the safe reference speed.
To accomplish path tracking, the NMPC formulation includes an objective to follow a predetermined reference path.A deviation from the reference lateral position, heading, or speed incurs a quadratic cost given in (20) where W e is the cost on lateral path tracking error, W ΔΨ is the cost on heading tracking error, and W ux is the cost on speed tracking error.

E. Actuator Rates
A penalty on actuator slew rates helps minimize sharp changes in steering and acceleration and smooth the final trajectory.The slew rate objective is given in (21) where W δ is the cost on steering slew rate, and W Ḟx is the cost on longitudinal force slew rate.These costs, in addition to the cost on matching driver commands, influence the aggressiveness of an intervention maneuver.Increasing the penalty on large actuator slew rates incentivizes the controller to deviate from the driver's commands sooner when an unsafe condition is predicted to avoid a sudden large sudden deviation.The actuator slew rate costs thus provide a method to tune the invasiveness of the system to fit the skill and comfort level of an individual driver.A less skilled driver or one that desires an earlier but smoother intervention can increase the slew rate costs and achieve that behavior from this system.A more skilled driver or one that desires less intervention can decrease these costs, yielding fewer but more aggressive and abrupt interventions.

F. Terminal State
The NMPC formulation includes a strong incentive to keep the vehicle in a known safe state at the end of the prediction horizon by including a large terminal cost on the path tracking terms at the terminal stage of the controller.This promotes the feasibility of future solves.The terminal objective is given in (22) where k = N is the final stage of the NMPC prediction horizon, W e,N is the cost on lateral path tracking error, W ΔΨ,N is the cost on heading tracking error, and W ux,N is the cost on speed tracking error, all at the final stage of the NMPC prediction horizon.
IV. SIMULATION RESULTS In the following simulations, we show firstly that our choice of optimization parameters is appropriate for the stated goal.Secondly, we compare progress objective formulations to show the advantages of matching a safe reference speed rather than maximizing speed.

A. Validating Optimization Parameters
A capable ADAS must respond appropriately to the full range of possible driver commands while maintaining the safety of the vehicle.In this section, we show that the proposed system effectively responds to different driver steer angles given the parameters listed in Table I.All simulations and experiments presented in this article make use of these parameters.The following simulations show the system's response to five equally spaced driver steering commands from the left-most to the right-most steering limits.In each simulation, the vehicle travels down a straight road near the right road boundary at 8 m/s.Because the vehicle is nearer the right road boundary, the system will need to intervene significantly when the driver steers right, but very little when they steer left.The initial steer angle state is set to match the driver's input, and the driver's steering rate is set to zero.Fig. 4 shows the optimal NMPC horizon of steer angles for each of the driver's commanded steering angles.The driver may freely steer the vehicle toward the left boundary, as there is no danger of the vehicle leaving the roadway in that direction.As the driver steers right, however, the system takes significant action to keep the vehicle on the road.In each case, the system responds appropriately to the driver's commands.The incurred cost of the driver steer angle matching term in (18) provides a metric for the severity of the intervention.

B. Maximum Speed vs. Safe Speed Matching
Increasing the speed of a vehicle in a challenging maneuver increases the risk of dangerous outcomes including loss of vehicle control.As illustrated by Brown and Gerdes [20], naively operating near the vehicle's limits can reduce robustness to unmodeled disturbances in MPC implementations.Maintaining a buffer between the vehicle's operating states and its modeled limits improves the robustness of the controller.To do this, it is important to reduce the vehicle's speed before entering a maneuver that would challenge its limits.
Potentially in conflict with this objective of lowering speed during challenging maneuvers is the progress objective of the NMPC formulation.As described in Section III, this formulation incentivizes the vehicle to match a safe reference speed rather than maximizing speed as a proxy for progress along the reference path.We show the significance of how to influence the vehicle's speed with two sets of simulations implementing both NMPC formulations through a challenging hair-pin turn maneuver, turn ten of Thunderhill Raceway Park's west track.The simulations assume icy conditions with a nominal road-tire friction value of μ = 0.3.Each simulation also demonstrates the performance of two conventional lane keeping assistants as a baseline.The first implements the potential field lane keeping assistant proposed by Rossetter et al. [21], which is a reasonable proxy for a production lane keeping assist.The second adds a simple speed limit monitor to the potential field lane keeping assistant.This monitor augments the driver's force command with a linear feedback term between the vehicle's current speed and the speed limit.The monitor will only become active when the vehicle's speed exceeds the speed limit, and then may only use up to twenty percent of the vehicle's maximum braking capability to reduce it's speed.The speed limit for this controller is identical to the safe reference speed used in the NMPC formulation presented in this work.These two systems differ fundamentally from the NMPC formulations as they actively nudge the driver toward the center of the lane.
The vehicle begins each simulation aligned with the road at its lateral center near the entry of the corner.The initial position of the vehicle and one candidate trajectory is show in Fig. 5.The safe reference speed of this corner is determined as 7 meters per second using (19).The vehicle's initial speed is 9.2 meters per second (5 miles per hour greater than the safe speed), and the driver commands zero steer angle and 250 newtons of longitudinal force throughout each simulations Fig. 6.Comparison of different driver assistance formulations maneuvering through the hairpin corner shown in Fig. 5.Both NMPC formulations and the lane keeping assistant implementing a speed limit perform well with perfect knowledge of road-tire friction.The classic lane keeping assistant cannot maintain a safe trajectory through the corner.The lane keeping assistant with speed limit monitoring is much more invasive, restricting the driver's speed more than the other systems.
representing a very light acceleration.In the maximum speed objective formulation, the stage-wise cost on tracking the safe reference speed as shown in (20) becomes a maximum speed objective.Additionally, the terminal cost in (22) on matching the safe reference speed becomes a maximum speed objective.All other parameters remain equal for both NMPC based systems.
In the first set of simulations, each system has perfect knowledge of the friction between the vehicle's tires and the roadway.As shown in Fig. 6, the difference in behaviors of two NMPC formulations with different progress objectives is small.The classic lane keeping assistant that cannot influence speed is unable to keep the vehicle safely on the roadway.The lane keeping assistant with speed limit monitoring does successfully keep the vehicle near the center of the lane, but as shown in Fig. 6, it enforces a very conservative speed which can be frustrating to a driver.
In real-world operation, there are often slight differences in the modeled and true behavior.One example, described by Martin et al. [22], is winter conditions where friction between vehicle tires and the road varies with several conditions.To highlight the sensitivity to unmodeled disturbances, we introduce a relatively small but typical mismatch in the friction coefficient used in the NMPC formulation and in the simulator.The modeled friction coefficient each of the systems at both tires is μ = 0.3; however, the friction actually available to the front tire is μ actual = 0.28.Fig. 7 illustrates the improved robustness of the safe reference speed objective over a maximum progress objective.Like the previous simulation results, the conventional lane keeping assistant cannot keep the vehicle safely on the road because it cannot influence the vehicle's speed.The lane keeping assistant with speed limit monitoring again does well to keep the vehicle safe, but it is much more invasive in restricting the driver's desired speed.The simulated vehicle implementing the NMPC 7. Comparison of driver assistance formulations maneuvering through the hairpin corner shown in Fig. 5 with a mismatch in modeled and experienced friction.The safe reference speed progress objective is more resilient to this model mismatch and keeps the vehicle safely on the road, while the maximum speed progress objective exits the roadway.The classic lane keeping assistant again cannot maintain a safe trajectory through the corner, and the lane keeping assistant with speed limit monitoring does keep the vehicle safely on the road, but is much more restrictive than the other systems.
formulation with the maximum progress objective exited the roadway, while the simulated vehicle with the safe reference speed objective remained safely on the road.This improvement in robustness comes without introducing conservatism because the safe reference speed is based on the modeled friction value, so the NMPC will allow the driver to take the vehicle up to the modeled limits.The proposed controller has similar performance to the maximum speed objective controller when the vehicle is operating near the safe reference speed with no model mismatch; however, under even a small amount of friction variation, the proposed controller has much better performance, marking one novel contribution of this work.

V. EXPERIMENTAL RESULTS
In this section, real-world experiments performed on a fullsized test vehicle will show firstly that the vehicle responds naturally to driver commands other than a simulated constant input.Secondly, the experiments will validate our choice of dynamic model, and the system architecture performs adequately amid real-world conditions on a test track.Keeping an instrumented vehicle safe up to its handling limits adds confidence that this system can perform its safety tasks in a dynamic and challenging environment.
In these experiments, information about the environment, namely the position of road boundaries and the safe reference speed of the track are determined a priori and embedded into the NMPC formulation.For simplicity, in these experiments, the nominal trajectory is the centerline of the racetrack, and the road bounds were set at 2.8 m to either side of the center line for a total navigable width of 5.6 m.These values enforce a physical safety buffer when experimenting on track but are easily adjustable to values appropriate for the scenario at hand, e.g. the lane width of a road.For production implementations this information could come from surveyed maps of the roadways being used along with their posted speed limits.An online perception stack could augment this information improving reliability and adding adaptability for a changing environment as shown by Wille et al. [23] A. Experimental Setup The driver assistance system in this article was experimentally validated using X1, the student designed and built electric test platform shown in Fig. 1.X1 relies on steer-, brake-, and throttleby-wire systems to control the on-board actuators.Sensors measure the position and speed of the steering handwheel as well as the position of the throttle pedal.A dSpace MicroAutoBoxII provides low-level actuator control and communications between all measurement and actuation systems and the PC described below.An OxTS RT4003 dual antenna RTK GNSS unit produces precision estimates of the vehicle's position and velocity states in real time.The nonlinear program (14a) was implemented using the open-source automatic differentiation package Casadi [24], and solved using IPOPT, an open source interior point method solver [25].The generated solver was run on a conventional PC with an Intel i7 CPU running Ubuntu 20.04 the Robotic Operating System, (ROS) [26].

B. Improving Responsiveness to Driver Commands
Assumptions on the driver's future behavior become important when employing this framework on an experimental vehicle where it must run in real-time.Average solve times during the experiments performed range between 50 to 100 milliseconds.Solves of this length introduce a delay between when current values are sampled and the solution which results from them.To mitigate this delay for vehicle stability and control, we adopt the replanning strategy proposed by Brown and Gerdes [15].This strategy reduces the effect of delays caused by the relatively large solve time in two ways.First, it linearly interpolates the previously solved trajectory to supply commands to low-level controllers operating at a faster rate.Second, it uses the previously solved trajectory to project the current state of the vehicle forward in time by 50 milliseconds to become the initial conditions of the next solved trajectory.
Such solve times also introduce a delay between the driver's commands and those executed on the vehicle.This delay leads to confusion and frustration and may ultimately lead to the driver's rejection of the safety system.To mitigate these delays, we assume the driver's steering rate remains constant throughout the horizon, rather than assuming their current command will remain constant.
Experiments performed on X1, implementing different assumptions on driver behavior, illustrate the effect of these assumptions on system performance.In these experiments, the driver manually inputs an oscillating steering input to the handwheel, and measurements of the handwheel and road wheel positions were captured.The first experiment assumes that the driver will hold their steer angle constant over the full prediction Fig. 8. Example of delay between driver commands and NMPC output.In this example, the driver's current steering inputs are held constant over the prediction horizon.The driver's commands are effectively delayed by roughly 100 milliseconds.Fig. 9. Delay mitigated using first-order-hold.Assuming the driver's input rates remain constant over the horizon mitigates the delaying effects and improves responsiveness to the driver.horizon.Fig. 8 shows the delay in steering between the driver's handwheel position and the position of the road wheels.This delay between the driver and vehicle results from a few aspects of the NMPC formulation, namely: 1) Solve times taking at least 50 milliseconds 2) The first stage of the NMPC prediction horizon is constrained to the current measured actuator state 3) The driver's commands are assumed constant throughout These factors effectively delay the driver's commands by approximately 50 to 100 milliseconds.Assuming the driver will hold a constant steering rate over the horizon yields a significant improvement in system responsiveness, making driving feel much more natural.Fig. 9 shows the same experiment with the constant steering rate assumption.Importantly, though the predictions made using either assumption quickly lose accuracy Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.Fig. 10.Vehicle position for skilled driving behavior.The driver remains near the center of the road throughout, so no intervention is necessary.compared to the driver's actual future inputs, the driver predictions only have a significant effect on the NMPC solution very early in the prediction horizon.As described in Section III, the objective transitions from driver matching to path tracking after the first few hundred milliseconds.It is also important to note that this assumption does not affect the safety objective of the formulation.
While this assumption tends to work quite well, it can cause a temporary deviation from the driver's intentions in some situations.Specifically, if the driver changes the direction of steering rate at the same time the solver experiences an especially long solve, the constant rate assumption will lead to a greater deviation from the driver's command than a constant position assumption.
Because this delay stems from the significant amount of time it takes to solve the optimization problem, it could also be mitigated with greater computing resources.We provide this solution as a means to implement this algorithm in a situation where computing resources may be constrained, which is often the case in production vehicles.

C. Driver Matching and Safety
Two experiments demonstrate the proposed system's actions given two different driving behaviors, that of a skilled driver and of a worst-case unskilled driver.In both experiments, the vehicle enters turn ten of the Thunderhill Raceway Park west track at a high speed.
The first experiment illustrates the systems reaction to skilled driving behavior.At the entry of the corner, the driver releases the throttle, applies moderate brake pressure, and steers to navigate through the corner while stabilizing the vehicle.By braking before entering the corner, the driver can navigate safely without reaching the limits of the vehicle.While the controller does include a cost on tracking the path later in the horizon, the driver is able to control the vehicle as desired while they are driving safely and effectively.Fig. 10 shows the driver moving freely within the road boundaries.With the driver acting appropriately, both maintaining operation within the vehicle's limits and the road boundaries, the assistance system does not need to intervene.Indeed Fig. 11 shows that the driver's commands are effectively passed through the assistance system with no modification.Fig. 12 shows the estimated limits of the tires and utilized tire forces according to the model in Section II.Even though the driver operates the vehicle near the modeled friction limits, the vehicle remains safe and does not approach the road boundaries.
The next experiment illustrates the system's reaction to unskilled driving behavior in the same scenario.This provides an intuitive worst-case driver to benchmark the controller.Approaching the corner, the driver maintains full throttle and applies no steering to navigate through the corner.The vehicle would leave the roadway at a high speed if no intervention occurred, likely leading to a serious accident.In this case, the assistance system deviates significantly from the driver's Fig. 13.Vehicle position for unskilled driving behavior.The vehicle nears and eventually touches the right road boundary.The driver does not take action to keep the vehicle on the road, so the ADAS system must make a significant intervention.Fig. 14.Driver inputs vs. ADAS commands for unskilled driving behavior.Notice the ADAS must make a significant intervention in both longitudinal and lateral commands to keep the vehicle safely on the road.commands as shown in Fig. 14.The assistance system both applies brake pressure to slow the vehicle down and turns left to compensate for the driver's lack of steering.Fig. 13 shows that the assistance system succeeds in keeping the vehicle within the safe operating boundaries of the roadway.Fig. 15 shows the tire force utilization through the worstcase driver test.The controller successfully drives the vehicle at the limits of handling as required to safely navigate the turn at speed.This behavior represents the most important feature of this system: while it does not push the vehicle up to its limits during normal operation, the system effectively operates at the limits when necessary to keep the vehicle safe.

VI. CONCLUSION
In this article we have presented a novel approach to ADAS using NMPC.This controller tries to directly match driver commands until those commands would result in an unsafe situation Fig. 15.Lateral tire forces for unskilled driving behavior.The system must operate at the modeled limits of tire friction for a short time to keep the vehicle on the road, but does not remain at the tire limits when it is not necessary to.
for the vehicle.To do this effectively, the controller modulates the lateral and longitudinal inputs of the vehicle to ensure the tire limits are not exceeded and indeed not reached unless necessary.Simulations show the importance of choosing an appropriate progress objective that does not incentivize the vehicle to operate near its limits when unnecessary, as this reduces the robustness of the system.Two experiments on a test vehicle demonstrate the effectiveness of this system with two distinct driving behaviors: one of an experienced driver who can successfully navigate the vehicle on their own, and one of an unskilled driver who would run the vehicle off the road without intervention.

Fig. 3 .
Fig.3.Relative contribution of objective function terms.Further into the prediction horizon, the projected driver intent becomes less valid.To mitigate this, the objective of the NMPC formulation transitions from driver matching to path tracking.

Fig. 4 .
Fig. 4. Optimal NMPC horizons of steering angle for several different driver steering inputs.The driver may steer left relatively unimpeded as even the leftmost steer angle would not cause the vehicle to leave the road.The system intervenes increasingly aggressively as the driver steers farther right near the right road boundary.Note the incurred costs are colored based on a logarithmic scale.

Fig. 5 .
Fig. 5. Turn ten of the west track at Thunderhill Raceway Park.This hairpin corner will be used in the simulations detailed below as well as the experiments in Section V.

Fig. 11 .
Fig.11.Driver inputs vs ADAS commands for skilled driving behavior.Notice the commands output by the ADAS system follow the driver's commands closely while the driver operates the vehicle safely.

Fig. 12 .
Fig.12.Lateral tire forces for skilled driving behavior.Even though the driver approaches the modeled tire limits, the system does not intervene, because their commands result in a safe trajectory for the vehicle.