L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo...
Transcript of L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo...
![Page 1: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/1.jpg)
L22. A trip through the sensor zoo
EECS568 Mobile Robotics: Methods and PrinciplesProf. Edwin Olson
Monday, November 28, 11
![Page 2: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/2.jpg)
Kinematics• The study of motion without
regard to cause/forces
‣ As opposed to dynamics: study of motion with regard to forces.
• Examples
‣ If my robot arm’s joints are 30 deg and -60 deg, where will my gripper be?
‣ If my robot’s wheels are pointed at 12 degrees, what direction will the robot go?
30
-60
Monday, November 28, 11
![Page 3: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/3.jpg)
Kinematics: Position
• Chains of actuators ==> Chains of rigid-body transformations
• Express P in coordinate frame of elbow
θ1=30
θ1=-60 L1
L2
p
Monday, November 28, 11
![Page 4: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/4.jpg)
Kinematics: Velocity• How do you suppose we get velocity?
‣ Derivative of position WRT time
• Example: What is velocity of p (dp/dt) if dθ/dt = K?
How does this extend if multiple quantities are time-varying? (Repeat this process for each DOF that is moving, add results. Velocity is linear).
θ1=30
θ1=-60 L1
L2
p
Monday, November 28, 11
![Page 5: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/5.jpg)
Kinematics: Locomotion• How will our robot move if our wheels are
pointed?
• General assumption: assume infinite lateral holding force. (No sideways skidding)
‣ Wheels always move tangent to some circle
θ
Monday, November 28, 11
![Page 6: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/6.jpg)
Differential Drive• Two driven wheels plus a
passive caster
• Mechanically simple, robust. Very common!
• Basic geometry
• Solve for θ
dL
dR
dB
rL
Monday, November 28, 11
![Page 7: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/7.jpg)
Tank Drive• No way to make all
wheels be on tangents of circles with same origin
‣ Wheels must slip.
‣ Lots of torque required
Monday, November 28, 11
![Page 8: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/8.jpg)
Bicycle Model• Each wheel moves along a circle
‣ Not necessarily the same circle
‣ But they have the same center
• Total “straight” angle = π
• Total triangle angle
• We can measure the baseline
• Pick back wheel as frame of reference
‣ Solve for r1
θ
r1
r2
d
π/2-θ
θ
Monday, November 28, 11
![Page 9: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/9.jpg)
Ackerman Vehicle• In a car, front two wheels must be at different angles in order
for all wheels to be on circles with a common origin.
• Main complication for us:
‣ Curvature determined by front wheels
‣ Coordinate system usually defined by back wheels
- Extra geometric busy-work.
Monday, November 28, 11
![Page 10: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/10.jpg)
Dubins Model
• A car that can only go forward and turn
‣ (No reverse)
• Shortest path between two points is always a combination of minimum turning radius curves and straight lines
‣ (Ignoring obstacles, naturally)
Monday, November 28, 11
![Page 11: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/11.jpg)
Dubins
Monday, November 28, 11
![Page 12: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/12.jpg)
Omni/Mecanum/Swedish Drive
• Idea: instead of “infinite” lateral friction force, how about zero?
‣ Allow wheels to slip so that robot can move in any direction
‣ Wheels on top of wheels
• Kinematics:
‣ Think force diagram
Monday, November 28, 11
![Page 13: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/13.jpg)
Rocker-Bogie• NASA’s favorite design
• Front pair/rear pair can turn in place. (Ackerman type geometry: single origin but with 6 wheels.)
• Rockers and bogies freely pivot
‣ Good obstacle handling
‣ No dampening-- gotta go slow!
• All 6 wheels stay on the ground
• Motion of robot chassis is minimized
Monday, November 28, 11
![Page 14: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/14.jpg)
Monday, November 28, 11
![Page 15: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/15.jpg)
Inverse Kinematics• Kinematics:
‣ If joint angles are known, where is end-effector?
‣ If the wheels are angled, which direction will we go?
• Inverse-Kinematics
‣ If I want the end effector to be at point pG, how do I pick the joint angles?
‣ If I want to drive on a path with curvature K, what angle do I point the wheels?
Monday, November 28, 11
![Page 16: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/16.jpg)
Inverse Kinematics
• Sometimes it’s easy!
• Just solve for θ:
• The transformation is invertible.
θ
r1
r2
d
π/2-θ
θ
Monday, November 28, 11
![Page 17: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/17.jpg)
Inverse Kinematics
• Sometimes it’s hard.
‣ Multiple solutions
‣ Singularities
‣ Forbidden states
- Joint limits
- Self collisions
Monday, November 28, 11
![Page 18: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/18.jpg)
Geometrical Solutions
• In some cases, possible to solve inverse kinematics using geometry
• For this arm, can be zero, one, or two solutions.
θ1
θ2
θ1
θ2
Monday, November 28, 11
![Page 19: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/19.jpg)
Inverse Kinematics
Monday, November 28, 11
![Page 20: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/20.jpg)
Inverse Kinematics: Numerical Approach
• Consider error of end-effector position
• Even for “simple” systems, often highly non-linear
‣ Gradient descent
‣ Compute numerically
θ1
θ2 Goal
Error
Monday, November 28, 11
![Page 21: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/21.jpg)
Gradient Descent
x
Monday, November 28, 11
![Page 22: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/22.jpg)
Motion Planning• Inverse Kinematics: Determine actuator
commands that achieve a desired robot pose
• Motion Planning: Computing a series of actuator commands that put the robot into the desired state.
‣ State can include the robots position in the world
‣ Superset of inverse kinematics
‣ But often applied to simpler vehicles.
Monday, November 28, 11
![Page 23: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/23.jpg)
Motion Planner Properties
• Complete
‣ Finds a path if one exists
‣ “Doesn’t get stuck”
• Optimality
‣ Finds the best path
‣ Computational complexity irrelevant
Monday, November 28, 11
![Page 24: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/24.jpg)
Simple motion planning with “bug” algorithms
• Algorithm 0
‣ Drive towards goal
‣ If an obstacle gets in the way, follow the obstacle until we can once again drive towards the goal.
Monday, November 28, 11
![Page 25: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/25.jpg)
The bug is buggy
We could pick a direction randomly, but today is about deterministic planners.
Monday, November 28, 11
![Page 26: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/26.jpg)
A Better Bug• Algorithm 1
• Drive towards goal
• If an obstacle gets in the way, circumnavigate the entire obstacle.
• Note the point of closest approach to the goal
• Back track to closest point
• Loop
Monday, November 28, 11
![Page 27: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/27.jpg)
Bug 1• This bug is special: it’s provably complete.
‣ Definitions: Let d() be the distance to goal. Let entryi be the point at which we encountered the ith obstacle, exiti is the point at which we left that obstacle.
• Lemma1: The bug leaves every obstacle at a point no farther from the goal than the point it arrived at. I.e., d(exiti) <= d(entryi).
• Proof: Both d(entryi) and d(exiti) belong to the perimeter, and d(exiti) is the closest point to the goal.
• Lemma 2: d(entryi+1) < d(exiti)
• Proof: The robot heads directly towards the goal after exiting obstacle i. The obstacles do not overlap, and so the robot makes finite progress towards the goal before hitting another obstacle.
• Thm: Bug 1 is complete.
• Proof: The sequence {d(entry1), d(exit1), d(entry2), d(exit2), d(entry3), d(exit3), …} is monotonically decreasing by Lemma 1 and 2.
Monday, November 28, 11
![Page 28: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/28.jpg)
Bug 1 Summary• Complete?
• Optimal?
• Best case runtime of Bug 1
• Worst case runtime of Bug 1
complete: yesOptimal: no
P_i = perimeter of ith obstacle.
Monday, November 28, 11
![Page 29: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/29.jpg)
Bug 1 Summary• Complete?
• Optimal?
• Best case runtime of Bug 1
• Worst case runtime of Bug 1
complete: yesOptimal: no
P_i = perimeter of ith obstacle.
Monday, November 28, 11
![Page 30: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/30.jpg)
Bug 1 Summary• Complete?
• Optimal?
• Best case runtime of Bug 1
• Worst case runtime of Bug 1
complete: yesOptimal: no
P_i = perimeter of ith obstacle.
Monday, November 28, 11
![Page 31: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/31.jpg)
Bug 2• Algorithm 1
‣ Construct “m line”
‣ Drive toward goal on “m line”
‣ If an obstacle gets in the way, begin to circumnavigate the entire obstacle.
- When we encounter the m-line again closer to the goal, leave the obstacle and drive towards goal on m line.
• Loop
Monday, November 28, 11
![Page 32: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/32.jpg)
Bug 2 Challenge
• Bug 2 seems to be much better than bug 1.
• It turns out that it is not always better.
• Challenge
‣ Find a world in which Bug 1 outperforms Bug 2.
Monday, November 28, 11
![Page 33: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/33.jpg)
Bug Challenge: Solution
Monday, November 28, 11
![Page 34: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/34.jpg)
Bug 2 Summary• Complete?
• Optimal?
• Best case runtime of Bug 2
• Worst case runtime of Bug 2
complete: yesOptimal: no
n_i = # of times “m-line” intersects polygon. At most n_i/2 of these can be valid exit points. Each time, we might go all the way around the obstacle.
Hard to compare bug2 and bug1 directly, since complexity depends on the number of obstacles they encounter, which may not be the same. But if same # of obstacles, bug2 is better for convex polygons (where ni=2).
Monday, November 28, 11
![Page 35: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/35.jpg)
Planning on a (known) grid• Assume we know the whole world.
‣ Obviously, we can do better than a bug algorithm!
• Suppose robot can move in any direction
Goal
Start Monday, November 28, 11
![Page 36: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/36.jpg)
State-space search
(2,2)
(2,3) (1,2) (2,3) (2,1)
(4,3)
(3,3)
Up Left Right Down
Up Left Right Down
ULRD ULRD ULR D
Monday, November 28, 11
![Page 37: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/37.jpg)
Depth-First Search• Recursively explore each
action until no additional actions are possible.
• In many problems, we can always do something
‣ Infinite search depth
• Complete?
• Optimal?
(2,4)
(2,2)
Up Left Right Down
(2,3)
(2,5)
Monday, November 28, 11
![Page 38: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/38.jpg)
Breadth-First Search• Expand all action
sequences of depth n before considering sequences of depth n+1
• Complete?
• Optimal?
• Complexity?
(2,2)
Up Left Right Down
(2,3) (2,3) (2,3) (2,3)
Monday, November 28, 11
![Page 39: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/39.jpg)
Informed Search
• Some of these paths are getting farther away from the goal!
‣ Why search a bad solution when a better possibility exists?
Monday, November 28, 11
![Page 40: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/40.jpg)
A*root = new node();root.state = initialstate; root.parent = null;root.cost-so-far = 0;root.cost-to-go = h(root.state);
fringe = { root }
do foreverparent = get node from fringe with minimum cost-so-far + min-cost-to-go
if parent.state == goalstate return solution parent;
for each action: child = new node(); child.state = propagate(parent.state); child.parent = parent; child.cost-so-far = parent.cost-so-far + cost(action); child.min-cost-to-go = h(child.state); child.action = action;
add child to fringeend for
end do
node {
state state; node parent; double cost-so-far; double min-cost-to-go; action theaction;
}
Monday, November 28, 11
![Page 41: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/41.jpg)
A* Optimality Proof• Thm: A* is optimal
• Proof: (By Contradiction) Suppose that A* computes a sub-optimal answer x. This means that cost(x) > cost(x’) for some other x’.
• We know that some prefix p of x’ exists in the fringe. Since the heuristic never over-estimates the cost to goal, we have cost-so-far(p) + cost-to-go(p) <= cost(x’).
• Since nodes are removed in order of least total cost, node p will be expanded before node x. Thus, we never expand a sub-optimal node.
Monday, November 28, 11
![Page 42: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/42.jpg)
A* Summary• Pros:
‣ Complete
‣ Optimal
‣ Optimally Efficient
‣ Easy to implement
‣ Works well when number of actions is small
‣ Size of state space does not directly search complexity
• Cons:
‣ Worst-case cost no better than breadth-first: O(bd)
‣ Large memory costs: O(bd)
‣ Large computational costs
‣ Even with a MinHeap
‣ Can be exceptionally slow if the optimal solution has large cost and there are many good looking (but ultimately futile) paths
Monday, November 28, 11
![Page 43: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/43.jpg)
Wavefront Planning• Searching in along possible action sequences can be
wasteful
‣ Same cells are visited over and over again.
• Idea: Compute best path from every node to the goal
‣ This is trivial for the goal node
‣ It’s trivial for the nodes adjacent to the goal node
‣ It’s trivial for the nodes adjacent to the nodes adjacent to the goal node
‣ It’s trivial for nodes (n+1) away from the goal, given the best path for nodes (n) away from the goal.
Monday, November 28, 11
![Page 44: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/44.jpg)
Wavefront: Algorithm
wavefront = { goal }
for all nodes n
dist(n) = infinity
end for
dist(goal) = 0
do
newwavefront = { }
for each node n in wavefront
for each reachable neighbor n’ of n
dist(n’) = min(dist(n’), dist(n) + cost(n, n’))
newwavefront = { newwavefront, n’ }
end for
end for
wavefront = newwavefront
until (wavefront contains start node)
The algorithm computes cost-to-goal at every grid cell. Once computed, do
gradient descent!
Monday, November 28, 11
![Page 45: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/45.jpg)
Monday, November 28, 11
![Page 46: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/46.jpg)
Monday, November 28, 11
![Page 47: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/47.jpg)
Monday, November 28, 11
![Page 48: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/48.jpg)
Monday, November 28, 11
![Page 49: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/49.jpg)
Wavefront, A*, Dijkstra• In EECS492, you might be familiar with two variants of A*
‣ Tree Search
‣ Graph Search
- Only first path to any particular state is retained
• A* (GraphSearch) is essentially Wavefront
• Dijkstra’s Algorithm is a (slight) generalization of Wavefront
‣ If different cells have different costs, then wavefront doesn’t propagate at uniform speed.
‣ Don’t literally maintain a wavefront; maintain a front heap
• Recommendation:
‣ Dijkstra is the best way to go in almost all cases
Monday, November 28, 11
![Page 50: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/50.jpg)
Configuration Space• What if the robot takes up more than one grid
cell?
• We could check for obstacles anywhere under the robot’s “footprint”
‣ Lots of collision tests!
• Can we pre-compute the set of acceptable locations?
Monday, November 28, 11
![Page 51: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/51.jpg)
Configuration Space• Conventional collision checking using
free space
‣ Must check for collisions at every position occupied by the robot
• Configuration space
‣ Precompute collision test for every state of the robot
- Can be easily computed via convolution of obstacles with robot footprint
‣ Collision check requires testing configuration space at a single point
Robot
Obstacle map
Must check entire robot footprint for collisions
Collision check: only check a single point in configuration space
Monday, November 28, 11
![Page 52: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/52.jpg)
Configuration Space• What if our vehicle is not radially symmetric?
‣ I.e., legality of a position depends not just on (x,y) but also θ?
• No problem!
‣ Expand configuration space into third dimension
- Pre-compute collision for all (x,y,θ) tuples.
- Um, wait….
Legal configuration Illegal configuration
Monday, November 28, 11
![Page 53: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/53.jpg)
Approximate Configuration Spaces
• Solution 1: Use bounding circle of robot as footprint
‣ Overly conservative
‣ Collision checks = single point test in configuration space (fast).
Obstacle
Monday, November 28, 11
![Page 54: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/54.jpg)
Approximate configuration spaces
• Exploit kinematic constraints
‣ Cars can only drive in a (mostly) straight line.
‣ We need to perform collision tests for vehicle trajectories, not just stationary vehicles.
‣ For a car, the collision test for the trajectory is coincident with the collision test for the vehicle body.
- I.e., the same line test used to test trajectories for collisions can compute the collision test for the vehicle body “for free”!
- (Actually, we have to extend the trajectory by the length of the car.)
• Huge win: Only need a 2D configuration space. Get speed and most of the accuracy of a 3D configuration space.
With a 3D configuration space, we’d have to search along this line
With a 2D configuration space, we have to search an additional car length.
Monday, November 28, 11
![Page 55: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/55.jpg)
Multi-valued configuration spaces
• The “goodness” of a path is complex
‣ Not just the minimum distance path
‣ Not just a path that avoids obstacles
• For example:
‣ Avoid obstacles by as much of a margin as possible (why?)
‣ Well, not so much that we leave our lane
‣ Well, maybe we shouldn’t run over the pedestrian either.
‣ We have trade-offs to make. How do we express them?
Avoid obstacles with margin because: 1) humans do it, and 2) our sensors have noise. Without prefering some margin, we might plan a near-miss, only to have the obstacle position estimate jump around a bit. That will invalidate the plan and can cause us to have to recompute plans more often than desirable.
Monday, November 28, 11
![Page 56: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/56.jpg)
Multi-valued cost functions
position
cost
Obstacle
Infeasible High cost
Low cost
position
cost
right lane
Infeasible High cost
Low cost
passing lane
Monday, November 28, 11
![Page 57: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/57.jpg)
Real-world cost maps• Take all your cost
functions, add/max them up!
• Planner minimizes the integral of cost along trajectory
‣ Includes a total distance component
‣ Includes penalties for “close calls”
Lane cost function
Obstacle cost function
Monday, November 28, 11
![Page 58: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/58.jpg)
Approximate configuration spaces
• Step 1: Build configuration space for a useful shape
‣ E.g., a circle whose diameter = width of car
• A collision test now becomes a line test in configuration space
• Only slightly conservative, at the expense of more configuration space testing.
Monday, November 28, 11
![Page 59: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/59.jpg)
Handling continuous search spaces• Continuous action space makes exact answer intractable (in general)
• We can sometimes consider only a few discrete actions
‣ Consider “nudges” and “veers”
• Shortcomings
‣ Algorithm is no longer complete.
‣ Paths are not optimal
describe stanford parking planner
two heuristics- holonomic vehicle avoiding obstcles-non-holonomic vehicle without obstacles
Both are admissible...
Monday, November 28, 11
![Page 60: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/60.jpg)
Non-deterministic Planning• Consider sequences of random actions
‣ Build a tree of actions
- Node = state
- Edge = action
‣ A plan is a sequence of actions, i.e., a path from the root (initial state) to a leaf near the goal
• Skeleton algorithm:
‣ while time remains
- Select a node (call it parent) in the tree
- Generate a new action a
- Create new node: child = propagate(parent, a)
- If action is safe
• Add node to tree
‣ return best plan found so far
Implementation here is critical
Monday, November 28, 11
![Page 61: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/61.jpg)
Taking into account kinematic and dynamic constraints
• These forward-search methods can easily take into account kinematic and dynamic constraints
‣ Action propagation can correspond to arbitrary non-linear transformation
‣ Typical: run differential equations forward
Monday, November 28, 11
![Page 62: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/62.jpg)
Random Tree Growth
• Random: Pick a parent node at random. Pick an action at random.
Monday, November 28, 11
![Page 63: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/63.jpg)
Random Policy
• Complete?
• Optimal?
• Practical?
Complete: yes, as time -> infinityOptimal: yes, as time -> infinityPractical: not a chance.
Monday, November 28, 11
![Page 64: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/64.jpg)
RRT Policy• RRT: Pick a destination at random. Find parent node that is
closest to destination. Compute action that goes towards destination
Monday, November 28, 11
![Page 65: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/65.jpg)
RRT-Biased Policy• RRT-Biased: Pick the destination randomly, but in a biased
way (i.e., prefer directions that are heuristically likely to work out)
Monday, November 28, 11
![Page 66: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/66.jpg)
RRT: Trunking• State of tree at ti limits
state of tree at ti+1
• New edges always connect closest part of tree: never generate a completely new route to an area we already have a route to
• How could you address this?
Run RRT several times: get different “trunks” each time.
Monday, November 28, 11
![Page 67: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/67.jpg)
RRT: Analysis
• Complete?
• Optimal?
• Practical?
Complete: NoOptimal: NoPractical: sure, maybe.
Monday, November 28, 11
![Page 68: L22. A trip through the sensor zoo - University of Michigan...L22. A trip through the sensor zoo EECS568 Mobile Robotics: Methods and Principles Prof. Edwin Olson Monday, November](https://reader033.fdocuments.us/reader033/viewer/2022060222/5f07a0c07e708231d41deeb1/html5/thumbnails/68.jpg)
RRT on an Arm
Monday, November 28, 11