Collision Avoidance with Stochastic Model Predictive Control for Systems with a Twofold Uncertainty Structure - arXiv
←
→
Page content transcription
If your browser does not render page correctly, please read the page content below
This work has been accepted to the IEEE 2021 International Conference on Intelligent Transportation Systems.
Collision Avoidance with Stochastic Model Predictive Control for
Systems with a Twofold Uncertainty Structure
Tim Brüdigam1 , Jie Zhan1 , Dirk Wollherr1 , and Marion Leibold1
Abstract— Model Predictive Control (MPC) has shown to be handled robustly by using Robust Model Predictive Control
a successful method for many applications that require control. (RMPC) methods for bounded uncertainties [1], [2]. How-
Especially in the presence of prediction uncertainty, various ever, these robust controllers are often highly conservative.
types of MPC offer robust or efficient control system behavior.
For modeling, uncertainty is most often approximated in such a Stochastic Model Predictive Control (SMPC) approaches
arXiv:2106.08463v1 [eess.SY] 15 Jun 2021
way that established MPC approaches are applicable for specific [3], [4] provide more efficient solutions compared to RMPC
uncertainty types. However, for a number of applications, by utilizing probabilistic chance constraints instead of hard
especially automated vehicles, uncertainty in predicting the constraints. These chance constraints enable increased effi-
future behavior of other agents is more suitably modeled by ciency by allowing a small probability of constraint violation,
a twofold description: a high-level task uncertainty and a low-
level execution uncertainty of individual tasks. In this work, we limited by a predefined acceptable risk. Various SMPC
present an MPC framework that is capable of dealing with this methods exist, approximating the chance constraint to obtain
twofold uncertainty. A scenario MPC approach considers the a tractable representation that may be solved in an optimal
possibility of other agents performing one of multiple tasks, control problem. In general, each SMPC method considers
with an arbitrary probability distribution, while an analytic one type of uncertainty within the prediction model.
stochastic MPC method handles execution uncertainty within a
specific task, based on a Gaussian distribution. Combining both Analytic SMPC approaches [5]–[7] yield an analytic ap-
approaches allows to efficiently handle the twofold uncertainty proximation of the chance constraint, but these approaches
structure of many applications. Application of the proposed are mostly restricted to Gaussian uncertainties. In particle-
MPC method is demonstrated in an automated vehicle simula- based SMPC [8] and Scenario Model Predictive Control
tion study. (SCMPC) [9], samples of the uncertainty are drawn that
are then used to approximate the chance constraint. While
I. I NTRODUCTION
arbitrary uncertainty distributions are possible, large numbers
Advances in research on automated systems are facilitating of samples are required to provide sufficient approxima-
the use of controllers for complex applications, which is tions for some uncertainty distributions, which increases
especially evident for automated vehicles. In many of these computational complexity. If mixed uncertainty structures
applications, there is one controlled agent, e.g., a vehicle or best describe the system behavior, the chance constraint
mobile robot, which is required to act and move among other approximations of these SMPC approaches are not neces-
agents. In order to move efficiently and avoid collisions, it sarily suitable. In [10] an SMPC framework, S+SC MPC,
is necessary for the controlled agent to anticipate the future was introduced that utilizes both SCMPC and a Gaussian
behavior of the surrounding agents. uncertainty-based SMPC method, specifically designed for a
The challenge here is that future behavior of other agents is simple automated vehicle example.
subject to uncertainty. In many applications, this uncertainty In this paper, we propose an S+SC MPC framework
consists of two types, task uncertainty and task execution that significantly generalizes the work of [10]. In [10] a
uncertainty. Using automated vehicles as an example, the simple S+SC MPC framework was specifically designed for
future motion of other surrounding vehicles is first subject automated vehicles, where only one surrounding vehicle and
to specific maneuvers, such as lane keeping or lane changing. two possible maneuvers are considered. Here, we present
Second, the execution of these maneuvers may vary again. a general S+SC MPC framework, applicable to a variety
A lane change may be executed quickly and aggressively, or of automated systems. We specifically focus on collision
slowly over a longer period of time. avoidance, which requires considering multiple other agents
Model Predictive Control (MPC) is a suitable method that may perform multiple different tasks.
to plan motion and trajectories for automated systems in The proposed S+SC MPC approach utilizes an SCMPC
environments with uncertainty. In MPC an optimal control approach for task uncertainty and an analytic SMPC ap-
problem is solved on a finite horizon, utilizing prediction proach for task execution uncertainty. Combining these two
models to take into account the controlled agent dynamics approaches into a single MPC optimal control problem al-
and the future behavior of other agents. Constraints subject to lows to efficiently consider the twofold uncertainty structure
environment uncertainty, e.g., for collision avoidance, may be of many practical applications with task and task execution
uncertainty, e.g., automated vehicles [7], [11]–[14]. An au-
1 T. Brüdigam, J. Zhan, D. Wollherr, and M. Leibold are with
tomated vehicle simulation study illustrates the applicability
the Chair of Automatic Control Engineering at the Technical Univer-
sity of Munich, Germany. {tim.bruedigam; jie.zhan; dw; of the proposed S+SC MPC framework.
marion.leibold}@tum.de The paper is structured as follows. Section II introducesthe problem statement. The S+SC MPC method is derived denoted by k. The SMPC OCP is given by
in Section III. A simulation study is presented in Section IV,
J ∗ = min JN (ξ0 , U ) (5a)
followed by conclusive remarks in Section V. U
s.t. ξk+1 = f (ξk , uk ) (5b)
II. P ROBLEM S TATEMENT
DO
ξk+1 = ADO ξkDO +B DO
uDO
k (Ti ) + G DO
wkex (5c)
uk ∈ U, k = 0, ..., N − 1 (5d)
MPC for collision avoidance with multiple agents requires
ξk ∈ X , k = 1, ..., N (5e)
two prediction models, one for the controlled agent (CA) and
Pr ξk ∈ Ξsafe
one for the dynamic obstacles (DOs) to be avoided. k ≥ β, k = 1, ..., N (5f)
We consider the CA dynamics with U = [u0 , ..., uN −1 ], cost function JN , horizon N ,
actuator constraints U, and deterministic state constraints
ξk+1 = f (ξk , uk ) (1) X . As the DO dynamics (5c) are subject to uncertainty, the
chance constraint (5f) is employed for collision avoidance.
depending on the nonlinear function f with state ξk and At each time step k, the probability of the CA state ξk
input uk at time step k. lying within the safe set Ξsafe must be larger than the risk
k
Two types of uncertainties are considered for the DOs: task parameter β = h (β ta , β ex ), 0 ≤ β ≤ 1. The function
uncertainty and task execution uncertainty. This distinction h (β ta , β ex ) indicates that β depends on a task uncertainty risk
reflects the situation of many applications, where the motion parameter β ta and a task execution uncertainty risk parameter
of surrounding agents is divided into discrete tasks with β ex .
multiple task execution possibilities. It is not possible to directly solve the chance-constrained
Definition 1 (Tasks): At each time step, a DO decides OCP. In the following, a method is derived that approximates
to execute exactly one task Ti defined by the task set the chance constraint (5f) to obtain a tractable OCP. We first
T = {Ti | i = 1, ..., nT }. Each task Ti is assigned a prob- focus on task uncertainty in Section III-A, followed by task
PnT pi , subject to the probability distribution PT , where
ability execution uncertainty in Section III-B, which then allows to
i=1 pi = 1 and 0 < p1 ≤ ... ≤ pnT ≤ 1. A DO input consider both uncertainties simultaneously as described in
corresponding to task Ti is denoted by uDO (Ti ). Section III-C.
Definition 2 (Task Execution): Each task Ti is subject to
III. M ETHOD
a nominal motion governed by the DO dynamics, a reference
state, and an additive Gaussian uncertainty wkex ∼ N (0, Σex In the following, the S+SC MPC framework is derived,
k)
with covariance matrix Σex , representing uncertainty while starting with individually focusing on SCMPC and SMPC.
k
executing task Ti . A. SCMPC for Task Uncertainty
We consider multiple DOs. The dynamics for a single DO We first focus on task uncertainty. At each time step,
is then given by one task is performed. The control action, corresponding
to different tasks, may significantly vary between different
DO
ξk+1 = ADO ξkDO + B DO uDO
k (Ti ) + G
DO ex
wk (2) tasks. Therefore, describing task uncertainty with Gaussian
noise is impractical, rendering analytic SMPC approaches
with the DO state ξkDO , the input uDO
k as well as the state and inapplicable. Considering every possible task may lead to
input matrices ADO , B DO , GDO . A DO stabilizing feedback highly conservative control behavior. However, applying
controller is assumed of the form SCMPC is a suitable approach to handle task uncertainty.
DO With SCMPC, task uncertainty may be approximated by a
uDO ξkDO − ξk,ref
DO
k (Ti ) = K (Ti ) (3) small number of samples, as the number of possible tasks is
usually small. In this section no task execution uncertainty
with feedback matrix K DO and a reference state ξk,ref
DO
is considered, i.e., wkex = 0.
depending on task Ti . The nominal state, assuming zero Here, an SCMPC approach inspired by [9] is used. By
uncertainty and task Ti , follows drawing K samples from the probability distribution PT , the
DO DO
task uncertainty is approximated, yielding the set of samples
ξ DO
k+1 = A ξ k + B DO uDO
k (Ti ) . (4)
S = {si | i = 1, ..., K} , (6)
Collisions with DOs are avoided by determining a set of where a task Ti is assigned to each sample si . An agent may
safe states for the CA. execute the same task for multiple time steps. However, the
Definition 3: The safe set Ξsafe
k for time step k ensures agent task may change at every time step.
that all CA states ξk ∈ Ξsafe
k guarantee collision avoidance Assumption 1: Within each SCMPC OCP, each sampled
at time step k. task is assumed to be executed for the entire prediction
We now formulate the optimal control problem (OCP) to horizon.
be solved within this work. Without loss of generality, the In other words, within the prediction, a sampled task
SMPC OCP starts at time step 0 where prediction steps are is assumed to continue. This assumption is reasonable, asmultiple tasks may be sampled and a new OCP with new with stage cost l and terminal cost Jf .
samples is initiated at each time step. The constraint ξk ∈ Ξsafek may be described by a set of
If Assumption 1 holds, a DO input sequence is ob- functions
tained for each sample of S.DOThe resulting input sequence
dk ξk , ξkDO ≥ 0 ⇔ ξk ∈ Ξsafe
(10)
U DO (si ) = uDO 0 (si ) , ..., u N −1 (si ) depends on the indi- k
vidual inputs uDO (T ), performing task T corresponding to
k i i
with dk = [dk,1 , ..., dk,nd ]> , where nd denotes the number
sample si . Based on U DO (si ), the predicted DO states for
of constraint functions.
each sample are obtained according to the DO dynamics (2)
In order to find an analytic approximation for the chance
with wkex = 0, resulting in the predicted states ξkDO (si ) for
constraint (5f) with only one task, a linearized description
k = 1, ..., N .
of the chance constraint is required. Therefore, the nonlinear
Depending on the predicted DO states, a safe set Ξsafe k (si ) constraint (10) is linearized around the nominal states with
may be computed for each drawn sample si . Each safe set
ξkDO = ξ DO DO DO
k + ek and the prediction error ek . Based on
requires an individual constraint in the SCMPC OCP. There- ex
wk , the prediction error follows ek ∼ N (0, Σek ) where
DO
fore, for the SCMPC approach, the chance constraint (5f) is
adapted to >
Σek+1 = ΦΣek Φ> + GDO Σex
kG
DO
(11)
Pr ξk ∈ Ξsafe ta
k (s) ≥ β , k = 1, ..., N, s ∈ S. (7)
with Φ = ADO + B DO K DO .
Multiple methods exist to generate safe sets, e.g., signed The resulting linearized description of (10) is
distance [15] or grid-based methods [13].
dk ξk , ξ DO + ∇dDO DO
The sample size K depends on the chosen risk parameter. k k ek ≥ 0 (12)
We propose a strategy to obtain K that focuses on the least
with
likely task T1 in T .
Theorem 1: The sample size ∂dk
∇dDO
k = . (13)
∂ξkDO
1 − β ta
ξk ,ξDO
k
K > log1−p1 (8)
p1 The linearized chance constraint is then given by
ensures that the probability of not having sampled the least
Pr ∇dDO DO DO
≥ β ex ,
probable task T1 , if it later occurs, is lower than the allowed k ek ≥ −dk ξk , ξ k (14)
risk 1 − β ta , i.e., (7) is satisfied. which is still a probabilistic expression. However, (14) may
Proof: The proof is based on [10]. Given independent be approximated into an analytic expression similar to [10].
and identically distributed samples, the worst-case probabil- Theorem 2: The probabilistic chance constraint (14) may
ity of not sampling task T1 , if it later occurs, is given by be approximated by the analytic expression
p1 (1 − p1 )K . The sample size K in (8) then follows from
solving for K with 1 − β ta > p1 (1 − p1 )K , i.e., bounding dk,i ξk , ξ DO
k ≥ γk,i (15a)
the worst-case probability given the risk parameter β ta . q
DO > −1
If the least likely task T1 is actually performed by the DO, γk,i = 2∇dDO e
k,i Σk ∇dk,i erf (1 − 2β ex ) (15b)
this worst-case probability of not having sampled task T1 is
with γk = [γk,1 , ..., γk,nd ]> and 0.5 ≤ β ex ≤ 1.
lower than the acceptable risk, defined by the SCMPC risk
Proof: The proof follows [7], [10]. Dueto (11) it holds
parameter β ta .
DO >
After having introduced an SCMPC approach to handle that ∇dDOk e DO
k ∼ N 0, ∇d DO e
k,i Σk ∇dk,i in (14). The
task uncertainty, the following section introduces an analytic quantile function for univariate normal distributions allows
SMPC approximation for task execution uncertainty. to reformulate (14) into (15).
Note that ∇dDO
k,i is defined similar to (13). The individual
B. SMPC Task Execution Uncertainty approaches for handling task uncertainty and task execution
We now focus on task execution uncertainty, assuming uncertainty are combined in the following section.
only one task is possible. In the DO dynamics (2), task
execution uncertainty is described by the additive Gaussian C. S+SC MPC Algorithm
uncertainty, representing uncertainty considering the nominal The results of Section III-A and Section III-B are now
trajectory of a task. Approximating a Gaussian distribution combined in order to obtain the S+SC MPC framework,
potentially requires a large number of samples, therefore, which is able to efficiently handle the mixed uncertainty
an analytic SMPC approach is more suitable than SCMPC. structure. In addition, multiple DOs are considered with the
The cost function (5a) may depend on the DO uncertainty. DO dynamics
Therefore, the cost is adjusted based on the expectation
value, yielding DO,j
ξk+1 = ADO,j ξkDO,j + B DO,j uDO,j
k Tij + GDO,j wkex,j (16)
−1
N
!
with stabilizing feedback matrix K DO,j for the DOs j =
X
JN = E l (ξk , uk , wkex ) + Jf (ξN , wN
ex
) (9)
k=0 1, ..., nDO .TABLE I: Initial Vehicle Configuration
TV4 TV5
TV3 EV TV1 TV2 TV3 TV4 TV5
EV
x-pos. (m) 0 -25 25 40 -30 -10
TV1 TV2 x-vel. (m s−1 ) 27 17 27 27 27 22
Fig. 1: Initial scenario configuration.
As a special case of (1), the EV dynamics are represented
The tractable S+SC MPC OCP for multiple DOs is then using the linear, discrete-time point mass model
given by EV
ξk+1 = AξkEV + BuEV
k (18)
−1
N
!
>
X
∗ ex ex
J = min E l (ξk , uk , wk ) + Jf (ξN , wN ) (17a) with the EV states ξk = [xk , vx,k , yk , vy,k ] and inputs
U >
k=0 uk = [ux,k , uy,k ] where
s.t. ξk+1 = f (ξk , uk ) (17b)
0.5∆t2
DO,j
1 ∆t 0 0 0
ξ k+1,i = ADO,j ξ DO,j
k,i +B DO,j
uDO,j
k sji (17c) 0 1 0 0 ∆t 0
A= 0 0 1 ∆t , B = 0
. (19)
uk ∈ U, k = 0, ..., N − 1 (17d) 0.5∆t2
ξk ∈ X , k = 1, ..., N (17e) 0 0 0 1 0 ∆t
djk,i ξk , ξ DO,j
k,i
j
≥ γk,i , k = 1, ..., N (17f) The TV dynamics are assumed to be subject to un-
r certainties. In the case of vehicles, tasks are maneuvers.
>
j
γk,i = 2∇dDO,j e,j DO,j
k,i Σk ∇dk,i erf −1 (1 − 2β ex ) (17g) Therefore, we consider maneuver uncertainty and maneuver
execution uncertainty. The TV dynamics are in the form
with i = 1, ..., Kj where Kj is determined according to (8) of (2) with ADO , B DO , states, and inputs similar to (19)
for each DO, given the DOs j = 1, ..., nDO . as well as GDO = diag (0.05, 0.067, 0.013, 0.03) account-
In (17f), an individual approximated chance constraint ing for diverse TV uncertainty in longitudinal and lateral
is generated for each sample si , depending on Kj . While direction. The covariance matrix of the normally distributed
this approach is reasonable for a small number of samples, TV maneuver execution uncertainty wkex ∼ N (0, Σex k ) is an
it becomes computationally expensive for a larger Kj . A identity matrix Σexk = diag(1, 1, 1, 1). Furthermore, additive
possible alternative for application is to combine similar measurement noise νk ∼ N (0, Σνk ) is considered for xTV k
individual task in order to reduce the number of total and ykTV with Σνk = diag (0.16, 0.01). The TVs have multiple
constraints. This approach is illustrated in the simulation maneuver options with associated maneuver probabilities.
example in Section IV. The possible maneuvers consist of lane changes to left (LCL)
If it is required to guarantee safety or recursive feasibility, and right (LCR), lane keeping (LK), accelerating (AC),
the proposed S+SC MPC method may be extended by the braking (BR), and insignificant acceleration (IA), as well
safety framework for SMPC approaches proposed in [16]. as a combination of the lateral and longitudinal maneuvers,
resulting in a total of nine possible maneuvers.
IV. S IMULATION S TUDY The road consists of three lanes with lane width llane =
To evaluate the effectiveness of the S+SC MPC algorithm 3.5 m, where the center of the left lane represents y = 0.
presented in Section III-C, a highway scenario involving All vehicles are aveh = 6 m in length and bveh = 2 m in
five target vehicles (TVs) is simulated, using the Control width. The initial lateral position of all vehicles coincides
Toolbox [17]. Here, the CA and DOs become ego vehicle with the lateral center of the vehicles’ respective lanes with
(EV) and TVs, respectively. The initial vehicle configuration zero lateral velocity. The initial longitudinal positions and
is depicted in Figure 1. velocities of all vehicles are summarized in Table I. The TV
TV
reference state is chosen as ξref,k TV
= [0, vx,ref,k TV
, yref,k , 0]> ,
We first present the results of the simulation study with TV TV
the proposed S+SC MPC algorithm, and then, for compar- where vx,ref,k and yref,k may vary over time depending on
ison, we investigate the stand-alone algorithms SMPC and the scenario. The feedback controller for the TVs is
SCMPC. Eventually, we investigate applying S+SC MPC to
DO 0 −1.0 0 0
varying scenario configurations. K = . (20)
0 0 −0.8 −2.2
A. Simulation Setup To prevent collisions, a region around the TV is inadmis-
sible for the EV. This is referred to as the safety constraint,
All simulations are run on an Intel i5-2500K CPU @
where the admissible area is the safe set Ξsafe
k . In line with
3.30GHz with 15.6GB RAM. Each simulation consists of
[10], we impose a safety constraint modeled as an ellipse.
niter = 100 MPC iterations, which is equivalent to a scenario
Its definition adheres to
duration of 20 s with ∆t = 0.2 s. In the following, SI units
are assumed for variables and parameters expressed without 2 2
(∆xk ) (∆yk )
units. dk = 2
+ − 1 ≥ 0, (21)
a b22 2
kuk kR , Jf = k∆ξN kS , with the cost function weights
TVBR,LCL TVIA,LCL TVAC,LCL 2
k+2 k+2 k+2
Q, S ∈ R4×4 , and R ∈ R2×2 , as well as kzkZ = z > Zz
TVIA,LK TVBR,LK TVIA,LK TVAC,LK
k k+2 k+2 k+2 and ∆ξk = ξk − ξref,k with reference ξref,k . For positional
TVBR,LCR
k+2 TVIA,LCR
k+2 TVAC,LCR
k+2
reference tracking in y-direction, the EV reference is set to
its current lane center, while vy,ref,k = 0 and vx,ref,k =
k k+1 k+2
MPC time step 27 m s−1 . For the cost function, the first element of ∆ξk
is neglected, since no reference for xk is imposed. Here,
Fig. 2: Qualitative depiction of the combined safety con- Q = S = diag(0, 3, 0.5, 0.1), R = diag(1, 0.1) are selected.
straint ellipse. Safety ellipses for step k + 1 omitted.
While the maneuver probabilities are scenario specific and
different task uncertainty risk parameters β ta are evaluated,
where we decompose the distance between the EV and TV the task execution risk parameter is chosen to be β ex = 0.8.
TV
into a longitudinal and a lateral component ∆xk = xEV k −xk In case the original MPC problem is infeasible, a recovery
EV TV
and ∆yk = yk − y k . MPC OCP is solved with slack variables to soften the
The ellipse center coincides with the TV center. Therefore, constraints, as described in [10]. For the recovery OCP,
(21) is fulfilled if the EV center lies outside the inner space the slack variable weight in the cost function is λ = 50
or on the edge of the ellipse, i.e., dk ≥ 0. The parameters and the task execution risk parameter is changed to a more
a = 30 and b = 2 represent the semi-major and semi-minor conservative value βλex = 0.995, to prioritize safety. In case
axis of the ellipse, respectively. The values of a and b are the recovery problem fails, the solver selects the last feasible
chosen conservatively, i.e., the area covered by the safety point as the solution to the OCP.
ellipse is larger than the vehicle shape. Apart from the safety constraint, the EV plans its motion
To reduce the number of constraints for sampled TV subject to the constraints −1.75 ≤ yk ≤ 8.75, −5 ≤ ux,k ≤
maneuvers, we first introduce a method to adapt the safety 5, −0.5 ≤ uy,k ≤ 0.5, −1 ≤ ∆ux,k ≤ 1, −0.2 ≤ ∆uy,k ≤
constraint ellipse (21). As an example, we assume that all 0.2 with ∆ux,k = ux,k − ux,k−1 , ∆uy,k = uy,k − uy,k−1 .
possible maneuvers are sampled. Then, as mentioned as
a possibility in Section III-C, we combine the individual B. Simulation Results
constraint ellipses of all sampled maneuvers at each time
step, as shown in Figure 2. If less maneuvers are sampled, In the following, the S+SC MPC algorithm is evaluated
the aggregated ellipse only covers the sampled maneuvers. in the presented scenario. As mentioned, the maneuver risk
The result is the aggregated ellipse parameter β ta is varied, resulting in a varying sample size K.
Monte Carlo simulations are conducted 150 times for each
2 2
(∆x̃k ) (∆ỹk ) risk parameter value.
d˜k = 2 + − 1 ≥ 0, (22a)
ãk b̃2k Each simulation consists of two parts. For the first 20
∆x̃k = xk − x̃TV steps, the EV follows a conservative behavior with β ta =
k , (22b)
0.999, representing a behavior prediction initialization phase.
∆ỹk = yk − ỹkTV , (22c) The EV assumes that the TV probabilities for lane changes
xTV,IA + xTV,BR + xTV,AC or changes in acceleration are pLC = 0.80 and pAC = pBR =
x̃TV
k =
k k k
, (22d)
3 0.40. In case a lane changes is possible to the left or right,
ykTV,LK + ykTV,LCL + ykTV,LCR pLC is assigned equally. In the second part from step 21 to
ỹkTV = (22e) step 100, it is assumed that the EV has adapted its behavior
3
prediction. Therefore, the probabilities of TV maneuvers
with center x̃TV TV
k , ỹk . The longitudinal and lateral posi- change to pLC = 0.20 and pAC = pBR = 0.10. For the
tion of the TV corresponding to the respective maneuvers second part of the simulation, different risk parameters β ta ∈
are indicated by the variables xTV,M k and ykTV,M , M ∈ {0.99, 0.95, 0.89, 0.83} are evaluated. Within the actual sim-
{IA, BR, AC, LK, LCL, LCR}, respectively. The combined ulation, all TVs maintain their respective lanes, except TV4,
ellipse exhibits the adjusted semi-major and semi-minor axes which moves to the center lane. The reference velocities
TV1
in x-direction are vx,ref = 22 m s−1 , vx,ref
TV2
= 22 m s−1 ,
2
vx,ref = 17 m s , vx,ref = 17 m s , vx,ref = 27 m s−1 .
TV3 −1 TV4 −1 TV5
ãk = a + 0.5 xTV,
k
AC
− xTV,
k
BR
+ b̃k − b , (23a)
llane The result of an individual example with β ta = 0.95 is
b̃k = b + 0.5 ykTV, LCL − ykTV, LCR . (23b) illustrated in Figure 3. While there initially is a gap between
TV3 and TV5, the EV does not plan to overtake, as a
By generating the aggregated safety ellipse, the number of potential lane change of either TV3 or TV5 would result in an
necessary constraints is reduced. As seen in Figure 2, the inevitable collision. Therefore, the EV slows down such that
aggregated ellipse does not necessarily cover all individual TV5 passes TV3 first. Subsequently, the EV safely moves to
safety ellipses perfectly, which is still reasonable as the the left lane to overtake TV3.
individual safety ellipses are designed larger than necessary. Even though SMPC, in general, allows a small probability
For the MPC OCP (17), a prediction horizon N = 12 is of constraint violation, in regular scenarios collisions are
2
selected. The cost function terms are set to l = k∆ξk kQ + avoided as the repetitively updated SMPC inputs allow toTABLE III: SMPC and SCMPC Simulation Results
SMPC SCMPC
risk parameter 0.8 0.99 0.95 0.89 0.83
collisions 79 49 43 45 41
cost J100 3.22e4 6.77e4 6.27e4 6.88e4 7.08e4
infeasible OCP
31.2 54.1 53.6 55.7 54.7
steps
infeasible rec.
21.8 33.8 33.6 34.1 34.3
OCP steps
Fig. 3: Vehicle motion for simulation steps 21 (top) and 45
(bottom). The EV is shown in red, TVs in blue. Fading boxes [7], is analyzed with β ex = 0.8. The advantage of S+SC
represent past states. MPC is that the mixed uncertainty structure is exploited.
Applying only SMPC, in order to account for maneuver and
TABLE II: S+SC MPC Simulation Results execution uncertainty, multiple possible maneuvers would
need to be approximated by a Gaussian uncertainty. However,
risk parameter β ta 0.99 0.95 0.89 0.83
this would result in a major increase of the safety ellipse,
collisions 0 0 0 0 covering the entire road width, rendering overtaking other
cost J100 3.64e4 3.40e4 3.59e4 3.76e4 TVs impossible. Therefore, in the SMPC simulation, the
infeasible OCP SMPC algorithm only accounts for maneuver execution
26.3 25.2 24.2 26.6
steps uncertainty.
infeasible rec.
2.2 3.2 5.2 7.4
A total of 79 collisions occurred. While the cost is slightly
OCP steps lower compared to S+SC MPC, significantly more steps with
infeasible OCPs occur, especially for the recovery problem.
constantly adjust. For example, it may not be possible to sat- In the SCMPC simulation, inspired by [11], the maneuver
isfy the chance constraint for a late prediction step within the execution uncertainty is approximated by samples. To com-
SMPC horizon, due to an unexpected uncertainty realization. pare a similar situation as in the SMPC simulation, no task
The OCP is therefore infeasible. However, a collision may uncertainty is considered here. Again, a significant number
still be prevented in the next steps, depending on the future of simulation runs result in collisions, while the cost also
uncertainty realizations. Here, we designed a challenging increases compared to S+SC MPC. The steps with infeasible
situation for the EV, as lane changes are considered to be OCPs appear more often than in the S+SC MPC simulation
probable for all TVs and must be accounted for. The results runs. The computation times for SMPC and SCMPC are
of the Monte Carlo simulations are shown in Table II. similar to S+SC MPC.
Summarizing the simulation results, the first important
observation is that no collisions occurred. While the safety D. Varying Vehicle Settings
ellipse is slightly violated in some simulation runs, the safety So far, only one vehicle setting is considered. Therefore,
ellipse is chosen large enough that no collisions followed. we additionally ran 150 simulations with randomly chosen
The performance is evaluated by computing the cost at TV settings for each simulation run (similar initial EV state
each time step, based on the actual states and inputs, with as before). The TVs get assigned random initial positions
99
X xTV
0 ∈ [−150, 150] and are placed on one of the three lanes,
2 2
J100 = k∆ξk+1 kQ + kuk kR . (24) i.e., y0 ∈ {0, 3.5, 7}. The constant longitudinal velocity for
k=0 each TV is randomly chosen according to vxTV ∈ [17, 27] with
The cost remains on a similar level for all risk parameters, vyTV = 0. It is ensured that all vehicles positioned on similar
where the best choice in this scenario is β ta = 0.95. Lower lanes have enough longitudinal distance ∆x ≥ 50, and
risk increases conservatism, while high risk results in less velocities are selected such that TV collisions are avoided.
smooth control inputs, again increasing the cost. The proposed S+SC MPC method successfully handled all
As mentioned before, the potential lane changes of all TVs 150 simulation runs and no collisions occurred.
pose a challenging situation for the EV, resulting in steps Overall, S+SC MPC allows exploiting the uncertainty
where the OCP becomes infeasible. However, the steps with structure of the simulation setting, achieving adequate perfor-
successfully solved recovery OCPs are significantly more mance and avoiding collisions. While the results presented
likely, especially for a low accepted level of risk. The average here are promising, it is to note that the benefits of the
computation time is 214 ms. proposed method depend on the application setting and to
which degree the uncertainty structure may be exploited.
C. Comparison to SMPC and SCMPC
We now compare the results of S+SC MPC to only V. C ONCLUSION
applying SMPC or SCMPC. the results are shown in Ta- The proposed S+SC MPC method allows considering the
ble III. First, an analytic SMPC algorithm, inspired by specific uncertainty structure found in many applications,where both task uncertainty and task execution uncertainty
are present. As SCMPC is suitable for non-Gaussian task
uncertainty and SMPC copes well with Gaussian execution
uncertainty, the combination shows promising results.
While in this work the S+SC MPC method is applied to
a vehicle scenario, the framework is designed in a general
way, such that it is applicable also to other applications,
e.g., human-robot collaboration. In this robotics setting, a
robotic arm may have the option of moving to one of several
items, while the exact motion towards the specific item may
vary. Without specifically focusing on agents, the proposed
framework may also be applicable to process control or
finance.
R EFERENCES
[1] W. Langson, I. Chryssochoos, S.V. Raković, and D.Q. Mayne. Robust
model predictive control using tubes. Automatica, 40(1):125 – 133,
2004.
[2] J.B. Rawlings, D.Q. Mayne, and M. Diehl. Model Predictive Control:
Theory, Computation, and Design. Nob Hill Publishing, 2017.
[3] A. Mesbah. Stochastic model predictive control: An overview and
perspectives for future research. IEEE Control Systems, 36(6):30–44,
Dec 2016.
[4] M. Farina, L. Giulioni, and R. Scattolini. Stochastic linear model
predictive control with chance constraints – a review. Journal of
Process Control, 44(Supplement C):53 – 67, 2016.
[5] A.T. Schwarm and M. Nikolaou. Chance-constrained model predictive
control. AIChE Journal, 45(8):1743–1752, 1999.
[6] B. Kouvaritakis, M. Cannon, S.V. Rakovic, and Q. Cheng. Explicit use
of probabilistic distributions in linear predictive control. Automatica,
46(10):1719 – 1724, 2010.
[7] A. Carvalho, Y. Gao, S. Lefevre, and F. Borrelli. Stochastic predictive
control of autonomous vehicles in uncertain environments. In 12th
International Symposium on Advanced Vehicle Control, Tokyo, Japan,
2014.
[8] L. Blackmore, M. Ono, A. Bektassov, and B.C. Williams. A proba-
bilistic particle-control approximation of chance-constrained stochastic
predictive control. Trans. Rob., 26(3):502–517, June 2010.
[9] G. Schildbach, L. Fagiano, C. Frei, and M. Morari. The scenario
approach for stochastic model predictive control with bounds on
closed-loop constraint violations. Automatica, 50(12):3009 – 3018,
2014.
[10] T. Brüdigam, M. Olbrich, M. Leibold, and D. Wollherr. Combining
stochastic and scenario model predictive control to handle target
vehicle uncertainty in autonomous driving. In 21st IEEE International
Conference on Intelligent Transportation Systems (ITSC), 2018.
[11] G. Schildbach and F. Borrelli. Scenario model predictive control for
lane change assistance on highways. In 2015 IEEE Intelligent Vehicles
Symposium (IV), pages 611–616, Seoul, South Korea, June 2015.
[12] G. Cesari, G. Schildbach, A. Carvalho, and F. Borrelli. Scenario model
predictive control for lane change assistance and autonomous driving
on highways. IEEE Intelligent Transportation Systems Magazine,
9(3):23–35, Fall 2017.
[13] T. Brüdigam, F. di Luzio, L. Pallottino, D. Wollherr, and M. Leibold.
Grid-based stochastic model predictive control for trajectory planning
in uncertain environments. In 23rd IEEE International Conference on
Intelligent Transportation Systems (ITSC), 2020.
[14] A. Muraleedharan, A. Tran, H. Okuda, and T. Suzuki. Grid-based
stochastic model predictive control for trajectory planning in uncertain
environments. In IFAC World Congress 2020, Berlin, Germany, 2020.
[15] J. Schulman, J. Ho, A. Lee, I. Awwal, H. Bradlow, and P. Abbeel.
Finding locally optimal, collision-free trajectories with sequential
convex optimization. In Robotics: Science and Systems 2013, 2013.
[16] T. Brüdigam, M. Olbrich, D. Wollherr, and M. Leibold. Stochastic
model predictive control with a safety guarantee for automated driving.
IEEE Transactions on Intelligent Vehicles, pages 1–1, 2021.
[17] M. Giftthaler, M. Neunert, M. Stäuble, and J. Buchli. The Control
Toolbox - an open-source C++ library for robotics, optimal and
model predictive control. In 2018 IEEE International Conference
on Simulation, Modeling, and Programming for Autonomous Robots
(SIMPAR), pages 123–129, May 2018.You can also read
