arXiv:1612.05594v1 [] 16 Dec 2016

Importance sampling-based approximate
optimal planning and control
Jie Fu1

1

February 18, 2018

11

Jie Fu is with Robotics Engineering Program, Department of Electrical and
Computer Engineering, Worcester Polytechnic Institute, 01609, Worcester, MA, US.
jfu2@wpi.edu

Abstract
In this paper, we propose a sampling-based planning and optimal control method
of nonlinear systems under non-differentiable constraints. Motivated by developing scalable planning algorithms, we consider the optimal motion plan to be
a feedback controller that can be approximated by a weighted sum of given
bases. Given this approximate optimal control formulation, our main contribution is to introduce importance sampling, specifically, model-reference adaptive
search algorithm, to iteratively compute the optimal weight parameters, i.e.,
the weights corresponding to the optimal policy function approximation given
chosen bases. The key idea is to perform the search by iteratively estimating
a parametrized distribution which converges to a Dirac’s Delta that infinitely
peaks on the global optimal weights. Then, using this direct policy search, we
incorporated trajectory-based verification to ensure that, for a class of nonlinear systems, the obtained policy is not only optimal but robust to bounded
disturbances. The correctness and efficiency of the methods are demonstrated
through numerical experiments including linear systems with a nonlinear cost
function and motion planning for a Dubins car.

0.1

Introduction

This paper presents an importance sampling based approximate optimal planning and control algorithm. Optimal motion planning in deterministic and
continuous systems is computationally NP-complete [1] except for linear time
invariant systems. For nonlinear systems, there is a vast literature on approximate solutions and algorithms. In optimal planning, the common approximation
scheme is discretization-based. By discretizing the state and input spaces, optimal planning is performed by solving the shortest path problem in the discrete
transition systems obtained from abstracting the continuous dynamics, using
heuristic-based search or dynamic programming. Comparing to discretizationbased methods, sampling-based graph search, includes Probabilistic RoadMap
(PRM) [2], RRT [3], RRT* [4], are more applicable for high-dimensional systems. While RRT has no guarantee on the optimality of the path [4], RRT*
compute an optimal path asymptotically provided the cost functional is Lipschitz continuous. However, such Lipschitz conditions may not be satisfied for
some cost functions under specific performance consideration.
The key idea in the proposed sampling-based planning method builds on a
unification of importance sampling and approximate optimal control [5, 6]. In
approximate optimal control, the objective is to approximate both the value
function, i.e., optimal cost-to-go, and the optimal feedback policy function by
weighted sums of known basis functions. As a consequence, the search space is
changed from infinite trajectory space or policy space to a continuous space of
weight vectors, given that each weight vector corresponds to a unique feedback
controller.
Instead of solving the approximate optimal control through training actor
and critic neural networks (NNs) using trajectory data [7, 8], we propose a
sampling-based method for sampling the weight vectors for a policy function
approximation and searching for the optimal one. This method employs Model
Reference Adaptive Search (MRAS) [9], a probabilistic complete global optimization algorithm, for searching the optimal weight vector that parametrizes
the approximate optimal feedback policy. The fundamental idea is to treat the
weight vector as a random variable over a parameterized distribution and the
optimal weight vector corresponds to a Dirac’s Delta function which is the target distribution. The MRAS algorithm iteratively estimates the parameter that
possesses the minimum Kullback-Leibler divergence with respect to an intermediate reference model, which assigns a higher probability mass on a set of
weights of controllers with improved performance over the previous iteration.
At the meantime, a set of sampled weight vectors are generated using the parameterized distribution and the performance of their corresponding policies are
evaluated via simulation-based policy evaluation. Under mild conditions, the
parameterized distribution converges, with probability one, to the target distribution that concentrates on the optimal weight vector with respect to given
basis functions.
MRAS resembles another adaptive search algorithm called cross-entropy(CE)
method and provides faster and stronger convergence guarantee for being less

sensitive to input parameters [9, 10]. Previously, CE algorithm has been introduced for motion planning [11, 12] based on sampling in the trajectory space.
The center idea is to construct a probability distribution over the set of feasible paths and to perform the search for an optimal trajectory using CE. The
parameters to be estimated is either a sequence of motion primitives or a set of
via-points for interpolation-based trajectory planning. Differ to these methods,
ours is the first to integrate importance sampling to estimate parameterization
of the optimal policy function approximation for continuous nonlinear systems.
Since the algorithm performs direct policy search, we are able to enforce robustness and stability conditions to ensure the computed policy is both robust and
approximate optimal, provided these conditions can be evaluated efficiently.
To conclude, the contributions of this paper are the following: First, we introduce a planning algorithm by a novel integration of model reference adaptive
search and approximate optimal control. Second, based on contraction theory, we introduce a modification to the planning method to directly generate
stabilizing and robust feedback controllers in the presence of bounded disturbances. Last but not the least, through illustrative examples, we demonstrate
the effectiveness and efficiency of the proposed methods and share our view on
interesting future research along this direction.

0.2

Problem formulation

Notation: The inner product between two vectors w, v ∈ Rn is denoted w⊺ v
or hw, vi. Given √
a positive semi-definite matrix P , the P -norm of a vector is
denoted kxkP = x⊺ P x. We denote kxk for P being the identity matrix. I{E}
is the indicator function, i.e., IE = 1 if event E holds, otherwise 0. For a real
α ∈ R, ⌈α⌉ is the smallest integer that is greater than α.

0.2.1

System model

We consider continuous-time nonlinear systems of the form
Σ : ẋ(t) = f (x(t), u(t)),
x(t) ∈ X, u(t) ∈ U.

(1)

where x ∈ X is the state, u ∈ U is the control input, x0 ∈ X is the initial state,
and f (x, u) is a vector field. We assume that X and U are compact. A feedback
controller u : X → U takes the current state and outputs a control input.
The objective is to find a feedback controller u∗ that minimizes a finitehorizon cost function for a nonlinear system
Z T
min J(x0 , u) =
ℓ(x(t), u(t))dt + g(x(T ), u(T ))
u
0
(2)
subject to: ẋ(t) = f (x(t), u(t)),
x(t) ∈ X, u(t) ∈ U, x(0) = x0 .

where T is the stopping time, ℓ : X × U → R+ defines the running cost when
the state trajectory traverses through x and the control input u is applied and
g : X → R+ defines the terminal cost. As an example, a running cost function
can be a quadratic cost ℓ(x, u) = kxkR + kukQ for some positive semi-definite
matrices Q and R, and a terminal cost can be g(x, u) = kx − xf kR where xf is
a goal state.
We denote the set of feedback policies to be Π. For infinite horizon optimal
control, the optimal policy is independent of time and a feedback controller
suffices to be a minimizing argument of (2) (see Ref. [13]). For finite-horizon
optimal control, the optimal policy is time-dependent. However, for simplicity,
in this paper, we only consider time-invariant feedback policies and assume the
time horizon T is of sufficient length to ignore the time constraints.

0.2.2

Preliminary: Model reference adaptive search

MRAS algorithm, introduced in [9], aims to solve the following problem:
z ∗ ∈ arg max H(z),
z∈Z

z ∈ Rn

where Z is the solution space and H : Rn → R is a deterministic function
that is bounded from below. It is assumed that the optimization problem has a
unique solution, i.e., z ∗ ∈ Z and for all z 6= z ∗ , H(z) < H(z ∗ ).
The following regularity conditions need to be met for the applicability of
MRAS.
Assumption 1. For any given constant ξ < H(z ∗ ), the set {z | H(z) ≥ ξ} ∩ Z
has a strictly positive Lebesgue or discrete measure.
This condition ensures that any neighborhood of the optimal solution z ∗ will
have a positive probability to be sampled.
Assumption 2. For any constant δ > 0, supz∈Aδ H(z) < H(z ∗ ), where Aδ :=
{z | kz − z ∗ k ≥ δ} ∩ X, and we define the supremum over the empty set to be
−∞.
• Selecte a sequence of reference distributions {gk (·)} with desired convergence properties. Specifically, the sequence {gk (·)} will converge to a
distribution that concentrates only on the optimal solution.
• Selecte a parametrized family of distribution f (·, θ) over X with parameter
θ ∈ Θ.
• Optimize the parameters {θk } iteratively by minimizing the following KL
distance between f (·, θk ) and gk (·).
Z
gk (z)
ln
d(gk , f (·, θ)) :=
gk (z)ν(dz).
f
(z, θ)
Z

where ν(·) is the Lebesgue measure defined over Z. The sample distributions {f (·, θk )} can be viewed as compact approximations of the reference distributions and will converge to an approximate optimal solution
as {gk (·)} converges provided certain properties of {gk (·)} is retained in
f (·, θk ).
Note that the reference distribution {gk (·)} is unknown beforehand as the optimal solution is unknown. Thus, the MRAS algorithm employs the estimation
of distribution algorithms [14] to estimate a reference distribution that guides
the search. To make the paper self-contained, we will cover details of MRAS in
the development of the planning algorithm.

0.3

Approximate optimal motion planning using MRAS

In this section, we present an algorithm that uses MRAS in a distinguished way
for approximate optimal feedback motion planning.

0.3.1

Policy function approximation

The policy function approximation ū : X → U is a weighted sum of basis
functions,
N
X
wi φi (x)
ū(x) =
i=1

where φi : X → R, i = 1, . . . , N are basis functions, and the coefficients wi
are the weight parameters, i = 1, . . . , N . An example of basis function can be
polynomial basis φ = [1, x, x2 , x3 , . . . , xN ] for one-dimensional system. A commonly used class of basis functions is Radial basis function (RBF). It can be
constructed by determining a set of centers ci , . . . , cN ∈ X, and then construct2
ik
ing RBF basis functions φi = exp(− kx−c
2σ2 ), for each center ci , where σ is a
pre-defined parameter.
In vector form, a policy function approximation is represented by ū = hw, φi
⊺
where vector φ = [φ1 , . . . φN ] and w = [w1 , . . . , wN ]⊺ . We let the domain of
weight vector be W and denote it by Πφ = {hw, φi | w ∈ W, hw, φi ∈ Π} the set
of all policies that can be generated by linear combinations of pre-defined basis
functions. In the following context, unless specifically mentioned, the vector of
basis functions is φ.
Clearly, for any weight vector w, J(x0 , hw, φi) ≥ minu∈Π J(x0 , u). Thus, we
aim to solve minw J(x0 , hw, φi) so as to minimize the error in the optimal cost
introduced by policy function approximation.
Definition 1 (Approximate optimal feedback policy). Given a basis vector φ,
a weight vector w∗ with respect to φ is optimal if and only if hw∗ , φi ∈ Πφ and

for all w ∈ W such that hw, φi ∈ Πφ ,
J(x0 , hw∗ , φi) ≤ J(x0 , hw, φi).
The approximate optimal feedback policy is ū∗ = hw∗ , φi.
By requiring J(x0 , hw∗ , φi) ≤ J(x0 , hw, φi), it can be shown that the optimal
weight vector w∗ minimizes the difference between the optimal cost achievable
with policies in Πφ and the cost under the global optimal policy.
For clarity in notation, we denote J(x0 , hw, φi) by J(x0 ; w) as φ is a fixed
basis vector throughout the development of the proposed method.
Clearly, if the actual optimal policy u∗ can be represented by a linear combination of selected basis functions, then we obtain the optimal policy by computing the optimal weight vector, i.e., u∗ = hw∗ , φi.
Remark: Here, we assume a feedback policy can be represented by hw, φi for
some weight vector w ∈ W . In cases when the basis functions are continuous,
a feedback policy must be a continuous function of the state. However, this
requirement is hard to satisfy for many physical systems due to, for example,
input saturation. In cases when a feasible controller is discontinuous, we can
still use a continuous function to approximate, and then project the continuous
function to the set Π of applicable controllers.
Using function approximation, we aim to solve the optimal feedback planning
problem in (2) approximately by finding the optimal weight vector with respect
to a pre-defined basis vector. The main algorithm is presented next.

0.3.2

Integrating MRAS in approximate optimal planning

In this section, we present an adaptive search-based algorithm to compute the
approximate optimal feedback policy. The algorithm is “near” anytime, meaning that it returns a feasible solution after a small number of samples. If more
time is permitted, it will quickly converge to the globally optimal solution that
corresponds to the approximate optimal feedback policy. The algorithm is probabilistic complete under regularity conditions of MRAS.
We start by viewing the weight vector as a random variable w governed by a
multivariate Gaussian distribution with a compact support W . The distribution
is parameterized by parameter θ = (µ, Σ), where µ is a N -dimensional mean
vector and Σ is the N by N covariance matrix. Recall N is the number of basis
functions.
The optimal weight vector w∗ can be represented as a target distribution
pgoal as a Dirac’s Delta, i.e., pgoal (w∗ ) = ∞ and pgoal (w) = 0 for w 6= w∗ .
Dirac’s Delta is a special case of multivariate Gaussian distribution with zero
in the limit case of vanishing covariance. Thus, it is ensured that the target
distribution can be arbitrarily closely approximated by multivariate Gaussian
distribution by a realization of parameter θ.

Recall that the probability density of a multivariate Gaussian distribution
is defined by
1
1
p(w; θ) = p
exp(− (x − µ)⊺ Σ−1 (x − µ)),
N
2
(2π) |Σ|

θ = (µ, Σ), ∀w ∈ W,

where N is the dimension of weight vector w ∈ W and |Σ| is the determinant
of Σ.
Now, we are ready to represent the main algorithm, called Sampling-based
Approximate-Optimal Planning (SAOP), which includes the following steps.
1) Initialization: The initial distribution is selected to be p(·, θ0 ), for θ0 =
(µ0 , Σ0 ) ∈ Θ which can generate a set of sample to achieve a good coverage
of the sample space W . For example, µ0 = 0 ∈ RN and Σ0 = I ∈ RN which
is an identity matrix. The following parameters are used in this algorithm:
ρ ∈ (0, 1] for specifying the quantile, the improvement parameter ε ∈ R+ ,
a sample increment percentage α, an initial sample size N1 , a smoothing
coefficient λ ∈ (0, 1]. Let k = 1.
2) Sampling-based policy evaluation: At each iteration k, generate a set of
Nk samples Wk ⊆ W from the current distribution p(·, θk ). For each w ∈ Wk ,
using simulation we evaluate the cost J(x0 ; w) from the initial state x0 and
the feedback policy u(x) = hw, φ(x)i with system model in (1). The cost
J(x0 ; w) is determined because the system is deterministic and has a unique
solution.
3) Policy improvement with Elite samples: Next, the set {J(x0 ; w) | w ∈
Wk } is ordered from largest (worst) to smallest (best) among given samples:
Jk,(0) ≥ . . . ≥ Jk,(Nk )
We denote κ to be the estimated (1 − ρ)-quantile of cost J(·; w), i.e., κ =
Jk,⌈(1−ρ)Nk ⌉ .
The following cases are distinguished.
• If k = 1, we introduce a threshold γ = κ.

• If k 6= 1, the following cases are further distinguished:

– κ ≤ γ − ε, i.e., the estimated (1 − ρ)-quantile of cost has been
reduced by the amount of ε from the last iteration, then let γ = κ.
Let Nk+1 = Nk and continue to step 4).
– Otherwise κ > γ − ε, we find the largest ρ′ , if it exists, such that
the estimated (1 − ρ′ )-quantile of cost κ′ = Jk,⌈(1−ρ′ )Nk ⌉ satisfies
κ′ ≤ γ − ε. Then let γ = κ′ and also let ρ = ρ′ . Continue to step
4). However, if no such ρ′ exists, then there is no update in the
threshold γ but the sample size is increased to Nk+1 = ⌈(1+α)Nk ⌉.
Let θk+1 = θk , k = k + 1, and continue to step 2).

• Parameter(Policy) update: We update parameters θk+1 for iteration k + 1. First, we define a set E = {w | J(x0 ; w) ≤ γ, w ∈ Wk , j =
1, . . . , k} of elite samples. Note that the parameter update in θ is to
ensure a higher probability for elite samples. To achieve that, for each
elite sample w ∈ E, we associated a weight such that a higher weight
is associated with a weight vector with a lower cost and a lower probability in the current distribution. The next parameter θk+1 is selected
to maximize the weighted sum of probabilities of elite samples. To this
end, we update the parameter as follows.
∗
θk+1

= arg max Eθk
θ∈Θ




S(J(x0 , w))k
IJ(x0 ,w)≤γ ln p(w, θ)
p(w, θk )

where Eθ (ν) is the expected value of a random variable ν given distribution p(·, θ), S : R → R+ is a strictly decreasing and positive function
1
. S(J(x0 ; w))k /p(w, θk ) is the weight for parameter w.
Assumption 3. The optimal parameter θ∗ is the interior point of Θ for all k.
Lemma 1 (based on Theorem 1 [9]). Assuming 1,2, and 3 and the compactness
of W , with probability one,
lim µk = w∗ , and lim Σk = 0N ×N .

k→∞

k→∞

where w∗ is the optimal weight vector and 0N ×N is an N -by-N zero matrix.
Note that since Σk converges in the limit a zero matrix, the stopping criterion
is justified.
Building on the convergence result of MRAS, the proposed sampling-based
planner ensures a convergence to a Dirac Delta function concentrating on the optimum. In practice, the parameter update is performed using the expectation—
maximization (EM) algorithm.
EM-based parameter update/policy improvement Since our choice
∗
of probability distribution is the multivariate Gaussian, the parameter θk+1
=
(µ, Σ) is computed as follows
µ=

Eθk [S(J(x0 , w))k /p(w, θk )]Iw∈E w
Eθk [S(J(x0 , w))k /p(w, θk )]Iw∈E
P
k
w∈Wk [S(J(x0 , w)) /p(w, θk )]Iw∈E w
, (3)
≈ P
k
w∈Wk [S(J(x0 , w)) /p(w, θk )]Iw∈E

1 Possible

choices can be S(x) = exp(−x) or S(x) =

1
x

if x is strictly positive.

and
Σ=

Eθk [S(J(x0 , w))k /p(w, θk )]Iw∈E (w − µ)(w − µ)⊺
Eθk [S(J(x0 , w))k /p(w, θk )]Iw∈E
P
[S(J(x0 , w))k /p(w, θk )]Iw∈E (w − µ)(w − µ)⊺
≈ w∈Wk P
, (4)
k
w∈Wk [S(J(x0 , w)) /p(w, θk )]Iw∈E

P
where we approximate Eθk (h(w)) with its estimate N1k w∈Wk h(w) for w ∼
p(·, θk ) and the fraction N1k was canceled as the term is shared by the numerator
and the denominator.
Smoothing: Due to limited sample size, a greedy maximization for parameter update can be premature if too few samples are used. To ensure the
convergence to the global optimal solution, a smoothing update is needed. To
this end, we select the parameter for the next iteration to be
∗
θk+1 ← λθk + (1 − λ)θk+1
.

where λ ∈ [0, 1) is the smoothing parameter.
Let k = k + 1. We check if the iteration can be terminated based on a given
stopping criterion. If the stopping criterion is met, then we output the latest
θk . Otherwise, we continue to update of θ by moving to step 2).
Stopping criterion Given the probability distribution will converge to a
degenerated one that concentrates on the optimal weight vector. We stop the iteration if the covariance matrix Σk becomes near-singular given the convergence
condition in Lemma 1.
To conclude, the proposed algorithm using MRAS is probabilistic complete
and converges to the global optimal solution. If the assumptions are not met,
the algorithm converges to a local optimum.

0.3.3

Robust control using trajectory verification in sampling

Being able to directly search within continuous control policy space, one major
advantage is that one can enforce stability condition such that the search is
restricted to stable and robust policy space. In this subsection, we consider
contraction theory to compute conditions that need to be satisfied by weight
vectors to ensure stability and robustness under bounded disturbances.
Definition 2. [15] Given the system equation for the closed-loop system ẋ =
f (x, t), a region of the state space is called a contraction region if the Jacobian
∂f
∂x is uniformly negative definite in that region, that is,


∂f ⊺
1 ∂f
 −βM.
M + Ṁ + M
∃β > 0, ∀x, ∀t > 0,
2 ∂x
∂x
where M (t) is a positive definite matrix for all t ≥ 0.

Theorem 1. [15] Given the system model ẋ = f (x, t), any trajectory, which
starts in a ball of constant radius with respect to the matrix M , centered about a
given trajectory and contained at all times in a contraction region with respect to
the matrix M , remains in that ball and converges exponentially to this trajectory.
Furthermore, global exponential convergence to the given trajectory is guaranteed
if the whole state space is a contraction region.
Theorem 1 provides a necessary and sufficient condition for exponential convergence of an autonomous system. Under bounded disturbances, the key idea
is to incorporate a contraction analysis in the planning algorithm such that it
searches for a weight vector w that is not only optimal in the nominal system
but also ensures that the closed-loop actual system under the controller u = w⊺ φ
has contraction dynamics within a tube around the nominal trajectory. Using a
similar proof in [16], we can show that for systems with contracting dynamics,
the actual trajectory under bounded disturbances will be ultimately uniformly
bounded along the nominal trajectory.
Lemma 2. Consider a closed-loop system ẋ = f (x) + ω(t) where ω(t) is a
disturbance with maxt kω(t)k ≤ ρmax , let a state trajectory x(t) be in the contraction region Xℓ at all time t ≥ t0 , then for any time t ≥ t0 , the deviation
between x(t) and the nominal trajectory x̄(t), whose dynamic model is given by
x̄˙ = f (x̄), satisfies
kx(t) − x̄(t)k2M ≤

2ℓρmax
(1 − eβt ))
β

In other words, the error is uniformly ultimately bounded with the ultimate bound
2ℓρmax
.
β
Proof. Let’s pick the Lyapunov function
V = (x − x̄)T M (x − x̄),
whose time derivative is
V̇ = (x − x̄)T M (f (x) + ω − f (x̄))
+ (f (x) + ω − f (x̄))T M (x − x̄)

= (x − x̄)T M (f (x) − f (x̄)) + 2(x − x̄)T M ω
(M is symmetric)
∂f T
∂f
|x̃ )(x − x̄)
|x̃ M + M
∂x
∂x
+ 2(x − x̄)T M ω,
= (x − x̄)T (

where the following property is used: f (x) − f (x̄) =
x̃ ∈ [x̄, x] if x̄ 4 x or x̃ ∈ [x, x̄] otherwise.

∂f T
∂x

|x̃ (x − x̄) for some

Since the trajectories stays within the contraction region, the following conT
∂f
dition holds ∂f
∂x |x̃ M + M ∂x |x̃ ≤ −2βM , and we have
V̇ ≤ −(x − x̄)T βM (x − x̄) + 2(x − x̄)T M ω.
Meanwhile, kx(t) − x̄(t)kM ≤ ℓ as the trajectory x(t) stays within the region of
contraction, and also
q
p
M (x − x̄) ≤ (x − x̄)T M M (x − x̄) = kx − x̄kM
we conclude that as ω ≤ ρmax ,
V̇ ≤ −(x − x̄)T (βM )(x − x̄) + 2ω

p
kx − x̄kM

≤ −(x − x̄)T (βM )(x − x̄) + 2ρmax ℓ,

Since V̇ ≤ −βV + 2ρmax ℓ and under the condition that x(0) = x̄(0), we
−βt
), and therefore
obtain V (t) ≤ 2ℓ
β ρmax (1 − e
kx − x̄k2M =

2ℓ
ρmax (1 − e−βt )
β

Thus, to search for the optimal and robust policies, we modify the algorithm
by introducing the following step.
Contraction verification step: Suppose the closed-loop system is subject to
bounded disturbances, the objective is to ensure the trajectory is contracting
within the time-varying tube {x | kx − x̄k ≤ ℓ}, for all t, where x̄ is the nominal
state trajectory. The following condition translates the contraction condition
into verifiable condition for a closed-loop system: Choose positive constants β, a
positive definite symmetric and constant matrix M = [mij ]i=1,...,n,j=1,...,n , and
verify whether, at each time step along the nominal trajectory x̄(t) in the closedloop system under control u(t) = w⊺ φ(x(t)), the following condition holds.
max

x:kx−x̄kM ≤ℓ

gij (x) ≤ −βmij ,

∀i = 1, . . . , n, ∀j = 1, . . . , n
⊺

(5)

∂f
where gij is the (i, j)th component in the matrix ∂f
∂x M + M ∂x . We verify this
condition numerically at discrete time steps instead of continous time. Further,
if the function gij (x) is semi-continuous, according to the Extreme Value Theorem, this condition can be verified by evaluating gij (x) at all critical points
dg (x)
where ij
= 0 and the boundary of the set {x | kx − x̄kM ≤ ℓ}.
dx
The modification to the planning algorithm is made in Step 3), if a controller
u = hw, φi of elite sample w does not meet the condition, then w is rejected from
the set of elite samples. Alternatively, one can do so implicitly by associating
w with a very large cost. However, since the condition is sufficient but not
necessary as we have the matrix M , constant β and ℓ pre-fixed and M is chosen

to be a constant matrix, the obtained robust controller may not necessary be
optimal among all robust controllers in Πφ . A topic for future work is to extend
joint planning and control policies with respect to adaptive bound β, ℓ, and a
uniformly positive definite and time-varying matrix M (x, t).

0.4

Examples

In this section, we use two examples to illustrate the correctness and efficiency
of the proposed method. The simulation experiments are implemented in MATLAB on a desktop with Intel Xeon E5 CPU and 16 GB of RAM.

0.4.1

Feedback gain search for linear systems

To illustrate the correctness and sampling efficiency in the planning algorithm,
we consider an optimal control of linear time invariant (LTI) systems with nonquadratic cost. For this class of optimal control problems, since there is no
admissible heuristic, one cannot use any planning algorithm facilitated by the
usage of a heuristic function. Moreover, the optimal controller is nonlinear given
the non-quadratic cost.
Consider a LTI system
ẋ = Ax + Bu


 
−1 1
0
where A =
and B =
with x ∈ X = R2 and u ∈ R1 . The initial
0 0
1
state is x0 = [5, 5].
RT
The cost functional is J(x0 , u) = 0 (kxk2 + kuk2 + 0.5kxk4 + 0.8kxk6 )dt +
kx(T )k2 .
For a non-quadratic cost functional, the optimal controller is no longer linear
and cannot be computed by LQR unless the running cost can be written in
the sum-of-square form. Thus, we consider an approximate feedback controller
with basis vector φ = [x1 , x2 , x21 , x22 , x31 , x32 ]⊺ . Suppose the magnitude of external
disturbance is bounded by ρmax = 0.5.
The following parameters are used in stability verification: β = 2, at any
time t, for all x such that kx(t)− x̄(t)k ≤ ℓ, the controller ensures kx(t)− x̄(t)k ≤
2ℓρmax
(1−eβt) because 2 ℓρmax
= 0.5ℓ ≤ ℓ. With this choice for stability analysis,
β
β
the constraint


∂f ⊺
∂f
−2 3w(5)x21 + 2w(3)x1 + w(1) + 1
+
=
Sym.
6w(6)x22 + 4w(4)x2 + 2w(2)
∂x ∂x


−2 0
≤
0 −2
In this case, if we select w(6), w(4), w(5), w(3) nonpositive, w(1) ≤ −1 and
w(2) ≤ −1, then closed-loop system, which is a nonlinear polynomial system,
will become globally contracting.

0.5
µ1

Cost function

4600

4400

4200

4000

3800
102

103

µ2

µ3

µ4

µ5

µ6

6

0

nominal x1
nominal x2
actual x1
actual x2

5
-0.5

4

-1

3

-1.5

2

y

Mean of multivariant Gaussian

4800

1
-2
0
-2.5
-3

-1
0

500

1000

1500

2000

2500

3000

3500

-2

0

Number of samples

Number of samples

(a)

1

2

3

4

5

x

(b)

(c)

Figure 1: Convergence of the SAOP algorithm on the LTI system with a nonquadratic cost functional. (a) The mean of multivariate Gaussian as weight
vector over iterations. (b) The state trajectory of the closed-loop system under bounded disturbance ρmax = 0.5 under feedback controller computed with
SAOP.
Figures 1a and 1b show the convergence result with SAOP in one simulation
in terms of cost and the mean of the multivariant Gaussian over iterations.
The following parameters are used: Initial sample size N1 = 50, improvement
parameter ǫ = 0.1, quantile percentage ρ = 0.1, smoothing parameter λ = 0.5,
sample increment parameter α = 0.1.
The algorithm converges after 38 iterations with 3301 samples to the mean
w̄∗ = [−1.0629 − 2.7517 0 − 1.7939 − 0.0987 − 2.1474]⊺ and the covariance
matrix with a norm 3.3401e−4. Each iteration took less than 10 seconds. The
approximate optimal cost under feedback controller u = hw̄∗ , φi is 3863.3. Figure 1c shows the state trajectory for the closed-loop system with bounded disturbances. With 25 independent runs of SAOP, the mean of J(x0 ; w̄i∗ ), i = 1, . . . , 25
is 3903.3 and the standard deviation is 104.1683, 2.6% of the approximate optimal cost.
Note, if we only use linear feedback u = Kx, the optimal cost is 1.0943e4,
which is about three times the optimal cost that can be achieved with a nonlinear
controller.

0.4.2

Example: approximate optimal planning of a Dubins
car

Consider a Dubins car dynamics
ẋ = u cos θ,

ẏ = u sin θ

θ̇ = v

where ~x = (x, y, θ) ∈ R2 ×S1 being the state (coordinates and turning angle with
respect to x-axis) and u and v are control variables including linear and angular
velocities. The system is kinematically constrained by its positive minimum
turning radius r which implies the following bound |v| ≤ r1 . Without loss of
generality, we assume |v| ≤ 5 and |u| ≤ 10 are the input constraints. The control
objective is to reach the goal xf = 20, yf = 20 while avoiding static obstacles.
RT
The cost function J = 0 ℓ(x, u)dt + g(x, u) where T = 100, the running cost
is ℓ(x, u) = 0.1 × (kxk + kuk), and the terminal cost is g(x(T ), u(T )) = 1000 ×

25

3

20

2.5

2

Cost

y

15

10

1.5

5

1

0

0.5

-5
-5

×104

0
0

5

10

x

(a)

15

20

25

0

500

1000

1500

2000

2500

Number of samples

(b)

Figure 2: Convergence of the the planning algorithm algorithm on the Dubins
car. (a) The planned trajectory under feedback policy hµ, φi computed using the
mean of multivariate Gaussian over iterations (from the lightest to the darkest).
(b) The convergence of the covariance matrix. (c) The total cost evaluated at
the mean of the multivariate Gaussian over iterations.
k(x(T ), y(T )) − (xf , yf )k. The initial state is ~x0 = 0. In simulation, we consider
the robot reaches the target if k(x, y)′ −(xf , yf )k ≤ ε for ε ∈ [0, 1]. In simulation,
ε = 0.5.
We select RBF as basis functions and define φrbf = [φ1 , . . . , φN ]⊺ for N
center points. In the experiment, the center points are includes 1) uniform grids
in x−y coordinates with step sizes δx = 5, δy = 5; and 2) vertices of the obstacle.
We also include linear basis functions φlinear = [(x − xf ), (y − yf ), θ]. The basis
vector is φ = [φ⊺rbf , φ⊺linear ]⊺ . We consider a bounded domain −5 ≤ x ≤ 30 and
−5 ≤ y ≤ 30 and θ ∈ [0, 2π] and thus the total number of basis functions is 80.
The control input ~u = [u, v]⊺ where u = wu⊺ φ and v = wv⊺ φ. The total number
of weight parameters is twice the number of bases and in this case 160.
The following parameters are used: Initial sample size N1 = 100, improvement parameter ǫ = 0.1, smoothing parameter λ = 0.5, sample increment percentage α = 0.1, and ρ = 0.1. In Fig. 2a we show the trajectory computed
using the estimated mean of multivariate Gaussian distribution over iterations,
from the lightest (1-th iteration) to the darkest (the last iteration when stopping criterion is met). The optimal trajectory is the darkest line. In Fig. 2b we
show the cost computed using the mean of multivariate Gaussian over iterations.
SAOP converges after 22 iterations with 2200 samples and the optimal cost is
697.29. Each iteration took about 20 to 30 seconds. However, it generates a
collision-free path only after 5 iterations. Due to input saturation, the algorithm
is only ensured to converge to a local optimum. However, in 24 independent
runs, all runs converges to a local optimum closer to the global one, as shown
in the histogram in Fig. 3. Our current work is to implement trajectory-based
contraction analysis using time-varying matrices M (x, t) and adaptive bound β,
which are needed for nonlinear Dubins car dynamics.

Figure 3: The frequency distribution of the optimal costs with 24 independent
runs.

0.5

Conclusion

In this paper, an importance sampling-based approximate optimal planning and
control method is developed. In the control-theoretic formulation of optimal motion planning, the planning algorithm performs direct policy computation using
simulation-based adaptive search for an optimal weight vector corresponding to
an approximate optimal feedback policy. Each iteration of the algorithm runs
time linear in the number of samples and in the time horizon for simulated runs.
However, it is hard to quantify the number of iterations required for MRAS to
converge. One future work is to consider incorporate multiple-distribution importance sampling to achieve faster and better convergence results. Based on
contraction analysis of the closed-loop system, we show that by modifying the
sampling-based policy evaluation step in the algorithm, the proposed planning
algorithm can be used for joint planning and robust control for a class of nonlinear systems under bounded disturbances. In future extension of this work,
we are interested in extending this algorithm for stochastic optimal control.

Bibliography
[1] S. M. LaValle, Planning algorithms.

Cambridge university press, 2006.

[2] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,”
IEEE transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580,
1996.
[3] S. M. Lavalle, “Rapidly-exploring random trees: A new tool for path planning,” Iowa State University, Tech. Rep., 1998.
[4] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30,
no. 7, pp. 846–894, 2011.
[5] D. P. Bertsekas, “Approximate policy iteration: A survey and some new
methods,” Journal of Control Theory and Applications, vol. 9, no. 3, pp.
310–335, 2011.
[6] ——, “Dynamic programming and optimal control 3rd edition, volume ii,”
Belmont, MA: Athena Scientific, 2011.
[7] M. Abu-Khalaf and F. L. Lewis, “Nearly optimal control laws for nonlinear
systems with saturating actuators using a neural network hjb approach,”
Automatica, vol. 41, no. 5, pp. 779–791, 2005.
[8] D. P. Bertsekas and J. N. Tsitsiklis, Neuro-dynamic programming. Athena
Scientific, 1996.
[9] J. Hu, M. C. Fu, and S. I. Marcus, “A model reference adaptive search
method for global optimization,” Operations Research, vol. 55, no. 3, pp.
549–568, 2007.
[10] T. Homem-de Mello, “A study on the cross-entropy method for rare-event
probability estimation,” INFORMS Journal on Computing, vol. 19, no. 3,
pp. 381–394, 2007.
[11] M. Kobilarov, “Cross-entropy randomized motion planning,” Robotics: Science and Systems VII, p. 153, 2012.
15

[12] S. C. Livingston, E. M. Wolff, and R. M. Murray, “Cross-entropy temporal
logic motion planning,” in Proceedings of the 18th International Conference
on Hybrid Systems: Computation and Control. ACM, 2015, pp. 269–278.
[13] D. P. Bertsekas, Dynamic programming and optimal control. Athena Scientific Belmont, MA, 1995, vol. 1, no. 2.
[14] M. Hauschild and M. Pelikan, “An introduction and survey of estimation
of distribution algorithms,” Swarm and Evolutionary Computation, vol. 1,
no. 3, pp. 111–128, 2011.
[15] W. Lohmiller and J.-J. E. Slotine, “On contraction analysis for non-linear
systems,” Automatica, vol. 34, no. 6, pp. 683–696, 1998.
[16] X. Liu, Y. Shi, and D. Constantinescu, “Robust constrained model predictive control using contraction theory,” in IEEE Conference on Decision
and Control, Dec 2014, pp. 3536–3541.

