Reference Tracking Optimization With Obstacle Avoidance via Task Prioritization for Automated Driving

Obstacle avoidance is a fundamental operation for automated driving and its formulation traditionally originates from robotics and decision making control fields. Given the high complexity required to compute an obstacle-free trajectory, this operation is usually demanded to a lower frequency planning layer that provides then a trajectory reference to be followed by a higher frequency control layer. As a result, whenever replanning is needed (for example, due to a new detected obstacle), the control layer must wait for a new planned trajectory to be generated. In this paper, we propose a novel methodology to approach obstacle avoidance already in the control layer, which allows a prompter response. In particular, we show how obstacle avoidance and reference tracking can be integrated, thus with no need to switch among different controllers, based on a null-space based behavioral control approach, implemented in a (possibly nonlinear) model predictive control scheme. We demonstrate practical implementation of the proposed methodology employing two different vehicle dynamic models and in four different (urban and highway) scenarios. Furthermore, we provide a sensitivity analysis to understand how parameters choice affects the automated vehicle behavior.


I. INTRODUCTION
A UTOMATED Vehicles (AVs) are making their way through the automotive industry, while vast efforts are devoted to research for safety [1], [2], common practices, and new technologies [3].Among the latter, a considerable amount of work is related to sensing the environment, e.g., 3D object detection [4] and sensor fusion [5].Likewise, advances in communication technologies, such as 5G [6], allow vehicular communications [7], [8] giving rise to Connected and Automated Vehicles (CAVs).
CAVs are embraced with enthusiasm because of their potential improvement in terms of energy efficiency, environmental sustainability [9], as well as safety impacts [10], [11].At the same time, however, new and more complex challenges arise for the scientific and industrial community, such as how to integrate them with conventional (human-driven) vehicles, guarantee connectivity, update (physical and digital) infrastructures, integrate within traffic management strategies, and develop vehicle real-time planning and control strategies (see, e.g., [12], [13], [14], [15]).
A typical architecture for AV operations consists in a hierarchical structure, where the trajectory/path planning layer, which aims at generating a feasible route given a goal and constraints dependent on the surrounding environment, is implemented in the upper layer; whereas, a reference tracking control layer, aiming at continuously minimizing the error with respect to the planned trajectory, is implemented in the lower layer [16].
Finding a collision-free path or trajectory for a mobile agent has always been a topic of concern, because of the increasing interest in mobile robots for various purposes such as warehousing, automation industry, rough terrain exploration, as well as AVs (see, e.g., [17], [18], [19]).Typically, for applications such as AVs, similarly as for other mobile robots, Obstacle Avoidance (OA) is handled at the higher level of control in a hierarchical control system [20].In such settings, collision avoidance is more often treated as a trajectory planning problem that provides the (lower level) control loop with a collision-free trajectory or path for Reference Trajectory Tracking (RTT).As a result, whenever a new obstacle is detected, or when a monitored dynamic obstacle has a discrepancy with respect to its predicted behavior, a trajectory replanning procedure is needed in the guidance loop.This implies that the agent's response to the surrounding environment is paced by the planning loop, which is typically slower than the low-level control loop, due to the high complexity of such process, caused by the non-linearity of the involved models and the non-convexity of the resulting search problems [21].To address trajectory planning, various strategies, aiming at solving a collision-free trajectory problem, have been proposed in different fields of mobile robotics [22].
A widely used technique for OA is the Artificial Potential Field approach (see, e.g., [23], [24], [25], [26]), which considers the agent moving in a field of forces, where the target position is modeled as a minimum, while obstacles are maxima.Obstacles may represent either objects that must be avoided or rules that should be abided ( [27], [28], [29]).This intuitive, yet elegant, method is not free from problems and, in [30], several issues were identified during experiments, mainly due to attraction to local minima and oscillatory behavior.
Moreover, search algorithms can be applied for obstacle-free path planning, with the drawback of having to cope with the high-dimensional states.The introduction of RRT [31] and subsequent variants, including, in particular, randomized algorithms [32], contributed to overcome such issues.
Another well-known search method for OA management relies on the idea of Velocity Obstacle [33].This method consists in selecting avoidance maneuvers generated by agent velocities outside of the velocity obstacle set, which includes velocities that would lead to a collision with a given obstacle at some point in the future.The trajectory from start to goal is computed by searching a tree of feasible avoidance maneuvers, computed at discrete time intervals.Different variants of this technique have been further proposed, including [34], where the Reciprocal Velocity Obstacle concept was introduced for real-time multi-agent navigation.This new version assumes that the other agents react by running a similar collisionavoidance algorithm, guaranteeing safe and oscillation-free motion for all agents.
Reachability-based methods have also been investigated especially in an attempt to bridge the gap between safety and real-time performance (see, e.g., [35], [36]).Another example is [37], where funnel libraries are utilized to compute a suitable trajectory while taking into account uncertainties.
Nonlinear Model Predictive Control (NMPC) [38] has been exploited in many research works (e.g., [39], [40]), due to its ability in managing nonlinear constraints in optimization problems, given by the dynamics of the vehicle and the geometry of the OA task.In particular, for trajectory optimization, Mixed-Integer Linear Program (MILP) has been explored [41], where integer variables are used to define a geometrical area around an agent at a given time instant, not to be accessed by any other one.Nevertheless, integer variables in an optimization problem complicate the process of finding a suitable solution, thus the viability of such a method is subordinate to the specific solver in use and the problem size [42].
Neural networks inspired other approaches for path planning, considering both static and moving obstacles, aiming at making the decision process faster [43] and closer to optimal [44].
Recent research works were devoted to improve the performance of vehicles interacting with systems including uncertainty.Among these, various approaches were investigated, including: robust MPC [45], Gaussian probability distribution propagating along the nominal path [46], Markov processes [47], [48], Monte Carlo dropout and bootstrapping [49], and multi-modal constraint uncertainty [50].
Finally, reactive path following is an alternative approach for the OA problem, where decisions are made directly at the control lower level.This methodology usually comes with a behavior-switching logic [51] that selects which algorithm to trigger among a library of different options, including go-togoal, collision avoidance, etc.
A novel approach, which slightly differs from traditional behavior-switching logic, was presented in [52], where the Null-Space-Based (NSB) behavioral control was employed to demonstrate how a proper composition of outputs based on single elementary behaviors may be effective.The key point is sorting tasks by their priority, so that the input of each task is projected into the null-space of the higher-priority tasks.A stability analysis of such a technique is provided in [53], while another remarkable result in this context is discussed in [54], where an input saturation management procedure was used for a non-holonomic mobile robot.This latter method derives from an effective intuition on how tasks input with different priorities can be properly saturated so that they: a) are compliant with the system physical constraints; and b) do not cause degradation of higher tasks performance [55].
In this paper, we propose a novel approach to achieve prompt, i.e., real-time, reactions to unplanned situations in the context of vehicle control in road traffic.The proposed method is composed of two ingredients, namely: a) an NSB OA algorithm that determines proper velocities to perform evasive maneuvers whenever obstacles are detected; and b) the formulation of an NMPC problem where the underlying optimization problem is subject to proper constraints taking into account the velocities computed by the NSB OA algorithm.Our contribution to this paper is fourfold: a) we design a novel methodology to integrate an NSB OA algorithm with reference tracking within an NMPC framework; b) we propose a novel formulation to identify the minimum safety distance in a 2D environment; c) we present how such a combination of techniques can be implemented with different (vehicle) dynamic systems to tackle different realistic road traffic scenarios; d) we show the applicability of the proposed technique to react promptly to abrupt obstacles with no need for an immediate replanning of the reference trajectory.
In addition, it is worth noting that previous research works have typically tested the NSB approach for autonomous mobile robots whose physical and environmental constraints are less restricting than those of a CAV.Therefore, to the best of our knowledge, this is the first implementation of such a technique considering road vehicles.The proposed minimum safety distance gives the possibility to compute a 2D distance and to employ it without much of a computation burden.Indeed, a vast variety of spacing policies are available for the sole longitudinal control [56], and, hence, they are suitable for 1D operations.On the other hand, potential fields, which are usually employed in lane-changing decision-making (as for references above), consist of articulated formulations to be managed by the optimization problem.Conversely, our approach identifies a 2D spacing policy to return a simple constant value before computing an instance of the optimization problem.If such a minimum safety distance is violated, then the OA constraint is constructed.An earlier version of this work appeared in [57].Here, we extend such work by i) considering both a kinematic and a dynamic bicycle model for the controlled vehicle; ii) including sensitivity analysis simulation experiments; iii) performing a comparison with a state-of-the-art approach; and iv) showing the advantage of using the proposed approach in terms of computation time, improving the presentation regarding practical implementation, and considering additional, more realistic traffic scenarios for different circumstances that might be encountered.This paper is structured as follows.In Section II, we present the formulation of the proposed task priority management method and we describe how to apply it for obstacle avoidance; in Section III, we show how the two components of this method are integrated.In Section IV, we show how to implement our method for two different (vehicle) dynamic models; while in Section V, we present and discuss simulation results.Finally, Section VI summarizes the main findings of this paper and introduces future challenges.

II. TASK PRIORITY
In order to manage both trajectory tracking and collision avoidance operations according to priorities, we adopt the tasks formulation proposed in [52].First, the general mathematical formulation of a task is given (Section II-A), then it is applied to the OA and stopping operations (Sections II-B and II-C).Sections II-D and II-E propose strategies to combine both operations, as well as some operating assumptions.

A. Mathematical Background
Let us introduce σ (t) ∈ R m as the task variable to be controlled and q(t) ∈ R n as the system configuration vector, where n and m are proper state and task dimensions.We can express the task vector as a function of the system configuration, namely while its time derivative is where J (q(t)) ∈ R m×n is the configuration-dependent task Jacobian matrix and q(t) ∈ R n is the system velocity.We can then introduce the task error vector as where the dependencies are dropped for the sake of readability, and σ des ∈ R m is the vector of desired values for the task related to the motion reference q des ∈ R n .In order to avoid the well-known problem of numerical drift, the system desired velocity can be expressed in a Closed Loop Inverse Kinematics (CLIK) version [58] as where J # may be either the transpose or the pseudoinverse of J , i.e., either J # = J ⊺ or J # = J + = J ⊺ (J J ⊺ ) −1 , depending on the adopted strategy, and ∈ R m×m is a positive-definite gain matrix.
Let us turn our attention to a regulation problem where N T > 1 tasks are to be fulfilled.Denoting tasks as σ i , characterized by σ i = 0, for i = 1, . . ., N T , we can rewrite (4) as where is a projector in the null space of Ji−1 , I n×n is the n × n identity matrix, and Ji−1 is the augmented task Jacobian, i.e., which allows us to perform a so-called internal motion, that is, we generate a motion of a robotic system without affecting that of primary tasks [59].

B. Obstacle Avoidance
The proposed method abstracts the concept of an obstacle, which may be whichever type of obstruction a vehicle may find in its planned path, be it either idle or moving, a vehicle or a person, a lane edge, or any other signal.The only care is to make sure that the OA task related to such an obstacle has a higher priority than the RTT task.Should the ego-vehicle detect collisions with more than one obstacle, we may want to consider: a) only the most dangerous one; b) all of them using (5) and treating RTT as the lowest priority task while the more dangerous an obstacle is, the higher priority it has in the OA algorithm; c) a combination of the previous strategies, e.g., considering only some of the obstacles, etc., where the closer to collision an obstacle is, the more dangerous it is considered.In Section V, whenever needed, we consider as a primary task the avoidance of another vehicle, while as a secondary task the avoidance of the road boundaries.Such a choice is motivated by solid results in the literature, including, e.g., [52] and [54], thus, we do not focus further on this aspect.On the other hand, we do focus on the novel part, which involves the implementation of this approach in NMPC to perform our simulations.
The obstacle avoidance operation can be tackled by imposing an adequate distance between the ego-vehicle and the obstacle.Such a distance should be carefully computed taking into account the position, speed, and orientation of both.It should be stressed that minimum safety distances for vehicle control are typically constructed to perform longitudinal control operations, such as, e.g., adaptive cruise control.Therefore, while well-known methods are addressing 1-D scenarios, we formulate here a novel method suitable for 2-D applications.To accomplish this, we consider two ellipses to define forbidden areas: one of them centered at the center of gravity of the ego-vehicle and the other one centered at the center of gravity of the obstacle.Ellipses are oriented so that the major axis lies on the yaw direction of the object they refer to.If the object is static, the yaw direction can be chosen arbitrarily, e.g., the direction pointing towards the controlled vehicle.The semi-major and semi-minor axes of the ellipse are denoted as L ego and l ego when referred to the ego-vehicle, and L obs and l obs when referred to the obstacle, respectively.Omitting the dependence on the time step, we define L ego := 0.5 s 0 + v ego t + 0.5 a ego 2 t and l ego := 0.5 s 0 for the ego-vehicle, and L obs := 0.5 s 0 + v obs t + 0.5 a obs 2 t and l obs := 0.5 s 0 for the obstacle, where s 0 is a minimum safety distance, t is the time sample, v ego and a ego are the speed and acceleration of the ego-vehicle, respectively, and v obs and a obs are the speed and acceleration of the obstacle, respectively.Therefore, the resulting safety distance to be maintained (see Fig. 1 for a clarification) is where (•) is a saturation function defined as variable θ ego (θ obs ) denotes the angle between the position of the obstacle (ego-vehicle) and the orientation of the egovehicle (obstacle); and θ max is a maximum range angle encompassing the feasible region of future input.Note that the formula provided as an argument to (•) in ( 8) is the distance between the center of an ellipse and the point on such ellipse positioned at a θ i angle.In addition, notice that (•) gives as an output at least 0.5 s 0 , namely half of the minimum safety distance, so that the summation in (8) will always result in σ des ≥ s 0 .
We can now proceed formulating our tasks based on the quantities introduced above.Let q = p ego = x ego y ego ⊺ be the position coordinates of the ego-vehicle, namely its state configuration vector.We construct a task aiming at keeping a given distance from the obstacle, thus while the desired distance is, e.g., σ des as in (8).Knowing both σ and σ des , we can compute σ from (3).It is then  (11) and poe is the unit vector pointing from the position of the center of gravity of the obstacle to that of the ego-vehicle.
The vector of velocities is then and the projector in the null space of the OA task is therefore Recalling that the OA task should be considered with a higher priority, while still tending to accomplish the RTT task, we can define the control input as where J obs , obs , σobs , and N obs are the task Jacobian, gain, error to a desired behavior, and null projector of the OA task, respectively, while J des , des , and σdes are the task Jacobian, gain, and error to a desired trajectory RTT task, respectively.Hence, ( 14) summarizes the obstacle avoidance maneuver strategy that will be considered as a constraint in our NMPC problem (see Section III).

C. Defining Stopping Maneuvers as Obstacle Avoidance
Now we present an extension of ( 14) to handle the special case of defining a task when a stopping maneuver is clearly identified, e.g., when a vehicle approaches a red traffic signal or any other contingency for which stopping is deemed necessary by sensors (see section II-D).Before we proceed, we present a graphical example illustrating how the above-described task priority management algorithm combines the OA and the RTT tasks.As we can observe in Fig. 2a, if the two tasks agree on the input to give, [0, 1] ⊺ in the example, such an input remains unchanged.If the two tasks are completely discordant, namely the OA input direction points towards the anti-parallel direction [0, −1] ⊺ with respect to the RTT desired input, then the RTT task is ignored, while not all of the final control inputs lie in the feasible region, denoted by the gray triangle.A first way to overcome this issue is to let the optimization problem in the subsequent NMPC find the control input within the feasible region.However, in such a way, there is no guarantee that the optimal solution is indeed the most appropriate for a given stopping scenario.A possible solution could employ a different projection method, like the one illustrated in Fig. 2b, where all the feasible inputs do not change, while only the unfeasible ones are projected to obtain a feasible and suitable control.As we can see, the completely discordant OA task control is projected onto the 0 velocity.Theoretically, this would mean that providing the completely discordant control input would be enough to effectively brake in front of a stopping signal.Of course, this strategy is inadmissible in a real-world scenario where not only do we need an infinite precision to provide a completely discordant control input, but we also need an infinite precision in the Fig. 2. Example of mapping of primary task velocities towards the final ones, given a secondary task velocity.The ego-vehicle (centered at [0, 0] ⊺ ) is required to drive with desired speed [0, 1] ⊺ .All dots around its position are possible (OA yielded) primary task velocity unit vectors.(a) Lines joining OA velocities with other points are mappings of the primary task, considering the secondary one, to the resulting final input velocity as in (14).(b) Mapping of primary task velocities towards the final velocities and saturated to the feasible region, given a secondary task velocity pointing upward.
projection process as well.Nonetheless, we can still take advantage of this algorithm by assigning the higher priority task input to 0 and the null projector to the all-zeros matrix to force a braking maneuver.

D. Dealing With Complex Driving Scenarios
A combination of the previous two solutions to OA and stopping scenarios may be implemented to cope with a wide amount of cases.For example, suppose that we predefine a "giving way to vehicles from the right" rule.Then, whenever a vehicle from the right is predicted to intersect the ego-vehicle trajectory, it is possible to: a) apply the stopping algorithm as far as the vehicle is still on the right; and b) eventually apply the OA algorithm to safely continue on the desired direction.

E. Obstacles Measurement Assumptions
A prediction for future states of an obstacle is retrievable either by direct communication with it, e.g., if vehicles are connected, or by some other prediction algorithm, considering, e.g., the current state and constant speed and orientation for the following time steps.
Finally, we assume that sensors are able to provide preliminary useful information.For example, if the controlled vehicle sensed two obstacles too close to each other, so that it is not possible to pass between them, they should be considered as a single obstacle.This assumption, again, is quite reasonable, considering currently available technologies [5], [60], [61].

III. PROPOSED ALGORITHMS
In this section, we introduce the proposed algorithms to implement the problem described above.The block diagram in Fig. 3 aims at clarifying the context where the proposed method is assumed to operate, which is described hereafter.The sensors block (S) provides internal measurements to the navigation block (N) and external measurements to the mission block (M); in this context, internal measurements include any quantity measured from the vehicle that are useful for vehicle state estimation, while external measurements are the ones originated from, e.g., cameras, radars, and LiDARs.The navigation block (N) is assumed to implement an estimator that is used to reconstruct the full (vehicle) system state.The mission block (M) accommodates, in addition to external measurements, possible communication with connected vehicles infrastructures.It is divided into (M C ) and (M G ), which feed the control block (C) and the guidance block (G), respectively, with the corresponding required data.Block (G) employs the full vehicle state estimate, as well as the foreseen tasks (i.e., tasks that are related to obstacles already detected that limit the configuration space), to compute a trajectory reference that is then fed to block (C).Block (C), besides the trajectory end for Solve NMPC problem (15) instance at time t Apply control input u 1 at time t reference, employs the full state estimate and any unforeseen tasks (i.e., sudden obstacles or any obstacle that was not considered in the trajectory planning), to generate the inputs implemented in the actuators block (A).Note that blocks (C) and (M C ) run at a higher frequency than blocks (G) and (M G ).Typical values for the time sample of the (G) block are of the order of tenths of a second, while for the (C) block, they are smaller up to one order of magnitude less (see, e.g., [16], [32], [50], [62], [63], [64]).In this paper, we focus on block (C), for which Algorithm 1 is designed, and (M C ), for which Algorithm 2 is designed.Since the problem is solved in an NMPC framework, both algorithms are supposed to run at each control step.
Algorithm 1 describes all the calculations to be performed at each control step.Vectors z and ζ i are the state vector of the ego-vehicle and of the i th possible obstacle detected, respectively.In what follows, we consider the state (control) vector as the vector of all states (controls) stacked in time, i.e., , where j = 1, . . ., n denotes the different variables and k = 1, . . ., H represents the different time of the receding horizon.
We formulate the NMPC problem as where the state error from a reference vector is denoted as z = z des − z and z des is the state reference vector.Input error from reference is given by ũ = ūdes − ū, where ū = [u, u aux ] ⊺ is the augmented input vector calculated by solving the optimization problem, and ūdes = [u des , v des ] ⊺ is the augmented desired input vector.Vectors u aux and v des are an auxiliary vector and the vector of desired velocities, respectively.These two Compute J i and σi as in ( 11) and ( 3) 6) end for end if latter vectors are deliberately defined generically, since they can be adapted to the vehicle dynamic model in use (for examples, see Section IV).Again, we consider the velocity v des as the vector of stacked desired velocities over time.In the cost function, Q is a positive semidefinite matrix and R is a positive definite matrix.Note that weights for desired speed tracking are placed in the R matrix, while the Q matrix values may be null at the corresponding entries.In particular, we define , where q z j k is the weight associated to state variable z j k , and, similarly, is the weight associated to control variable ū j k .The first constraint guarantees that z and ū are in their respective feasible regions; the second constraint is a discretized version of the vehicle dynamic model; whereas the third constraint corresponds to the result of the OA algorithm, returned from Algorithm 2, where v is the set of stacked velocities over the time horizon as it is defined in the state vector (whatever way it is defined), η i is the input yielded by the obstacle with priority i in the OA algorithm, and Ni is given by (6).Basically, we are allowing an auxiliary velocity vector u aux to track the desired speed v des ; then, projecting u aux into the null space of higher tasks will eventually result in the final velocity; note that the latter is part of the state vector.As a last remark, one may consider excessive to perform the OA constraint at each NMPC time step.Indeed, we could identify a check horizon H smaller than or equal to the control horizon.In such a case, the OA constraint would be applied from the detection of an obstacle to no more than H time instants in the NMPC instance.This aspect will be further investigated in a set of sensitivity analysis experiments presented in Section V-B.

IV. SYSTEM DYNAMICS
We present here two vehicle models that can be incorporated within our method.In particular, Section IV-A describes a kinematic bicycle model and Section IV-B describes a dynamic Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.bicycle model.These dynamical models are chosen as examples to validate the proposed methodology.Indeed, both the kinematic and dynamic models are broadly used in literature for similar applications (see, e.g., [12], [65]), and, in particular, the kinematic model is typically used for trajectory planning, while the dynamic model is more employed for reference tracking control.However, it is worth mentioning that also different, more complex, models, have been investigated in literature (see, e.g., [64]) and a strength of the proposed method is that Problem (15) can be straightforwardly adaptable to them.Note that, in both formulations, time dependency is dropped for the sake of readability and, with abuse of notation, all operations are meant component-wise.

A. Kinematic Bicycle Model
We consider the following kinematic bicycle model, depicted in Fig. 4a, where ( 16)- (20) are from [66], and (21) describes the OA task, defined as in (5): with where (x, y) is the ego-vehicle coordinate vector, v is the speed, and ψ is the heading angle; all variables are referred to a Cartesian global coordinate system.The angle between the vehicle velocity and its longitudinal axis is denoted as β, while a is the acceleration along the same direction.The distance from the center of gravity of the vehicle to the front and rear axles are denoted as l f and l r , respectively.Finally, δ f denotes the front steering angle, while the rear steering angle is assumed to be null.In (21), η i is the velocity obtained to avoid the i th obstacle, N O is the number of obstacles to be avoided, v aux and ψ aux are the auxiliary inputs as already discussed, and Ni is the set of null projectors appropriately stacked according to other vectors.For this system, the state

B. Dynamic Bicycle Model
We consider the following dynamic bicycle model, depicted in Fig. 4, where ( 22)-( 27) are from [66], and (28) describes the OA task, defined as in (5): where (X, Y) is the position vector in the global coordinate system and, analogously for ψ orientation, v x and v y are the longitudinal and lateral speed, while ψ is the yaw rate.The distance from the front axle and rear axle to the center of gravity are l f and l r , respectively.The exerted force, the front tire force, and the rear tire force are, respectively, F x , F cf , and , and , where C f and C r are the cornering stiffness of the front and rear tire respectively.In (28), η i is the velocity obtained to avoid the i th obstacle, N O is the number of obstacles to be avoided, v xaux and v yaux are the auxiliary inputs as already discussed, and Ni is the set of null projectors appropriately stacked according to other vectors.Here, the state vector is Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.V. SIMULATION EXPERIMENTS In this section, we present a set of simulation experiments with the goal of evaluating the suitability and effectiveness of the proposed method for different road traffic scenarios.First, in Section V-A, we consider a warm-up scenario where four vehicles, aligned along a circumference, are moving towards the diametrically opposed point so that they are expected to intersect their trajectories in the middle of their path.Such a simulation, even though not realistic in traffic situations, is widely employed when testing obstacle avoidance algorithms (see, e.g., [34]).Then, in Section V-B, we present a scenario characterized by an overtaking maneuver; in addition, for this scenario, we perform a sensitivity analysis to assess the impact of parameters' choice on the proposed algorithm, discuss the computational aspects of the proposed method, and present a comparison with a state-of-the-art method.After that, we include two additional scenarios, representative of different realistic traffic situations.In particular, in Section V-C, we investigate a giving way-turningovertaking scenario; while, in Section V-D, we consider a lane drop in a multi-lane environment.Note that, besides the specific settings, these last scenarios incorporates many different behaviors that can be induced by our method, such as stopping, giving way, and overcoming a sequence of obstacles.Mastering these actions are, of course, indispensable for an AV to move safely in a typical (urban) road environment.
All simulation experiments are performed utilizing forward Euler discretized versions of the dynamical systems.Each NMPC step time instant is in the order of a tenth of a second, which is a commonly employed value for the dynamic bicycle model (see, e.g., [65]).Whenever non-connected vehicles are involved in the following, the controlled vehicle assumes they will maintain a constant speed and orientation for the prediction horizon used in the NMPC.

A. Scenario 1: Simple Crossing
In this warm-up simulation experiment, we consider four CAVs, which can communicate with one another to inform about their future positions.The model we implement here is the kinematic bicycle model ( 16)- (21).The desired trajectories are set in such a way to all intersect at (0, 0).Fig. 5a shows how vehicles properly deviate from their desired trajectory to avoid collisions.The vertical color bar on the right side confirms that vehicles positions do not overlap at any time step.Notice that, in this case, only the closest obstacle was considered to perform the evasive maneuver and, therefore, no other traffic rule was considered; namely, vehicles were not forced to overtake on a given side or avoid going outside boundaries.The proper functioning of our algorithm is confirmed by inspecting Fig. 5, which shows that distances between each vehicle and its relative closest obstacle is always greater than a constant minimum safety distance.

B. Scenario 2: Overtaking Maneuver
An overtaking scenario is considered in this second simulation experiment.As a prediction within the NMPC, the controlled AV assumes that the other vehicle (obstacle) moves at constant speed and orientation.For this scenario, we employ the dynamic bicycle model ( 22)-( 28) with parameters defined as in Table I.
First, we test a situation where the controlled vehicle considers the sole vehicle in front as an obstacle, which results in the ego-vehicle succeeding to perform the overtaking maneuver.Fig. 6a shows that the trajectories of the vehicles only intersect at different time instants (see the color bar on the right for a clarified view), while Fig. 6b shows that the distance between the two vehicles never goes below the minimum safety distance.However, in practice, road boundaries restrict the space where a vehicle is allowed to move.To address that, we include road boundaries as additional obstacles by considering a static (i.e., with zero velocity) virtual obstacle on the left side of the controlled vehicle, which is present at any time instant for which a collision with the edge is detected.In this case, the ego-vehicle succeeds in performing the overtaking maneuver while still remaining within the road boundaries, as we can see from Figs. 6c and 6d.Indeed, first the ego-vehicle detects a possible collision with another vehicle, starting the overtaking maneuver as it identified there is available space on the left side.Then, when the orientation of the ego-vehicle reaches a value that could lead to a collision with the left road boundary, the latter is seen as a second obstacle and a corresponding constraint is activated.Once the vehicle on the right is not seen as an obstacle anymore, the overtaking is completed and the ego-vehicle returns to its nominal trajectory tracking behavior.
Sensitivity Analysis: It is important to understand how parameters affect the via the proposed algorithms.Therefore, we perform a sensitivity analysis to investigate the behavior of the proposed method for a set of significant parameters, evaluating their impact on the results in this scenario.In particular, we consider various values for the receding horizon size, the cost function weights, and the gain .
In Fig. 7, we show different results obtained for different H and H , namely, for how many steps the algorithm updates the OA constraint for an obstacle once it is detected.As expected, the time at which a collision with an obstacle is sensed is the same for all the simulations and the longer an obstacle is considered as a possible collision, the longer the OA constraint is triggered.On the other hand, it is interesting to notice that how long the left boundary is considered as a secondary obstacle does not show significantly different patterns.In any case, the minimum safety distance is always kept and the distance between the two vehicles differs very slightly for the set of parameters tested in our study.
Let us turn our attention to the cost function weights in Q and R. Note that positive weights should necessarily be set for q X , q Y , r v xaux , and r v yaux , since they are applied to the error related to the desired trajectory (i.e., X, Ỹ, ṽxaux , and ṽxaux ) and, hence, allow a proper RTT operation.Therefore, we set to 1 the corresponding entries in Q and R and vary the others.The most interesting weights to probe are those for the acceleration a and steering angle δ f .We tested different values for them (smaller and bigger than 1 and various combinations).For the sake of brevity, in Fig 8 , we show results obtained by only two particular configurations: one with acceleration and steering weights set to 0.1 (hence, smaller than the others), and one with acceleration and steering weights set to 10 (hence, bigger than the others).As expected, acceleration and steering angle are smaller if they have bigger weights, as can be observed from Figs. 8a, 8b, 8c, and 8d.On the other hand, when the weights set for the steering angle and acceleration are higher, the controlled vehicle drives less aggressively, it implements less pronounced orientations and, as a consequence, it perceives the left boundary as an obstacle for shorter time (Fig. 8f) than otherwise (Fig. 8e).Of course, if a too large penalty is set in the cost function, then the vehicle may not be able to avoid the obstacles as in the case depicted in Figs. 9 and 10, where r a and r δ f are set to 45. Table II shows the maximum of the absolute value obtained for acceleration and steering angle to varying of their respective weights.Modifying other weights do not seem to strongly affect results.Testing different weights for the cost function.(a) Acceleration when its corresponding weight is smaller than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 0.1.(b) Acceleration when its corresponding weight is larger than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 10.(c) Steering angle when its corresponding weight is smaller than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 0.1.(d) Steering angle when its corresponding weight is larger than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 10.(e) Distance to the obstacle when acceleration and steering have smaller weight than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 0.1.(f) Distance to the obstacle when acceleration and steering have bigger weight than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 10.
Lastly, we consider the gain parameter , which is utilized in Algorithm 2. As a rule of thumb, it should not be too large, otherwise an excessive avoidance maneuver could lead the ego-vehicle outside the road, whereas it is more advisable to keep it smaller when an obstacle is in front, so that the OA operation can begin gently.Figs.11a and 11b show what happens when the obstacle is in front, and is set to 0.1 and 0.2, respectively, while always employing = 1 when the obstacle is behind the ego-vehicle.Indeed, already for = 0.2, the vehicle performs a too aggressive maneuver that leads it slightly out of the road.On the other hand, Fig. 12a and 12b, show results for = 0.01 and = 100, respectively, when the obstacle is behind the ego-vehicle, while = 0.1 is employed when the obstacle is in front.In these cases, the gain does not seem to significantly affect the final results, even if we can observe that a slightly larger maneuver is performed for = 100.This is probably due to the fact that the obstacle behind yields an OA velocity that is more in accordance with the RTT one, so that no big disruption is introduced.Based on our experimental results, it is reasonable to employ ≤ 0.1 when the obstacle is in front and ≤ 1 when it is behind.Thus, for the simulation shown in the rest of this  paper, we employ = 0.05 when the obstacle is in front of the ego-vehicle and = 0.5 when the obstacle is behind the ego-vehicle, respectively.
Computational Efficiency: We used MATLAB and nlmpc (part of Model Predictive Control Toolbox [67]).For the first simulation, we considered a 2.5 s optimization window with a 0.5 s time sample.For the remaining simulations (excluding those for the sensitivity analysis, as already discussed) we  On an average, an NMPC step optimization plus OA took 60% more time than a simple RTT.Nevertheless, this result is not to be considered of much concern, since there are many solutions to improve the performance of NMPC [68].In fact, many other papers have already focused on faster Comparison between the proposed algorithm, considering an unforeseen obstacle, and an artificial potential field method, where the obstacle is known in advance.a) Most of the difference is evident during the first part of the overtaking maneuver, since the proposed algorithm is reacting to an unforeseen obstacle, while the artificial potential field algorithm is aware of it well in advance.b) The lateral position error, i.e. the distance between the X coordinate between the two algorithms is higher at the beginning and then goes back to zero.implementation of NMPC on vehicle nonlinear dynamics even for smaller time steps (see, e.g., [62], [63], [69]).Moreover, the proposed technique enjoys a remarkable generalization and it is applicable to different systems, for which we only presented the nonlinear bicycle models.Therefore, such algorithms would adapt to linearized models as well.Furthermore, even if our algorithms run in series by construction, some expedients can quicken the whole process.For example, if the time sample is sufficiently small, it could be an option to parallelize the two algorithms, so that the NMPC procedure at time instant t runs while data on obstacles that will be dangerous in time instant t + 1 are being processed.Finally, using a dedicated machine, with optimized code using a more efficient programming language, such as C++, and the compiler would allow us to get efficient online performances.
Comparison With an Artificial Potential Field Method: An interesting evaluation of the proposed method can be given by comparing our solution with that obtained via a state-of-theart trajectory planning method, where the latter optimizes the vehicle trajectory by knowing in advance information about obstacles.In particular, we consider an artificial potential fieldbased method, where the objective function penalizes the error from a desired trajectory and the input effort similarly to the proposed algorithm, while the closeness to obstacles (vehicles and roadside) are defined as in [26].Figs.13a and 13b show how most of the difference between the two solutions lies in the first phases of the overtaking maneuver.This result is as expected since, in the artificial potential field method, the presence of the obstacle is known a-priori, giving the possibility to consider the evasive maneuver since the beginning.Nevertheless, even though the simulation for the proposed algorithm is constructed so that the ego-vehicle learns about the presence of the obstacle only later, it is still capable to react to it and accurately track the desired trajectory.In order to solve the mixed integer nonlinear programming problem resulting from the artificial potential field strategy, the optimization software APMonitor [70], [71] is utilized.Within our experiments, the solution took 4.206 s to be found, which means that, in case a sudden obstacle is encountered, a vehicle would need such a long time to re-plan a trajectory and then apply its own tracking control algorithm to follow it.On the other hand, our optimization problem, also coded in APMonitor for the sake of fair comparison, yielded a solution in 0.418 s, namely about 10 times faster, and this is already comprehensive of the tracking control capabilities, i.e., no further algorithm (and time) is needed.

C. Scenario 3: Giving Way -Turning -Overtaking
In this simulation experiment, we consider a four-leg intersection with two vehicles: a controlled vehicle willing to perform a left turn (hence whose planned desired trajectory exhibits a left turn) and a "traditional" manually-driven nonconnected vehicle arriving from the right and willing to go straight ahead (see also Fig. 14).The two techniques of stopping and obstacle avoidance are implemented here.First (Fig. 14a), the controlled vehicle senses the other vehicle approaching from its right side with a trajectory that may lead to a collision and, consequently, it decelerates, since there are no other options.Once the non-connected vehicle crosses the intersection (Fig. 14b), the controlled vehicle can freely accelerate and keep the desired trajectory again.When the two vehicles are getting too close to each other (Fig. 14c), the controlled vehicle performs an overtaking maneuver to avoid collision and continues on its way.

D. Scenario 4: Lane Drop
In this last simulation experiment, we consider a scenario in which an unforeseen lane drop, hence unplanned from the trajectory planning, is sensed.The setup consists of four vehicles: two of them are assumed to be connected and automated, i.e., they are controlled by our proposed methodology and they are able to exchange information on their predicted states resulting from the NMPC process, while other two are assumed to be human-driven, moving at a constant speed and heading.In this example, we consider again the dynamic bicycle model ( 22)-( 28) as a dynamic model for the CAVs.We present three different figures to illustrate the resulting behavior.Fig. 15a shows one of the CAVs, vehicle A (the rightmost one), sensing the lane drop; although its desired trajectory suggests to proceed straight, the controlled vehicle starts a lane-changing maneuver to avoid the obstacle in front of it.The second CAV, vehicle B, senses that vehicle A is going to enter its lane and intersect its path (moreover, in this case, they actually communicate to each other their intents).Vehicle B has, thus, an obstacle on the right, but it senses a non-CAV, vehicle C, on its left as well.In this case, since there is no space for an OA maneuver, vehicle B starts slowing down implementing a stopping maneuver (see Section II-C).In Fig. 15b, we observe that vehicle A senses another non-CAV vehicle (vehicle D) and, therefore, it starts an OA maneuver with respect to it, while still coping with the left side of the road.Meanwhile, vehicle B does not detect any obstacle, since vehicle A is not sensed anymore as an obstacle, and, consequently, it accelerates to keep up with its desired trajectory.Finally, Fig. 15 shows that vehicle A is again able to follow its reference trajectory, while vehicle B is approaching vehicle D and, insofar, engages an OA maneuver.Of course, this last maneuver is deemed safe since vehicle C is not sensed as a close obstacle.
Notice that this simulation is of great significance because it summarizes how the proposed technique behaves in case of: 1) stopping required, i.e. when vehicle B is surrounded by other vehicles; 2) giving way, again for vehicle B, while sensing vehicle A approaching; and 3) coping with different obstacles in sequence, namely vehicle A that senses a lane drop, communicates with vehicle B so that it can safely change lane, and overtakes vehicle D paying attention to the left side of the road.

VI. CONCLUSION
In this paper, we presented a novel approach for providing a prompt response to unforeseen contingencies in the control loop of a vehicle in an (urban) traffic context.In particular, we integrated a null-space-based behavioral control-like algorithm with reference tracking in a Nonlinear Model Predictive Control framework.Such a solution allows us to take advantage of the robustness of NMPC while running an Authorized licensed use limited to the terms of the applicable license agreement with IEEE.Restrictions apply.
obstacle avoidance algorithm in real-time.The validity of our approach was evaluated via a sensitivity analysis, identifying some reasonable bounds for the most interesting and relevant parameters coming into play.Since this strategy permits to react promptly to unexpected obstacles, the trajectory replanning process can safely take more time, as it normally requires, to generate a more convenient reference trajectory.Of course, this would allow the guidance operation to reach a solution closer to the optimal one.This work paves the way for future developments, besides the possibility to relax the requirements for trajectory planning in terms of computation time.Indeed, it would be interesting to test the proposed framework in a larger variety of more complex cases, both for urban and highway traffic, and even more interestingly with real-vehicle data, in order to, e.g., fine-tune some of the parameters.Another interesting extension would be the development of a fully distributed version of the proposed method, so that (connected) vehicles are able to cooperate rather than to simply react to each other.Finally, some more features regarding sensors may be considered, especially investigating the preferred type of data, the amount of pre-processing that may be needed, as well as the robustness of the proposed method to noise and inaccuracy.

Fig. 1 .
Fig. 1.(a) The ego-vehicle (blue) computes its minimum safety distance to the obstacle vehicle (red) as σ des = σ ego only.(b) The ego-vehicle computes its minimum safety distance to the obstacle vehicle as σ des = σ ego + σ obs , given a higher risk of collision.

Fig. 3 .
Fig. 3. Schematic block diagram of the proposed framework: guidance (G), control (C), navigation (N), mission (M), mission for guidance (M G ), mission for control (M C ), actuators (A), sensors (S).The blocks implemented in this work are grouped in the red dashed rectangle.

Fig. 4 .
Fig. 4. (a) Kinematic bicycle model used in the simulations.(b) Dynamic bicycle model used in the simulations.

Fig. 5 .
Fig. 5. (a) Four vehicles with intersecting desired trajectories deviate their paths to avoid collisions.Rectangles represent vehicles positions colored depending on the time step they refer to (starting instant = light blue, final instant = dark blue).(b) Distances between each vehicle and the relative closest obstacle throughout the simulation.

Fig. 6 .
Fig. 6.(a) Overtaking maneuver considering only the vehicle in front as an obstacle.(b) The controlled vehicle and the obstacle vehicle never collide.(c) Overtaking considering both the vehicle in front and the left boundary of the road as obstacles.(d) The controlled vehicle never collides with the obstacle vehicle.

Fig. 8 .
Fig. 8.Testing different weights for the cost function.(a) Acceleration when its corresponding weight is smaller than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 0.1.(b) Acceleration when its corresponding weight is larger than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 10.(c) Steering angle when its corresponding weight is smaller than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 0.1.(d) Steering angle when its corresponding weight is larger than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 10.(e) Distance to the obstacle when acceleration and steering have smaller weight than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 0.1.(f) Distance to the obstacle when acceleration and steering have bigger weight than the others, i.e. q X = q Y = r v xaux = r v yaux = 1 and r a = r δ f = 10.

Fig. 9 .
Fig. 9. Cost function weights set as q X = q Y = r v xaux = r v xaux = 1 and r a = r δ f = 45.(a) Changes in acceleration are barely noticeable.(b) Changes in steering are barely noticeable.(c) A collision with the obstacle vehicle does occur.

Fig. 10 .
Fig. 10.If too high weights are set on the acceleration and steering, then the vehicle is unable to effectively avoid the obstacle and go back on track.

Fig. 11 .
Fig. 11.Testing different values when the obstacle is in front, while keeping = 1 when the vehicle is behind.(a) = 0.1 for the obstacle in front.(b) = 0.2 for the obstacle in front.

Fig. 12 .
Fig. 12. Testing different values when the obstacle is behind, while keeping = 1 when the vehicle is in front.(a) = 0.01 for the obstacle behind.(b) = 100 for the obstacle behind.

Fig. 13 .
Fig. 13.Comparison between the proposed algorithm, considering an unforeseen obstacle, and an artificial potential field method, where the obstacle is known in advance.a) Most of the difference is evident during the first part of the overtaking maneuver, since the proposed algorithm is reacting to an unforeseen obstacle, while the artificial potential field algorithm is aware of it well in advance.b) The lateral position error, i.e. the distance between the X coordinate between the two algorithms is higher at the beginning and then goes back to zero.

Fig. 14 .
Fig. 14.Simulation of an intersection.(a) The ego-vehicle (coming from the down leg) decelerates to let the non-CAV vehicle go.(b) The ego-vehicle turns left and detects the non-CAV vehicle again.(c) The ego-vehicle performs an OA maneuver to overtake the slower obstacle.

Fig. 15 .
Fig. 15.CAVs A and B driving together with non-CAVs C and D. Desired trajectories for A and B are denoted by a green line.(a) Vehicle A senses a lane drop and starts a lane change maneuver.Vehicle B starts decelerating as there is no room for any other maneuver.(b) Vehicle A detects vehicle D and overtakes it.Vehicle B, once vehicle A has gone, accelerates.(c) Vehicle A is free to continue tracking its reference.Vehicle B detects the possibility to overtake vehicle D without colliding with vehicle C.
Algorithm 1 Algorithm to Run at Each NMPC Step Require: ζ i , i ∈ {1, . . ., N PO } possible obstacles states, and NMPC horizon H Ensure: Updated vehicle state z O ← ∅ ordered set of actual obstacles for h ← 1 to H do for k ← 1 to N PO do Compute σ as in (10) Compute σ des as in (8) if σ ≤ σ des then Insert ζ i into the proper position of O end if end for Let N O ← |O| Run Algorithm 2 (returns η 1 Algorithm 2 Obstacle Avoidance Algorithm Require: z Vehicle states, O ordered set of obstacles states, N O cardinality of O, and obs gains Ensure: := η 1 + N O i=2 Ni−1 η i , and NN O Let ← 0 Let NN O ← I n×n if N O > 0 then Extract ζ 1 from O and use it to: Let NN O ← as in equation (13) for i ← 2 to N O do Extract ζ i from O and use it to:

TABLE I PARAMETERS
USED FOR THE DYNAMIC BICYCLE MODEL

TABLE II ACCELERATION
AND STEERING TO VARYING OF THEIR WEIGHTS