Upload
razi
View
28
Download
1
Embed Size (px)
DESCRIPTION
EXAMPLES OF PRACTICAL ALGORITHMS FOR PATH PLANNING. Bug Algorithms. Bug 2. H j. L j. Bugs 2. q target. M-line. New leave point condition: d
Citation preview
EXAMPLES OF EXAMPLES OF PRACTICAL PRACTICAL
ALGORITHMS ALGORITHMS FOR PATH FOR PATH PLANNING PLANNING
Bug Bug AlgorithmsAlgorithms
Bug 2Bug 2
Bugs 2
qinit
qtargetM-line
Lj
Hj
New leave point condition: d<d(Hj,Target)
qinit
qtargetM-line
Lj
Hj
New leave point condition: d<d(Hj,Target)1. From point Lj-1 move along M-line until:
a. Target is reached. Stop
b. An obstacle is hit at Hj. Goto 2
2. Turn left and follow the boundary until:a. Target is reached. Stopb. M-line met at distance d from target such that:
d < dist(Hj,qtarget)Define Lj, set j=j+1, and goto 1.
c. If we return to Hj without meeting the M-line, stop. The target is trapped.
Bug 2 AlgorithmBug 2 Algorithm
1. From point Lj-1 move along M-line until:a. Target is reached. Stop
b. An obstacle is hit at Hj. Goto 2
2. Turn left and follow the boundary until:a. Target is reached. Stopb. M-line met at distance d from target such that:
d < dist(Hj,qtarget)Define Lj, set j=j+1, and goto 1.
c. If we return to Hj without meeting the M-line, stop. The target is trapped.
A HardHard Example for Bug 2
A HardHard Example for Bug 2
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
Bug + Non Contact Sensor
Probabilistic Probabilistic RoadmapsRoadmaps
Probabilistic Roadmaps Probabilistic Roadmaps 1. Path planning algorithm are expensive. Search on small grid cannot
be used.
2. If #DOFs’ is large, deterministic algorithms are not effective
3. Solution: use a probabilisticprobabilistic roadmap planner (PRM).
4. PRM are completecomplete in a probabilistic sense
5.5. An heuristic plannerAn heuristic planner: • potential fields.
1. Disadvantage: • local minima.
2. Solution: • randomize to escape local minima
Description of Description of Probabilistic Roadmap Probabilistic Roadmap Algorithm Algorithm
1. Roadmap = undirected graph R = R = ((N, EN, 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
Preprocessing PhaseThree stages
1. Roadmap construction. Objectives:a) Obtain reasonable connected graph
b) Be sure “difficult regions contain a few nodes
2. Roadmap expansion. Objectives:Improve graph connectivity by selecting nodes of R
which lie in (heuristic) difficult regions and adding nodes there
3. Roadmap Component reduction. Optional. Attempts to simplify the graph
Roadmap Construction1. 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
Create random configurationsAS a first step we create random configurations in the space.The algorithm creates the points one at a time, but we’re 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)
Update Neighboring Nodes’ Edges
As edges are added to the graph we start forming connected regions.
Connected nodes
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 difficult areas of the space.
Expansion Step
The nodes are added and will be connected again using the local planner on the closest nodes.
End of Expansion Step
At the end of the expansion step we end up with the same number, or fewer connected components.
Select start and goal
Start Goal
We represent our start and end goal
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.
Find the Path from Start to Goal
Start Goal
Now simply search for a path.
Roadmap construction algorithmalgorithm1. Nø , E ø 2. Loop3. q randomly selected free configuration
4. Nq a set of candidate neighbors of q from N5. N = N {q}
6. q’ NNqq in order of increasing D(q,q’ ), do7. If q, q’ are not in the same connected comp. and
(q,q’) =1, then E E {(q, q’ )}8. Update R’s connected components
How?
Algorithm?
Meaning?
Select
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
Ideas for the Local Planner
1. Deterministic vs. Non-deterministic planner
2. Powerful planer = slower planner
3.3. Empirical resultsEmpirical 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
Distance Function DD
• DD 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’)=maxxRobot||x(q)-x(q’)||
Roadmap Expansion1.1. NN should be a fair enough covering of Cfree
2.2. RR 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
End of slides