Reachability Based Controller Synthesis for Switched Systems

1. Reachability Based Controller Synthesis for Switched Systems ICRA 2010 workshop Formal methods for robotics and automation May 3, 2010 Jerry Ding Eugene Li Haomiao Huang Prof. Claire Tomlin

2. Outline • Motivation • Switched System Model • Reachable Set Computation • Control Law Synthesis

3. 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 Receding Horizon Control [Belta, Isler, Pappas, IEEE Trans. Robot. 2005] [Wongpiromsarn, Topcu, Murray, HSCC 2010]

4. Problem Formulation • Given: • Switched system dynamics • Target states we want to reach • Unsafe states we want to avoid Target set Unsafe set Mode Mode

5. 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 Reach-avoid set Mode Mode

6. Problem Formulation • For any (q,x) in the reach-avoid set, automatically synthesize a feedback policy that achieves the specifications Target set Unsafe set Reach-avoid set Mode Mode

7. 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

8. Outline • Motivation • Switched System Model • Reachable set computation • Control Law Synthesis

9. Switched System Model – Dynamics Discrete State Space Continuous State Space Continuous Dynamics Reset Relations

10. Switched System Model – Inputs • Sampled-data system for practical implementation • Quantized input to ease computation and analysis Switching Signal Piece-wise constant Continuous Input Disturbance Time-Varying 0 T 2T 3T 4T 5T

11. Switched System Model – Control and Disturbance Policies • On sampling interval [kT, (k+1)T], define One step control policy One step disturbance strategy (k+1)T (k+1)T kT kT

12. Reach-avoid Problem Specification • Reach-avoid problem: Given safe initial condition Choose control policy so that regardless of disturbance strategy 1) 2) • Denote the set of feasible initial conditions by • Reachability problem: compute • Synthesis problem: synthesize

13. Outline • Motivation • Switched Mode System Model • Reachable set computation • Control Law Synthesis

14. 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 This set can be computed numerically using Level Set Methods [Mitchell, et al, TAC, 2005] Unsafe Region Safe Trajectory Unsafe Trajectory One step unsafe reachable set for fixed input Level Set Representation: Mode

15. 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 One Step Target Reachable Set Mode

16. 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 For and One step reach-avoid set for fixed input Let then Mode

17. 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

18. Reach-avoid Set Computation – Step 3 • Take union over possible mode switches in mode qi Target set Unsafe set Reach-avoid set Mode Mode Reach-avoid set for mode 1 over all input levels Reach-avoid set for mode 2 over all input levels

19. Reach-avoid Set Computation – Step 3 • Take union over possible mode switches in mode qi Target set Unsafe set Reach-avoid set Mode Mode One step reach-avoid sets under switching

20. Reach-avoid Set Computation – Step 4 • Iterate to compute the reach-avoid set over [0,NT] • By induction, can show that • Let Target set j time step reach-avoid set Unsafe set Initialization: for to end One step reach-avoid set computation Return:

21. Outline • Motivation • Switched Mode System Model • Reachable set computation • Control Law Synthesis

22. Reach-avoid control law synthesis • Compute and store the reach-avoid sets 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 Number of quantization levels in mode qi Length of time horizon Number of discrete modes

23. Reach-avoid control law synthesis • At time k < N Step 1: Obtain state measurement Target set Unsafe set State Space j time step reach-avoid set

24. Reach-avoid control law synthesis • At time k < N Step 2: Find minimum time to reach Target set Unsafe set State Space j time step reach-avoid set

25. Reach-avoid control law synthesis • At time k < N Step 3: Find set of possible control inputs is the set of states that can safely reach within one step using an admissible input Target set One step reach-avoid set for input Unsafe set One step reach-avoid set for input State Space j time step reach-avoid set

26. Reach-avoid control law synthesis • Over entire time horizon Step 1: Obtain state measurement Step 2: Find minimum time to reach Step 3: Find set of possible control inputs Step 4: Choose and apply control input Step 5: Iterate steps 1 through 4

27. Outline • STARMAC Quadrotor Platform • Problem Set-Up • Reach-avoid Set • Experimental Results • Conclusion and Future Work

28. High Level Control Gumstix PXA270, or ADL PC104 Carbon Fiber Tubing Low Level Control Atmega128 Fiberglass Honeycomb GPS Novatel Superstar II Sensorless Brushless DC Motors Axi 2208/26 Electronic Speed Controllers Castle Creations Phoenix-25 Inertial Meas. Unit Microstrain3DM-GX1 UltrasonicRanger Senscomp Mini-AE Battery Lithium Polymer STARMAC Background

29. Problem Set-Up F u Let Then

30. 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]

31. Reach-avoid Set - Plots

32. Reach-avoid Set - Plots Reach-avoid at Time Step 1 for All Inputs

33. Reach-avoid Set - Plots

34. Experimental Results

35. Experimental Results

36. Experimental Results

37. Experimental Results

38. 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

39. Future Work • Mode transitions with state dependent guards • Multiple inputs within each mode • Approximation methods for continuous time reachable sets • Stochastic reachability problems

40. Thank You • Acknowledgements • Patrick Bouffard • Jeremy Gillula • Haomiao Huang • Tony Mercer • Michael Vitus