Optimal Control and Dynamic Programming

Transcription

1 Optimal Control and Dynamic Programming SC Q 7- Duarte Antunes

2 Outline Shortest paths in graphs Dynamic programming Dijkstra s and A* algorithms Certainty equivalent control

3 Graph Weighted Graph Nodes Edges V := {,...,n} E := {(i,j ),...,(i r,j r ) i,...i r,j,...,j r V} Weights Undirected if w ij w ij = w ji (i, j) E E = {(, 6), (, ),...} w 6 =7,w =, Undirected w ij = w ji Directed

4 Applications Graphs model networks (road, social, transportation, etc.) and can be found in numerous applications

5 Shortest path problem Find a path from an initial node to a destination node in a weighted graph, with minimum length (sum of the weights of its edges) Final 6 7 Minimum length 6 Initial Can we use the DP algorithm to find the shortest path?

6 Discussion Computing an optimal path in a transition diagram can be seen as computing the shortest path from the nodes at stage to the node at stage h +of the following weighted graph: c n c n c n c h n h c c c c c h c h n h c c c c c h c h c h artificial node Stage Stage Stage h Stage h artificial stage h + For graphs with this structure we already know how to use DP to compute shortest paths. Adjustments are needed for general graphs (e.g. cycles may occur) but DP can still be used to provide the shortest path, as we show next.

7 Dynamic programming formulation Given a weighted graph construct a transition diagram: stages, states at decision stages and only the destination at the terminal stage. Make, if there is no link from to, and. h = n n w ij = i j Destination Initial w ii = c k ij = w ij

8 Dynamic programming solution Apply the DP algorithm to this transition diagram Costs-to-go at a stage k are the costs of the shortest path with n k hops. In particular costs-to-go at the initial stage are the optimal costs for each initial condition. To find an optimal path follow the policy for a given initial state. Cost-to-go at stage of a given state is infinite if there is no path from that initial state to the destination. State x k 6 6 Destination Initial 7 Stage k The implementation can be made more efficient and one does not need to first construct the transition diagram. Moreover, one can stop when the costs-to-go remain unchanged. 6

9 Example Another example for an undirected graph 6 7 State x k Stage k 7

10 Shortest paths in road networks What is the shortest distance from Bucharest to Lugoj? 7 7 Zerind Oradea Neamt 7 Arad Timisoara Sibiu 99 Fagaras Rimnicu Vilcea Iasi 9 Vaslui Lugoj 97 Pitesti 7 7 Dobreta Mehadia 6 Craiova 9 Urziceni Bucharest 9 Hirsova 6 Eforie Rode map of Romania Giurgiu

11 Shortest paths in road networks km (Route: Bucharest, Pitesti, Craiova, Dobreta, Mehadia, and Lugoj) 9

12 Robot path planning What is the shortest path for a robot to go from point A to B? B A -

13 Assumptions It takes nodes and distance unit to move horizontally or vertically between adjacent units to move diagonally. p Distances to obstacle nodes are infinite. Distance between two diagonally adjacent nodes, adjacent to the same obstacle node is infinite. p p p p

14 Robot path planning What is the shortest path for a robot to go from point A to B? B A

15 Robot path planning Simpler example to show the costs-to-go Side remark: the cost-to-go can be view as a Lyapunov function and the policy can be obtained by following the direction of maximum decrease of this function.

16 Time-varying graphs Initial position How to design a shortest path from A to B when the obstacles are moving? Final t = t = t = t = T

17 Time-varying graphs. Consider the set of static graphs for each time step t = t = t = t = T

18 Time-varying graphs. Build a time-invariant graph in D p p Example p p p t = t = t =. Compute shortest path for D graph Initial node: initial node at time t = Final node: final node at time t = T t = T 6

19 Outline Shortest paths in graphs Dynamic programming Dijkstra s and A* algorithms Certainty equivalent control

20 Discussion DP can be quite inefficient when computing an optimal path in enough. n n > > > > > > > > initial destination Figure example: DP searches the full space - not necessary to compute the optimal path. For shortest path problems in graphs, there are many alternative algorithms. We describe next the Dijkstra s and the A* algorithms. 7

21 Dijkstra s algorithm Main ideas Iteratively generate shorter paths from the origin to every node. Updates list of nodes (wavefront) which can be explored next. New nodes are added to the wavefront based on the cost: neighbors of node with the smallest distance to the origin. source: wikipedia

22 Dijkstra s algorithm Initialization d i = for i V {p}, d p =, and OPEN = {p} p initial node t - final node Steps. Remove a node i from OPEN with the minimum estimate d i. If i = t stop, otherwise execute step for every node j for which there is a path (arrow) from i to j.. If d i + w ij <d j : set d j = d i + w ij, set (j) =i, place j in OPEN if it is not there already. Otherwise do not update, (j). d j. After executing Step for all the nodes j corresponding to out-neighbors of i, go to step I. Optimal path To keep track of the shortest paths if suffices to save for every node i the next node (i) along the optimal path (discovered so far) leading to the initial node. The optimal path is then given by (i,i,...,i L ) for i L = t, i L = (t)...,i = (i ) or equivalently i` = (i`), ` {,,...,L}, where L is such that i = p. If OPEN is empty at a given step of the algorithm then there is no path to the destination. 9

23 Example I Dijkstra s algorithm requires only three iterations for this example Iteration Pairs (i, d i ),i OPEN (, ) n n > > > > > > > > (, ) + other pairs pertaining to other neigh. of node () = initial destination (, ) + other pairs pertaining to other neigh. of nodes & () = Destination/final node removed from OPEN - terminate

24 Example II 6 7 Iteration Pairs (i, d i ),i OPEN 6 (, ) (, ), (, ), (, 6) () = () = () = (, 6), (, ) () = () = (, 6), (, 7) () = (, 7), (6, ) (6) = (6, ) (6) = Optimal path (from end to start) (6, (6), ( (6)),...,) = (6,,,, )

25 Shortest paths in road networks What is the shortest distance from Bucharest to Lugoj? 7 7 Zerind Oradea Neamt 7 Arad Timisoara Sibiu 99 Fagaras Rimnicu Vilcea Iasi 9 Vaslui Lugoj 97 Pitesti 7 7 Dobreta Mehadia 6 Craiova 9 Urziceni Bucharest 9 Hirsova 6 Eforie Rode map of Romania Giurgiu

26 Example III Shortest path from Bucharest to Lugoj Iteration Pairs {i, d i }, i OPEN {Lugoj, } {Mehadia, 7}, {Timisoara, } {Timisoara, }, {Dobreta, } {Dobreta, }, {Arad, 9} {Arad, 9}, {Craiova, 6} {Craiova, 6}, {Sibiu, 69}, {Zerind, } 6 {Sibiu, 69}, {Zerind, }, {Pitesti, }, {Rimnicu Vilcea, } 7 {Sibiu, 69}, {Pitesti, }, {Rimnicu Vilcea, }, {Oradea, 7} {Pitesti, }, {Rimnicu Vilcea, }, {Oradea, 7}, {Fagaras, 6} 9 {Pitesti, }, {Rimnicu Vilcea, }, {Fagaras, 6} {Rimnicu Vilcea, }, {Fagaras, 6}, {Bucharest, } {Fagaras, 6}, {Bucharest, } {Bucharest, } From this data we can obtain and compute the optimal path.

27 A* Similar to Dijkstra s algorithm but an estimate (heuristic) h(i) of the distance to the destination for each node i V is also taken into account when picking the node to be explored next. New nodes are added to the wavefront based on d i + h(i). If the heuristic is: (i) smaller than the optimal cost from that node to the destination; (ii) is such that h(i) apple w ij + h(j) for every i, j then optimal path is found. Otherwise no optimality guarantees. To run the A* algorithm under the two heuristic assumptions:. Change the weights to w ij = w ij + h(j) h(i).. Run Dijkstra s algorithm and get optimal path.. Obtain optimal cost in the original graph with weights w ij. The general algorithm is given next, which works under or without these assumptions.

28 A* Initialization d i = for i V {p}, d p =, and OPEN = {p} p initial node t - final node Steps. Remove a node i from OPEN with the minimum d i + h(i). If i = t stop, otherwise execute step for every node j for which there is a path (arrow) from i to j.. If d i + w ij <d j : set d j = d i + w ij, set (j) =i, place j in OPEN if it is not there already. Otherwise do not update, (j). d j. After executing Step for all the nodes j corresponding to out-neighbors of i, go to step I. (same algorithm as the Dijkstra s algorithm except for remarks to find optimal path as in slide 9) d i + h(i), same

29 A* and Dijkstra s algorithm Dijkstra s A* source: wikipedia A* typically much faster if we have good a heuristic (might not be easy to find! especially if we require it to satisfy two conditions discussed before) 6

30 Example Straight line distance to Bucharest h(i) 7 Oradea 7 Zerin Arad Sibi Timisoara Lugoj 7 7 Mehadia Dobreta Rimnicu 9 9 Craiova Fagaras Pitesti Neamt Giurgiu 9 Urzice Bucharest 9 Iasi 9 Vaslui Hirsova Eforie Neamt Lasi 6 Vaslui 99 Urziceni Hirsova Eforie 6 Bucharest Giurgi 77 Pitesti 9 Craiova 6 Fagaras 7 Sibiu Rimnicu Vilcea 9 Lugoj Mehadia Dobreta Timisoara 9 Arad 66 Zerind 7 Oradea 7

31 Example. Change the weights to w ij = w ij + h(j) h(i).. Run Dijkstra s algorithm and get optimal path.. Obtain optimal cost in the original graph with weights. w ij Iteration Pairs {i, d i } in OPEN {Lugoj,} {Mehadia,67}, {Timisoara,96} {Timisoara,96}, {Dobreta,} {Timisoara,96}, {Craiova,} {Timisoara,96}, {Pitesti,7}, {Rim. Vilcea,6} {Pitesti,7}, {Rim. Vilcea,6}, {Arad,} 6 {Rim. Vilcea,6}, {Arad,}, {Bucharest,6} From this data we can obtain and compute the optimal path.

32 Discussion For large graphs one cannot even store the number of nodes and initialise d i but we can still run the algorithm if we keep track of a list of closed nodes (removed in step, see slide 9) so that they are not visited again (on slide 9 this is assured by d i + w ij <d j ) If optimality is not needed, there are many more graph search algorithms, e.g., breath-first search, depth-first search (see label correcting methods in Bertseka s book, Ch.) For robot motion planning Dijktra and A* are in general naive: construct nodes as we move along (so the graph is only implicit). random placement of nodes are in general better. A popular method that improve upon previous methods based on these two remarks is Rapidly-exploring random tree (RRT) (see LaValle s book). 9

33 Discussion The Dijkstra s algorithm and other search algorithms (e.g. A*) are typically computationally more efficient than DP to compute optimal paths. DP explores every node providing the optimal paths from every node to the destination. This is inefficient when interested in one optimal path. Why then dynamic programming? Provides a policy which allows to cope with disturbances - see lecture. We discuss next how to use the Dijkstra s algorithm to provide the optimal policy in real time (online). In Appendix A, the Dijkstra s algorithm is used to obtain the same optimal policy obtained in the first lecture with DP. Thus, again, why DP then? Stochastic DP! + other advantages.

34 Edsger W. Dijkstra Historical note Edsger W. Dijkstra was a professor at TU/Eindhoven from 96 to 9 What's the shortest way to travel from Rotterdam to Groningen? It is the algorithm for the shortest path, which I designed in about minutes. One morning I was shopping in Amsterdam with my young fiancée, and tired, we sat down on the café terrace to drink a cup of coffee and I was just thinking about whether I could do this, and I then designed the algorithm for the shortest path. As I said, it was a -minute invention. In fact, it was published in 99, three years later. The publication is still quite nice. One of the reasons that it is so nice was that I designed it without pencil and paper. Without pencil and paper you are almost forced to avoid all avoidable complexities. Eventually that algorithm became, to my great amazement, one of the cornerstones of my fame. Edsger W. Dijkstra (9-)

35 Outline Shortest paths in graphs Dynamic programming Dijkstra s and A* algorithms Certainty equivalent control

36 Shortest paths in graphs and policies A transition diagram is just a weighted graph and therefore we can compute optimal paths with methods to compute shortest paths in graphs (e.g. Dijkstra s, A*) initial stage final stage h Doing this for every stage and every state and taking the first decision of the optimal paths we obtain the optimal policy! (function that for each state give the first decision of the optimal path from each state to last stage) example (see also appendix A) optimal policy However, this is typically computationally less efficient than DP

37 Certainty equivalent control Yet, we can implement the method just described (using e.g., the Dijkstra s algorithm) online. Compute the optimal path for the initial sate and take the first decision initial stage final stage h. If no disturbance occurred use the next decision along the optimal path, otherwise recompute (online!) and apply first decision disturbance (recompute) another disturbances (recompute)

38 Discussion Doing this we end up with the same policy as DP neglecting disturbances considered in the previous lecture. The policy obtained with DP is explicit whereas this new (equivalent) one is implicit and requires online computations! In the literature this (equivalent) policy is called certainty equivalent control and is very related to model predictive control (to be addressed later) To summarize: Optimal paths Certainty equivalent control Stochastic DP DP Dijkstra s (more efficient) DP (might be computationally hard) Dijkstra s offline (less efficient) Dijkstra s online (requires online computations) Stochastic DP Dijkstra

39 Concluding remarks Summary DP can be used to solve shortest paths in graphs. Discussed alternative methods, Dijkstra s and A*. Introduced certainty equivalent control. Main message there are other methods to compute optimal paths and optimal policies (except stochastic DP!) - (dis)advantages depend on the application (e.g. can we use online computations?). After this lecture, you should be able to: Compute the shortest path in a graph with DP, Dijkstra and A*.

40 Appendix A Solving a DP problem with Dijkstra s algorithm

41 Example Consider the same initial transition diagram considered in the first lecture and follow steps I Initial transition diagram. Add artificial terminal node with a cost to arrive to it at the final stage coinciding with the terminal cost. Compute the optimal paths (using Dijkstra s algorithm) for each state to the artificial terminal node and keep track for each initial state of the first decision of the optimal path (this is the optimal policy)

42 Example Initial state Iteration Pairs (i, d i ),i OPEN (, ) (, ), (, ) (, ), (7, ), (, ) (, ), (, ), (, ), (, ) (6, ), (, ), (, ), (, ) (6, ), (, ), (, ), (, 7) (6, ), (, ), (, 7), (, ), (, 7) (, ), (, 7), (, ), (, 7) (, ), (, 7), (, ), (, 7) (, ), (, 7), (, ), (, 7) (, ), (, 6), (, 7) (, 6), (, 7) Belongs to the optimal policy It is clear that if we consider the states, 7, as initial states the transitions(arrows) along this path are also optimal first decisions for these initial states (belong to the optimal policy)

43 Example Initial state Iteration Pairs (i, d i ),iopen (, ) (, ), (, ) (, ), (9, ), (, ) (7, ), (9, ), (, ) (, ), (, 7) (9, ), (, ) (, ), (, 7) (, ), (, ) (, ), (, 7) (, ), (, ), (, ) (, ), (, 7), (, 9), (, ), (, ) 9 Belongs to the optimal policy (, ), (, 7), (, 9), (, ) 9 (, 7), (, 9), (, ) (, 9), (, ) It is clear that if we consider the states, 9,, as initial states the transitions(arrows) along this path are also optimal first decisions for these initial states (belong to the optimal policy)

44 Example Initial state Iteration Pairs (i, d i ),iopen (, ) (6, ), (7, ), (, ) (, ), (, ) (7, ), (, ) (, ), (, ) (7, ), (, 6) (, ), (, ) (, 6) (, ), (, 7), (, 6) (, 6) (, 7), (, 6) (, 6) 9 Belongs to the optimal policy 7 (, 7), (, ), (, 6) (, 7), (, 6) 9 (, 7), (, 6) (, 7) It is clear that if we consider the states,, as initial states the transitions(arrows) along this path are also optimal first decisions for these initial states (belong to the optimal policy)

45 Example Initial state 6 Iteration Pairs (i, d i ),i OPEN 6 (6, ) (, ), (, ) (, ), (, 7), (, ) 7 9 (, 7), (, ) Belongs to the optimal policy (, 7), (, 9) (, 7) It is clear that if we consider the states,, as initial states the transitions(arrows) along this path are also optimal first decisions for these initial states (belong to the optimal policy)

46 Example Combining the first decisions leading to the end stage for each node we obtain the optimal policy (the same obtained with the DP algorithm in the first lecture) Optimal policy

47 Appendix B DP with terminal constraints

48 DP with terminal constraints Suppose that we want to reach a given state at the final stage of a transition diagram starting at a given initial state with minimum cost (as opposed to simply reaching the final stage) Initial state terminal state Since a transition diagram is simply a weighted graph, we can apply graph search methods, and in particular repeat the trick just used to apply DP. B

49 B DP with terminal constraints.relabel nodes transform graph to trans. diagram weighted graph with final and terminal nodes Apply DP

50 DP with terminal constraints By inspection we can see that this is the only only part that matters 6 Conclusion: if there is a terminal constraint: Remove the arrows from nodes at the final decision states that do not lead to the desired terminal state. For each state choose the arrow with minimum cost and set the cost-to-go of that node to be the terminal cost of the desired terminal node plus the cost of such arrow. If the state has no arrows, set the cost-to-go to infinity. Apply DP. B