Introduction to Reachability Somil Bansal Hybrid Systems Lab, UC Berkeley

Transcription

1 Introduction to Reachability Somil Bansal Hybrid Systems Lab, UC Berkeley

2 Outline Introduction to optimal control Reachability as an optimal control problem Various shades of reachability

3 Goal of This Presentation Reachability is nothing but an optimal control/differential game problem Everything in reachability ultimately amounts to solving a PDE. Any (small enough) optimal control problem (including reachability problems) can be solved using the Level Set Toolbox.

4 Optimal Control Optimize a cost function subject to system dynamics. Discrete-time systems: Continuous-time systems:

5 A Quick Example: Dubins Car Terminal/Final Time Running Cost Initial Time Terminal Cost Dynamics Initial State Quick question: what are we trying to do to the system here?

6 Solving Optimal Control Problems Approach1: Calculus of Variations (CoV) Takes an optimization perspective Uses Lagrange multipliers to eliminate constraints Derive first-order optimality conditions Globally optimal solution is not guaranteed

7 Solving Optimal Control Problems Approach2: Principle of Dynamic Programming Gives the globally optimal solution. Principle: The optimal state trajectory remains optimal at intermediate points in time.

8 Dynamic Programming Example* n Step T: V T (x = 4) = 0 m 1(5) 2(1) Step T-1: V T 1 (2) = w down (2) + V T (4) V T 1 (2) = = 1 3(4) 4(0) V T 1 (3) = w right (3) + V T (4) V T 1 (3) = = 4 Intersection w right w down Step T-2: V T 2 (1) = min [(w right (1) + V T 1 (2)), (w down (1) + V T 1 (3))] V T 2 (1) = min [(5 + 1), (1 + 4)] = *Shamelessly copied from Sylvia Herbert s presentation 8

9 Magic of Dynamic Programming Target set Compute the set of states that can reach the target set within 4s? Assume that we can compute the set of states that can reach any other given set of states within 1s. t = -1 t = -2 t = -3 t = -4

10 Principle of Dynamic Programming Recall our optimal control problem: + Minimize J x, t = C x t, u t dt, Subject to x = f x, u, t + l x T Define cost from t, x V x t, t = min C x t, u t dt, + l x T We are interested in finding cost from state x and time 0 V x 0, 0 = min C x t, u t dt : + l x T

11 Principle of Dynamic Programming Dynamic programming principle implies that:,<= V x t, t = min 6 V x t + δ, t + δ + 8 C x, u dt, 8 C x, u dt,<=,

12 Principle of Dynamic Programming,<= V x t, t = min 6 V x t + δ, t + δ + 8 C x, u dt, V x t, t + dv dt x t, t δ + V x t, t dx dt δ Taylor Expansion C x, u δ Approximation V x t, t = min 6, V x t, t + dv dt x t, t δ + V x t, t dx dt δ + C x, u δ

13 Principle of Dynamic Programming V x t, t = min 6, V x t, t + dv dt x t, t δ + V x t, t dx dt δ + C x, u δ V x t, t = V x t, t + min 6, dv dt x t, t δ + V x t, t dx dt δ + C x, u δ V x t, t = V x t, t + min 6, dv dt x t, t δ + V x t, t dx dt δ + C x, u δ 0 = min 6, dv dt x t, t δ + V x t, t dx dt δ + C x, u δ 0 = min 6, dv dt x t, t + V x t, t dx dt + C x, u

14 Principle of Dynamic Programming 0 = min 6, dv dt x t, t + V x t, t dx dt + C x, u dv dt + min 6 V x t, t f x, u + C x, u = 0 Hamilton-Jacobi Bellman PDE V x T, T = l x T Final Value of PDE

15 Hamilton-Jacobi Bellman(HJB) PDE Problem: + Minimize J x, t = C x t, u t dt + l x T, Subject to x = f x, u, t Solution A final-value PDE dv dt + min 6 V x t, t f x, u + C x, u = 0 V x T, T = l x T What is V x t, t? Where is my optimal control?

16 Curse of Dimensionality Wow! One single PDE for any optimal control problem. What is the problem here?

17 Level Set Toolbox Developed by Prof. Ian Mitchell during his PhD Can solve any initial-value PDE of the form: dv dt + H (x, V x t, t), t = 0 V x 0, 0 = l x 0 But optimal control problem is a final value PDE.L dv dt + min 6 V x t, t f x, u + C x, u = 0 V x T, T = l x T

18 Level Set Toolbox Good news: They are interchangeable! dv dt + H (x, V x t, t), t = 0 V x 0, 0 = l x 0 dw dt H (x, W x t, t), t = 0 W x T, T = l x T W x, T t = V(x, t) So we can solve any optimal control problem with the toolbox!

19 Optimal Control: Quick Recap Optimal control problem: Optimize a cost function subject to system dynamics. Two approaches: Calculus of Variations: Gives local solutions, but faster to compute. Dynamic Programming: Gives global solution A Final-value PDE needs to be solved May not even have a classical solution! Curse of dimensionality! Can be solved using Level Set Toolbox for low dimensional problems.

20

21 Reachability Problem: Find the set of all states that can reach a given set of states L within a time duration of T. Backward Reachable Tube R T Target Set (L) R T = {x : : u, s.t. x K satisfies x = f x, u, x 0 = x : ; t 0, T, s.t. x(t) L} Any thoughts?

22 Reachability Hint 1: Define a function l x such that, L = x: l x 0 Now consider the problem, V x t, t = min 6 l(x T ) Subject to x = f x, u, t What does V x t, t represent?

23 Reachability What are V x t, t for each of the following trajectories? x 1 (t) x 2 (t) x 1 (T) x 2 (T) l x = 1 l x = 0 l x = 1 x 3 (T) x 3 (t) V x t, t = min 6 l(x T )

24 Reachability So what does V x t, t represent? The value of l x that we will reach at time T How do I answer my original question? x 0 R T V x 0, 0 0 x 0 R T V x 0, 0 > 0

25 Reachability So reachability is nothing but an optimal control problem. min 6 l(x T ) Subject to x = f x, u, t L = x: l x 0 What is the corresponding PDE? Co-state Value Function dv dt + H (x, V x t, t), t = 0 V x T, T = l x T Hamiltonian H = min 6 V x t, t f x, u, t How to get "target-reaching control? u = argmin 6 V x t, t f x, u, t

26 But hold on What are V x t, t for each of the following trajectories? l x = 1 l x = 0 L x 2 (T) x 3 (T) x 2 (t) x 3 (t) V x t, t = min 6 l(x T ) What they should be? R T = {x : : u, s.t. x K satisfies x = f x, u, x 0 = x : ; t 0, T, s.t. x(t) L} x 0 R T V x 0, 0 0

27 What s going on? Need to account for the fact that trajectories can reach the target but then leave it. min ( min l(x t )) 6, :,+ Subject to x = f x, u, t L = x: l x 0 Does this fix the issue? L l x = 1 l x = 0 x 2 (T) x 3 (T) x 2 (t) x 3 (t) V x t, t = min 6 l(x T )

28 Freezing the Trajectories in the Target Need to account for the fact that trajectories can reach the target but then escape it. min ( min l(x t )) 6, :,+ Subject to x = f x, u, t L = x: l x 0 What is the corresponding PDE? dv dt + min {0, H (x, V x t, t), t } = 0 V x T, T = l x T H = min 6 V x t, t f x, u, t

29 Reachability: Reachable Sets vs Tubes Backward Reachable Set (BRS): the set of all states that can reach a target set of states L exactly at time T. R T = {x : : u, s.t. x K satisfies x = f x, u, x 0 = x : ; x(t) L} min 6 l(x T ) Subject to x = f x, u, t L = x: l x 0 dv dt + H (x, V x t, t), t = 0 V x T, T = l x T H = min 6 V x t, t f x, u, t Backward Reachable Tube (BRT): the set of all states that can reach a target set of states L within a duration of time T. R T = {x : : u, s.t. x K satisfies x = f x, u, x 0 = x : ; t 0, T, s.t. x(t) L} min ( min l(x t )) 6, :,+ Subject to x = f x, u, t L = x: l x 0 dv dt + min {0, H (x, V x t, t), t } = 0 V x T, T = l x T H = min 6 V x t, t f x, u, t

30 Backward Reachable Tube: Example Target set Backward reachable tubes t = -1 t = -2 t = -3 t = -4

31 Backward Reachable Set: Example Target set t = -1 Backward reachable sets t = -2 t = -3 t = -4

32 Computing Backward Reachable Tube 1. Define target set L for the system to reach within a given time horizon: R T 2. Define implicit level set function for final time: l z, L = z: l z 0 3. Find an appropriate value function V z(t), t dv dt + min {0, H (x, V x t, t), t } = 0 V x T, T = l x T H = min 6 V x t, t f x, u, t 4. Retrieve zero sub-level of level set function at initial time

33 Reachability: Key Takeaways Reachability is just an optimal control problem. Backward Reachable Set (BRS) vs Backward Reachable Tube (BRT) Both can be computed using the Level Set Toolbox. Suffers from the curse of dimensionality

34

35 Introducing the Disturbance Suppose our dynamics were: x = f x, u, d u control, d disturbance + And cost were: J x, t = C x t, u t, d t dt, + l x T Now, we want to solve the following differential game: V x t, t = min 6 max a + 8 C x t, u t, d t dt, + l x T A similar PDE can be derived in this case (called HJI PDE)

36 Optimal Control vs Differential Game Static Evolving Over Time One agent (input) Optimization Optimal Control Multiple agents (input and disturbance) Game Theory Differential Games

37 Hamilton-Jacobi Isaacs(HJI) PDE Problem: + Minimize J x, t = C x t, u t, d(t) dt + l x T, Subject to x = f x, u, d, t Solution A final-value PDE dv dt + min max 6 a V x t, t f x, u, d + C x, u, d = 0 V x T, T = l x T What is V x t, t? Where is my optimal control?

38 Reachability With Disturbance Problem: Find the set of all states that can reach a given set of states L despite the disturbance within a time duration of T. Backward Reachable Tube R T Target Set (L) R T = x : : u, s.t. d, x K satisfies x = f x, u, d, x 0 = x : ; t 0, T, s.t. x t L What does this definition mean? How to solve this problem?

39 Reachability With Disturbance We can again formulate it as a differential game min max min l(x t ) 6 a, :,+ Subject to x = f x, u, d, t L = x: l x 0 What is the corresponding PDE? dv dt + min {0, H (x, V x t, t), t } = 0 V x T, T = l x T H = min 6 max a V x t, t f x, u, d, t How to get "target-reaching control? u = argmin 6 max a V x t, t f x, u, d, t

40 Reachability With Disturbance: Key Takeaways Very similar to classic reachability; just an extra max Backward Reachable Set (BRS) vs Backward Reachable Tube (BRT) Both can be computed using the Level Set Toolbox. Suffers from the curse of dimensionality

41 Reachability With Disturbance: Quick Trivia Problem: Find the set of all states that can reach a given set of states L for some disturbance within a time duration of T. R T = x : : u, d, s.t. x K satisfies x = f x, u, d, x 0 = x : ; t 0, T, s.t. x t L What does this definition mean? How to solve this problem?

42 Reachability With Disturbance: Trivia Solution Disturbance is like control here min min min l(x t ) 6 a, :,+ Subject to x = f x, u, d, t L = x: l x 0 What is the corresponding PDE? dv dt + min {0, H (x, V x t, t), t } = 0 V x T, T = l x T H = min min 6 a V x t, t f x, u, d, t

43 Shades of Reachability: Avoid Set Problem: Find the set of all states that can avoid a given set of states L despite the disturbance for a time duration of T. Backward Reachable Tube R T Avoid Set (L) R T = x : : d, s.t. u, x K satisfies x = f x, u, d, x 0 = x : ; t 0, T, s.t. x t L What does this definition mean? How to solve this problem?

44 Reachability: Avoid Set Simply exchange the role of input and disturbance max min min l(x t ) 6 a, :,+ Subject to x = f x, u, d, t L = x: l x 0 What is the corresponding PDE? dv dt + min {0, H (x, V x t, t), t } = 0 V x T, T = l x T H = max min 6 a V x t, t f x, u, d, t How to get "target-avoiding control? u = argmax 6 min a V x t, t f x, u, d, t

45 Avoid Set: Example Target set What do the green sets represent here? Backward reachable tubes t = -1 t = -2 t = -3 t = -4

46 Shades of Reachability: Forward Reachable Set Problem: Find the set of all states that I can reach from a given set of states L at time T. Forward Reachable Set R T Initial Set (L) F T = x + : u, s.t. x K satisfies x = f x, u, x 0 L; x T = x + What does this definition mean? How to solve this problem?

47 Forward Reachable Set Define a function l x such that, L = x: l x 0 Now consider the problem, V x t, t = max l(x T ) 6 Subject to x = f x, u, t Will this work?

48 Forward Reachable Set Again it is an optimal control problem. max l(x T ) 6 Subject to x = f x, u, t L = x: l x 0 What is the corresponding PDE? An Initial Value PDE dv dt + H (x, V x t, t), t = 0 V x 0, 0 = l x 0 H = max 6 V x t, t f x, u, t

49 Forward Reachable Set Trivia Problem: Find the set of all states that I can reach from a given set of states L despite the disturbance at time T. F T = x + : u, s.t. d, x K satisfies x = f x, u, d, x 0 L; x T = x + Solve the following PDE: dv dt + H (x, V x t, t), t = 0 V x 0, 0 = l x 0 H = max min 6 a V x t, t f x, u, t

50 Forward Reachable Set: Key Takeaways Gives an initial value PDE Forward Reachable Set (FRS) vs Forward Reachable Tube (FRT) Both can be computed using the Level Set Toolbox. Suffers from the curse of dimensionality

51 Shades of Reachability: Obstacles Problem: Find the set of all states that can reach a given set of states L without hitting the obstacle (G) despite the disturbance within a time duration of T. Backward Reachable Tube R T Target Set (L) Obstacle (G) R T = x :: u, s.t. d, x K satisfies x = f x, u, d, x 0 = x : ; t 0, T, x t G and t 0, T, s.t. x t L What does this definition mean?

52 Reachability With Obstacles Again can be formulated as a differential game Value function can be shown to satisfy the following equation: max { ae a, + min {0, H x, V x t, t), t, g x V x, t } = 0 V x T, T = max l x, T, g x, T H = min 6 max a V x t, t f x, u, d, t How to get "target-reaching control without hitting the obstacle? u = argmin 6 max a V x t, t f x, u, d, t

53 Reachability With Obstacles: Example Target set Fixed obstacle

54 Reachability With Obstacles: Example Reachable Tube t = -1 t = -2 t = -3 t = -4

55 Shades of Reachability: Key Takeaways Everything in reachability ultimately amounts to solving a PDE. Different min-max combinations appear in the PDE based on what control and disturbance are trying to do. Min with zero appears in the PDE depending on whether a set or a tube is being computed. Initial or final value PDE is solved based on whether a FRS or a BRS is being computed Obstacles can be considered Any combination above can be computed using the Level Set Toolbox.

56 Reachability: Final Remarks Reachability theory has much more rigorous mathematical foundation. Time-varying targets/obstacles can also be treated very easily.

57 Thank You!