1 / 34

# Administration - PowerPoint PPT Presentation

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

I am the owner, or an agent authorized to act on behalf of the owner, of the copyrighted work described.

Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author.While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server.

- - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - -
Presentation Transcript
• 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
• Visibility Graph Methods
• Cell Decomposition:
• Exact Decomposition
• Approximate: Uniform discretization & quadtree approaches
• Potential Fields
The Visibility Graph Method (cont’d)

We can find the shortest path

using Dijkstra’s Algorithm

GOAL

Path

START

13

12

10

11

9

7

6

5

4

2

3

1

8

Exact Cell Decomposition Method

GOAL

START

1) Decompose Region Into Cells

13

12

10

11

9

7

6

5

4

2

3

1

8

Exact Cell Decomposition Method (cont’d)

GOAL

START

10

12

1

2

7

6

9

Exact Cell Decomposition Method (cont’d)

GOAL

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

Perform cell tessellation of configuration space C

• 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
Approximate Cell Decomposition
Cell Decomposition

Uniform

Continuity of trajectory a function of resolution

• Computational complexity increases dramatically with resolution
• Inexactness. Is this cell an obstacle or in Cfree?

r

Neighbors

Cell Decomposition Issues

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 treeT  Gof 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)

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* Algorithm (cont’d)
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);

end

insert(OPEN, vPlus); % Change f(v) value to reflect better path

end

end

end % End while loop

if v==vgoal

return the path constructed from vgoal to vinit in T

else

return failure; % We didn’t find a valid path

end

The algorithm requires an admissible heuristich(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
The Optimality of A*
A* Simulations

No Obstacles

Single Obstacle

A* Simulations (cont’d)

Multiple Obstacles

No Path

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
Approximate Cell Summary

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 functionU(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
Some Background…

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

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
Some Characteristics of Potential Field Approaches

Generating potential functions for motion control

• PRINT OUT COPY OF POTENTIAL FIELD NOTES by R. Beard (they’re on the web page).
Next Time…