{'fraction_non_alphanumeric': 0.04072003489155862, 'fraction_numerical': 0.014115221442448754, 'mean_word_length': 4.234952262349522, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 4, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Coordinated lane change in autonomous driving: a computationally aware solution

: This paper addresses the design of coordinated maneuvers in an autonomous driving set-up involving multiple vehicles. In particular, we consider a lane change problem where a vehicle has to merge in a platoon traveling in the adjacent lane of a two-lane one way road. We propose a cooperative solution that trades optimality for computational feasibility without simplifying the merging vehicle dynamics. The key idea is decoupling the problem into two phases: an online coordination phase where vehicles in the platoon create a gap where the merging vehicle can safely enter, and a merging phase, where the merging vehicle change lane by tracking a pre-computed optimal maneuver. A numerical case study shows the achieved trade oﬀ between performance degradation and reduction in computing time of the proposed solution.


INTRODUCTION
Autonomous driving proved to have potential in reducing motor vehicle accidents, improving driving experience (e.g., minimizing traffic jam), and increasing overall efficiency (e.g., cut down energy consumption and CO 2 emissions). Many complex maneuvers in traffic comprise a vehicle changing its lane and lane change is the reason of a significant amount of accidents, see, e.g., (Fitch et al., 2009).
Planning a lane change comprises two interwoven layers: the strategic layer which decides whether a lane change is desirable and feasible, and the trajectory generation layer which computes the corresponding trajectory to be tracked.
Several methods have been proposed for making strategic decisions. If the vehicles are connected, a cooperative strategy can be devised (see e.g. Manzinger et al., 2017), if not, then the controlled vehicle can update its strategy exploiting only the information coming from the environment (see e.g. Coskun et al., 2019, Yang et al., 2018, Luo et al., 2016. Once a lane change strategy is determined, it is necessary to compute the corresponding trajectory. For cooperative cases, trajectory planning for multiple vehicles can be posed as a single nonlinear optimal control problem (Li et al., 2017). For non-cooperative cases, Nilsson et al. (2015) proposed a model predictive control framework assuming that the future behavior of the surrounding vehicles is available. A limited literature exists also for cases when the behavior of the others is unpredictable (Yang et al., 2018).
While computing the trajectory of the controlled vehicle, the aforementioned research considers the kinematic model of a single-track (Li et al., 2017), uses parametric curves (Yang et al., 2018), or decouples the longitudinal motion from the lateral one (Nilsson et al., 2015). All of these approaches rely on the no-slip (pure rolling) assumption on the tires, which is typically satisfied when the vehicle moves at low speed but it does not hold when computing agile maneuvers. For single vehicle trajectory generation, efficient methods exist that take into account vehicle inertia and tire slip. However, these methods either rely on a linearized model (Hesse and Sattel, 2007) or apply to specific optimization objectives such as minimizing the trajectory duration (e.g. Jeon et al., 2013, Rucco et al., 2012. Interested reader can refer to (Bevly et al., 2016) for a survey on the recent advances in computing lane change maneuvers for connected and automated vehicles and (Paden et al., 2016) on motion planning and control techniques for single vehicle cases. Enabling technologies for positioning, sensing, and communication are discussed in (Bevly et al., 2016) besides the low level control platform acting on throttle, brake, steering wheel to track some reference trajectory. All these aspects are indeed key to the actual implementation of any strategy.
In this work, we consider a cooperative multi-vehicle setting and address the lane change problem in a two-lane, one way road when a vehicle needs to merge into a platoon in the adjacent lane. All vehicles are automated and they coordinate to ease the merging maneuver. We assume that the sensing, positioning, and communication systems, and the low level control layer are in place to allow for the implementation of a coordinated merging maneuver. Our focus is on the design of such a joint maneuver involving the merging vehicle and the vehicles in the platoon.
Computing an optimal maneuver while considering all the agents simultaneously and adopting a realistic model for the merging vehicle results in a nonlinear (non-convex) mathematical program that is hard and excessively time consuming for the currently available solvers. We then head for a computationally aware solution which trades optimality for computational feasibility but without resorting to an over-simplification of the merging vehicle model, as it is typically done in the literature by, e.g., treating separately its longitudinal and lateral dynamics or neglecting tires slip.
To this purpose, we decouple the problem into (i) a coordination phase to ensure that a gap is created which satisfies the collision avoidance constraints; and (ii) a lane change phase to make the vehicle merge into the platoon.
The coordination phase is formulated as a quadratic optimal control problem which is solved online based on the current state of the multi-vehicle (merging vehicle plus platoon) system so as bring the overall system to a given state where a gap in the platoon is created for the merging vehicle to join safely. The merging phase is the computationally intense part of the overall problem solution since it entails solving a non-convex optimization problem. However, such a computation can be done once and offline, since it involves only the merging vehicle with initial and final states that are fixed and it does not depend on the initial state of the multi-vehicle system. It is thanks to the introduction of this offline phase that, differently from the existing works in the literature, we are able to account for the underlying accurate dynamic model of the vehicle.

DESCRIPTION OF THE LANE CHANGE PROBLEM AND ITS OPTIMAL SOLUTION
We consider a coordination problem for autonomous vehicles where a vehicle has to merge into a platoon that is traveling on a parallel lane of a two-lane, one way road, due to, e.g., the presence of an obstacle, as shown in Fig. 1.
Our goal is to optimally plan the lane change by designing accelerations and steering angles for all (automated) vehicles to perform the joint maneuver safely.
This can be formulated as an optimal control problem subject to actuation and safety constraints. As a criterion for optimality, we consider the time needed for the merging vehicle to enter the platoon, while safety is associated with ensuring the vehicles to be at appropriate distances between each other during the whole maneuver.
Before formulating the optimal control problem, we shall describe the employed car dynamical model.

Vehicle dynamics
The dynamics of the vehicle is described by a normalized version of the bicycle model in Rajamani (2011) where (x, y), ψ, and ω are the center of mass position, vehicle heading angle, and yaw rate with respect to a Cartesian absolute reference frame, with the x axis along the direction of the lanes, while v x and v y are the components of the vehicle velocity in a reference frame centered in the vehicle center of mass and rotated by ψ with respect to the absolute reference frame. The acceleration a x (aligned with v x ) and the steering angle δ are the control inputs of the vehicle. The normalized lateral forces a f and a r acting perpendicularly to the vehicle longitudinal axis on the front and rear wheels, respectively, are given by being the side slip angles related to the front and rear wheels, respectively, k f = µgc f r /L and k r = µgc r f /L the front and rear normalized force coefficients, and F the maximum normalized force. Finally, c f and c r are the front and rear tire stiffness coefficients, f and r are the distances of the center of mass from the front and rear axle, L = f + r is the vehicle length, µ is a friction coefficient, g is the acceleration of gravity, m is the mass of the vehicle, and J is the moment of inertia. The main quantities in (1) and the two reference frames are also depicted in Fig. 2 for the reader's convenience.
Note that if we set the input δ(t) = 0, t ≥ 0, and initialize (1) with y(0) =ȳ, v y (0) = ψ(0) = ω(0) = 0, then, the dynamics described in (1) becomeṡ with all other state variables constant: This entails that we can reduce the dynamics of the vehicles in the platoon to a (linear) double integrator model along the lane, and adopt the nonlinear model (1) only for the vehicle that has to change lane to merge the platoon.

Optimal coordinated maneuver design
We consider m vehicles in a platoon traveling at a constant speed from left to right on the same lane. Vehicles are numbered consecutively from 1 to m with vehicle m leading the platoon, while in the other lane there is a single vehicle (vehicle number 0) that has to merge the platoon (see Fig. 1). We assume that vehicles in the platoon will keep traveling in the same straight path along the lane and will only modify their speed to accommodate for vehicle 0 merging maneuver. Accordingly, we shall set δ i (t) = 0 for all t and i = 1, . . . , m, and describe the platooning vehicles by (2), while vehicle 0 will obey the dynamics in (1). In the rest of the paper, superscript i will denote quantities related to the corresponding vehicle. We shall instead use label j to denote the merging scenarios: j = 0 represents the scenario in which the merging vehicle joins the platoon at its tail (i.e., behind vehicle 1), j = m the scenario in which the merging vehicle joins the platoon at its head (i.e., in front of vehicle m), and j = i the scenario in which the merging vehicle joins the platoon between vehicles i and i + 1, i = 1, . . . , m − 1.
As a safety restriction, for vehicle 0 to be able to leave its current lane and join the platoon, the following merging conditions have to be satisfied: • if j > 0, then the distance along the lane between vehicle 0 and vehicle i = j during the lane change is no-smaller than v j x (t) t min gap ; • if j < m, then the distance along the lane between vehicle 0 and vehicle i = j + 1 during the lane change is no-smaller thanẋ 0 (t) t min gap , where t min gap is a suitably defined time interval representing a minimum required safe reaction time. Note that for vehicle 0 we usedẋ 0 in place of v 0 x since safety distances are measured in terms of projection onto the lane direction. If we let ∆y denote the lane width and take the absolute reference frame so as the two lanes span the interval [0, 2∆y] (bottom to top in Fig. 1), then the safety restriction can be coded as the following implications for all t, in the case when j > 0, and for all t, if j < m. Furthermore, at any time instant t, we also impose the safety restriction that the distance between vehicle i and i+1, i = 1, . . . , m−1, in the platoon should be no-smaller than v i x (t)t gap , with t gap > t min gap being a nominal reaction time. This is formally stated as and for all t. We also enforce the speed limits for all t, lateral position constraints for vehicle 0 to belong to the roadway 0 ≤ y 0 (t) ≤ 2∆y, for all t, and the actuation constraints for all t. Finally, we shall require that, at the end of the maneuver, vehicle 0 merged the platoon and all vehicles travel at some platooning speed v des .
The optimal maneuver can then be obtained by solving the following optimal control problem min (9) subject to: y 0 (0) = 0.5∆y where the decision variables a 0 x (t), a 1 x (t) . . . , a m x (t) and δ 0 (t) are continuous functions of time, while T f is a scalar quantity. Note that the cost function in (9) minimize the time T f needed to complete the maneuver while (slightly) penalizing the control effort with the (small) weights ε δ , ε a > 0 and that problem (9) is parametric in the vehicles initial positions x i (0) and velocities v i x (0), i = 0, . . . , m, and in the merging position j. To optimize also the merging position j one can solve (9) for j = 0, . . . , m and then choose the solution with the minimum T f .
The infinite-dimensional nonlinear optimal control problem in (9) can be transformed, e.g., via direct collocation methods, into a finite-dimensional nonlinear optimization problem (see Betts, 2009), which can then be solved numerically. However, the resolution strategy is timeconsuming and computationally intensive and we cannot expect it to run on vehicle 0, online, since in case of an obstacle causing the lane change, the obstacle would be reached before completing the computations.

PROPOSED COMPUTATIONALLY AWARE RESOLUTION STRATEGY
By looking at the structure of (9), we can see that the nonlinear (hence hard) portion of the optimization problem is given by the dynamics constraints (1) for vehicle 0, together with the safety constraints (3) and (4).
To tackle the design problem, we thus propose to approximately solve (9) by splitting the merging maneuver into two phases: This splitting has several benefits discussed next. In the coordination phase vehicle 0 is forced to stay on the center of its lane, which implies that it also obeys the dynamics in (2) and, consequently, y 0 (t) = 0.5∆y, for all t. This simplifies dealing with the safety conditions, as the coupling between x 0 (t) and y 0 (t) due to (3) and (4) is removed. At the end of the coordination phase all vehicles have a specific state (in terms of relative position, velocity and orientation), which is independent from their initial state. This implies that we can design the (computationally intensive) lane change maneuver of vehicle 0 alone, offline, and independently from the initial state of all vehicles. Moreover, if all vehicles maintain the platoon speed v des along the lane, we do not need to include safety constraints in the lane change design, thus leaving the optimization problem with vehicle 0 dynamics as the only nonlinear part.
Next, we describe in more details the design of the two phases of the merging maneuver.

Coordination phase
In the coordination phase, each vehicle dynamics is described by (2). To ease the computation, we discretize the model in (2) with a sampling time interval of duration dt seconds. For a generic vehicle i, we assume that the acceleration is kept constant within each time slot, i.e., a i which is typically the case in digital control implementations with zero-order-hold (ZOH) converters. Then, the following discrete time linear model is derived from (2) where k denotes the time instant kdt.
Safety constraints (5) between platooning vehicles, speed limit constraints (6), and actuation constraints (8) with k in place of t and v 0 x (k) in place ofẋ 0 (k), are linear in the decision variables a i x (k), i = 0, . . . , m, k ≥ 0, and can be easily enforced.
The goal of the coordination phase is to optimize a i x (k) so as to satisfy the merging conditions and reach the target speed v des (two conditions referred in the sequel as the target set of states) as quickly as possible, given the initial state of all vehicles. The minimum-time design, compliant with the constraints above, can thus be obtained by solving the following quadratic optimal control problem over the where the decision vector a = [a 0 x (0) · · · a m x (0) a 0 x (1) · · · a m x (1) · · · a 0 x (N − 1) · · · a m x (N − 1)] contains the accelerations of all vehicles along [0, N ], h = [h(0) · · · h(N )] , x i (0) and v i x (0) are the initial position and velocity of vehicle i, and x i (k) and v i x (k) depend on the optimization variables a i x (0), a i x (1), . . . , a i x (N − 1) and the initial states x i (0) and v i x (0) through the constraint enforcing the satisfaction of the dynamic equations in (11).
For each k = 0, . . . , N , h(k) represents the maximum violation between the merging conditions and the constraints that the velocity of all vehicles must be equal to the target platoon speed v des , and measures the distance at time instant kdt of the overall system from the target set. The cost function in (12) increasingly penalizes such a distance h(k) as k (i.e., time) increases, thus encouraging the earliest possible attainment of the target condition.
Let ε th be a tolerance threshold and denote by k lc,j (lc stands for lane change) the index of the first discrete time instant such that h(k lc,j ) < ε th . Since h(k) is a measure of the distance from the target set at k, h(k lc,j ) < ε th implies that the state of the system belongs to the target set within the tolerance prescribed by the threshold ε th . We can therefore consider the coordination phase completed at k = k lc,j , which correspond to time instant k lc,j dt. Now, recall that j is related to the position where the merging vehicle enters the platoon. Clearly, the optimal solution (a j , h j ) of (12) as well as k lc,j both depend on j. We can then explore different values of j = 0, . . . , m and select a = a j , with j = arg min j=0,...,m k lc,j , as the minimum-time acceleration profile for the coordination phase and let k lc = k lc,j denote the number of time slots required to complete the coordination phase. The reader should note that, in the worst case, this procedure amounts to solve m + 1 times the quadratic program (12), whose complexity also scales with m.
The proposed coordination mechanism requires only to measure the relative positions and velocities among all vehicles and, if m is low, the approach can be sufficiently lightweight to be performed online by the merging vehicle, which then transmits to the other vehicles the optimal acceleration profiles. In case of a big platoon (i.e., large value of m) it may be the case that vehicle 0 alone cannot afford the resulting computational burden. In this case, all vehicles can join forces to solve (12) in a distributed way, e.g., via the approaches in Falsone et al. (2017Falsone et al. ( , 2020. This way, problem (12) would be split into m smaller sub-problems: one per vehicle with only those decision variables and constraints pertaining to that vehicle. The sub-problems would then be repeatedly solved in parallel and their solution shared among the vehicles, in an iterative process that is guaranteed to converge to the optimal solution. Assessing the threshold value of m such that a distributed solution is needed would require addressing technological aspects such as those discussed in the survey paper Bevly et al. (2016).

Lane change phase
The lane change phase starts right after the end of the coordination phase, so that, in particular, all vehicles are traveling along the center of their respective lane, with zero lateral velocity v i y and longitudinal velocity v i x = v des , i = 0, . . . , m.
We can therefore start from this initial state and optimize the acceleration a 0 x (t) and steering angle δ 0 (t) profiles of the merging vehicle so that it moves from the current lane to the target lane within time T f (to be minimized).
If, during the lane change, all other vehicles are forced to keep traveling at v des along the lane and we constrain the merging vehicle to also keepẋ 0 equal to v des during the lane change, then all relative distances along the lane between each pair of vehicles will not change and we are guaranteed to meet all safety requirements during the whole merging maneuver.
Clearly, for the lane change to be completed, we also need to ensure the merging vehicle reaches a state where it keeps traveling on the center of the target line at a constant speed equal to v des , at and after T f .
The optimal values for a 0 x (t), δ 0 (t), and T f can thus be determined by solving the following infinite-dimensional nonlinear optimal control program min which is simpler than (9) as the nonlinearities appear only inside the dynamics in (1). Still, problem (13) is hard to solve, and one needs to resort to a numerical solver to get an approximate solution. However, since (13) is independent from the vehicles' initial state at the beginning of the coordination phase, we can solve it once offline and store the lane change reference profile onboard the vehicle without the need to re-optimize every time.

NUMERICAL CASE STUDY
We consider a platoon of m = 3 vehicles traveling at a platoon speed v des = 70 km/h along one lane and the merging vehicle traveling at a lower speed in the other lane. Each lane is ∆y = 3.5 m wide. Safety regulation are encoded in the parameters t min gap = 1 s and t gap = 1.5 s, vehicles cannot go backwards, so v min = 0 m/s, and the speed limit is set to v max = 90 km/h. For each vehicle, actuation constraints are set to a min = −3 m/s 2 and a max = 2 m/s 2 for the acceleration and to δ min = −45 • and δ max = 45 • for the steering angle. The maximum normalized force is set to F = 1 m/s 2 .
The origin of the absolute reference frame is placed at the tail vehicle of the platoon (i.e., x 1 (0)) for the longitudinal (x) direction and to the side of the road near the merging vehicle for the lateral (y) direction. Positive x's are in the direction of motion of the vehicles and positive y's toward the center of the road.
The initial states of the vehicles in the platoon are x i (0) = (i − 1)t gap v des and v i (0) = v des , i = 1, . . . , m, while the initial state for the merging vehicle at the same time instant is given by x 0 (0) = 1.6 · t gap v des and v 0 (0) = 35 km/h. The strategy proposed in Sections 3.1 and 3.2 have been implemented in MATLAB R2019a on a laptop equipped with an Intel Core i7-8565U CPU and 16 GB of RAM. Implementation of the coordination phase is based on YALMIP (Löfberg, 2004), with CPLEX 12.8 as solver, while for the design of the lane change phase, we resorted to the interface ICLOCS2 (Nie et al., 2018), which translates, using a direct collocation method, the optimal control problem (13) into a nonlinear program that is then solved by IPOPT 3.11.8 (Wächter and Biegler, 2006).
As for the coordination phase, we design the optimal joint maneuver using a sampling time interval dt = 0.05 s and a control horizon equal to T = 10 s, giving rise to N = 200 time slots. For the lane change phase we used 50 equally spaced collocation points with linear and cubic Hermite spline interpolation for the input and state variables, respectively.
Computing time for the design of the coordination and lane change phases is about 10 s and 57 s, respectively, and, in Fig. 3a, we report the optimal vehicles trajectories in terms of absolute position (x i (t), y i (t)) (upper and middle plots) and velocityẋ i (t) along the x direction (lower plot), for all vehicles i = 0, . . . , m. The red dashed vertical line identifies the first instant after which the merging conditions (3) and (4) are satisfied, while the black dashed vertical line identifies the separation between the coordination phase and the lane change phase at k lc dt, which also requiresẋ i (k lc dt) = v des , for all i = 0, . . . , m.
As can be seen from Fig. 3a, vehicles 2 and 3 (yellow and violet lines, respectively) increase their speed and vehicle 1 (red line) slightly reduces its speed to accommodate for vehicle 0 (blue line). The latter tunes its acceleration profile to approach the midpoint between vehicle 1 and 2. This optimal solution resulted in a j = 1 and a k lc = 110, which is equal to 5.5 s to reach the lane change phase. The lane change then takes 3.2 s for an overall 8.7 s for vehicle 0 to merge the platoon.
For comparison purposes we also computed the optimal solution to problem (9) using ICLOCS2 with IPOPT 3.11.8 as solver. We initialized ICLOCS2 with our solution leaving all other solver parameters unchanged and, after about 4 hours, it returned the optimal solution reported in Fig. 3b (color coding is the same of Fig. 3a). The optimal merging maneuver takes 5.8 seconds, 2.9 seconds less than the proposed solution. By comparing the longitudinal position and speed (upper and lower plots) in Fig.s 3a and 3b, we can see that the profiles of the coordination phase of the proposed solution are remarkably similar to the optimal ones, the main difference being that the optimal strategy is able to handle the coordination and the lateral motion of the merging vehicle simultaneously, while we need to solve the two problems on separate time intervals. Note also that the proposed strategy satisfies the merging conditions already after 4.1 s, but it then has to wait for the velocities to reach v des before it can move to the lane change phase.  The increased maneuver time is the price to pay for the incredibly reduced computational time.

CONCLUSION
In this paper we propose a computationally efficient resolution strategy for addressing optimal coordinated lane change of a vehicle merging into a platoon. The approach appears amenable for onboard implementation. Its actual implementation in real vehicles requires to consider technological aspects related to sensing, positioning and communication systems, and to assess the actual computational time required when adopting embedded systems for vehicle control.