1. Probabilistic Techniques for Mobile Robot Navigation
Wolfram Burgard
University of Freiburg
Department of Computer Science
Autonomous Intelligent Systems
Special Thanks to Frank Dellaert, Dieter Fox, Giorgio Grisetti, Dirk Haehnel, Cyrill Stachniss, Sebastian Thrun, …

2. Probabilistic Robotics
              

3. Robotics Today

4. Overcoming the uncanny valley
“Humanoids”
Overcoming the uncanny valley
[Courtesy by Hiroshi Ishiguro]

5. Humanoid Robots
[Courtesy by Sven Behnke]

6. DARPA Grand Challenge
[Courtesy by Sebastian Thrun]

7. DCG 2007

8. Robot Projects: Interactive Tour-guides
Rhino:
Albert:
Minerva:

9. Minerva

10. Robot Projects: Acting in the Three-dimensional World
Herbert:
Zora:
Groundhog:

11. Nature of Data
Range Data
Odometry Data

12. Probabilistic Techniques in Robotics
Perception = state estimation
Action = utility maximization
Key Question
How to scale to higher-dimensional spaces

13. Axioms of Probability Theory
Pr(A) denotes probability that proposition A is true.

14. A Closer Look at Axiom 3 B

15. Using the Axioms

16. Discrete Random Variables
X denotes a random variable.
X can take on a countable number of values in {x1, x2, …, xn}.
P(X=xi), or P(xi), is the probability that the random variable X takes on value xi.
P( ) is called probability mass function.
E.g. .

17. Continuous Random Variables
X takes on values in the continuum.
p(X=x), or p(x), is a probability density function.
E.g.
p(x) x

18. Joint and Conditional Probability
P(X=x and Y=y) = P(x,y)
If X and Y are independent then P(x,y) = P(x) P(y)
P(x | y) is the probability of x given y P(x | y) = P(x,y) / P(y) P(x,y) = P(x | y) P(y)
If X and Y are independent then P(x | y) = P(x)

19. Law of Total Probability, Marginals
Discrete case
Continuous case

20. Bayes Formula

21. Normalization
Algorithm:

22. Conditioning
Law of total probability:

23. Bayes Rule with Background Knowledge

24. Conditioning
Total probability:

25. Conditional Independence
Equivalent to
and

26. Simple Example of State Estimation
Suppose a robot obtains measurement z
What is P(open|z)?

27. Causal vs. Diagnostic Reasoning
P(open|z) is diagnostic.
P(z|open) is causal.
Often causal knowledge is easier to obtain.
Bayes rule allows us to use causal knowledge:
count frequencies!

28. Example P(z|open) = 0.6 P(z|open) = 0.3 P(open) = P(open) = 0.5
z raises the probability that the door is open.

29. Combining Evidence Suppose our robot obtains another observation z2.
How can we integrate this new information?
More generally, how can we estimate P(x| z1...zn )?

30. Recursive Bayesian Updating
Markov assumption: zn is independent of z1,...,zn-1 if we know x.

31. Example: Second Measurement
P(z2|open) = 0.5 P(z2|open) = 0.6
P(open|z1)=2/3
z2 lowers the probability that the door is open.

32. A Typical Pitfall Two possible locations x1 and x2 P(x1)=0.99
P(z|x2)=0.09 P(z|x1)=0.07

33. Often the world is dynamic since
Actions
Often the world is dynamic since
actions carried out by the robot,
actions carried out by other agents,
or just the time passing by
change the world.
How can we incorporate such actions?

34. Typical Actions The robot turns its wheels to move
The robot uses its manipulator to grasp an object
Plants grow over time…
Actions are never carried out with absolute certainty.
In contrast to measurements, actions generally increase the uncertainty.

35. Modeling Actions
To incorporate the outcome of an action u into the current “belief”, we use the conditional pdf
P(x|u,x’)
This term specifies the pdf that executing u changes the state from x’ to x.

36. Example: Closing the door

37. State Transitions
P(x|u,x’) for u = “close door”:
If the door is open, the action “close door” succeeds in 90% of all cases.

38. Integrating the Outcome of Actions
Continuous case:
Discrete case:

39. Example: The Resulting Belief

40. Bayes Filters: Framework
Given:
Stream of observations z and action data u:
Sensor model P(z|x).
Action model P(x|u,x’).
Prior probability of the system state P(x).
Wanted:
Estimate of the state X of a dynamical system.
The posterior of the state is also called Belief:

41. Markov Assumption Underlying Assumptions Static world
Independent noise
Perfect model, no approximation errors

42. Bayes Filters z = observation u = action x = state Bayes Markov
Total prob.
Markov
Markov

43. Bayes Filter Algorithm
Algorithm Bayes_filter( Bel(x),d ):
h=0
If d is a perceptual data item z then
For all x do
Else if d is an action data item u then
Return Bel’(x)

44. Bayes Filters are Familiar!
Kalman filters
Discrete filters
Particle filters
Hidden Markov models
Dynamic Bayesian networks
Partially Observable Markov Decision Processes (POMDPs)

45. Summary
Bayes rule allows us to compute probabilities that are hard to assess otherwise.
Under the Markov assumption, recursive Bayesian updating can be used to efficiently combine evidence.
Bayes filters are a probabilistic tool for estimating the state of dynamic systems.

46. Dimensions of Mobile Robot Navigation
SLAM
localization
mapping
integrated approaches
active localization
exploration
motion control

47. Outline
Localization
Mapping
Exploration

48. Probabilistic Localization

49. Localization with Bayes Filters
p(z|x)
observation x
laser data
p(o|s,m) s’ a
p(x|u,x’)

50. Localization with Sonars in an Occupancy Grid Map

51. Resulting Beliefs

52. What is the Right Representation?
Kalman filters
Multi-hypothesis tracking
Grid-based representations
Topological approaches
Particle filters

53. Monte-Carlo Localization
Set of N samples {<x1,w1>, … <xN,wN>} containing a state x and an importance weight w
Initialize sample set according to prior knowledge
For each motion u do:
Sampling: Generate from each sample a new pose according to the motion model
For each observation z do:
Importance sampling: weigh each sample with the likelihood
Re-sampling: Draw N new samples from the sample set according to the importance weights

54. Particle Filters Represent density by random samples
Estimation of non-Gaussian, nonlinear processes
Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter, Particle filter
Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]
Computer vision: [Isard and Blake 96, 98]
Dynamic Bayesian Networks: [Kanazawa et al., 95]

55. Monte Carlo Localization (MCL) Represent Density Through Samples

56. Importance Sampling
Weight samples:

57. Mobile Robot Localization with Particle Filters

58. MCL: Sensor Update

59. PF: Robot Motion

60. Particle Filter Algorithm
1. Draw from
3. Importance factor for
2. Draw from
4. Re-sample

61. Beam-based Sensor Model

62. Typical Measurement Errors of an Range Measurements
Beams reflected by obstacles
Beams reflected by persons / caused by crosstalk
Random measurements
Maximum range measurements

63. Proximity Measurement
Measurement can be caused by …
a known obstacle.
cross-talk.
an unexpected obstacle (people, furniture, …).
missing all obstacles (total reflection, glass, …).
Noise is due to uncertainty …
in measuring distance to known obstacle.
in position of known obstacles.
in position of additional obstacles.
whether obstacle is missed.

64. Beam-based Proximity Model
Measurement noise
Unexpected obstacles
zexp
zmax
zexp
zmax

65. Beam-based Proximity Model
Random measurement
Max range
zexp
zmax
zexp
zmax

66. Resulting Mixture Density
How can we determine the model parameters?

67. Measured distances for expected distance of 300 cm.
Raw Sensor Data
Measured distances for expected distance of 300 cm.
Sonar
Laser

68. Approximation Maximize log likelihood of the data
Search space of n-1 parameters.
Hill climbing
Gradient descent
Genetic algorithms …
Deterministically compute the n-th parameter to satisfy normalization constraint.

69. Approximation Results
Laser
Sonar
400cm
300cm

70. Example z
P(z|x,m)

71. Odometry Model
Robot moves from to
Odometry information

72. Sampling from a Motion Model
Start

73. MCL: Global Localization (Sonar)

74. Using Ceiling Maps for Localization
[Dellaert et al. 99]

75. Vision-based Localization
h(x) s
P(s|x)

76. Under a Light
Measurement:
Resulting density:

77. Next to a Light
Measurement:
Resulting density:

78. Elsewhere
Measurement:
Resulting density:

79. MCL: Global Localization Using Vision

80. Vision-based Localization
Odometry only:
Vision:
Laser:

81. Vision-based Localization

82. Adaptive Sampling

83. KLD-sampling Idea: Observation: Assume we know the true belief.
Represent this belief as a multinomial distribution.
Determine number of samples such that we can guarantee that, with probability (1- d), the KL-distance between the true posterior and the sample-based approximation is less than e.
Observation:
For fixed d and e, number of samples only depends on number k of bins with support:

84. MCL: Adaptive Sampling (Sonar)

85. MCL: Adaptive Sampling (Laser)

86. Performance Comparison
Grid-based localization
Monte Carlo localization

87. Dimensions of Mobile Robot Navigation
SLAM
localization
mapping
integrated approaches
active localization
exploration
motion control

88. Occupancy Grid Maps
Store in each cell of a discrete grid the probability that the corresponding area is occupied.
Approximative
Do not require features
All cells are considered independent
Their probabilities are updated using the binary Bayes filter

89. Updating Occupancy Grid Maps
Update the map cells using the inverse sensor model
Or use the log-odds representation

90. Typical Sensor Model for Occupancy Grid Maps
Combination of a linear function and a Gaussian:

91. Incremental Updating of Occupancy Grids (Example)

92. Resulting Map Obtained with Ultrasound Sensors

93. Resulting Occupancy and Maximum Likelihood Map
The maximum likelihood map is obtained by clipping the occupancy grid map at a threshold of 0.5

94. Occupancy Grids: From scans to maps

95. Tech Museum, San Jose
CAD map
occupancy grid map

96. Dimensions of Mobile Robot Navigation
SLAM
localization
mapping
integrated approaches
active localization
exploration
motion control

97. Simultaneous Localization and Mapping (SLAM)
To determine its position, the robot needs a map.
During mapping, the robot needs to know its position to learn a consistent model
Simultaneous localization and mapping (SLAM) is a “chicken and egg problem”

98. Outline
Localization
Mapping
Exploration

99. Why SLAM is Hard: Raw Odometry

100. Why SLAM is Hard: Ambiguity
End
Same position
Start
[Courtesy of Eliazar & Parr]

101. Probabilistic Formulation of SLAM
three dimensions
n=a£b dimensions
map m

102. Key Question/Problem
How to maintain multiple map and pose hypotheses during mapping?
Ambiguity caused by the data association problem.

103. A Graphical Model for SLAMx z u 2
... t 1
t-1

104. Techniques for Generating Consistent Maps
Scan matching (online)
Probabilistic mapping with a single map and a posterior about poses Mapping + Localization (online)
EKF SLAM (online, mostly landmarks or features only)
EM techniques (offline)
Lu and Milios (offline)

105. Scan Matching
Maximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map.
To compute consistent maps, we apply a recursive scheme.
At each point in time we compute the most likely position of the robot, given the map constructed so far.
Based on the position hat l-t, we then extend the map an incorporate the scan obtained at time t.
current measurement
map constructed so far
robot motion

106. Scan Matching Example

107. Rao-Blackwellized Particle Filters for SLAM
Observation:
Given the true trajectory of the robot, we can efficiently compute the map (mapping with known poses).
Idea:
Use a particle filter to represent potential trajectories of the robot.
Each particle carries its own map.
Each particle survives with a probability that is proportional to the likelihood of the observation given that particle and its map.
[Murphy et al., 99]

108. Factorization Underlying Rao-Blackwellization
Mapping with known poses
Particle filter representing trajectory hypotheses

109. Example 3 particles map of particle 3 map of particle 1

110. Limitations A huge number of particles is required.
This introduces enormous memory and computational requirements.
It prevents the application of the approach in realistic scenarios.

111. Challenge Reduction of the number of particles. Approaches:
Focused proposal distributions (keep the samples in the right place)
Adaptive re-sampling (avoid depletion of relevant particles)

112. Motion Model for Scan Matching
Raw Odometry
Scan Matching

113. Graphical Model for Mapping with Improved Odometryz k x 1 u' u
k-1
...
k+1
2k-1 2k 2 n
n·k
(n+1)·k-1
n·k+1

114. Application Example

115. The Optimal Proposal Distribution
[Doucet, 98]
For lasers is extremely peaked and dominates the product.
We can safely approximate by a constant:

116. Resulting Proposal Distribution
Gaussian approximation:

117. Resulting Proposal Distribution
Approximate this equation by a Gaussian:
maximum reported by a scan matcher
Gaussian approximation
Draw next generation of samples
Sampled points around the maximum

118. Estimating the Parameters of the Gaussian for each Particle
xj are a set of sample points around the point x* the scan matching has converged to.
 is a normalizing constant

119. Computing the Importance Weight
Sampled points around the maximum of the observation likelihood

120. Improved Proposal
The proposal adapts to the structure of the environment

121. Incorporating the Measurements
End of a corridor:
Free space:
Corridor:

122. Selective Resampling
Resampling is dangerous, since important samples might get lost (particle depletion problem)
In case of suboptimal proposal distributions resampling is necessary to achieve convergence.
Key question: When should we resample?

123. Effective Number of Particles
Empirical measure of how well the goal distribution is approximated by samples drawn from the proposal
Neff describes “the variance of the particle weights”
Neff is maximal for equal weights. In this case, the distribution is close to the proposal

124. Resampling with Neff
If our approximation is close to the proposal, no resampling is needed
We only resample when Neff drops below a given threshold (N/2)
See [Doucet, ’98; Arulampalam, ’01]

125. Typical Evolution of Neff
visiting new areas
closing the first loop
visiting known areas
second loop closure

126. Map of the Intel Lab 15 particles
four times faster than real-time (P4, 2.8GHz)
5cm resolution during scan matching
1cm resolution in final map

127. Outdoor Campus Map 30 particles 250x250m2 1.75 km (odometry)
20cm resolution during scan matching
30cm resolution in final map

128. MIT Kilian Court

129. MIT Kilian Court

130. Dimensions of Mobile Robot Navigation
SLAM
localization
mapping
integrated approaches
active localization
exploration
motion control

131. Outline
Localization
Mapping
Exploration

132. Exploration The approaches seen so far are purely passive.
By reasoning about control, the mapping process can be made much more effective.

133. Decision-Theoretic Formulation of Exploration
reward
(expected information gain)
cost
(path length)

134. Exploration with Known Poses
Move to the place that provides you with the best tradeoff between information gathered and cost of reaching that point.

135. Exploration With Known Poses
Real robot, ultrasounds only:

136. Dimensions of Mobile Robot Navigation
SLAM
localization
mapping
integrated approaches
active localization
exploration
motion control

137. Where to Move Next?

138. Naïve Approach to Combine Exploration and Mapping
Learn the map using a Rao-Blackwellized particle filter.
Apply an exploration approach that minimizes the map uncertainty.

139. Disadvantage of the Naïve Approach
Exploration techniques only consider the map uncertainty for generating controls.
They avoid re-visiting known areas.
Data association becomes harder.
More particles are needed to learn a correct map.

140. Application Example True map and trajectory
Path estimated by the particle filter

141. Map and Pose Uncertainty
map uncertainty

142. Goal Integrated approach that considers to control the robot.
exploratory actions,
place revisiting actions, and
loop closing actions
to control the robot.

143. Dual Representation for Loop Detection

144. Application Example

145. Real Exploration Example

146. Comparison
Map uncertainty only:
Map and pose uncertainty:

147. Example: Entropy Evolution

148. map and pose uncertainty
Quantitative Results
Localization error:
avg. localization error [m]
map and pose uncertainty
map uncertainty

149. Corridor Exploration

150. Summary
Probabilistic techniques are a powerful tool to deal with a variety of problems in mobile robot navigation.
By actively controlling mobile robots one can more effectively solve high-dimensional state estimation problems.

151. Questions? No Arrows Left … localization mapping motion control SLAM
active localization
exploration
integrated approaches

152. 3D Mapping

153. 3D Map Example

154. The Autonomous Blimp Project

155. Navigation in Environments with Deformable Objects

156. Navigation in Environments with Deformable Objects

157. Autonomous Cars