Patent Publication Number: US-2020298859-A1

Title: Safe Autonomous Overtaking with Intention Estimation

Description:
CROSS-REFERENCES TO RELATED APPLICATIONS 
     Not Applicable. 
     STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH 
     Not Applicable. 
     BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     This invention relates to the overtaking of a lead vehicle by an autonomous ego vehicle, and in particular, to the overtaking of a lead vehicle by an autonomous ego vehicle on a two-lane road in the presence of an oncoming vehicle. 
     2. Description of the Related Art 
     Autonomous vehicles have been a very active topic of research recently due to numerous potential benefits such as driver comfort, reduced road fatalities, improved mobility, etc., and various aspects of them such as perception, real-time decision making and motion planning have been extensively studied (see, e.g., the review disclosures Ref. (1-4) for more details). In particular, safely overtaking a slower vehicle moving on a two-lane road is of interest. 
     Autonomous overtaking has been studied using various strategies in the literature. The authors in Ref. (5) use model predictive control (MPC) for generating overtaking trajectories in the presence of an oncoming vehicle, while a hierarchical planning approach including strategic-level and low-level trajectory optimization for lane changing is presented in Ref. (6). Receding horizon control has also been used to deal with lane changing problems, where the surrounding vehicles are modeled as Markov Gaussian Processes in Ref. (7), and stochastic MPC is employed to account for the uncertainty in other vehicles&#39; motion. Similarly, the approach in Ref. (8) samples independent and identically distributed (i.i.d.) prediction trajectories of other vehicles from a generic model and build multi-level optimization problems based on these independent and identically distributed (i.i.d.) scenario samples. Other approaches use multi-policy decision making for high-level lane-changing behavior, where buffered Voronoi cells were used to compute a geometric partition and then, reducing the safety control design to ensuring that the vehicles avoid the partition boundaries Ref. (9). However, these works do not consider the reaction of the other vehicles to the ego vehicle&#39;s overtaking maneuver. 
     Another set of relevant literature pertains to approaches for active model discrimination, which injects a small input signal that causes the behaviors of all models to be distinct, thus can be discriminated from each other; see Ref. (10) and Ref. (11). These approaches have been successfully adopted for intention estimation in Ref. (12), Ref. (13). In Ref. (12), an active model discrimination approach was proposed by solving an mixed-integer linear program (MILP) and applied to the problem of intention identification of other autonomous vehicle or human drivers in scenarios of intersection crossing and lane changing, while in Ref. (13), an affine abstraction-based separating input design problem was studied for a set of uncertain nonlinear models and used in an intention estimation example of a highway lane changing scenario with Dubins vehicle models. 
     Therefore, what is needed is an improved method system for the overtaking of a lead vehicle by an autonomous ego vehicle. 
     SUMMARY OF THE INVENTION 
     The present disclosure provides a method of overtaking a lead vehicle using an estimated intention of the lead vehicle. 
     In one aspect, the present disclosure provides a method in a data processing system that includes at least one processor and at least one memory. The at least one memory includes instructions executed by the at least one processor to implement a vehicle overtaking system. The method includes causing a vehicle control system in an ego vehicle to execute at least a portion of an input control sequence, receiving, from a first plurality of sensors coupled to the ego vehicle, lead vehicle data about a lead vehicle, estimating an intention of the lead vehicle based on the lead vehicle data, and causing the vehicle control system to perform a vehicle maneuver based on the intention of the lead vehicle. 
     The method may further include receiving, from a second plurality of sensors coupled to the ego vehicle, oncoming vehicle data about an oncoming vehicle, and receiving, from a third plurality of sensors coupled to the ego vehicle, ego vehicle data about the ego vehicle. 
     In the method, at least one sensor of the first plurality of sensors may belong to the second plurality of sensors and the third plurality of sensors. 
     In the method, the input control sequence may include a plurality of discrete time points and at least a portion of the ego vehicle data, lead vehicle data, and the oncoming vehicle data may be associated with one of the discrete time points. 
     In the method, the vehicle maneuver may include determining the ego vehicle is unable to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based on at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle, piloting the ego vehicle to follow behind the lead vehicle within the predetermined set of safety constraints. The predetermined set of safety constraints may include a predetermined headway separation distance between the ego vehicle and the lead vehicle. 
     In the method, the vehicle maneuver may include determining the ego vehicle is able to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based on at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle, and piloting the ego vehicle along a trajectory to overtake the lead vehicle within the predetermined set of safety constraints. The method may further include calculating a trajectory based on an ego vehicle speed, a lead vehicle speed, an ego vehicle location, and a lead vehicle location, wherein each of the ego vehicle speed, the lead vehicle speed, the ego vehicle location, and the lead vehicle location are based on at least one of the ego vehicle data and the lead vehicle data. In the method, the trajectory may include a desired location located ahead of the lead vehicle determined based on dynamics of the lead vehicle corresponding to the determined intention. In the method, the determining the ego vehicle is able to overtake the lead vehicle while keeping the ego vehicle within the predetermined set of safety constraints may be further based on at least one of an oncoming vehicle speed and an oncoming vehicle location, each of the oncoming vehicle speed and the oncoming vehicle location based on the oncoming vehicle data. 
     In the method, determining the intention of the lead vehicle may include providing the lead vehicle data to a model selection algorithm, receiving an output of the model selection algorithm, determining a most likely intention of the lead vehicle based on the output of the model selection algorithm. The lead vehicle data may include a lead vehicle speed. 
     In another aspect, the present disclosure provides a driving control system for an ego vehicle. The driving control system includes a first plurality of sensors coupled to the ego vehicle and a controller in electrical communication with the first plurality of sensors. The controller may be configured to execute a program stored in the controller to cause a vehicle control system in the ego vehicle to execute at least a portion of an input control sequence, receive, from a first plurality of sensors coupled to the ego vehicle, lead vehicle data about a lead vehicle, estimate an intention of the lead vehicle based on the lead vehicle data, and cause the vehicle control system to perform a vehicle maneuver based on the intention of the lead vehicle. 
     The system may include a second plurality of sensors coupled to the ego vehicle, and a third plurality of sensors coupled to the ego vehicle. The controller may be further configured to receive, from the second plurality of sensors coupled to the ego vehicle, oncoming vehicle data about an oncoming vehicle, and receive, from the third plurality of sensors coupled to the ego vehicle, ego vehicle data about the ego vehicle. 
     In the system, at least one sensor of the first plurality of sensors may belong to the second plurality of sensors and the third plurality of sensors. 
     In the system, the input control sequence may include a plurality of discrete time points and at least a portion of the ego vehicle data, lead vehicle data, and the oncoming vehicle data may be associated with one of the discrete time points. 
     In the system, the vehicle maneuver may include additional steps in the program and the controller may be further configured to determine if the ego vehicle is unable to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle, and pilot the ego vehicle to follow behind the lead vehicle within the predetermined set of safety constraints. The predetermined set of safety constraints may include a predetermined headway separation distance between the ego vehicle and the lead vehicle. 
     In the system, the vehicle maneuver may include additional steps in the program. The controller may be further configured to determine the ego vehicle is able to overtake the lead vehicle while keeping the ego vehicle within a predetermined set of safety constraints based on at least one of the ego vehicle data, lead vehicle data, oncoming vehicle data, or the estimated intention of the lead vehicle, and pilot the ego vehicle along a trajectory to overtake the lead vehicle within the predetermined set of safety constraints. The ego vehicle data may include an ego vehicle speed and an ego vehicle location, the lead vehicle data comprises a lead vehicle speed and a lead vehicle location, and the method may further include calculating a trajectory that is determined based on the ego vehicle speed, the lead vehicle speed, the ego vehicle location, and the lead vehicle location, and the trajectory includes a desired location located ahead of the lead vehicle determined based on dynamics of the lead vehicle corresponding to the determined intention. 
     These and other features, aspects, and advantages of the present invention will become better understood upon consideration of the following detailed description, drawings and appended claims. 
    
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         FIG. 1  shows a coordinate frame of an ego vehicle, a lead vehicle, and an oncoming vehicle. 
         FIG. 2  shows a piece-wise state-dependent constraint on an ego vehicle&#39;s lateral input. 
         FIG. 3  shows a vector field definition around a vehicle. 
         FIG. 4  shows initial conditions of an oncoming vehicle for an unsafe overtake. 
         FIG. 5A  shows a graph of weighted ∞-norm distances between an ego car, ε, and a lead car,  . 
         FIG. 5B  shows a graph of weighted ∞-norm distances between an ego car, ε, and an oncoming car, O. 
         FIG. 6A  shows 2-norm distance between an ego car, ε, and a desired point r d  ahead of a lead car,  . 
         FIG. 6B  shows an acceleration input of an ego car, ε. 
         FIG. 7  shows an exemplary embodiment of an overtaking with an intention estimation system. 
         FIG. 8  shows an exemplary embodiment of a process for implementing an overtaking with an intention estimation algorithm. 
     
    
    
     DETAILED DESCRIPTION OF THE INVENTION 
     Elements of this disclosure could provide new consumer protections by allowing a semi-autonomous or autonomous ego vehicle to more safely and effectively overtake a lead vehicle in the presence of an oncoming vehicle. 
     In this disclosure, we propose a safe, autonomous overtaking algorithm for an ego vehicle that accounts for the intentions of a lead vehicle. The intention estimation algorithm consists of two phases: (1) an offline phase where an optimal control input sequence for the ego vehicle over a finite time horizon is found to distinguish the intentions of the lead vehicle, and (2) an online phase where the computed sequence is applied to actively distinguish and find out the intention of the lead vehicle. This intention is then used to generate an overtaking trajectory for the ego vehicle, based on vector field guidance. Some contributions of this disclosure are: 
     (1) the generalization of the active model discrimination formulation to allow piece-wise state-dependent constraints on the controlled inputs, making it applicable to a more general class of affine models when compared to those in Ref. (12), Ref. (13), and 
     (2) an intent-aware, finite-time tracking controller for overtaking the lead vehicle, based on a novel vector field guidance approach that uses super-elliptic contours and safely guides the ego vehicle around the lead vehicle. The rest of the disclosure is organized as follows: The Modeling and Problem Statement section provides the mathematical modeling. The intent estimation and trajectory generation and control algorithms are discussed in the Intention Estimation of The Lead Vehicle, and Trajectory Generation and Control for the Ego Vehicle sections, respectively. Simulation results are provided below. 
     Modeling and Problem Statement 
     Notations: 
     The set of positive integers up to n is denoted by    n   + , and the set of non-negative integers up to n is denoted by    n   0 . Vectors are denoted in bold letters (r). ∥⋅∥ is the 2-norm and ∥⋅∥ ∞ , is the infinity vector norm. 
     A. Vehicle Dynamics 
     We consider a two-lane road  ⊆   2  of lane width y lane , and 3 vehicles: an ego vehicle, ε, a lead vehicle,   and an oncoming vehicle, O. All vehicles are identical in shape and are rectangular with length l c  and width w c (&lt;y lane ). The vehicles are assumed to have the following dynamics: 
     
       
         
           
             
               
                 
                   
                     
                       
                         r 
                         . 
                       
                       j 
                     
                     = 
                     
                       
                         [ 
                         
                           
                             
                               
                                 
                                   x 
                                   . 
                                 
                                 j 
                               
                             
                           
                           
                             
                               
                                 
                                   y 
                                   . 
                                 
                                 j 
                               
                             
                           
                         
                         ] 
                       
                       = 
                       
                         
                           [ 
                           
                             
                               
                                 
                                   v 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     j 
                                   
                                 
                               
                             
                             
                               
                                 
                                   v 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     j 
                                   
                                 
                               
                             
                           
                           ] 
                         
                         = 
                         
                           v 
                           j 
                         
                       
                     
                   
                   , 
                   
                     
                       
                         v 
                         . 
                       
                       
                         x 
                          
                         
                             
                         
                          
                         j 
                       
                     
                     = 
                     
                       
                         u 
                         
                           x 
                            
                           
                               
                           
                            
                           j 
                         
                       
                       - 
                       
                         
                           C 
                           d 
                         
                          
                         
                           v 
                           
                             x 
                              
                             
                                 
                             
                              
                             j 
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
           
         
       
     
     for j∈{e,l,o}. r j =[x j  y j ] T , for j∈{e,l,o}, are the position vectors of ε,   and O, respectively, in the X-Y plane with respect to (w.r.t.) a global inertial frame    g (î,ĵ,{circumflex over (k)}) fixed on the outer edge of the rightmost lane as shown in  FIG. 1 ; v xk  are longitudinal velocities; u xi  and v yi  are longitudinal acceleration inputs and lateral velocity inputs, respectively, which are bounded as: 
       | u   xj   |≤u   j   max   ,∥v   yi   |≤v   j   max ,  (2)
 
     where j={e,l,o} and C d  is the coefficient of drag. Since overtaking requires more control authority for ε, we assume a larger control bound on ε while overtaking: 
       | u   xe   |≤u   e   max,over   (3)
 
     This can be interpreted as that ε will not fully utilize its control authority in normal driving, but in overtaking, it will leverage that to accomplish the task. 
     The vehicles are modeled as rectangles with their edges aligned with the axes    g , defined as: 
     
       
         
           
             
               
                 
                   
                     ɛ 
                     = 
                     
                       { 
                       
                         
                           
                             r 
                             ∈ 
                             
                               ℝ 
                               2 
                             
                           
                            
                           
                             
                                
                               
                                 x 
                                 - 
                                 
                                   x 
                                   e 
                                 
                               
                                
                             
                             ≤ 
                             
                               
                                 l 
                                 c 
                               
                               2 
                             
                           
                         
                         , 
                         
                           
                              
                             
                               y 
                               - 
                               
                                 y 
                                 e 
                               
                             
                              
                           
                           ≤ 
                           
                             
                               w 
                               c 
                             
                             2 
                           
                         
                       
                       } 
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     ℒ 
                     = 
                     
                       { 
                       
                         
                           
                             r 
                             ∈ 
                             
                               ℝ 
                               2 
                             
                           
                            
                           
                             
                                
                               
                                 x 
                                 - 
                                 
                                   x 
                                   l 
                                 
                               
                                
                             
                             ≤ 
                             
                               
                                 l 
                                 c 
                               
                               2 
                             
                           
                         
                         , 
                         
                           
                              
                             
                               y 
                               - 
                               
                                 y 
                                 l 
                               
                             
                              
                           
                           ≤ 
                           
                             
                               w 
                               c 
                             
                             2 
                           
                         
                       
                       } 
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                      
                     = 
                     
                       { 
                       
                         
                           
                             r 
                             ∈ 
                             
                               ℝ 
                               2 
                             
                           
                            
                           
                             
                                
                               
                                 x 
                                 - 
                                 
                                   x 
                                   o 
                                 
                               
                                
                             
                             ≤ 
                             
                               
                                 l 
                                 c 
                               
                               2 
                             
                           
                         
                         , 
                         
                           
                              
                             
                               y 
                               - 
                               
                                 y 
                                 o 
                               
                             
                              
                           
                           ≤ 
                           
                             
                               w 
                               c 
                             
                             2 
                           
                         
                       
                       } 
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   4 
                   ) 
                 
               
             
           
         
       
     
     For simplicity, we assume that 0 is moving with a constant speed v xo  on the left lane in the opposite direction. 
     Intention Models of the Lead Vehicle  
     During the intention estimation phase, we assume that ε is behind   in the same lane and the headway h(k)=x l (k)−x e (k) between   and ε satisfies h(k)&gt;h min &gt;0 to avoid collision and ensure safety. We also limit our intention estimation problem to the case where   has a constant lateral position and its longitudinal velocity under different driving behaviors does not saturate in the process of intention estimation. Specifically, we consider two driver intentions i∈{A, C} for  , corresponding to Annoying and Cautious drivers, which are modeled as: 
     
       
         
           
             
               
                 
                   
                     u 
                     
                       
                         x 
                          
                         
                             
                         
                          
                         l 
                       
                       , 
                       i 
                     
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 u 
                                 
                                   x 
                                    
                                   
                                       
                                   
                                    
                                   l 
                                 
                               
                               , 
                               
                                 0 
                                 + 
                                 
                                   
                                     K 
                                     
                                       i 
                                       , 
                                       1 
                                     
                                   
                                    
                                   Δ 
                                    
                                   
                                       
                                   
                                    
                                   y 
                                 
                                 + 
                                 
                                   
                                     K 
                                     
                                       i 
                                       , 
                                       2 
                                     
                                   
                                    
                                   Δ 
                                    
                                   
                                       
                                   
                                    
                                   
                                     h 
                                     i 
                                   
                                 
                                 + 
                                 
                                   δ 
                                   i 
                                 
                               
                               , 
                             
                           
                           
                             
                               
                                 
                                   h 
                                   min 
                                 
                                 &lt; 
                                 h 
                                 &lt; 
                                 
                                   h 
                                   max 
                                 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 u 
                                 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     l 
                                   
                                   , 
                                   0 
                                 
                               
                               , 
                             
                           
                           
                             otherwise 
                           
                         
                       
                       , 
                     
                   
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
           
         
       
     
     where Δy=y e −y l , Δh A =h max −h, Δh C =h, K i,1  and K i,2  are constants and δ i  is an input uncertainty accounting for nonlinear non-deterministic driving behavior. u x1,0 =−K 0 (v xl −v xl   des )+C d v xl   des  is a baseline controller that represents the default lead vehicle   behavior to maintain a desired speed v xl   des . We choose K A,1 &gt;0 and K A,2 &gt;0 in (5) such that the annoying driver drives aggressively and speeds up when ε tries to overtake  , while K C,1 &lt;0 and K C,2 &lt;0 for the cautious driver who slows down and makes it easier for ε to overtake  . 
     The problem of autonomously overtaking a lead vehicle   in the presence of an oncoming vehicle O on the left lane is a hard task when there is no knowledge on how   is going to behave. Since the knowledge of  &#39;s intention would be beneficial in motion planning, we propose to split the overall overtaking problem into two sequential sub-problems: (1) Intent estimation of   based on the intent models (5), and (2) Overtaking control for ε given the intention of  . Formally, the sub-problems are: 
     Problem 1: Estimate the intention of   over a fixed time horizon T. This includes the following two parts: 
     Active model discrimination: Find an optimal ego vehicle&#39;s input sequence u T   * =(u e   * (0), . . . , u e   * (T−1)) (with zero-order hold) over a finite horizon T offline such that the observed trajectory of   is only consistent with one intention model regardless of any realization of uncertainties. 
     Model selection: Implement the obtained optimal input sequence alongside a model selection algorithm in real-time to identify the intention of   based on its observed output trajectories. 
     Problem 2: Find a control input u e =[u xe  v ye ] T  for the ego vehicle ε to overtake the lead vehicle   safely in the presence of the oncoming vehicle O based on the estimated intention from Problem 1. 
     Intention Estimation of the Lead Vehicle 
     In this section, we design an optimal ε&#39;s input sequence and an intention estimation algorithm to distinguish the two intentions of  . The following time-discretized model for   and ε is used in this section: 
         v   xe ( k+ 1)=(1− C   d   δt ) v   xe ( k )+ u   xe ( k )δ t   (6a)
 
         y   e ( k+ 1)= y   e ( k )+ v   ye ( k )δ t   (6b)
 
         h ( k+ 1)= h ( k )− v   xe ( k )δ t+v   xl ( k )δ t   (6c)
 
         v   xl ( k+ 1)=(1− C   d   δt ) v   xl ( k )+ u   xl ( k )δ t   (6d)
 
     where h(k)=x l (k)−x e (k) is the headway between   and ε and δt is the sampling time. The output of the discrete time model is  &#39;s velocity, i.e. z(k)=v xl (k). Note that the separating input u e =[u xe  v ye ] T  is equal for both intention models. 
     Vehicle Dynamics with Different Intentions 
     The discrete-time state-space models    i , i∈{A, C}, for both the annoying vehicle and cautious vehicle, are given by 
       {right arrow over ( x   l )}( k+ 1)= A   i   {right arrow over (x)}   l ( k )+ B   i   u   e ( k )+ B   δ,i δ i ( k )+ f   i ,  (7)
 
         z   i ( k )= C   i   {right arrow over (x)}   i ( k ),  (8)
 
     where {right arrow over (x)}(k)=[v xe,i (k) y e,i (k) h i (k) v xl,i (k)] T , z i (k)=v xl,i (k), and the system matrices are 
     
       
         
           
             
               
                 A 
                 i 
               
               = 
               
                 [ 
                 
                   
                     
                       
                         1 
                         - 
                         
                           
                             C 
                             d 
                           
                            
                           δ 
                            
                           
                               
                           
                            
                           t 
                         
                       
                     
                     
                       0 
                     
                     
                       0 
                     
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                     
                       1 
                     
                     
                       0 
                     
                     
                       0 
                     
                   
                   
                     
                       
                         
                           - 
                           δ 
                         
                          
                         
                             
                         
                          
                         t 
                       
                     
                     
                       0 
                     
                     
                       1 
                     
                     
                       
                         δ 
                          
                         
                             
                         
                          
                         t 
                       
                     
                   
                   
                     
                       0 
                     
                     
                       
                         
                           K 
                           
                             i 
                             , 
                             1 
                           
                         
                          
                         δ 
                          
                         
                             
                         
                          
                         t 
                       
                     
                     
                       
                         
                           
                             a 
                             _ 
                           
                           i 
                         
                          
                         
                           K 
                           
                             i 
                             , 
                             2 
                           
                         
                          
                         δ 
                          
                         
                             
                         
                          
                         t 
                       
                     
                     
                       
                         1 
                         - 
                         
                           
                             ( 
                             
                               
                                 K 
                                 0 
                               
                               + 
                               
                                 C 
                                 d 
                               
                             
                             ) 
                           
                            
                           δ 
                            
                           
                               
                           
                            
                           t 
                         
                       
                     
                   
                 
                 ] 
               
             
             , 
             
               
 
             
              
             
               
                 B 
                 i 
               
               = 
               
                 [ 
                 
                   
                     
                       
                         δ 
                          
                         
                             
                         
                          
                         t 
                       
                     
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                     
                       
                         δ 
                          
                         
                             
                         
                          
                         t 
                          
                         
                             
                         
                          
                         0 
                       
                     
                   
                   
                     
                       0 
                     
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                     
                       0 
                     
                   
                 
                 ] 
               
             
             , 
             
               
                 B 
                 
                   δ 
                   , 
                   i 
                 
               
               = 
               
                 [ 
                 
                   
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                   
                   
                     
                       
                         δ 
                          
                         t 
                       
                     
                   
                 
                 ] 
               
             
             , 
             
               
 
             
              
             
               
                 f 
                 i 
               
               = 
               
                 [ 
                 
                   
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                   
                   
                     
                       0 
                     
                   
                   
                     
                       
                         
                           
                             ( 
                             
                               
                                 K 
                                 0 
                               
                               + 
                               
                                 C 
                                 d 
                               
                             
                             ) 
                           
                            
                           
                             v 
                             
                               x 
                                
                               
                                   
                               
                                
                               l 
                             
                             des 
                           
                            
                           δ 
                            
                           
                               
                           
                            
                           t 
                         
                         + 
                         
                           
                             f 
                             _ 
                           
                           i 
                         
                       
                     
                   
                 
                 ] 
               
             
             , 
             
               
                 C 
                 i 
               
               = 
               
                 [ 
                 
                   
                     
                       0 
                     
                     
                       0 
                     
                     
                       0 
                     
                     
                       1 
                     
                   
                 
                 ] 
               
             
             , 
           
         
       
     
     where ā i  and  f   i  are defined as 
     
       
         
           
             
               
                 a 
                 _ 
               
               i 
             
             = 
             
               { 
               
                 
                   
                     
                       
                         
                           - 
                           1 
                         
                         , 
                         
                           i 
                           = 
                           A 
                         
                       
                     
                   
                   
                     
                       
                         1 
                         , 
                         
                           i 
                           = 
                           C 
                         
                       
                     
                   
                 
                 , 
                 
                   
                     and 
                      
                     
                         
                     
                      
                     
                       
                         f 
                         _ 
                       
                       i 
                     
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 K 
                                 
                                   A 
                                   , 
                                   2 
                                 
                               
                                
                               
                                 h 
                                 max 
                               
                                
                               δ 
                                
                               
                                   
                               
                                
                               t 
                             
                             , 
                             
                               i 
                               = 
                               A 
                             
                           
                         
                       
                       
                         
                           
                             0 
                             , 
                             
                               i 
                               = 
                               C 
                             
                           
                         
                       
                     
                   
                 
               
             
           
         
       
     
     For both vehicle models    i , i∈{A, C}, we assume that the initial condition for model i, denoted by {right arrow over (x)} i   0 ={right arrow over (x)} i (0), is constrained to a polyhedral set defined as: 
         {right arrow over (x)}   i   0 ∈χ 0   ={{right arrow over (x)}∈     4   :P   0   {right arrow over (x)}≤p   0   },∀i∈{A,C}   (9)
 
     The constraints on controlled input u e  defined in (2) are equivalently written as the following polyhedral set (for k∈   T−1   0 ): 
         u   e ( k )∈ ={ u   e ∈   2   :Q   u   u   e   ≤q   u }  (10)
 
     In addition, the uncertainty δ i  is also contained to a polyhedral set (for k∈   T−1   0 ) defined as 
       δ i ( k )∈   i   ={δ∈     :Q   δ,i   δ≤q   δ,i }  (11)
 
     The states {right arrow over (x)} i (k) are divided into controlled state x i (k)=[v xe,i (k) y e,i (k) h i (k)] T ∈   3  and uncontrolled state y i (k)=v xl,i (k)∈ . The controlled state x i (k) are constrained to the following polyhedral set (for k∈   T   + ) 
         x   i ( k )∈χ x,i   ={x∈     3   :P   x,i   x≤p   x,i }.  (12)
 
     Moreover, we require   and ε to keep a minimum distance to avoid collision during the intention estimation process. This safety constraint is given by 
         h   i ( k )∈   i   ={h∈     :h≥h   min }  (13)
 
     where h min &gt;0. In addition, we constrain ε longitudinal velocity to an interval with v ex,i ∈[v e   min , v e   max ], and ε&#39;s lateral velocity input has to satisfy a piece-wise state-dependent constraint defined as 
     
       
         
           
             
               
                 
                   
                     v 
                     
                       ye 
                       , 
                       i 
                     
                   
                   ∈ 
                   
                     { 
                     
                       
                         
                           
                             
                               { 
                               0 
                               } 
                             
                             , 
                             
                               
                                 v 
                                 e 
                                 min 
                               
                               ≤ 
                               
                                 v 
                                 
                                   xe 
                                   , 
                                   i 
                                 
                               
                               ≤ 
                               
                                 v 
                                 xe 
                                 dz 
                               
                             
                             , 
                           
                         
                       
                       
                         
                           
                             
                               { 
                               
                                 
                                   
                                     v 
                                     
                                       ye 
                                       , 
                                       i 
                                     
                                   
                                    
                                   
                                     : 
                                   
                                    
                                   
                                     
                                       β 
                                       1 
                                     
                                      
                                     
                                       ( 
                                       
                                         
                                           v 
                                           
                                             xe 
                                             , 
                                             i 
                                           
                                         
                                         - 
                                         
                                           v 
                                           xe 
                                           dz 
                                         
                                       
                                       ) 
                                     
                                   
                                 
                                 ≤ 
                                 
                                   v 
                                   
                                     ye 
                                     , 
                                     i 
                                   
                                 
                                 ≤ 
                                 
                                   
                                     β 
                                     2 
                                   
                                    
                                   
                                     ( 
                                     
                                       
                                         v 
                                         
                                           xe 
                                           , 
                                           i 
                                         
                                       
                                       - 
                                       
                                         v 
                                         xe 
                                         dz 
                                       
                                     
                                     ) 
                                   
                                 
                               
                               } 
                             
                             , 
                             
                               
                                 v 
                                 xe 
                                 dz 
                               
                               ≤ 
                               
                                 v 
                                 
                                   xe 
                                   , 
                                   i 
                                 
                               
                               ≤ 
                               
                                 v 
                                 e 
                                 max 
                               
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   14 
                   ) 
                 
               
             
           
         
       
     
     where v xe   dz  can be considered as a dead-zone, within which ε does not have lateral motion, while β 1  and β 2  are slopes that mimic curvature constraints of real vehicles. The constraint in (14) implies that ε is allowed to overtake   only when its longitudinal velocity is greater than v xe   dz . An example of the piece-wise state-dependent constraint with v xe   db =10, β 1 =−1, β 2 =1 is shown in  FIG. 2 . 
     Remark 1: We assume that the intention models are well-posed, whose definition can be found in Ref. (12, Remark 1). Vehicle dynamic models are impractical if they are not well-posed, since the responsibilities of the inputs will be impossible to be satisfied; thus, we shall assume throughout the disclosure that vehicle models with different intentions are always well-posed. Now, the offline active model discrimination problem in Problem 1 can be formally redefined as follows: 
     Problem 3: Given two intention models    i , i∈{A, C}, and state, input and uncertainty constraints, (9)-(14), find an optimal input sequence u T   * ={u e   * (0), . . . , u e   * (T−1)} over a finite horizon T to minimize a given cost function ∥u T ∥ ∞  that ensures comfort (with small maximum input amplitudes) such that for all possible initial states {right arrow over (x)} i (0) and uncertainty δ i (k), ∀k∈   T   0 , only one model is valid, i.e., the output trajectories of the intention models    i  over a finite horizon T have to differ by a separation threshold ϵ in at least one time instance. The optimization problem can be formally stated as: 
     
       
         
           
             
               
                 
                   
                     
                       min 
                       
                         
                           u 
                           T 
                         
                         , 
                         
                           x 
                           
                             i 
                             , 
                             T 
                           
                         
                       
                     
                      
                     
                       
                         
                            
                           
                             u 
                             T 
                           
                            
                         
                         ∞ 
                       
                        
                       
                           
                       
                        
                       
                         s 
                         . 
                         t 
                         . 
                         
                           ∀ 
                           
                             k 
                             ∈ 
                             
                               
                                 ℤ 
                                 
                                   T 
                                   - 
                                   1 
                                 
                                 0 
                               
                                
                               
                                 : 
                               
                                
                               
                                 ( 
                                 10 
                                 ) 
                               
                                
                               
                                   
                               
                                
                               holds 
                             
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     15 
                      
                     a 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     ∀ 
                     
                       i 
                       ∈ 
                       
                         
                           { 
                           
                             A 
                             , 
                             C 
                           
                           } 
                         
                          
                         
                             
                         
                          
                         
                           ∀ 
                           
                             k 
                              
                             
                                 
                             
                             ∈ 
                             
                               
                                 ℤ 
                                 T 
                                 + 
                               
                                
                               
                                 : 
                               
                                
                               
                                 ( 
                                 14 
                                 ) 
                               
                                
                               
                                   
                               
                                
                               holds 
                             
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     15 
                      
                     b 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           
                             
                               
                                 ∀ 
                                 i 
                               
                               , 
                               
                                 j 
                                 ∈ 
                                 
                                   { 
                                   
                                     A 
                                     , 
                                     C 
                                   
                                   } 
                                 
                               
                               , 
                               
                                   
                               
                                
                               
                                 i 
                                 ≠ 
                                 j 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 ∀ 
                                 
                                   k 
                                   ∈ 
                                   
                                     ℤ 
                                     T 
                                     0 
                                   
                                 
                               
                               , 
                               
                                 ∀ 
                                 
                                   
                                     x 
                                     → 
                                   
                                   i 
                                   0 
                                 
                               
                               , 
                               
                                 
                                   y 
                                   i 
                                 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               , 
                               
                                 
                                   
                                     δ 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                  
                                 
                                   : 
                                 
                               
                             
                           
                         
                         
                           
                             
                               
                                 ( 
                                 7 
                                 ) 
                               
                               , 
                               
                                 ( 
                                 9 
                                 ) 
                               
                               , 
                               
                                 
                                   ( 
                                   11 
                                   ) 
                                 
                                  
                                 
                                     
                                 
                                  
                                 hold 
                               
                             
                           
                         
                       
                       } 
                     
                      
                     
                       : 
                     
                      
                     
                       ∀ 
                       
                         k 
                         ∈ 
                         
                           
                             ℤ 
                             T 
                             + 
                           
                            
                           
                             : 
                           
                            
                           
                               
                           
                            
                           
                             ( 
                             12 
                             ) 
                           
                         
                       
                     
                   
                   , 
                   
                     
                       ( 
                       13 
                       ) 
                     
                      
                     
                         
                     
                      
                     hold 
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     15 
                      
                     c 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           
                             
                               
                                 ∀ 
                                 i 
                               
                               , 
                               
                                 j 
                                 ∈ 
                                 
                                   { 
                                   
                                     A 
                                     , 
                                     C 
                                   
                                   } 
                                 
                               
                               , 
                               
                                   
                               
                                
                               
                                 i 
                                 ≠ 
                                 j 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 ∀ 
                                 
                                   k 
                                   ∈ 
                                   
                                     ℤ 
                                     T 
                                     0 
                                   
                                 
                               
                               , 
                               
                                 ∀ 
                                 
                                   
                                     x 
                                     → 
                                   
                                   i 
                                   0 
                                 
                               
                               , 
                               
                                 
                                   y 
                                   i 
                                 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               , 
                               
                                 
                                   
                                     δ 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                  
                                 
                                   : 
                                 
                               
                             
                           
                         
                         
                           
                             
                               
                                 
                                   ( 
                                   7 
                                   ) 
                                 
                                 - 
                                 
                                   ( 
                                   8 
                                   ) 
                                 
                               
                               , 
                               
                                 
                                   ( 
                                   11 
                                   ) 
                                 
                                 - 
                                 
                                   
                                     ( 
                                     13 
                                     ) 
                                   
                                    
                                   
                                       
                                   
                                    
                                   hold 
                                 
                               
                             
                           
                         
                       
                       } 
                     
                      
                     
                       ∃ 
                       
                         k 
                         ∈ 
                         
                           
                             
                               ℤ 
                               T 
                               0 
                             
                              
                             
                               : 
                             
                              
                             
                                
                               
                                 
                                   
                                     z 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                 - 
                                 
                                   
                                     z 
                                     j 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                               
                                
                             
                           
                           ≥ 
                           ϵ 
                         
                       
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     
                       where 
                        
                       
                           
                       
                        
                       
                         x 
                         
                           i 
                           , 
                           T 
                         
                       
                     
                     = 
                     
                       
                         
                           { 
                           
                             
                               
                                 x 
                                 i 
                               
                                
                               
                                 ( 
                                 0 
                                 ) 
                               
                             
                             , 
                             … 
                              
                             
                                 
                             
                             , 
                             
                               
                                 x 
                                 i 
                               
                                
                               
                                 ( 
                                 T 
                                 ) 
                               
                             
                           
                           } 
                         
                          
                         
                             
                         
                          
                         with 
                          
                         
                             
                         
                          
                         i 
                       
                       ∈ 
                       
                         
                           { 
                           
                             A 
                             , 
                             C 
                           
                           } 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     1 
                      
                     5 
                      
                     d 
                   
                   ) 
                 
               
             
           
         
       
     
     Intention Estimation Approach 
     To solve Problem 1, an optimization-based approach is proposed, consisting of an offline active model discrimination algorithm and an online model selection algorithm. For brevity, we only give the main results of the approach, and its proofs are omitted as they follow similar steps that can be found in Ref. (12). 
     Proposition 1 (Active Model Discrimination): Given any separation threshold, the active model discrimination problem in Problem 3 is equivalent to a bilevel optimization problem with the following outer problem: 
     
       
         
           
             
               
                 
                   
                     
                       min 
                       
                         u 
                         T 
                       
                     
                      
                     
                       
                          
                         
                           u 
                           T 
                         
                          
                       
                       ∞ 
                     
                   
                    
                   
                       
                   
                 
               
               
                 
                   ( 
                   
                     P 
                     outer 
                   
                   ) 
                 
               
             
             
               
                 
                   
                       
                   
                    
                   
                     
                       s 
                       . 
                       t 
                       . 
                       
                         ∀ 
                         
                           k 
                           ∈ 
                           
                             
                               ℤ 
                               
                                 T 
                                 - 
                                 1 
                               
                               0 
                             
                              
                             
                               : 
                             
                              
                             
                               ( 
                               10 
                               ) 
                             
                              
                             
                                 
                             
                              
                             holds 
                           
                         
                       
                     
                     , 
                   
                 
               
               
                 
                   ( 
                   
                     16 
                      
                     a 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     ∀ 
                     
                       i 
                       ∈ 
                       
                         
                           { 
                           
                             A 
                             , 
                             C 
                           
                           } 
                         
                          
                         
                             
                         
                          
                         
                           ∀ 
                           
                             k 
                              
                             
                                 
                             
                             ∈ 
                             
                               
                                 ℤ 
                                 T 
                                 + 
                               
                                
                               
                                 : 
                               
                                
                               
                                 ( 
                                 14 
                                 ) 
                               
                                
                               
                                   
                               
                                
                               holds 
                             
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     16 
                      
                     b 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           
                             
                               
                                 ∀ 
                                 i 
                               
                               , 
                               
                                 j 
                                 ∈ 
                                 
                                   { 
                                   
                                     A 
                                     , 
                                     C 
                                   
                                   } 
                                 
                               
                               , 
                               
                                   
                               
                                
                               
                                 i 
                                 ≠ 
                                 j 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 ∀ 
                                 
                                   k 
                                   ∈ 
                                   
                                     ℤ 
                                     T 
                                     0 
                                   
                                 
                               
                               , 
                               
                                 ∀ 
                                 
                                   
                                     x 
                                     → 
                                   
                                   i 
                                   0 
                                 
                               
                               , 
                               
                                 
                                   y 
                                   i 
                                 
                                  
                                 
                                   ( 
                                   k 
                                   ) 
                                 
                               
                               , 
                               
                                 
                                   
                                     δ 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                  
                                 
                                   : 
                                 
                               
                             
                           
                         
                         
                           
                             
                               
                                 ( 
                                 7 
                                 ) 
                               
                               , 
                               
                                 ( 
                                 9 
                                 ) 
                               
                               , 
                               
                                 
                                   ( 
                                   11 
                                   ) 
                                 
                                  
                                 
                                     
                                 
                                  
                                 hold 
                               
                             
                           
                         
                       
                       } 
                     
                      
                     
                       ∀ 
                       
                         k 
                         ∈ 
                         
                           
                             ℤ 
                             T 
                             + 
                           
                            
                           
                             : 
                           
                            
                           
                               
                           
                            
                           
                             ( 
                             12 
                             ) 
                           
                         
                       
                     
                   
                   , 
                   
                     
                       ( 
                       13 
                       ) 
                     
                      
                     
                         
                     
                      
                     hold 
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     16 
                      
                     c 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         δ 
                         * 
                       
                        
                       
                         ( 
                         
                           u 
                           T 
                         
                         ) 
                       
                     
                     ≥ 
                     ϵ 
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     16 
                      
                     d 
                   
                   ) 
                 
               
             
           
         
       
     
     where δ*(u T ) is the solution to the inner problem: 
     
       
         
           
             
               
                 
                   
                     
                       δ 
                       * 
                     
                      
                     
                       ( 
                       
                         u 
                         T 
                       
                       ) 
                     
                   
                   = 
                   
                     
                       
                         
                           min 
                         
                       
                       
                         
                           
                             δ 
                             , 
                             
                               
                                 x 
                                 → 
                               
                               i 
                               0 
                             
                             , 
                             
                               δ 
                               i 
                             
                             , 
                             T 
                           
                         
                       
                     
                      
                     δ 
                   
                 
               
               
                 
                   ( 
                   
                     P 
                     
                       inne 
                        
                       r 
                     
                   
                   ) 
                 
               
             
             
               
                 
                   
                     s 
                     . 
                     t 
                     . 
                     
                       ∀ 
                       
                         i 
                         ∈ 
                         
                           { 
                           
                             A 
                             , 
                             C 
                           
                           } 
                         
                       
                     
                   
                   , 
                   
                     ∀ 
                     
                       k 
                       ∈ 
                       
                         
                           
                             ℤ 
                             
                               T 
                               - 
                               1 
                             
                             0 
                           
                            
                           
                             ( 
                             7 
                             ) 
                           
                         
                          
                         
                             
                         
                          
                         holds 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     17 
                      
                     a 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     s 
                     . 
                     t 
                     . 
                     
                       ∀ 
                       
                         i 
                         ∈ 
                         
                           { 
                           
                             A 
                             , 
                             C 
                           
                           } 
                         
                       
                     
                   
                   , 
                   
                     ∀ 
                     
                       k 
                       ∈ 
                       
                         
                           ℤ 
                           T 
                           + 
                         
                          
                         
                           : 
                         
                          
                         
                             
                         
                          
                         
                           ( 
                           8 
                           ) 
                         
                          
                         
                             
                         
                          
                         holds 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   
                     17 
                      
                     b 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           
                             
                               
                                 
                                   ∀ 
                                   i 
                                 
                                 , 
                                 
                                   j 
                                   ∈ 
                                   
                                     { 
                                     
                                       A 
                                       , 
                                       C 
                                     
                                     } 
                                   
                                 
                                 , 
                                 
                                     
                                 
                                  
                                 
                                   i 
                                   ≠ 
                                   j 
                                 
                                 , 
                               
                             
                           
                           
                             
                               
                                 ∀ 
                                 
                                   k 
                                   ∈ 
                                   
                                     ℤ 
                                     T 
                                     0 
                                   
                                 
                               
                             
                           
                         
                         } 
                       
                        
                       
                         : 
                       
                        
                       
                           
                       
                        
                       
                         
                           z 
                           i 
                         
                          
                         
                           ( 
                           k 
                           ) 
                         
                       
                     
                     - 
                     
                       
                         z 
                         j 
                       
                        
                       
                         ( 
                         k 
                         ) 
                       
                     
                   
                    
                   
                     ≤ 
                     δ 
                   
                 
               
               
                 
                   ( 
                   
                     17 
                      
                     c 
                   
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         
                           
                             
                               
                                 
                                   ∀ 
                                   i 
                                 
                                 , 
                                 
                                   j 
                                   ∈ 
                                   
                                     { 
                                     
                                       A 
                                       , 
                                       C 
                                     
                                     } 
                                   
                                 
                                 , 
                                 
                                   ∀ 
                                   
                                     k 
                                     ∈ 
                                     
                                       ℤ 
                                       T 
                                       0 
                                     
                                   
                                 
                                 , 
                               
                             
                           
                           
                             
                               
                                 
                                   ∀ 
                                   
                                     
                                       x 
                                       → 
                                     
                                     i 
                                     0 
                                   
                                 
                                 , 
                                 
                                   
                                     y 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                                 , 
                                 
                                   
                                     δ 
                                     i 
                                   
                                    
                                   
                                     ( 
                                     k 
                                     ) 
                                   
                                 
                               
                             
                           
                         
                         } 
                       
                        
                       
                         : 
                       
                        
                       
                           
                       
                        
                       
                         ( 
                         9 
                         ) 
                       
                     
                     , 
                     
                       
                         ( 
                         11 
                         ) 
                       
                        
                       
                           
                       
                        
                       hold 
                     
                   
                    
                   
                     
 
                   
                    
                   
                     
                       where 
                        
                       
                           
                       
                        
                       
                         δ 
                         
                           i 
                           , 
                           T 
                         
                       
                     
                     = 
                     
                       
                         
                           { 
                           
                             
                               
                                 δ 
                                 i 
                               
                                
                               
                                 ( 
                                 0 
                                 ) 
                               
                             
                             , 
                             … 
                              
                             
                                 
                             
                             , 
                             
                               
                                 δ 
                                 i 
                               
                                
                               
                                 ( 
                                 T 
                                 ) 
                               
                             
                           
                           } 
                         
                          
                         
                             
                         
                          
                         with 
                       
                       ∈ 
                       
                         
                           { 
                           
                             A 
                             , 
                             C 
                           
                           } 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   
                     17 
                      
                     d 
                   
                   ) 
                 
               
             
           
         
       
     
     Based on the bilevel problem proposed in Proposition 1, we can further leverage Karush-Kuhn-Tucker (KKT) conditions to convert the bi-level problem into a single level MILP problem, which can be solved by off-the-shelf optimization software, such as Gurobi Ref. (14) and CPLEX Ref. (15). These details can be found in our previous disclosure Ref. (12, Theorem 1). Comparing with Ref. (12), we consider additional piece-wise state-dependent constraints and responsibilities on the controlled input in the outer problem of this disclosure, in order to make the intention estimation problem more realistic and practical. In addition, to estimate the intention of  , the optimal separating input is applied in real-time and the following model selection problem is considered: 
     Proposition 2 (Model Selection): Given two vehicle model    i , i∈{A, C}, and an input-output sequence {u e (k), z m (k)} k=0   T , where u e (k) is obtained in Proposition 1, the model selection problem in Problem 1 is equivalent to the following feasibility problem: 
     Find {right arrow over (x)}(k), δ(k), i, ∀k∈   T−1   0    
         s.t.i∈{A,C},∀k∈     T   0   :z   m ( k )− z   i ( k )=0,  (18a)
 
       ∀ k∈     T−1   0 :(7)holds,  (18b)
 
       ∀ k∈     T−1   0 :(9)-(11)hold,  (18c)
 
       ∀ k∈     T   + :(12)-(14)hold.  (18d)
 
     We can solve the above feasibility check problem by leveraging the model invalidation algorithm in Ref. (16) and identify the true intention, which is guaranteed since the separating input in Proposition 1 is applied. 
     Trajectory Generation and Control for the Ego Vehicle 
     In this section, we provide a solution to Problem 2, i.e., we design the  control actions u xe  (t) and v ye (t) of ε so that it safely overtakes   at some finite time T&lt;∞. To assess the safety of the vehicles, we define a weighted ∞-norm distance between two rectangular vehicles i,j as: 
     
       
         
           
             
               
                 
                   
                     
                       
                          
                         
                           
                             r 
                             i 
                           
                           - 
                           
                             r 
                             j 
                           
                         
                          
                       
                       
                         ∞ 
                         _ 
                       
                     
                     = 
                     
                       max 
                        
                       
                         ( 
                         
                           
                              
                             
                               
                                 
                                   x 
                                   i 
                                 
                                 - 
                                 
                                   x 
                                   j 
                                 
                               
                               
                                 l 
                                 c 
                               
                             
                              
                           
                           , 
                           
                              
                             
                               
                                 
                                   y 
                                   i 
                                 
                                 - 
                                 
                                   y 
                                   j 
                                 
                               
                               
                                 w 
                                 c 
                               
                             
                              
                           
                         
                         ) 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   19 
                   ) 
                 
               
             
           
         
       
     
     where l c  and w c  are the dimensions of the rectangles. The distance is referred to as ∞-distance hereafter. 
     Definition 1 (Safety): Two vehicles i and j are safe with regards to each other if ∥r i (t)−r j (t)∥   ∞   &gt;1,∀t&gt;t 0 . 
     To ensure the safety of the ego vehicle ε from the lead vehicle  , we inflate   as 
     
       
         
           
             
               
                 ℒ 
                 _ 
               
               = 
               
                 { 
                 
                   
                     
                       r 
                       ∈ 
                       
                         ℝ 
                         2 
                       
                     
                      
                     
                       
                          
                         
                           x 
                           - 
                           
                             x 
                             1 
                           
                         
                          
                       
                       ≤ 
                       
                         
                           
                             l 
                             _ 
                           
                           c 
                         
                         2 
                       
                     
                   
                   , 
                   
                     
                        
                       
                         y 
                         - 
                         
                           y 
                           l 
                         
                       
                        
                     
                     ≤ 
                     
                       
                         
                           w 
                           _ 
                         
                         c 
                       
                       2 
                     
                   
                 
                 } 
               
             
             , 
             where 
           
         
       
       
         
           
             
               
                 
                   l 
                   _ 
                 
                 c 
               
               = 
               
                 
                   l 
                   c 
                 
                 + 
                 
                   2 
                    
                   
                     ( 
                     
                       x 
                       safe 
                     
                     ) 
                   
                 
               
             
             , 
             
               
                 
                   w 
                   _ 
                 
                 c 
               
               = 
               
                 
                   w 
                   c 
                 
                 + 
                 
                   2 
                    
                   
                     ( 
                     
                       y 
                       safe 
                     
                     ) 
                   
                 
               
             
             , 
             
               
                 and 
                  
                 
                     
                 
                  
                 
                   x 
                   safe 
                 
               
               ≥ 
               
                 
                   l 
                   c 
                 
                 2 
               
             
             , 
             
               
                 y 
                 safe 
               
               ≥ 
               
                 
                   w 
                   c 
                 
                 2 
               
             
           
         
       
     
     are safety parameters. Further, let 
     
       
         
           
             
               R 
               1 
             
             = 
             
               
                 { 
                 
                   
                     y 
                     ∈ 
                     ℝ 
                   
                    
                   
                     
                       
                         w 
                         c 
                       
                       2 
                     
                     ≤ 
                     y 
                     ≤ 
                     
                       
                         y 
                         lane 
                       
                       - 
                       
                         
                           w 
                           c 
                         
                         2 
                       
                     
                   
                 
                 } 
               
               . 
             
           
         
       
     
     Definition 2 (Safe overtake): A maneuver and the resulting trajectory r e (t; t 0 , r e (t 0 )) for ε where the initial position of ε is behind   on the same lane (i.e., x e (t 0 )&lt;x(t 0 ) and y e (t 0 ),y l (t 0 )∈R 1 ) and its final position is ahead of   on the same lane (i.e., x e (t f )&gt;x l (t f ) and y e (t f ),y l (t f )∈R l ), is a safe overtake if ε remains safe with regards to   and O (i.e., ∥r e (t)−r l (t)∥   ∞   &gt;1 and ∥r e (t)−r l (t)∥   ∞   &gt;1, ∀t∈[t 0 , t f ]). 
     To enable safe overtake, we design a finite-time tracking controller based on a safe vector field. First, we choose a desired point r d  ahead of   that is subject to vehicle dynamics corresponding to the estimated intent from the previous section. Next, we define a vector field F that safely guides ε around   to r d , and a tracking controller such that ε tracks the vector field in finite-time. The vector field F is a combination of two vector fields: (1) Guide-away vector field (F l ), and (2) Guide-to vector field (F d ). The Guide-away vector field F l  is defined to closely follow the rectangular shape of the vehicles using superquadric isopotential contours [17], called E-contours in this disclosure for brevity. The super-elliptic distance from   is defined as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           E 
                           l 
                         
                          
                         
                           ( 
                           r 
                           ) 
                         
                       
                       = 
                       
                         
                           
                              
                             
                               
                                 x 
                                 - 
                                 
                                   x 
                                   l 
                                 
                               
                               a 
                             
                              
                           
                           
                             2 
                              
                             
                                 
                             
                              
                             n 
                           
                         
                         + 
                         
                           
                              
                             
                               
                                 y 
                                 - 
                                 
                                   y 
                                   l 
                                 
                               
                               b 
                             
                              
                           
                           
                             2 
                              
                             n 
                           
                         
                         - 
                         1 
                       
                     
                     , 
                     where 
                   
                    
                   
                     
 
                   
                    
                   
                     
                       a 
                       = 
                       
                         
                           
                             
                               l 
                               _ 
                             
                             c 
                           
                           2 
                         
                          
                         
                           
                             ( 
                             2 
                             ) 
                           
                           
                             1 
                             
                               2 
                                
                               
                                   
                               
                                
                               n 
                             
                           
                         
                       
                     
                     , 
                     
                       b 
                       = 
                       
                         
                           
                             
                               w 
                               _ 
                             
                             c 
                           
                           2 
                         
                          
                         
                           
                             ( 
                             2 
                             ) 
                           
                           
                             1 
                             
                               2 
                                
                               
                                   
                               
                                
                               n 
                             
                           
                         
                       
                     
                     , 
                     
                       n 
                       &gt; 
                       1 
                     
                     , 
                   
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
           
         
       
     
     and is referred to as E-distance hereafter. For larger values of n, the contours corresponding to a constant value of E-distance E l  tend to be of rectangular shape.  FIG. 3  shows three contours for E l =0, E l =Ē l =1.94 and E l =E l   u =19.43, with n=1.5 and   situated at [30,1.85] T . The inflated lead vehicle   can be approximated by a bounding smallest super-ellipse that contains  :   m ={r∈   2 |E 1 (r)≤0} Then the slope of the tangent to E-contours at a point with position vector r∈   2  is: 
     
       
         
           
             
               
                 
                   
                     tan 
                      
                     
                       ( 
                       
                         
                           
                             ℬ 
                             ¯ 
                           
                           l 
                         
                          
                         
                           ( 
                           r 
                           ) 
                         
                       
                       ) 
                     
                   
                   = 
                   
                     - 
                     
                       
                         
                           
                             b 
                             
                               2 
                                
                               
                                   
                               
                                
                               n 
                             
                           
                            
                           
                             ( 
                             
                               x 
                               - 
                               
                                 x 
                                 l 
                               
                             
                             ) 
                           
                         
                          
                         
                           
                              
                             
                               x 
                               - 
                               
                                 x 
                                 l 
                               
                             
                              
                           
                           
                             
                               2 
                                
                               
                                   
                               
                                
                               n 
                             
                             - 
                             2 
                           
                         
                       
                       
                         
                           
                             a 
                             
                               2 
                                
                               
                                   
                               
                                
                               n 
                             
                           
                            
                           
                             ( 
                             
                               y 
                               - 
                               
                                 y 
                                 l 
                               
                             
                             ) 
                           
                         
                          
                         
                           
                              
                             
                               y 
                               - 
                               
                                 y 
                                 l 
                               
                             
                              
                           
                           
                             
                               2 
                                
                               
                                   
                               
                                
                               n 
                             
                             - 
                             2 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   21 
                   ) 
                 
               
             
           
         
       
     
     Let the unit tangent vector at r be 
     
       
         
           
             
               
                 
                   t 
                   ^ 
                 
                 l 
               
                
               
                 ( 
                 r 
                 ) 
               
             
             = 
             
               [ 
               
                 
                   
                     
                       cos 
                        
                       
                           
                       
                        
                       
                         ( 
                         
                           
                             
                               ℬ 
                               ¯ 
                             
                             l 
                           
                            
                           
                             ( 
                             r 
                             ) 
                           
                         
                         ) 
                       
                     
                   
                 
                 
                   
                     
                       sin 
                        
                       
                         ( 
                         
                           
                             
                               ℬ 
                               ¯ 
                             
                             l 
                           
                            
                           
                             ( 
                             r 
                             ) 
                           
                         
                         ) 
                       
                     
                   
                 
               
               ] 
             
           
         
       
     
     Vector Fields for Trajectory Generation 
     1) Guide-away vector field: The Guide-away vector field F l  around the obstacle   is defined as: 
     
       
         
           
             
               
                 
                   
                     
                       
                         F 
                         l 
                       
                        
                       
                         ( 
                         r 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           [ 
                           
                             
                               cos 
                                
                               
                                 ( 
                                 
                                   
                                     
                                       ℬ 
                                       ¯ 
                                     
                                     l 
                                     ′ 
                                   
                                    
                                   
                                     ( 
                                     r 
                                     ) 
                                   
                                 
                                 ) 
                               
                             
                              
                             
                               sin 
                                
                               
                                 ( 
                                 
                                   
                                     
                                       ℬ 
                                       ¯ 
                                     
                                     l 
                                     ′ 
                                   
                                    
                                   
                                     ( 
                                     r 
                                     ) 
                                   
                                 
                                 ) 
                               
                             
                           
                           ] 
                         
                         ⊤ 
                       
                        
                       where 
                     
                   
                    
                   
                     
 
                   
                    
                   
                     
                       
                         ℬ 
                         ¯ 
                       
                       l 
                       ′ 
                     
                      
                     
                       ( 
                       r 
                       ) 
                     
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 
                                   
                                     
                                       
                                         ℬ 
                                         ¯ 
                                       
                                       l 
                                       ′ 
                                     
                                      
                                     
                                       ( 
                                       r 
                                       ) 
                                     
                                   
                                   - 
                                   π 
                                 
                                 , 
                                 
                                   
                                     
                                       ( 
                                       
                                         
                                           ( 
                                           
                                             r 
                                             - 
                                             
                                               r 
                                               d 
                                             
                                           
                                           ) 
                                         
                                         × 
                                         
                                           
                                             
                                               t 
                                               ^ 
                                             
                                             l 
                                           
                                            
                                           
                                             ( 
                                             r 
                                             ) 
                                           
                                         
                                       
                                       ) 
                                     
                                     · 
                                     
                                       k 
                                       ^ 
                                     
                                   
                                   &gt; 
                                   0 
                                 
                                 , 
                               
                             
                           
                           
                             
                               
                                 
                                   
                                     ℬ 
                                     d 
                                   
                                   - 
                                   π 
                                 
                                 , 
                                 otherwise 
                               
                             
                           
                         
                          
                         with 
                          
                         
                           
 
                         
                          
                         
                           ℬ 
                           d 
                         
                       
                       = 
                       
                         atan 
                          
                         
                             
                         
                          
                         2 
                          
                         
                           
                             ( 
                             
                               
                                 y 
                                 - 
                                 
                                   y 
                                   d 
                                 
                               
                               
                                 x 
                                 - 
                                 
                                   x 
                                   d 
                                 
                               
                             
                             ) 
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   22 
                   ) 
                 
               
             
           
         
       
     
     This way, F l  takes the form shown in regions    l ={r∈   2 |0&lt;E l (r)&lt;Ē l , ((r−r d )×{circumflex over (t)} l (r))·{circumflex over (k)}&gt;0} (see violet vectors in  FIG. 3 ) and    l ={r∈   2 |0&lt;E l (r)&lt;E l   u , ((r−r d )×{circumflex over (t)} l (r)·{circumflex over (k)}≤0 (see green vectors in  FIG. 3 ) 
     2) Guide-to vector field: The Guide-to vector field F l  is defined as a radially converging vector field to the desired location r d : 
     
       
         
           
             
               
                 
                   
                     
                       F 
                       d 
                     
                      
                     
                       ( 
                       r 
                       ) 
                     
                   
                   = 
                   
                     
                       
                         r 
                         d 
                       
                       - 
                       r 
                     
                     
                        
                       
                         
                           r 
                           d 
                         
                         - 
                         r 
                       
                        
                     
                   
                 
               
               
                 
                   ( 
                   23 
                   ) 
                 
               
             
           
         
       
     
     We set F d (r d )=0 so that F d  is defined everywhere. 
     3) Combining vector fields: In order to combine the Guide-to and Guide-away vector fields smoothly near  , a blending function is defined as: 
     
       
         
           
             
               
                 
                   
                     σ 
                      
                     
                       ( 
                       
                         E 
                         l 
                       
                       ) 
                     
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             1 
                             , 
                             
                               0 
                               ≤ 
                               
                                 E 
                                 l 
                               
                               ≤ 
                               
                                 
                                   E 
                                   _ 
                                 
                                 l 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 A 
                                  
                                 
                                     
                                 
                                  
                                 
                                   E 
                                   l 
                                   3 
                                 
                               
                               + 
                               
                                 BE 
                                 l 
                                 2 
                               
                               + 
                               
                                 C 
                                  
                                 
                                     
                                 
                                  
                                 
                                   E 
                                   i 
                                 
                               
                               + 
                               D 
                             
                             , 
                             
                               
                                 
                                   E 
                                   _ 
                                 
                                 l 
                               
                               ≤ 
                               
                                 E 
                                 l 
                               
                               ≤ 
                               
                                 E 
                                 l 
                                 u 
                               
                             
                           
                         
                       
                       
                         
                           
                             0 
                             , 
                             
                               
                                 E 
                                 l 
                                 u 
                               
                               ≤ 
                               
                                 E 
                                 l 
                               
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   24 
                   ) 
                 
               
             
           
         
       
     
     where Ē l , E l   u  are the super-elliptic distances defining the inner and outer boundaries of the blending region    l =(r∈   2 |Ē l &lt;E i (r)&lt;E l   u }, and the coefficients A, B, C, D are chosen as: 
     
       
         
           
             
               A 
               = 
               
                 2 
                 
                   
                     ( 
                     
                       
                         E 
                         l 
                         u 
                       
                       - 
                       
                         
                           E 
                           _ 
                         
                         l 
                       
                     
                     ) 
                   
                   3 
                 
               
             
             , 
             
               B 
               = 
               
                 
                   
                     - 
                     3 
                   
                    
                   
                     ( 
                     
                       
                         E 
                         l 
                         u 
                       
                       + 
                       
                         
                           E 
                           _ 
                         
                         l 
                       
                     
                     ) 
                   
                 
                 
                   
                     ( 
                     
                       
                         E 
                         l 
                         u 
                       
                       - 
                       
                         
                           E 
                           _ 
                         
                         l 
                       
                     
                     ) 
                   
                   3 
                 
               
             
             , 
             
               C 
               = 
               
                 
                   6 
                    
                   
                     E 
                     l 
                     u 
                   
                    
                   
                     
                       E 
                       _ 
                     
                     l 
                   
                 
                 
                   
                     ( 
                     
                       
                         E 
                         l 
                         u 
                       
                       - 
                       
                         
                           E 
                           _ 
                         
                         l 
                       
                     
                     ) 
                   
                   3 
                 
               
             
             , 
             
               
 
             
              
             
               D 
               = 
               
                 
                   
                     
                       ( 
                       
                         E 
                         l 
                         u 
                       
                       ) 
                     
                     2 
                   
                    
                   
                     ( 
                     
                       
                         E 
                         l 
                         u 
                       
                       - 
                       
                         3 
                          
                         
                           
                             E 
                             _ 
                           
                           l 
                         
                       
                     
                     ) 
                   
                 
                 
                   
                     ( 
                     
                       
                         E 
                         l 
                         u 
                       
                       - 
                       
                         
                           E 
                           _ 
                         
                         l 
                       
                     
                     ) 
                   
                   3 
                 
               
             
             , 
           
         
       
     
     so that (24) is a C 1  function. We choose 
     
       
         
           
             
               E 
               l 
               u 
             
             = 
             
               
                 
                   ( 
                   
                     
                       
                         2 
                          
                         
                           y 
                           lane 
                         
                       
                       - 
                       
                         
                           
                             w 
                             c 
                           
                           2 
                         
                          
                         
                           y 
                           l 
                         
                       
                     
                     b 
                   
                   ) 
                 
                 
                   2 
                    
                   
                       
                   
                    
                   n 
                 
               
               - 
               1 
             
           
         
       
     
     such that F l  is active for 
     
       
         
           
             
               
                 w 
                 c 
               
               2 
             
             ≤ 
             
               y 
               e 
             
             ≤ 
             
               
                 2 
                  
                 
                   y 
                   lane 
                 
               
               - 
               
                 
                   
                     w 
                     c 
                   
                   2 
                 
                 . 
               
             
           
         
       
     
     The value of Ē l  is chosen such that Ē l &lt;E l   u . The maximum distance x rep   max  from ε along {circumflex over (l)} at which the Guide-away vector field F l  is active is given as x rep   max =a(E l   u +1) 1/2n . Then the Guide-to and Guide-away vector fields are blended as: 
         F =(1−σ( E   l ) F   d +σ( E   l ) F   l =[ F   x   F   y ] T .  (25)
 
     A vector field similar to F l  is considered around O when ε attempts to overtake. However, the decision making and analysis is just based on the vector field around  . 
     Control Design for the Ego Vehicle 
     The desired position r d  of ε is chosen ahead of  , and is assumed to follow dynamics described by: 
     
       
         
           
             
               
                 
                   
                     
                       
                         r 
                         . 
                       
                       d 
                     
                     = 
                     
                       
                         [ 
                         
                           
                             
                               
                                 
                                   x 
                                   . 
                                 
                                 d 
                               
                             
                           
                           
                             
                               
                                 
                                   y 
                                   . 
                                 
                                 d 
                               
                             
                           
                         
                         ] 
                       
                       = 
                       
                         
                           [ 
                           
                             
                               
                                 
                                   v 
                                   
                                     x 
                                      
                                     
                                         
                                     
                                      
                                     d 
                                   
                                 
                               
                             
                             
                               
                                 
                                   v 
                                   
                                     y 
                                      
                                     
                                         
                                     
                                      
                                     d 
                                   
                                 
                               
                             
                           
                           ] 
                         
                         = 
                         
                           v 
                           d 
                         
                       
                     
                   
                   , 
                   
                     
                       
                         v 
                         . 
                       
                       
                         x 
                          
                         
                             
                         
                          
                         d 
                       
                     
                     = 
                     
                       
                         u 
                         xd 
                       
                       - 
                       
                         
                           C 
                           d 
                         
                          
                         
                           v 
                           
                             x 
                              
                             
                                 
                             
                              
                             d 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   26 
                   ) 
                 
               
             
           
         
       
     
     where v yd =0, u xd =u xl.i0 +δ i   min  and u xl,i0 =u xl,i −δ i , i={A,C,N}; i=N stands for the case when ε is not estimating the intention of  , which we consider for comparison. In the latter case,   is assumed to be moving with constant average traffic speed (v tr   avg ), with some uncertainty in its acceleration such that the corresponding input is u xl,N =C d v tr   avg +δ N , where δ N ∈[δ N   min ,δ N   max ]. 
     We obtain the dynamics of the motion of ε and   relative to a coordinate frame attached on r d . Define 
     
       
         
           
             
               
                 
                   r 
                   ¯ 
                 
                 e 
               
               = 
               
                 
                   r 
                   e 
                 
                 - 
                 
                   r 
                   d 
                 
               
             
             , 
             
               
                 
                   v 
                   ¯ 
                 
                 e 
               
               = 
               
                 
                   
                     v 
                     e 
                   
                   - 
                   
                     
                       v 
                       d 
                     
                      
                     
                         
                     
                      
                     and 
                      
                     
                         
                     
                      
                     
                       F 
                       ^ 
                     
                   
                 
                 = 
                 
                   
                     F 
                     
                        
                       F 
                        
                     
                   
                   . 
                 
               
             
           
         
       
     
     Then the dynamics of ε and   expressed in the new frame are: 
     
       
         
           
             
               
                 
                   
                     
                       
                         r 
                         
                           _ 
                           . 
                         
                       
                       j 
                     
                     = 
                     
                       
                         [ 
                         
                           
                             
                               
                                 
                                   
                                     x 
                                     . 
                                   
                                   j 
                                 
                                 - 
                                 
                                   
                                     x 
                                     . 
                                   
                                   d 
                                 
                               
                             
                           
                           
                             
                               
                                 
                                   
                                     y 
                                     . 
                                   
                                   j 
                                 
                                 - 
                                 
                                   
                                     y 
                                     . 
                                   
                                   d 
                                 
                               
                             
                           
                         
                         ] 
                       
                       = 
                       
                         
                           [ 
                           
                             
                               
                                 
                                   
                                     v 
                                     
                                       x 
                                        
                                       
                                           
                                       
                                        
                                       k 
                                     
                                   
                                   - 
                                   
                                     v 
                                     
                                       x 
                                        
                                       
                                           
                                       
                                        
                                       d 
                                     
                                   
                                 
                               
                             
                             
                               
                                 
                                   
                                     v 
                                     
                                       y 
                                        
                                       
                                           
                                       
                                        
                                       k 
                                     
                                   
                                   - 
                                   
                                     v 
                                     
                                       y 
                                        
                                       
                                           
                                       
                                        
                                       d 
                                     
                                   
                                 
                               
                             
                           
                           ] 
                         
                         = 
                         
                           
                             v 
                             _ 
                           
                           j 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   27 
                   ) 
                 
               
             
           
         
       
     
     for j∈{e,l}, where ū xe =u xe −u xd  and ū xl =u xl −u xd =δ i   max −δ i   min . With this choice, we have that   is either stationary or moving towards the desired position r d . In the worst case, the maximum value of ū xl  is ū xl   max =δ i   max −δ i   min . This results in a maximum velocity of   relative to r d  of 
     
       
         
           
             
               
                 v 
                 _ 
               
               xl 
               max 
             
             = 
             
               
                 
                   
                     u 
                     _ 
                   
                   xl 
                   max 
                 
                 
                   C 
                   d 
                 
               
               . 
             
           
         
       
     
     We design a desired velocity profile for ε that is safe and convergent to the desired position r d  along F as: 
     
       
         
           
             
               
                 
                   
                     
                       v 
                       ¯ 
                     
                     e 
                     des 
                   
                   = 
                   
                     
                       [ 
                       
                         
                           
                             
                               
                                 v 
                                 ¯ 
                               
                               
                                 x 
                                  
                                 
                                     
                                 
                                  
                                 e 
                               
                               des 
                             
                           
                         
                         
                           
                             
                               
                                 v 
                                 ¯ 
                               
                               
                                 y 
                                  
                                 
                                     
                                 
                                  
                                 e 
                               
                               des 
                             
                           
                         
                       
                       ] 
                     
                     = 
                     
                       { 
                       
                         
                           
                             
                               
                                 
                                   F 
                                   ^ 
                                 
                                  
                                 
                                   k 
                                   r 
                                 
                                  
                                 
                                   
                                      
                                     
                                       
                                         r 
                                         ¯ 
                                       
                                       e 
                                     
                                      
                                   
                                   
                                     α 
                                     r 
                                   
                                 
                               
                               , 
                               
                                 
                                    
                                   
                                     
                                       r 
                                       ¯ 
                                     
                                     e 
                                   
                                    
                                 
                                 ≤ 
                                 
                                   R 
                                   e 
                                   f 
                                 
                               
                               , 
                             
                           
                         
                         
                           
                             
                               
                                 
                                   F 
                                   ^ 
                                 
                                  
                                 
                                   
                                     v 
                                     ¯ 
                                   
                                   e 
                                   over 
                                 
                               
                               , 
                               otherwise 
                             
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         v 
                         ¯ 
                       
                       . 
                     
                     e 
                     des 
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                 
                                   
                                     F 
                                     . 
                                   
                                   ^ 
                                 
                                  
                                 
                                   k 
                                   r 
                                 
                                  
                                 
                                   
                                      
                                     
                                       
                                         r 
                                         ¯ 
                                       
                                       e 
                                     
                                      
                                   
                                   
                                     α 
                                     r 
                                   
                                 
                               
                               + 
                               
                                 
                                   k 
                                   r 
                                 
                                  
                                 
                                   α 
                                   r 
                                 
                                  
                                 
                                   F 
                                   ^ 
                                 
                                  
                                 
                                   
                                      
                                     
                                       
                                         r 
                                         ¯ 
                                       
                                       e 
                                     
                                      
                                   
                                   
                                     
                                       α 
                                       r 
                                     
                                     - 
                                     2 
                                   
                                 
                                  
                                 
                                   ( 
                                   
                                     
                                       
                                         r 
                                         ¯ 
                                       
                                       e 
                                       ⊤ 
                                     
                                      
                                     
                                       
                                         v 
                                         ¯ 
                                       
                                       e 
                                     
                                   
                                   ) 
                                 
                               
                             
                             , 
                             
                               
                                  
                                 
                                   
                                     r 
                                     ¯ 
                                   
                                   e 
                                 
                                  
                               
                               ≤ 
                               
                                 R 
                                 e 
                                 f 
                               
                             
                             , 
                           
                         
                       
                       
                         
                           
                             
                               
                                 
                                   F 
                                   . 
                                 
                                 ^ 
                               
                                
                               
                                 
                                   v 
                                   ¯ 
                                 
                                 e 
                                 over 
                               
                             
                             , 
                             otherwise 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   29 
                   ) 
                 
               
             
           
         
       
     
     where  v   e   over &gt;0 is a constant overtake speed, while k r &gt;0 and α r ∈(0,1) are chosen such that v e   over =k r (R e   f ) α     r   , and R e   f  is the radius around the desired position where the finite-time convergent controller takes over. 
     We now propose the form of the tracking controllers for the control inputs u xe  and v xe  of ε. For the lateral velocity v ye  of ε, we define the control input as: 
         v   ye   = v     ye   des   +v   yd   (30)
 
     where  v   ye   des  is the desired relative velocity along the y-axis. The acceleration input ū xe  to track the desired longitudinal velocity  v   xe   des  is designed as: 
         ū   xe   =C   d     v     xe   des   + v     xe   des   −k   v   ē   v     xe     |ē   v     xe   | α     v     −1   (31)
 
     where ē v     xe   = v   xe − v   xe   des . In the global coordinate frame, the acceleration input u xe  of ε is: 
         u   xe   =ū   xe   +u   xd   (32)
 
     Theorem 1: The relative velocity  v   xe  converges to the desired relative velocity profile  v   xe   des  in finite time under the control action ū xe  given by (31). 
     Proof: Under the control law ū xe  given by (31), the error dynamics is: 
         ē   v     xe     =−k   v   ē   v     xe     |ē   v     xe   | α     v     −1   (33)
 
     From Ref. (18, Lemma 1], we obtain that the equilibrium ē v     xe   = v   xe − v   xe   des =0 of (33) is finite-time stable. 
     The safe initial distance | x   l (t 0 )| between   and r d  at the start of the overtake (t=t 0 ) should be such that   does not come x rep   max +R e   f  close to r d  when ε converges to r d . Since the time of convergence T f  depends on | x   l (t 0 )|, we use an iterative approach to find | x   l (t 0 )| as follows: Initialize | x   l (t 0 )| 0 =x rep   max +R e   f , then | z   l (t 0 )| is iteratively updated using: 
     
       
         
           
             
               
                 
                   
                     
                        
                       
                         
                           
                             x 
                             _ 
                           
                           l 
                         
                          
                         
                           ( 
                           
                             t 
                             0 
                           
                           ) 
                         
                       
                        
                     
                     
                       m 
                       + 
                       1 
                     
                   
                   = 
                   
                     
                       
                          
                         
                           
                             
                               x 
                               _ 
                             
                             l 
                           
                            
                           
                             ( 
                             
                               t 
                               0 
                             
                             ) 
                           
                         
                          
                       
                       m 
                     
                     + 
                     
                        
                       
                         
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                           ( 
                           
                             
                               t 
                               0 
                             
                             + 
                             
                               Δ 
                                
                               
                                   
                               
                                
                               t 
                             
                           
                            
                         
                          
                         
                             
                         
                          
                         
                           
                             
                               v 
                               _ 
                             
                             xl 
                             max 
                           
                           
                             
                               v 
                               _ 
                             
                             e 
                             over 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   34 
                   ) 
                 
               
             
           
         
       
     
     where  v   xl   max  is the worst-case relative velocity, and 
     
       
         
           
             
               Δ 
                
               
                   
               
                
               t 
             
             = 
             
               
                 
                   
                     
                        
                       
                         
                           
                             x 
                             _ 
                           
                           l 
                         
                          
                         
                           ( 
                           
                             t 
                             0 
                           
                           ) 
                         
                       
                        
                     
                     j 
                   
                   - 
                   
                     x 
                     rep 
                     max 
                   
                   - 
                   
                     R 
                     e 
                     f 
                   
                 
                 
                   
                     v 
                     _ 
                   
                   xl 
                   max 
                 
               
               . 
             
           
         
       
     
     The iterative approach is terminated when 
     
       
         
           
             
               
                  
                 
                   
                     
                       r 
                       _ 
                     
                     e 
                   
                    
                   
                     ( 
                     
                       
                         t 
                         0 
                       
                       + 
                       
                         Δ 
                          
                         
                             
                         
                          
                         t 
                       
                     
                     ) 
                   
                 
                  
               
               &lt; 
               
                 
                   y 
                   lane 
                 
                 - 
                 
                   
                     w 
                     c 
                   
                   2 
                 
                 - 
                 
                   
                     y 
                     d 
                   
                    
                   
                     ( 
                     
                       t 
                       0 
                     
                     ) 
                   
                 
               
             
             , 
           
         
       
     
     where y d (t 0 )=y e (t 0 ). This iterative approach is guaranteed to converge if  v   xl   max &lt; v   e   over  because after exiting    l , ∥r e (t 0 +Δt)∥ is convergent to 0 which is discussed later. After ε has successfully overtaken  , ε keeps tracking the desired position r d  that moves with a constant cruise speed v e   cr . 
     Control Constraints 
     We want to design  v   e   over  such that the constraint in (3) is satisfied. Since |u xd |=|u xl,i0 +δ i   min |≤u l   max  from (2), ∀i={A,C,N}, we require ū xe  to satisfy (|ū xe |)≤u e   max,over −u l   max , from (32). The overtaking maneuver constitutes of: (i) initially moving under the Guide-to vector field F d  until converging to the desired relative velocity (28), discussed later, (ii) entering the blending region    l , where Ē l ≤E l ≤E l   u , and (iii) entering the circular region {r e ∈   2 |∥r e −r d ∥≤R e   f }. The ego vehicle has a constant relative speed  v   e   over  in the blending region, and the velocity vector  v   xe  is aligned with the vector field F before it enters the blending region. Then the acceleration ū xe  is bounded as: 
     
       
         
           
             
               
                 
                   
                     
                       u 
                       _ 
                     
                     xe 
                   
                   ≤ 
                   
                     { 
                     
                       
                         
                           
                             
                               
                                  
                                 
                                   
                                     C 
                                     d 
                                   
                                    
                                   
                                     
                                       v 
                                       _ 
                                     
                                     e 
                                     over 
                                   
                                 
                                  
                               
                               + 
                               
                                 
                                   
                                     k 
                                     r 
                                   
                                    
                                   
                                     
                                       v 
                                       _ 
                                     
                                     e 
                                     over 
                                   
                                 
                                 
                                   R 
                                   e 
                                   f 
                                 
                               
                             
                             , 
                             
                               
                                  
                                 
                                   r 
                                   e 
                                 
                                  
                               
                               &lt; 
                               
                                 R 
                                 e 
                                 f 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                  
                                 
                                   
                                     C 
                                     d 
                                   
                                    
                                   
                                     
                                       v 
                                       _ 
                                     
                                     e 
                                     over 
                                   
                                 
                                  
                               
                               + 
                               
                                  
                                 
                                   
                                     v 
                                     _ 
                                   
                                   xe 
                                   des 
                                 
                                  
                               
                             
                             , 
                             
                               
                                  
                                 
                                   r 
                                   e 
                                 
                                  
                               
                               ≥ 
                               
                                 R 
                                 e 
                                 f 
                               
                             
                             , 
                             
                               
                                 
                                   E 
                                   _ 
                                 
                                 i 
                               
                               ≤ 
                               
                                 E 
                                 l 
                               
                               ≤ 
                               
                                 E 
                                 l 
                                 u 
                               
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                  
                                 
                                   
                                     C 
                                     d 
                                   
                                    
                                   
                                     
                                       v 
                                       _ 
                                     
                                     e 
                                     over 
                                   
                                 
                                  
                               
                               + 
                               
                                  
                                 
                                   
                                     k 
                                     v 
                                   
                                    
                                   
                                     
                                       e 
                                       _ 
                                     
                                     
                                       v 
                                       
                                         xe 
                                          
                                         
                                             
                                         
                                          
                                         0 
                                       
                                     
                                     
                                       α 
                                       v 
                                     
                                   
                                 
                                  
                               
                             
                             , 
                             otherwise 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   35 
                   ) 
                 
               
             
           
         
       
     
     where ē v     xe0   ≤ v   e   over , since ε starts approaching   so it is moving at least as fast as  . 
     Now, since  v   xe   des =ψ e   v   e   over  sin ψ≤ v   e   over |ψ e |, where 
     
       
         
           
             
               
                 ψ 
                 e 
               
               = 
               
                 
                   tan 
                   
                     - 
                     1 
                   
                 
                  
                 
                   ( 
                   
                     
                       F 
                       y 
                     
                     
                       F 
                       x 
                     
                   
                   ) 
                 
               
             
             , 
           
         
       
     
     the problem reduces to finding a bound on |ψ e | and designing the velocity  v   e   over  such that it satisfies the overall control bound max(|ū xe |)+u l   max ≤u e   max,over . 
     Denote 
     
       
         
           
             
               
                 
                   F 
                   _ 
                 
                 v 
               
                
               1 
             
             = 
             
               
                 
                   
                     [ 
                     
                       
                         F 
                         y 
                       
                       - 
                       
                         F 
                         x 
                       
                     
                     ] 
                   
                   
                     
                       F 
                       x 
                       2 
                     
                     + 
                     
                       F 
                       y 
                       2 
                     
                   
                 
                  
                 
                   [ 
                   
                     
                       
                         
                           
                             ∂ 
                             
                               F 
                               x 
                             
                           
                           
                             ∂ 
                             
                               
                                 x 
                                 _ 
                               
                               e 
                             
                           
                         
                       
                       
                         
                           
                             ∂ 
                             
                               F 
                               x 
                             
                           
                           
                             ∂ 
                             
                               
                                 y 
                                 _ 
                               
                               e 
                             
                           
                         
                       
                     
                     
                       
                         
                           
                             ∂ 
                             
                               F 
                               y 
                             
                           
                           
                             ∂ 
                             
                               
                                 x 
                                 _ 
                               
                               e 
                             
                           
                         
                       
                       
                         
                           
                             ∂ 
                             
                               F 
                               y 
                             
                           
                           
                             ∂ 
                             
                               
                                 y 
                                 _ 
                               
                               e 
                             
                           
                         
                       
                     
                   
                   ] 
                 
               
                
               
                   
               
                
               
                 
                   
                     [ 
                     
                       
                         F 
                         x 
                       
                        
                       
                         F 
                         y 
                       
                     
                     ] 
                   
                   T 
                 
                 
                   
                     
                       F 
                       x 
                       2 
                     
                     + 
                     
                       F 
                       y 
                       2 
                     
                   
                 
               
                
               
                   
               
                
               and 
             
           
         
       
       
         
           
             
               
                 F 
                 _ 
               
               
                 v 
                  
                 
                     
                 
                  
                 2 
               
             
             = 
             
               
                 
                   
                     [ 
                     
                       
                         F 
                         y 
                       
                       - 
                       
                         F 
                         x 
                       
                     
                     ] 
                   
                   
                     
                       F 
                       x 
                       2 
                     
                     + 
                     
                       F 
                       y 
                       2 
                     
                   
                 
                  
                 
                     
                 
                 [ 
                 
                   
                     
                       
                         
                           ∂ 
                           
                             F 
                             x 
                           
                         
                         
                           ∂ 
                           
                             
                               x 
                               _ 
                             
                             l 
                           
                         
                       
                     
                     
                       
                         
                           ∂ 
                           
                             F 
                             x 
                           
                         
                         
                           ∂ 
                           
                             
                               y 
                               _ 
                             
                             l 
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           ∂ 
                           
                             F 
                             y 
                           
                         
                         
                           ∂ 
                           
                             
                               x 
                               _ 
                             
                             l 
                           
                         
                       
                     
                     
                       
                         
                           ∂ 
                           
                             F 
                             y 
                           
                         
                         
                           ∂ 
                           
                             
                               y 
                               _ 
                             
                             l 
                           
                         
                       
                     
                   
                 
                 ] 
               
               . 
             
           
         
       
     
     The turning rate is then given as: ψ e = F   v1   v   e   over + F   v2   r   l ≤| F   v1 | v   e   over +∥ F   v2 ∥∥ r   l ∥. We consider the following problems: 
     
       
         
           
             
               
                 
                   
                     
                       
                         max 
                         
                           
                             
                               
                                 r 
                                 _ 
                               
                               l 
                             
                             ∈ 
                             
                               D 
                               s 
                             
                           
                           , 
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                         
                       
                        
                       
                         
                            
                           
                             
                               F 
                               _ 
                             
                             
                               v 
                                
                               
                                   
                               
                                
                               1 
                             
                           
                            
                         
                          
                         
                             
                         
                          
                         
                           s 
                           . 
                           t 
                           . 
                           
                               
                           
                            
                           
                             
                               E 
                               _ 
                             
                             l 
                           
                         
                       
                     
                     ≤ 
                     
                       
                         E 
                         l 
                       
                        
                       
                         ( 
                         
                           
                             
                               r 
                               _ 
                             
                             l 
                           
                           , 
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                         
                         ) 
                       
                     
                     ≤ 
                     
                       E 
                       l 
                       u 
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   36 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         max 
                         
                           
                             
                               
                                 r 
                                 _ 
                               
                               l 
                             
                             ∈ 
                             
                               D 
                               s 
                             
                           
                           , 
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                         
                       
                        
                       
                         
                            
                           
                             
                               F 
                               _ 
                             
                             
                               v 
                                
                               
                                   
                               
                                
                               2 
                             
                           
                            
                         
                          
                         
                             
                         
                          
                         
                           s 
                           . 
                           t 
                           . 
                           
                               
                           
                            
                           
                             
                               E 
                               _ 
                             
                             l 
                           
                         
                       
                     
                     ≤ 
                     
                       
                         E 
                         l 
                       
                        
                       
                         ( 
                         
                           
                             
                               r 
                               _ 
                             
                             l 
                           
                           , 
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                         
                         ) 
                       
                     
                     ≤ 
                     
                       E 
                       l 
                       u 
                     
                   
                   , 
                 
               
               
                 
                     
                 
               
             
             
               
                 
                   
                     where 
                      
                     
                         
                     
                      
                     
                       D 
                       s 
                     
                   
                   = 
                   
                     { 
                     
                       
                         
                           
                             ( 
                             
                               
                                 
                                   x 
                                   _ 
                                 
                                 l 
                               
                               , 
                               
                                 
                                   y 
                                   _ 
                                 
                                 l 
                               
                             
                             ) 
                           
                           ∥ 
                           
                             
                               x 
                               _ 
                             
                             l 
                           
                         
                          
                         
                           ≤ 
                           
                             y 
                             
                               lane 
                               - 
                               
                                 
                                   w 
                                   ɛ 
                                 
                                 2 
                               
                             
                           
                         
                       
                       , 
                       
                         
                           
                             y 
                             _ 
                           
                           l 
                         
                         ∈ 
                         
                           [ 
                           
                             
                               b 
                               min 
                             
                             , 
                             
                               b 
                               max 
                             
                           
                           ] 
                         
                       
                     
                     } 
                   
                 
               
               
                 
                     
                 
               
             
           
         
       
     
     is a 2-D square region capturing that   stays in lane and maintains a bounded distance away from r d , and b min , b max  are the search limits along the x-axis. Due to the non-convexity and non-linearity of these problems, we first search in the discretized 4-D space to obtain a sub-optimal solution  r   l   *  and  r   e   * . We then optimize around the neighborhood  ( r   l   * , r   e   * ) of the sub-optimal solution: 
     
       
         
           
             
               
                 
                   
                     
                       
                         max 
                         
                           
                             ( 
                             
                               
                                 
                                   r 
                                   _ 
                                 
                                 l 
                               
                               , 
                               
                                 
                                   r 
                                   _ 
                                 
                                 e 
                               
                             
                             ) 
                           
                           ∈ 
                           
                              
                              
                             
                               ( 
                               
                                 
                                   
                                     r 
                                     _ 
                                   
                                   l 
                                   * 
                                 
                                 , 
                                 
                                   
                                     r 
                                     _ 
                                   
                                   e 
                                   * 
                                 
                               
                               ) 
                             
                           
                         
                       
                        
                       
                         
                            
                           
                             
                               F 
                               _ 
                             
                             
                               v 
                                
                               
                                   
                               
                                
                               1 
                             
                           
                            
                         
                          
                         
                             
                         
                          
                         
                           s 
                           . 
                           t 
                           . 
                           
                               
                           
                            
                           
                             
                               E 
                               _ 
                             
                             l 
                           
                         
                       
                     
                     ≤ 
                     
                       
                         E 
                         l 
                       
                        
                       
                         ( 
                         
                           
                             
                               r 
                               _ 
                             
                             l 
                           
                           , 
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                         
                         ) 
                       
                     
                     ≤ 
                     
                       E 
                       l 
                       u 
                     
                   
                   , 
                   
                     
                       
                         r 
                         _ 
                       
                       l 
                     
                     ∈ 
                     
                       D 
                       s 
                     
                   
                 
               
               
                 
                   ( 
                   37 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       
                         max 
                         
                           
                             ( 
                             
                               
                                 
                                   r 
                                   _ 
                                 
                                 l 
                               
                               , 
                               
                                 
                                   r 
                                   _ 
                                 
                                 e 
                               
                             
                             ) 
                           
                           ∈ 
                           
                              
                              
                             
                               ( 
                               
                                 
                                   
                                     r 
                                     _ 
                                   
                                   l 
                                   * 
                                 
                                 , 
                                 
                                   
                                     r 
                                     _ 
                                   
                                   e 
                                   * 
                                 
                               
                               ) 
                             
                           
                         
                       
                        
                       
                         
                            
                           
                             
                               F 
                               _ 
                             
                             
                               v 
                                
                               
                                   
                               
                                
                               2 
                             
                           
                            
                         
                          
                         
                             
                         
                          
                         
                           s 
                           . 
                           t 
                           . 
                           
                               
                           
                            
                           
                             
                               E 
                               _ 
                             
                             l 
                           
                         
                       
                     
                     ≤ 
                     
                       
                         E 
                         l 
                       
                        
                       
                         ( 
                         
                           
                             
                               r 
                               _ 
                             
                             l 
                           
                           , 
                           
                             
                               r 
                               _ 
                             
                             e 
                           
                         
                         ) 
                       
                     
                     ≤ 
                     
                       E 
                       l 
                       u 
                     
                   
                   , 
                   
                     
                       
                         r 
                         _ 
                       
                       l 
                     
                     ∈ 
                     
                       D 
                       s 
                     
                   
                   , 
                 
               
               
                 
                     
                 
               
             
           
         
       
     
     Given the solutions  F   v1  and  F   v2  to (37), we design  v   e   over  such that the maximum of the ū xe  upper bounds in three phases described above satisfies the requirements: 
     
       
         
           
             
               u 
               l 
               max 
             
             + 
             
               max 
                
               
                 { 
                 
                   
                     
                       ( 
                       
                         
                           C 
                           d 
                         
                         + 
                         
                           
                             k 
                             r 
                           
                           
                             R 
                             e 
                             f 
                           
                         
                       
                       ) 
                     
                      
                     
                         
                     
                      
                     
                       
                         v 
                         _ 
                       
                       e 
                       over 
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     
                       
                         ( 
                         
                           
                             C 
                             d 
                           
                           + 
                           
                             
                                
                               
                                 
                                   F 
                                   _ 
                                 
                                 
                                   v 
                                    
                                   
                                       
                                   
                                    
                                   2 
                                 
                               
                                
                             
                              
                             
                                 
                             
                              
                             
                                
                               
                                 
                                   r 
                                   
                                     _ 
                                     . 
                                   
                                 
                                 l 
                               
                                
                             
                           
                         
                         ) 
                       
                        
                       
                         
                           v 
                           _ 
                         
                         e 
                         over 
                       
                     
                     + 
                     
                       
                          
                         
                           
                             F 
                             _ 
                           
                           
                             v 
                              
                             
                                 
                             
                              
                             1 
                           
                           * 
                         
                          
                       
                        
                       
                           
                       
                        
                       
                         
                           ( 
                           
                             
                               v 
                               _ 
                             
                             e 
                             over 
                           
                           ) 
                         
                         2 
                       
                        
                       
                           
                       
                        
                       and 
                        
                       
                           
                       
                        
                       
                         ( 
                         
                           
                             C 
                             d 
                           
                           + 
                           
                             k 
                             v 
                           
                         
                         ) 
                       
                        
                       
                         
                           
                             v 
                             _ 
                           
                           e 
                           
                             max 
                             , 
                             over 
                           
                         
                         . 
                       
                     
                   
                 
               
             
           
         
       
     
     Safety and Convergence Analysis 
     To ensure that the relative velocity  v   e  is aligned with  v   e   des  when ε enters the blending region    l , the overtaking starts with ε at x e (t 0 ) such that x e (t 0 )&lt;x l (t 0 )−x rep   max −x c , where x c  is the maximum bound on the distance that ε would have traveled along {circumflex over (l)} relative to r d  before it converges to  v   e   des . Integrating (33) yields: 
     
       
         
           
             
               
                 
                   
                     e 
                     _ 
                   
                   
                     v 
                     xe 
                   
                 
                  
                 
                   ( 
                   t 
                   ) 
                 
               
               = 
               
                 
                   
                     
                       e 
                       _ 
                     
                     
                       v 
                       0 
                     
                   
                    
                   
                     ( 
                     
                       1 
                       - 
                       
                         
                           
                             
                               k 
                               v 
                             
                              
                             
                               ( 
                               
                                 1 
                                 - 
                                 
                                   α 
                                   v 
                                 
                               
                               ) 
                             
                           
                            
                           t 
                         
                         
                           
                              
                             
                               
                                 e 
                                 _ 
                               
                               
                                 v 
                                 0 
                               
                             
                              
                           
                           
                             1 
                             - 
                             
                               α 
                               v 
                             
                           
                         
                       
                     
                     ) 
                   
                 
                 
                   1 
                   
                     1 
                     - 
                     
                       α 
                       v 
                     
                   
                 
               
             
             , 
           
         
       
     
     where ē v     0   =ē v     xe   (0). The error ē v     xe   (t) becomes 0 at 
     
       
         
           
             t 
             = 
             
               
                 t 
                 c 
               
               = 
               
                 
                   
                      
                     
                       
                         e 
                         _ 
                       
                       
                         v 
                         0 
                       
                     
                      
                   
                   
                     1 
                     - 
                     
                       α 
                       v 
                     
                   
                 
                 
                   
                     k 
                     v 
                   
                    
                   
                     ( 
                     
                       1 
                       - 
                       
                         α 
                         v 
                       
                     
                     ) 
                   
                 
               
             
           
         
       
     
     and remains zero thereafter. Since  v   xe ≤ v   xe ≤ v   e   over , we have x c =t c   v   e   over . 
     Theorem 2: The velocity  v   e   des  given by Eq. (28) is safe and convergent to r d  in some finite time T f , if ∀t&lt;T f , r d (t)∉   l , and 
     
       
         
           
             
               
                 
                   min 
                   
                     r 
                     ∈ 
                     
                       S 
                       1 
                     
                   
                 
                  
                 
                    
                   
                     
                       r 
                        
                       
                         ( 
                         t 
                         ) 
                       
                     
                     - 
                     
                       
                         r 
                         d 
                       
                        
                       
                         ( 
                         t 
                         ) 
                       
                     
                   
                    
                 
               
               &gt; 
               
                 R 
                 e 
                 f 
               
             
             , 
           
         
       
     
     where S l ={r∈   2 |E l (r)=E l   u } is the boundary of    l . 
     Consider    l   − ={r∈   2 |E l (r)=E l   u +ϵ E } and    l   + ={r∈   2 |E l (r)=E l   u −ϵ E }, with ϵ E &gt;0 arbitrarily small. Then: 
     
       
         
           
             
               
                 ∇ 
                 
                   
                     S 
                     l 
                   
                    
                   
                     ( 
                     r 
                     ) 
                   
                 
               
               = 
               
                 [ 
                 
                   
                     
                       
                         2 
                          
                         n 
                       
                       
                         x 
                         - 
                         
                           x 
                           l 
                         
                       
                     
                      
                     
                       
                          
                         
                           
                             x 
                             - 
                             
                               x 
                               l 
                             
                           
                           a 
                         
                          
                       
                       
                         2 
                          
                         n 
                       
                     
                   
                   , 
                   
                     
                       
                         2 
                          
                         
                             
                         
                          
                         n 
                       
                       
                         y 
                         - 
                         
                           y 
                           l 
                         
                       
                     
                      
                     
                       
                          
                         
                           
                             y 
                             - 
                             
                               y 
                               l 
                             
                           
                           b 
                         
                          
                       
                       
                         2 
                          
                         
                             
                         
                          
                         n 
                       
                     
                   
                 
                 ] 
               
             
             , 
             
               = 
               
                 
                   ∇ 
                   
                     
                       S 
                       l 
                       + 
                     
                      
                     
                       ( 
                       r 
                       ) 
                     
                   
                 
                 = 
                 
                   
                     ∇ 
                     
                       
                         S 
                         l 
                         - 
                       
                        
                       
                         ( 
                         r 
                         ) 
                       
                     
                   
                   . 
                 
               
             
           
         
       
     
     From the definition of F, we have ∇S l   − F=∇S l F d , ∇S l F=∇S l F d . For ∇ l   + F, consider following cases: 
     1) Let r∈S + s. t. ((r−r d )×t l (r))·{circumflex over (k)}&gt;0, then by definition ∇S l F l =0. This implies that ∇S l   + F=(1−σ(E l ))∇S l   + F d . This gives us (∇S l   − F)(∇S l   + F)&gt;0, meaning that the integral curves cross the surface S l . Let ∇S l   + F&gt;0, this implies ∇S l F&gt;0. This contradicts the fact that the integral curves cross S l . Then ∇S l   + F&lt;0, which means that the integral curves approach the boundary T l ={r∈   2 |E l (r)=Ē l } of    l . Let T l   − ={r∈   2 |E l (r)=Ē l +ϵ E }. We have ∇T l   − F=∇S l   + F&lt;0 and ∇T l F=0 because on T l  we have σ(Ē l )=1. This implies that the integral curves do not cross T l  and keep sliding over T l  until reaching    l . Since, by definition, the vector field in    l  points toward r d  that is situated outside    l , one can show using a similar analysis that the integral curves exit    l . 
     (2) ((r−r d )×{circumflex over (t)} l (r))·{circumflex over (k)}≤0. By definition, the vector fields F l  and F d  both point towards the desired location, which implies F=F d . Then, using a similar analysis as in the previous case, it can be shown that the integral curves exit the switching surface S l  in this region. This proves that F is safe and so is  v   e   des.  Now, since the blending region    l  is finite, the integral curves of F inside    l  are of finite length. Then, an object moving with nonzero speed along the vector field F exits the blending region    l  in finite time. Further, we have that outside    l , the vector field F is convergent to r d ∉   l  by definition. Thus, given a non-zero speed and by moving along F, there exists a finite time T 1  such that ∥ r   e ∥=R e   f . At this instance, 
     
       
         
           
             
               F 
               ^ 
             
             = 
             
               
                 - 
                 
                   
                     r 
                     _ 
                   
                   e 
                 
               
               
                  
                 
                   
                     r 
                     _ 
                   
                   e 
                 
                  
               
             
           
         
       
     
     because σ(E l (r e ))=0. This renders the relative position dynamics as:  {dot over (r)}   e =−k r   r   e ∥ r   e ∥ α     r     −1 . The equilibrium r e =r d  of this system is finite-time stable as shown in Ref. (18, Lemma 1). Let T 2  be time for  r   e (t) to converge to 0 with  r   e (0)=R e   f . Then, r e  converges to r d  in finite time T f =T 1 +T 2  with  v   e = v   e   des . 
     Case Study and Simulation Results 
     We apply our proposed intention estimation approach to the overtaking scenario when ε is behind   in the same lane, where the underlying goal is to detect the intention of  . The parameters of  &#39;s intention are shown in Table 1, while the initial conditions and state and input bounds during the intention estimation are given in Table 2. The computed optimal input from Proposition 1 is 
     
       
         
           
             
               u 
               e 
             
             = 
             
               
                 [ 
                 
                   
                     
                       0 
                     
                     
                       0 
                     
                     
                       0 
                     
                     
                       0 
                     
                   
                   
                     
                       0.7919 
                     
                     
                       0.7919 
                     
                     
                       0 
                     
                     
                       0 
                     
                   
                 
                 ] 
               
               . 
             
           
         
       
     
     Then, this optimal separating input is implemented in real-time while running the model selection algorithm in Proposition 2 to identify the intention of  . 
     
       
         
           
               
             
               
                 TABLE 1 
               
             
            
               
                   
               
               
                 Parameters in intentions (5) 
               
            
           
           
               
               
               
               
               
               
            
               
                 Parameter 
                 Value 
                 Parameter 
                 Value 
                 Parameter 
                 Value 
               
               
                   
               
            
           
           
               
               
               
               
               
               
            
               
                 K A, 1   
                 0.1 
                 K C, 1   
                 −0.5 
                 δ A   
                 [−0.1, 0.1] 
               
               
                 K A, 2   
                 0.002 
                 K C, 2   
                 −0.04 
                 δ C   
                 [−0.1, 0.1] 
               
               
                 K 0   
                 0.1 
                 h min [m] 
                 4 
                 h max [m] 
                 32 
               
               
                   
               
            
           
         
       
     
     
       
         
           
               
             
               
                 TABLE 2 
               
             
            
               
                   
               
               
                 Initial conditions and parameters in intention 
               
               
                 estimation example 
               
            
           
           
               
               
               
               
               
            
               
                   
                 Parameter 
                 Value 
                 Parameter 
                 Value 
               
               
                   
                   
               
               
                   
                 
                   
                     
                       
                         
                           
                             v 
                             xe 
                           
                            
                           
                             ( 
                             0 
                             ) 
                           
                         
                          
                         
                           [ 
                           
                             m 
                             s 
                           
                           ] 
                         
                       
                     
                   
                 
                 [22, 26] 
                 
                   
                     
                       
                         
                           v 
                           xe 
                         
                          
                         
                           [ 
                           
                             m 
                             s 
                           
                           ] 
                         
                       
                     
                   
                 
                  [0, 34] 
               
               
                   
                   
               
               
                   
                 y e (0) [m] 
                 [1.85, 2.15] 
                 y e  [m] 
                 [0.9, 2.8] 
               
               
                   
                   
               
               
                   
                 h(0) [m] 
                 [10, 20] 
                 
                   
                     
                       
                         
                           v 
                           xl 
                         
                          
                         
                           [ 
                           
                             m 
                             s 
                           
                           ] 
                         
                       
                     
                   
                 
                 [10, 28] 
               
               
                   
                   
               
               
                   
                 
                   
                     
                       
                         
                           
                             v 
                             xl 
                           
                            
                           
                             ( 
                             0 
                             ) 
                           
                         
                          
                         
                           [ 
                           
                             m 
                             s 
                           
                           ] 
                         
                       
                     
                   
                 
                 [21, 23] 
                 
                   
                     
                       
                         
                           u 
                           xe 
                         
                          
                         
                           [ 
                           
                             m 
                             
                               s 
                               2 
                             
                           
                           ] 
                         
                       
                     
                   
                 
                 [−4.2, 4.2]  
               
               
                   
                   
               
               
                   
                 δt [s] 
                 0.25 
                 
                   
                     
                       
                         
                           v 
                           ye 
                         
                          
                         
                           [ 
                           
                             m 
                             2 
                           
                           ] 
                         
                       
                     
                   
                 
                 [−2.5, 2.5]  
               
               
                   
                   
               
               
                   
                 
                   
                     
                       
                         
                           u 
                           xl 
                         
                          
                         
                           [ 
                           
                             m 
                             
                               s 
                               2 
                             
                           
                           ] 
                         
                       
                     
                   
                 
                 [−4.2, 4.2]  
                 
                   
                     
                       
                         
                           v 
                           xl 
                           des 
                         
                          
                         
                           [ 
                           
                             m 
                             s 
                           
                           ] 
                         
                       
                     
                   
                 
                 22 
               
               
                   
                   
               
            
           
         
       
     
     Next, we provide simulation results for three case studies that demonstrate the efficacy of the proposed intention-aware overtaking algorithm for ε in the presence of O: 
     Case 1: The intention of   is not known (i.e., i=N). The control law for ε is u xd =u xl,N0 +δ N   min =C d v tr   avg +δ N   min . 
     Case 2: The intention of   is known and the driver is cautious (i.e., i=C), then u xd =u xl,C0 +δ C   min . 
     Case 3: The intention of   is known and the driver is annoying (i.e., i=A), then u xd =u xl,A0 +δ A   min . 
     For the chosen r d (t 0 )=[x 1 (t 0 )+| x   l (t 0 )|y e (t 0 )] T , we calculate the worst-case time and x-distance traveled by ε before it overtakes  , assuming maximum relative velocity of   relative to r d , i.e.,  v   xl = v   xl   max . Denote the worst-case time and x-distance by Δt W  and Δx e,W , respectively, see  FIG. 4 , and the corresponding actual values as Δt and Δx e , respectively. 
     The ego vehicle ε is safe with respect to the lead vehicle   as established in the previous section. To ensure that it is also safe with respect to O, we require ∥r e (t)−r o (t)∥ E     ∞   &gt;1 for all t∈[t 0 , t 0 +Δt W ]. Then, if r o (t)∉Z 1 ={(r o ∈ |x e (t 0 )&lt;x 0 (t)&lt;x e (t 0 )+Δx e,W }, we have ∥r e (t)−r o (t)∥ E     ∞   &gt;1, for all t∈[t 0 , t 0 +Δ W ]. For this to hold, O should start outside the region Z 1 ∪Z 2 , shown in  FIG. 4 , so that O does not enter Z 1  during the worst-case overtake maneuver of E, whose trajectory is denoted with the blue curve. This implies that (X 0 (t 0 )&lt;x e (t 0 )) OR (x 0 (t 0 )−x e (t 0 ))&gt;Δx e,W +Δt W |v xo (t 0 )|), i.e., O requires at least Δt W  seconds to enter the unsafe zone Z 1  if it starts with x e (t 0 )&lt;x o (t 0 ) or it has already crossed Z 1  and thereby renders the overtake by ε safe. 
     Table 4 shows Δt, Δx, Δt W , Δx W  and the cumulative control effort during the overtake, U=∫ t     0     t     0     +Δt u xe  for the three cases with the initial conditions for ε,  , O, as given in Table 3. It can be observed that, when there is no estimation of the intention of  , then it is deemed unsafe for ε to overtake for the given initial conditions. However, if ε estimates the intention as in Cases 2 and 3, then overtaking is deemed safe. Additionally, when the  &#39;s driver is cautious, the x-distance that ε traveled to overtake   and the control effort during the overtake U is smaller than the corresponding values when the  &#39;s driver is annoying. This is expected because the cautious driver slows down to ease out the overtaking maneuver of ε. Intention estimation helps in effectively deciding whether to overtake or not, and also in guiding ε smoothly to overtake   in the presence of O. 
       FIGS. 5A and 5B  show the oo-distances between ε and  , and between ε and O, respectively, for the Case 3 when  &#39;s driver is annoying. As observed in  FIGS. 5A and 5B , the o-distances are always greater than 1, demonstrating that there is no collision between ε and the other two vehicles.  FIG. 6A  shows the 2-norm distance between ε and r d , which goes to zero in finite time, and hence ε converges to the desired dynamics (26) in finite time. Moreover,  FIG. 6B  shows that control input applied by ε is within the control bounds for all times. 
     
       
         
           
               
             
               
                 TABLE 3 
               
             
            
               
                   
               
               
                 Initial conditions and other parameters 
               
            
           
           
               
               
            
               
                 Initial Conditions 
                 Other Parameters 
               
            
           
           
               
               
               
               
               
               
            
               
                   
                 j = e 
                 j = l 
                 j = 0 
                 Parameter 
                 Value 
               
               
                   
                   
               
            
           
           
               
               
               
               
               
               
            
               
                 x j (0) [m] 
                 −22.96 
                 0 
                 835.19 
                 [δ N   min , δ N   max ] 
                 [−0.15, 0.15] 
               
               
                 y j (0) [m] 
                 1.85 
                 2.8 
                 5.55 
                 
                   v 
                   e 
                   over 
                 
                 2.5 
               
               
                 ν xj (0) [m] 
                 22 
                 22 
                 −22 
                 R e   f   
                 3 
               
               
                   
                   
                   
                   
                 |u e   max, over | 
                 8.7 
               
            
           
           
               
            
               
                 Road Parameters 
               
            
           
           
               
               
               
               
               
               
               
               
            
               
                 y lane   
                 3.7 
                 l c   
                 4 
                 w c   
                 1.8 
                 C d   
                 0.15 
               
               
                   
               
            
           
         
       
     
     
       
         
           
               
             
               
                 TABLE 4 
               
             
            
               
                   
               
               
                 Comparison of 3 cases (x 0 (t 0 ) − x e (t 0 ) = 1335) 
               
            
           
           
               
               
               
               
               
               
            
               
                   
                   
                   
                 Is 
                   
                   
               
               
                 Case 
                 Δx e, W [m] 
                 Δx o, W   
                 overtake 
               
               
                 (Intent) 
                 (Δt W [s]) 
                 [m] 
                 safe? 
                 Δx e [m](Δt[s]) 
                 U 
               
               
                   
               
            
           
           
               
               
               
               
               
               
            
               
                 1 (N) 
                 1588.10 (65.45)  
                 1439.90 
                 No 
                 — 
                 — 
               
               
                 2 (C) 
                 650.14 (29.34) 
                 645.48 
                 Yes 
                 559.97 (25.07) 
                 86.30 
               
               
                 3 (A) 
                 714.86 (29.34) 
                 645.48 
                 Yes 
                 616.23 (25.08) 
                 94.98 
               
               
                   
               
            
           
         
       
     
     Example 1 
     Referring now to  FIG. 7 , an exemplary embodiment of a overtaking with intention estimation system  700  is shown. The system includes a plurality of sensors are coupled to the ego vehicle  705 . The sensors can sense information associated with the ego vehicle  705 , a lead vehicle  750 , and/or an oncoming vehicle  760 . The lead vehicle  750  may be traveling on a roadway in the same direction and lane as the ego vehicle  705 . The oncoming vehicle  760  may be traveling in the opposite direction of the ego vehicle  705 , and may be traveling in a different lane of the roadway. The plurality of sensors can include a first sensor  710  that can be a speedometer global positioning system sensor, or other applicable sensor configured to sense a speed and/or velocity of the ego vehicle  710 . 
     The first sensor can be coupled to a controller  740  having a memory and a processor and coupled to the ego vehicle  705 . The controller  740  can have an overtaking with intention estimation algorithm stored in the memory, which will be explained in detail in  FIG. 8 . The controller  740  can be coupled to a vehicle control system (not shown) of the ego vehicle  705 . The vehicle control system can be an autonomous or semi-autonomous vehicle control system with any number of controllers, interfaces, actuators, and/or sensors capable of controlling a motor, engine, transmission, braking system, steering system, or other subsystem of the ego vehicle. The vehicle control system can be used to perform a vehicle maneuver such as overtaking a lead vehicle  750 , stopping the ego vehicle  705 , or following the lead vehicle  750 . In some embodiments, the controller  740  may be a portion of the vehicle control system. 
     The plurality of sensors can include a second sensor  720  coupled to the controller  740  and configured to sense surroundings of the ego vehicle  705 . The second sensor  720  can be a sensor such as a LiDAR sensor, a camera such as an infrared camera or visible light camera, an ultrasonic sensor, a radar sensor, or any other type of sensor capable of sensing the location, speed, and or velocity of objects around the ego vehicle  705 . The second sensor  720  may sense information about a location, speed, or velocity of the lead vehicle  750  or oncoming vehicle  760 . The information, either directly or indirectly, may be used by the controller  740  to calculate a location of the lead vehicle  750  relative to the ego vehicle  705 , a headway distance between the lead vehicle  750  and the ego vehicle  705 , a lateral velocity or acceleration of the lead vehicle  750 , a longitudinal velocity or acceleration of the lead vehicle  750 , a location of the oncoming vehicle  760  relative to the ego vehicle  705 , a lateral velocity or acceleration of the oncoming vehicle  760 , or a longitudinal velocity or acceleration of the oncoming vehicle  760 , a lateral location of the ego vehicle  705 , and/or a lateral location of the lead vehicle  750 . Lateral location can be the location of the vehicle within a lane or the location along the j direction shown in  FIG. 1 , while longitudinal location can be the location of the vehicle within a lane or the location along the f direction shown in  FIG. 1 . The second sensor  720  can be capable of sensing a speed or velocity of an object natively. Alternatively, the speed or velocity of the object can be calculated by the controller  740  information sensed by the sensor using methods known in the art, such as deriving a velocity of a vehicle from location information sensed by a LiDAR sensor. 
     Any number of first sensors  710  and second sensors  720  can be coupled to the ego vehicle  705  in order to improve the speed, velocity, and/or object location sensing capabilities of the ego vehicle  705 . For example, multiple second sensors  720   a  and  720   b  can be mounted to the front of the ego vehicle  705 . The second sensors  720  may include different sensor types, i.e., some of the second sensors  720  are cameras while others are LiDAR sensors. The plurality of sensors can be divided up as a number of sub-pluralities of sensors, i.e. a first plurality of sensors, a second plurality of sensors, and a third plurality of sensors. Some of the sub-pluralities of sensors may share sensors, i.e. a sensor may belong to the first plurality of sensors and the second plurality of sensors. It is contemplated that a single sensor capable of sensing all of the parameters described above could be used in place of the first sensor  710  and second sensor  720 . Additionally, multiple controllers  740  may be used in order to implement the overtaking with intention estimation system  700 . 
     Referring now to  FIG. 7  as well as  FIG. 8 , an exemplary embodiment of process  800  for implementing an overtaking with intention estimation algorithm is shown. Generally, the process performs an input control sequence using a driving control system of an ego vehicle, determines an intention of a lead vehicle, and performs a driving maneuver based on the intention. The process  800  can be implemented as instructions on a memory of a computational device such as the controller  740 . 
     At  804 , the process can determine an optimal input control sequence for the ego vehicle. The input control sequence can include a number of discrete time points, each of which have an associated longitudinal acceleration and lateral velocity value. The input control sequence u T   *  can be determined using equations (15a)-(15d). In some embodiments, the input control sequence may be predetermined, and the process  800  may not include  804 . The process  800  may then proceed to  808 . 
     At  808 , the process  800  can receive information about the lead vehicle and the ego vehicle from a plurality of sensors coupled to the ego vehicle, cause the vehicle control system to execute at least a portion of the input control sequence, and execute a model selection algorithm. The process  800  can cause the vehicle control system to control the ego vehicle have the longitudinally acceleration and the lateral velocity specified by the input control sequence at a given time point. The process  800  can receive lead vehicle data which can be used to directly or indirectly determine a velocity of the lead vehicle, a location of the lead vehicle relative to the ego vehicle, a headway separation distance between the lead vehicle and the ego vehicle, a lateral location of the ego vehicle, and/or a lateral location of the lead vehicle. The process  800  can then provide the lead vehicle data, the ego vehicle data, and/or the velocity of the lead vehicle to the model selection algorithm. The model selection algorithm can include an aggressive vehicle model and a cautious driver model. The vehicle models can include equation (5), with K A,1 &gt;0 and K A,2 &gt;0 for the aggressive vehicle model and K C,1 &lt;0 and K C,2 &lt;0 for the cautious vehicle model. The model selection algorithm can then estimate if the lead vehicle data, the ego vehicle data, and/or the velocity of the lead vehicle has an aggressive intention, or a cautious intention by utilizing the aggressive vehicle model and the cautious vehicle model in conjunction with equations (18a)-(18d). The process  800  can then proceed to  812 . 
     At  812 , the process  800  can determine if the estimated intention of the lead vehicle is unknown. If the intention is unknown, the process  800  can proceed to  808  (i.e. “NO” at  812 ) in order to execute the next time point of the input control sequence. The process  800  is guaranteed to estimate either an aggressive intention or a cautious intention by the end of the input control sequence. If the intention is either aggressive or cautious (i.e. “YES” at  812 ), the process  800  can proceed to  816 . 
     At  816 , the process  800  can receive information about the lead vehicle, the ego vehicle, and an oncoming vehicle from the plurality of sensors coupled to the ego vehicle. The information can be used to directly or indirectly determine a location of the lead vehicle relative to the ego vehicle, a headway distance between the lead vehicle and the ego vehicle, a lateral velocity or acceleration of the lead vehicle, a longitudinal velocity or acceleration of the lead vehicle, a location of the oncoming vehicle relative to the ego vehicle, a lateral velocity or acceleration of the oncoming vehicle, a longitudinal velocity or acceleration of the oncoming vehicle, a longitudinal location of the lead vehicle relative to the ego vehicle, a longitudinal location of the oncoming vehicle relative to the ego vehicle, a lateral location of the oncoming vehicle relative to the ego vehicle, and/or a lateral location of the lead vehicle relative to the ego vehicle. The process  800  may continue to receive at least a portion of the above information during subsequent steps of the process  800 . The process  800  can provide the information received to subsequent steps of the process  800 . The process can then proceed to  820 . 
     At  820 , the process  800  can determine whether the ego vehicle can perform an overtaking maneuver while keeping the ego vehicle within a predetermined set of safety constraints. The safety constraints may include the safety (Definition 1) and safe overtake (Definition 2) definitions found in the “Trajectory Generation and Control For The Ego Vehicle” section of this disclosure. The safety constraints may include a lateral separation distance between the ego vehicle and the lead vehicle, a longitudinal separation distance between the ego vehicle and the lead vehicle, a lateral separation distance between the ego vehicle and the oncoming vehicle, and/or a longitudinal separation distance between the ego vehicle and the oncoming vehicle. The process  800  can calculate a trajectory to navigate the ego vehicle to a desired point, which can be desired point r d , located a safe distance in front of the lead vehicle. The safe distance can be | x   l (t 0 )| which can be found by initializing the safe distance to be x rep   max +R e   f  and updating the safe distance at regular time intervals using equation (34) as described above. The desired point can be subject to dynamics according to equation (26) and the estimated intention of the lead vehicle. The process  800  can calculate the trajectory using a vector field such as vector field F. The vector field can be calculated using equation (25). The vector field can be used to calculate a desired velocity profile for the ego vehicle. The desired velocity profile can be calculated using equation (28). The process may determine if the ego vehicle can pass the lead vehicle by calculating a worst-case trajectory based on a worst-case overtaking time and x-distance which can be Δt W  and Δx e,W . The worst-case time and x-distance can be calculated using a maximum expected lead vehicle velocity such as  v   xl   max , which is the maximum value of a predetermined lead vehicle velocity range. The predetermined lead vehicle velocity range and other vehicle parameter ranges such as those found in Table 2 may be predetermined before the process  800  begins. The process  800  can then estimate the location of the oncoming vehicle during the worst-case trajectory. In some embodiments, the process  800  may estimate the projected position of the oncoming vehicle during the worst-case trajectory by assuming the oncoming vehicle moves with constant velocity. If the oncoming vehicle is determined to violate any of the safety constraints during the worst-case trajectory, the overtaking maneuver may be determined to be unsafe. If the oncoming vehicle is not determined to violate any of the safety constraints during the worst-case trajectory, the overtaking maneuver may be determined to be safe. The process  800  can then proceed to  824 . 
     At  824 , the process  800  can receive the safety determination of the overtaking maneuver. If the overtaking maneuver is determined to be safe (i.e. “YES” at  824 ), the process  800  can proceed to  828 . If the overtaking maneuver is determined to be unsafe (i.e. “NO” at  824 ), the process  800  can proceed to  832 . 
     At  828 , the vehicle can perform the overtaking maneuver while staying within the predetermined safety constraints and receive information about the lead vehicle, the ego vehicle, and an oncoming vehicle from the plurality of sensors coupled to the ego vehicle. The information can be used to directly or indirectly determine a location of the lead vehicle relative to the ego vehicle, a headway distance between the lead vehicle and the ego vehicle, a lateral velocity or acceleration of the lead vehicle, a longitudinal velocity or acceleration of the lead vehicle, a location of the oncoming vehicle relative to the ego vehicle, a lateral velocity or acceleration of the oncoming vehicle, a longitudinal velocity or acceleration of the oncoming vehicle, a longitudinal location of the lead vehicle relative to the ego vehicle, a longitudinal location of the oncoming vehicle relative to the ego vehicle, a lateral location of the oncoming vehicle relative to the ego vehicle, and/or a lateral location of the lead vehicle relative to the ego vehicle. The overtaking maneuver can include maneuvering the ego vehicle to the desired point in front of the lead vehicle using an overtaking trajectory. The overtaking trajectory can be the path taken by the ego vehicle when following a desired velocity profile such as the desired velocity profile in equation (28). The desired velocity profile can be used to determine control inputs for the longitudinal acceleration and lateral velocity of the ego vehicle. The control inputs can be provided to the driving control system in order to pilot the vehicle along the trajectory to reach the desired point. If at any point during the overtaking maneuver the predetermined safety constraints are violated or are projected to be violated, the vehicle control system may perform a safety maneuver in order to protect the ego vehicle. For example, the safety maneuver can be stopping the ego vehicle in response to determining from sensor information that the lead vehicle has stopped. The ego vehicle may then cancel or restart the overtaking maneuver. The process  800  can then proceed to  832 . 
     At  832 , the process  800  can continue driving within the predetermined safety constraints. If the ego vehicle performed the overtaking maneuver, the ego vehicle can perform a cruising maneuver. The cruising maneuver can include continuing to track the desired point with a predetermined constant cruise speed that may be based on an average traffic speed of the roadway. If the ego vehicle did not perform the overtaking maneuver, the ego vehicle may perform a following maneuver. The following maneuver may include following the lead vehicle with a minimum separation distance between the lead vehicle and ego vehicle. Regardless of whether or not the overtaking maneuver was performed, the ego vehicle may continue driving within the predetermined driving safety constraints. The process  800  may then end. In some embodiments, the process  800  could proceed to  804  in order to restart the overtaking process. 
     In some embodiments, any suitable computer readable media can be used for storing instructions for performing the functions and/or processes described herein. For example, in some embodiments, computer readable media can be transitory or non-transitory. For example, non-transitory computer readable media can include media such as magnetic media (such as hard disks, floppy disks, etc.), optical media (such as compact discs, digital video discs, Blu-ray discs, etc.), semiconductor media (such as RAM, Flash memory, electrically programmable read only memory (EPROM), electrically erasable programmable read only memory (EEPROM), etc.), any suitable media that is not fleeting or devoid of any semblance of permanence during transmission, and/or any suitable tangible media. As another example, transitory computer readable media can include signals on networks, in wires, conductors, optical fibers, circuits, or any suitable media that is fleeting and devoid of any semblance of permanence during transmission, and/or any suitable intangible media. 
     It should be noted that, as used herein, the term mechanism can encompass hardware, software, firmware, or any suitable combination thereof. 
     It should be understood that the above described steps of the processes of  FIG. 8  can be executed or performed in any order or sequence not limited to the order and sequence shown and described in the figures. Also, some of the above steps of the processes of  FIG. 8  can be executed or performed substantially simultaneously where appropriate or in parallel to reduce latency and processing times. 
     Thus, the invention provides an improved method of overtaking of a lead vehicle by an autonomous ego vehicle in the presence of an oncoming vehicle. 
     Although the invention has been described in considerable detail with reference to certain embodiments, one skilled in the art will appreciate that the present invention can be practiced by other than the described embodiments, which have been presented for purposes of illustration and not of limitation. Therefore, the scope of the appended claims should not be limited to the description of the embodiments contained herein. 
     REFERENCES 
     
         
         [1] J. Van Brummelen, M. O&#39;Brien, D. Gruyer, and H. Najjaran, “Autonomous vehicle perception: The technology of today and tomorrow,” Transportation research part C: emerging technologies, 2018. 
         [2] W. Schwarting, J. Alonso-Mora, and D. Rus, “Planning and decision-making for autonomous vehicles,” Annual Review of Control, Robotics, and Autonomous Systems, 2018. 
         [3] S. Lefivre, D. Vasquez, and C. Laugier, “A survey on motion prediction and risk assessment for intelligent vehicles,” Robomech Journal, vol. 1, no. 1, p. 1, 2014. 
         [4] C. Katrakazas, M. Quddus, W.-H. Chen, and L. Deka, “Realtime motion planning methods for autonomous on-road driving: State-of-the-art and future research directions,” Transportation Research Part C: Emerging Technologies, vol. 60, pp. 416-442, 2015. 
         [5] N. Murgovski and J. Sjöberg, “Predictive cruise control with autonomous overtaking,” in 2015 IEEE 54th Conference on Decision and Control, 2015, pp. 644-649. 
         [6] J. F. Fisac, E. Bronstein, E. Stefansson, D. Sadigh, S. S. Sastry, and A. D. Dragan, “Hierarchical game-theoretic planning for autonomous vehicles,” arXiv:1810.05766, 2018. 
         [7] A. Carvalho, Y. Gao, S. Lefevre, and F. Borrelli, “Stochastic predictive control of autonomous vehicles in uncertain environments,” in 12th Int. Sym. on Advanced Vehicle Control, 2014. 
         [8] G. Cesari, G. Schildbach, A. Carvalho, and F. Borrelli, “Scenario model predictive control for lane change assistance and autonomous driving on highways,” IEEE Intelligent Transportation Systems Magazine, vol. 9, no. 3, pp. 23-35, 2017. 
         [9] M. Wang, Z. Wang, S. Paudel, and M. Schwager, “Safe distributed lane change maneuvers for multiple autonomous vehicles using buffered input cells,” in 2018 IEEE International Conference on Robotics and Automation, May 2018, pp. 1-7. 
         [10] R. Nikoukhah and S. Campbell, “Auxiliary signal design for active failure detection in uncertain linear systems with a priori information,” Automatica, vol. 42, no. 2, pp. 219-228, February 2006. 
         [11] J. K. Scott, R. Findeison, R. D. Braatz, and D. M. Raimondo, “Input design for guaranteed fault diagnosis using zonotopes,” Automatica, vol. 50, no. 6, pp. 1580-1589, June 2014. 
         [12] Y. Ding, F. Harirchi, S. Z. Yong, E. Jacobsen, and N. Ozay, “Optimal input design for affine model discrimination with applications in intention-aware vehicles,” in ACM/IEEE International Conference on Cyber-Physical Systems, 2018, available from: arXiv:1702.01112. 
         [13] K. Singh, Y. Ding, N. Ozay, and S. Z. Yong, “Input design for nonlinear model discrimination via affine abstraction,” in IFAC World Congress, vol. 51, no. 16, 2018, pp. 175-180. 
         [14] Gurobi Optimization Inc., “Gurobi optimizer reference manual,” 2015. 
         [Online]. Available: http://www.gurobi.com 
         [15] IBM ILOG CPLEX, “V12. 1: User&#39;s manual for CPLEX,” Int. Business Machines Corporation, vol. 46, no. 53, p. 157, 2009. 
         [16] F. Harirchi, S. Z. Yong, E. Jacobsen, and N. Ozay, “Active model discrimination with applications to fraud detection in smart buildings,” in IFACWorld Congress, vol. 50, no. 1, 2017, pp. 9527-9634. 
         [17] R. Volpe and P. Khosla, “Manipulator control with superquadric artificial potential functions: Theory and experiments.” IEEE Transactions on Systems, Man, and Cybernetics, vol. 20, no. 6, pp. 1423-1436, 1990. 
         [18] K. Garg and D. Panagou, “New results on finite-time stability: Geometric conditions and finite-time controllers,” in 2018 American Control Conference, June 2018, pp. 442-447. 
         [19] D. Panagou, “A distributed feedback motion planning protocol for multiple unicycle agents of different classes,” IEEE Trans. on Automatic Control, vol. 62, no. 3, pp. 1178-1193, 2017. 
       
    
     The citation of any document is not to be construed as an admission that it is prior art with respect to the present invention.