l22. a trip through the sensor zoo - university of michigan...l22. a trip through the sensor zoo...

L22. A trip through the sensor zoo

EECS568 Mobile Robotics: Methods and PrinciplesProf. Edwin Olson

Monday, November 28, 11

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


Kinematics: Position

• Chains of actuators ==> Chains of rigid-body transformations

• Express P in coordinate frame of elbow

θ1=30

θ1=-60 L1

L2

p


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


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

θ


Differential Drive• Two driven wheels plus a

passive caster

• Mechanically simple, robust. Very common!

• Basic geometry

• Solve for θ

dL

dR

dB

rL


Tank Drive• No way to make all

wheels be on tangents of circles with same origin

‣ Wheels must slip.

‣ Lots of torque required


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-θ

θ


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.


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)


Dubins


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


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


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?


Inverse Kinematics

• Sometimes it’s easy!

• Just solve for θ:

• The transformation is invertible.

θ

r1

r2

d

π/2-θ

θ


Inverse Kinematics

• Sometimes it’s hard.

‣ Multiple solutions

‣ Singularities

‣ Forbidden states

- Joint limits

- Self collisions


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


Inverse Kinematics


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


Gradient Descent

x


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.


Motion Planner Properties

• Complete

‣ Finds a path if one exists

‣ “Doesn’t get stuck”

• Optimality

‣ Finds the best path

‣ Computational complexity irrelevant


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.


The bug is buggy

We could pick a direction randomly, but today is about deterministic planners.


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


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.


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.


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


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.


Bug Challenge: Solution


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).


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

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


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)


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)


Informed Search

• Some of these paths are getting farther away from the goal!

‣ Why search a bad solution when a better possibility exists?


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;

}


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.


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


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.


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!


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


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?


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


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


Approximate Configuration Spaces

• Solution 1: Use bounding circle of robot as footprint

‣ Overly conservative

‣ Collision checks = single point test in configuration space (fast).

Obstacle


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.


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.


Multi-valued cost functions

position

cost

Obstacle

Infeasible High cost

Low cost

position

cost

right lane

Infeasible High cost

Low cost

passing lane


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


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.


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...


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


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


Random Tree Growth

• Random: Pick a parent node at random. Pick an action at random.


Random Policy

• Complete?

• Optimal?

• Practical?

Complete: yes, as time -> infinityOptimal: yes, as time -> infinityPractical: not a chance.


RRT Policy• RRT: Pick a destination at random. Find parent node that is

closest to destination. Compute action that goes towards destination


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)


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.


RRT: Analysis

• Complete?

• Optimal?

• Practical?

Complete: NoOptimal: NoPractical: sure, maybe.


RRT on an Arm


l22. a trip through the sensor zoo - university of michigan...l22. a trip through the sensor zoo...

Documents