Robotic Motion Planning: Bug Algorithms

Robotics Institute 16-735 http://www.cs.cmu.edu/~motion Howie Choset http://www.cs.cmu.edu/~choset

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Whats Special About Bugs

Many planning algorithms assume global knowledge Bug algorithms assume only local knowledge of the environment and a global goal Bug behaviors are simple:
1) Follow a wall (right or left) 2) Move in a straight line toward goal

Bug 1 and Bug 2 assume essentially tactile sensing Tangent Bug deals with finite distance sensing

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

A Few General Concepts

Workspace W
(2) or (3) depending on the robot could be infinite (open) or bounded (closed/compact)

Obstacle WOi Free workspace Wfree = W \ iWOi

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

The Bug Algorithms

Insect-inspired

provable results...

known direction to goal robot can measure distance d(x,y) between pts x and y

Goal

otherwise local sensing

walls/obstacles & encoders

reasonable world 1) finitely many obstacles in any finite area 2) a line will intersect an obstacle finitely many times 3) Workspace is bounded W Br(x), r < Br(x) = { y(2) | d(x,y) < r }

Start
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Buginner Strategy
Bug 0 algorithm
known direction to goal otherwise local sensing
walls/obstacles & encoders

Some notation: qstart and qgoal hit point qHi leave point qLi A path is a sequence of hit/leave pairs bounded by qstart and qgoal

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

how ?

Buginner Strategy
Bug 0 algorithm
known direction to goal otherwise local sensing
walls/obstacles & encoders

1) head toward goal 2) follow obstacles until you can head toward the goal again 3) continue

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

path ?

Buginner Strategy
Bug 0 algorithm
1) head toward goal 2) follow obstacles until you can head toward the goal again 3) continue

assume a leftturning robot The turning direction might be decided beforehand 16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

OK ?

Bug Zapper
What map will foil Bug 0 ? Bug 0 algorithm
1) head toward goal 2) follow obstacles until you can head toward the goal again 3) continue

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Bug Zapper
What map will foil Bug 0 ? Bug 0 algorithm
1) head toward goal 2) follow obstacles until you can head toward the goal again 3) continue
goal

start

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

A better bug?
But add some memory! known direction to goal otherwise local sensing
walls/obstacles & encoders

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

improvement ideas?

Bug 1
But some computing power! known direction to goal otherwise local sensing
walls/obstacles & encoders

Bug 1 algorithm
1) head toward goal 2) if an obstacle is encountered, circumnavigate it and remember how close you get to the goal 3) return to that closest point (by wall-following) and continue

Vladimir Lumelsky & Alexander Stepanov: Algorithmica 1987

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Bug 1
But some computing power! known direction to goal otherwise local sensing
walls/obstacles & encoders

Bug 1 algorithm
1) head toward goal 2) if an obstacle is encountered, circumnavigate it and remember how close you get to the goal 3) return to that closest point (by wall-following) and continue

Vladimir Lumelsky & Alexander Stepanov: Algorithmica 1987

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

BUG 1 More formally

Let qL0 = qstart; i = 1 repeat
repeat
from qLi-1 move toward qgoal

until goal is reached or obstacle encountered at qHi if goal is reached, exit repeat
follow boundary recording pt qLi with shortest distance to goal

until qgoal is reached or qHi is re-encountered if goal is reached, exit Go to qLi if move toward qgoal moves into obstacle
exit with failure

else
i=i+1 continue

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Quiz
Bug 1: Path Bounds

Bug 1 analysis
What are upper/lower bounds on the path length that the robot takes? D = straight-line distance from start to goal Pi = perimeter of the i th obstacle
Lower bound:
Whats the shortest distance it might travel?

P2
Upper bound:
Whats the longest distance it might travel?

P1
What is an environment where your upper bound is required?
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Quiz
Bug 1: Path Bounds

Bug 1 analysis
What are upper/lower bounds on the path length that the robot takes? D = straight-line distance from start to goal Pi = perimeter of the i th obstacle
Lower bound:
Whats the shortest distance it might travel?

P2
Upper bound:
Whats the longest distance it might travel?

D + 1.5 Pi
i

P1
What is an environment where your upper bound is required?
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

How Can We Show Completeness?

An algorithm is complete if, in finite time, it finds a path if such a path exists or terminates with failure if it does not. Suppose BUG1 were incomplete
Therefore, there is a path from start to goal
By assumption, it is finite length, and intersects obstacles a finite number of times.

BUG1 does not find it

Either it terminates incorrectly, or, it spends an infinite amount of time Suppose it never terminates
but each leave point is closer to the obstacle than corresponding hit point Each hit point is closer than the last leave point Thus, there are a finite number of hit/leave pairs; after exhausting them, the robot will proceed to the goal and terminate

Suppose it terminates (incorrectly) Then, the closest point after a hit must be a leave where it would have to move into the obstacle
But, then line from robot to goal must intersect object even number of times (Jordan curve theorem) But then there is another intersection point on the boundary closer to object. Since we assumed there is a path, we must have crossed this pt on boundary which contradicts the definition of a leave point. 16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Another step forward?

Call the line from the starting point to the goal the m-line

Bug 2 Algorithm

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

A better bug?
Call the line from the starting point to the goal the m-line

Bug 2 Algorithm
1) head toward goal on the m-line

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

A better bug?
Call the line from the starting point to the goal the m-line

Bug 2 Algorithm
1) head toward goal on the m-line 2) if an obstacle is in the way, follow it until you encounter the m-line again.

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

A better bug?
m-line

Bug 2 Algorithm
1) head toward goal on the m-line 2) if an obstacle is in the way, follow it until you encounter the m-line again. 3) Leave the obstacle and continue toward the goal

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

OK ?

A better bug?
Bug 2 Algorithm
1) head toward goal on the m-line Start 2) if an obstacle is in the way, follow it until you encounter the m-line again. 3) Leave the obstacle and continue toward the goal

Goal
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds NO! How do we fix this?

A better bug?
Bug 2 Algorithm
1) head toward goal on the m-line Start 2) if an obstacle is in the way, follow it until you encounter the m-line again closer to the goal. 3) Leave the obstacle and continue toward the goal

Goal

Better or worse than Bug1?

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

The Spiral

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

BUG 2 More formally

Let qL0 = qstart; i = 1 repeat
repeat
from qLi-1 move toward qgoal along the m-line

until goal is reached or obstacle encountered at qHi if goal is reached, exit repeat
follow boundary

until qgoal is reached or qHi is re-encountered or m-line is re-encountered, x is not qHi, d(x,qgoal) < d(qHi,qgoal) and way to goal is unimpeded if goal is reached, exit if qHi is reached, return failure else
qLi = m i=i+1 continue
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

head-to-head comparison
Bug 2 beats Bug 1

or thorax-to-thorax, perhaps

Draw worlds in which Bug 2 does better than Bug 1 (and vice versa). Bug 1 beats Bug 2

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

head-to-head comparison
Bug 2 beats Bug 1

or thorax-to-thorax, perhaps

Draw worlds in which Bug 2 does better than Bug 1 (and vice versa). Bug 1 beats Bug 2

?
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

head-to-head comparison
Bug 2 beats Bug 1

or thorax-to-thorax, perhaps

Draw worlds in which Bug 2 does better than Bug 1 (and vice versa). Bug 1 beats Bug 2

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

BUG 1 vs. BUG 2

BUG 1 is an exhaustive search algorithm
it looks at all choices before commiting

BUG 2 is a greedy algorithm

it takes the first thing that looks better

In many cases, BUG 2 will outperform BUG 1, but BUG 1 has a more predictable performance overall

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Quiz
Bug 2: Path Bounds

Bug 2 analysis
What are upper/lower bounds on the path length that the robot takes? D = straight-line distance from start to goal Pi = perimeter of the i th obstacle
Lower bound:
Whats the shortest distance it might travel?

Upper bound:
Whats the longest distance it might travel?

What is an environment where your upper bound is required?

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Quiz
Bug 2: Path Bounds

Bug 2 analysis
What are upper/lower bounds on the path length that the robot takes? D = straight-line distance from start to goal Pi = perimeter of the i th obstacle
Lower bound:
Whats the shortest distance it might travel?

Upper bound:
Whats the longest distance it might travel?

D+
i

ni
2

Pi

ni = # of s-line intersections of the

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

i th obstacle

What is an environment where your upper bound is required?

Quiz
Bug 2: Path Bounds

Bug 2 analysis
What are upper/lower bounds on the path length that the robot takes? D = straight-line distance from start to goal Pi = perimeter of the i th obstacle
Lower bound:
Whats the shortest distance it might travel?

Upper bound:
Whats the longest distance it might travel?

D+
i

ni
2

Pi

ni = # of s-line intersections of the

to

i th obstacle

What is an environment where your upper bound is required?

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

A More Realistic Bug

As presented: global beacons plus contact-based wall following The reality: we typically use some sort of range sensing device that lets us look ahead (but has finite resolution and is noisy). Let us assume we have a range sensor

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Raw Distance Function

Saturated raw distance function

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Intervals of Continuity
Tangent Bug relies on finding endpoints of finite, conts segments of R

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Motion-to-Goal Transition from Moving Toward goal to following obstalces

Transition

Currently, the motion-to-goal behavior thinks the robot can get to the goal Now, it starts to see something --- what to do? Ans: For any Oi such that d(Oi,qgoal) < d(x,qgoal), choose the pt Oi that minimizes d(x,Oi) + d(Oi,qgoal)
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Minimize Heuristic Example

At x, robot knows only what it sees and where the goal is,

so moves toward O2. Note the line connecting O2 and goal pass through obstacle

so moves toward O4. Note some thinking was involved and the line connecting O4 and goal pass through obstacle

For any Oi such that d(Oi,qgoal) < d(x,qgoal), choose the pt Oi that minimizes d(x,Oi) + d(Oi,qgoal)
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Motion To Goal Example

For any Oi such that d(Oi,qgoal) < d(x,qgoal), choose the pt Oi that minimizes d(x,Oi) + d(Oi,qgoal)

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Transition from Motion-to-Goal

Choose the pt Oi that minimizes d(x,Oi) + d(Oi,qgoal)

M is the point on the sensed obstacle which has the shorted distance to the goal

Problem: what if this distance starts to go up? Ans: start to act like a BUG and follow boundary

Followed obstacle: the obstacle that we are currently sensing Blocking obstacle: the obstacle that intersects the segment

They start as the same

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Transition from Motion-to-Goal

Choose the pt Oi that minimizes d(x,Oi) + d(Oi,qgoal)

M is the point on the sensed obstacle which has the shorted distance to the goal

Problem: what if this distance starts to go up? Ans: start to act like a BUG and follow boundary

Followed obstacle: the obstacle that we are currently sensing Blocking obstacle: the obstacle that intersects the segment

They start as the same

For any Oi such that d(Oi,qgoal) < d(x,qgoal), choose the pt Oi that minimizes d(x,Oi) + d(Oi,qgoal)
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Boundary Following

Move toward the Oi on the followed obstacle in the chosen direction

M is the point on the sensed obstacle which has the shorted distance to the goal

Followed obstacle: the obstacle that we are currently sensing Blocking obstacle: the obstacle that intersects the segment

Maintain dmin and dleave

They start as the same

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

d and d
min

leave

dmin is the shortest distance, observed thus far, between the sensed boundary of the obstacle and the goal dleave is the shortest distance between any point in the currently sensed environment and the goal

Terminate boundary following behavior when dleave < dmin Initialize with

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Example: Zero Senor Range

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

Robot moves toward goal until it hits obstacle 1 at H1 Pretend there is an infinitely small sensor range and the Oi which minimizes the heuristic is to the right Keep following obstacle until robot can go toward obstacle again Same situation with second obstacle At third obstacle, the robot turned left until it could not increase heuristic dmin is distance between M3 and goal, dleave is distance between robot and goal because sensing distance is zero

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Example: Finite Sensor Range

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Example: Infinite Sensor Range

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

dmin is constantly updated

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

The Basic Ideas

A motion-to-goal behavior as long as way is clear or there is a visible obstacle boundary pt that decreases heuristic distance A boundary following behavior invoked when heuristic distance increases. A value dmin which is the shortest distance observed thus far between the sensed boundary of the obstacle and the goal A value dleave which is the shortest distance between any point in the currently sensed environment and the goal Terminate boundary following behavior when dleave < dmin

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Tangent Bug Algorithm

1) repeat
a) Compute continuous range segments in view b) Move toward n {T,Oi} that minimizes h(x,n) = d(x,n) + d(n,qgoal)

until
a) goal is encountered, or b) the value of h(x,n) begins to increase

2) follow boundary continuing in same direction as before repeating a) update {Oi}, dleave and dmin until a) goal is reached b) a complete cycle is performed (goal is unreachable) c) dleave < dmin Note the same general proof reasoning as before applies, although the definition of hit and leave points is a little trickier.

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Implementing Tangent Bug

Basic problem: compute tangent to curve forming boundary of obstacle at any point, and drive the robot in that direction Let D(x) = minc d(x,c) c i WOi Let G(x) = D(x) - W* some safe following distance Note that G(x) points radially away from the object Define T(x) = ( G(x)) the tangent direction
in a real sensor (well talk about these) this is just the tangent to the array element with lowest reading

We could just move in the direction T(x)

open-loop control

Better is x = (T(x) - ( G(x)) G(x))

closed-loop control (predictor-corrector)

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Sensors!
Robots link to the external world...
sonar rangefinder

gyro

Sensors, sensors, sensors! and tracking what is sensed: world models

compass

IR rangefinder

sonar rangefinder

CMU cam with onboard processing

odometry

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Tactile sensors
on/off switch
as a low-resolution encoder

analog input: Active antenna

Surface acoustic waves

Capacitive array sensors

Resistive sensors

16-735, Howie Choset with slides from G.D. through Z. Dodds 100% of light passes through 90% of light passes Hager and 75% of light passes through

Tactile applications
Medical teletaction interfaces

haptics

daVinci medical system

Robotic sensing
Merritt systems, FL
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Infrared sensors
Noncontact bump sensor
(1) sensing is based on light intensity.

object-sensing IR

looks for changes at this distance

diffuse distance-sensing IR
(2) sensing is based on angle receved.

IR emitter/detector pair IR detector

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Infrared calibration
The response to white copy paper (a dull, reflective surface)

raw values (put into 4 bits)

inches 15 increments

in the dark

fluorescent light

incandescent light

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Infrared calibration

energy vs. distance for various materials

( the incident angle is 0, or head-on ) ( with no ambient light )
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Sonar sensing
single-transducer sonar timeline 0
a chirp is emitted into the environment

75s
typically when reverberations from the initial chirp have stopped

the transducer goes into receiving mode and awaits a signal... limiting range sensing

.5s
after a short time, the signal will be too weak to be detected

time response blanking time

Polaroid sonar emitter/receivers

No lower and Z. limit 16-735, Howie Choset with slides from G.D. HagerrangeDodds for paired sonars...

walls (obstacles)

Sonar effects

sonar

Draw the range reading that the sonar will return in each case

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

walls (obstacles)

Sonar effects

sonar

Draw the range reading that the sonar will return in each case

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

holding a sponge

Sonar effects
(a) Sonar providing an accurate range measurement (b-c) Lateral resolution is not very precise; the closest object in the beams cone provides the response (d) Specular reflections cause walls to disappear (e) Open corners produce a weak spherical wavefront (f) Closed corners measure to the corner itself because of multiple reflections --> sonar ray tracing
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

resolution: time / space

Sonar modeling
initial time response

accumulated responses

blanking time cone width

spatial response 16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Sonar Modeling
response model (Kuc)

Models the response, hR,with

c = speed of sound a = diameter of sonar element t = time z = orthogonal distance = angle of environment surface

sonar S reading z=

o obstacle

Then, allow uncertainty in the model to obtain a probability: p( S | o )

chance that the sonar reading is S, given an obstacle at location o

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Roman Kucs animals

Rodolph

Robat
16-735, Howie Choset with slides from G.D. Hager and Z. Dodds

Laser Ranging

LIDAR

Sick Laser

LIDAR map

Sick laser 16-735, Howie Choset with slides from G.D. Hager and Z. Dodds range finder

Summary
Bug 1: safe and reliable Bug 2: better in some cases; worse in others Should understand the basic completeness proof Tangent Bug: supports range sensing Sensors and control
should understand basic concepts and know what different sensors are

16-735, Howie Choset with slides from G.D. Hager and Z. Dodds