ME512: Mobile Robotics Path Planning Algorithms. Atul Thakur, Assistant Professor Mechanical Engineering Department IIT Patna

Transcription

1 ME512: Mobile Robotics Path Planning Algorithms Atul Thakur, Assistant Professor Mechanical Engineering Department IIT Patna

2 Path Planning Problem Given Robot state Obstacle positions Robot capabilities Compute collision free optimal path to a goal Goal Terrain features??? Obstacle

3 Completeness of Planning Algorithms Complete: If a solution exists, it finds one; otherwise, it reports failure. Semi-complete: If a solution exists, it finds one; otherwise, it may run forever. Resolution complete: If a solution exists, it finds one; otherwise, it terminates and reports that no solution within a specified resolution exists. Probabilistically complete: If a solution exists, the probability that it will be found tends to one as the number of iterations tends to infinity.

4 Configuration Space (C-Space) Configuration space (C-space) = space of all possible configurations of the robot Represents the set of all transformations that can be applied to a robot A given its kinematics Powerful abstraction of the planning problem Robot with a complex geometric shape is mapped to a single point configuration q in the C-space So it is possible to use the same planning algorithm on different geometry and kinematics Dimension of C-space = dimension of q = robot s #DOFs C-space obstacle region Cobs Includes self-collision region C-space free region Cfree

5 C-Space Computation Minkowski sum A B { a b a A, b B} Reference point of the robot Workspace (Source: Berg, 2007) Configuration Space

6 State Space Discretization Obstacles Continuous World Rectangular Grid Overlaid on Continuous World Discretized World

7 Graph Representation Robot NW N NE Portion of Graph Representing State Space W E SW S SE

8 Example of Search Search a path between two cities Arad and Bucharest Russel and Norvig, Artificial Intelligence: A Modern Approach

9 Searching for Solution Node: State Parent node Action Path cost Depth Russel and Norvig, Artificial Intelligence: A Modern Approach Collection of nodes is implemented aa a queue Operations on Queue

10 Tree Search Arad Arad Sibiu Timisoara Zerind Russel and Norvig, Artificial Intelligence: A Modern Approach

11 Performance of Algorithm Completeness Is the algorithm guaranteed to find the solution? Optimality Does the strategy find the optimal strategy? Time complexity How long does it take to find the solution? Space complexity How much memory is needed to perform the search?

12 Planning Algorithms Graph search based planning techniques Resolution Complete Uninformed search depth first (DFS), breadth first (BFS) Informed search A*, D*, Dynamic Programming, etc. Combinatorial planning techniques - Complete Sampling based planning techniques Probabilistically Complete We ll focus on graph search based planning techniques in this course

13 Planning Algorithms Graph search based planning techniques Resolution Complete Uninformed search depth first (DFS), breadth first (BFS) Informed search A*, D*, Dynamic Programming, etc. Combinatorial planning techniques - Complete Sampling based planning techniques Probabilistically Complete We ll focus on graph search based planning techniques in this course

14 Uninformed Search Also known as blind search No additional information about states beyond that provided in problem definition Can only generate successors and distinguish a goal state from non-goal states Cannot determine which non-goal state is better or close to the goal state among available non-goal states

15 Illustration of Uninformed Search Breadth First (BFS) 1 Obstacle Nodes in queue Open node 3 3 Closed node

16 Illustration of Uninformed Search Breadth First (BFS) Obstacle Nodes in queue Open node Closed node

17 Illustration of Uninformed Search Depth First (DFS) 1 Obstacle Nodes in queue Open node Closed node BFS is Tree Search with fringe as FIFO queue

18 BFS Algorithm Algorithm BFS (graph G) Initialize Boolean array visited by setting all entries to FALSE Initialize vertex array parent by setting all entries to NIL Initialize an empty queue Q for all v in V If visited[v] = FALSE then bfsfromvertex(g,v) Label start as visited Procedure bfsfromvertex(g,v) visited[v]=true Push v in Q while Q. is not empty u = pop(q) For each neighbor w of u If visited[w]=false then Visited[w] = TRUE Parent[w] = u push w in Q

19 Complexity of BFS Time complexity - O(b d+1 ) Space complexity is O(b d+1 ) b is branching factor and d is depth Russel and Norvig, Artificial Intelligence: A Modern Approach

20 Depth First Search Algorithm Algorithm dfs(graph G) Initialize Boolean array visited by setting all entries to FALSE Initialize Stack S For all v in V If visited[v] = FALSE then dfsfromvertex(g,v) Procedure dfsfromvertex(g,v) Push v in S While S is not empty V=pop(S) If visited[v]=false visited[v]=true For all neighbor w of v Push w in S

21 Complexity of DFS Time complexity: O(b d+1 ), b is branching factor, d is depth Space complexity is O(bd), At each depth level b nodes are added into the queue Thus for d depth levels b*d nodes need to be stored Backtracking Backtracking is used for improving the space complexity At each depth level, only 1 node instead of b nodes are added into the queue Thus overall only d nodes need to be stored Thus space complexity of DFS with backtracking is O(d)

22 Iterative Deepening Search (IDS) Combines DFS and BFS It is a depth limited search where the depth of the DFS is iteratively increased until goal node is searched Russel and Norvig, Artificial Intelligence: A Modern Approach

23 Complexity of IDS Time complexity O b d Space complexity O bd without backtracking Space complexity O d with backtracking

24 Informed Search A*

25 A* Algorithm Informed search technique Makes use of heuristic A* generates an optimal solution if h(n) is an admissible heuristic and the search space is a tree: h(n) is admissible if it never overestimates the cost to reach the destination node

26 Admissible Heuristics A heuristic is admissible if it is too optimistic, estimating the cost to be smaller than it actually is. Example: In the road map domain, h(n) = Euclidean distance to destination is admissible as normally cities are not connected by roads that make straight lines

27 Given: A graph of nodes, is start, n is goal. n s g To find out the path from n s to ng with the minimum cost. Procedure: 1.Create a search graph G,consisting solely of the start node s.put s on a list called OPEN. 2.Create a list called CLOSED that is initially empty. 3.LOOP:if OPEN is empty,exit with failure. 5

28 4.Select the first node on OPEN,remove it from OPEN and put it on CLOSED.Call this node n. 5.If n is a goal node,exit successfully with the solution obtained by tracing a path along the pointers from n to s in G. 6.Expand node n,generating the set, M, of its successors and install them as successors of n in G. 7.Establish a pointer to n from those members of M that were not already in G(I.e, not already on either OPEN or CLOSED). Add these members of M to OPEN.For each member of M that was already on OPEN or CLOSED,decide whether or not to redirect its pointer to n.for each member of M already on CLOSED,decide for each of its descendents in G whether or not to redirect its pointer. 6

29 8.Reorder the list OPEN,either according to some scheme or some heuristic merit. 9.Goto LOOP (Ref.: Principles of Artificial Intelligence by Nils J.Nilsson.) 7

30 Further Reading Choset, H. et al., Principles of Robot Motion Theory, Algorithms and Implementation, MIT Press, 2005