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

2. Introduction to Motion Planning
CSE350/
16 Sep 03

3. 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

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

5. 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?

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

7. The Visibility Graph Method
GOAL
START

8. The Visibility Graph Method (cont’d)
GOAL
START

9. The Visibility Graph Method (cont’d)
GOAL
START

10. The Visibility Graph Method (cont’d)
GOAL
START

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

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

13. 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

14. 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)

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

16. Exact Cell Decomposition Method Using Euclidean Metric
GOAL
START

17. Exact Cell Decomposition Method Using Euclidean Metric
GOAL
START

18. 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

19. Cell Decomposition
Uniform
Quadtree

20. 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

21. 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)

22. 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

23. 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

24. 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

25. A* Simulations
No Obstacles
Single Obstacle

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

27. 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

28. The Potential Field Approach

29. 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

30. Example Application: Target Tracking with Obstacle Avoidance

31. The Idea…
GOAL

32. 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

33. 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

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