Control of Uncertain Hybrid Nonlinear SystemsUsing Particle Filters

Leonhard Asselborn Martin Jilg Olaf Stursberg

Institute of Control and System TheoryUniversity of Kassel

l.asselborn@uni-kassel.de

martin.jilg@uni-kassel.de

stursberg@uni-kassel.de

www.control.eecs.uni-kassel.de

Introduction

• Considered class of models: hybrid nonlinear system with deterministic andnondeterministic transitions and uncertain continuous dynamics.

• Formulation of a point-to-region open-loop optimal control problem.• Contribution: proposal of a particle-filter based solution and feasibility

study for an example.

Introduction Model Definition Problem Setup Method Example Conclusion 2

Introduction

• Considered class of models: hybrid nonlinear system with deterministic andnondeterministic transitions and uncertain continuous dynamics.

• Formulation of a point-to-region open-loop optimal control problem.• Contribution: proposal of a particle-filter based solution and feasibility

study for an example.

x1

x2

Region 1 Region 2

t0

t1 t2te

t3

t4

Unsafe Region

Goal Region

initial state

uncertain hybrid trajectory

Introduction Model Definition Problem Setup Method Example Conclusion 2

Stochastic Hybrid Models (1)

• Stochastic hybrid models are a suitable tool for a wide range of processes

Introduction Model Definition Problem Setup Method Example Conclusion 3

Stochastic Hybrid Models (1)

• Stochastic hybrid models are a suitable tool for a wide range of processes

• Three main representations of Stochastic hybrid models in literature1. Stochastic Hybrid Systems (SHS)

◮ Randomness only in the continuous dynamics

2. Switching Diffusion Processes (SDP)◮ Random cont. dynamics and spontaneous transitions according to a Poisson

process

3. Piecewise Deterministic Markov Processes (PDMP)◮ Deterministic continuous dynamics and spontaneous or autonomous transitions

according to a Poisson Process and state space partitions, respectively.

Introduction Model Definition Problem Setup Method Example Conclusion 3

Stochastic Hybrid Models (2)

Control methods for Stochastic Hybrid Models in the literature (as far asrelevant for this paper):

Method specification Model specification

optimalcontrol

particlefilter

chanceconstraints

uncertaindynamics

nonlineardynamics

stochasticevents

deterministicevents

[1] X - X - - X X

[2] X X - X X X -

[3] X - - - X X -

[4] X - X X - - -

[5] - X - X X X -

[6] X - X X - - -

[1]: Bemporad et. al.: Model-Predictive Control of Discrete Hybrid Stochastic Automata, 2011,[2]: Blackmore et. al.: Optimal Robust Predictive Control of Nonlinear Systems under Probabilistic Uncertainty using Particles, 2007,[3]: Adamek et. al.: Stochastic Optimal Control for Hybrid Systems with Uncertain Dynamics, 2008,[4]: Ding et. al.: Increasing Efficiency of Optimization-based Path Planning for Robotic Manipulators, 2011,[5]: Li et. al.: Risk-Sensitive Cubature Filtering for Jump Markov Nonlinear Systems and Its Application to Land Vehicle Positioning, 2011,[6]: Vitus et. al.: Closed-Loop Belief Space Planning for Linear, Gaussian Systems, 2011

Introduction Model Definition Problem Setup Method Example Conclusion 4

Considered Class of Model

The “Jump Markov Nonlinear Hybrid Automaton (JMNHA)”:• No resets in the continuous state variable for autonomous transitions

based on a state space partition

• Spontaneous transitions according to a Markov Process.

• Uncertain nonlinear dynamics: deterministic behavior for t ∈]tk, tk+1[,stochastic pertubation at tk.

→ Model simplification by evaluating the nondeterministic events in discretetime.

Introduction Model Definition Problem Setup Method Example Conclusion 5

Considered Class of Model

The “Jump Markov Nonlinear Hybrid Automaton (JMNHA)”:• No resets in the continuous state variable for autonomous transitions

based on a state space partition

• Spontaneous transitions according to a Markov Process.

• Uncertain nonlinear dynamics: deterministic behavior for t ∈]tk, tk+1[,stochastic pertubation at tk.

PDMP

→ Model simplification by evaluating the nondeterministic events in discretetime.

Introduction Model Definition Problem Setup Method Example Conclusion 5

Considered Class of Model

The “Jump Markov Nonlinear Hybrid Automaton (JMNHA)”:• No resets in the continuous state variable for autonomous transitions

based on a state space partition

• Spontaneous transitions according to a Markov Process.

• Uncertain nonlinear dynamics: deterministic behavior for t ∈]tk, tk+1[,stochastic pertubation at tk.

PDMP

→ Model simplification by evaluating the nondeterministic events in discretetime.

Introduction Model Definition Problem Setup Method Example Conclusion 5

Considered Class of Model

The “Jump Markov Nonlinear Hybrid Automaton (JMNHA)”:• No resets in the continuous state variable for autonomous transitions

based on a state space partition

• Spontaneous transitions according to a Markov Process.

• Uncertain nonlinear dynamics: deterministic behavior for t ∈]tk, tk+1[,stochastic pertubation at tk.

PDMP

→ Model simplification by evaluating the nondeterministic events in discretetime.

Introduction Model Definition Problem Setup Method Example Conclusion 5

Considered Class of Model

The “Jump Markov Nonlinear Hybrid Automaton (JMNHA)”:• No resets in the continuous state variable for autonomous transitions

based on a state space partition

• Spontaneous transitions according to a Markov Process.

• Uncertain nonlinear dynamics: deterministic behavior for t ∈]tk, tk+1[,stochastic pertubation at tk.

PDMP

JMNHA

→ Model simplification by evaluating the nondeterministic events in discretetime.

Introduction Model Definition Problem Setup Method Example Conclusion 5

Considered Class of Model

The “Jump Markov Nonlinear Hybrid Automaton (JMNHA)”:• No resets in the continuous state variable for autonomous transitions

based on a state space partition

• Spontaneous transitions according to a Markov Process.

• Uncertain nonlinear dynamics: deterministic behavior for t ∈]tk, tk+1[,stochastic pertubation at tk.

PDMP

JMNHA

xk

xk+1

xk+1 = xk +∫ tk+1

tkf(τ)dτ + νk

→ Model simplification by evaluating the nondeterministic events in discretetime.

Introduction Model Definition Problem Setup Method Example Conclusion 5

A class of Stochastic Hybrid Systems

Definition 1:

A Jump Markov Nonlinear Hybrid Automaton is defined by

H = (T, Tk, Z,X,U,U , f, ψq, ψd, ν)

with t ∈ T , tk ∈ Tk

• nonlinear continuous dynamics x = f(x, u, d, q), x(t) ∈ X, u(t) ∈ U ,d(tk) ∈ D, q(tk) ∈ Q

• hybrid state space S = X × Z, x ∈ X, z = (d, q) ∈ Z

• update function ψq : X ×X → 2Q for the state space region

• update function ψd : D ×D → [0, 1]nd for the Markov process

• uncertainty ν in the continuous state variable x, xk+1 = xk+1 + νk+1

Introduction Model Definition Problem Setup Method Example Conclusion 6

Admissible behavior of the JMNHA for an example

Continuous dynamics: � f1 � f2 ⊲ f3 ⊲ f4

x1

x2

X1 X2

1

0.9

0.1 0.3

0.7

1

2

Markov process

• Two state space regions X1, X2 → Q = {1, 2}

• Markov Process with state set D = {1, 2}

• A sequence of hybrid states sk := s(tk) = ((dk, qk), xk) is denoted byφs = (s0, s1, s2, ...).

• A set of feasible runs of H for input functions U is denoted by Φs,U

Introduction Model Definition Problem Setup Method Example Conclusion 7

Admissible behavior of the JMNHA for an example

Continuous dynamics: � f1 � f2 ⊲ f3 ⊲ f4

x1

x2

X1 X2

s0

s1

s2tes3

s4

1

0.9

0.1 0.3

0.7

1

2

Markov process

• Two state space regions X1, X2 → Q = {1, 2}

• Markov Process with state set D = {1, 2}

• A sequence of hybrid states sk := s(tk) = ((dk, qk), xk) is denoted byφs = (s0, s1, s2, ...).

• A set of feasible runs of H for input functions U is denoted by Φs,U

Introduction Model Definition Problem Setup Method Example Conclusion 7

Robust Optimal Control Problem

The goal of the proposed method is to control H in an optimal manner w.r.t.chance, input and state constraints.

Introduction Model Definition Problem Setup Method Example Conclusion 8

Robust Optimal Control Problem

The goal of the proposed method is to control H in an optimal manner w.r.t.chance, input and state constraints.

• Perfomance index: Jφs=

∑nt

k=1 h(tk, xk, dk, qk, uk)

Introduction Model Definition Problem Setup Method Example Conclusion 8

Robust Optimal Control Problem

The goal of the proposed method is to control H in an optimal manner w.r.t.chance, input and state constraints.

• Perfomance index: Jφs=

∑nt

k=1 h(tk, xk, dk, qk, uk)

• Unsafe sets: A := ∪na

i=1Ai ⊂ X

Introduction Model Definition Problem Setup Method Example Conclusion 8

Robust Optimal Control Problem

The goal of the proposed method is to control H in an optimal manner w.r.t.chance, input and state constraints.

• Perfomance index: Jφs=

∑nt

k=1 h(tk, xk, dk, qk, uk)

• Unsafe sets: A := ∪na

i=1Ai ⊂ X

• A maximally permitted probability for entering an unsafe set: δ

Introduction Model Definition Problem Setup Method Example Conclusion 8

Robust Optimal Control Problem

The goal of the proposed method is to control H in an optimal manner w.r.t.chance, input and state constraints.

• Perfomance index: Jφs=

∑nt

k=1 h(tk, xk, dk, qk, uk)

• Unsafe sets: A := ∪na

i=1Ai ⊂ X

• A maximally permitted probability for entering an unsafe set: δ

• Goal set: G ⊂ X

Introduction Model Definition Problem Setup Method Example Conclusion 8

Robust Optimal Control Problem

The goal of the proposed method is to control H in an optimal manner w.r.t.chance, input and state constraints.

• Perfomance index: Jφs=

∑nt

k=1 h(tk, xk, dk, qk, uk)

• Unsafe sets: A := ∪na

i=1Ai ⊂ X

• A maximally permitted probability for entering an unsafe set: δ

• Goal set: G ⊂ X

Problem Definition

minuT∈U

E(Jφs)

s.t. s(t0) = ((d0, q0), x0 + ν0)

φs ∈ Φs,U

x(tf ) ∈ G

Prob(xT,φs∈ A for any t ∈ T ) ≤ δ

Introduction Model Definition Problem Setup Method Example Conclusion 8

Approximation by Particle Filters

The main properties of a Particle Filter 1

1[2]: L. Blackmore, B. C. Williams: Optimal Robust Predictive Control of Nonlinear Systemsunder Probabilistic Uncertainty using Particles, 2007

Introduction Model Definition Problem Setup Method Example Conclusion 9

Approximation by Particle Filters

The main properties of a Particle Filter 1

• A particle represents a sample of a random variable Ξ drawn from a givenprobability distribution:

{

ξ(1), . . . , ξ(N)}

• The expected value can be approximated by the sample mean:

E(Ξ) = 1N

∑Ni=1 ξ

(i)

• Each particle represents a deterministic realization of the JMNHA over a finitehorizon.

1[2]: L. Blackmore, B. C. Williams: Optimal Robust Predictive Control of Nonlinear Systemsunder Probabilistic Uncertainty using Particles, 2007

Introduction Model Definition Problem Setup Method Example Conclusion 9

Approximation by Particle Filters

The main properties of a Particle Filter 1

• A particle represents a sample of a random variable Ξ drawn from a givenprobability distribution:

{

ξ(1), . . . , ξ(N)}

• The expected value can be approximated by the sample mean:

E(Ξ) = 1N

∑Ni=1 ξ

(i)

• Each particle represents a deterministic realization of the JMNHA over a finitehorizon.

The particles are used to transform a stochastic optimization problem intoa deterministic variant.

1[2]: L. Blackmore, B. C. Williams: Optimal Robust Predictive Control of Nonlinear Systemsunder Probabilistic Uncertainty using Particles, 2007

Introduction Model Definition Problem Setup Method Example Conclusion 9

Approximation by Particle Filters

The main properties of a Particle Filter 1

• A particle represents a sample of a random variable Ξ drawn from a givenprobability distribution:

{

ξ(1), . . . , ξ(N)}

• The expected value can be approximated by the sample mean:

E(Ξ) = 1N

∑Ni=1 ξ

(i)

• Each particle represents a deterministic realization of the JMNHA over a finitehorizon.

The particles are used to transform a stochastic optimization problem intoa deterministic variant.

The setup of a chance constraint

• Indicator function 1A(x(i)1:nt

) denotes if the i-th trajectory is in A at any time.

• Binary vector λ ∈ {0, 1}N×1 is used for a mixed integer formulation:

PA = 1N

· w · λ ≤ δ with weighting vector w.

1[2]: L. Blackmore, B. C. Williams: Optimal Robust Predictive Control of Nonlinear Systemsunder Probabilistic Uncertainty using Particles, 2007

Introduction Model Definition Problem Setup Method Example Conclusion 9

Proposed Solution Scheme

x1

x2

X1 X2

s0

A1

G

Algorithm (1)

• Define and initialize H.

• Set up a suitable cost function h(tk, xk, dk, qk, uk), unsafe set A, goal setG, number of particles N and max. permitted probability δ.

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0

A1

G

Algorithm (2)

• Draw N samples d(i)0 , x

(i)0 according to to the corresponding probability

distribution.

• Generate a modified transition probability matrix according to theweighting concept and draw a sequence d

(i)1:nt

for each d(i)0 .

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0

A1

G

Algorithm (3)

• Generate a sequence of disturbance samples ν(i)1:nt

∼ N (µν , σν) andcalculate the weights wi.

• Choose an initial sequence of control inputs u0:nt−1 and compute the

future trajectories x(i)1:nt

for every particle.

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0 s1

A1

G

Algorithm (3)

• Generate a sequence of disturbance samples ν(i)1:nt

∼ N (µν , σν) andcalculate the weights wi.

• Choose an initial sequence of control inputs u0:nt−1 and compute the

future trajectories x(i)1:nt

for every particle.

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0 s1 s2te s3

A1

G

Algorithm (3)

• Generate a sequence of disturbance samples ν(i)1:nt

∼ N (µν , σν) andcalculate the weights wi.

• Choose an initial sequence of control inputs u0:nt−1 and compute the

future trajectories x(i)1:nt

for every particle.

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0 s1 s2te s3

A1

G

Algorithm (4)

• Formulate the chance constraint in terms of the weighted particles:PA = 1

N· w · λ ≤ δ

• Determine the approximated costs:

J =nt∑

k=1

Jk =nt∑

k=1

(

1N

N∑

i=1

wi · h(tk, x(i)k , uk, dk, qk)

)

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0 s1s2

te s3

A1

G

Algorithm (4)

• Formulate the chance constraint in terms of the weighted particles:PA = 1

N· w · λ ≤ δ

• Determine the approximated costs:

J =nt∑

k=1

Jk =nt∑

k=1

(

1N

N∑

i=1

wi · h(tk, x(i)k , uk, dk, qk)

)

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0 s1s2

te s3

A1

G

MINLP optimization problemu0:nt−1 = arg min

uT∈UJ

s.t. s(i)(t0) = ((d

(i)0 , q

(i)0 ), x

(i)0 + ν

(i)0 )

φ(i)s ∈ Φs,U , E(xnt

) ∈ G, PA =1

Nw · λ ≤ δ

Introduction Model Definition Problem Setup Method Example Conclusion 10

Proposed Solution Scheme

x1

x2

X1 X2

s0 s1s2

te s3

A1

G

MINLP optimization problemu0:nt−1 = arg min

uT∈UJ

s.t. s(i)(t0) = ((d

(i)0 , q

(i)0 ), x

(i)0 + ν

(i)0 )

φ(i)s ∈ Φs,U , E(xnt

) ∈ G, PA =1

Nw · λ ≤ δ

Introduction Model Definition Problem Setup Method Example Conclusion 10

Trajectory planning for a ground vehicle

• vehicle accelerates tomaximum speed

• vehicle turns left and staysclose to the obstacle

• vehicle decelerates in frontof the goal

• particles lead to manytrajectories with steeringand braking failure due tothe weighting concept

• at most 1 of the 10particle trajectories crossthe obstacle

• MINLP was solved with anSQP method within aBranch-and-Boundenvironment.

isocost lines

particles

goal

stop

start

trajectory

η

ζ

−2 0 2 4 6 8 10 12 14 16 18

5

10

15

20

25

Introduction Model Definition Problem Setup Method Example Conclusion 11

Conclusion

Conclusion

Summary:

• Introduction of an uncertain nonlinear hybrid system.

• Presentation of an algorithm for optimal open-loop control.

• A numerical example showed the results of the proposed method.

Remarks:

• Scheme generates a probabilistically safe sub-optimal trajectory

• Computationally expensive due to the complex model class and thechallenging problem.

• Branch-and-Bound may cut off branches which contain feasible solutions.

Future work:

• Efficient encoding of the chance constraints and alternatives for solvingthe optimization.

• Theoretical analysis concerning the convergence of the approximatedoptimization problem.

Introduction Model Definition Problem Setup Method Example Conclusion 12