Reachability-based Controller
Design for Switched Nonlinear
Systems
EE 291E / ME 290Q
Jerry Ding
4/18/2012
Hierarchical Control Designs
• To manage complexity, design of modern control
systems commonly done in hierarchical fashion
• e.g. aircraft, automobiles, industrial machinery
• Low level control tend to use continuous abstractions
and design methods
• ODE model
• Stability, trajectory tracking
• Linear/Nonlinear control methods
• High level control tend to use discrete abstractions
and design methods
• Finite state automata, discrete event systems
• Logic specifications of qualitative behaviors: e.g. LTL
• Model checking, supervisory control
2
Challenges of Interfacing Layers of Control
• Problem becomes more difficult at interface:
• Closed loop behavior results from composition of discrete
and continuous designs
• Discrete behaviors may not be implemented exactly by
continuous controllers
• Continuous designs may be unaware of high level
specifications
• In safety-critical control applications, specifications
often involves stringent requirements on closed-loop
behavior
• Current design approaches involve a mixture of
heuristics and extensive verification and validation
3
Hybrid Systems Approach
• Capture closed-loop system behavior through hybrid
system abstraction
4
Hybrid Systems Approach
• Formulate design methods within the framework of
hybrid system theory
• Challenges:
• Nonlinear dynamics, possibly with disturbances
• Controlled switching: switching times, switching
sequence, switching policy
• Autonomous switching: discontinuous vector
fields, state resets
5
Reachability-Based Design for Switched Systems
• Consider subclass of hybrid systems with:
• Controlled switches, no state resets
– Fixed mode sequence
– Variable mode sequence
• Nonlinear continuous dynamics, subject to bounded
disturbances
• Design controllers to satisfy reachability specifications
• Reach-avoid problem: Given target set R, avoid set A, design
a controller to reach R while avoiding A
• Methods based upon game theoretic framework for
general hybrid controller design
• [Lygeros, et al., Automatica, 1999]
• [Tomlin, et al., Proceedings of the IEEE, 2000]
6
Outline
• Switched Systems with Fixed Mode Sequences:
• Design of Safe Maneuver Sequence for Automated Aerial
Refueling (AAR)
• Switched Systems with Variable Mode Sequences:
• Sampled-data switched systems
• Controller synthesis algorithm for reach-avoid problem
• Application example: STARMAC quadrotor experiments
7
Automated Aerial Refueling Procedures
8
Discrete Transitions
q4
 45
Postcontact
qi  Flight maneuvers
 34
 i (i 1)  Command to transitio n from
maneuver i to i  1
q5
Detach
2
q3
 23
Contact
High Level Objective:
Visit waypoint sets Ri,
i = 1,…,6, in sequence
q2
Precontact
 12
 56
q1
Detach
1
Start
q6
Rejoin
End
9
Continuous Dynamics
• Relative States:
• x1, x2 = planar coordinates of tanker in UAV
reference frame
• x3 = heading of tanker relative to UAV
• Controlled inputs:
• u1 = translational speed of UAV
• u2 = turn rate of UAV
• Disturbance inputs:
• d1 = translational speed of Tanker
• d2 = turn rate of Tanker
Low Level Objective:
Avoid protected zone A
around tanker aircraft
 x1   u1  d1 cos x3  u2 x2 
d
x   x2    d1 sin x3  u2 x1 
dt
 x3  

d 2  u2
 f ( x, u , d )
Assume d1 (t )  D, t , d2  0
10
Maneuver Sequence Design Problem
• Given waypoint sets Ri, protected zone A, design
continuous control laws Ki(x) and switching policies
Fi(x) such that
• 1) The hybrid state trajectory (q, x) passes through the
waypoint sets qi× Ri in sequence
• 2) The hybrid state trajectory (q, x) avoids the protected zones
qi× A at all times
• Design approach:
• Select switching policy as follows: in maneuver qi, switch to
next maneuver if waypoint Ri is reached
x  Ri
 i 1 ,
Fi ( x)  
  i , otherwise
• Use reachable sets as design tool for ensuring
– safety and target attainability objectives for each maneuver
– compatibility conditions for switching between maneuvers
11
Capture sets and Unsafe sets
Capture Set :
R ( Ri , K i , T )  x(0)  X : d ()  DT ,  t  [0, T ], x(t )  Ri

where x() is the solution of x  f ( x, K i ( x), d )
DT  {d () : d (t )  D, t [0, T ]}
Target Set
Ri
Unsafe Set :
x ( 0)
A ( A, K i , T )  x(0)  X : d ()  DT ,  t  [0, T ], x(t )  A 
Avoid Set
A
x ( 0)
12
Computation of Reachable Sets
• Use terminal condition to encode avoid set
A  x  X : A ( x)  0, for some A : X  R
• Unsafe set computation (Mitchell, et al. 2005):
Let  : X  [T ,0]  R be the viscosity solution of
T



  
 min 0, min   f ( x, K i ( x), d )  0,  ( x,0)   A ( x)
t
 dD  x 

Then A ( A, Ki , T )  x  X ,  ( x,T )  0
• Capture set computation similar
• Numerical toolbox for MATLAB is available to
approximate solution
[Ian Mitchell, http://www.cs.ubc.ca/~mitchell/ToolboxLS/, 2007]
13
Maneuver Design Using Reachability Analysis
• For mode qN
• 1) Design a control law to drive RN -1 to RN
• 2) Compute capture set to first time instant N such that
RN 1  R ( RN , K N , N )
Waypoint
State Space X
R0
Avoid Set
A
Waypoint
R1
Waypoint
R2
14
Maneuver Design Using Reachability Analysis
• For mode qN
• 3) Compute unsafe set, and verify safety condition
RN 1  X \ A ( A, K N , N )
Modify control law design as necessary
Waypoint
State Space X
R0
Avoid Set
A
Waypoint
R1
Waypoint
R2
15
Maneuver Design Using Reachability Analysis
• For modes qk, k < N
• 3) Iterate procedures 1-3 recursively
For q1 , R0 = X0 , where X0 is the initial condition set
Waypoint
State Space X
R0
Avoid Set
A
Waypoint
R1
Waypoint
R2
16
Properties of Control Law
• Continuous control laws designed in this manner
satisfy a reach-avoid specification for each maneuver:
• Reach waypoint set Ri at some time, while avoiding
protected zone A at all times
• Furthermore, they satisfy a compatibility condition
between maneuvers
Ri  R ( Ri 1 , Ki 1 , i 1 ) \ A ( A, Ki 1 , i 1 )
• This ensures that whenever a discrete switch take
place, the specifications of next maneuver is feasible
• Execution time of refueling
sequence is upper
6
bounded by  f    i
i 1
17
Specifications for Aerial Refueling Procedure
• Target Sets of the form
Ri  B( xdi , r0 )  [ ,  ]
xdi  planar way point location
  heading tolerance
• Avoid sets of the form
A  {x  X : x12  x22 d 0} \ G
d 0  collision zone radius
G  neighborho od around boom location
• Control laws of the form
u1  k1 ( x1  xdi (1))  v0 v0  nominal tanker ve locity
u2  k2 ( x2  xdi (2))
18
Capture Set and Unsafe Set Computation Result
Precontact
(Mode q2)
Time Horizon
 2  3 seconds
19
Simulation of Refueling Sequence
Input bounds
Collision Zone
A
Unsafe Set
For Detach 1
u1  [40, 113]m/s
u2  [ / 6,  / 6]rad
v0  84.8m/s
Target Set Radius
r0  4m
Target Set
R1
Collision Set Radius
Capture Set
For Detach 1
d 0  30m
20
Accounting for Disturbances
• Capture sets and unsafe sets can be modified to
account for fluctuations in tanker velocity using
disturbance set D  [v0  v, v0  v] {0}
Unsafe set for
contact maneuver
without
disturbances
Collision Zone
In UAV
Coordinates
Rescaled coordinates:
distance units in tens
of meters
Reachable set slice
at relative angle 0
Unsafe set for
contact maneuver
with 10% velocity
deviation
21
Outline
• Switched Systems with Fixed Mode Sequences:
• Design of Safe Maneuver Sequence for Automated Aerial
Refueling (AAR)
• Switched Systems with Variable Mode Sequences:
• Sampled-data switched systems
• Controller synthesis algorithm for reach-avoid problem
• Application example: STARMAC quadrotor experiments
22
Switched System Model – Dynamics
Discrete State Space
Q  {q1 , q2 ,..., qm }
Continuous State Space
X  Rn
Continuous Dynamics
x  f i ( x, ui , di )
Reset Relations
R(qi , x)  Q {x}
23
Switched System Model – Inputs
• Sampled-data system for practical implementation
• Quantized input for finite representation of control
policy
Switching Signal     {1,  , m}
Piece-wise
constant

Continuous Input ui U i  {ui1 , ui 2 ,, ui Li }  R
u
TimeVarying
M
Disturbance d i  Di  R i
d
0
T
2T 3T
4T 5T
24
Switched System Model – Control and
Disturbance Policies
• On sampling interval [kT, (k+1)T], define
One step control policy
One step disturbance strategy
k :  U  DT
k : Q  X   U
Q X
 U
DT
(q(kT ), x(kT ))
( (kT ), u (kT ))
d[ kT ,( k 1)T ] ()

d
u
kT
(k+1)T
kT
(k+1)T
25
Outline
• Switched Systems with Fixed Mode Sequences:
• Design of Safe Maneuver Sequence for Automated Aerial
Refueling (AAR)
• Switched Systems with Variable Mode Sequences:
• Sampled-data switched systems
• Controller synthesis algorithm for reach-avoid problem
• Application example: STARMAC quadrotor experiments
26
Problem Formulation
• Given:
• Switched system dynamics; for simplicity, assume that
R(qi , x)  Q {x}
• Target set R
• Avoid set A
Target set
Avoid set
1
R
A
x  f1 ( x, u1 , d1 )
Mode q1
R
2
A
x  f 2 ( x, u2 , d2 )
Mode q 2
27
Problem Formulation
• Compute set of states (q, x) that can be controlled to
target set while staying away from avoid set over finite
horizon
• Call this reach-avoid set C0 N
Target set
Avoid set
Reach-avoid set
1
R
C0 N
A
x  f1 ( x, u1 , d1 )
Mode q1
R
2
C0 N
A
x  f 2 ( x, u2 , d2 )
Mode q 2
28
Problem Formulation
• For any (q, x) in the reach-avoid set, automatically
synthesize a feedback policy that achieves the
specifications
Target set
Avoid set
Reach-avoid set
1
x  f1 ( x, u1 , d1 )
Mode q1
2
x  f 2 ( x, u2 , d2 )
Mode q 2
29
One Step Capture and Unsafe sets
• For each ( i , ui )    U , compute one step capture and
unsafe sets assuming ( (t ), u (t ))  ( i , ui )
over one sampling interval
• One step capture set

R ( R, ( i , ui ), T )  x(0)  X : d ()  DTi , x(T )  Ri }
• One step unsafe set

A ( A, ( i , ui ), T )  x(0)  X : d ()  DTi ,
x(t )  A for some t  [0, T ]
where x () is solution of x  f i ( x, ui , d ) on [0, T ]
30
Reach-avoid Set Computation – Step 1
• For each ( i , ui )    U , compute one step reach-avoid
set using set difference
RA( R, A, ( i , ui ), T )  R( R, ( i , ui ), T ) \ A ( A, ( i , ui ), T )
R
RA( R, A, ( 1 , u12 ), T )
RA( R, A, ( 1 , u11 ), T )
Gi
R
RA( R, A, ( 2 , u12 ), T )
A
A
Mode q1
For sets
RA( R, A, ( 2 , u22 ), T )
Mode q 2
represented by level set functions
The set difference
G1 \ G2
is represented by
i : X  R
max{1 ,  2}
31
Reach-avoid Set Computation – Step 2
• Compute feasible set for one step reach-avoid
problem, by taking union over ( i , ui )    U
RA( R, A, T ) 
 RA(R, A, ( , u ), T )
i
( i ,ui )U
R
R
RA ( R, A, T )
RA ( R, A, T )
A
A
Mode q1
For sets
Gi
The set union
i
Mode q 2
i : X  R
min{ 1 , 2 }
represented by level set functions
G1  G2
is represented by
32
Reach-avoid Set Computation – Iteration
• Iterate to compute the reach-avoid set over [0,NT]
Initialization: S0 : R
for k  0 to N 1
Sk 1 : RA( Sk , A, T )  Sk
end
Return: S N
• By induction, can show that
S N  C0  N
33
Reach-avoid control law synthesis
• At time k < N
Step 1: Obtain state measurement x (kT )
Step 2: Find minimum time to reach kmin
State Space X
R  Target Set
A  Avoid Set
R
S1
S2
x (kT )
kmin  2
A
S3
S j  C0 j  j - time step controllab le set
34
Reach-avoid control law synthesis
• At time k < N
( i , ui )
x(kT )  RA ( S kmin 1 , A, ( i , ui ), T )
Step 3: Find a control input
such that
Step 4: Apply input and iterate steps 1-3
State Space X
R  Target Set
A  Avoid Set
R
S2
S1
x (kT )
Set of states controllab le to
S1 using ( 2 , u2 )
A
Set of states controllab le to
S1 using ( 1 , u1 )
35
Explicit Form of Control Laws
• Explicit control laws given by
FRA ( x)  {( i , ui ) : x  RA ( S kmin ( x ) 1 , A, ( i , ui ), T )}
for x  C0 N
where k min ( x)  min{ j  0,1,..., N : x  S j }
• 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
36
Outline
• Switched Systems with Fixed Mode Sequences:
• Design of Safe Maneuver Sequence for Automated Aerial
Refueling (AAR)
• Switched Systems with Variable Mode Sequences:
• Sampled-data switched systems
• Controller synthesis algorithm for reach-avoid problem
• Application example: STARMAC quadrotor experiments
37
STARMAC Quadrotor Platform
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
38
Experiment Setup
• Objectives:
• Drive a quadrotor to a neighborhood of 2D location in
finite time, while satisfying velocity bounds
• Disturbances: model uncertainty, actuator noise
• System model
x2  d1

 x1  

  
d  x2   g sin( q )  d 2 
x 


y2  d 3
dt  y1  

  
g
sin(


)

d
y
q
4
 2  

( x1 , x2 )  Position, velocity in x - direction
( y1 , y2 )  Position, velocity in y - direction
(q , q )  Roll, pitch commands
g  Gravitatio nal constant
39
Reach-avoid Problem Set-Up
• Target Set: +/- 0.2 m for
position, +/- 0.2 m/s for
velocity
• Avoid Set: +/- 1 m/s for
velocity
• Time Step: 0.1 seconds, 25
time steps
• Pitch and roll commands:
(q , q )  [10 ,10 ]
at 2.5 increments
• Disturbance bounds: d1 , d 3  [0.1, 0.1] m/s,
d 2 , d 4  [0.5, 0.5] m/s 2
40
Reach-avoid Set - Plots
41
Reach-avoid Set - Plots
Reach-avoid at Time Step 1 for All Inputs
42
Reach-avoid Set - Plots
43
Experimental Results
44
Experimental Results
45
Experimental Results
• Moving car experiment
46
References
• John Lygeros, Claire Tomlin, and S. Shankar Sastry. Controllers
for reachability specifications for hybrid systems. Automatica,
35(3):349 – 370, 1999.
• Claire J. Tomlin, John Lygeros, and S. Shankar Sastry. A game
theoretic approach to controller design for hybrid systems.
Proceedings of the IEEE, 88(7):949–970, July 2000.
• Jerry Ding, Jonathan Sprinkle, S. Shankar Sastry, and Claire J.
Tomlin. Reachability calculations for automated aerial refueling. In
47th IEEE Conference on Decision and Control, pages 3706–3712,
Dec. 2008.
• Jerry Ding, Jonathan Sprinkle, Claire Tomlin, S. Shankar Sastry,
and James L. Paunicka. Reachability calculations for vehicle
safety during manned/unmanned vehicle interaction. AIAA
Journal of Guidance, Control, and Dynamics, 35(1):138–152, 2012.
47
References
• Jerry Ding and Claire J. Tomlin. Robust reach-avoid controller
synthesis for switched nonlinear systems. In 49th IEEE
Conference on Decision and Control (CDC), pages 6481–6486,
Dec. 2010.
• Jerry Ding, Eugene Li, Haomiao Huang, and Claire J. Tomlin.
Reachability-based synthesis of feedback policies for motion
planning under bounded disturbances. In IEEE International
Conference on Robotics and Automation (ICRA), pages 2160 –
2165, May 2011.
48