Post on 21-Jan-2016
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
5
• Challenges:• Nonlinear dynamics, possibly with disturbances• Controlled switching: switching times, switching
sequence, switching policy• Autonomous switching: discontinuous vector
fields, state resets
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
8
Automated Aerial Refueling Procedures
9
Discrete Transitions
Detach1
1q
Precontact
2q
Contact
3q
Postcontact4q
Detach2
5q
Rejoin
6q
12
23
3445
56
Start
End
1 tomaneuver
fromn transitio toCommand
maneuversFlight
)1(
ii
q
ii
i
High Level Objective: Visit waypoint sets Ri, i = 1,…,6, in sequence
10
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
),,(
sin
cos
22
1231
22311
3
2
1
duxf
ud
xuxd
xuxdu
x
x
x
dt
dx
0,,)( Assume 21 dtDtd
Low Level Objective: Avoid protected zone A around tanker aircraft
11
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
• Use reachable sets as design tool for ensuring– safety and target attainability objectives for each maneuver– compatibility conditions for switching between maneuvers
otherwise,
,)( 1
i
iii
RxxF
12
Capture sets and Unsafe sets
)),(,( ofsolution theis )( where
)(],,0[ ,)(:)0(),,(
:Set Capture
dxKxfxx
RtxTtdXxTKR
i
iTii
DR
AtxTtdXxTKA Ti )(],,0[ ,)(:)0(),,(
:Set Unsafe
DA
iR)0(x
A
)0(x
SetTarget
Set Avoid
]},0[,)(:)({ TtDtddT D
13
Computation of Reachable Sets
• Unsafe set computation (Mitchell, et al. 2005):
Let be the viscosity solution of
)()0,(,0)),(,(min,0min xxdxKxfxt Ai
T
Dd
• Use terminal condition to encode avoid set
R XxXxA AA : somefor ,0)(:
Then
R ]0,[: TX
0),(,),,( TxXxTKA i A
• Capture set computation similar
• Numerical toolbox for MATLAB is available to approximate solution [Ian Mitchell, http://www.cs.ubc.ca/~mitchell/ToolboxLS/, 2007]
Maneuver Design Using Reachability Analysis
14
• 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),,(1 NNNN KRR R
2R1R
0R
ASet Avoid
Waypoint
Waypoint Waypoint
X Space State
Maneuver Design Using Reachability Analysis
15
• For mode qN
• 3) Compute unsafe set, and verify safety condition
Modify control law design as necessary
),,(\1 NNN KAXR A
ASet Avoid
Waypoint
Waypoint Waypoint
X Space State
2R1R
0R
Maneuver Design Using Reachability Analysis
16
• For modes qk, k < N• 3) Iterate procedures 1-3 recursively
For q1 , R0 = X0 , where X0 is the initial condition set
ASet Avoid
Waypoint
Waypoint Waypoint
X Space State
2R1R
0R
Properties of Control Law
17
• 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
• This ensures that whenever a discrete switch take place, the specifications of next maneuver is feasible
• Execution time of refueling sequence is upper bounded by
),,(\),,( 11111 iiiiii KAKRR AR
6
1iif
18
Specifications for Aerial Refueling Procedure
• Target Sets of the form
toleranceheading
locationpoint planar way
],[),( 0
id
idi
x
rxBR
• Avoid sets of the form
location boom around odneighborho
radius zonecollision
\}:{
0
022
21
G
d
GdxxXxA
• Control laws of the form
))2((
))1((
222
0111
id
id
xxku
vxxku
locity tanker venominal0 v
19
Capture Set and Unsafe Set Computation Result
Precontact(Mode q2) Time Horizon
seconds 32
20
Simulation of Refueling Sequence
m/s8.84
rad]6/,6/[
m/s]113,40[
0
2
1
v
u
u
Input bounds
Target Set Radius
m40 rCollision Set Radius
m300 d
Collision ZoneA
Unsafe SetFor Detach 1
Target Set
1R
Capture SetFor Detach 1
Accounting for Disturbances
21
• Capture sets and unsafe sets can be modified to account for fluctuations in tanker velocity using disturbance set }0{],[ 00 vvvvD
Collision ZoneIn UAV Coordinates
Unsafe set for contact maneuver without disturbances
Unsafe set for contact maneuver with 10% velocity deviation
Reachable set slice at relative angle 0
Rescaled coordinates: distance units in tens of meters
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
23
),,( iii duxfx Continuous Dynamics
nX RContinuous State Space
Discrete State Space},...,,{ 21 mqqqQ
Reset Relations
}{),( xQxqR i
Switched System Model – Inputs
24
0 T 2T 3T 4T 5T
Piece-wise constant
u
dTime-Varying
• Sampled-data system for practical implementation• Quantized input for finite representation of control
policy
Switching Signal
Continuous Input
Disturbance
},,1{ m
R },,,{ 21 iLiiiii uuuUu
iMii Dd R
Switched System Model – Control and Disturbance Policies
• On sampling interval [kT, (k+1)T], define
25
One step control policy
XQ U TD
))(),(( kTxkTq ))(),(( kTukT )(])1(,[ TkkTd
UXQk :One step disturbance strategy
u
kT (k+1)T kT (k+1)T
d
Tk U D:
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 • Target set R• Avoid set A
27
Target set Avoid set
),,( 111 duxfx ),,( 222 duxfx
1
21qMode2qMode
AR
AR
}{),( xQxqR i
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
28
Reach-avoid setTarget set Avoid set
NC 0
),,( 111 duxfx ),,( 222 duxfx
1
21qMode2qMode
AR
AR NC 0 NC 0
• For any (q, x) in the reach-avoid set, automatically synthesize a feedback policy that achieves the specifications
Problem Formulation
29
Reach-avoid setTarget set Avoid set
),,( 111 duxfx ),,( 222 duxfx
1
21qMode2qMode
One Step Capture and Unsafe sets
30
• For each , compute one step capture and unsafe sets assumingover one sampling interval
),())(),(( ii utut Uuii ),(
where is solution of on)(x ),,( duxfx ii
• One step capture set
})(,)(:)0()),,(,( iiTii RTxdXxTuR DR
• One step unsafe set
],0[ somefor )(
,)(:)0()),,(,(
TtAtx
dXxTuA iTii
DA
],0[ T
Reach-avoid Set Computation – Step 1
31
• For each , compute one step reach-avoid set using set difference
)),,(,(\)),,(,()),,(,,( TuATuRTuAR iiiiii ARRA
A A
1qMode2qMode
)),,(,,( 111 TuAR RA
R R)),,(,,( 211 TuAR RA
)),,(,,( 122 TuAR RA
)),,(,,( 222 TuAR RA
Uuii ),(
For sets represented by level set functions
The set difference is represented by iG RXi :
21 \GG },max{ 21
Reach-avoid Set Computation – Step 2
• Compute feasible set for one step reach-avoid problem, by taking union over
32
Uuii ),(
Uu
ii
ii
TuARTAR
),(
)),,(,,(),,(
RARA
A A
1qMode2qMode
R R
),,( TARRA ),,( TARRA
For sets represented by level set functions
The set union is represented by iG RXi :
21 GG },min{ 21
Reach-avoid Set Computation – Iteration
• Iterate to compute the reach-avoid set over [0,NT]
• By induction, can show that
kkk STASS ),,(:1 RA
NN CS 0
Initialization: RS :0
for 0k to 1N
end
Return: NS
33
Reach-avoid control law synthesis
34
• At time k < N
Step 2: Find minimum time to reach mink
)(kTx
1S
2S
3S
2min k
SetTarget RSet AvoidA
R
A
X Space State
set lecontrollab step time-j0 jj CS
)(kTxStep 1: Obtain state measurement
Reach-avoid control law synthesis
35
• At time k < NStep 3: Find a control input such that
1S2S
R
A
)),,(,,()( 1minTuASkTx iik RA),( ii u
Step 4: Apply input and iterate steps 1-3
)(kTx
SetTarget RSet AvoidA
X Space State
),( using
tolecontrollab states ofSet
111 uS
),( using
tolecontrollab states ofSet
221 uS
Explicit Form of Control Laws
36
• Explicit control laws given by
• Number of reachable sets required is given by
10
m
iiR LNN
N Length of time horizon
m Number of discrete modes
iL Number of quantization levels in mode qi
)}),,(,,(:),{()( 1)(minTuASxuxF iixkiiRA RA
NCx 0
where }:,...,1,0min{)(min jSxNjxk for
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
38
Ultrasonic
RangerSenscomp
Mini-AE
Ultrasonic
RangerSenscomp
Mini-AE
Inertial Meas. Unit
Microstrain3DM-GX1
Inertial Meas. Unit
Microstrain3DM-GX1
GPSNovatel
Superstar II
GPSNovatel
Superstar II
Low Level ControlAtmega128
Low Level ControlAtmega128
Carbon Fiber
Tubing
Carbon Fiber
Tubing
Fiberglass Honeycom
b
Fiberglass Honeycom
b
Sensorless Brushless DC MotorsAxi 2208/26
Sensorless Brushless DC MotorsAxi 2208/26
Electronic Speed Controllers
Castle Creations Phoenix-25
Electronic Speed Controllers
Castle Creations Phoenix-25Battery
Lithium PolymerBattery
Lithium Polymer
High Level Control
Gumstix PXA270, or ADL PC104
High Level Control
Gumstix PXA270, or ADL PC104
Experiment Setup
39
• Objectives: • Drive a quadrotor to a neighborhood of 2D location in
finite time, while satisfying velocity bounds
• Disturbances: model uncertainty, actuator noise
• System model
4
32
2
12
2
1
2
1
)sin(
)sin(
dg
dy
dg
dx
y
y
x
x
dt
dx
q
q
constant nalGravitatio
commandspitch Roll,),(
direction-yin velocity Position,),(
direction-in x velocity Position,),(
21
21
g
yy
xx
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:
• Disturbance bounds:
40
increments 5.2at
],1010[),(
24231 m/s 0.5],5.0[, m/s, 0.1]1,.0[, dddd
Reach-avoid Set - Plots
41
Reach-avoid Set - Plots
42
Reach-avoid at Time Step 1 for All Inputs
Reach-avoid Set - Plots
43
Experimental Results
44
Experimental Results
45
Experimental Results
46
• Moving car experiment
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