Reachability Based Controller
Synthesis for Switched Systems
ICRA 2010 workshop
Formal methods for robotics and automation
May 3, 2010
Jerry Ding
Eugene Li
Prof. Claire Tomlin
Haomiao Huang
Outline
• Motivation
• Switched System Model
• Reachable Set Computation
• Control Law Synthesis
2
Motivation
• Modern robotics applications commonly use digital
controllers to control continuous dynamics
• Overall system dynamics
features
• High level logic, e.g. maneuver
library
• Low level controls, e.g. motor
control
Linear Temporal Logic
[Kress-Gazit, Fainekos, Pappas, IEEE Trans. Robot. 2009]
Discrete Polygonal Planning
[Belta, Isler, Pappas, IEEE Trans. Robot. 2005]
Receding Horizon Control
[Wongpiromsarn, Topcu, Murray, HSCC 2010]
3
Problem Formulation
• Given:
• Switched system dynamics
• Target states we want to reach
• Unsafe states we want to avoid
Target set
Unsafe set
x  f1 ( x, u1 , d1 )
Mode
q1
x  f 2 ( x, u2 , d 2 )
Mode q2
4
Problem Formulation
• Compute set of states (q, x) that can be controlled to
target set while avoiding unsafe set over finite horizon
• Call this reach-avoid set
Target set
Unsafe set
x  f1 ( x, u1 , d1 )
Mode
q1
Reach-avoid set
x  f 2 ( x, u2 , d 2 )
Mode q2
5
Problem Formulation
• For any (q,x) in the reach-avoid set, automatically
synthesize a feedback policy that achieves the
specifications
Target set
Unsafe set
x  f1 ( x, u1 , d1 )
Mode
q1
Reach-avoid set
x  f 2 ( x, u2 , d 2 )
Mode q2
6
Reachability based methods
• This type of problem is naturally handled by
reachability based analysis tools
• Recovering implementable control law from reachable
sets in general non-trivial
• We describe an automatic controller synthesis
procedure in the case where
• 1) System is switched
• 2) Disturbance does not affect discrete transitions
7
Outline
• Motivation
• Switched System Model
• Reachable set computation
• Control Law Synthesis
8
Switched System Model – Dynamics
Discrete State Space
Q  {q1 , q2 ,...,qm }
Continuous State Space
X  Rn
Continuous Dynamics
x  f ( x, u , d )
Reset Relations
R(qi , x)  Q {x}
9
Switched System Model – Inputs
• Sampled-data system for practical implementation
• Quantized input to ease computation and analysis
Switching Signal     {1, , m}
Piece-wise
constant

Continuous Input ui Ui  {ui1, ui 2 ,, ui Li }  R
u
TimeVarying
Disturbance di  Di  R M i
d
0
T
2T 3T
4T 5T
10
Switched System Model – Control and
Disturbance Policies
• On sampling interval [kT, (k+1)T], define
One step control policy
k :   U  D
k : Q  X    U
Q X
(q(kT ), x(kT ))
One step disturbance strategy
 U
D
( (kT ),u(kT ))
d[ kT ,( k 1)T ] ()

d
u
kT
(k+1)T
kT
(k+1)T
11
Reach-avoid Problem Specification
• Reach-avoid problem:
Given safe initial condition (q(0), x(0))
Choose control policy  0N  {0 , 1 ,,  N 1} so that
regardless of disturbance strategy  0N  {0 ,1 ,, N 1}
1) (q(kT ), x(kT ))  target, k {1,, N}
2) (q(t ), x(t ))  safe, t [0, kT ]
• Denote the set of feasible initial conditions by C0 N
• Reachability problem: compute C0 N
• Synthesis problem: synthesize
 0 N
12
Outline
• Motivation
• Switched Mode System Model
• Reachable set computation
• Control Law Synthesis
13
Reach-avoid Set Computation – Building Blocks
• Fix input level ui in mode qi , compute one step unsafe
reachable set
• Take into account all possible realizations of di
Unsafe
Region
Safe Trajectory
This set can be computed
numerically using Level Set
Methods
[Mitchell, et al, TAC, 2005]
Unsafe
Trajectory
One step unsafe reachable
set for fixed input
Level Set Representation:
Mode
qi
:X R
G  {x  X :  ( x)  0}
14
Reach-avoid Set Computation – Building Blocks
• Fix input level ui in mode qi , compute one step target
reachable set
• Take into account all possible realizations of di
Target Region
This set can be computed
similarly as unsafe set
Trajectory reaches
target in one step
Trajectory does
not reach target
In one step
Mode
One Step Target
Reachable Set
qi
15
Reach-avoid Set Computation – Step 1
• Compute the one step reach-avoid set using set
difference
Target Region
Trajectory reaches target
in one step while avoiding
unsafe region
Unsafe
Region
G1  {x  X : 1 ( x)  0}
and G2  {x  X : 2 ( x)  0}
For
Mode
qi
One step reach-avoid set
for fixed input
Let 3  max{1 ,  2 }
then G  G \ G
3
1
2
 {x  X : 3 ( x)  0}
16
Reach-avoid Set Computation – Step 2
• Take union over possible inputs ui in mode qi
Target Region
One step reach-avoid set
for input 1
One step reach-avoid set
for input 2
One step reach-avoid set
for mode qi over all input
levels
Unsafe
Region
Mode
qi
17
Reach-avoid Set Computation – Step 3
• Take union over possible mode switches in mode qi
Target set
Mode
Unsafe set
q1
Reach-avoid set for mode 1
over all input levels
Reach-avoid set
Mode q2
Reach-avoid set for mode 2
over all input levels
18
Reach-avoid Set Computation – Step 3
• Take union over possible mode switches in mode qi
Target set
Mode
Unsafe set
q1
Reach-avoid set
Mode q2
One step reach-avoid sets
under switching
19
Reach-avoid Set Computation – Step 4
• Let
R
A
Target set
Sj 
j time step reach-avoid set
Unsafe set
• Iterate to compute the reach-avoid set over [0,NT]
Initialization: S0 : R
for j  0 to N  1
S j 1 : Reachavoid(S j , A, T )  S j
end
One step reach-avoid set
Return: S N
computation
• By induction, can show that
S N  C0N
20
Outline
• Motivation
• Switched Mode System Model
• Reachable set computation
• Control Law Synthesis
21
Reach-avoid control law synthesis
• Compute and store the reach-avoid sets
Sj,
j  0, 1, , N 1
and those corresponding to particular inputs
• These sets define an explicit state feedback policy for
the reach-avoid problem
• Number of reachable sets required is given by
M

N R  N   ( Li  1) 
 i 0

N  Length of time horizon
M  Number of discrete modes
Li  Number of quantization
levels in mode qi
22
Reach-avoid control law synthesis
• At time k < N
Step 1: Obtain state measurement (q(kT ), x(kT ))
R
A
R
Target set
Unsafe set
(q(kT ), x(kT ))
Sj 
j time step
reach-avoid set
A
State Space
Q X
23
Reach-avoid control law synthesis
• At time k < N
Step 2: Find minimum time to reach Nk  N  k
R
A
R
Target set
S1
S2
(q(kT ), x(kT ))
Sj 
Nk  2
Unsafe set
j time step
reach-avoid set
S3
A
State Space
Q X
24
Reach-avoid control law synthesis
• At time k < N
Step 3: Find set of possible control inputs
S j is the set of states that can safely reach
within one step using an admissible input
R
A
R
Target set
Unsafe set
S2
S1
(q(kT ), x(kT ))
Sj 
S j 1
j time step
reach-avoid set
One step reach-avoid
set for input (q2 , u2 )
A
One step reach-avoid
set for input (q , u )
1
1
State Space
Q X
25
Reach-avoid control law synthesis
• Over entire time horizon
Step 1: Obtain state measurement (q(kT ), x(kT ))
Step 2: Find minimum time to reach Nk  N  k
Step 3: Find set of possible control inputs
Step 4: Choose and apply control input
Step 5: Iterate steps 1 through 4
26
Outline
• STARMAC Quadrotor Platform
• Problem Set-Up
• Reach-avoid Set
• Experimental Results
• Conclusion and Future Work
27
STARMAC Background
High Level Control
Carbon Fiber
Tubing
Gumstix PXA270, or
ADL PC104
Low Level Control
Fiberglass
Honeycomb
Atmega128
GPS
Novatel
Superstar II
Sensorless
Brushless DC
Motors
Axi 2208/26
Inertial
Meas. Unit
Microstrain
3DM-GX1
Electronic Speed
Controllers
Ultrasonic
Ranger
Senscomp
Mini-AE
Battery
Castle Creations
Phoenix-25
Lithium Polymer
28
Problem Set-Up
u
• Let
F
x 1 = position; x 2 = velocity
• Then
·
x_1
x_2
¸
·
=
x 2 + d1
T
m si n(u) + d2
¸
·
=
x 2 + d1
gsi n(u) + d2
¸
29
Problem Set-Up
• Target Set: +/- 0.2 m for
position, +/- 0.2 m/s for
velocity
• Unsafe Set: +/- 1 m/s for
velocity
• Time Step: 0.1 seconds, 25
time steps
• Input Range: 9 possible
inputs, [-10, -7.5, -5, -2.5, 0,
2.5, 5, 7.5, 10]
30
Reach-avoid Set - Plots
31
Reach-avoid Set - Plots
Reach-avoid at Time Step 1 for All Inputs
32
Reach-avoid Set - Plots
33
Experimental Results
34
Experimental Results
35
Experimental Results
36
Experimental Results
37
Conclusion
• Proposed automatic controller synthesis method for
switched systems
• Nonlinear continuous dynamics, up to 3-4 state dimensions
• Differential game setting – possibly large disturbances
• Directly handles state and input constraints
• Provides explicit feedback policy that can be implemented in
sampled-data system
• Possible applications:
• Target/obstacle problems
• Stabilization problems
• Safety/invariance problems
38
Future Work
• Mode transitions with state dependent guards
• Multiple inputs within each mode
• Approximation methods for continuous time reachable
sets
• Stochastic reachability problems
39
Thank You
• Acknowledgements
• Patrick Bouffard
• Jeremy Gillula
• Haomiao Huang
• Tony Mercer
• Michael Vitus
40