Stochastic predictive control of autonomous vehicles in uncertain environments

Abstract

Autonomous vehicles operating in dynamic urban environments must account for the uncertainty arising from the behavior of other objects in the environment. For this purpose, we develop an integrated environment modeling and stochastic Model Predictive Control (MPC) framework. The trade–off between risk and conservativeness is managed by a risk factor which is a parameter in the control design process. The environment model consists of an Interacting Multiple Model Kalman Filter to estimate and predict the positions of target vehicles. The uncertain predictions are used to formulate a chance–constrained MPC problem. The overall goal is to develop a framework for safe autonomous navigation in the presence of uncertainty and study the effect of the risk parameter on controller performance. Simulations of an autonomous vehicle driving in the presence of moving vehicles show the effectiveness of the proposed framework.

Full text

AVEC ’14
Stochastic Predictive Control of Autonomous Vehicles in
Uncertain Environments
Ashwin Carvalhoa, Yiqi Gaoa, St´
ephanie Lef`
evrea, Francesco Borrellia
aUniversity of California at Berkeley, USA
Berkeley, CA 94720, USA
Phone: +1-510-643-2044
E-mail: ashwinc@berkeley.edu
Autonomous vehicles operating in dynamic urban environments must account for the uncertainty
arising from the behavior of other objects in the environment. For this purpose, we develop an
integrated environment modeling and stochastic Model Predictive Control (MPC) framework. The
trade–off between risk and conservativeness is managed by a risk factor which is a parameter in the
control design process. The environment model consists of an Interacting Multiple Model Kalman
Filter to estimate and predict the positions of target vehicles. The uncertain predictions are used to
formulate a chance–constrained MPC problem. The overall goal is to develop a framework for safe
autonomous navigation in the presence of uncertainty and study the effect of the risk parameter on
controller performance. Simulations of an autonomous vehicle driving in the presence of moving
vehicles show the effectiveness of the proposed framework.
Topics / Autonomous driving and collision avoidance, Environment modeling
1. INTRODUCTION
Recent advances in sensing, computing and control
technologies make autonomous passenger vehicles a
viable solution to minimize the number of road acci-
dents and resulting fatalities. Autonomous vehicles must
navigate in dynamic urban environments in the presence
of other static or moving objects such as vehicles,
bicycles and pedestrians. One of the main challenges
is to systematically account for the uncertainties due to
other objects in the control design process.
Nominal control approaches that do not account for
system uncertainties may result in unsafe performance.
On the other hand, robust control approaches that deal
with worst–case disturbances may be too conservative
and computationally expensive. We propose a stochastic
MPC framework for controlling autonomous vehicles
where the risk of constraint violations is a tunable design
parameter. MPC has been shown to be an effective strat-
egy for the automotive control problem as it can handle
system constraints, nonlinearities and uncertainties in an
unified manner [1]–[3].
Chance–constrained optimization has been studied in
the context of autonomous vehicles. The work in [4]
addresses the tactical planning problem for autonomous
vehicles in uncertain environments. The relative motion
between controlled and uncontrolled vehicles is modeled
as a jump Markov linear system. Sampling is used to ap-
proximate the multimodal distribution of the future sys-
tem state, and the collision avoidance chance–constraints
are approximated by convex bounding functions.
Stochastic MPC was applied to a driver assistance
system in [1] using the approach presented in [5] for
linear time–invariant (LTI) systems with stochastic dis-
turbances. The time–invariance allows the uncertainty
propagation to be performed offline for disturbances
with any known distribution. However, vehicle models
are nonlinear in general due to the complex tire–road
interaction. Hence, we propose modeling the vehicle
dynamics as a linear time–varying (LTV) system where
the linearization is performed online. Disturbances in the
system are modeled as Gaussian to allow for the easy
propagation of uncertainties and handling of probabilis-
tic constraints.
In addition to a vehicle dynamics model, a predictive
controller requires a model of the dynamic environment
in which the vehicle is operating. This is used to
incorporate predictions of the behavior of other objects
in the decision making process. Previous work by the
authors assumes that the future positions of all objects in
the environment are known over the prediction horizon
[1]–[3]. We aim to relax that assumption by developing
an environment modeling framework to forecast the
positions of nearby objects. In this paper, we focus on
the problem of predicting the positions of surrounding
vehicles (denoted as target vehicles).
The work in [6] classifies traffic motion prediction
approaches into three categories, (1) physics–based, (2)
maneuver–based, and (3) interaction–aware. Maneuver–
based and interaction–aware models generally involve a
higher degree of abstraction, and are suited for relatively
long–term predictions. This is accompanied by higher
computational costs. Since MPC applies inputs in a re-
ceding horizon manner, we choose the class of physics–
based approaches. Specifically, an Interacting Multiple
Model Kalman Filter (IMM–KF) is used to estimate and
predict the positions of target vehicles. The IMM–KF is
a hybrid system state estimator popular in target tracking
literature [7]. A multiple model algorithm such as the
IMM–KF is a natural choice for our application due to
the multimodal behavior of target vehicles.

AVEC ’14
Fig. 1. Block diagram of overall system.
In summary, the overall goals of this work are to
(1) develop an integrated environment modeling and
probabilistic control framework, and (2) study the trade–
off between conservatism and risk for autonomous ve-
hicles through a tunable risk parameter in the chance–
constrained MPC. Results from simulations in urban
driving scenarios are presented to demonstrate the ef-
fectiveness of the proposed framework.
The paper is organized as follows. Section 2 describes
the overall architecture of the system. In Section 3, we
introduce the environment model to make predictions
of target vehicle positions. Section 6 details the for-
mulation of the chance–constrained MPC problem and
the stochastic MPC framework. We present simulation
results in Section 7 followed by concluding remarks in
Section 8.
2. SYSTEM DESCRIPTION
The proposed system architecture is shown in Figure
1. The “Sensors” block is assumed to provide processed
information about the vehicle states, road geometry, and
relative positions and velocities of target vehicles in
real–time. This information is used by the “Environment
Model” block to make probabilistic predictions of the
positions of target vehicles in the near future. The
“MPC” block uses the sensory information along with
the forecast evolution of the environment to formulate a
chance–constrained receding horizon control problem to
optimize the steering and braking inputs to the vehicle.
In this work, we focus on developing the “Environment
Model” (Section 3) and “MPC” (Section 6) blocks.
3. ENVIRONMENT MODEL
3.1 Target Vehicle Models
The dynamics of the jth target vehicle are described
by a Markov jump affine system,
ξe
jk+1 =F(i)
kξe
jk+G(i)
kw(i)
k+E(i)
k(1a)
ye
jk=H(i)
kξe
jk+v(i)
k,(1b)
where ξe
jkand ye
jkdenote the target vehicle state and
measurement, respectively, at time k. The process noise
w(i)
kand measurement noise v(i)
kare assumed to be
i.i.d. as N(0, Q(i)
k)and N(0, R(i)
k), respectively. The
superscript (i)in (1) refers to the model m(i)in the
model set M={m(1), m(2) , . . . , m(M)}, and transitions
between modes have fixed probabilities given by a
matrix π∈RM×M, such that
πij =P(mk+1 =m(j)|mk=m(i)),(2)
where mkdenotes the mode at time k. For the predic-
tion problem, we decouple the longitudinal and lateral
motion of the target vehicle. The models used for each
are described below. The subscript jand superscript e
are dropped for visual clarity.
3.1.1 Longitudinal motion
We choose ξ= [s, ˙s, ¨s]Tand y= [s, ˙s]Tfor the
longitudinal state and measurement, respectively. Two
modes are used to model the longitudinal motion,
1) Constant velocity:
F(1)
k=

1Ts
T2
s
2
0 1 Ts
0 0 0

G(1)
k=

T2
s
2
Ts
1

,(3)
where Tsis the sampling time.
2) Constant acceleration:
F(2)
k=

1Ts
T2
s
2
0 1 Ts
0 0 1

G(2)
k=

T2
s
2
Ts
1

.(4)
3.1.2 Lateral motion
The target vehicle lateral state and measurement are
chosen to be ξ= [ey,˙ey]Tand y=ey, respectively.
We use state–feedback models to represent the lateral
dynamics in typical lane keeping and lane changing
maneuvers. The lateral acceleration of the target vehicle
with respect to the lane is assumed to evolve as
¨eyk≈˙eyk+1 −˙eyk
Ts
=−K2(eyk−ey,ref )−K1˙eyk.
(5)
Thus, eyis assumed to have a second–order response
where [K1, K2]affect the settling time and overshoot,
and ey→ey,ref . The system matrices are given by
F(i)
k=1Ts
−TsK(i)
21−TsK(i)
1(6a)
G(i)
k=T2
s/2
TsE(i)
k=0
TsK(i)
2e(i)
y,ref ,(6b)
where the superscript (i)denotes the mode. We use
a set of nine models (i.e. M= 9) corresponding to
three different tunings of K1and K2for each of the
lane keeping, lane change left and lane change right
maneuvers. Figure 2 shows the trajectories for the 9
modes with initial condition ξ0= [2,0]T, and a lane
width of 3m.
Remark 1: The parameters K1and K2in (6) are es-
timated heuristically in this paper. Tools from statistical
learning theory may be used to systematically estimate
parameters using data.
3.2 IMM–KF Algorithm
At each time step k, we estimate the following quan-
tities based on the current measurement and values at
the previous time step:
1) Model probability:
µ(i)
k=P(mk=i|y0, . . . , yk)
(i= 1, . . . , M )

AVEC ’14
0 2 4 6 8 10
−5
0
5
Lef t La ne
Center Lane
Right Lane
t[s]
ey[m]
Lane B oundar ies
Fig. 2. Predicted trajectories for the target vehicle lateral motion
models. The models represent typical maneuvers such as lane keeping,
left lane change and right lane change. Variations within a maneuver
come from different tunings of the gains K1and K2.
2) Model–conditioned posterior means and covari-
ances (i.e. assuming the mode is time–invariant and
equal to i):
ˆ
ξ(i)
k|k=E[ξ|y0, . . . , yk, mk−1=i, mk=i]
ˆ
Σ(i)
k|k=Cov[ξ|y0, . . . , yk, mk−1=i, mk=i]
(i= 1, . . . , M )
3) Fused posterior mean and covariance:
ˆ
ξk|k=E[ξk|y0, . . . , yk]
ˆ
Σk|k=Cov[ξk|y0, . . . , yk]
The algorithm to compute the above quantities is sum-
marized in [8], and stated below:
Step 1: Evaluate mixing probabilities:
µj|i
k−1=πij µ(j)
k−1
µ(i)
k|k−1
(7)
where µ(i)
k|k−1=
M
X
j=1
πji µ(j)
k−1
Mixing estimates:
¯
ξ(i)
k−1|k−1=
M
X
j=1
ˆ
ξ(j)
k−1|k−1µj|i
k−1(8)
Mixing covariances:
¯
Σ(i)
k−1|k−1=
M
X
j=1
[Σ(j)
k−1|k−1+ (¯
ξ(i)
k−1|k−1−ˆ
ξ(i)
k−1|k−1)·
(9)
(¯
ξ(i)
k−1|k−1−ˆ
ξ(i)
k−1|k−1)T]µj|i
k−1
Step 2: For each mode M(i), run Kalman filter with
inputs (¯
ξ(i)
k−1|k−1,¯
Σ(i)
k−1|k−1)using Algorithm 1. We
obtain the following model–conditioned estimates,
(ˆ
ξ(i)
k|k,ˆ
Σ(i)
k|k,˜y(i)
k, S(i)
k)
Step 3: Update the probability of each model,
µ(i)
k=µ(i)
k|k−1N(˜y(i)
k; 0, S(i)
k)
M
P
j=1
µ(j)
k|k−1N(˜y(j)
k; 0, S(j)
k)
(10)
where N(x;µ, Σ) is the probability density function
of a multivariate normal distribution with mean µand
covariance Σ, evaluated at x.
Step 4: Compute the fused estimates,
ˆ
ξk|k=
M
X
j=1
ˆ
ξ(i)
k|kµ(i)
k(11)
ˆ
Σk|k=
M
X
i=1
[Σ(i)
k|k+ (ˆ
ξk|k−ˆ
ξ(i)
k|k)(ˆ
ξk|k−ˆ
ξ(i)
k|k)T]µ(i)
k
(12)
Algorithm 1 Kalman filter equations for mode (i)
Inputs: (¯
ξ(i)
k−1|k−1,¯
Σ(i)
k−1|k−1)
Prediction step:
ˆ
ξ(i)
k|k−1=F(i)
k−1¯
ξ(i)
k−1|k−1+E(i)
k−1
ˆ
Σ(i)
k|k−1=F(i)
k−1¯
Σ(i)
k−1|k−1F(i)T
k−1+G(i)
k−1Q(i)
k−1G(i)T
k−1
Update step:
˜y(i)
k=yk−H(i)
kˆ
ξ(i)
k|k−1
S(i)
k=H(i)
kˆ
Σ(i)
k|k−1H(i)T
k+R(i)
k
K(i)
k=ˆ
Σ(i)
k|k−1H(i)T
kS(i)−1
k
ˆ
ξ(i)
k|k=ˆ
ξ(i)
k|k−1+K(i)
k˜y(i)
k
ˆ
Σ(i)
k|k=ˆ
Σ(i)
k|k−1−K(i)
kS(i)
kK(i)T
k
Outputs: (ˆ
ξ(i)
k|k,ˆ
Σ(i)
k|k,˜y(i)
k, S(i)
k)
3.3 Target State Prediction
Recall that one of the main goals of the environment
model is to forecast the state of the target vehicle over
the prediction horizon of the controller. At time k, we
first estimate the most likely mode mkas the mode (i)
with the highest probability µ(i)
k. Given the mode, the
state is assumed to evolve according to the dynamics
(1a). Note that the longitudinal and lateral state pre-
dictions are performed separately and later combined
for the control design. The predicted positions of target
vehicles are used to formulate constraints on the ego
vehicle state. This is described later in Section 5.1.
4. VEHICLE MODEL
Starting from a nonlinear model in Section 4.1, we
derive a linear parameter–varying (LPV) model of the
vehicle dynamics in Section 4.2.
4.1 Nonlinear Bicycle Model
The notation used in the vehicle model is depicted
in Figure 3. The following set of nonlinear differential
equations are used to describe the motion of the vehicle
in the road–aligned coordinate frame,
¨x= ˙y˙
ψ+ax(13a)
¨y=−˙x˙
ψ+1
m(2Fy,f + 2Fy,r )(13b)
¨
ψ=1
Iz
(2aFy,f −2bFy,r )(13c)
˙eψ=˙
ψ−κ˙s(13d)
˙ey= ˙xsin(eψ) + ˙ycos(eψ)(13e)

AVEC ’14
Fig. 3. Vehicle model notation
˙s=1
1−κey
( ˙xcos(eψ)−˙ysin(eψ),(13f)
where mand Izdenote the vehicle mass and yaw inertia,
respectively, aand bdenote the distances from the
vehicle’s center of gravity to the front and rear axles,
respectively. ˙xand ˙ydenote the longitudinal and lateral
speeds in the body frame, respectively, ˙
ψdenotes the
yaw rate, and axdenotes the commanded longitudinal
acceleration. eyand eψdenote the lateral position error
and angular error with respect to the road centerline,
respectively, κdenotes the curvature of the road, and s
denotes the longitudinal position of the vehicle along
the road. The lateral force in the body frame Fy,?
(?∈ {f, r}) is given by,
Fy,? =−C?α?,(14)
where C?denotes the tire cornering stiffness, and α?
denotes the lateral tire slip angle which is computed
using small angle approximations as,
αf=−δf+˙y+a˙
ψ
˙x, αr=˙y−b˙
ψ
˙x,(15)
where δfis the commanded front steering angle. The
nonlinear bicycle model can be compactly written as,
˙
ξ(t) = f(ξ(t), u(t), p(t)),(16)
where ξ= [ ˙x, ˙y, ˙
ψ, eψ, ey, s]T,u= [δf, ax]T, and p=κ
are the state, input and parameter vectors, respectively.
4.2 Linear Parameter-Varying (LPV) Model
For the control design, we linearize the bicycle model
described above around the current longitudinal speed
to obtain an LPV model of the vehicle dynamics. The
following assumptions are introduced,
Assumption 1: The angular error eψis small, hence
sin(eψ)≈eψ,cos(eψ)≈1.
Assumption 2: The lateral velocity ˙yand yaw rate ˙
ψ
are small, hence ˙y˙
ψ≈0,˙yeψ≈0.
Assumption 3: The road curvature κis assumed to be
known as a function of the longitudinal position salong
the road.
Under these assumptions, the system dynamics (13)
can be expressed as,
˙
ξ(t) = Ac(ρ(t))ξ(t) + Bcu(t),(17)
Fig. 4. Signed distance between convex shapes Aand B
where ρ= [ ˙x, κ]Tis the parameter vector about which
the linearization is performed, and
Ac(ρ) =








0 0 0 0 0 0
0−2(Cf+Cr)
m˙x
−2(aCf−bCr)
m˙x0 0 0
0−2(aCf−bCr)
Iz˙x
−2(a2Cf+b2Cr)
Iz˙x0 0 0
−κ0 1 0 0 0
0 1 0 ˙x0 0
1 0 0 0 0 0








,
Bc=








0 1
2Cf
m0
2aCf
Iz0
0 0
0 0
0 0








.
In the following sections, we drop the dependence of Ac
on ρfor visual clarity.
To account for uncertainties due to model mis-
match and linearization errors, we introduce an additive
stochastic disturbance in (17). The modified LPV model
can be written in discrete–time as,
ξk+1 =Akξk+Bkuk+Dkwk,(18)
where wk∼ N (0,Σw). Note that (18) is obtained by a
forward Euler discretization of (17).
Remark 2: The data from [3] is used to estimate the
disturbance covariance Σw
Remark 3: The LPV model was derived using a linear
tire model approximation. The MPC approach presented
in Section 6 can be directly applied to LTV models
which use nonlinear tire models for the linearization [2].
5. SAFETY CONSTRAINTS
5.1 Collision avoidance
We employ an approach similar to that in [9] for for-
mulating collision avoidance constraints. This is based
on the notion of signed distance in collision detection
literature, which is defined below.
Definition 1: Let Aand Bbe convex shapes. The
signed distance sd(A,B)is defined as the length of the
smallest translation that puts the two shapes Aand B
(1) in contact, if they are currently separated or (2) out
of contact, if they are currently in contact.
This is illustrated in Figure 4. Details about the
signed distance and its computation are presented in
[9]. Without loss of generality, we consider the case of

AVEC ’14
one moving target vehicle. Recall that the “Environment
Model” block provides us with an estimate of the
current and future positions of the target vehicle. At any
time instant t, let the predicted position of the target
vehicle at time t+kbe denoted as ¯
ξe
t+k,t, and that
of the ego vehicle be denoted as ¯
ξt+k,t. Let the target
vehicle be represented by a convex shape Tt+k,t , and the
ego vehicle by Et+k,t. Note that Tt+k,t and Et+k,t are
functions of ¯
ξe
t+k,t and ¯
ξt+k,t, respectively. The collision
avoidance constraint can be expressed as,
sd(Tt+k,t,Et+k ,t)≥0.(19)
The above constraint is nonlinear and non–convex in
general. We linearize (19) around the predicted ego
vehicle state ¯
ξt+k,t to obtain
dk+∇dk·∆ξk≥0,(20)
(dk=sd(Tt+k,t,Et+k ,t))
where ∆ξkis the deviation of the ego vehicle state from
¯
ξt+k,t. (20) can be expressed in the standard form of a
linear inequality as
gT
kξt+k,t ≤hk.(21)
Note that the RHS of (21) is uncertain due to the
uncertainty in the predicted target vehicle position. This
is dealt with by suitably increasing the dimensions of
Tt+k,t such that the target vehicle will be contained in
Tt+k,t with a high probability. Moreover, the LHS is also
uncertain due to the uncertainty in ξt+k,t arising from
the additive disturbance in (18). We account for this by
formulating (21) as a chance–constraint to be satisfied
with a specified probability p. That is,
P r(gT
kξt+k,t ≤hk)≥p, 0.5≤p≤1,(22)
where pdenotes the tunable risk parameter. Naturally,
a large value of pwould lead to conservative behavior,
possibly making the chance–constrained optimal control
problem (25) infeasible. On the other hand, a low value
of pmight lead to a higher risk of collision. The effect
of varying pof the driving performance is studied in
more detail in Section 7.
5.2 Lane boundaries
The requirement that the vehicle stays within the lane
can be expressed as,
elane
y,min ≤ey≤elane
y,max,(23)
where the bounds elane
y,min and elane
y,max account for the
width of the vehicle.
5.3 Input constraints
Physical limitations on the actuators impose bounds
on the control inputs and input rates,
umin ≤u(t)≤umax,˙umin ≤˙u(t)≤˙umax .(24)
6. CONTROL DESIGN
6.1 Chance–constrained MPC
The vehicle model and safety constraints described
above are used to formulate a chance–constrained re-
ceding horizon control problem. At each sampling time,
a constrained finite–time optimal control problem is
solved to determine a sequence of control inputs which
minimizes a given cost function. Only the first input
of the sequence is applied to the system. The process
is repeated at the next sampling instant using new
measurements. The optimization problem to be solved
at each time step is given by,
min
ut
Hp−1
X
k=0
E(kξt+k+1,t −ξref,k+1 k2
Q+kut+k,tk2
R)
(25a)
s.t. ξt+k+1,t =Akξt+k,t +Bkut+k,t +Dkwk(25b)
P r(gT
k+1ξt+k+1,t ≤hk+1 )≥p(25c)
(k= 0, . . . , Hp−1)
Guut∈ U (25d)
ξt,t =ξ(t),(25e)
where tdenotes the current time instant, Hpdenotes the
prediction horizon and ξt+k,t denotes the predicted ego
vehicle state at time t+kobtained by applying the con-
trol sequence ut={ut,t, ut+1,t , . . . , ut+Hp−1,t}to the
vehicle model (25b) with initial condition ξt,t =ξ(t).
(25d) is a compact representation of the input constraints
in (24). The reference state sequence {ξref,k }Hp
k=1 in
(25a) encodes control objectives such as tracking the
lane centerline and maintaining a desired speed.
The optimization problem (25) cannot be solved di-
rectly because of the presence of the stochastic distur-
bance win (25b), and the chance–constraints (25c). The
original constraints must be tightened to account for the
state uncertainty at each time step in the horizon. This
is presented below. The variables ξt+k,t and ut+k,t are
denoted as ξkand uk, respectively, for visual clarity.
6.2 Closed–loop approach
We use the closed–loop paradigm introduced in [5] to
decompose the state and input as,
ξk=zk+ek(26a)
uk=Kkξk+ck,(26b)
where Kkis a stabilizing feedback gain for the pair
(Ak, Bk),zkdenotes the deterministic component of ξk,
and ekdenotes the stochastic component. ckrepresents
perturbations on a given feedback control law. Substi-
tuting for ξkand ukin (25b) gives,
zk+1 = Φkzk+Bkck(27a)
ek+1 = Φkek+Dkwk,(27b)
where Φk=Ak+BkKk. We introduce the following
assumptions,
Assumption 4: The system has perfect state feedback,
i.e., ξ0=z0. This implies e0= 0 with probability 1.
Assumption 5: The pair (Ak, Bk)is controllable.
Hence, there exists Kksuch that Φkis Hurwitz.
Remark 4: Kkis chosen to be the infinite horizon
LQR gain for (Ak, Bk).

AVEC ’14
6.3 Uncertainty Propagation
The separation of the deterministic and stochastic
components of ξkin (26) allows us to determine the
distributions of ek(k= 1, . . . , Hp)knowing the dis-
tributions of e0and wk. Let ek∼ N (0,Σk). Then,
ek+1 ∼ N (0,Σk+1), where
Σk+1 = ΦkΣkΦT
k+DkΣwDT
k.(28)
The initial condition is Σ0= 0 by Assumption 4.
6.4 Constraint Tightening
Based on (26), the probabilistic constraint (25c) be-
comes
P r(gT
kzk+gT
kek≤hk)≥p. (29)
(k= 1, . . . , Hp)
This is satisfied if [5],
gT
kzk≤hk−γk(30a)
P r(gT
kek≤γk) = p(30b)
Since we know that gT
kek∼ N (0, gT
kΣkgk), we can
explicitly compute γkfrom the quantile function of a
univariate normal distribution as
γk=q2gT
kΣkgkerf −1(2p−1) (31)
where erf −1(·)is the inverse error function.
6.5 Modified Optimization Problem
By replacing (25c) with the tightened constraints
(30a) and using the feedback law (27b), we obtain the
following optimization problem to be solved at each time
step,
min
c
Hp−1
X
k=0
kzk+1 −ξref,k+1 k2
Q+kKkzk+ckk2
R
(32a)
s.t. zk=Akzk+Bkck(32b)
gT
kzk≤hk−γk(32c)
(k= 0, . . . , Hp−1)
Gu(Kz +c)∈ U (32d)
z0=ξ(t),(32e)
where,
K=blkdiag(K0, . . . , KHp−1)
z= [z0, . . . , zHp−1]T
c= [c0, . . . , cHp−1]T.
(32) is a quadratic program (QP) which can be solved
efficiently in real–time. Note that (32c) is implemented
as a soft constraint to prevent the QP from becoming
infeasible.
7. RESULTS
7.1 Simulation setup
Simulations are performed in MATLAB, and the
constrained optimization problem (32) is solved using
Gurobi [10]. The controller is connected in closed–
loop with a higher fidelity four–wheel nonlinear model
(which uses a Pacejka tire model) to simulate model
mismatch.
7.2 Test scenario
The scenario consists of two target vehicles (denoted
as T1and T2) on a straight road. At the start of
the simulation, T1is in the right lane moving at a
speed of 6m/s. It performs a lane change maneuver
at approximately 3seconds, and moves into the left
lane. T2starts in the right lane at a speed of 8m/s,
and stays in the right lane for the entire duration of the
simulation. The ego vehicle (denoted as E) starts off in
the left lane, just behind T1and T2, at a speed of 10
m/s. It’s goal is to track the left lane, while maintaining a
speed of 15 m/s. T1and T2are assumed to move with a
constant velocity of 6and 8m/s, respectively, perturbed
by the sum of a low–frequency sinusoidal term and a
Gaussian disturbance. The lane change trajectory for T1
is generated using a sigmoid function.
7.3 Results
To illustrate the effectiveness of the controller, we
show snapshots of the simulation at various time instants
in Figure 5. In each sub–figure, the red boxes depict the
current positions of the ego vehicle Eand target vehicles
T1and T2, while the gray boxes depict the predicted
positions. The predicted positions of Eare obtained
from the MPC, and those of T1and T2are given by the
IMM–KF. The red dashed lines for each target depict
the ground truth positions. As seen in Figure 5(a), the
IMM–KF predicts a lane change maneuver for T1. This
results in the Eslowing down (Figure 5(b)). In Figure
5(c), Etries to plan a path around T1, but is prevented
from doing so by the presence of T2. Finally, in Figure
5(e), we see that Eis able to plan a path in between T1
and T2, and increase its speed.
In addition, we demonstrate the ability of the envi-
ronment model to predict the lane change intention of
T1by plotting the lateral mode probabilities in Figure
6. For visual clarity, the lateral modes are classified
into two categories, “Right Lane” and “Left Lane”. The
probabilities for each category are shown in Figure 6 for
part of the simulation. It is observed that as T1starts
moving towards the left, the probability of the “Left
Lane” mode increases while that of the “Right Lane”
mode decreases.
7.4 Risk vs. conservativeness
An important element of the stochastic MPC problem
is the risk parameter p, defined as the probability of
violating the chance–constraints (22). The nominal MPC
problem where the disturbances are assumed to be zero
is given by p= 0.5. We would like to study the effect
of varying pon the performance of the controller.
For the scenario described in Section 7.2, the distance
traveled by the ego vehicle Ein 25 seconds is used as

AVEC ’14
20 30 40 50
−2
0
2
E
T1
T2
X[m]
Y[m]
(a) t= 2 s
40 45 50 55 60 65 70
−2
0
2
ET1
T2
X[m]
Y[m]
(b) t= 5 s
80 85 90 95 100
−2
0
2
E
T1
T2
X[m]
Y[m]
(c) t= 10 s
110 115 120 125 130 135 140
−2
0
2
E
T1
T2
X[m]
Y[m]
(d) t= 15 s
145 150 155 160 165 170 175 180
−2
0
2
E
T1
T2
X[m]
Y[m]
(e) t= 20 s
170 180 190 200 210
−2
0
2E
T1
T2
X[m]
Y[m]
(f) t= 23.5s
Fig. 5. Snapshots of simulation with current and predicted positions of ego vehicle Eand target vehicles T1and T2. The red boxes
depict the current positions, while the gray boxes depict the future positions. For each target vehicle, the ground truth is shown by
the dashed red line.
0 5 10 15 20 25 30
0
0.2
0.4
0.6
0.8
1
t[s]
M ode P r obabi lity
Right Lane
Lef t Lan e
Fig. 6. Mode probabilities for target vehicle T1which changes lanes
in front of the ego vehicle
the comparison metric. It is seen that Etravels about
214 m with p= 0.998, and about 220 m with p= 0.5.
Although the difference is small, it must be noted that
the scenario presented is simple. For more complex
scenarios encountered in day–to–day commutes, the
difference in travel distances (and hence, travel times)
can be much larger.
Another simple scenario gives insight on the effect
of varying pon the conservatism of the controller. The
scenario consists of a slow target vehicle T1moving in
the left lane at a speed of 5m/s, and a faster vehicle T2
moving in the right lane at a constant speed of 9m/s.
The ego vehicle Estarts in the left lane with a speed of
10 m/s.
The initial positions of E,T1and T2are chosen
such that the nominal MPC with no constraint tightening
(p= 0.5) is able to plan a path around T1while avoiding
a collision with T2, as shown in Figure 7(a). On the
other hand, the chance–constrained MPC with p= 0.998
results in the ego vehicle slowing down significantly to
wait for T2to pass before going around T1. A snapshot
of the simulation with the chance–constrained MPC is
shown in Figure 7(b). The variation in the distance
traveled by the ego vehicle with pis more significant
in this case. For the nominal MPC with p= 0.5, the
ego vehicle travels about 278 m in 20 seconds, while in

AVEC ’14
25 30 35 40 45 50 55
−2
0
2
E
T1
T2
X[m]
Y[m]
(a) p= 0.5(Nominal MPC)
25 30 35 40 45 50 55
−2
0
2
E
T1
T2
X[m]
Y[m]
(b) p= 0.998 (Stochastic MPC)
Fig. 7. Simulation showing the effect of varying the risk
parameter pon the conservatism of the controller. The notation
is the same as in Figure 5
case of the chance–constrained MPC with p= 0.998, it
covers only 193 m.
In the context of urban driving, scenarios like the
one described above are expected to occur often. The
use of conservative worst–case approaches may result
in the ego vehicle stopping or slowing down unneces-
sarily, leading to longer commute times. The framework
proposed in this work is expected to be less restrictive,
while retaining the ability to systematically account for
system uncertainties.
8. CONCLUSIONS
In this paper, we presented the integration of an envi-
ronment model with a stochastic predictive control ap-
proach for autonomous vehicles. A multimodal approach
is used to estimate and predict the state of surrounding
vehicles. The predictions are embedded in a chance–
constrained MPC which optimizes control inputs to
achieve certain objectives while avoiding collisions with
other vehicles. The ability of the ego vehicle to safely
traverse a typical urban driving scenario is demonstrated
through simulation. In addition, we studied the effect of
the tunable risk parameter in the stochastic MPC on the
performance of the autonomous vehicle. The proposed
framework allows us to vary the conservatism of the
controller as a function of risk. Future work aims at
exploring this relationship in greater detail.
9. ACKNOWLEDGMENTS
This material is based upon work supported by the
National Science Foundation under Grant No. 1239323.
Any opinions, findings, and conclusions or recommen-
dations expressed in this material are those of the authors
and do not necessarily reflect the views of the National
Science Foundation.
REFERENCES
[1] A. Gray, Y. Gao, J. K. Hedrick, and F. Borrelli,
“Stochastic predictive control for semi-autonomous
vehicles with an uncertain driver model,” in Intel-
ligent Transportation Systems (ITS), IEEE Confer-
ence on, 2013.
[2] A. Carvalho, Y. Gao, A. Gray, H. E. Tseng, and
F. Borrelli, “Predictive control of an autonomous
ground vehicle using an iterative linearization
approach,” in Intelligent Transportation Systems
(ITS), IEEE Conference on, 2013.
[3] Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli,
“A tube-based robust nonlinear predictive control
approach to semiautonomous ground vehicles,” Ve-
hicle System Dynamics, 2014.
[4] M. P. Vitus and C. J. Tomlin, “A probabilistic
approach to planning and control in autonomous
urban driving,” in Proceedings of the 52nd IEEE
Conference on Decision and Control, Florence,
Italy, December 2013.
[5] B. Kouvaritakis, M. Cannon, S. V. Rakovi´
c, and
Q. Cheng, “Explicit use of probabilistic distri-
butions in linear predictive control,” Automatica,
vol. 46, no. 10, pp. 1719–1724, 2010.
[6] S. Lef`
evre, D. Vasquez, and C. Laugier, “A sur-
vey on motion prediction and risk assessment for
intelligent vehicles,” Robomech Journal, to appear.
[7] E. Mazor, A. Averbuch, Y. Bar-Shalom, and
J. Dayan, “Interacting multiple model methods in
target tracking: a survey,” Aerospace and Elec-
tronic Systems, IEEE Transactions on, vol. 34,
no. 1, pp. 103–123, 1998.
[8] R. R. Pitre, V. P. Jilkov, and X. R. Li, “A compar-
ative study of multiple-model algorithms for ma-
neuvering target tracking,” in Defense and Security.
International Society for Optics and Photonics,
2005, pp. 549–560.
[9] J. Schulman, J. Ho, A. Lee, I. Awwal, H. Bradlow,
and P. Abbeel, “Finding locally optimal, collision-
free trajectories with sequential convex optimiza-
tion,” in Robotics: Science and Systems (RSS).
Citeseer, 2013.
[10] Gurobi Optimizer Reference Manual, Gurobi
Optimization, Inc., 2014. [Online]. Available:
http://www.gurobi.com