Probabilistic Fundamentals in Robotics

Probabilistic Fundamentals in Robotics Robot Perception Basilio Bona DAUIN Politecnico di Torino June 2011 Course Outline Basic mathematical framework Probabilistic models of mobile robots Mobile robot localization problem Robotic mapping Probabilistic planning and control Reference textbook Thrun, Burgard, Fox, Probabilistic Robotics, MIT Press, 2006 http://www.probabilistic robotics.org/ Basilio Bona June 2011 2 1

Probabilistic models of mobile robots Robot motion Kinematics Velocity motion model Odometry motion model Robot perception Maps Beam model of laser range finders Correlation based measurement models Feature based measurement models Basilio Bona June 2011 3 Maps A map of the environment is a list of objects in the environment, with their properties attached (location, signatures, characteristics, etc.) Map types: a) Location based maps b) Feature based maps Basilio Bona June 2011 4 2

LOCATION BASED MAPS Basilio Bona June 2011 5 Location based maps Basilio Bona June 2011 6 3

Examples of occupancy grid maps Basilio Bona June 2011 7 Example of a DEM X,Y,Z 0,0,0.0469611845911 0,1,0.0636116191745 0,2,0.0646667554975 0,3,0.0554484017193 0,4,0.0489766038954 0,5,0.0586052387953 0,6,0.0730350464582 0,7,0.05945667997 0 05945667997 0,8,0.0468478538096 0,9,0.0508458465338 0,10,0.0600770190358 0,11,0.0582068152726 0,12,0.0642780587077 0,13,0.0766600072384 0,14,0.076681882143 0,15,0.0644576400518 0,16,0.0439602993429 Basilio Bona June 2011 8 4

Sensors We consider only range sensors, and in particular, laser range sensors, that provides a measurement under the name of laser range scan Range values (scalars) K 1 Basilio Bona June 2011 9 Sensors Omnidirectional camera Multidimensional values (vectors) Basilio Bona June 2011 10 5

Sensor models beam based models Individual measurements are independent from each other, given the robot position and the map Basilio Bona June 2011 11 Errors Measurement errors can be caused by a known obstacle an unexpected obstacle (people walking, ) cross talk between beams false negative (missing all obstacles) due to total reflection, glass, Noise is due to uncertainty in measuring distance to known obstacle in position of known obstacles in position of unexpected obstacles false negatives and false positives Basilio Bona June 2011 12 6

Errors 1. Beams reflected by obstacles 2. Beams reflected by persons / caused by crosstalk 3. Random measurements 4. Maximum range measurements Basilio Bona June 2011 13 Noise models 1 Obstacle hit pdf Sum of four pdf s Unexpected obstacles pdf 0 0 Normalization factor Basilio Bona June 2011 14 7

Noise models 1 The values measured by the range sensor are limited in the interval [0,z max ] and in this interval they have normal distributions Moving objects, as people, produce unexpected short range distances, compared with static obstacles in the environment; they are treated as noise to make simpler models. The model used in this case is the exponential pdf, for reasons that are too long to explain here Basilio Bona June 2011 15 Noise models 2 Max range Random measurement 0 0 Basilio Bona June 2011 16 8

Noise models 2 Max range is a typical example of sensor failure to produce a correct answer; it often happens that the laser returns the maximum allowable distance, due to various reasons. In this case a point mass distribution is centered at z max Unexplained random measurements are often present; to account for such cases a uniform distribution is modeled all over the range Basilio Bona June 2011 17 Resulting noise mixture density 0 How can we determine the α model parameters? Basilio Bona June 2011 18 9

Raw Sensor Data max Measured distances for an expected distance of 300 cm 300 Sonar Laser We use these data to estimate the various parameters adjusting by hand the pdf s to fit them Basilio Bona June 2011 19 Approximation Other methods 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 Basilio Bona June 2011 20 10

Maximize log likelihood Basilio Bona June 2011 21 Approximation Results Laser Sonar 300cm 400cm Basilio Bona June 2011 22 11

Example z t p(z t x t,m) Laser scan of a part of the map The likelihood evaluated for all positions x t and projected into the map (shown in gray). The darker a position, the larger p(z t x t,m) Basilio Bona June 2011 23 Discrete model of proximity sensors Instead of densities, consider discrete steps along the sensor beam Consider dependencies between different cases Laser sensor Sonar sensor Basilio Bona June 2011 24 12

Approximation Results 1 Laser Sonar Basilio Bona June 2011 25 Approximation Results 2 "sonar-0" "sonar-1" 0.25 0.2 0.15 0.1 0.05 0 102030405060 70 0 10 20 30 40 50 60 70 0 0.3 0.25 0.2 0.15 0.1 0.05 0 102030405060 70 0 10 20 30 40 50 60 70 0 "sonar-2" different angles "sonar-3" 0.3 0.25 0.2 0.15 0.1 0.05 0 102030405060 70 0 10 20 30 40 50 60 70 0 0.25 0.2 0.15 0.1 0.05 0 102030405060 70 0 10 20 30 40 50 60 70 0 Basilio Bona June 2011 26 13

Beam based model summary Assumes independence between beams Model physical causes for measurements Mixture of densities for these causes Assumes independence between causes 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 Basilio Bona June 2011 27 Scan based models Beam based model is not smooth for small obstacles and at edges not very efficient Instead of following along the beam, just check the end point Likelihood fields Basilio Bona June 2011 28 14

Likelihood field for range finders Likelihood fields represent the likelihood to have an obstacle detection as a function of the global x-y coordinates, i.e., I am in x-y, what is the probability to sense an obstacle? high likelihood low likelihood Mapm Likelihood field p(z x,m) Basilio Bona June 2011 29 Example: likelihood field Occupancy grid map Likelihood field Basilio Bona June 2011 30 15

Likelihood field for range finders Advantages Smoothness: small changes in the robot pose have only small effect on the pdf distribution Disadvantages: People or other dynamic obstacles are not modeled The method is incapable to detect ray interception due to obstacles No map uncertainty is taken into account; unexplored areas cannot be handled Basilio Bona June 2011 31 Scan based models 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 Basilio Bona June 2011 32 16

Scan matching Extract likelihood field from most recent scan and use it to match different scans Extract likelihood field from first scan and use it to match second scan Basilio Bona June 2011 33 Properties of scan based model Highly efficient, uses 2D tables only Smooth with respect to small changes in robot position Allows gradient descent, scan matching Ignores physical properties of beams Will it work for ultrasound sensors Basilio Bona June 2011 34 17

FEATURE BASED MAPS Basilio Bona June 2011 35 Feature based maps Features are distinctive elements or geometric primitives characterizing the environment In feature based maps, the generic map element m k contains the property p (or properties) p of the feature and its location on the map Feature based maps only specify the shape of the environment at specific locations. They are popular in robotics, where particular features can be measured at no cost with vision sensors (e.g., color of an object, particular shapes, etc.) Features can be extracted from measurements and mathematically described as low level features (geometric primitives) like lines, circles high level features like walls, edges, corners, doors, trees, etc. Basilio Bona June 2011 36 18

Example of feature based maps Basilio Bona June 2011 37 Features extraction Line extraction using statistical methods: weighted least squares Different line tracts are extracted and clustered r α Basilio Bona June 2011 38 19

Landmarks on maps Instead of raw data from the environment z t...... we extract a reduced set of features, i.e., f(z t ) Reduction ofcomputational complexity: high dimensional measurement space is mapped into a low dimensional feature space It is common to define places as features, such as hallways, corridors, intersections, etc. Features often corresponds to distinct objects or places in the environment These objects or places are called landmarks Basilio Bona June 2011 39 Landmarks Landmarks can be Active (e.g., radio beacons, GPS, RFID) Passive (e.g., visual, retro reflective reflective, IR optical markers) Standard approach to localize with respect to landmarks is triangulation Sensors measure distance or bearing or both Basilio Bona June 2011 40 20

Landmarks Basilio Bona June 2011 41 Landmarks Basilio Bona June 2011 42 21

Landmark measurements We assume that sensors can measure the range and bearing of the landmark with respect to the local robot coordinate frame In addition, the feature extractor f(z t ) can provide one or more signatures (height, color, binary code, etc.) range bearing Statistically independent Basilio Bona June 2011 43 Landmark measurement model Basilio Bona June 2011 44 22

Sensor model with known correspondence The key issue to face is the data association problem. What is what? To solve this problem many algorithms exist We assume that a correspondence variable c i exists, able to uniquely identify with an integer label the i th landmark at time t i ct { 1, 2,, N 1} + i if c = j N then the i th feature corresponds to the j th landmark t i if c = N + 1 then the i th feature does not corresponds to any t feature in the map m Basilio Bona June 2011 45 Probabilistic model Basilio Bona June 2011 46 23

Landmark detection model i i Landmark_known_correspondence_model ( f, c, x, m ) 1: j = c i t 2 2 2: rˆ = ( m x) + ( m y) jx, jy, 3: φˆ = atan2( m y, m x) jy, jx, i i ˆ ˆ i σ φ φ σ σ t r t φ t j s 4: q = N( r r, ) N(, ) N( s s, ) 5: return q t t t t Normal probability distributions Basilio Bona June 2011 47 Distributions Posterior distribution of the robot pose given that it detected a landmark at 5m with a 30 o angle Posterior distribution Generated sample poses Basilio Bona June 2011 48 24

Distributions Basilio Bona June 2011 49 Distances only no uncertainty 2 2 2 x = ( a + d d )/2a 1 2 x 2 2 y =± ( d x ) 1 P 1 a P 2 d 1 2 x d P = (0, 0) 1 P = (,0) a 2 Basilio Bona June 2011 50 25

Bearings only no uncertainty P 3 P α P 2 2 D 2 z 2 β α z 3 D 1 z 2 D 1 D 3 z 1 P 1 z 1 α P 1 Law of cosine D = z + z 2 z z cosα 2 2 2 1 1 2 1 2 D = z + z 2z z cos( α) 2 2 2 1 1 2 1 2 2 2 2 = + 2 2 3 1 2 2 2 2 3 1 3 1 2 D z z 2z z cos( β) D = z + z 2z z cos( α+ β) Basilio Bona June 2011 51 Bearings only with uncertainty P 3 P 2 P 2 P 1 P 1 Most approaches attempt to find estimation mean Basilio Bona June 2011 52 26

Summary of sensor models The capacity to explicitly model the uncertainty is a key factor for robustness in robot sensing In many cases, good models can be found using the following approach: Determine a parametric model of the noise free measurement Analyze various sources of noise Add adequate noise to parameters (possibly mixing in noise densities) Learn (and validate) parameters by fitting model to data Compute likelihood of measurement by probabilistically comparing the actual with the expected measurements This procedure is good for motion models as well It is extremely important to be aware of the underlying assumptions Basilio Bona June 2011 53 Thank you Any question? Basilio Bona June 2011 54 27