EXAMPLES OF PRACTICAL ALGORITHMS FOR PATH PLANNING

1

• EXAMPLES OF PRACTICAL ALGORITHMS FOR PATH
  PLANNING

2

• Bug Algorithms

3

• Bug 2

4
Bugs 2
qtarget
M-line
New leave point condition dltd(Hj,Target)
qinit
5
qtarget
M-line

• From point Lj-1 move along M-line until
• Target is reached. Stop
• An obstacle is hit at Hj. Goto 2
• Turn left and follow the boundary until
• Target is reached. Stop
• M-line met at distance d from target such that
• d lt dist(Hj,qtarget)
• Define Lj, set jj1, and goto 1.
• If we return to Hj without meeting the M-line,
  stop. The target is trapped.

New leave point condition dltd(Hj,Target)
qinit
6
Bug 2 Algorithm

• From point Lj-1 move along M-line until
• Target is reached. Stop
• An obstacle is hit at Hj. Goto 2
• Turn left and follow the boundary until
• Target is reached. Stop
• M-line met at distance d from target such that
• d lt dist(Hj,qtarget)
• Define Lj, set jj1, and goto 1.
• If we return to Hj without meeting the M-line,
  stop. The target is trapped.

7
A Hard Example for Bug 2
8
A Hard Example for Bug 2
9
Bug Noncontact Sensors

• Suppose the robot is provided with info within a
  disk of radius r
• E.g. vision, infra-red, ultrasonic
• The robot reconstructs at each point, the path
  that it would generate with no info
• Result smooth version of previous paths

10
Bug Non Contact Sensor
11

• Probabilistic Roadmaps

12
Probabilistic Roadmaps

• Path planning algorithm are expensive. Search on
  small grid cannot be used.
• If DOFs is large, deterministic algorithms are
  not effective
• Solution use a probabilistic roadmap planner
  (PRM).
• PRM are complete in a probabilistic sense
• An heuristic planner
• potential fields.
• Disadvantage
• local minima.
• Solution
• randomize to escape local minima

13
Description of Probabilistic Roadmap Algorithm

1. Roadmap undirected graph R (N, E )
2. N (nodes) set of selected configurations in
   Cfree
3. E (edges) collection of simple paths. The Local
   Paths
4. Local paths are computed by the fast but not
   powerful local planner
5. Idea connect qinit and qgoal with qinit and
   qgoal in N
6. Search R for a path

14
Preprocessing Phase

• Three stages
• Roadmap construction. Objectives
• Obtain reasonable connected graph
• Be sure difficult regions contain a few nodes
• Roadmap expansion. Objectives
• Improve graph connectivity by selecting nodes of
  R which lie in (heuristic) difficult regions and
  adding nodes there
• Roadmap Component reduction. Optional. Attempts
  to simplify the graph

15
Roadmap Construction

1. Initially R is empty
2. Repeatedly, generate add a free configuration
   to R
3. For every new q, select some nodes in R and try
   to connect them to q using local planner
4. On success, add the edge (q, q ) to E

? Cfree Cfree ? 0,1 is a local function
returned by local planner D pseudo metric in
Cfree
16
Create random configurations
AS a first step we create random configurations
in the space. The algorithm creates the points
one at a time, but were not going to do that
with the slides. As configurations are created we
try to connect to already existing nodes in the
graph (if they are close enough they will get
connected)
17
Update Neighboring Nodes Edges
As edges are added to the graph we start forming
connected regions.
18
End of Construction Step
At the end of the construction step we end up
with one or more connected components. We can
probably improve connectivity by adding more
nodes in the expansion stage at the difficult
areas of the space.
19
Expansion Step
The nodes are added and will be connected again
using the local planner on the closest nodes.
20
End of Expansion Step
At the end of the expansion step we end up with
the same number, or fewer connected components.
21
Select start and goal
Start
Goal
We represent our start and end goal
22
Connect Start and Goal to Roadmap
Start
Goal
Find the nodes in the graph that are closest to
the start and goal and connect then with edges.
23
Find the Path from Start to Goal
Start
Goal
Now simply search for a path.
24
Roadmap construction algorithm

1. N?ø , E ?ø
2. Loop
3. q ? randomly selected free configuration
4. Nq a set of candidate neighbors of q from N
5. N N ? q
6. ? q ?Nq in order of increasing D(q,q ), do
7. If q, q are not in the same connected comp. and
   ?(q,q) 1, then E ?E ? (q, q )
8. Update Rs connected components

How?
Meaning?
Select
Algorithm?
25
Creation of Random Configurations

• Nodes of R uniform random sampling of Cfree
• Obtain q by drawing each of its coordinates.
• From allowed interval for the corresponding DOF
• Check q for collisions
• If q is collision-free, add it to N. Otherwise
  discard

26
Ideas for the Local Planner

1. Deterministic vs. Non-deterministic planner
2. Powerful planer slower planner
3. Empirical results use deterministic and simple
   but very fast planner
4. Example connect two configurations by a straight
   line in configuration space
5. Check for collisions by bisection

27
Node Neighbors

• Choice of Nq is important.
• Local planner is used for all qs in Nq
• Nqq? N / MaxDist ? D(q,q)
• Skip nodes in the same connected component
• Heuristics bound the size of Nq by some K. This
  gives independence on the size of R

28
Distance Function D

• D is used for constructing sorting Nq
• Hopefully, D reflect the chance that the local
  planner will fail
• Simple selection that works in practice
• D(q,q)maxx?Robotx(q)-x(q)

29
Roadmap Expansion

1. N should be a fair enough covering of Cfree
2. R often consists of a few large components and
   several small ones
3. Expansion add nodes to form a large component
   with as many nodes as possible
4. Try to cover the difficult parts of Cfree
5. Define weight w(q). Large w ? q is in a difficult
   region
6. Add M nodes to the collection

30
End of slides
31
Some results on BUG2

• Ni number of intersections of Bi with M-line
• Lemma in Bug2, any segment of the boundary is
  passed at most Ni times
• Theorem the length of the path verifies