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

Lixing Huang and Dimitra Panagou

Abstract— Accidents at intersections are highly related to thedriver’s mis-decision while performing turning and mergingmaneuvers. This paper proposes a merging/turning controllerfor an automated vehicle, called the ego vehicle, which avoidscollisions with surrounding (target) vehicles. An optimization-based control problem is defined based on receding horizoncontrol, that parameterizes the system trajectory with thecontrol input and employs a nonlinear model on the ego vehicledynamics. Most existing solutions focus on 1-D (longitudinal)motion for the vehicles. In this paper, the 2-D motion of theturning/merging vehicle is considered instead. The intersectionis modeled under realistic traffic conditions, a probabilisticmodel 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 fol-lowing when turning/merging, and collision avoidance, whilesimulations of several scenarios validate its performance.

I. INTRODUCTION

Recent advances in sensor technology have enabled the de-velopment of control systems that possess more sophisticatedperception of their environment and own state with lowercomputational effort. Automated and autonomous vehiclesis a relevant application domain. GPS can locate the vehiclewithin the error of 8m with the aid of WLAN [1]. Radar[2] and Lidar [3], [4] provide an accurate assessment of thevelocity and the spatial information of the surrounding envi-ronment (other vehicles, physical obstacles). The maturenessof the visual odometry allows the vehicle to compute its owntrajectory and velocity [5]. Benefited from the improvementof the perception module, much effort can be devoted intodesigning advanced controllers to guide the vehicle withcertain levels of safety, and reduce road accidents.

Research addressing the safe passing through an intersec-tion is rich. Some researchers focus on centralized control ap-proaches; [6] proposes a scheduling system that coordinatesthe departure time of the engaged vehicles. Kamal et al. de-fine several cross-collision points according to the number oflanes on each road, and schedule the arrival time to improvetraffic efficiency [7], [8]. Similarly, [9] transfers the problemof computing a maximal controlled invariant set to a well-formed scheduling problem, where a centralized controllercoordinates the sequence of vehicles passing through theintersection. Decentralized control approaches have also been

Lixing Huang is with the Department of Electrical Engineeringand Computer Science, University of Michigan, Ann Arbor, MI, USAlixhuang@umich.edu

Dimitra Panagou with the Department of Aerospace Engineering, Uni-versity of Michigan, Ann Arbor, MI, USA dpanagou@umich.edu

This work was in part supported by the U. of Michigan SummerUndergrate REsearch (SURE) Program.

developed; in [10], each vehicle receives an identifier froma coordinator, and optimizes fuel consumption with respectto the constraints of the time for exiting the merging zone,based on the solution of the vehicle preceding to it. Qian etal. define a collision region in an 1-D frame regarding to thepotential collision object, and optimize the trajectory to keepit out of this region [11]. Multiple techniques are employedfor the merging problem; Kotsialos et al. focus on the trafficflow instead of a single vehicle, and build an optimizationproblem based on a macroscopic traffic model [12]. Awal etal. also consider a stream of cars, and develop a mergingalgorithm to compute a feasible merging list [13]. This lineof research focuses on how to improve the efficiency of thetraffic and prevent congestion. However, the 1-D model of theintersection, and the linear model adopted for the vehicles, inprinciple neglect that the driver may have to perform complexmaneuvers for collision avoidance, as well as the vehicledynamics. Hybrid control approaches have also appeared: Adiscrete decision-making system regarding to the stoplighton single or two lanes is proposed in [14]. The recent workin [15] assumes no coordinator and V2V communication,considering only vehicle’s longitudinal motion; the afore-mentioned paper models the uncertain behavior of targetvehicles as a distance range with respect to an accelerationbound, and utilizes an MPC approach so that the ego vehiclemaintains a reference position and speed. A recent detailedliterature review on related topics is provided in [16].

This paper proposes a decentralized optimization-basedcontrol approach for the merging/turning of an automatedvehicle into an intersection. No explicit communication isassumed among vehicles. A Nonlinear Model PredictiveControl (NMPC) method is used to predict the evolution ofthe merging vehicle trajectories, as well as of the environ-ment (target vehicles). The computed control sequence forthe merging vehicle can be either applied on the vehicle inan automated fashion, or be informed to the driver so thats/he takes action to avoid collisions. A 2-D intersection andmerging model is built to imitate realistic road conditions.A nonlinear dynamics model for the merging vehicle isadopted to improve the accuracy of the prediction. Hencethe NMPC approach on one hand provides a line-followingcontroller that accounts for realistic maneuverability capa-bilities. A probabilistic model is employed to predict theuncertain maneuver of the target vehicles, and is built withina novel collision avoidance model. The main contributionof the paper is the use of a nonlinear vehicle model andof probabilistic collision prediction to take realistic roadconditions into account, while accomplishing lane keeping

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 givenin Section II. Section III presents the proposed optimization-based control formulation, while Section IV includes sim-ulations to evaluate the performance of the solution. Con-clusions and thoughts on future research are summarized inSection V.

II. PROBLEM FORMULATION

We consider a road configuration in which a car intendsto make a right turn from road ζ to the target road γ. In asimilar spirit, one may consider the problem of a car mergingfrom the ramp to the highway. In both cases, the turning ormerging car needs to make a smooth transition into the newroad by following the center line of its current lane to thetarget lane, while avoiding collisions with other cars.

We consider a sequence of M vehicles Vj , j ∈ {1, ..., V },called targets in the sequel, that move straight on the targetroad γ. We assume that each target vehicle Vj moves alongthe reference center line of the road γ. Let vtj , θtj , xtj ,ytj denote the linear velocity, the heading angle, and theposition coordinates with respect to a global coordinate frameG, respectively; the kinematic equations of motion for thetarget vehicle Vj are then as follows:

xtj = vtj cos θtj , (1a)ytj = vtj sin θtj . (1b)

A probability model on vtj is built to account for uncertaineffects, 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 thatthe ego vehicle moves forward following a reference centerline, and that has access to the velocity and position of thetarget vehicles that lie within its sensing region through GPS,Lidar and other sensors. The ego vehicle should: (i) Avoidcollisions with cars running on the target road. (ii) Follow areference line corresponding to the road configuration. (iii)Be within lateral acceleration bounds while maintaining adesired speed as closely as possible.

Fig. 1. A typical intersection configuration: The ego vehicle on road ζmakes a right turn to merge into road γ, trying to avoid targets V1 and V2.

Fig. 2. The parameters of the dynamic model of the ego vehicle.

We assume that the geometry of the road and the locationsof the points C1 and C2 are known beforehand as part of amap available to the ego vehicle. In this paper, an orthogonalintersection is chosen as shown in Figure 1, and the egovehicle is considered to make a right turn. For simplicity, theroads are modeled as straight lines. The intermediate phaseis modeled as an arc of an ellipse that connects C1 and C2,so that the center line from road ζ to road γ is a C1 function.

To describe the behavior of the ego vehicle, a nonlineardynamic bicycle model is adopted [17]. In the sequel, xe, ye,θe denote the position coordinates and heading angle of thevehicle with respect to (w.r.t.) the global frame G, and vl, vc,and ω denote the longitudinal, lateral and angular velocityof the vehicle expressed in the vehicle-fixed frame B.

Furthermore, we use postfix {}∗f and {}∗r to denote frontand rear part; {}l∗ and {}c∗ to denote longitudinal and lateraldirection. Referring to Fig. 2, we define:

δf , δr : steering anglevxf , vxr, vyf , vyr : velocity along/vertical to main axisvlf , vlr, vcf , vcr : longitudinal/lateral velocity of tire

αf , αr : slipping ratio of tireFlf , Flr, Fcf , Fcr : longitudinal/lateral force on tireFxf , Fxr, Fyf , Fyr : longitudinal/lateral force on body frame

a, b : distance from gravity center to thefront, rear tire

m, I : vehicle mass and inertia about zB axisCf , Cr : tire stiffness

For compactness, ? ∈ {f, r} in the sequel indicates front andrear part. The dynamics of the ego vehicle are expressed inthe body frame [18] as:

vl = vcω +2Fxf + 2Fxr

m, (2a)

vc = −vlω +2Fyf + 2Fyr

m, (2b)

ω =2aFyf + 2bFyr

I. (2c)

The kinematics of the front and rear wheel are given as:

vx? = vl, (3a)vyf = vc + aω, (3b)

vyr = vc − bω, (3c)vl? = vy? sin δ? + vx? cos δ?, (3d)vc? = vy? cos δ? − vx? sin δ?. (3e)

The slip angle of front and rear tire is defined as:

α? =vc?vl?

, (4a)

respectively, while the forces on the tires are given as:

Fc∗ = −C?α?, (5a)Fx? = Fl? cos δ? − Fc? sin δ?, (5b)Fy? = Fl? sin δ? + Fc? cos δ?. (5c)

A linear tire model [19], [20] connects the lateral tireforce with the slip angle and the stiffness constant. Thelongitudinal force on the rear tire is set to zero, since weassume the car is front-wheel drive. The rear steering angleis set to zero, δr = 0. The longitudinal force Flf is thus setequal to traction force which is one input of the system, andthe front steering angle δf is another control input.

The kinematics of the ego vehicle w.r.t. the global frameG are described as:

xe = vl cos θe − vc sin θe, (6a)ye = vl sin θe + vc cos θe, (6b)

θe = ω. (6c)

Combining equations (2)-(6), the system model is writtenin compact form as:

x = f(x, u), (7)

where x = [vl vc ω xe ye θe]T is the state vector, u =

[Flf δf ]T is the control input vector, and the vector functionf(., .) comprises equations (2)-(6).

III. MOVE BLOCKING MPC APPROACH

A nonlinear receding horizon control approach is em-ployed to compute safe trajectories for the ego vehicle whilerespecting the requirements described earlier. This approachsolves an optimal control problem for a finite predictionhorizon, applies the first control input over a (shorter) controlhorizon, and repeats the process recursively as the systemevolves. The standard MPC formulation is [21]:

minimizeN−1∑i=0

l(xi, ui) (8)

subject to xi+1 = xi + f(xi, ui) T, (9)x0 = x0, (10)

umin ≤ui ≤ umax, (11)

where N is the prediction horizon, i ∈ {0, 1, 2, . . . , N − 1},xi ∈ Rn is the state vector, ui ∈ Rm is the vector of controlinputs, f(·, ·) is the vector system dynamics; x0 is the initialstate, and T is the time step. The bounds on the controlinputs and on the system dynamics are viewed as linear andnonlinear constraints, respectively. Since the state trajectoryand control input trajectory are free variables, the search

dimension of this optimization problem grows very fast asthe prediction horizon increases. In addition, for a nonlinearMPC problem the time step of the system evolution shouldbe small enough to ensure the accuracy of the nonlineardiscrete model. This results in high-dimensional state andcontrol vectors for a fixed time horizon, i.e., in a high-dimensional search space. This in turn limits the performanceand applicability of the controller in terms of the requiredcomputational effort. To speed up the optimization process,the move blocking technique is employed. A sequentialapproach is also used to parametrize the system trajectorywith block control inputs, which now become the only freevariables. Briefly, with the move blocking technique a singlecontrol input is kept the same over several time steps insteadof one, so that a fixed length of control input sequence cancover a longer horizon. The parametrized trajectory is alsoallowed to be sampled in order to decrease the number ofpoints on the trajectory, which keeps the computational timeof the cost function small.

A. Problem Formulation

Instead of solving for U ′ = [u′0T, u′1

T, ..., u′N−1

T]T ∈

RmN , where N is the prediction horizon and m is thedimension of the control vector, we will parametrize U ′ withthe block control sequence U = [u0

T , u1T , ..., uM−1

T ]T ∈RmM , such that U ′ = (S ⊗ Im)U , where ⊗ denotes theKronecker product, Im is the identity matrix with size ofm × m and S ∈ RN×M is a blocking matrix [22]. Ifwe denote 1r as column vector with all values equal to1 and of length r, the blocking matrix can be presented

as: S =

1r0 0 ... 00 1r1 ... 0

......

. . ....

0 0 ... 1rM

To interpret this transform, we

should notice u′0 = u′1 = ... = u′r0 = u1. We use uniformblocking here and suppose N is a multiple of M , such thatr0 = r1 = ... = rM = N/M = r.

Despite that the free dimension of the problem decreasesfrom N to M , the long trajectory still leads to a high com-putational effort demanded in cost function. The trajectory issubsampled to reduce the computation time. If the originaltrajectory of the ego vehicle is computed as

X = {xi|xi+1 = xi + Tf(xi, ui), i = 0, 1, ..., N, x0 = x0},(12)

then using k = NK , where K is the length of the sampled

trajectory, as the sample rate, we have our sampled egovehicle trajectory C as:

C = {xkn−1|xkn−1 ∈ X , n = 1, 2...,K}, (13)

whereas the sampled target vehicle trajectory is similarlycomputed and denoted as Ct.

The goal for the ego vehicle is to follow the centerline andavoid collisions with least traction/braking force and changein steering angle. The cost function is defined as:

J =geff (U) + gfol(C) + gper(C) + gcol(C, Ct), (14)

where geff is the cost of control effort, gfol penalizes thedeviation from the reference line, gper penalizes the deviationfrom the desired velocity, and gcol is the cost of collision.These are defined in detail in the sequel.

With the definition of the sampled trajectory set andcontrol sequence from equations (12) and (13), respectively,the optimization problem is summarized as:

minimizeU

J(C,Ct,U),

subject to umin ≤U ≤ umax.

1) Effort cost: We wish the ego vehicle to achieve thecontrol goal with as little traction or braking force, and aslittle change in steering angle as possible. Hence we define:

geff (U) = w1

M−1∑i=0

Flf i2 + w2

M−1∑i=0

δf2

i , (15)

where w1 and w2 are positive weights. This part penalizesthe tracking force and the change of the steering angle. Thevehicle is thus expected to keep its current state.

2) Line following: If the ego vehicle is following a lineperfectly, the distance between its current position and theclosest point on the center line should be zero. Besides, thevehicle’s curvature and the curvature of the closest point onthe center line should be the same. This part of cost functionis expected to lay every point of optimal trajectory on thereference line. We define:

gfol(C) = w3

K−1∑i=0

D2i + w4

K−1∑i=0

(kvi − kci)2, (16)

where D is the distance from the vehicle to closest point onthe curve; w3 and w4 are positive weights. kv = ω√

v2l +v2c

and kc are the curvature of the path followed by the vehicleand of the reference curve respectively. Since the counter-clockwise is defined to be the positive direction, a right turnwill have a negative kv .

The control space is split into three regions according tovehicle’s position, as shown in Figure 1. In each region thecontroller uses the corresponding section of reference line tocompute the closest point on the line.

3) Performance: To improve the levels of comfort duringthe maneuver, we impose constraints on the centripetalacceleration ac. We assume that if ac is below a giventhreshold α, then the comfort level for the passenger isacceptable. Since kv = ω√

v2l +v2c

and ac = ω√v2l + v2c ,

if we restrict ac ≤ α and assume vehicle’s curvature is thesame as road’s kc, then we get:

vcap =

{√αkc, in region 2,

vlimit, in region 1,3,(17)

where vcap is a soft speed upper bound. For the controlregion that is modeled as a straight line, the velocity capacityvcap is set to the speed limit of the road. To ensure that theego 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 thislimit as closely as possible. Hence, we define:

gper(C) = w5

K−1∑i=0

(√v2li + v2ci − vcap)

2. (18)

4) Collision avoidance: In reality, sensors are noisy andadditionally, there is uncertainty in the driver’s behavior. Topredict the behavior of the target vehicles, this paper employsa probabilistic model for the motion of target cars [23].We assume the target vehicle will stay at the center of thelane, and keep its current speed by its driver, regardless ofthe existence of ego vehicle. The random lateral fluctuationand speed fluctuation due to driver’s uncertain behavior andsensor noise are modeled as a normal distribution in lateraland longitudinal velocity. The heading angle θtj of the j-thtarget vehicle is assumed equal to the tangential directionof the road. Let us denote vtjx and vtjy the longitudinaland lateral velocity components, respectively, and Wj =[vtjx vtjy ]T the velocity vector of the target vehicle Vj . Thenthe aforementioned uncertainty is incorporated by modelingvtjx and vtjy as Gaussian, white processes, that are uncor-related to each other, of known mean values v′txj

, v′tyj , andstandard deviations σxj and σyj , respectively. In summary,

we have: Wj =

[vtjxvtjy

]∼ N(W ′j , Qj), where W ′j is the mean

value of the velocity vector Wj , and Qj =

[σ2xj

0

0 σ2yj

]is

the covariance matrix. Similarly we model the uncertainty onthe position coordinates Zj = [xtj ytj ]T of the j-th target

vehicle: we define Zj =

[xtjytj

]∼ N(Z ′j , Pj), where Z ′j

is the mean value and Pj is the covariance matrix of Zj .If the rotation matrix of j-th vehicle at time instant i is

Rj,i =

[cos(θtj ,i) − sin(θtj ,i)sin(θtj ,i) cos(θtj ,i)

], then we have:

Zj,(i+1) =[1 T

] [ Zj,iRj,i Wj,i

]. (19)

Under the assumption that θtj ,i is known with certainty, andsince Zj,i, Wj,i are Gaussian, we have: Zj,(i+1) ∼

N

([1 T

] [ Z ′j,iRj,i W

′j,i

],[1 T

] [Pj,i 00 Rj,iQj,iR

Tj,i

] [1T

])Thus the iterative equation of the covariance matrix is:

Pj,(i+1) = Pj,i +Rj,iQj,iRTj,iT

2, (20)

Qj,(i+1) = Rj,(i+1)Qj,iRTj,(i+1). (21)

Using the same time step and sample rate as the predictionof ego vehicle, the sampled trajectories of target vehicles canbe computed as in Figure 3. For each point, the multivariatenormal distribution of position vector appears like an ellipse,and we assume the position of vehicle is within 3σ of itsposition vector, which can be acquired from diagonalizingthe covariance matrix Pj,i.

To compute the cost of collision, this model also encodesthe size of the car and a safe margin, which implies that

Fig. 3. Propagating the uncertainty of the position of the target car duringthe prediction stage. Green ellipses indicate the uncertainty area, and redellipses indicate collision free region.

any possible collision in this region is prohibited. Thisregion takes the size of ego vehicle, the size of targetvehicle (dsl and dsc) and a constant braking space (dbland dbc) into account. {}∗l and {}∗c still denote the valuealong longitudinal and lateral direction. Coincide with theuncertainty region, the shape of the vehicle is also treatedas an ellipse whose length of longitudinal and lateral axesare assumed to be the length and width of a vehicle. Sincethe target vehicles are assumed to stay in their lanes, thelateral braking distance is small, so that it does not block thelane nearby. The speed-based longitudinal braking distanceconsiders the driver’s reaction time tr, such that

dbl = trmax{vxj , vl}+ | v2l2vlmax

−v2xj

2vxjmax

|.

The longitudinal and lateral length dl and dc of the finalcollision free region are:dl = dul + dsl + dsl + dbl, dc = duc + dsc + dsl + dbc.

The first two terms express the space occupied by the targetvehicles; the third term denotes the minimal distance fromthe mass center to the target vehicle, and the last term standsfor the braking distance as shown in Figure 4.

Fig. 4. The left part is the space occupied by the target vehicle and theright 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 forma potential field whose value is infinite at the center anddecaying to zero as the input approaches infinity. The barrierfunction h is defined as:

h(η) =P

(η − c)5 + d(η − c) + c5 + dc, (22)

where: η(xe,i, ye,i) =

= (

[xe,i − xtj ,iye,i − ytj ,i

]R−1j,i )

[d2l 00 d2c

](R−Tj,i

[xe,i − xtj ,iye,i − ytj ,i

]T)

Fig. 5. The considered barrier function.

where xe,i, ye,i is the position of the ego vehicle at timeinstant i, and P is a large constant determining the heightof the barrier at the edge of the safe margin; c controls theposition of the potential ”wall”; d is a tuning value to makesure the derivative this barrier function is negatively definedeverywhere. Note that η(xe,i, ye,i) = 1 stands for a ellipserotated by Rj,i, and dl and dc are the length of major andminor axes. If no collision happens, then η(xe,i, ye,i) remainsgreater than 1 for all i. We define:

gcol(C, Ct) = w6

K−1∑i=0

V∑j=1

wnh(Ci, Ctij ), (23)

where wn is a varying weight through time. We set wn = 1for first half of sample points because possible collisionsin near future should have the highest priority. The weightthen drops from 1 to 0.5 linearly for the last half of samplepoints. Since the controller should favor decelerating whenit comes to safety consideration, a decaying weight assigns anegative derivative of the cost function along time axis, andencourages the controller to postpone collision to later time.

IV. SIMULATION RESULTS

The performance of the merging controller is evaluatedthrough computer simulations in MATLAB. Mercedes CLS63 AMG is used as the vehicle model [24]. The parametersin the car model (2)-(6) are set equal to: a = 1.105m,b = 1.738m, I = 1549kg m2, m = 2220kg, Cf =26018.6N/m, Cr = 34704.6N/m. The road width is 3m.The origin of the global frame is set at the intersectionentrance C1, and the position coordinates of C2 are (9, 7).The bounds of the control inputs are −π3 ≤ δf ≤ π

3 , and−14000N ≤ Flf ≤ 8000N , respectively. The centripetalacceleration bound is set to 3m/s2. The speed limit of theroad is 15m/s ≈ 35mph. The time step is T = 0.005s; thetime horizon is 1.6s; the length of the control sequence isM = 8, which means r = 40; the number of the samplepoints on the trajectory is K = 16. The MATLAB function

(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 withboth target vehicles.

fmincon with algorithm sqp is used as nonlinear optimizer.The vehicle size is dsl = 2.5m and dsc = 1m. The lateralbraking distance is dbc = 0.5m. The driver reaction timeis tr = 0.7s The radius of the sensing region of the egovehicle is 35m. The control input computed by controllerapplies for half control horizon, which makes the controllerrun at 10Hz.

We performed two sets of simulations. In the first scenario,one target vehicle is initially 43m away from the center ofintersection, and a second target vehicle is 56m away fromthe center of intersection. Both target vehicles move withvelocity vl = 15m/s. The ego vehicle is 22.5m away fromthe center of intersection with velocity components vl =9.3m/s and vc = 0.01m/s. The ego vehicle heads along thethe tangential direction of the center line of the road, but hasan offset of 0.3m from the center line. The covariances ofthe velocity components of the target vehicles are σ2

x = 3m2

and σ2y = 0.707m2, respectively.

The average runtime for a single iteration is 20.59s.Figures 6(a) to 6(b) illustrate the ego vehicle approaching theintersection 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 thereis no safe space between the two target vehicles, the egovehicle keeps decelerating to maintain safe separation withthe second target vehicle. Then, the ego vehicle acceleratesto meet the speed criteria (Fig. 6(c) to 6(d)). The velocityof the ego vehicle (Fig. 7(a)) conveys the same result. Nocollision occurs as verified by Fig. 7(b), while Fig. 7(c) andFig. 7(d) illustrate that the bounds of the control inputs arenever violated. Towards the end of simulation, the velocityof the ego vehicle approaches the 15m/s speed limit.

In the second scenario we kept the same initial systemconfiguration, except that the second target vehicle is 78m

(a) The velocity of the ego vehicleis depicted in blue. For comparison,the resulting velocity under the samecontroller for the case of no targetvehicles is plotted in red.

(b) The evolution of the barrier func-tions of ego vehicle w.r.t. each one ofthe targets (in blue and green, respec-tively); no collisions occur as veri-fied by the value remaining greaterthan 1.

(c) The computed control input Flf

vs time. The red lines stand for thecontrol input bounds.

(d) The computed control input δfvs time. The red lines stand for thecontrol 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 decel-

erates to avoid the first target vehicle. Since there is enoughsafe space, the ego vehicle accelerates to overtake the secondtarget vehicle (Fig. 8(c) to 8(d)). The evolution of the egovehicle’s velocity in Fig. 9(a) is in agreement and conveys thesame result. Finally, no collisions occur as verified by Fig.9(b). The control input bounds are not violated as shown inFig. 9(c) and Fig. 9(d).

V. CONCLUSIONS

This paper presented a line following and collision avoid-ance method for automated turning and merging, whiletaking into account the 2-D geometry of the intersection andthe uncertain behavior of target vehicles. An optimizationproblem is solved utilizing a receding horizon control ap-proach. Emphasis is given on reducing the search dimensionand on adopting a nonlinear model. The simulations verifythat the approach succeeds in keeping the ego vehicle on thecenter line and close to the desired speed, while the controlinputs remain within bounds and collisions are avoided.Ongoing work focuses on establishing formal guarantees onthe recursive feasibility of the approach, further improvingthe computational effort, as well as on considering vehicleson adjacent lanes.

REFERENCES

[1] M. Caceres, F. Sottile, and M. A. Spirito, “WLAN-based real timevehicle locating system,” in IEEE 69th Vehicular Technology Confer-ence, 2009, April 2009, pp. 1–5.

(a) t = 1.865 sec (b) t = 3.105 sec

(c) t = 3.345 sec (d) t = 5.585 sec

Fig. 8. Scenario 2: The ego vehicle decelerates to avoid collision with thefirst target. It then detects the second target in the rear, and accelerates toavoid collision.

[2] X. Mao, D. Inoue, S. Kato, and M. Kagami, “Amplitude-modulatedlaser radar for range and speed measurement in car applications,” IEEETransactions on Intelligent Transportation Systems, vol. 13, no. 1, pp.408–413, March 2012.

[3] S. Sato, M. Hashimoto, M. Takita, K. Takagi, and T. Ogawa, “Multi-layer Lidar-based pedestrian tracking in urban environments,” in 2010IEEE Intelligent Vehicles Symposium, June 2010, pp. 849–854.

[4] T. Ogawa and K. Takagi, “Lane recognition using on-vehicle LIDAR,”in 2006 IEEE Intelligent Vehicles Symposium, 2006, pp. 540–545.

[5] H. G. Min, X. C. Li, P. P. Sun, X. M. Zhao, and Z. G. Xu, “Visualodometry for on-road vehicles based on trifocal tensor,” in 2015 IEEEFirst International Smart Cities Conference (ISC2), Oct 2015, pp. 1–5.

[6] Q. Jin, G. Wu, K. Boriboonsomsin, and M. Barth, “Multi-agentintersection management for connected vehicles using an optimalscheduling approach,” in 2012 International Conference on ConnectedVehicles and Expo, Dec 2012, pp. 185–190.

[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 TransportationSystems, Oct 2013, pp. 922–927.

[8] M. A. S. Kamal, J. i. Imura, T. Hayakawa, A. Ohata, and K. Aihara,“A vehicle-intersection coordination scheme for smooth flows oftraffic without using traffic lights,” IEEE Transactions on IntelligentTransportation Systems, vol. 16, no. 3, pp. 1136–1147, June 2015.

[9] A. Colombo and D. D. Vecchio, “Least restrictive supervisors forintersection collision avoidance: A scheduling approach,” IEEE Trans-actions on Automatic Control, vol. 60, no. 6, pp. 1515–1527, June2015.

[10] Y. J. Zhang, A. A. Malikopoulos, and C. G. Cassandras, “Optimalcontrol and coordination of connected and automated vehicles at urbantraffic intersections,” in 2016 American Control Conference, July 2016,pp. 6227–6232.

[11] A. De La Fortelle and X. Qian, “Autonomous driving at intersections:combining theoretical analysis with practical considerations,” in ITSWorld Congress 2015, Bordeaux, France, Oct. 2015.

[12] A. Kotsialos and M. Papageorgiou, “Nonlinear optimal control appliedto coordinated ramp metering,” IEEE Transactions on Control SystemsTechnology, vol. 12, no. 6, pp. 920–933, Nov 2004.

[13] T. Awal, L. Kulik, and K. Ramamohanrao, “Optimal traffic mergingstrategy for communication- and sensor-enabled vehicles,” in 16thInternational IEEE Conference on Intelligent Transportation Systems,Oct 2013, pp. 1468–1474.

[14] S. M. Loos and A. Platzer, “Safe intersections: At the crossing of hy-

(a) The velocity of the ego vehicleis depicted in blue. For comparison,the resulting velocity under the samecontroller for the case of no targetvehicles has been plotted in red.

(b) The evolution of the barrier func-tions of ego vehicle w.r.t. each one ofthe targets (in blue and green, respec-tively); no collisions occur as veri-fied by the value remaining greaterthan 1.

(c) control input Flf vs time. Redline is the bound

(d) control input δf vs time. Redline is the bound

Fig. 9. Performance analysis of Controller

brid systems and verification,” in 14th IEEE International Conferenceon Intelligent Transportation Systems, Oct 2011, pp. 1181–1186.

[15] G. Schildbach, M. Soppert, and F. Borrelli, “A collision avoidancesystem at intersections using robust model predictive control,” in 2016IEEE Intelligent Vehicles Symposium, June 2016, pp. 233–238.

[16] J. Rios-Torres and A. A. Malikopoulos, “Automated and cooperativevehicle merging at highway on-ramps,” IEEE Transactions on Intelli-gent Transportation Systems, vol. PP, no. 99, pp. 1–10, 2016.

[17] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic anddynamic vehicle models for autonomous driving control design,” in2015 IEEE Intelligent Vehicles Symposium, June 2015, pp. 1094–1099.

[18] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat, “MPC-based approach to active steering for autonomous vehicle systems,”International Journal of Vehicle Autonomous Systems, vol. 3, no. 2-4,pp. 265–291, 2005.

[19] G. Palmieri, M. Baric, F. Borrelli, and L. Glielmo, “A robust lateralvehicle dynamics control,” in 10th International Symposium on Ad-vanced Vehicle Control, 2010.

[20] V. A. Shia, Y. Gao, R. Vasudevan, K. D. Campbell, T. Lin, F. Bor-relli, and R. Bajcsy, “Semiautonomous vehicular control using drivermodeling,” IEEE Transactions on Intelligent Transportation Systems,vol. 15, no. 6, pp. 2696–2709, Dec 2014.

[21] T. A. Johansen, “Introduction to nonlinear model predictivecontrol and moving horizon estimation.” [Online]. Available:http://folk.ntnu.no/torarnj/nonlinear.pdf

[22] R. Cagienard, P. Grieder, E. C. Kerrigan, and M. Morari, “Move block-ing strategies in receding horizon control,” in 43rd IEEE Conferenceon Decision and Control, Dec 2004, pp. 2023–2028.

[23] G. R. de Campos, A. H. Runarsson, F. Granum, P. Falcone, andK. Alenljung, “Collision avoidance at intersections: A probabilisticthreat-assessment and decision-making system for safety interven-tions,” in 17th International IEEE Conference on Intelligent Trans-portation Systems, Oct 2014, pp. 649–654.

[24] C. Constant, “Car engineering, Mercedes CLS 63 AMG,” 2016,http://www.car-engineer.com/.