1. 4-1 Probabilistic Robotics: Motion and Sensing Slide credits: Wolfram Burgard, Dieter Fox, Cyrill Stachniss, Giorgio Grisetti, Maren Bennewitz, Christian Plagemann, Dirk Haehnel, Mike Montemerlo, Nick Roy, Kai Arras, Patrick Pfaff and others Sebastian Thrun & Alex Teichman Stanford Artificial Intelligence Lab

2. 4-2 Bayes Filters

3. 4-3 ActionSensing Bayes Filters

4. 4-4 Probabilistic Motion Models

5. 4-5 Action: p(x’ | u, x) Bayes Filters

6. 4-6 Robot Motion Robot motion is inherently uncertain. How can we model this uncertainty?

7. 4-7 Probabilistic Motion Models To implement the Bayes Filter, we need the transition model p(x | x’, u). The term p(x | x’, u) specifies a posterior probability, that action u carries the robot from x’ to x. In this section we will specify, how p(x | x’, u) can be modeled based on the motion equations.

8. 4-8 Locomotion of Wheeled Robots Locomotion (Oxford Dict.): Power of motion from place to place Differential drive (AmigoBot, Pioneer 2-DX) Car drive (Ackerman steering) Synchronous drive (B21) Mecanum wheels, XR4000

9. 4-9 Instantaneous Center of Curvature ICC For rolling motion to occur, each wheel has to move along its y-axis

10. 4-10 Differential Drive R ICC  (x,y) y l/2  x v l v r

11. 4-11 Differential Drive: Forward Kinematics ICC R P(t) P(t+  t)

12. 4-12 Differential Drive: Forward Kinematics ICC R P(t) P(t+  t)

13. 4-13 Mecanum Wheels

14. 4-14 Example

15. 4-15 XR4000 Drive  y x v i (t)  i (t) ICC

16. 4-16 XR4000 [courtesy by Oliver Brock & Oussama Khatib]

17. 4-17 Tracked Vehicle: Urban Robot

18. 4-18 Tracked Vehicle: OmniTread [courtesy by Johann Borenstein]

19. 4-19 Odometry

20. 4-20 Reasons for Motion Errors bump ideal case different wheel diameters carpet and many more …

21. 4-21 Coordinate Systems In general the configuration of a robot can be described by six parameters. Three-dimensional cartesian coordinates plus three Euler angles pitch, roll, and tilt. Throughout this section, we consider robots operating on a planar surface. The state space of such systems is three- dimensional (x,y,).

22. 4-22 Typical Motion Models In practice, one often finds two types of motion models: Odometry-based Velocity-based (dead reckoning) Odometry-based models are used when systems are equipped with wheel encoders. Velocity-based models have to be applied when no wheel encoders are given. They calculate the new pose based on the velocities and the time elapsed.

23. 4-23 Example Wheel Encoders These modules require +5V and GND to power them, and provide a 0 to 5V output. They provide +5V output when they "see" white, and a 0V output when they "see" black. These disks are manufactured out of high quality laminated color plastic to offer a very crisp black to white transition. This enables a wheel encoder sensor to easily see the transitions. Source: http://www.active-robots.com/

24. 4-24 Dead Reckoning Derived from “deduced reckoning.” Mathematical procedure for determining the present location of a vehicle. Achieved by calculating the current pose of the vehicle based on its velocities and the time elapsed.

25. Odometry Model Robot moves from to. Odometry information.

26. 4-26 The atan2 Function Extends the inverse tangent and correctly copes with the signs of x and y.

27. Noise Model for Odometry The measured motion is given by the true motion corrupted with noise.

28. Typical Distributions for Probabilistic Motion Models Normal distributionTriangular distribution

29. 4-29 Calculating the Probability (zero-centered) For a normal distribution For a triangular distribution 1.Algorithm prob_normal_distribution( a,b ): 2.return 1.Algorithm prob_triangular_distribution( a,b ): 2.return

30. 4-30 Calculating the Posterior Given x, x’, and u 1.Algorithm motion_model_odometry(x,x’,u) 2. 3. 4. 5. 6. 7. 8. 9. 10. 11.return p 1 · p 2 · p 3 odometry values (u) values of interest (x,x’)

31. 4-31 Examples

32. Application Repeated application of the sensor model for short movements. Typical banana-shaped distributions obtained for 2d-projection of 3d posterior. x’ u p(x|u,x’) u x’

33. Sample-based Density Representation

34. 4-34 Sample-based Density Representation

35. 4-35 How to Sample from Normal or Triangular Distributions? Sampling from a normal distribution Sampling from a triangular distribution 1.Algorithm sample_normal_distribution( b ): 2.return 1.Algorithm sample_triangular_distribution( b ): 2.return

36. 4-36 Normally Distributed Samples 10 6 samples

37. 4-37 For Triangular Distribution 10 3 samples10 4 samples 10 6 samples 10 5 samples

38. Sample Odometry Motion Model 1.Algorithm sample_motion_model(u, x): 1. 2. 3. 4. 5. 6. 7.Return sample_normal_distribution

39. Sampling from Our Motion Model Start

40. Examples

41. Map-Consistent Motion Model  Approximation:

42. 4-42 Summary We discussed motion models for odometry-based motion (see book for velocity-based systems) We discussed ways to calculate the posterior probability p(x| x’, u). We also described how to sample from p(x| x’, u). Typically the calculations are done in fixed time intervals  t. In practice, the parameters of the models have to be learned. We also discussed an extended motion model that takes the map into account.

43. 4-43 Probabilistic Sensor Models

44. 4-44 Sensing: p(z | x) Bayes Filters

45. 4-45 Sensors for Mobile Robots Contact sensors: Bumpers, Whiskers Internal sensors Accelerometers (spring-mounted masses) Gyroscopes (spinning mass, laser light) Compasses, inclinometers (earth magnetic field, gravity) Proximity sensors Sonar (time of flight) Radar (phase and frequency) Laser range-finders (triangulation, tof, phase) Infrared (intensity) Stereo Visual sensors: Cameras, Stereo Satellite-based sensors: GPS

46. 4-46 Ultrasound Sensors Emit an ultrasound signal Wait until they receive the echo Time of flight sensor Polaroyd 6500

47. 4-47 Time of Flight sensors v : speed of the signal t : time elapsed between broadcast of signal and reception of the echo. emitter object

48. 4-48 Properties of Ultrasounds Signal profile [Polaroid]

49. 4-49 Sources of Error Opening angle Crosstalk Specular reflection

50. 4-50 Typical Ultrasound Scan

51. 4-51 Laser Range Scanner

52. 4-52 Properties High precision Wide field of view Approved security for collision detection

53. 4-53 Robots Equipped with Laser Scanners Herbert: Zora: Groundhog:

54. 4-54 Typical Scans

55. 4-55 Proximity Sensors The central task is to determine P(z|x), i.e., the probability of a measurement z given that the robot is at position x. Question: Where do the probabilities come from? Approach: Let’s try to explain a measurement.

56. 4-56 Beam-based Sensor Model Scan z consists of K measurements. Individual measurements are independent given the robot position.

57. 4-57 Beam-based Sensor Model

58. 4-58 Typical Measurement Errors of an Range Measurements 1.Beams reflected by obstacles 2.Beams reflected by persons / caused by crosstalk 3.Random measurements 4.Maximum range measurements

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

60. 4-60 Beam-based Proximity Model Measurement noise z exp z max 0 Unexpected obstacles z exp z max 0

61. 4-61 Beam-based Proximity Model Random measurementMax range z exp z max 0 z exp z max 0

62. 4-62 Resulting Mixture Density How can we determine the model parameters?

63. 4-63 Raw Sensor Data Measured distances for expected distance of 300 cm. SonarLaser

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

65. 4-65 Approximation Results Sonar Laser 300cm 400cm

66. 4-66 Example zP(z|x,m)

67. 4-67 Discrete Model of Proximity Sensors Instead of densities, consider discrete steps along the sensor beam. Consider dependencies between different cases. Laser sensor Sonar sensor

68. 4-68 Approximation Results Laser Sonar

69. 4-69 Influence of Angle to Obstacle

70. 4-70 Influence of Angle to Obstacle

71. 4-71 Influence of Angle to Obstacle

72. 4-72 Influence of Angle to Obstacle

73. 4-73 Summary Beam-based Model Assumes independence between beams. Justification? Overconfident! Models physical causes for measurements. Mixture of densities for these causes. Assumes independence between causes. Problem? Implementation Learn parameters based on real data. Different models should be learned for different angles at which the sensor beam hits the obstacle. Determine expected distances by ray-tracing. Expected distances can be pre-processed.

74. 4-74 Scan-based Model Beam-based model is … not smooth for small obstacles and at edges. not very efficient. Idea: Instead of following along the beam, just check the end point.

75. 4-75 Scan-based Model Probability is a mixture of … a Gaussian distribution with mean at distance to closest obstacle, a uniform distribution for random measurements, and a small uniform distribution for max range measurements. Again, independence between different components is assumed.

76. 4-76 Example P(z|x,m) Map m Likelihood field

77. 4-77 San Jose Tech Museum Occupancy grid map Likelihood field

78. 4-78 Scan Matching Extract likelihood field from scan and use it to match different scan.

79. 4-79 Scan Matching Extract likelihood field from first scan and use it to match second scan. ~0.01 sec

80. 4-80 Properties of Scan-based Model Highly efficient, uses 2D tables only. Smooth w.r.t. to small changes in robot position. Allows gradient descent, scan matching. Ignores physical properties of beams. Will it work for ultrasound sensors?

81. 4-81 Landmarks Active beacons (e.g., radio, GPS) Passive (e.g., visual, retro-reflective) Standard approach is triangulation Sensor provides distance, or bearing, or distance and bearing.

82. 4-82 Distance and Bearing

83. 4-83 Probabilistic Model 1.Algorithm landmark_detection_model(z,x,m): 2. 3. 4. 5.Return

84. 4-84 Distributions

85. 4-85 Distances Only No Uncertainty P1P1 P2P2 d1d1 d2d2 x X’ a P 1 =(0,0) P 2 =(a,0)

86. 4-86 P1P1 P2P2 D1D1 z1z1 z2z2  P3P3 D2D2  z3z3 D3D3 Bearings Only No Uncertainty P1P1 P2P2 D1D1 z1z1 z2z2   Law of cosine

87. 4-87 Bearings Only With Uncertainty P1P1 P2P2 P3P3 P1P1 P2P2 Most approaches attempt to find estimation mean.

88. 4-88 Summary of Sensor Models Explicitly modeling uncertainty in sensing is key to robustness. In many cases, good models can be found by the following approach: 1.Determine parametric model of noise free measurement. 2.Analyze sources of noise. 3.Add adequate noise to parameters (eventually mix in densities for noise). 4.Learn (and verify) parameters by fitting model to data. 5.Likelihood of measurement is given by “probabilistically comparing” the actual with the expected measurement. This holds for motion models as well. It is extremely important to be aware of the underlying assumptions!