Given the fact that self-driving cars (or autonomous vehicles (AV)) will not immediately replace all human-driven cars, they will have to share roads with human drivers for a long time. In the area of autonomous vehicle control and planning [gu2015tunable, Ames2014, luo2016distributed], safety is always the primary focus. The safety requirement becomes more strict as the scenario complexity increases. Ramp merging is a typical scenario where autonomous vehicles interact with human-driven cars. Human drivers introduce uncertainty into merge scenarios, and AV will collide if they can’t plan and be controlled safely.
Current control methods are not able to guarantee safety and efficiency at the same time. Traditional Automated Cruise Control (ACC)-like distance control methods force the vehicle to brake when distance is less than the specified minimum safety distance, while maintaining the vehicle at a desired driving speed. However, the National Highway Traffic Safety Administration (NHTSA) has reported that these kinds of methods sometimes lead to abrupt or aggressive behavior in response to a lead vehicle’s velocity change, which can be very dangerous, especially in the highway ramp merging scenario [nhtsa-2015]. In addition, vehicle control with ACC in the real world can be difficult, since sometimes its objectives can conflict with each other . NHTSA categorizes these methods as convenience features, rather than safety features [nhtsa-2015].
CBF-based methods [7782377, inproceedings, luo2020multi, 9196757, zeng2020safetycritical, 9029446, 7040372, 7857061, 8917473] have become increasingly popular in the control application domain due to their forward-invariant property, which can provide a safety guarantee. However, the forward-invariant property relies on the solution feasibility, i.e., as long as a control solution satisfying the CBF constraints can be found at each time step, then the safety for future time steps can always be guaranteed. In reality, we may not always find such a solution and alternative solutions include switching to a full braking mode [7857061, celi2019deconfliction]. However, this could lead to serious consequences in autonomous driving scenarios, e.g., when there is another vehicle behind the ego vehicle, or it is too late for the AVs to stop. Therefore, systematic solution feasibility consideration becomes critical for CBF applications, particularly in autonomous driving that involves complicated safety objectives in the presence of varied driving behaviors.
The scenario studied here is the highway on-ramp merging problem, where the goal is to enable the AV to merge with human-driven cars safely and efficiently. We aim to achieve a multiple-vehicle adaptive merging strategy with safety assurance. Our main contributions are: 1) a bi-level optimization-based control framework with chance-constrained CBF constraints to enable AVs to achieve consistent safe ramp merging behavior; 2) a novel extension of CBF-based safe control constraints to a probabilistic setting for stochastic system dynamics with formally provable safety guarantee; 3) a theoretical analysis to discuss solution feasibility guarantee and design factors reflecting different vehicle behaviors.
Ii Related Work
In the area of behavior planning and control of autonomous driving, learning-based methods are widely used. Dong et al. [Dong2017:PGM] proposed a Probabilistic Graphical Model-based method to help the ego vehicle decide whether to yield or not in ramp merging, followed by distance-keeping control. Chen [Chen-2018-106042]
presents a framework to learn a maneuver controller for autonomous vehicles, which uses LSTM-based methods and deep reinforcement learning for behavior prediction and learning. Nishitani et al. introduced a vehicle controller using deep reinforcement learning to improve the merging efficiency while tracking the expected vehicle speed. However, learning-based methods cannot provide a provably correct safety guarantee.
The CBF-based method was initially proposed by Wieland and Allower [inproceedings] in 2007 to describe an admissible control space that renders forward invariance of a safe set. Aaron et al.  extended CBF to a minimally restrictive setting and applied it to the ACC and lane keeping problem. CBF-related methods have recently become increasingly popular in different control scenarios. Notomista et al. , Zeng et al. [zeng2020safetycritical] and Son et al.  proposed CBF-based control methods specifically for the two-car racing scenario. Due to the special property of the problem setup, conservativeness is minimized in control effort in order to attain the strongest racing performance. Therefore, as discussed in , the methods are not applicable to everyday traffic scenarios, where balance between aggressiveness and conservativeness needs to be maintained for the sake of safety and efficiency.
The validity of using CBF-based methods to achieve safe control has been proved by , [zeng2020safetycritical], ,  and Ames et al.  in their work. However, none of them gave an explicit quantitative analysis of the solution feasibility condition.  mentions that the solution feasibility can be guaranteed by assuming that, in the worst case, making all robots decelerate to zero velocity immediately at the next time step can always prevent collision, and therefore the feasible solution space will always be non-empty. However, a more principled scheme with explicit theoretical grounding is desirable to automatically decide whether the vehicle needs to apply full braking before it is too late. Xiao et al. [xiaocdc2020]
used machine learning techniques to identify the feasibility of deterministic CBF constraints under fixed parameterization. In this paper, we consider adaptive parameterization of chance-constrained probabilistic CBF constraints to identify as well as modify the feasibility to achieve desired merging behaviors.
To address model uncertainty, several works ([luo2020multi],  and [nikolay2020l4dc]
) proposed to employ the CBF approach with noisy system dynamics. However, their works either assumed that the uncertainty is bounded, which could limit the probability distribution, or could not provide a general consistent solution feasibility guarantee. On the other hand, integrating CBF with Model Predictive Control (MPC) is also a common planning method. Zeng et al.[zeng2020safetycritical] and Son et al.  introduced MPC-based safety-critical control. However, these works fail to take different driving behavior styles into account and are not able to consider various safe driving strategies. In this paper, we propose a bi-level optimization-based control framework with assured safety to account for different driving styles and a feasibility guarantee.
Iii-a Background on Control Barrier Functions
A Control Barrier Functions (CBF) [ames2019control] is used to define an admissible control space for safety assurance of dynamical systems. One of its important properties is its forward-invariance guarantee of a desired safety set. Consider the following nonlinear system in control affine form:
where and are the system state and control input with and assumed to be locally Lipschitz continuous. A desired safety set can be denoted by the following safety function:
Thus the control barrier function for the system to remain in the safety set can be defined as follows [ames2019control]:
where with as the Lie derivatives of
along the vector fieldsand . Similar to , in this paper we use the particular choice of extended class function with the form as where is a CBF design parameter controlling system behaviors near the boundary of . Hence, the admissible control space in Eq. 3 can be redefined as
It is proved in [ames2019control] that any controller will render the safe state set forward-invariant, i.e., if the system (1) starts inside the set with , then it implies for all under controller .
In this paper, we consider the particular choice of pairwise vehicle safety function , safety set , and admissible safe control space as follows.
where are the positions of ego vehicle and each merging vehicle with as the minimum allowed safety distance between pairwise vehicles.
Iii-B Problem Statement
In this section, the problem formulation in the ramp merging scenario is introduced (Fig. 1). The goal is to control the ego vehicle (host vehicle) on the main road to merge safely with the human-driven vehicles (merge cars) on the ramp with motion uncertainty. The system dynamics of a vehicle can be described by double integrators as follows, since acceleration plays a key role in the safety considerations.
where are the position and linear velocity of each car respectively and represents the acceleration control input. is a random Gaussian variable with known mean
and variance, representing the uncertainty in each vehicle’s motion. We assume the human-driven merging vehicle’s velocity and the motion uncertainty distribution of are known to the ego vehicle per time step with . To create a nonlinear car-like vehicle model from the double integrators formulation, we can use a kinematics mapping method similar to that in [wang2019game], which extends single integrator dynamics to car-like robots.
Iii-B1 Quadratic Programming Problem Formulation
While performing safe merging with human vehicles, the ego vehicle is expected to maintain task efficiency, passing the merging point as fast as possible. Therefore, the objective function can be formulated as a quadratic programming problem for the ego vehicle with the control input .
where is the nominal expected acceleration for the ego vehicle to follow, and and are the ego vehicle’s maximum and minimum allowed acceleration. We assume is computed by a higher-level planner, for example, a behavior planner. is the minimum allowed distance between two vehicles to avoid collision for safety. Different from most existing CBF work with deterministic perfect model information [7857061, celi2019deconfliction], the stochastic model in Eq. 6 leads to infinite support of and hence we consider the chance-constrained optimization problem to accommodate uncertainty with as the desired confidence of probabilistic safety.
denotes the probability of a condition to be true. We employ the chance constraints over vehicle controllerto ensure the resulting lower-bounded probability of vehicles being collision-free. This is due to the fact that given the forward invariance set theory in a deterministic setting in Eq. 4: as proved in [ames2019control].
Iii-B2 Bi-level Optimization Problem Formulation
In most prior CBF work, the CBF design parameter in Eq. 4 that determines a particular safe behavior of the robots is often pre-defined and remains fixed, e.g. indicates overly restrictive robot motions for a non-decreasing while a larger could yield a more permissive control space. However, a fixed could make Eq. 7 not solvable under certain circumstances , i.e., an empty set of , and hence no longer ensure forward-invariant safety. One of the main contributions of our work is formulating the original problem Eq. 7 as the following bi-level optimization process with two layers: one for optimization over , and the other one for optimization over for feasibility guarantee.
where is the feasible set for that will be proved to ensure solution feasibility of . is a nominal value from the user to specify the desired conservativeness of the safe behavior. The feasible set changes over time and to ensure solution feasibility of , the goal is to ensure the set is consistently non-empty so that we can always find an that causes a nonempty set of (if it exists) to satisfy the safety constraint. Details will be covered in the following sections.
Iii-C Active and Feasible Condition of CBF in a Probabilistic Setting
This section will first present a novel approach to CBF with probabilistic safety consideration under uncertainty and discuss the feasibility analysis with the CBF constraints.
where for ego vehicle and each merging vehicle . To tackle the probabilistic version with Pr, we consider Eq. 10
as a chance constraint regarding the Gaussian random variable. From [blackmore2011chance], a general chance constraint problem takes the following form for an inequality and can be transformed to a deterministic constraint as
where10 into the form of Eq. 11 with and eventually get:
where is a time unit and we derived control constraints for pairwise chance-constrained safety between ego vehicle and each merging vehicle . This concludes the proof. ∎∎
Next, we will discuss the feasibility analysis of Eq. 8 with safety control constraints in Eq. 9 and bounded control constraints. In particular, we will present how to ensure non-emptiness of set for non-emptiness of set that preserves the forward-invariant safety. Given Eq. 13, the set feasibility analysis is decomposed into two situations based on the positiveness of . The active and feasible conditions of CBF depend on the overlap set between the CBF solution set and the bounded control constraints shown in Fig. 2. To simplify the discussion, here we assume is determined by the ego vehicle’s linear acceleration along the ramp and the rotation matrix by the road geometry. Thus we reformulate Eq. 9 by and redefine .
Case 1: . We have , which provides an upper bound for , meaning the acceleration cannot exceed . While the value of is decreasing and all the other terms remain the same, is also decreasing. Imagine as the red vertical dashed line shown in Fig. 2, moving from right to left on the 1-D axis of . The feasible space resulting from this CBF constraint is anywhere on the left-hand side of the dashed line. The blue intervals represent the feasible solution set while the dashed line is moving. When is very large, the dashed line is on the right-hand side of the maximum allowed acceleration . Then the space between and is feasible, since both the points fall on the left-hand side of the dashed line. The situation changes at a certain point, where overlaps with , and CBF is activated. The dashed line will keep moving to the left until it overlaps with , where infeasibility is about to happen due to the empty overlap interval of the two constraints.
Case 2: . We have , which provides a lower bound for , meaning the acceleration must be greater than . While the value of is decreasing and all the other terms remain the same, is increasing given the fact . Again, imagine as the red vertical dashed line moves from left to right on the 1-D axis of . The feasible space resulting from this CBF constraint is anywhere on the right-hand side of the dashed line. When is very large, the dashed line is on the left-hand side of minimum allowed acceleration . Then the space between and is feasible, since both the points fall on the left-hand side of the dashed line. The situation changes at a certain point, where overlaps with . From this point, the CBF constraint becomes active in optimization while keeps decreasing. The dashed line will keep moving to the right until it overlaps with , where infeasibility is about to happen due to the empty overlap interval of the two constraints.
In both cases, after the feasible solution set is determined by updated information on , the optimal control input will be chosen based on the objective function definition. Here, the optimal solution is the closest point from in the feasible solution set.
In conclusion, the boundary conditions of the CBF are:
Generally, the larger is, the more admissive action space the vehicle will have. To better understand the boundary conditions we derived, intuitively, when the ego vehicle approaches the merging vehicle from behind, the CBF constraint will not be activated until it accelerates and the distance between the two vehicles decreases rapidly. When the ego vehicle drives in front of the merging vehicle, the CBF constraint is activated if the relative speed between the two vehicles decreases rapidly, leading to shorter relative distance. The ego vehicle must maintain at least a certain velocity to prevent the distance from continuing to decrease. The boundary conditions can be seen as a kind of manipulation of the original kinematics constraint, in the form of shrink and shift.
Iii-D Consistent Solution Feasibility Guarantee
In the previous section, the relationship between and the solution feasibility was analyzed, and explicit feasible conditions on were given. Here, a Safe Adaptive Algorithm (Algorithm 1) is introduced for guaranteed solution feasibility. For time steps 1 to N, at each time step , is calculated through the first-layer optimization. Then given states of both vehicles, is calculated to ensure the feasible solution set is non-empty at . The second-layer optimization is performed and is updated at each iteration. The advantage of this algorithm, compared to fixing the value, is that it provides a dynamic solution feasibility guarantee at run time.
The problem can still be infeasible with our proposed method if the initial conditions make it impossible to ensure safety, e.g. the ego vehicle is driving too fast, and it’s already too late to avoid collision, and no matter what we choose, Eq. 4 can never be satisfied. However, the proposed method does guarantee solution feasibility as long as such a solution exists.
Iv Experiment & Discussion
Iv-a Validity test
To prove that the proposed method is valid, we conduct experiments against one merging vehicle, with randomly generated ego vehicle initial conditions, including position and velocity and desired driving strategy , and observe the resulting collision rate. The results are shown in Fig. 3. The black dashed line stands for the minimum allowed safety distance , which is set to be 8m. The confidence level is set to be 99%. From Fig. 3, it is observed that all 400 trials keep the minimum distance as required, and the collision rate is 0%. The two different kinds of curve shapes correspond to two merging results: asymptotically approaching indicates merging after the merging vehicle and increasing Euclidean distance indicates merging in front of the merging vehicle. In conclusion, no matter what the ego vehicle initial conditions are, and whatever driving strategy the ego vehicle takes, the proposed method can always ensure safety.
To better illustrate the advantage of the proposed method, a comparison with traditional CBF with fixed is made, as shown in Fig. IV-A. The proposed method updates in the green zone, while traditional CBF does not, which leads to solution infeasibility from to and violation of the minimum safety distance requirement. The proposed method maintains solution feasibility consistently and performs the merging safely.
Iv-B Vehicle behavior factors
Iv-B1 Effect of the CBF parameter
The choice of the CBF parameter is a key factor in shaping a vehicle’s behavior. As mentioned in Section III, the larger is, the more admissive action space the ego vehicle will have.
To verify this statement, ego vehicle behaviors with different values are compared. For better visualization effect, we observe ego vehicle merging control with a single merging vehicle, while the initial conditions of the ego vehicle and the profile of the merging vehicle are kept the same to make the comparison fair. The minimum allowed safety distance is set to 8 m. The result is shown in Fig. 5 and Fig. 6. We observe that the smaller is, the more conservative the driving strategy of the ego vehicle will be, which leaves more tightly bounded action space.
If we take a closer look at Eq. 13, we find that given the same states of both vehicles at time step , decreasing the value of results in a smaller matrix . Regardless of whether is positive or negative, provides a narrower solution space for . This agrees with our observations. Before (Fig. 5) and (Fig. 6), all trials share exactly the same states, meaning the CBF is not active yet, and therefore the value of does not make any difference in the ego vehicle’s behavior. After those times, the ego vehicle decides to decelerate or accelerate at a certain point. The smaller is, the earlier the deceleration or acceleration decision is made to avoid collision in future steps. In other words, the larger is, the more aggressive the driving strategy is for the ego vehicle, and the deceleration as a precautionary action is more and more delayed. This finding also agrees with the experiment results from .
Iv-B2 Effect of initial conditions
The initial conditions also affect the merging behavior of the ego vehicle. Consider the merging control with two merging vehicles, shown in Fig. 7. From the high-level decision-making standpoint, the ego vehicle has three merging options: merging in front of , merging in between and , and merging behind . Since the actions of the merging vehicles are not controllable by the ego vehicle, whether the ego vehicle can freely choose to merge into any of the three slots depends on the initial conditions, including relative distance and speed, as well as the nominal acceleration set for the ego vehicle to follow and the minimum allowed safety distance .
Intuitively, if is set to be very large, the ego vehicle has to keep far enough from both of the merging vehicles, and that makes it difficult for it to squeeze into the gap between the two merging vehicles without breaking the minimum safety distance requirement.
The relative distance and relative speed also matter. Together with , they decide the reachability set for the ego vehicle, which is the set of positions the ego vehicle can achieve. We take a look at two specific cases for detailed illustration. The experimental results are shown in Fig. 8. As the initial condition, . The minimum allowed safety distance is set to 5 m. The ego vehicle’s initial positions for the two cases are shown in Fig. 7.
In case 1, . While performing conservative merging, with = 1, the ego vehicle merges in between and safely. The ego vehicle keeps the distance to both of the merging vehicles without much change. While performing aggressive merging, with , the ego vehicle merges in front of safely. The ego vehicle accelerates obviously and completes the merging around , and the distance to both merging vehicles increases after that.
In case 2, . While performing conservative merging, with = 1, the ego vehicle merges after safely. The ego vehicle accelerates while maintaining the required safety distance. While performing aggressive merging, with , the ego vehicle merges in between the two merging vehicles safely. The ego vehicle accelerates to pass and merges in between and . It keeps approaching until it decreases its acceleration to meet the future safety guarantee.
We present a novel adaptive merging control algorithm for autonomous driving vehicles in highway scenarios with probabilistic safety guarantee. Simulations with different conditions are used to demonstrate the power of CBF in applying different driving strategies to the ego vehicle via a single parameter . The problem is uniquely formulated as a chance-constrained Control Barrier Function-based bi-level optimization, which provides a theoretically consistent solution feasibility analysis with explicit bounds on the CBF parameter , and thus makes the future use and applications of CBF more flexible without concerns about solution infeasibility under certain circumstances. The proposed feasibility-guaranteed CBF-based method not only works in the single highway merging problem, but presents a paradigm for application to other problems, along with formulation and analysis guidelines. In future work, we plan to combine the proposed framework with learning-based methods that use real-world datasets to realize safe control with a data-driven approach to determine the appropriate strategy.