Probabilistic Fundamentals in Robotics

Transcript

1 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, robotics.org/ Basilio Bona June

2 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 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

3 LOCATION BASED MAPS Basilio Bona June Location based maps Basilio Bona June

4 Examples of occupancy grid maps Basilio Bona June Example of a DEM X,Y,Z 0,0, ,1, ,2, ,3, ,4, ,5, ,6, ,7, ,8, ,9, ,10, ,11, ,12, ,13, ,14, ,15, ,16, Basilio Bona June

5 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 Sensors Omnidirectional camera Multidimensional values (vectors) Basilio Bona June

6 Sensor models beam based models Individual measurements are independent from each other, given the robot position and the map Basilio Bona June 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

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

8 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 Noise models 2 Max range Random measurement 0 0 Basilio Bona June

9 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 Resulting noise mixture density 0 How can we determine the α model parameters? Basilio Bona June

10 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 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

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

12 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 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

13 Approximation Results 1 Laser Sonar Basilio Bona June Approximation Results 2 "sonar-0" "sonar-1" "sonar-2" different angles "sonar-3" Basilio Bona June

14 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 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

15 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 Example: likelihood field Occupancy grid map Likelihood field Basilio Bona June

16 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 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

17 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 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

18 FEATURE BASED MAPS Basilio Bona June 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

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

20 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 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

21 Landmarks Basilio Bona June Landmarks Basilio Bona June

22 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 Landmark measurement model Basilio Bona June

23 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 Probabilistic model Basilio Bona June

24 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 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

25 Distributions Basilio Bona June Distances only no uncertainty 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

26 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α D = z + z 2z z cos( α) = D z z 2z z cos( β) D = z + z 2z z cos( α+ β) Basilio Bona June Bearings only with uncertainty P 3 P 2 P 2 P 1 P 1 Most approaches attempt to find estimation mean Basilio Bona June

27 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 Thank you Any question? Basilio Bona June