# CS计算机代考程序代写 arm algorithm CS 4610/5335

CS 4610/5335

Path and Motion Planning

Robert Platt Northeastern University 1/22/20

Material adapted from:

1. Lawson Wong, CS 4610/5335

2. Peter Corke, Robotics, Vision and Control

3. Marc Toussaint, U. Stuttgart Robotics Course 4. Oussama Khatib, Stanford CS 223A

5. Howie Choset

6. Kai Arras

Where we are so far

– Rotations and transformations – Forward kinematics

f (q) x

– Inverse kinematics

Input: x*

Output: q*

1. repeat until dx is small:

2. 3. 4. 5. 6. 7. 8.

init q to random joint configuration repeat K times:

x = FK(q)

dx = x*-x

dq = stepsize * J# dx q = q + dq

return q* = q

Problem we want to solve

Given:

– description of the robot

– description of the obstacle environment

Find:

– path from start to goal that does not result in a collision

Starting configuration

Goal configuration

Problem we want to solve

Given:

– a point-robot (robot is a point in space) – a start and goal configuration

Find:

– path from start to goal that does not result in a collision

Assumptions:

– the position of the robot can always be measured perfectly – the motion of the robot can always be controlled perfectly – the robot can move in any direction instantaneously

Heuristic Solutions

Bug algorithms:

– assume only local knowledge of the environment is available – simple behaviors: follow a wall; follow straight line toward goal

Wavefront planner (distance transform) Potential functions

First attempt: BUG 0

assume a left- turning robot

The turning direction might be decided beforehand…

BUG 0:

1. head toward goal

2. if hit a wall, turn left

3. follow wall until a line toward goal will move you away from wall.

(assume we only have local sensing – we cannot sense position of walls we are not touching)

Question

goal

start

What does BUG0 do here?

Second attempt: BUG 1

BUG 1:

1. move on straight line toward goal

2. if obstacle encountered, circumnavigate entire obstacle and remember how

close bug got to goal

3. return to closest point and continue on a straight line toward goal

Second attempt: BUG 1

BUG 1:

1. move on straight line toward goal

2. if obstacle encountered, circumnavigate entire obstacle and remember how

close bug got to goal

3. return to closest point and continue on a straight line toward goal

Question

goal

start

What does BUG1 do here?

m-line

Another bug: BUG 2

1. head toward goal on m-line

m-line

Another bug: BUG 2

1. head toward goal on m-line

2. if you encounter obstacle, follow it until you encounter m-line again at a point closer to goal

m-line

Another bug: BUG 2

1. head toward goal on m-line

2. if you encounter obstacle, follow it until you encounter m-line again at a point closer to goal

3. leave line and head toward goal again

Question

goal

start

What does BUG2 do here?

Wavefront planner (distance transform)

– intensity of a point denotes its (obstacle-respecting) distance from the goal

Wavefront planner (distance transform) Algorithm:

1. 2. 3. 4. 5. 6.

L: d:

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

list of nodes in wave front; initially just the goal state

distance function over nodes; initially zero everywhere except goal state

Wavefront planner (distance transform) Algorithm:

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

Wavefront planner (distance transform) Algorithm:

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

Wavefront planner (distance transform) Algorithm:

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

Wavefront planner (distance transform) Algorithm:

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

c

Wavefront planner (distance transform) Algorithm:

1. 2. 3. 4. 5. 6.

L={goal state}, d(goal state) = 2, d(obstacle states) = 1, d(rest of states) = 0 while L != null

pop item i from L

for all neighbors j of i such that d(j)==0

d(j) = d(i)+1 push j onto L

For node j, how many steps to goal state in terms of d(j)?

c

Complete? Optimal?

Questions

Guaranteed to find a solution if one exists? Does it find shorted path solutions?

c

Potential Functions

Potential Functions

Potential Functions

Potential Functions

After computing U, follow the negative gradient:

Potential Function Limitations

Bug trap

Heuristic Solutions

Bug algorithms:

– assume only local knowledge of the environment is available – simple behaviors: follow a wall; follow straight line toward goal

Wavefront planner (distance transform)

Potential functions

So far, these only work for 2-D path planning

or are not optimal / principled approximations

Problem we want to solve

Given:

– description of the robot

– description of the obstacle environment

Find:

– path from start to goal that does not result in a collision

Starting configuration

Goal configuration

Problem we want to solve

This problem statement is actually very general – manipulators

Problem we want to solve

This problem statement is actually very general – manipulators

– mobile robots

Approach: plan in “configuration space”

Convert the original planning problem into a planning problem for a single point.

workspace

Convert the original planning problem into a planning problem for a single point.

configuration space

Approach: plan in “configuration space”

workspace

Convert the original planning problem into a planning problem for a single point.

configuration space

Approach: plan in “configuration space”

Approach: plan in “configuration space” Conversion to c-space: Minkowski Sum

workspace

Convert the original planning problem into a planning problem for a single point.

configuration space

Approach: plan in “configuration space”

Approach: plan in “configuration space”

Convert the original planning problem into a planning problem for a single point.

workspace

configuration space

Original problem Equivalent problem:

– plan path for robot arm – plan path for a point

Approach: plan in “configuration space”

workspace

configuration space

Notice the axes!

Cartesian space! Joint angles!

Approach: plan in “configuration space”

workspace

configuration space

Every point here corresponds to a single robot configuration here

Approach: plan in “configuration space”

workspace

configuration space

Every point that intersects an obstacle here corresponds to an arm configuration that intersects an obstacle

Approach: plan in “configuration space”

workspace

configuration space

C-obstacles Free space

workspace

configuration space

Question

Which object is responsible for this c-obstacle?

Configuration space

The dimension of a configuration space is the minimum number of parameters needed to specify the configuration of the robot completely. – also called the number of “degrees of freedom” (DOFs)

Dimension = 3

Question

The dimension of a configuration space is the minimum number of parameters needed to specify the configuration of the robot completely. – also called the number of “degrees of freedom” (DOFs)

Dimension = ?

Formalization of the motion planning problem

Configuration space: Obstacle space: Free space:

Path: where must be continuous Collision-free path:

Formalization of the motion planning problem

Given:

– configuration space – free space

– start state

– goal region

Find:

– a collision-free path , such that and

Configuration-space Motion Planning

Visibility graphs

Voronoi diagrams

Exact cell decomposition Approximate cell decomposition

Method #1: Visibility Graphs

n = num of obstacle vertices

Method #2: Generalized Voronoi Diagram

Method #2: Generalized Voronoi Diagram

Method #2: Generalized Voronoi Diagram

Method #3: Exact Cell Decomposition

Method #3.1: Trapezoidal Decomposition

Method #3.2: Uniform Approximate Cell Decomposition

c

Uniform cell shape: e.g. wavefront planner

Method #3.3: Quadtrees

Non-Uniform cell shape: e.g. quadtree decomposition

2. 3. 4. 5. 6. 7. 8.

create coarse grid

collision-check G

for all occupied cells c in G:

delete c from G

subdivide c into four cells (sub) add sub into G

collision-check sub

Method #3.3: Quadtrees

define G = Decompose(G,resolution):

1. if G null:

Collision-check: check whether each cell is completely free or not

define FindPath(maxresolution):

1. for resolution = coarse to maxresolution:

2. 3. 4.

G = Decompose(G,resolution) if Check-for-path(G) == True:

Success!

Why do you think this method is called “quadtree”?

Method #3.3: Quadtrees

How deep is the decomposition shown here?

Method #3.4: Octomaps

Same as quadtrees, but in three dimensions…

Examples of solutions found using octomaps

Exact vs approximate cell decomposition

Key challenge: high dimensions and complex geometry of free space

None of the methods we have looked at so far scale well to manipulator path planning

– e.g. a 6-DOF UR5 arm

Methods studied so far: – visibility graphs

– Voronoi diagrams

– cell decomposition

– potential functions

Only work for small num of obstacles Only work for small dimensional spaces

Not complete

Approach: Plan in “configuration space”

Convert the original planning problem into a planning problem for a single point.

workspace

configuration space

Original problem Equivalent problem:

– plan path for robot arm – plan path for a point

Formalization of the motion planning problem

Configuration space: Obstacle space: Free space:

Path: where must be continuous Collision-free path:

Naive approach for constructing C-space obstacles

workspace configuration space

– Densely sample the space of all robot configurations

– For each config q, use known environment and robot geometric models

to construct the workspace shapes

– Apply standard collision checking routines to check if in collision

– If in collision, add to Cobs ; else add to Cfree

Naive approach for constructing C-space obstacles

Checking collision between two convex polyhedra is easy

All non-convex polyhedra can be decomposed into a union of convex polyhedra Approximate all other shapes as polyhedra / unions of spheres

Sampling-based Motion Planning

Two methods:

Probabilistic roadmaps (PRM) Rapidly-exploring random trees (RRT)

Probabilistic Roadmap (PRM)

75

This produces a roadmap

Shortest paths between roadmap nodes can be found using Dijkstra’s algorithm

To answer a motion planning query, connect init and target to closest node

76

77

Collision checking: Static configuration

Checking collision between two convex polyhedra is easy

All non-convex polyhedra can be decomposed into a union of convex polyhedra Approximate all other shapes as polyhedra / unions of spheres

Collision checking: Local path

Check for collisions between small straight-line segments in C-space

Collision checking: Local path

Form the swept volume of the polyhedra by taking the convex hull of two configurations

Collision checking: Local path

Form the swept volume of the polyhedra by taking the convex hull of two configurations

Note: This could be an approximation – depends on collision check resolution

Alternatives: – Connect all neighbors within some distance (called sPRM) – Connect to all possible points! (inefficient but exhaustive)

82

Does this algorithm work? Is it complete?

83

Is it optimal?

Does this algorithm work?

Is it complete? – depends on n Is it optimal?

84

Does this algorithm work?

Is it complete? – depends on n

Is it optimal? – depends on the graph, and the search algorithm

85

Probabilistic completeness of PRM

What guarantees do we have?

Probability of not finding a solution to a robustly feasible problem decreases exponentially with the number of vertices

86

PRM sampling strategies Problem: it may take a lot of samples to reach a fully connected graph

PRM sampling strategies

89

Gaussian sampler

So far, we have only discussed uniform sampling…

Problem: uniform sampling is not a great way to find paths through narrow passageways.

PRM Roadmap

C-obst C-obst

goal

C-obst

C-obst

start

90

Gaussian sampler

Gaussian sampler:

1. Sample points uniformly at random (as before)

2. For each sampled point, sample a second point from a Gaussian distribution centered at the first sampled point

3. Discard both samples if both samples are either free or in collision

4. Keep the free sample if the two samples are NOT both free or both in collision (that is, keep the sample if the free/collision status of the second sample is different from the first).

91

PRM sampling strategies

Rapidly Exploring Random Trees (RRTs)

Problems with PRM:

– two steps: graph construction, then graph search

– hard to apply to problems where edges are directed, i.e. kinodynamic problems

RRTs solve both of these problems:

– create a tree instead of graph: no graph search needed! – tree rooted at start or goal – edges can be directed

RRT Algorithm

RRT Algorithm

RRT Algorithm

RRT Algorithm

Strategies for finding q_near: – take closest vertex in tree

– check intermediate points at regular intervals and split edge

RRT Algorithm

Strategies for reaching q_new: – straight line in c-space

– some kind of local planner

If can’t reach q_new:

– just go as far as you can and create a new vertex there

RRT versus a naïve random tree

RRT

Naïve random tree

Growing the naïve random tree: 1. pick a node at random

2. sample a new node near it

3. grow tree from random node to new node

RRTs and

Bias toward large Voronoi regions

http://msl.cs.uiuc.edu/rrt/gallery.html

Biases

Bias toward larger spaces Bias toward goal

When generating a random sample, with some probability pick the goal instead of a random node when expanding

This introduces another parameter 5-10% is probably the right choice

● ●

RRT Algorithm

Kinodynamic planning with RRTs

So far, we have assumed that the system has no dynamics

– the system can instantaneously move in any direction in c-space – but what if that’s not true?

Consider the Dubins car:

– c-space: x-y position and velocity, angle

– control forward velocity and steering angle – plan a path through c-space with the corresponding control signals

where:

x_t – state (x/y position and velocity, steering angle) u_t – control signal (forward velocity, steering angle)

RRT Algorithm

Replace highlighted line with a feasible trajectory

– e.g., try a small set of feasible actions/controls starting from qnear ,

add trajectory that results in configuration closest to qtarget

Left-turn only forward car

Hovercraft with 2 Thusters

RRT probabilistic completeness

Notice that this is exactly the same bound as for PRM.

112

RRT does not find optimal paths

Does this algorithm work?

Is it complete? – depends on n

Is it optimal? – depends on the graph, and the search algorithm

114

Is there a version of RRT that is optimal?

Yes: RRG and RRT*

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Is there a version of RRT that is optimal?

Don’t just

connect to

Attempt to connect to every vertex within a radius r

RRG Properties

RRG is probabilistically complete and asympototically optimal.

But, why might RRT still be preferable to RRG?

RRT* Algorithm

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

RRT* Algorithm

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

RRT* Algorithm

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

Don’t just

connect to

Attempt to connect to every vertex within a radius r

Get position and cost of min-cost vertex in

Rewire parents of nodes in to go through if that’s faster

RRT* Algorithm

Path Smoothing

Paths produced by sample based planners are generally not smooth

– RRT* and PRM* converge to optimal paths in the limit, but it’s generally not possible to run these algorithms long enough to converge.

Smoothing the path

127

Smoothing the path

128

Smoothing the path

129

Smoothing the path

130

Smoothing the path

131

Smoothing the path

132

Smoothing the path

133

Smoothing the path

134

Smoothing the path

135

Smoothing the path

136

Smoothing the path

137

Smoothing the path

138

139

140