Administration Feedback on assignment Late Policy Some Matlab Sample Code makeRobot.m, moveRobot.m

Introduction to Motion Planning CSE350/450-011 16 Sep 03

Class Objectives Cell decomposition procedures for motion planning Review exact approaches from last week Introduce approximating approaches A* Algorithm Introduction to potential field approaches to motion planning

Supporting References Motion Planning “Motion Planning Using Potential Fields”, R. Beard & T. McClain, BYU, 2003 *** PRINT THIS OUT FOR NEXT TIME *** “Robot Motion Planning”, J.C. Latombe, 1991

The Basic Motion Planning Problem Given an initial robot position and orientation in a potentially cluttered workspace, how should the robot move in order to reach an objective pose?

Planning Approaches Roadmap Approach: Cell Decomposition: Visibility Graph Methods Cell Decomposition: Exact Decomposition Approximate: Uniform discretization & quadtree approaches Potential Fields

The Visibility Graph Method GOAL START

The Visibility Graph Method (cont’d) GOAL START

The Visibility Graph Method (cont’d) GOAL START

The Visibility Graph Method (cont’d) GOAL START

The Visibility Graph Method (cont’d) We can find the shortest path using Dijkstra’s Algorithm GOAL Path START

Exact Cell Decomposition Method GOAL 2 6 10 12 7 1 4 9 13 3 11 5 8 START 1) Decompose Region Into Cells

Exact Cell Decomposition Method (cont’d) GOAL 2 6 10 12 7 1 4 9 13 3 11 5 8 START 2) Construct Adjacency Graph

Exact Cell Decomposition Method (cont’d) GOAL 2 6 10 12 7 1 9 START Construct Channel from shortest cell path (via Depth-First-Search)

Exact Cell Decomposition Method (cont’d) GOAL START 4) Construct Motion Path P from channel cell borders

Exact Cell Decomposition Method Using Euclidean Metric GOAL START

Exact Cell Decomposition Method Using Euclidean Metric GOAL START

Approximate Cell Decomposition Perform cell tessellation of configuration space C Uniform or quadtree Generate the cell graph G(V,E) Each cell in Cfree corresponds to a vertex in V Two vertices vi,vj V are connected by an edge eij if they are adjacent (8-connected for exact) Edges are weighted by Euclidean distance Find the shortest path from vinit to vgoal

Cell Decomposition Uniform Quadtree

Cell Decomposition Issues Continuity of trajectory a function of resolution Computational complexity increases dramatically with resolution Inexactness. Is this cell an obstacle or in Cfree? Neighbors r

So how do we find the shortest path? A* Algorithm [Hart et al, 1968] Explores G(V,E) iteratively by following paths originating at the initial robot position vinit For each OPEN node, only keep track of its minimum weight path from vinit The set of all such paths forms a spanning tree T  G of the vertices OPEN so far Define a cost function f(v) = g(v) + h(v) where g(v) is the distance from vinit to v h(v) is a heuristic estimate of the distance from v to vgoal Define a cost function k(v1,v2) = g(v1) + distance(v1,v2)

A* Algorithm (cont’d) Define a list OPEN with the following operations insert(OPEN, v) inserts node v into the list delete(OPEN, v) removes node v from the list minF(OPEN) returns the node v of minimum weight f(v) from OPEN, and removes it from the list isMember(OPEN, v) returns TRUE if v is in the list, false otherwise isEmpty(OPEN) returns TRUE if the list is empty, false otherwise Define the following operations over our spanning tree T insert(T, v2, v1) inserts node v2 into the tree with ancestor v1 delete(T, v) removes node v from the tree

A* - The Algorithm (Latombe, 1991) function A*(G, vinit, vgoal, h, k) insert(T, vinit, nil); % vinit is the tree root insert(OPEN, vinit); while isEmpty(OPEN)==FALSE v = minF(OPEN); % optimal node from f(v) metric if v==vgoal break; end for vPlus  adjacent(v) % All of the cells adjacent to cell v if vPlus is NOT visited insert(T, vPlus, v); % Expansion of spanning tree insert(OPEN, vPlus); else if g(vPlus)>g(v)+k(v,vPlus) % Better way to reach vPlus if true delete(T, vPlus); insert(T, vPlus, v); % Change ancestor to reflect better path if isMember(OPEN, vPlus) delete(OPEN, vPlus); insert(OPEN, vPlus); % Change f(v) value to reflect better path end % End while loop return the path constructed from vgoal to vinit in T else return failure; % We didn’t find a valid path

The Optimality of A* The algorithm requires an admissible heuristic h(v) where h*(v) is the optimal path distance from v to vgoal For admissible h(v) A* is guaranteed to generate a minimum cost path from vinit to vgoal for the given cell decomposition QUESTION: What would be a reasonable function choice for h(v)? When h(v)=0 (a “non-informed” heuristic), A* reduces to our friend Dijkstra’s Algorithm

A* Simulations No Obstacles Single Obstacle

A* Simulations (cont’d) Multiple Obstacles No Path

Approximate Cell Summary PROS Applicable to general obstacle geometries With A*, provides shorter paths than exact decomposition CONS Performance a function of discretization resolution (δ) Inefficiencies Lost paths Undetected collisions Number of graph vertices |V| grows with δ2 and A* runs in O(|V|2) time -> O(δ4) running time

The Potential Field Approach

Some Background… Introduced by Khatib [1986] initially as a real-time collision avoidance module, and later extended to motion planning Robot motion is influenced by an artificial potential field – a force field if you will – induced by the goal and obstacles in C The field is modeled by a potential function U(x,y) over C Motion policy control law is akin to gradient descent on the potential function A shortcoming of the approach is the lack of performance guarantees: the robot can become trapped in local minima Koditschek’s extension introduced the concept of a “navigation function” - a local-minima free potential function

Example Application: Target Tracking with Obstacle Avoidance

The Idea… GOAL

Flashback: What is the Gradient? In 2D, the gradient of a function f is defined as The gradient points in the direction where the derivative has the largest value (the greatest rate of increase in the value of f) The gradient descent optimization algorithm searches in the opposite direction of the gradient to find the minimum of a function Potential field methods employ a similar approach

Some Characteristics of Potential Field Approaches Potential fields can live in continuous space No cell decomposition issues Local method Implicit trajectory generation Prior knowledge of obstacle positions not required The bad: Weaker performance guarantees

Next Time… Generating potential functions for motion control PRINT OUT COPY OF POTENTIAL FIELD NOTES by R. Beard (they’re on the web page).