Probabilistic Fundamentals in Robotics Probabilistic Models of Mobile Robots Robot Perception Basilio Bona DAUIN Politecnico di Torino July 2010
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 July 2010 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 July 2010 3
Maps A map of the environment is a list of objects in the environment, with their properties p attached (location, signatures, etc.) Map types: a) Location based maps b) Feature based maps Basilio Bona July 2010 4
Location based maps Basilio Bona July 2010 5
Example of an occupancy grid map Basilio Bona July 2010 6
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,8,0.0468478538096 0,9,0.0508458465338 0,10,0.0600770190358 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 July 2010 7
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) Basilio Bona July 2010 8
Sensors Omnidirectional camera Multidimensional values (vectors) Basilio Bona July 2010 9
Sensor models beam based models Individual measurements are independent from eachother other, given the robot position and the map Basilio Bona July 2010 10
Errors Measurement errors (noise) can be caused by a known obstacle cross talk between beams an unexpected obstacle (people, furniture, ) missing all obstacles (total reflection, glass, ) Noise is due to uncertainty in measuring distance to known obstacle in position ii of known obstacles in position of additional obstacles whether h obstacle is missed Basilio Bona July 2010 11
Errors 1. Beams reflected by obstacles 2. Beams reflected by persons / caused by crosstalk 3. Random measurements 4. Maximum range measurements Basilio Bona July 2010 12
Noise models 1 Measurement noise Unexpected obstacles z exp z max 0 exp 0 z exp z max Basilio Bona July 2010 13
Noise models 2 Random measurement Max range z exp z max 0 exp max 0 z exp z max Basilio Bona July 2010 14
Resulting noise mixture density 0 z exp z max How can we determine the modelparameters? Basilio Bona July 2010 15
Raw Sensor Data Measured distancesfor expected distanceof300cm cm Sonar Laser Basilio Bona July 2010 16
Approximation Maximize log likelihood of the data Search space of n 1 parameters Hill climbing Gradient descent Genetic algorithms Deterministically compute then th parameterto to satisfy normalization constraint Basilio Bona July 2010 17
Approximation Results Laser Sonar 300cm 400cm Basilio Bona July 2010 18
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 July 2010 19
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 July 2010 20
Approximation Results 1 Laser Sonar Basilio Bona July 2010 21
Approximation Results 2 "sonar-0" "sonar-1" 0.25 0.3 0.2 025 0.25 0.15 0.2 0.15 0.1 0.1 0.05 0.05 0 0 102030405060 70 102030405060 70 0 10 0 20 10 30 20 40 30 50 40 60 50 70 0 60 70 0 different angles "sonar-2" "sonar-3" 0.3 0.25 0.2 0.15 0.1 005 0.05 0 102030405060 70 0 10 20 30 40 50 60 70 0 0.25 0.2 0.15 0.1 005 0.05 0 102030405060 70 0 10 20 30 40 50 60 70 0 60 70 0 60 70 0 Basilio Bona July 2010 22
Beam based model summary Assumes independence between beams Modelsphysical causes for measurements Mixture of densities for these causes Assumesindependence 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 July 2010 23
Scan based models Beam based model dlis 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 July 2010 24
Likelihood field for range finders Likelihood fields represent the likelihood of an obstacle detection as a function of the global x-y coordinates high likelihood low likelihood Map m Likelihood klh dfield P(z x,m) Basilio Bona July 2010 25
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 July 2010 26
Scan matching Extract likelihood field from scan and use it to match different scan Extract likelihood field from first scan and use it to match second scan Basilio Bona July 2010 27
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 July 2010 28
Feature based maps Features are distinctive elements or geometric primitives of the environment In feature based maps the element m k contains the property (or properties) 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 ii sensors (e.g., color of an object) Featurescan be extracted frommeasurementsand measurements and mathematically described as low levelfeatures level (geometricprimitives) likelines, lines, circles high level features like walls, edges, corners, doors, trees, tables or trash cans Basilio Bona July 2010 29
Example of feature based maps Basilio Bona July 2010 30
Line extraction using statistical methods: weighted least squares Features extraction Different line tracts are extracted and clustered r α Basilio Bona July 2010 31
Landmarks on maps Instead of raw data from the environment... z t... we extract a reduced set of features, i.e., ie f(z t ) Reduction of computational complexity: high dimensional measurement space 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 We call these objects or places landmarks Basilio Bona July 2010 32
Landmarks Activebeacons (e.g., radio, GPS) Passive (e.g., visual, retro reflective) Standard approach is triangulation Sensor provides distance, or bearing, or distance and bearing Basilio Bona July 2010 33
Landmarks Basilio Bona July 2010 34
Landmarks Basilio Bona July 2010 35
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.) Basilio Bona July 2010 36
Landmark measurement model Landmark measurement models are defined only for feature based maps The maps consist of a list of features Usually we have location properties m k = (m k,x m k,y ) Given the i thh feature at time t, that corresponds to the j thh landmark on the map, we can formulated the following model 37
Sensor model with known correspondence Key problem: data association. Who is what? To solve this problem many algorithms exist We assume that a correspondence variable exists, able to uniquely identifying the landmark if if c = j N then the i th feature corresponds to the j th landmark i t i c = N + 1 then the i th feature does not corresponds to any t feature in the map m Basilio Bona July 2010 38
Probabilistic model Basilio Bona July 2010 39
Landmark detection model i i Landmark_known_correspondence_model known model ( f, c, x, m ) 1: j = c i t t t t t 2 2 2: rˆ = ( m x) + ( m y) jx, jy, 3: φˆ = atan2 ( m y, m x) jy, jx, i i 4: ( ˆ, ) ( ˆ i q = N r r σ N φ φ, σ ) N( s s, σ ) 5: return q t r t φ t j s 40
Distributions 41
Distances only no uncertainty x x = ( a + d d )/2a 2 2 2 1 2 y =± ( d x ) 2 2 1 P 1 a d d 1 2 x P 2 P = (0,0) 0) P 1 2 = (,0) a 42
Bearings only no uncertainty P 3 P 2 D 1 α P2 z 2 D 1 D 2 D 3 z2 z 1 β α z 3 P 1 z 1 α P 1 D = z + z 2 z z cosα 2 2 2 1 1 2 1 2 2 2 2 D = z + z z z α Law of cosine 1 1 2 1 2 2 2 2 2 2 3 1 2 2 2 2 3 1 3 1 2 2 cos( ) D = z + z 2z z cos( β) D = z + z 2 z z cos( α + β) 43
Bearings only with uncertainty P 3 P 2 P 2 P 1 P 1 Most approaches attempt to find estimation mean 44
Summary of sensor models Explicitly modeling uncertainty in sensing is key to robustness In many cases, good modelscan be found by the following approach: Determine parametric model of noise free measurement Analyze sources of noise Add adequate noise to parameters (eventually mix in densities for noise) Learn (and verify) parameters by fitting model to data Likelihood of measurement is given by probabilistically bili i ll 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 Basilio Bona July 2010 45
Thank you. Any question? Basilio Bona July 2010 46