Safe Adaptive Cruise Control with Road Grade Preview and V2V Communication
Abstract
We present the design of a safe Adaptive Cruise Control (ACC) which uses road grade and lead vehicle motion preview. The ACC controller is designed by using a Model Predictive Control (MPC) framework to optimize comfort, safety, energyefficiency and speed tracking accuracy. Safety is achieved by computing a robust invariant terminal set. The paper presents a novel approach to compute such set which is less conservative than existing methods. The proposed controller ensures safe intervehicle spacing at all times despite changes in the road grade and uncertainty in the predicted motion of the lead vehicle. Simulation results compare the proposed controller with a controller that does not incorporate prior grade knowledge on two scenarios including carfollowing and autonomous intersection crossing. The results demonstrate the effectiveness of the proposed control algorithm.
I Introduction
Recent advances in autonomous driving have accelerated the need for high performance and reliable Advanced Driving Assistance Systems (ADAS) which guarantee safety and comfort in various driving conditions. At the same time, connected car technologies begin to make their first appearance in the passenger vehicle market. Connected and automated vehicles will enhance mobility and safety by integrating autonomous driving with communication technologies. Connectivity enables ADAS to leverage VehicletoVehicle (V2V) and VehicletoInfrastructure (V2I) communication to further improve performance [GUANETTI201818].
Adaptive cruise control (ACC) is a widely used ADAS module that controls the vehicle longitudinal dynamics. ACC is triggered once a preceding vehicle is detected within a certain distance range from the ego vehicle. It automatically adjusts the vehicle acceleration and deceleration to maintain a proper minimum safe distance from the vehicles ahead. ACC enhances mobility, improves safety and comfort, and reduces energy consumption. The design of ACC based on Model Predictive Control (MPC) is common in the literature. For example, in [Luo2010, article, 4436187, 7035071, 7963297, 8002647], the authors present ACC design using MPC based on one or more particular performance criteria including traffic flow, energy efficiency and safety or comfort considerations.
Conventional adaptive cruise control systems operate in two modes: ACC and Cruise Control (CC), depending on the presence or absence of the lead vehicle in detection range of the ego vehicle. In ACC mode the objective is to maintain a safe distance from the lead car, whereas in CC mode the objective is to track the reference velocity set by the driver or the maximum speed limit of the road. In the literature, ACC and CC modes are also referred to as distance tracking and velocity tracking modes, respectively. Discrete switching between the modes may result in aggressive control action or repetitive mode change. In [SHAKOURI201112964, ZHENHAI2016581], the authors suggest more sophisticated rulebased switching strategies to prevent chattering caused by switching. We formulate and design our proposed controller without employing any switch in order to adapt the velocity and distance automatically and smoothly, if a lead car is detected.
In conventional ACC systems, the desired intervehicle gap can be specified as a constant space or time gap between the vehicles. The desired gap also might be defined via a constant time headway policy which relates the intervehicle desired distance to the current velocity of the ego vehicle as , where is the static gap or the distance between the cars when they are at standstill and is the dynamic gap which changes with velocity. In general these simple approaches do not guarantee safety. To guarantee safety at all times, the theory of invariant sets can be used to compute the safe distance. In [ALAM201433], reachability analysis with level set method is proposed to compute the safe set for heavy duty platooning. The authors of [7336573] use a kinematic model to describe the system as a linear timeinvariant (LTI) system and apply backward reachable set analysis to calculate a polytopic control invariant set for ACC. In [turri2017cooperative], a control invariant set is presented as the safe set for platooning by considering the lower and upper bounds on the acceleration. The aforementioned studies compute the control invariant set by bounding the unknown parameters such as road slope or the front car’s motion. However, computing the control invariant safe set by considering the lower and upper bounds on the unknown parameters can lead to an overly conservative safe set and thus an undesirable large gap between the vehicles. In the proposed study, instead of bounding these parameters, we assume the availability of road grade preview from a high fidelity map and the availability of lead car’s future state trajectory from a V2V communication device. Employing this parameters for computing the control invariant safe set, instead of their boundary values, yields a less conservative safe set and consequently a closer intervehicle safe distance.
Road grade can considerably affect the ACC controller performance. The work in [8002647] considers an ACC which takes into account road elevation data to improve energy efficiency. In [javad] realtime estimation of road grade and vehicle mass are utilized to improve comfort. In this study, we investigate how the ACC controller performance improves in terms of safety, comfort, tracking accuracy and energy efficiency, by exploiting the road grade knowledge.
The contribution of this paper can be summarized as follows:

We propose the design of an ACC controller which incorporates the road grade preview information using a prior grade map of the road to predict the vehicles’ longitudinal dynamics. Furthermore, the proposed controller is formulated to switch automatically and smoothly between distance and velocity tracking.

A numerical approach to compute a control invariant set which makes use of the road grade preview and the future state trajectory of the leading car, transmitted through V2V communication, is presented. This set is incorporated as terminal constraint of the ACC controller to guarantee safety.

We demonstrate through two example scenarios that the proposed approach is more efficient compared to the controller that does not use grade preview.
The rest of the paper is structured as follows: Section II introduces some preliminaries about vehicle model and road grade map generation. Section III describes the problem statement and the MPC formulation. Section IV explains the design of control invariant set. Section V describes carfollowing and autonomous intersection passing scenarios as two example applications of the proposed approach. Section VI illustrates the simulation results and finally Section VII makes the concluding remarks.
Ii Preliminaries
Iia Vehicle Model
The vehicle is modeled as a point mass moving along a road. The system state at time t is
where and are the vehicle position and velocity, respectively. The longitudinal motion of the vehicle can be described by the following equations
(1)  
where and are traction and braking forces, respectively and is the total longitudinal force. The aerodynamic drag is determined by vehicle speed , air density , air drag coefficient and frontal area .
(2) 
The rolling resistance is defined as
(3) 
where is gravitational force and is rolling friction coefficient. The gravity force due to road slope can be expressed as
(4) 
The system (1) is discretized using Euler method with constant sampling time interval .
(5)  
The parameters of model (5) are then estimated by nonlinear least squares from data collected on a test vehicle, at various speeds and accelerations. All the data are collected in a proving ground on level roads with homogeneous surfaces, i.e. and is constant for the purpose of model identification. Fig. 1 shows the fit of the identified model to the experimental data, while Table I summarizes the identified model parameters. Specifically, we use 7 datasets for a total duration of about 16 minutes; the predicted data in Fig. 1 are generated simulating model (5) with the identified parameters and the measured input torque from the initial condition to the end of the dataset.
vehicle mass  kg  2278  

vehicle frontal surface  m  2.63  
air density  kg/m  1.206  
vehicle drag coefficient    0.2791  
vehicle roll coefficient    0.0089  
sampling time  s  0.5 
IiB Road Grade Map Generation
Road topology data is a valuable source of information in autonomous driving applications. On roads with updown slopes, ADAS systems like ACC benefit from accurate priorknown road grade. ACC can exploit the grade preview extracted from a map to ensure keeping a safe distance from the lead vehicle. Different methods for generating a highresolution grade map of the road have been discussed in our previous work [04167]. The road grade profile can be obtained using a survey vehicle equipped with highprecision GPS system. An alternative approach is using APIs like Google Elevation API. Google Elevation API provides elevation data for all locations on earth surface. Elevation data can be queried for specific coordinates to generate road elevation profile. In this study, we utilize Google Elevation API to generate the road grade profile.
Iii Problem definition and formulation
The goal is to design an ACC and CC controller with safety guarantees. The proposed ACC/CC controller is formulated as Model Predictive Control (MPC) which repeatedly solves the following constrained finite horizon nonlinear optimization problem:
(6a)  
(6b)  
(6c)  
(6d)  
subject to  
(6e)  
(6f)  
(6g)  
(6h)  
(6i)  
(6j)  
where and are the state and control input at step predicted at time , respectively. The MPC horizon is and denotes the sequence of control inputs . The multiobjective quadratic cost function represents the tradeoff between minimizing reference tracking error (6a), control effort (6b) and jerk (6c) and , and are their corresponding weight factors, respectively. The terminal cost (6d) is weighted by . The state and input are constrained as (6f) and (6g) to lie within their lower and upper bounds denoted as , and , , respectively. The reference velocity is the desired velocity set for the ego vehicle by cruise control. The vehicle longitudinal dynamics (5), denoted as , introduce nonlinear constraints, parameterized by the road grade . Since the road grade map is available as positiondependent data, the controller has to convert it from spatial domain to time domain to make use of the grade preview. To do so, the ego car’s velocity is assumed to remain constant over the MPC horizon and grade data corresponding to the predicted positions is extracted. The lead car’s velocity and position at step predicted at time are denoted as and , respectively. The predicted future trajectory of the lead car is transmitted through V2V communication. The safe distance between the two vehicles, denoted as , is obtained by calculating the control invariant set, described in Section IV. is a function of the position and velocity of the ego and lead vehicles, and of the road grade from the current time till , which is the time at which the lead car comes to full stop if it exert its maximum braking force. The optimal solution of the problem (6) is
and the receding horizon control law is obtained by applying the first control input
(7) 
The above formulation includes switching behavior from distance tracking (ACC) mode to reference velocity tracking (CC) mode implicitly. In other words, there is no explicit term for discrete switching between the two modes, but the controller is designed to operate in ACC mode as long as the front vehicle is detected and automatically and smoothly adapts the velocity to CC reference once there is no vehicle in the front.
The multiobjective cost in problem (6) simultaneously fulfills multiple performance criteria. To assess the performance of the controller we introduce a performance index for each objective. The first term of the cost function (6a) denotes reference tracking error and tracking performance index can be defined as
(8) 
where is the total time of simulation or experiment, is the ego vehicle velocity at time obtained as the closedloop state of system (5) controlled with described in (7). The second term of the cost (6b) penalizes the control effort and presents energy consumption criteria. We introduce the energy consumption performance index as
(9) 
assuming no penalty for braking. The third term in the cost (6c) minimizes the change of acceleration (jerk) which is a performance criteria for longitudinal ride comfort. The comfort performance index is
(10) 
In both (9) and (10), is the closedloop control input described in (7).
Iv Control Invariant Set
The adaptive cruise control is considered to be safe if the follower vehicle can avoid collisions with the front vehicle regardless of the front vehicle action. Consider the combined model of the front and follower vehicles in Adaptive Cruise Control (ACC) problem,
(11) 
, , , and denote the ego vehicle tractive force, braking force, aerodynamic drag force, rolling resistance force, and gravity force respectively defined in (2)(4). and are the ego and lead vehicle masses, respectively. and are the ego vehicle velocity and position. and are the lead vehicle velocity and position. In this system, the ego (follower) vehicle braking/tractive force is the control input and the lead (front) vehicle’s total longitudinal force denoted as is treated as disturbance to the system (11) and described based on (1). The set of all admissible states for this system are defined as follows:
where is the minimum required distance between the mass center of the two vehicles. We define the robust control invariant set as a set with the following property:
The set is a function of the road slope at time until the time at which the front car comes to full stop after exerting the brake at time . Therefore, . denotes the state of the system (11) at time , is the input feasible set (6g), represents the system dynamics (11). is the set of all possible longitudinal forces of the lead vehicle. Hence, the robust control invariant set for the above ACC system is such that for any maneuver of the front vehicle, there is a control signal that keeps the system (11) within for all future times [borrelli2017predictive].
A conservative estimation of the closed form of for this problem is presented in [turri2017cooperative] that computes the control invariant set by finding the bounds on the road grade and calculating the bounds on accelerations of the lead car. In this work a numerical method is employed to compute the boundary of the safe set more accurately using the road slope preview and future trajectory of the lead car. By using such preview information, we calculate the exact maneuver of the front car by assuming it exerts maximum braking force at each time step, using a twostep approach.

In the first step, the equations of motion of the front vehicle are integrated using forward Euler discretization, assuming the front vehicle applies its maximum braking force, ,
(12a) (12b) where the initial condition for (12) and (12b) is the current velocity and position of the front vehicle. In (12) the road grade depends on the position of the front vehicle at each time instant. is the integration interval. The integration stops when the front vehicle reaches zero velocity, . Define the position when the front vehicle reaches as .

In the second step, the equations of motion of the following vehicle are integrated backwards in time, assuming that the following vehicle is also applying its maximum braking force, , with the initial condition of and . The integration continues till the velocity reaches its upper bound, .
(13a) (13b)
This method, although computationally more expensive compared to the closed form, takes into account the exact road grade profile. The minimum safe distance at each step time of the above integration is computed as follows,
(14) 
Note (12b) and (13b) require the backward and forward simulation of absolute vehicle positions. One can rewrite the dynamics to highlight that only is important, as expressed in (11).
Fig. 2 shows examples of the safe set boundary for various velocity values of the front car. The safe set boundary is obtained by fitting a seconddegree polynomial on data points calculated using the above twostep approach. Sufficiently small integration interval increases the data points and results in achieving higher fitting accuracy. The polynomial as the function of ego vehicle velocity defines the required minimum safe distance which is imposed as safety constraint (6j) in the MPC formulation (6). Also Fig. 3 demonstrates the safe time gap plot corresponding to the Fig. 2 safe distances. The time gap is computed for the cases that ego and lead vehicles’ velocity are the same.
To guarantee feasibility of problem (6) at all times, persistent feasibility should be proven by showing the existence of a feasible control sequence at all times when starting from a feasible initial point. Assume that at time a lead vehicle be in front of the follower vehicle and the problem (6) be feasible. Let be the state of the system (1) in closedloop with the MPC controller (6) at . Since the problem is feasible at , there exist an optimal control sequence at . Apply and let the system evolve to . At , apply at the end of the MPC horizon. The control sequence , since is input feasible and state feasible. In fact, from the construction of the safe set, by applying at step will guarantee that will be at a safe distance . In conclusion, the closedloop system is persistently feasible.
V Example Application Scenarios
The application of an ACC controller that adapts the vehicle’s longitudinal velocity based on the other vehicle’s states, communicated over V2V network, is not restricted to the carfollowing scenario. The same formulation of ACC can be extended further to other applications like autonomous intersection crossing. In an uncontrolled intersection, two cars that simultaneously approach an intersection at crossing directions, communicate with each other and adapt their speed to avoid collision and pass the intersection safely and efficiently [RiosTorres2017a]. A possible approach to coordinate the vehicles in an autonomous intersection is to define a circle centered at the center of the intersection and with radius of the range of communication , as shown in Fig. 4(a). Once the cars enter this virtual circle, they communicate with each other and assign the priority to each other based on their current velocity. Priority assignment for autonomous intersection is not the focus of this work and has been discussed in [RiosTorres2017a]. We assume we already know which car is prioritized to pass through the intersection first and play the role of the leader for the other car. The car with lower priority is the one that runs the ACC controller and adapts its velocity and distance to the intersection corresponding to the lead car. At each time step we project the lead car in the crossing direction in front of the ego car, as shown in Fig. 4(b), as a virtual car that the ego vehicle follows.
Vi Simulation Results and Discussion
We modeled and solved the optimization problem (6) using YALMIP [YALMIP] and IPOPT. The vehicle model and MPC parameters are presented in Table I and II, respectively. To conduct realistic simulations, we drove a car on roads with significant slope (located near UC Berkeley) and collected latitude, longitude and velocity data. We generated the grade maps of the roads by querying the Google Elevation API, and obtaining elevation data for the collected latitudes and longitudes. The recorded velocity data of the car is considered as the front car’s velocity in the simulations. We ran simulation tests with different real roads grade profiles and assessed the performance of the proposed approach against the baseline approach. Our proposed ACC considers the road grade preview in both planning and safe set calculation, while the baseline controller does not include knowledge of grade.
minimum velocity  m/s  0  
maximum velocity  m/s  30  
minimum control input  kN  3  
maximum control input  kN  3  
sampling time  s  0.2  
tracking error cost weight    10  
control effort cost weight    1  
jerk cost weight    10  
terminal cost weight    100 
Fig. 5 represents the results for a carfollowing scenario. The first graph compares the velocity of the front and the ego car; the tracking accuracy with and without knowledge of grade is essentially the same. The second graph depicts the relative distance between the ego and front vehicles, with and without grade knowledge. The safe distance in the graph represents the minimum required distance between the vehicles calculated by the robust control invariant set. The relative distance obtained by the proposed controller is lowerbounded by the safe distance. However, the relative distance obtained by the baseline controller (that has no knowledge of the road grade) is not safe, since the relative distance violates the minimum required safe distance. The third graph represents the control input . As seen, the control input obtained by the proposed controller is much smoother compared to the baseline controller. The fourth graph depicts the road grade profile obtained by Google Elevation API.
To assess the performance of both controllers quantitatively, we used the performance indexes introduced in Section III. Table III compares, for various simulation runs, the energy consumption, tracking and comfort performance indexes when the road grade preview is either known or not known. The total cost, which is the sum of all performance indexes, is also reported; the average total cost without grade knowledge is considerably higher than with grade preview.
Road Segment  Energy Consumption  Tracking  Comfort  Total Cost  

W/ grade  W/O grade  W/ grade  W/O grade  W/ grade  W/O grade  W/ grade  W/O grade  
1  311.9  324.3  68.5  127.2  49.0  50.0  430.3  501.5 
2  615.1  625.2  47.1  64.2  60.9  86.5  723.1  775.9 
3  0  0.81  28.7  30.9  30.8  189.7  59.6  221.4 
4  0  0  45.0  37.0  52.3  205.6  97.3  242.6 
5  0  0.11  32.6  30.6  36.2  286.4  68.8  317.1 
6  149.1  158.0  70.2  106.0  109.8  175.0  329.1  439 
For the autonomous intersection scenario the results are similar to the carfollowing case, and are not reported here for brevity. We just note that in this scenario, the road grade profiles for the leader and follower cars are different; at each time step the vehicles’ distances to the center of intersection are measured and their relative distance is calculated by projecting the lead car in front of the follower car.
Both scenarios (carfollowing and autonomous intersection crossing) are set up in the PreScan simulation environment.
PreScan has an interface with MATLAB/Simulink and is a suitable platform for developing ADAS systems as well as modeling V2V communication.
We also modeled the road grade for both scenarios in PreScan environment with sinusoidally varying profiles.
The lead vehicle velocity is also generated as another sinusoidal profile.
The PreScan video for both scenarios is available online.
As previously described, the proposed controller is capable of automatically, safely and smoothly switching between CC and ACC modes. Fig. 6 shows the results for this case. The front car’s velocity is obtained by collecting real driving data and the associated road grade map is created using Google Elevation API. The cruise control speed is set to a constant value. By comparing the velocity and distance plots, we can see that the ego car velocity tracks the cruise control reference velocity when the front car is far, for example between the time of to . Afterwards, for example between the time to , since the front car’s velocity is less than the cruise control reference speed, and the distance between the cars is closer, the ego car tracks the front cars’ velocity instead. The relative distance shown in the second plot is also lowerbounded by the safe distance.
Vii Conclusion
We designed a performanceenhanced ACC controller that exploits preview information about road grade as well as lead vehicle’s motion to predict the evolution of the system and plan accordingly. To guarantee recursive feasibility of the closedloop system, we computed a less conservative robust control invariant set with a numerical approach that computes the minimum required safe distance at each time step in accordance with the associated road grade data and the lead car states. The proposed controller is robust with respect to any aggressive braking of the lead vehicle as well as any arbitrary road slope.
We conducted simulations using realistic data of lead car’s velocity and road grade profile. We verified through simulation for two application scenarios that the proposed controller improves the performance in terms of comfort, safety and energy efficiency compared to the baseline controller. In addition, we showed that our proposed ACC design is able to switch between CC and ACC mode automatically, smoothly and safely.
Viii Acknowledgement
The information, data, or work presented herein was funded in part by the Advanced Research Projects AgencyEnergy (ARPAE), U.S. Department of Energy, under Award Number DEAR0000791. The views and opinions of authors expressed herein do not necessarily state or reflect those of the United States Government or any agency thereof.