Probabilistic Fundamentals in Robotics. DAUIN Politecnico di Torino July 2010

Transcript

1 Probabilistic Fundamentals in Robotics Probabilistic Models of Mobile Robots Robot Perception Basilio Bona DAUIN Politecnico di Torino July 2010

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

3 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

4 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

5 Location based maps Basilio Bona July

6 Example of an occupancy grid map Basilio Bona July

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

8 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

9 Sensors Omnidirectional camera Multidimensional values (vectors) Basilio Bona July

10 Sensor models beam based models Individual measurements are independent from eachother other, given the robot position and the map Basilio Bona July

11 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

12 Errors 1. Beams reflected by obstacles 2. Beams reflected by persons / caused by crosstalk 3. Random measurements 4. Maximum range measurements Basilio Bona July

13 Noise models 1 Measurement noise Unexpected obstacles z exp z max 0 exp 0 z exp z max Basilio Bona July

14 Noise models 2 Random measurement Max range z exp z max 0 exp max 0 z exp z max Basilio Bona July

15 Resulting noise mixture density 0 z exp z max How can we determine the modelparameters? Basilio Bona July

16 Raw Sensor Data Measured distancesfor expected distanceof300cm cm Sonar Laser Basilio Bona July

17 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

18 Approximation Results Laser Sonar 300cm 400cm Basilio Bona July

19 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

20 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

21 Approximation Results 1 Laser Sonar Basilio Bona July

22 Approximation Results 2 "sonar-0" "sonar-1" different angles "sonar-2" "sonar-3" Basilio Bona July

23 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

24 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

25 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

26 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

27 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

28 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

29 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

30 Example of feature based maps Basilio Bona July

31 Line extraction using statistical methods: weighted least squares Features extraction Different line tracts are extracted and clustered r α Basilio Bona July

32 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

33 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

34 Landmarks Basilio Bona July

35 Landmarks Basilio Bona July

36 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

37 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

38 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

39 Probabilistic model Basilio Bona July

40 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

41 Distributions 41

42 Distances only no uncertainty x x = ( a + d d )/2a y =± ( d x ) P 1 a d d 1 2 x P 2 P = (0,0) 0) P 1 2 = (,0) a 42

43 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α D = z + z z z α Law of cosine cos( ) D = z + z 2z z cos( β) D = z + z 2 z z cos( α + β) 43

44 Bearings only with uncertainty P 3 P 2 P 2 P 1 P 1 Most approaches attempt to find estimation mean 44

45 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

46 Thank you. Any question? Basilio Bona July