Reachability-based Controller Design for Switched Nonlinear Systems

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


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


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


Refueling (AAR)


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


Refueling (AAR)


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


Refueling (AAR)


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

qq

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[),(

qq

24231 m/s 0.5],5.0[, m/s, 0.1]1,.0[, dddd

Reach-avoid Set - Plots

41


42

Reach-avoid at Time Step 1 for All Inputs


43

Experimental Results

44


45


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

Reachability-based Controller Design for Switched Nonlinear Systems

Documents

Transcript of Reachability-based Controller Design for Switched Nonlinear Systems