motion planning, part iii graph search, part i€¦ · enlarge the obstacles to account for the...
TRANSCRIPT
![Page 1: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/1.jpg)
Motion Planning, Part III
Graph Search, Part I
Howie Choset
![Page 2: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/2.jpg)
Happy President’s Day
![Page 3: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/3.jpg)
The Configuration Space
• What it is
– A set of “reachable” areas constructed from
knowledge of both the robot and the world
• How to create it
– First abstract the robot as a point object. Then,
enlarge the obstacles to account for the robot’s
footprint and degrees of freedom
– In our example, the robot was circular, so we
simply enlarged our obstacles by the robot’s radius
(note the curved vertices)
![Page 4: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/4.jpg)
Example of a World (and Robot)
Obstacles
Free Space
Robot
x,y
![Page 5: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/5.jpg)
Configuration Space: Accommodate Robot Size
Obstacles
Free Space
Robot
(treat as point object) x,y
![Page 6: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/6.jpg)
Translate-only, non-circularly
symmetric
Pick a reference point…
![Page 7: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/7.jpg)
Translate-only, non-circularly
symmetric
Pick a reference point…
![Page 8: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/8.jpg)
With Rotation: how much distance to rotate
Pick a reference point…
![Page 9: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/9.jpg)
Where do we put ?
Configuration Space “Quiz”
An obstacle in the robot’s workspace
b
a
270
360
180
90
0 180 360 270 90
qA
b
a
qB
A
B
Torus
(wraps horizontally and vertically)
![Page 10: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/10.jpg)
Where do we put ?
Configuration Space “Quiz”
An obstacle in the robot’s workspace
b
a
270
360
180
90
0 90 180 135 45
qA
b
a
qB
A
B
Torus
(wraps vertically)
![Page 11: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/11.jpg)
Configuration Space Obstacle
An obstacle in the robot’s workspace
b
a
270
360
180
90
0 90 180 135 45
qB
qA
The C-space representation
of this obstacle…
b
a
How do we get from A to B ?
A
B
Reference configuration
![Page 12: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/12.jpg)
Two Link Path
Thanks to Ken Goldberg
![Page 13: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/13.jpg)
Two Link Path
![Page 14: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/14.jpg)
Total Potential Function
+ =
)()()( repatt qUqUqU
)()( qUqF
![Page 15: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/15.jpg)
Local Minimum Problem with the Charge Analogy
![Page 16: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/16.jpg)
Representations
• World Representation – You could always use a large region and distances
– However, a grid can be used for simplicity
![Page 17: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/17.jpg)
Representations: A Grid
• Distance is reduced to discrete steps
– For simplicity, we’ll assume distance is uniform
• Direction is now limited from one adjacent cell to another
– Time to revisit Connectivity (Remember Vision?)
![Page 18: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/18.jpg)
Representations: Connectivity
• 8-Point Connectivity • 4-Point Connectivity – (approximation of the L1 metric)
![Page 19: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/19.jpg)
The Wavefront Planner: Setup
![Page 20: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/20.jpg)
The Wavefront in Action (Part 1)
• Starting with the goal, set all adjacent cells with
“0” to the current cell + 1
– 4-Point Connectivity or 8-Point Connectivity?
– Your Choice. We’ll use 8-Point Connectivity in our example
![Page 21: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/21.jpg)
The Wavefront in Action (Part 2)
• Now repeat with the modified cells
– This will be repeated until no 0’s are adjacent to cells with values >= 2
• 0’s will only remain when regions are unreachable
![Page 22: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/22.jpg)
The Wavefront in Action (Part 3)
• Repeat again...
![Page 23: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/23.jpg)
The Wavefront in Action (Part 4)
• And again...
![Page 24: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/24.jpg)
The Wavefront in Action (Part 5)
• And again until...
![Page 25: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/25.jpg)
The Wavefront in Action (Done)
• You’re done
– Remember, 0’s should only remain if unreachable regions exist
![Page 26: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/26.jpg)
The Wavefront, Now What?
• To find the shortest path, according to your metric, simply always
move toward a cell with a lower number
– The numbers generated by the Wavefront planner are roughly proportional to their
distance from the goal
Two
possible
shortest
paths
shown
![Page 27: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/27.jpg)
This is really a Continuous Solution
Not pixels
Waves bend
L1 distance
![Page 28: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/28.jpg)
Pretty Wavefront
![Page 29: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/29.jpg)
Rapidly-Exploring Random Tree
![Page 30: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/30.jpg)
Path Planning with RRTs
(Rapidly-Exploring Random Trees)
BUILD_RRT (qinit) {
T.init(qinit);
for k = 1 to K do
qrand = RANDOM_CONFIG();
EXTEND(T, qrand)
}
EXTEND(T, qrand)
qnear
qnew
qinit
qrand
[ Kuffner & LaValle , ICRA’00]
![Page 31: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/31.jpg)
Path Planning with RRTs
(Some Details)
BUILD_RRT (qinit) {
T.init(qinit);
for k = 1 to K do
qrand = RANDOM_CONFIG();
EXTEND(T, qrand)
}
EXTEND(T, qrand)
qnear
qnew
qinit
qrand
STEP_LENGTH: How far to sample
1. Sample just at end point
2. Sample all along
3. Small Step
Extend returns
1. Trapped, cant make it
2. Extended, steps toward node
3. Reached, connects to node
STEP_SIZE
1. Not STEP_LENGTH
2. Small steps along way
3. Binary search
![Page 32: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/32.jpg)
Grow two RRTs towards each other
qinit
qgoal
qnew
qtarget
qnear
[ Kuffner, LaValle ICRA ‘00]
![Page 33: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/33.jpg)
Map-Based Approaches:
Roadmap Theory • Properties of a roadmap:
– Accessibility: there exists a collision-free
path from the start to the road map
– Departability: there exists a collision-free
path from the roadmap to the goal.
– Connectivity: there exists a collision-free
path from the start to the goal (on the
roadmap).
a roadmap exists a path exists
Examples of Roadmaps – Generalized Voronoi Graph (GVG)
– Visibility Graph
![Page 34: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/34.jpg)
Roadmap: Visibility Graph
• Formed by connecting all “visible” vertices, the
start point and the end point, to each other
• For two points to be “visible” no obstacle can
exist between them
– Paths exist on the perimeter of obstacles
• In our example, this produces the shortest path
with respect to the L2 metric. However, the close
proximity of paths to obstacles makes it dangerous
![Page 35: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/35.jpg)
The Visibility Graph in Action (Part 1)
• First, draw lines of sight from the start and goal to all
“visible” vertices and corners of the world.
start
goal
![Page 36: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/36.jpg)
The Visibility Graph in Action (Part 2)
• Second, draw lines of sight from every vertex of every obstacle
like before. Remember lines along edges are also lines of sight.
start
goal
![Page 37: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/37.jpg)
The Visibility Graph in Action (Part 3)
• Second, draw lines of sight from every vertex of every obstacle
like before. Remember lines along edges are also lines of sight.
start
goal
![Page 38: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/38.jpg)
The Visibility Graph in Action (Part 4)
• Second, draw lines of sight from every vertex of every obstacle
like before. Remember lines along edges are also lines of sight.
start
goal
![Page 39: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/39.jpg)
The Visibility Graph (Done)
• Repeat until you’re done.
start
goal
![Page 40: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/40.jpg)
Visibility Graph Overview
• Start with a map of the world, draw lines of sight from the
start and goal to every “corner” of the world and vertex of
the obstacles, not cutting through any obstacles.
• Draw lines of sight from every vertex of every obstacle
like above. Lines along edges of obstacles are lines of
sight too, since they don’t pass through the obstacles.
• If the map was in Configuration space, each line
potentially represents part of a path from the start to the
goal.
![Page 41: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/41.jpg)
Graph Search Howie Choset
16-311
![Page 42: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/42.jpg)
42
Outline
• Overview of Search Techniques
• A* Search
![Page 43: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/43.jpg)
43
Graphs
Collection of Edges and Nodes (Vertices)
A tree
![Page 44: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/44.jpg)
44
Grids
![Page 45: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/45.jpg)
45
Stacks and Queues
Stack: First in, Last out (FILO)
Queue: First in, First out (FIFO)
![Page 46: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/46.jpg)
46
Depth First Search
Worst case
performance
O( | V | + | E | ) for explicit graphs traversed without
repetition,
Worst case space
complexity
O( | V | ) if entire graph is traversed without repetition,
O(longest path length searched) for implicit graphs
without elimination of duplicate nodes
algorithm dft(x)
visit(x)
FOR each y such that (x,y) is an edge
IF y was not visited yet
THEN dft(y)
Copied from wikipedia
![Page 47: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/47.jpg)
47
Breadth First Search visit(start node)
queue <- start node
WHILE queue is nor empty DO
x <- queue
FOR each y such that (x,y) is an edge
and y has not been visited yet DO
visit(y)
queue <- y
END
END wikipedia
http://www.cse.ohio-state.edu/~gurari/course/cis680/cis680Ch14.html
![Page 48: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/48.jpg)
48
Depth First and Breadth First
![Page 49: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/49.jpg)
49
Wavefront Planner: A BFS
![Page 50: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/50.jpg)
50
Search
• Uninformed Search
– Use no information obtained from the
environment
– Blind Search: BFS (Wavefront), DFS
• Informed Search
– Use evaluation function
– More efficient
– Heuristic Search: A*, D*, etc.
![Page 51: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/51.jpg)
51
B C
G
D
FE
I J
A
M
H K
NL
Uninformed Search
Graph Search from A to N
BFS
![Page 52: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/52.jpg)
52
Dijkstra’s Search: f(n) = g(n)
Pop lowest f first
![Page 53: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/53.jpg)
53
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
Pop lowest f first
![Page 54: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/54.jpg)
54
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
2. O = {1, 2, 4, 5}; C = {S} (1,2,4,5 all back point to S)
Pop lowest f first
![Page 55: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/55.jpg)
55
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
2. O = {1, 2, 4, 5}; C = {S} (1,2,4,5 all back point to S)
3. O = {1, 4, 5}; C = {S, 2} (there are no adjacent nodes not in C)
Pop lowest f first
![Page 56: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/56.jpg)
56
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
2. O = {1, 2, 4, 5}; C = {S} (1,2,4,5 all back point to S)
3. O = {1, 4, 5}; C = {S, 2} (there are no adjacent nodes not in C)
4. O = {1, 5, 3}; C = {S, 2, 4} (1, 2, 4 point to S; 5 points to 4)
Pop lowest f first
![Page 57: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/57.jpg)
57
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
2. O = {1, 2, 4, 5}; C = {S} (1,2,4,5 all back point to S)
3. O = {1, 4, 5}; C = {S, 2} (there are no adjacent nodes not in C)
4. O = {1, 5, 3}; C = {S, 2, 4} (1, 2, 4 point to S; 5 points to 4)
5. O = {5, 3}; C = {S, 2, 4, 1}
Pop lowest f first
![Page 58: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/58.jpg)
58
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
2. O = {1, 2, 4, 5}; C = {S} (1,2,4,5 all back point to S)
3. O = {1, 4, 5}; C = {S, 2} (there are no adjacent nodes not in C)
4. O = {1, 5, 3}; C = {S, 2, 4} (1, 2, 4 point to S; 5 points to 4)
5. O = {5, 3}; C = {S, 2, 4, 1}
6. O = {3, G}; C = {S, 2, 4 1,5} (goal points to 5 which points to 4 which points to S)
Pop lowest f first
![Page 59: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/59.jpg)
59
Dijkstra’s Search: f(n) = g(n)
1. O = {S}
2. O = {1, 2, 4, 5}; C = {S} (1,2,4,5 all back point to S)
3. O = {1, 4, 5}; C = {S, 2} (there are no adjacent nodes not in C)
4. O = {1, 5, 3}; C = {S, 2, 4} (1, 2, 4 point to S; 5 points to 4)
5. O = {5, 3}; C = {S, 2, 4, 1}
6. O = {3, G}; C = {S, 2, 4 1,5} (goal points to 5 which points to 4 which points to S)
7. O = {3}; C = {S, 2, 4, 1,5} (Path found because Goal was popped)
Pop lowest f first
![Page 60: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/60.jpg)
60
Informed Search: A*
Notation
• n → node/state
• c(n1,n2) → the length of an edge connecting
between n1 and n2
• b(n1) = n2 → backpointer of a node n1 to a
node n2.
![Page 61: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/61.jpg)
61
Informed Search: A*
• Evaluation function, f(n) = g(n) + h(n)
• Operating cost function, g(n)
– Actual operating cost having been already traversed
• Heuristic function, h(n)
– Information used to find the promising node to traverse
– Admissible → never overestimate the actual path cost
Cost on a grid
![Page 62: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/62.jpg)
62
A*: Algorithm
The search requires 2
lists to store
information about
nodes 1) Open list (O) stores nodes
for expansions
2) Closed list (C) stores
nodes which we have
explored
Start
Pick nbest
from O such
that f(nbest
) < f(n)
Remove nbest
from O
and add it to C
nbest
= goal?
End
YN
Expand all nodes x
that are neighbors of
nbest
and not in C
x is not in O?
Add x to Oupdate b(x)=nbest
if
(g(nbest
)+c(nbest
,x)<g(x))
O is empty? EndY
N
YN
![Page 63: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/63.jpg)
63
A*: Example (1/6)
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Heuristics
A = 14 H = 8
B = 10 I = 5
C = 8 J = 2
D = 6 K = 2
E = 8 L = 6
F = 7 M = 2
G = 6 N = 0
Legend operating cost
![Page 64: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/64.jpg)
64
A*: Example (2/6)
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Heuristics A = 14, B = 10, C = 8, D = 6, E = 8, F = 7, G = 6
H = 8, I = 5, J = 2, K = 2, L = 6, M = 2, N = 0
Closed List Open List - Priority Queue
A(0) E(11)
B(14)
H(14)
E(y) = E(f(n))
where f(n) = g(n) + h(n)
= 3 + 8 = 11
Expand
A(x) = A(g(n))
![Page 65: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/65.jpg)
65
A*: Example (3/6)
Heuristics A = 14, B = 10, C = 8, D = 6, E = 8, F = 7, G = 6
H = 8, I = 5, J = 2, K = 2, L = 6, M = 2, N = 0
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Since A → B is smaller
than A → E → B, the f-
cost value of B in an
open list needs not be
updated
Closed List Open List - Priority Queue
A(0)
B(14)
H(14)
E(3)
F(14)
Update Add new node
I(11)
B(16) >
H(15) >
![Page 66: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/66.jpg)
66
A*: Example (4/6)
Heuristics A = 14, B = 10, C = 8, D = 6, E = 8, F = 7, G = 6
H = 8, I = 5, J = 2, K = 2, L = 6, M = 2, N = 0
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Closed List Open List - Priority Queue
A(0)
H(14)
F(14)
E(3)
L(15)
Update Add new node
J(12)
H(18)
F(17) >
M(12)
B(14)I(6)
>
![Page 67: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/67.jpg)
67
A*: Example (5/6)
Heuristics A = 14, B = 10, C = 8, D = 6, E = 8, F = 7, G = 6
H = 8, I = 5, J = 2, K = 2, L = 6, M = 2, N = 0
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Closed List Open List - Priority Queue
A(0)
H(14)
F(14)
E(3)
L(15)
Update Add new node
F(21) >
M(12)
B(14) I(6)
J(10)
N(13)
K(16)
G(19)
M(16) >
![Page 68: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/68.jpg)
68
A*: Example (6/6)
Heuristics A = 14, B = 10, C = 8, D = 6, E = 8, F = 7, G = 6
H = 8, I = 5, J = 2, K = 2, L = 6, M = 2, N = 0
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Since the path to N from
M is greater than that
from J, the optimal path
to N is the one traversed
from J
Closed List Open List - Priority Queue
A(0)
B(14)
H(14)
E(3)
F(14)
I(6)
Update Add new node
J(10)
N(13)
L(15)
K(16)
G(19)
M(10)
N(14)
L(24)
>
>
Goal
![Page 69: Motion Planning, Part III Graph Search, Part I€¦ · enlarge the obstacles to account for the robot’s footprint and degrees of freedom – In our example, the robot was circular,](https://reader034.vdocument.in/reader034/viewer/2022050400/5f7d9734f20a3236d1445d1c/html5/thumbnails/69.jpg)
69
A*: Example Result
B C
G
D
FE
I J
A
M
H K
NL
3
4
6
4
4 4 4
4 46
4
4
4
4 3 4
32
4 4 4
23
4 3
3 3 33 3
Generate the path
from the goal node
back to the start node
through the back-
pointer attribute