Automated Turning and Merging for Autonomous Vehicles using a Nonlinear Model Predictive Control Approach

Transcription

1 Automated Turning and Merging for Autonomous Vehicles using a Nonlinear Model Predictive Control Approach Lixing Huang and Dimitra Panagou Abstract Accidents at intersections are highly related to the driver s mis-decision while performing turning and merging maneuvers. This paper proposes a merging/turning controller for an automated vehicle, called the ego vehicle, which avoids collisions with surrounding (target) vehicles. An optimizationbased control problem is defined based on receding horizon control, that parameterizes the system trajectory with the control input and employs a nonlinear model on the ego vehicle dynamics. Most existing solutions focus on 1-D (longitudinal) motion for the vehicles. In this paper, the 2-D motion of the turning/merging vehicle is considered instead. The intersection is modeled under realistic traffic conditions, a probabilistic model is used to predict the trajectories of the target vehicles, and is integrated within a novel collision avoidance model. These models allow our controller to perform both line following when turning/merging, and collision avoidance, while simulations of several scenarios validate its performance. I. INTRODUCTION Recent advances in sensor technology have enabled the development of control systems that possess more sophisticated perception of their environment and own state with lower computational effort. Automated and autonomous vehicles is a relevant application domain. GPS can locate the vehicle within the error of 8m with the aid of WLAN [1]. Radar [2] and Lidar [3], [4] provide an accurate assessment of the velocity and the spatial information of the surrounding environment (other vehicles, physical obstacles). The matureness of the visual odometry allows the vehicle to compute its own trajectory and velocity [5]. Benefited from the improvement of the perception module, much effort can be devoted into designing advanced controllers to guide the vehicle with certain levels of safety, and reduce road accidents. Research addressing the safe passing through an intersection is rich. Some researchers focus on centralized control approaches; [6] proposes a scheduling system that coordinates the departure time of the engaged vehicles. Kamal et al. define several cross-collision points according to the number of lanes on each road, and schedule the arrival time to improve traffic efficiency [7], [8]. Similarly, [9] transfers the problem of computing a maximal controlled invariant set to a wellformed scheduling problem, where a centralized controller coordinates the sequence of vehicles passing through the intersection. Decentralized control approaches have also been Lixing Huang is with the Department of Electrical Engineering and Computer Science, University of Michigan, Ann Arbor, MI, USA lixhuang@umich.edu Dimitra Panagou with the Department of Aerospace Engineering, University of Michigan, Ann Arbor, MI, USA dpanagou@umich.edu This work was in part supported by the U. of Michigan Summer Undergrate REsearch (SURE) Program. developed; in [10], each vehicle receives an identifier from a coordinator, and optimizes fuel consumption with respect to the constraints of the time for exiting the merging zone, based on the solution of the vehicle preceding to it. Qian et al. define a collision region in an 1-D frame regarding to the potential collision object, and optimize the trajectory to keep it out of this region [11]. Multiple techniques are employed for the merging problem; Kotsialos et al. focus on the traffic flow instead of a single vehicle, and build an optimization problem based on a macroscopic traffic model [12]. Awal et al. also consider a stream of cars, and develop a merging algorithm to compute a feasible merging list [13]. This line of research focuses on how to improve the efficiency of the traffic and prevent congestion. However, the 1-D model of the intersection, and the linear model adopted for the vehicles, in principle neglect that the driver may have to perform complex maneuvers for collision avoidance, as well as the vehicle dynamics. Hybrid control approaches have also appeared: A discrete decision-making system regarding to the stoplight on single or two lanes is proposed in [14]. The recent work in [15] assumes no coordinator and V2V communication, considering only vehicle s longitudinal motion; the aforementioned paper models the uncertain behavior of target vehicles as a distance range with respect to an acceleration bound, and utilizes an MPC approach so that the ego vehicle maintains a reference position and speed. A recent detailed literature review on related topics is provided in [16]. This paper proposes a decentralized optimization-based control approach for the merging/turning of an automated vehicle into an intersection. No explicit communication is assumed among vehicles. A Nonlinear Model Predictive Control (NMPC) method is used to predict the evolution of the merging vehicle trajectories, as well as of the environment (target vehicles). The computed control sequence for the merging vehicle can be either applied on the vehicle in an automated fashion, or be informed to the driver so that s/he takes action to avoid collisions. A 2-D intersection and merging model is built to imitate realistic road conditions. A nonlinear dynamics model for the merging vehicle is adopted to improve the accuracy of the prediction. Hence the NMPC approach on one hand provides a line-following controller that accounts for realistic maneuverability capabilities. A probabilistic model is employed to predict the uncertain maneuver of the target vehicles, and is built within a novel collision avoidance model. The main contribution of the paper is the use of a nonlinear vehicle model and of probabilistic collision prediction to take realistic road conditions into account, while accomplishing lane keeping

2 and collision avoidance at the same time. This paper is organized as follows. The intersection model and the nonlinear model for the merging vehicle are given in Section II. Section III presents the proposed optimizationbased control formulation, while Section IV includes simulations to evaluate the performance of the solution. Conclusions and thoughts on future research are summarized in Section V. II. PROBLEM FORMULATION We consider a road configuration in which a car intends to make a right turn from road ζ to the target road γ. In a similar spirit, one may consider the problem of a car merging from the ramp to the highway. In both cases, the turning or merging car needs to make a smooth transition into the new road by following the center line of its current lane to the target lane, while avoiding collisions with other cars. We consider a sequence of M vehicles V j, j {1,..., V }, called targets in the sequel, that move straight on the target road γ. We assume that each target vehicle V j moves along the reference center line of the road γ. Let v tj, θ tj, x tj, y tj denote the linear velocity, the heading angle, and the position coordinates with respect to a global coordinate frame G, respectively; the kinematic equations of motion for the target vehicle V j are then as follows: ẋ tj = v tj cos θ tj, (1a) ẏ tj = v tj sin θ tj. (1b) A probability model on v tj is built to account for uncertain effects, as will be explained later in Section III-A.4. We also consider a vehicle E, called the ego vehicle, that moves along road ζ and approaches the intersection, and whose model is provided in the sequel. We assume that the ego vehicle moves forward following a reference center line, and that has access to the velocity and position of the target vehicles that lie within its sensing region through GPS, Lidar and other sensors. The ego vehicle should: (i) Avoid collisions with cars running on the target road. (ii) Follow a reference line corresponding to the road configuration. (iii) Be within lateral acceleration bounds while maintaining a desired speed as closely as possible. Fig. 2. The parameters of the dynamic model of the ego vehicle. We assume that the geometry of the road and the locations of the points C 1 and C 2 are known beforehand as part of a map available to the ego vehicle. In this paper, an orthogonal intersection is chosen as shown in Figure 1, and the ego vehicle is considered to make a right turn. For simplicity, the roads are modeled as straight lines. The intermediate phase is modeled as an arc of an ellipse that connects C 1 and C 2, so that the center line from road ζ to road γ is a C 1 function. To describe the behavior of the ego vehicle, a nonlinear dynamic bicycle model is adopted [17]. In the sequel, x e, y e, θ e denote the position coordinates and heading angle of the vehicle with respect to (w.r.t.) the global frame G, and v l, v c, and ω denote the longitudinal, lateral and angular velocity of the vehicle expressed in the vehicle-fixed frame B. Furthermore, we use postfix {} f and {} r to denote front and rear part; {} l and {} c to denote longitudinal and lateral direction. Referring to Fig. 2, we define: δ f, δ r : v xf, v xr, v yf, v yr : v lf, v lr, v cf, v cr : α f, α r : F lf, F lr, F cf, F cr : F xf, F xr, F yf, F yr : steering angle velocity along/vertical to main axis longitudinal/lateral velocity of tire slipping ratio of tire longitudinal/lateral force on tire longitudinal/lateral force on body frame a, b : distance from gravity center to the front, rear tire m, I : vehicle mass and inertia about z B axis C f, C r : tire stiffness For compactness, {f, r} in the sequel indicates front and rear part. The dynamics of the ego vehicle are expressed in the body frame [18] as: Fig. 1. A typical intersection configuration: The ego vehicle on road ζ makes a right turn to merge into road γ, trying to avoid targets V 1 and V 2. v l = v c ω + 2F xf + 2F xr, (2a) m v c = v l ω + 2F yf + 2F yr, (2b) m ω = 2aF yf + 2bF yr. (2c) I The kinematics of the front and rear wheel are given as: v x = v l, v yf = v c + aω, (3a) (3b)

3 v yr = v c bω, v l = v y sin δ + v x cos δ, v c = v y cos δ v x sin δ. The slip angle of front and rear tire is defined as: α = v c v l, respectively, while the forces on the tires are given as: F c = C α, F x = F l cos δ F c sin δ, F y = F l sin δ + F c cos δ. (3c) (3d) (3e) (4a) (5a) (5b) (5c) A linear tire model [19], [20] connects the lateral tire force with the slip angle and the stiffness constant. The longitudinal force on the rear tire is set to zero, since we assume the car is front-wheel drive. The rear steering angle is set to zero, δ r = 0. The longitudinal force F lf is thus set equal to traction force which is one input of the system, and the front steering angle δ f is another control input. The kinematics of the ego vehicle w.r.t. the global frame G are described as: ẋ e = v l cos θ e v c sin θ e, ẏ e = v l sin θ e + v c cos θ e, θ e = ω. (6a) (6b) (6c) Combining equations (2)-(6), the system model is written in compact form as: ẋ = f(x, u), (7) where x = [v l v c ω x e y e θ e ] T is the state vector, u = [F lf δ f ] T is the control input vector, and the vector function f(.,.) comprises equations (2)-(6). III. MOVE BLOCKING MPC APPROACH A nonlinear receding horizon control approach is employed to compute safe trajectories for the ego vehicle while respecting the requirements described earlier. This approach solves an optimal control problem for a finite prediction horizon, applies the first control input over a (shorter) control horizon, and repeats the process recursively as the system evolves. The standard MPC formulation is [21]: minimize N 1 l(x i, u i ) (8) subject to x i+1 = x i + f(x i, u i ) T, (9) x 0 = x 0, (10) u min u i u max, (11) where N is the prediction horizon, i {0, 1, 2,..., N 1}, x i R n is the state vector, u i R m is the vector of control inputs, f(, ) is the vector system dynamics; x 0 is the initial state, and T is the time step. The bounds on the control inputs and on the system dynamics are viewed as linear and nonlinear constraints, respectively. Since the state trajectory and control input trajectory are free variables, the search dimension of this optimization problem grows very fast as the prediction horizon increases. In addition, for a nonlinear MPC problem the time step of the system evolution should be small enough to ensure the accuracy of the nonlinear discrete model. This results in high-dimensional state and control vectors for a fixed time horizon, i.e., in a highdimensional search space. This in turn limits the performance and applicability of the controller in terms of the required computational effort. To speed up the optimization process, the move blocking technique is employed. A sequential approach is also used to parametrize the system trajectory with block control inputs, which now become the only free variables. Briefly, with the move blocking technique a single control input is kept the same over several time steps instead of one, so that a fixed length of control input sequence can cover a longer horizon. The parametrized trajectory is also allowed to be sampled in order to decrease the number of points on the trajectory, which keeps the computational time of the cost function small. A. Problem Formulation Instead of solving for U = [u 0T, u 1T,..., u N 1T ] T R mn, where N is the prediction horizon and m is the dimension of the control vector, we will parametrize U with the block control sequence U = [u T 0, u T 1,..., u T M 1 ] T R mm, such that U = (S I m )U, where denotes the Kronecker product, I m is the identity matrix with size of m m and S R N M is a blocking matrix [22]. If we denote 1 r as column vector with all values equal to 1 and of length r, the blocking matrix can be presented as: S = 1 r r rm To interpret this transform, we should notice u 0 = u 1 =... = u r 0 = u 1. We use uniform blocking here and suppose N is a multiple of M, such that r 0 = r 1 =... = r M = N/M = r. Despite that the free dimension of the problem decreases from N to M, the long trajectory still leads to a high computational effort demanded in cost function. The trajectory is subsampled to reduce the computation time. If the original trajectory of the ego vehicle is computed as X = {x i x i+1 = x i + T f(x i, u i ), i = 0, 1,..., N, x 0 = x 0 }, (12) then using k = N K, where K is the length of the sampled trajectory, as the sample rate, we have our sampled ego vehicle trajectory C as: C = {x kn 1 x kn 1 X, n = 1, 2..., K}, (13) whereas the sampled target vehicle trajectory is similarly computed and denoted as C t. The goal for the ego vehicle is to follow the centerline and avoid collisions with least traction/braking force and change in steering angle. The cost function is defined as: J =g eff (U) + g fol (C) + g per (C) + g col (C, C t ), (14)

4 where g eff is the cost of control effort, g fol penalizes the deviation from the reference line, g per penalizes the deviation from the desired velocity, and g col is the cost of collision. These are defined in detail in the sequel. With the definition of the sampled trajectory set and control sequence from equations (12) and (13), respectively, the optimization problem is summarized as: minimize J(C,C t, U), U subject to u min U u max. 1) Effort cost: We wish the ego vehicle to achieve the control goal with as little traction or braking force, and as little change in steering angle as possible. Hence we define: M 1 g eff (U) = w 1 M 1 F 2 lf i + w 2 2 δ f i, (15) where w 1 and w 2 are positive weights. This part penalizes the tracking force and the change of the steering angle. The vehicle is thus expected to keep its current state. 2) Line following: If the ego vehicle is following a line perfectly, the distance between its current position and the closest point on the center line should be zero. Besides, the vehicle s curvature and the curvature of the closest point on the center line should be the same. This part of cost function is expected to lay every point of optimal trajectory on the reference line. We define: g fol (C) = w 3 Di 2 + w 4 (k vi k ci ) 2, (16) where D is the distance from the vehicle to closest point on ω the curve; w 3 and w 4 are positive weights. k v = v 2 l +v 2 c and k c are the curvature of the path followed by the vehicle and of the reference curve respectively. Since the counterclockwise is defined to be the positive direction, a right turn will have a negative k v. The control space is split into three regions according to vehicle s position, as shown in Figure 1. In each region the controller uses the corresponding section of reference line to compute the closest point on the line. 3) Performance: To improve the levels of comfort during the maneuver, we impose constraints on the centripetal acceleration a c. We assume that if a c is below a given threshold α, then the comfort level for the passenger is ω acceptable. Since k v = and a c = ω vl 2 + v2 c, v 2 l +v 2 c if we restrict a c α and assume vehicle s curvature is the same as road s k c, then we get: { α k v cap = c, in region 2, (17) v limit, in region 1,3, where v cap is a soft speed upper bound. For the control region that is modeled as a straight line, the velocity capacity v cap is set to the speed limit of the road. To ensure that the ego vehicle will not take very long time in the intersection, and will keep with the car flow after completing the turning maneuver, we impose that its velocity should approach this limit as closely as possible. Hence, we define: g per (C) = w 5 ( vl 2 i + vc 2 i v cap ) 2. (18) 4) Collision avoidance: In reality, sensors are noisy and additionally, there is uncertainty in the driver s behavior. To predict the behavior of the target vehicles, this paper employs a probabilistic model for the motion of target cars [23]. We assume the target vehicle will stay at the center of the lane, and keep its current speed by its driver, regardless of the existence of ego vehicle. The random lateral fluctuation and speed fluctuation due to driver s uncertain behavior and sensor noise are modeled as a normal distribution in lateral and longitudinal velocity. The heading angle θ tj of the j-th target vehicle is assumed equal to the tangential direction of the road. Let us denote v tjx and v tjy the longitudinal and lateral velocity components, respectively, and W j = [v tjx v tjy ] T the velocity vector of the target vehicle V j. Then the aforementioned uncertainty is incorporated by modeling v tjx and v tjy as Gaussian, white processes, that are uncorrelated to each other, of known mean values v tx j, v ty j, and standard deviations [ σ] xj and σ yj, respectively. In summary, vtjx we have: W j = N(W v j, Q j), where W j is the mean tjy [ ] σ 2 value of the velocity vector W j, and Q j = xj 0 0 σy 2 is j the covariance matrix. Similarly we model the uncertainty on the position coordinates Z j [ = [x ] tj y tj ] T of the j-th target xtj vehicle: we define Z j = N(Z y j, P j), where Z j tj is the mean value and P j is the covariance matrix of Z j. If the rotation [ matrix of j-th ] vehicle at time instant i is cos(θtj,i) sin(θ R j,i = tj,i), then we have: sin(θ tj,i) cos(θ tj,i) Z j,(i+1) = [ 1 T ] [ ] Z j,i. (19) R j,i W j,i Under the assumption that θ tj,i is known with certainty, and since Z j,i, W j,i are Gaussian, we have: Z j,(i+1) ( [1 ] [ ] Z N T j,i R j,i W j,i, [ 1 T ] [ [ ]) P j,i R j,i Q j,i Rj,i] T T Thus the iterative equation of the covariance matrix is: P j,(i+1) = P j,i + R j,i Q j,i R T j,it 2, (20) Q j,(i+1) = R j,(i+1) Q j,i R T j,(i+1). (21) Using the same time step and sample rate as the prediction of ego vehicle, the sampled trajectories of target vehicles can be computed as in Figure 3. For each point, the multivariate normal distribution of position vector appears like an ellipse, and we assume the position of vehicle is within 3σ of its position vector, which can be acquired from diagonalizing the covariance matrix P j,i. To compute the cost of collision, this model also encodes the size of the car and a safe margin, which implies that

5 where: η(x e,i, y e,i ) = [ xe,i x = ( tj,i y e,i y tj,i ] R 1 j,i ) [ d 2 l 0 0 d 2 c ] (R T j,i [ ] T xe,i x tj,i ) y e,i y tj,i Fig. 3. Propagating the uncertainty of the position of the target car during the prediction stage. Green ellipses indicate the uncertainty area, and red ellipses indicate collision free region. any possible collision in this region is prohibited. This region takes the size of ego vehicle, the size of target vehicle (d sl and d sc ) and a constant braking space (d bl and d bc ) into account. {} l and {} c still denote the value along longitudinal and lateral direction. Coincide with the uncertainty region, the shape of the vehicle is also treated as an ellipse whose length of longitudinal and lateral axes are assumed to be the length and width of a vehicle. Since the target vehicles are assumed to stay in their lanes, the lateral braking distance is small, so that it does not block the lane nearby. The speed-based longitudinal braking distance considers the driver s reaction time t r, such that d bl = t r max{v xj, v l } + v2 x j. 2 v lmax 2 v xjmax The longitudinal and lateral length d l and d c of the final collision free region are: d l = d ul + d sl + d sl + d bl, d c = d uc + d sc + d sl + d bc. The first two terms express the space occupied by the target vehicles; the third term denotes the minimal distance from the mass center to the target vehicle, and the last term stands for the braking distance as shown in Figure 4. Fig. 4. The left part is the space occupied by the target vehicle and the right part is the final collision free region A barrier function is defined to encode the collision cost. We treat each ellipse as an equipotential surface and form a potential field whose value is infinite at the center and decaying to zero as the input approaches infinity. The barrier function h is defined as: h(η) = v 2 l P (η c) 5 + d(η c) + c 5 + dc, (22) Fig. 5. The considered barrier function. where x e,i, y e,i is the position of the ego vehicle at time instant i, and P is a large constant determining the height of the barrier at the edge of the safe margin; c controls the position of the potential wall ; d is a tuning value to make sure the derivative this barrier function is negatively defined everywhere. Note that η(x e,i, y e,i ) = 1 stands for a ellipse rotated by R j,i, and d l and d c are the length of major and minor axes. If no collision happens, then η(x e,i, y e,i ) remains greater than 1 for all i. We define: g col (C, C t ) = w 6 j=1 V w n h(c i, C tij ), (23) where w n is a varying weight through time. We set w n = 1 for first half of sample points because possible collisions in near future should have the highest priority. The weight then drops from 1 to 0.5 linearly for the last half of sample points. Since the controller should favor decelerating when it comes to safety consideration, a decaying weight assigns a negative derivative of the cost function along time axis, and encourages the controller to postpone collision to later time. IV. SIMULATION RESULTS The performance of the merging controller is evaluated through computer simulations in MATLAB. Mercedes CLS 63 AMG is used as the vehicle model [24]. The parameters in the car model (2)-(6) are set equal to: a = 1.105m, b = 1.738m, I = 1549kg m 2, m = 2220kg, C f = N/m, C r = N/m. The road width is 3m. The origin of the global frame is set at the intersection entrance C 1, and the position coordinates of C 2 are (9, 7). The bounds of the control inputs are π 3 δ f π 3, and 14000N F lf 8000N, respectively. The centripetal acceleration bound is set to 3m/s 2. The speed limit of the road is 15m/s 35mph. The time step is T = 0.005s; the time horizon is 1.6s; the length of the control sequence is M = 8, which means r = 40; the number of the sample points on the trajectory is K = 16. The MATLAB function

6 (a) t=1.865s (b) t=3.105s (c) t=3.725s (d) t=4.345s Fig. 6. Scenario 1: The ego vehicle decelerates to avoid collision with both target vehicles. f mincon with algorithm sqp is used as nonlinear optimizer. The vehicle size is dsl = 2.5m and dsc = 1m. The lateral braking distance is dbc = 0.5m. The driver reaction time is tr = 0.7s The radius of the sensing region of the ego vehicle is 35m. The control input computed by controller applies for half control horizon, which makes the controller run at 10Hz. We performed two sets of simulations. In the first scenario, one target vehicle is initially 43m away from the center of intersection, and a second target vehicle is 56m away from the center of intersection. Both target vehicles move with velocity vl = 15m/s. The ego vehicle is 22.5m away from the center of intersection with velocity components vl = 9.3m/s and vc = 0.01m/s. The ego vehicle heads along the the tangential direction of the center line of the road, but has an offset of 0.3m from the center line. The covariances of the velocity components of the target vehicles are σx2 = 3m2 and σy2 = 0.707m2, respectively. The average runtime for a single iteration is 20.59s. Figures 6(a) to 6(b) illustrate the ego vehicle approaching the intersection and decelerating to avoid both target vehicles. More specifically: the first vehicle is detected at t = 1s, while the second vehicle is detected at t = 1.6s. Since there is no safe space between the two target vehicles, the ego vehicle keeps decelerating to maintain safe separation with the second target vehicle. Then, the ego vehicle accelerates to meet the speed criteria (Fig. 6(c) to 6(d)). The velocity of the ego vehicle (Fig. 7(a)) conveys the same result. No collision occurs as verified by Fig. 7(b), while Fig. 7(c) and Fig. 7(d) illustrate that the bounds of the control inputs are never violated. Towards the end of simulation, the velocity of the ego vehicle approaches the 15m/s speed limit. In the second scenario we kept the same initial system configuration, except that the second target vehicle is 78m (a) The velocity of the ego vehicle is depicted in blue. For comparison, the resulting velocity under the same controller for the case of no target vehicles is plotted in red. (b) The evolution of the barrier functions of ego vehicle w.r.t. each one of the targets (in blue and green, respectively); no collisions occur as verified by the value remaining greater than 1. (c) The computed control input Flf (d) The computed control input δf vs time. The red lines stand for the vs time. The red lines stand for the control input bounds. control input bounds. Fig. 7. Performance analysis of Controller away from the center of the intersection. Figures 8(a) to 8(b) illustrate that the ego vehicle decelerates to avoid the first target vehicle. Since there is enough safe space, the ego vehicle accelerates to overtake the second target vehicle (Fig. 8(c) to 8(d)). The evolution of the ego vehicle s velocity in Fig. 9(a) is in agreement and conveys the same result. Finally, no collisions occur as verified by Fig. 9(b). The control input bounds are not violated as shown in Fig. 9(c) and Fig. 9(d). V. C ONCLUSIONS This paper presented a line following and collision avoidance method for automated turning and merging, while taking into account the 2-D geometry of the intersection and the uncertain behavior of target vehicles. An optimization problem is solved utilizing a receding horizon control approach. Emphasis is given on reducing the search dimension and on adopting a nonlinear model. The simulations verify that the approach succeeds in keeping the ego vehicle on the center line and close to the desired speed, while the control inputs remain within bounds and collisions are avoided. Ongoing work focuses on establishing formal guarantees on the recursive feasibility of the approach, further improving the computational effort, as well as on considering vehicles on adjacent lanes. R EFERENCES [1] M. Caceres, F. Sottile, and M. A. Spirito, WLAN-based real time vehicle locating system, in IEEE 69th Vehicular Technology Conference, 2009, April 2009, pp. 1 5.

7 (a) t = sec (b) t = sec (a) The velocity of the ego vehicle (b) The evolution of the barrier functions of ego vehicle w.r.t. each one of is depicted in blue. For comparison, the resulting velocity under the same the targets (in blue and green, respectively); no collisions occur as veri- controller for the case of no target vehicles has been plotted in red. fied by the value remaining greater than 1. (c) t = sec (d) t = sec Fig. 8. Scenario 2: The ego vehicle decelerates to avoid collision with the first target. It then detects the second target in the rear, and accelerates to avoid collision. (c) control input F lf line is the bound Fig. 9. vs time. Red (d) control input δ f vs time. Red line is the bound Performance analysis of Controller [2] X. Mao, D. Inoue, S. Kato, and M. Kagami, Amplitude-modulated laser radar for range and speed measurement in car applications, IEEE Transactions on Intelligent Transportation Systems, vol. 13, no. 1, pp , March [3] S. Sato, M. Hashimoto, M. Takita, K. Takagi, and T. Ogawa, Multilayer Lidar-based pedestrian tracking in urban environments, in 2010 IEEE Intelligent Vehicles Symposium, June 2010, pp [4] T. Ogawa and K. Takagi, Lane recognition using on-vehicle LIDAR, in 2006 IEEE Intelligent Vehicles Symposium, 2006, pp [5] H. G. Min, X. C. Li, P. P. Sun, X. M. Zhao, and Z. G. Xu, Visual odometry for on-road vehicles based on trifocal tensor, in 2015 IEEE First International Smart Cities Conference (ISC2), Oct 2015, pp [6] Q. Jin, G. Wu, K. Boriboonsomsin, and M. Barth, Multi-agent intersection management for connected vehicles using an optimal scheduling approach, in 2012 International Conference on Connected Vehicles and Expo, Dec 2012, pp [7] M. A. S. Kamal, J. Imura, A. Ohata, T. Hayakawa, and K. Aihara, Coordination of automated vehicles at a traffic-lightless intersection, in 16th IEEE International Conference on Intelligent Transportation Systems, Oct 2013, pp [8] M. A. S. Kamal, J. i. Imura, T. Hayakawa, A. Ohata, and K. Aihara, A vehicle-intersection coordination scheme for smooth flows of traffic without using traffic lights, IEEE Transactions on Intelligent Transportation Systems, vol. 16, no. 3, pp , June [9] A. Colombo and D. D. Vecchio, Least restrictive supervisors for intersection collision avoidance: A scheduling approach, IEEE Transactions on Automatic Control, vol. 60, no. 6, pp , June [10] Y. J. Zhang, A. A. Malikopoulos, and C. G. Cassandras, Optimal control and coordination of connected and automated vehicles at urban traffic intersections, in 2016 American Control Conference, July 2016, pp [11] A. De La Fortelle and X. Qian, Autonomous driving at intersections: combining theoretical analysis with practical considerations, in ITS World Congress 2015, Bordeaux, France, Oct [12] A. Kotsialos and M. Papageorgiou, Nonlinear optimal control applied to coordinated ramp metering, IEEE Transactions on Control Systems Technology, vol. 12, no. 6, pp , Nov [13] T. Awal, L. Kulik, and K. Ramamohanrao, Optimal traffic merging strategy for communication- and sensor-enabled vehicles, in 16th International IEEE Conference on Intelligent Transportation Systems, Oct 2013, pp [14] S. M. Loos and A. Platzer, Safe intersections: At the crossing of hy- brid systems and verification, in 14th IEEE International Conference on Intelligent Transportation Systems, Oct 2011, pp [15] G. Schildbach, M. Soppert, and F. Borrelli, A collision avoidance system at intersections using robust model predictive control, in 2016 IEEE Intelligent Vehicles Symposium, June 2016, pp [16] J. Rios-Torres and A. A. Malikopoulos, Automated and cooperative vehicle merging at highway on-ramps, IEEE Transactions on Intelligent Transportation Systems, vol. PP, no. 99, pp. 1 10, [17] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, Kinematic and dynamic vehicle models for autonomous driving control design, in 2015 IEEE Intelligent Vehicles Symposium, June 2015, pp [18] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat, MPCbased approach to active steering for autonomous vehicle systems, International Journal of Vehicle Autonomous Systems, vol. 3, no. 2-4, pp , [19] G. Palmieri, M. Baric, F. Borrelli, and L. Glielmo, A robust lateral vehicle dynamics control, in 10th International Symposium on Advanced Vehicle Control, [20] V. A. Shia, Y. Gao, R. Vasudevan, K. D. Campbell, T. Lin, F. Borrelli, and R. Bajcsy, Semiautonomous vehicular control using driver modeling, IEEE Transactions on Intelligent Transportation Systems, vol. 15, no. 6, pp , Dec [21] T. A. Johansen, Introduction to nonlinear model predictive control and moving horizon estimation. [Online]. Available: [22] R. Cagienard, P. Grieder, E. C. Kerrigan, and M. Morari, Move blocking strategies in receding horizon control, in 43rd IEEE Conference on Decision and Control, Dec 2004, pp [23] G. R. de Campos, A. H. Runarsson, F. Granum, P. Falcone, and K. Alenljung, Collision avoidance at intersections: A probabilistic threat-assessment and decision-making system for safety interventions, in 17th International IEEE Conference on Intelligent Transportation Systems, Oct 2014, pp [24] C. Constant, Car engineering, Mercedes CLS 63 AMG, 2016,