Localization and Map Making: Part II

Transcription

1 Localization and Map Making: Part II November 19, 2002 Class Meeting 25 Accuity Laser Range Scanner Automatically-Generated Map

2 Announcements Remember: 1 week until your final project is due!!! Remember: FINAL EXAM: Tuesday, December 3 rd (in class 2 weeks from today) Review for final will be in class this coming Thursday, November 21.

3 Key Questions to Address Sensor Error Models: How does robot accurately interpret noisy range and encoder data? Localization: How does robot take noisy sensor data and a partial map, and determine its own most likely position? Map Making: How does robot build up a map from incremental sensor data? Exploration: How does robot travel to ensure that all of the environment is explored and incorporated into the map?

4 Overall Localization and Map Making Picture S E N S O R S D E R I V E D M A P Exploration Map Update Localization motor commands x,y prob. distribution Recent Motor Command History expected sensory changes A C T U A T O R S

5 Sensor Models Need sensor model to deal with uncertainty Methods for generating sensor models: Empirical (i.e., through testing) Analytical (i.e, through understanding of physical properties) Subjective (i.e., through experience)

6 Modeling Common Sonar Sensor β Region I R Region III Region II Region I: Probably occupied Region II: Probably empty Region III: Unknown

7 How to Convert to Numerical Values? Need to translate model (previous slide) to specific numerical values for each occupancy grid cell Three methods: Bayesian Dempster-Shafer Theory HIMM (Histogrammic in Motion Mapping) We ll cover: Bayesian We won t cover: Dempster-Shafer HIMM

8 Bayesian: Most popular evidential method Approach: Convert sensor readings into probabilities Combine probabilities using Bayes rule Pioneers of approach: Elfes and Moravec at CMU in 1980s

9 Review: Basic Probability Theory Probability function: Gives values from 0 to 1 indicating whether a particular event, H (Hypothesis), has occurred For sonar sensing: Hypothesis: Sensing out acoustic wave and measuring time of flight Outcome: Range reading reporting whether the region being sensed is Occupied or Empty H = {Occupied, Empty) Probability that H has occurred: 0 < P(H) < 1 Probability that H has not occurred: 1 P(H)

10 Unconditional and Conditional Probabilities Unconditional probability: P(H) Probability of H Only provide a priori information For robotics, unconditional probabilities are not based on sensor readings Conditional probability: P(H s) Probability of H, given s For robotics, based on sensor readings, s Note: P(H s) + P(not H s) = 1.0

11 Probabilities for Occupancy Grids For each grid[i][j] covered by sensor scan: Compute P(Occupied s) and P(Empty s) For each grid element, grid[i][j], store tuple of the two probabilities: typedef struct { double occupied; double empty; } P; P occupancy_grid[rows][columns];

12 Converting Sonar Reading to Probability: Region I Region I: R r β α P(Occupied) = R + β x Max occupied 2 P(Empty) = 1.0 P(Occupied) where r is distance to grid element, α is angle to grid element Max occupied = highest probability possible (e.g., 0.98) NOTE: The closer to the acoustic axis, the higher the belief The nearer the grid element to the origin of the sonar beam, the higher the belief

13 Converting Sonar Reading to Probability: Region II Region II: P(Empty) = R r β α R + β 2 P(Occupied) = 1.0 P(Empty) where r is distance to grid element, α is angle to grid element NOTE: The closer to the acoustic axis, the higher the belief The nearer the grid element to the origin of the sonar beam, the higher the belief

14 Sonar Tolerance Sonar range readings have resolution error Thus, specific reading might actually indicate range of possible values E.g., reading of 0.87 meters actually means within (0.82, 0.92) meters Therefore, tolerance in this case is 0.05 meters. Tolerance gives width of Region I

15 Tolerance in Sonar Model Tolerance determines Region I Width β Region I R Region III Region II Region I: Probably occupied Region II: Probably empty Region III: Unknown

16 Example: What is value of grid cell? Which region? 3.5 < ( ) Region II β = 15 Region I s = 6 r = 3.5 α = 0 R = 10 P(Empty) = Region III Region II = 0.83 P(Occupied) = (1 0.83) = 0.17

17 Conditional Probabilities Note that previous calculations gave: P(s H), not P(H s) Thus, use Bayes Rule: P(H s) = P(H s) = P(s H) P(H) P(S H) P (H) + P(s not H) P(not H) P(s Empty) P(Empty) P(S Empty) P (Empty) + P(s Occupied) P(Occupied) P(s Occupied) and P(s Empty) are known from sensor model P(Occupied) and P(Empty) are unconditional, prior probabilities (which may or may not be known) If not known, okay to assume P(Occupied) = P(Empty) = 0.5

18 Returning to Example Let s assume we re on Mars, and we know that P(Occupied) = 0.75 P(Empty s=6) = = P(s Empty) P(Empty) P(S Empty) P (Empty) + P(s Occupied) P(Occupied) 0.83 x x x 0.75 = 0.62 P(Occupied s=6) = 1 P(Empty s=6) = 0.38

19 Updating with Bayes Rule How to fuse multiple readings? First time: Each element of grid initialized with a priori probability of being occupied or empty Subsequently: Use Bayes rule iteratively Probability at time t n-1 becomes prior and is combined with current observation at t n : P (H s n ) = P(s n H) P(H s n-1 ) P(s n H) P (H s n-1 ) + P(s n not H) P(not H s n-1 )

20 Two Other Strategies for Updating Dempster-Shafer Theory (section 11.4) HIMM (section 11.5) You are not responsible for these methods for this class!

21 Case Study: Another Approach to Multi-Robot Localization Recent work at University of Southern California SLAM Relaxation on a mesh Maximum likelihood estimation SDR scenario modeling The Stage simulator Multi-operator, multi-robot tasking

22 Localization Past approaches include: Filtering inertial sensors for location estimation Using landmarks (based on vision, laser etc.) Using maps Algorithms vary from Kalman filters, to Markov localization to Particle filters This case study approach Exploit communication for in-network localization Physics-based models

23 Static Localization System contains beacons and beacon detectors Assumptions: beacons are unique, beacon detectors determine correct identity. Static localization: determine the relative pose of each pair of beacons/detectors

24 Mesh Definition: Damped spring mass system

25 Mesh Energy Kinetic energy Potential energy = = = Γ = j j a b j b a j j j j j U U x x z x x z z z k U j j j i ) ( ), ( ) ( = = i i i i i V V m x V 2 2 1

26 Mesh Forces and Equations of Motion Forces z = U F j i x U = i j x i z j j Equations of motion 0 As = x + νx i t i F i / V m i 0, U U min

27 Encoding

28 SLAM: Simultaneous Localization and Mapping Localization

29 Multi-robot SLAM Localization

30 Team Localization using MLE Construct a set of estimates H = {h} where h is the pose of robot r at time t. Construct a set of observations O = {o} where o is either: the measured pose of robot r b relative to robot r a at time t, or the measured change in pose of robot r between times t a and t b. Assuming statistical independence between observations find the set of estimates H that maximizes:

31 Approach Equivalently, find the set H that minimizes:

32 Gradient-based Estimation Each estimate h = ( qˆ, r, t ) Each observation Measurement uncertainty assumed normal Relative Absolute o = ( µ,, ra, ta, rb, tb) 1 T U( o H ) = ( µ ˆ) µ Σ( µ ˆ) µ 2 ˆ µ = Γ ( q ˆa, q ˆ b )

33 Gradient Descent h U(O H) = o O µ ˆ U(o H) h µ ˆ Compute set of poses q that minimizes U(O H) Gradient-based algorithm

34 Results (large environment)

35 Range Error vs. Time Robots bump into each other

36 Bearing Error vs. Time

37 Orientation Error vs. Time

38 Summary of USC Team Localization Approach Runs on any platform as long as it can compute its motion via inertial sensing Unique beacons: robots, people, fixed locations etc. No model of the environment Indifferent to changes in the environment Robust to sensor noise Permits both centralized and distributed implementation

39 Preview of Next Class Localization and Map Building, Part III (1/2 class period) Review for Final (1/2 class period)