of 8
Trajectory Optimization for Chance-Constrained
Nonlinear Stochastic Systems
Yashwanth Kumar Nakka and Soon-Jo Chung
Abstract
ó This paper presents a new method of comput-
ing a sub-optimal solution of a continuous-time continuous-
space chance-constrained stochastic nonlinear optimal control
problem (SNOC) problem. The proposed method involves two
steps. The rst step is to derive a deterministic nonlinear
optimal control problem (DNOC) with convex constraints that
are surrogate to the SNOC by using generalized polynomial
chaos (gPC) expansion and tools taken from chance-constrained
programming. The second step is to solve the DNOC problem
using sequential convex programming (SCP) for trajectory
generation. We prove that in the unconstrained case, the optimal
value of the DNOC converges to that of SNOC asymptotically
and that any feasible solution of the constrained DNOC is a
feasible solution of the chance-constrained SNOC because the
gPC approximation of the random variables converges to the
true distribution. The effectiveness of the gPC-SCP method is
demonstrated by computing safe trajectories for a second-order
planar robot model with multiplicative stochastic uncertainty
entering at the input while avoiding collisions with a specied
probability.
I. I
NTRODUCTION
Model-based design strategies for planning [1] and con-
trol [2] of robotic systems often take a deterministic approach
with robustness guarantees [3] to quantify performance un-
der worst-case uncertainties. These approaches assume a
bounded value of uncertainty leading to conservative tra-
jectories and control laws. Central to a condence-based
design solution for a robot or vehicle operating in dynamic
environment is a systematic approach that accounts for un-
certainties in the dynamic model, state and input constraints,
and even state estimation of highly-nonlinear systems to
guarantee safety and performance with high probability.
Examples of systems that require safety guarantees under
uncertainty include spacecraft with thrusters as actuators
during proximity operations [4], and a quadrotor ying in
turbulent winds [5].
Considering safety in conjunction with optimality be-
comes more difcult if the dynamics themselves are sub-
ject to stochastic noise. Such optimal motion planning or
guidance problems with stochastic dynamics can be for-
mulated as a continuous-time continuous-space stochastic
nonlinear optimal control problem (SNOC) with chance con-
straints. Suboptimal-solution methods to solve model-based
SNOC problem based on Pontryagin's minimum principle
include differential dynamic programming [6] and iterative
linear-quadratic-Gaussian [7]. Sampling-based methods like
Monte Carlo motion planning [8] for trajectory optimization,
Yashwanth Kumar Nakka and Soon-Jo Chung are with the De-
partment of Aerospace (GALCIT), California Institute of Technology.
Email:
f
ynakka@caltech.edu, sjchung@caltech.edu
g
Markov chain approximation method [9], and path-integral
approach [10] can be used under certain assumptions on the
cost function, dynamics, and uncertainty for systems afne
in control.
In this paper, we present a novel approach to compute
solution trajectories of a chance-constrained SNOC problem.
The method involves deriving a deterministic nonlinear op-
timal control (DNOC) problem with convex constraints that
are a surrogate to the SNOC problem by systematically ac-
counting for nonlinear stochastic dynamics using generalized
polynomial chaos expansions (gPC) [11], [12] and obtaining
deterministic convex approximations of linear and quadratic
chance constraints using tools taken from chance constrained
programming [13], [14]. The DNOC problem is then solved
using sequential convex programming (SCP) [15], [16] for
real-time trajectory optimization. The gPC approach uses
function approximation theory to model an unknown ran-
dom process with basis functions that are chosen based
on knowledge of the uncertainty affecting the process [11].
For example, a Hermite polynomial basis with the standard
normal distribution is known to yield exponential conver-
gence [11] if the uncertainties in the system are Gaussian.
The gPC approximation is used to derive ordinary differential
equations (ODEs) in terms of gPC coefcients. The DNOC
problem and convex constraints are reformulated in terms of
gPC coefcients as decision variables to apply a trust-region-
based SCP method to compute feasible trajectories.
The gPC expansion approach was used for stability anal-
ysis and control design of uncertain systems [17], [18],
[19]. For trajectory optimization, recent work focuses on
nonlinear systems with parametric uncertainty [20], [21]
with no constraints on the state, or linear systems with
linear chance-constraints that do not extend to the SNOC
problem considered here and lack analysis on the determin-
istic approximation of the uncertain system. In [22], [23]
linear chance constraints were considered for probabilistic
optimal planning, strictly for linear systems. The literature
on chance-constrained programming focuses on problems
with deterministic decision variable and uncertain system
parameters for both linear [13] and nonlinear [14] cases. The
linear chance-constraint results can be readily transformed
to the case with a random decision variable for an unknown
measure. The quadratic chance constraint would lead to an
inner semi-denite program [24] that adds complexity to the
SNOC problem considered in this paper.
The main contribution of the present paper is hence to
solve a class of optimal control problems that include both
chance or probabilistic inequality constraints and stochastic
nonlinear dynamics via gPC-SCP. In order to character-
ize the deterministic approximation obtained using a gPC
method, we present analysis on convergence of a DNOC
problem to the SNOC problem for an unconstrained case.
Then, we prove that any feasible solution of the constrained
DNOC problem is a feasible solution of chance-constrained
SNOC problem with an appropriate gPC transformation
step applied. This proves that solving a DNOC problem
ensures constraint satisfaction with the specied probability.
In order to bound the deviation of the random variable from
its mean, we derive a conservative deterministic quadratic
constraint approximation of the quadratic chance-constraint
using multivariate Chebyshev inequality [25], which can be
used to nd trajectories that have a specied variance.
The paper is organized as follows. We discuss the stochas-
tic nonlinear optimal control (SNOC) problem with results
on deterministic approximations of chance constraints along
with preliminaries on gPC expansions in Sec. II. The deter-
ministic surrogate of the SNOC problem in terms of the gPC
coefcients and a SCP formulation of the DNOC problem are
presented with analysis in Sec. III. In Sec. IV, we apply the
proposed gPC-SCP method for computing a safe trajectory
for Dubin's second-order model with specied probability for
avoiding collisions and discuss the limitations of the method.
We conclude the paper in Sec. V with brief discussion on the
results of the analysis and the application of the gPC-SCP
method.
II. P
ROBLEM AND
P
RELIMINARIES
A. Stochastic Nonlinear Optimal Control Problem
In this section, we present the nite-horizon stochas-
tic nonlinear optimal control problem with joint chance
constraints in continuous time and continuous space. The
problem considered here has an expectation cost function
which is quadratic in the random state variable
x
(
t
)
and
the deterministic control policy

u
(
t
)
. The evolution of the
stochastic process
x
(
t
)
for all sampled paths is dened by a
stochastic differential equation. The joint chance constraints
guarantee constraint feasibility with a probability of
1

,
where
 >
0
and is chosen to be a small value (example:

2
[0
:
001
;
0
:
05]
) for better constraint satisfaction. The
following optimal control problem is considered with the
state and control as the decision variables.
J

=
min
x
(
t
)
;

u
(
t
)
E
h
R
t
f
t
0
L
(
x
(
t
)
;

u
(
t
))
dt
+
L
f
(
x
(
t
f
))
i
s.t.
_
x
(
t
) =
f
(
t;x
(
t
)
;

u
(
t
)) +
g
(
t;x
(
t
)
;

u
(
t
))

(
t
)
Pr(
x
(
t
)
2
F
)

1


u
(
t
)
2
U
8
t
2
[
t
0
;t
f
]
x
(
t
0
) =
x
0
E(
x
(
t
f
)) = ^
x
f

(
t
)
2N
(
^

(
t
)
;


(
t
))
t
2
[
t
0
;t
f
]
(1)
where the cost functional
L
and the terminal cost
L
f
are
dened as
L
(
x
(
t
)
;

u
(
t
)) =
x
(
t
)
>
Qx
(
t
) + 
u
(
t
)
>
R

u
(
t
)
L
f
(
x
(
t
f
)) =
x
(
t
f
)
>
Q
f
x
(
t
f
)
(2)
The matrices
Q
and
Q
f
are positive semi-denite and
R
is a positive denite matrix. In the following, we dene
each of the aforementioned elements of the problem (1)
and discuss convex approximations of linear and quadratic
chance constraints.
1) Stochastic Differential Equation (SDE) [26]:
The dy-
namics of the system is modeled as a controlled diffusion
process with Ito assumptions. The random variable
x
(
t
)
is
dened on a probability space
(
;
F
;
Pr)
where
is the
sample space,
F
forms a

-eld with measure
Pr
.
dx
(
t
) =
f
(
t;x
(
t
)
;

u
(
t
))
dt
+
g
(
t;x
(
t
)
;

u
(
t
))
dw
(
t
)
;
Pr(
j
x
(
t
0
)
x
0
j
= 0) = 1
;
8
t
0

t

t
f
<
1
(3)
where:
f
(
:;:
) :
R
+

S

U
!
R
d
x
,
g
(
:;:
) :
R
+

S

U
!
R
d
x

d

, and
w
(
t
)
is a
d

-dimensional Wiener process and
the initial random variable
x
0
is independent of
w
(
t
)
w
(
t
0
)
for
t

t
0
. The sets
S

R
d
x
and
U

R
d
u
are compact sets.
Assumption
1
.
The
functions
f
(
t;x
(
t
)
;

u
(
t
))
and
g
(
t;x
(
t
)
;

u
(
t
))
are dened and measurable on
R
+

S

U
.
Assumption
2
.
Equation (3) has a unique solution
x
(
t
)
, that
is continuous with probability 1, and
9
a
K
2
R
++
such
that the following conditions are satised:
a) Lipschitz condition:
8
t
2
[
t
0
;t
f
]
,
(
x
j
;

u
j
)
2
S

U
,
j
=
1
;
2
,
k
f
(
t;x
1
;

u
1
)
f
(
t;x
2
;

u
2
)
k
+
k
g
(
t;x
1
;

u
1
)
g
(
t;x
2
;

u
2
)
k
F

K
k
(
x
1
;

u
1
)
(
x
2
;

u
2
)
k
(4)
b) Restriction on growth:
8
t
2
[
t
0
;t
f
]
,
(
x
1
;

u
1
)
2
S

U
k
f
(
t;x
1
;

u
1
)
k
2
+
k
g
(
t;x
1
;

u
1
)
k
2
F

K
2
(1 +
k
(
x
1
;

u
1
)
k
2
)
(5)
The noise

(
t
) =
dw
dt
is assumed to be driven by a Gaussian
process with known mean
^

(
t
)
and variance


(
t
)
that are
bounded piece-wise continuous functions of time
t
.
2) Control Policy:
The deterministic control policy is
motivated by a hardware implementation strategy, where
a state dependent Markov control policy dened on the
compact set
S
is sampled for a value with highest probability
(or) for the mean.
Assumption
3
.
The control policy

u
(
t
)
2
U

R
d
u
is
deterministic and the set
U
is a convex set.
3) Chance Constraints [13]:
The feasible region or set
F
is given by the linear and quadratic inequality constraint:
F
=
f
x
(
t
)
2
S
:
f
i
(
x
(
t
))

0
8
i
2 f
1
;:::;m
gg
;
(6)
which can be relaxed by the chance constraint set with a
guaranteed constraint satisfaction probability of
1


CC
=
f
x
(
t
)
2
S
: Pr(
x
(
t
)
2
F
)

1

g
(7)
A convex relaxation of the chance constraint for an arbitrary
distribution of the state vector
x
(
t
)
due to the nonlinearity
in the system is intractable, so an extension of the problem
called Distributionally Robust Chance Constraints (DRCC)
given as follows,

DRCC
=
f
x
(
t
)
2
S :
inf
x
(
t
)

(^
x;

x
)
Pr(
x
(
t
)
2
F
)

1

g
(8)
where the chance constraint is satised for all distributions
with known mean and variance of the decision variable is
used. The set dened by the DRCC in (8) is a conservative
approximation [14] of the chance constraint i.e.,

DRCC


CC
.
a)
Distributionally Robust Linear Constraint (DRLC)
[13]:
Consider a single linear chance constraint with
a
2
R
n
and
b
2
R
:

LCC
=
f
x
(
t
)
2
S
: Pr(
a
>
x
(
t
) +
b

0)

1

g
(9)
Assuming that the mean
^
x
and the covariance

x
of
x
are
known, a distributionally robust constraint version of (9) is
given as

DRLC
=
f
x
(
t
)
2
S
:
inf
x
(
t
)

(^
x;

x
)
Pr(
a
>
x
(
t
)+
b

0)

1

g
(10)
Equivalently, (10) can be rewritten in the following determin-
istic form, which will be later used to derive a second-order
cone constraint for the DNOC:

DRLC
=
f
x
(
t
)
2
S
:
a
>
^
x
(
t
) +
b
+
r
1


p
a
>

x
a

0
g
(11)
Remark
1
.
The set dened in (11) is a subset of the original
constraint in (9) i.e.,

DRLC


LCC
, See [13] for the proof.
If the dynamics is linear, constraint in (11) is replaced
with a much tighter equivalent deterministic constraint given
by the following equation

GLCC
=
f
x
(
t
)
2
S
:
a
>
^
x
(
t
)+
b
+erf(1
2

)
p
a
>

x
a

0
g
(12)
where the function erf
(
:
)
is dened as erf
(

) =
2
p

R

0
e
t
2
dt
and the constraint set

GLCC
is transformed to a second-
order cone constraint for

2
(0
;
0
:
5)
.
b)
Quadratic Constraint:
Proposition 1 presents a new con-
servative deterministic relaxation for the quadratic chance
constraint that can be used to bound the deviation of the
random vector
x
(
t
)
from the mean
^
x
(
t
)
.
Proposition 1.
The constraint set

DQ
=
f
x
(
t
)
2
S
:
1
c
tr
(
A

x
)


g
(13)
is a conservative approximation of the original quadratic
chance constraint

CCQ
=
f
x
(
t
)
2
S
: Pr((
x
^
x
)
>
A
(
x
^
x
)

c
)


g
(14)
i.e.,

DQ


CCQ
, where
A
2
R
n

n
is a positive denite
matrix and
c >
0
2
R
and

x
(
t
)
is the covariance of the
random variable
x
(
t
)
at time
t
.
Proof:
We will prove that any random vector
x
2
S
that
is in the set

DQ
is also in the set

CCQ
implying

DQ


CCQ
. The two sets are dened by the following inequalities
respectively. The proof follows from the approach taken
to prove the multivariate Chebyschev's inequality [25]. Let
F
(
x
)
be the Cumulative Distribution Function (CDF) of the
random variable
x
and
v
=
x
^
x
.
G
=
f
v
2
S
:
v
>
Av

c
g
=
)
1
c
v
>
Av

1
8
v
2
G
Using the denition of probability in terms of the CDF,
Pr((
x
^
x
)
2
G
)

1
c
Z
v
2
G
v
>
AvdF
(
v
)

1
c
Z
v
2
R
n
v
>
AvdF
(
v
)
Let
a
ij
be an element of matrix
A
in the
i
th
row and
j
th
column, and
v
i
be the
i
th
element in the vector
v
. Using the
expansion
v
>
Av
=
P
n
i
=1
P
n
j
=1
a
ij
v
i
v
j
in the inequality
above, the integral is simplied.
Z
v
2
R
n
v
>
AvdF
(
v
) =
Z
v
2
R
v
n
X
i
=1
n
X
j
=1
a
ij
v
i
v
j
dF
(
v
)
=
n
X
i
=1
n
X
j
=1
a
ij
Z
v
2
R
n
v
i
v
j
dF
(
v
)
=
tr
(
A

x
)
(15)
Now consider the inequality in (13). The quadratic chance
constraint holds if (13) is satised, as
Pr((
x
^
x
)
2
G
)

1
c
tr
(
A

x
)
. Therefore, (13) is a conservative deterministic
approximation of the quadratic chance constraint
Pr((
x
^
x
)
>
A
(
x
^
x
)

c
)


i.e.,

DQ


CCQ
. Note that
if

is a design variable, the approximation can be made
tight by solving an inner semi-denite program following
the approach in [24].
c)
Joint Chance Constraints (JCC)
[14]: The joint quadratic
chance constraints are split into multiple individual con-
straints using Boole's inequality.
inf
x
(
t
)

(^
x;

x
)
Pr(
^
m
i
=1
a
>
i
x
+
b
i

0)

1

()
sup
x
(
t
)

(^
x;

x
)
Pr(
_
m
i
=1
a
>
i
x
+
b
i

0)



m
X
i
=1
sup
x
(
t
)

(^
x;

x
)
Pr(
a
>
i
x
+
b
i

0)


(16)
inf
x
(
t
)

(^
x;

x
)
Pr(
a
>
i
x
+
b
i

0)

1

i
(17)
The distributionally robust joint chance constraints (DR-
JCC) are split into multiple single chance constraints using
Bonferroni's inequality [14] method in (16) with naive risk
allocation. Equations (16), (17) show the steps for DRJCC
using Bonferroni's inequality, a similar approach is followed
for JCC using Boole's inequality [22]. The risk

is split
between all the
m
constraints equally
P
m
i
=1

i
=

such
that we get
m
individual DRCC of the form given in (17).
The risk allocation can be tightened by solving a two stage
optimization problem as discussed in [27].
B. Wiener-Askey Polynomial Chaos ([11], [12], [20])
The generalized Polynomial Chaos (gPC) expansion the-
ory is used to model uncertainty with nite second-order
moments as a series expansion of orthogonal polynomials.
The polynomials are orthogonal with respect to a known
density function

(
:
)
. Consider the random vector

with
independent identically distributed (iid) random variables
f

i
g
d

i
=1
as elements. Each

i
is normally distributed with
zero mean and unit variance. The random vector
x
(
t
)
, dened
by the SDE in (3) can be expressed as the following series
of polynomials
x
i
(
t
) =
P
1
j
=0
x
ij
(
t
)

j
(

)
(18)
where
x
i
denote the
i
th
element in the vector
x
2
S
and
x
ij
is the
j
th
coefcient in the series expansion. The dimension
d

is the sum of number of random inputs in the SDE (3)
and the number of random initial conditions. The func-
tions

j
(

)
are constructed using Hermite polynomial [11]
basis such that are orthogonal with respect to the joint
probability density function

(

) =
%
(

1
)
%
(

2
)

%
(

d
)
,
where
%
(

k
) =
1
p
2

e

2
k
2
. We refer to [28] for details on
construction of the polynomials. The series expansion is
truncated to a nite number
`
+ 1
as
x

P
`
j
=0
x
ij
(
t
)

j
(

)
based on the maximum degree of the polynomials required
to represent the variable
x
.
x
ij
(
t
) =
R
D

(

)
x
i
(
t
)

j
(

)
d
h

j
(

)
;
j
(

)
i
(19)
The
coefcients
x
ij
(
t
)
are
computed
using
the
Galerkin
projection
given
in
(19),
where
h

i
(

)
;
j
(

)
i
=
R
D

(

)

i
(

)

j
(

)
d
.
Z
D

(

)
x
i
(
t
)

j
(

)
d
=
m
X
k
=1
w
k

(
n
k
)
x
i
(
t
)

j
(
n
k
)
(20)
For non-polynomial functions the Galerkin projection can be
approximately computed using Stochastic collocation [12]
method as shown in (20) using Gauss-Hermite quadrature,
where
n
k
are quadrature nodes and
w
k
are the respective
node weights. In the following section, we derive an approx-
imate nonlinear ordinary differential equation system for the
SDE in (3) using gPC expansion and the Galerkin scheme.
Lemma 1.
(Cameron-Martin Theorem [29]) The gPC series
approximation in (18) converges to the true value
x
i
2L
2
.
k
x
i
(
t
)
P
`
j
=0
x
ij
(
t
)

j
(

)
k
L
2
!
0
;
as
`
! 18
t
2
[
t
0
;t
f
]
(21)
Remark
2
.
The expectation
E(
x
i
)
and variance

x
i
of
the random variable
x
i
can be expressed in terms of the
coefcients of the expansion as given below
E(
x
i
) =
x
i
0

x
i

`
X
j
=1
x
2
ij
h

j
;
j
i
as
`
!1
(22)
Lemma 1 and Remark 2 will be used in studying the
convergence of the gPC approximation of the cost function,
the SDE and the chance constraints. Furthermore, the higher-
order moments can be expressed as a polynomial function
of the coefcients.
III. D
ETERMINISTIC SURROGATE OF THE
SNOC
P
ROBLEM
The stochastic nonlinear optimal control problem dis-
cussed in Sec. II-A is reformulated in terms of the coef-
cients of the gPC expansion, with decision variables as
the gPC coefcients and the control

u
. In the following,
we discuss the existence and uniqueness of a solution to
the coupled Ordinary Differential Equations (ODE) obtained
form gPC approximation of SDE, and present the convex
constraints for the gPC coefcients obtained from deter-
ministic approximation of chance constraints. The main
theorem that discusses the convergence and feasibility of the
approximation is presented at the end of this section.
A. Deterministic ODE Approximation of the SDE
The gPC expansion in (18) is applied for all the elements
in the vector
x
2
S

R
d
x
and the matrix representation
using Kronecker product is given in the following, where
X
=

x
10

x
1
`

x
d
x
0

x
d
x
`

>
(

) =


0
(

)


`
(

)

>
(23)
x



X
;
where

 =
I
d
x

d
x
(

)
>
(24)
The dynamics of the coefcients
x
ij
with the above notation
is given in (25), where:
f
i
and
g
i
are the
i
th
element of
the vector
f
(
t;x;

u
)
and
i
th
row of the matrix
g
(
t;x;

u
)
respectively.
_
x
ij
(
t
) =

f
ij
(
t;X;

u
) + 
g
ij
(
t;X;

u;
^
;


)

f
ij
=
R
D

(

)

j
(

)
f
i
(
t;


X;

u
)
d
h

j
(

)
;
j
(

)
i

g
ij
=
R
D

(

)

j
(

)
g
i
(
t;


X;

u
)

(
;t
)
d
h

j
(

)
;
j
(

)
i

(
;t
) =
^

(
t
)

0
(

) +
p


(
t
)

1
(

)
(25)
The full nonlinear ODE with the stacked vector
X
is given
as
_
X
=

f
(
t;X;

u
) + 
g
(
t;X;

u;
^

(
t
)
;


(
t
))
(26)
The sequential convex programming method used for tra-
jectory optimization involves linearization of the dynamics
about a given trajectory and discretization for time inte-
gration. The existence and uniqueness of solution to the
ODE surrogate ensure convergence of any Picard iteration
scheme used for integration. In Proposition 2, we will present
conditions for existence and uniqueness of solution to the
ODE in (25).
Proposition 2.
The ODE system in (25) obtained using gPC
approximation of the SDE has a solution and the solution
is unique assuming that the SDE satises the existence
and uniqueness conditions in (4), (5) and the expectation
in (27) is bounded for each
j
= 0
;
1
;

;`
, where:
P
=


 0
0
I

,
k
j
=
h

j
;
j
i
,
L
f
j
(

) =
j

j
(

)
jk
P
k
2
,
L
g
j
(

) =
L
f
j
(

)
j

1
(

)
j
and
K
f
ij
=
K
k
j
E(
L
f
j
(

))
.
K
g
ij
=
k
^

(
t
)
k
2
K
f
ij
+
K
k
j
k


(
t
)
k
2
E(
L
g
j
(

))
(27)
Proof:
The existence and uniqueness is proved
by showing local Lipschitz continuity of the func-
tions
f
ij
;g
ij
in (25) w.r.t
(
X;

u
)
8
t
2
[
t
0
;t
f
]
in
the ball
B
=
f
(
X;

u
)
2
R
d
x
+
d
u
jk
(
X;

u
)
(
X
0
;

u
0
)
k
2

r
g
where
(
X
0
;

u
0
)
is the initial condition for each
ij
and piece-
wise continuity w.r.t
t
. The piece-wise continuity w.r.t
t
is implied due to growth condition in (5) that bounds

f
i
and

g
i
uniformly with respect to
t
2
[
t
0
;t
f
]
, see [26].
Let
y
1
= (
X
1
;

u
1
)
and
y
2
= (
X
2
;

u
2
)
2
S

U
and
g
i
1
g
i
2
=
g
i
(
t;


X
1
;

u
1
)
g
i
(
t;


X
2
;

u
2
)
.
j

g
ij
(
t;y
1
)

g
ij
(
t;y
2
)
j
1
k
j
Z
D

(

)

j
(

) (
g
i
1
g
i
2
)

d
(28)
j

g
ij
(
t;y
1
)

g
ij
(
t;y
2
)
j
K
g
ij
k
y
1
y
2
k
2
(29)
j

f
ij
(
t;y
1
)

f
ij
(
t;y
2
)
j
K
f
ij
k
y
1
y
2
k
2
(30)
In the inequality (28), using the boundedness of
f
and
g
in (4), the Cauchy-Schwartz inequality, the sub-multiplicative
property of norms, and
k
:
k
2
 k
:
k
F
, we obtain the
inequality (29). The inequality (30) can be proved following
same steps as shown above. Therefore, if the integrals in (27)
are bounded the functions

g
ij
and

f
ij
are locally Lipschitz
continuous.
B. Convex Approximation of the Chance Constraint
The deterministic approximation of the chance constraints
discussed in the Sec. II-A are expressed in terms of the gPC
coefcients that dene a feasible set for the deterministic
optimal control problem with gPC coefcients as decision
variables.
Lemma 2.
The second-order cone constraint given below
(
a
>
M
)
X
+
b
+
r
1


p
X
>
UNN
>
U
>
X

0
(31)
is equivalent to the deterministic approximation of the DRLC
in (10) as
`
! 1
:
, where the matrices
M;U;N
are given
by
M
=

1 0

0

1

(
`
+1)
U
=
2
6
4
a
1
0
0
0
.
.
.
0
0
0
a
d
x
3
7
5
I
(
`
+1)

(
`
+1)
N
=
1
d
x

d
x
H
;
H
=

0
O
O
p
E(
HH
>
)

where
H
=


1
(

)


`
(

)

>
(32)
and
1
is a matrix with entries as
1
.
Proof:
It is sufcient to prove that
(
a
>
M
)

a
>
^
x
and
X
>
UNN
>
U
>
X

a
>

x
a
as
`
!1
. Invoking Lemma 1
and Remark 2, the polynomials of gPC coefcients can be
replaced by mean and variable of the variable
x
.
(
a
>
M
)
X
=

a
1
M a
2
M

a
d
x
M

X
=
a
1
x
10
+
a
2
x
20
+

+
a
d
x
x
dx
0

a
>
^
x
(33)
Equation (33) shows the steps involved to prove
(
a
>
M
)

a
>
^
x
. Let us dene a vector
p
i
=

x
i
0

p
>
i

>
where

p
i
=

x
i
1

x
i`

>
.
U
>
X
=

a
1
p
>
1
a
2
p
>
2

a
d
x
p
>
d
x

>
(34)
NN
>
U
>
X
=

H
a
1
p
1
H
a
2
p
2

H
a
d
x
p
d
x

(35)
X
>
UNN
>
U
>
X
=
d
x
X
i
=1
d
x
X
j
=1
a
i
a
j
p
>
i
H
p
j
(36)
=
d
x
X
i
=1
d
x
X
j
=1
a
i
a
j

p
>
i
E(
HH
>
) 
p
j

a
>

x
a
Using this notation, the matrices in (31) are expanded as
shown in (34), (35), and (36).Therefore the equivalence is
proved by Lemma 1 as
`
!1
.
Lemma 3.
The quadratic inequality
d
x
X
i
=1
`
X
k
=1
a
i
h

k
;
k
i
x
2
ik

c
(37)
expressed in terms of the gPC coefcients is equivalent to the
SDP constraint in (13) as
`
!1
given that
A
is a diagonal
matrix with
i
th
diagonal element as
a
i
.
Proof:
Consider
the
deterministic
approximation
tr(
A

x
)

c
of the quadratic constraint, we can expand it
as follows.
tr(
A

x
)

c

d
x
X
i
=1
a
i
E((
x
i
^
x
i
)(
x
i
^
x
i
))

c

d
x
X
i
=1
`
X
j
=1
a
i
h

j
;
j
i
x
2
ij

c
(38)
The equivalence is proved by directly expanding the trace
and using Remark 2 as shown in (38).
C. Cost Function
Remark
3
.
The expectation cost function dened in (1) with
the functions
L
dened in (2) expressed in terms of the
gPC coefcients is given as the following, using the notation
in (24).
L
d
(
X
(
t
)
;

u
(
t
)) =
X
(
t
)
>


>
Q


X
(
t
) + 
u
(
t
)
>
R

u
(
t
)
L
df
(
X
(
t
f
)) =
X
(
t
f
)
>


>
Q
f


X
(
t
f
)
(39)
The procedure can be extended to any polynomial cost
function in terms of state.
Theorem 1.
The deterministic nonlinear optimal control
problem (DNOC) with convex constraints given below
J

d
=
min
X
(
t
)
;

u
(
t
)
Z
t
f
t
0
L
d
(
X
(
t
)
;

u
(
t
))
dt
+
L
df
(
X
(
t
f
))
s.t.
Equations
f
(26)
;
(31)
;
(37)
g

u
(
t
)
2
U
8
t
2
[
t
0
;t
f
]
X
(
t
0
) =
X
0
X
(
t
f
) =
X
f
(40)
is an approximate surrogate for the stochastic nonlinear
optimal control (SNOC) problem in (1) with following being
true:
(a)
In the case with no chance constraints, the cost
j
J

d
J

j!
0
as
`
!1
(b)
In the case with linear and quadratic chance constraints,
any feasible solution of problem DNOC is a feasible solution
of SNOC as
`
!1
, assuming that a feasible solution exists.
Proof:
Case (a)
: It is sufcient to prove that the cost
function and the dynamics are exact as
`
! 1
. Using the
Kronecker product notation, due to Lemma 1, we have the
following
k
x


X
k
L
2
!
0
as
`
!1
(41)
(41) =
) k
_
x
!


_
X
k
L
2
!
0
as
`
!1
(42)
(41) =
) jL
d
Lj!
0
as
`
!1
jL
df
L
f
j!
0
as
`
!1
(43)
From (42), (43), we can conclude that since the cost function,
the dynamics and the initial and terminal conditions are exact
as
`
!1
, the optimal value
j
J

d
J

j!
0
as
`
!1
.
Case (b):
Consider the sets

LgPC
, and

QgPC
dened
below.

LgPc
=
f
x
2
S
:
x



X
where
X
2
(31)
g
(44)

QgPC
=
f
x
2
S
:
x



X
where
X
2
(37)
g
(45)
Using Lemmas 2 and 3, we have the approximate convex
constraints converge to the deterministic equivalent of the
distributionally robust chance constraint as
`
!1
.
Lemma 2 =
)

LgPc

DRLC
;
as
`
!1
Lemma 3 =
)

QgPC

DC
;
as
`
!1
(46)
Using Remark 1 and Proposition 1 we have the following
Remark 1 =
)

DRLC


LCC
Proposition 1 =
)

DC


CCQ
(47)
f
(46)
;
(47)
g
=
)
(

LgPc


LCC
as
`
!1

QgPC


CCQ
as
`
!1
(48)
Combining (46) and (47), we can conclude that (48) holds
as
`
! 1
. This proves that if a feasible solution exists for
the DNOC in (40) then it is a feasible solution of the SNOC
in (1) as
`
!1
.
D. Sequential Convex Programming
The approximate deterministic optimal control problem
is solved using sequential convex programming (SCP) for
trajectory optimization. In the SCP method, we reformulate
the NDOP as a convex optimization problem by linearizing
the nonlinear dynamics about a given trajectory that forms
a set of linear constraints on the state and control action.
The linear constraints and the integral cost function are
discretized for N time steps between the time horizon
[
t
0
;t
f
]
such that the state and action variables at each time step act
as decision variables of the convex optimization as follows:
_
X
(
i
+1)
=
A
(
i
)
X
(
i
+1)
+
B
(
i
)

u
(
i
+1)
+
Z
(
i
)
(49)
A
(
i
)
=
@
(

f
+
g
)
@X
(
X
(
i
1)
;

u
(
i
1)
)
;
B
(
i
)
=
@
(

f
+
g
)
@

u
(
X
(
i
1)
;

u
(
i
1)
)
Z
(
i
)
=

f
(
X
(
i
1)
;

u
(
i
1)
) + 
g
(
X
(
i
1)
;

u
(
i
1)
)
A
(
i
)
X
(
i
1)
B
(
i
)

u
(
i
1)
(50)
X
(
i
)
[
k
+ 1] =
A
(
i
)
[
k
]
X
(
i
)
[
k
] +
B
(
i
)
[
k
]
u
(
i
)
[
k
] +
Z
(
i
)
[
k
]
(51)
A
(
i
)
[
k
] =
e
A
(
i
)

t
B
(
i
)
[
k
] =
Z

t
0
e
A
(
i
)

B
(
i
)
d
Z
(
i
)
[
k
] =
Z

t
0
e
A
(
i
)

Z
(
i
)
d
(52)
The discretized dynamics (51), reformulated chance con-
straints (31), (37), the initial and terminal conditions are used
as constraints at each iteration.
min
X
(
i
)
;

u
(
i
)
N
X
k
=1
L
d
(
X
(
i
)
[
k
])
;

u
(
i
)
[
k
])
t
+
L
df
(
X
(
i
)
[
N
])
s.t.
Equations
f
(51)
;
(31)
;
(37)
g

u
(
i
)
[
k
]
2
U
8
k
2
[1
;N
]
X
(
i
)
[1] =
X
0
X
(
i
)
[
N
] =
X
f
k
X
i
X
(
i
1)
k
2

i
8
k
(53)
Equation (53) shows the SCP formulation of the DNOC
problem given trajectory of
(
i
1)
th
iteration with the con-
straint set at each time step
k
and iteration
i
. An additional
constraint
k
X
(
i
)
X
(
i
1)
k 
i
called trust region is
used to handle infeasibility of the sub-convex problem, where
>
0
and
2
(0
;
1)
. The trust region shrinks as the number
of iterations increases which acts as a convergence criterion.
The SCP algorithm is known to converge to the KKT point
of the DNOC problem under mild conditions. For detailed
analysis on convergence, see [15], [16].
IV. E
XAMPLE
: M
OTION
P
LANNING
W
ITH
O
BSTACLES
In this section, we discuss the implementation of the gPC-
SCP approach on the following unicycle dynamic model:
2
4

x

y


3
5
=
2
4
cos

0
sin

0
0
1
3
5

(1 +

)
u
1
(1 +

)
u
2

(54)
where
(
x;y
)
is the position of the centre of mass with respect
to an inertial frame and

is the heading angle.
0


u
1

1
j

u
2
j
10
(55)
The dynamics is underactuated with a nonlinear control input
matrix and has a Gaussian multiplicative uncertainty term
entering at the input

 N
(0
;


)
with zero mean and
known variance.
x
(
t
0
) = [0; 0
:
4; 0; 0; 0];
E(
x
(
t
f
)) = [10; 0
:
4; 0; 0; 0]
(56)
The initial and terminal conditions are given in (56). In the
scenario considered here, the robotic car needs to compute
a sub-optimal dynamically feasible, safe trajectory from
initial state to the expected terminal state in (56), while
avoiding collision with the obstacle and the wall with
specied probability. We have the knowledge of the initial
state with probability 1 and only know the expected state
information at the terminal state. The environment map and
initial conditions were carefully chosen such that a feasible
trajectory exists between the wall and the obstacle for low
variance simulations. For the situation with high variance of

the gPC-SCP method computes a feasible solution that lies
on the other side of the obstacle.
gPC Form of the Dynamics:
The nonlinear functions
cos
and
sin
are approximated by the following technique, often
used for Galerkin projection of harmonic functions.
cos


cos

0
+
P
`
j
=1

j

j


cos

0
cos
P
`
j
=1

j

j

sin

0
sin
P
`
j
=1

j

j

(57)
In the above equation, we use Taylor series approximation
for the
sin
and
cos
of the higher order terms
P
`
j
=1

j

j
for
computing the Galerkin projection. We use the rst four Her-
mite polynomials for the gPC expansion. The computation
of the gPC form and the Galerkin projection was done using
Matlab. For the sake of brevity we leave out the gPC form
of the dynamics from this paper. The Gaussian uncertainty
in the system can be expressed in terms of the Hermite
polynomials, where

2N
(0
;
1)
.

(

) =
^

0
(

) +
p



1
(

)
(58)
The controls in this particular example are assumed to be
deterministic

u
i
= 
u
i

0
(

)
as

0
= 1
.
Chance Constraints:
The joint chance constraint for
collision checking with the wall and the obstacle is give in
the following
Pr(
x
2
(
t
)
<
2;
k
p
(
t
)
p
obs
k
2

R
safe
)

1

(59)
The position of the robot and obstacle are
p
(
t
) =
[
x
1
(
t
)
;x
2
(
t
)]
, the position
p
obs
= [5
;
0]
respectively. The
radius of the obstacle is given by
R
safe
= 1
:
2
. The joint
chance constraints are split into individual chance constraints
using Bonferroni's inequality with

1
+

2
=

given value
of

. We use naive risk allocation such that

1
=

2
= 0
:
005
given that

= 0
:
01
.
Pr(
x
2
(
t
)
>
2)


1
; Pr(
k
p
(
t
)
p
obs
k
2

R
safe
)


2
(60)
The individual chance constraints are given in (60). In
the SCP approach, for each iteration
i
the second con-
straint for collision avoidance,
k
p
(
t
)
p
obs
k
2

R
safe
is linearized about position trajectory

p
(
i
1)
(
t
)
using the
technique in [15], [16], where
i
is the iteration of the SCP.
Pr(
x
2
(
t
)

2)


1
Pr(
a
>
p
(
t
)

b
)


2
(61)
^
x
i
2
+
r
1

1

1
q

i
x
2

2
a
>
^
p
(
i
)
(
t
) +
r
1

2

2
q
a
>

i
p
(
t
)
a

b
(62)
Equation (61) shows the linear chance constraints for
collision checking with this approach and (62) shows
the
distributionally
robust
convex
approximation
in
terms of the mean and variance of the variables
x
2
(
t
)
and
p
(
t
)
at each time
t
. The second-order constraints
are expressed in terms of the gPC coefcients for the
gPC-SCP method, where
a
=
p
obs

p
(
i
1)
(
t
)
and
b
=
x
obs

p
(
i
1)
(
t
)

>
p
obs
R
safe
k

p
(
i
1)
(
t
)
x
obs
k
2
.
SCP Implementation and Simulation Details:
For the
SCP implementation, we choose a time horizon of 10 sec-
onds for robot to move from initial point to the terminal point
with
N
steps
= 50
time steps. The continuous ODE dynamics
obtained from the gPC transformation was linearized and
then discretized and imposed as constraints for all the time
steps. The constraints in (62) and the control at each time
step
t
are already convex, that are directly used in the SCP
algorithm.
Results:
The trajectory optimization was run with an initial
trajectory as the line joining initial to the terminal point for
two situations


=
f
0
:
01
2
;
0
:
05
2
g
. We run the algorithm
for following cost functions 1) control effort i.e.,
R
=
I
and
Q
f
=
Q
= 0
as the cost function for


=
f
0
:
01
2
;
0
:
05
2
g
shown in Fig. 1 as trajectories T1 and T2 respectively, 2) both
control and state penalized in the cost function i.e.,
R
=
I
and
Q
=
I
for


= 0
:
05
2
shown as trajectory T3 in Fig. 1.
0
1
2
3
4
5
6
7
8
9
10
X
-4
-2
0
2
4
6
Y
Wall
Obstacle
T1 -
= 0.01
2
T2 -
= 0.05
2
T3 -
= 0.05
2
Fig. 1: Plot showing mean of the state and uncertainty ellipse
corresponding to
3

condence for

= 0.05.
Note that when we minimize both state and control cost
we can nd a safe trajectory T3 between the wall and
the obstacle. The trajectories T1 and T2 were compared
in Fig. 2 with the open-loop simulations of the original
dynamics in (54) using the policy computed by gPC-SCP
approach on the approximated dynamics. Using the open-
loop deterministic policy the robot reaches the terminal state
with out collision.
Limitations:
The Galerkin scheme used in the paper is
computationally expensive, to use the gPC-SCP technique
for higher-dimensional problems, numerical methods like
stochastic collocation need to be implemented for Galerkin
projection. The problem formulation would be same with the
stochastic collocation method. The SCP problem requires a
good initialization for faster convergence. For longer time