Comments
Description
Transcript
Robot Localization - LaDiSpe
ROBOTICS 01PEEQW Basilio Bona DAUIN – Politecnico di Torino Mobile Robots Navigation Overview, examples and ongoing research Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Localization Mapping and world representations Simultaneous Localization and Mapping (SLAM) Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 3 Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Robot Localization Mapping and world representations Simultaneous Localization and Mapping (SLAM) Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 4 Mobile Robotics Mobile robotics is an emerging field of research and a booming market: Robots are no longer employed in repetitive tasks within a predefined workspace Robotics Industrial Robotics Mobile Robotics They can move autonomously in the environment exploiting their perceptions and performing required actions Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 5 Application scenarios Mobile Robotics Domestic Robots Basilio Bona - DAUIN - PoliTo Personal Robots ROBOTICS 01PEEQW - 2015/2016 Elderly and health care 6 Application Scenarios Mobile Robotics Military Robots Basilio Bona - DAUIN - PoliTo Service Robots ROBOTICS 01PEEQW - 2015/2016 Science-oriented 7 Robot perception and action Reasoning Perception Gyros Accelero -meters Wheel encoders Basilio Bona - DAUIN - PoliTo Radio devices Actuation Locomotion Laser range finder Manipulation GPS Other actuators Camera ROBOTICS 01PEEQW - 2015/2016 8 Robot perception and action Perception modelling Perception Gyros Accelerometers Wheel encoders They do not measure external variables but quantities related to robot motion Radio devices GPS Not available Laser range in indoor finder scenarios Camera These measurements are often referred to as odometry or commands Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 9 Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Robot Localization Mapping and world representations Simultaneous Localization and Mapping Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 10 Mobile Robot Navigation A crucial prerequisite for a mobile robot to perform tasks in the surrounding environment is the capability to autonomously navigate Navigation requires the following capabilities: Localization: the robot has to be able to determine its pose wrt a given reference frame Mapping: the agent has to build Target a consistent and meaningful point representation of the environment Rr Path planning: the robot has to plan the motion strategy to reach a given target position R0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 11 Robot Localization The localization problem for planar indoor scenarios and wheeled robots can be stated as follows: Determine the pose of the robot in a given reference frame using odometry and sensor measurements State vector describing robot pose (planar case): pt = [xt yt θt] u1:t motion estimates from odometry (controls) z1:t measurements from onboard sensors (measurements) u1 u 2 L z1 z 2 L Basilio Bona - DAUIN - PoliTo ut zt ROBOTICS 01PEEQW - 2015/2016 12 Robot Localization Why you cannot simply trust odometry for motion estimation? Let us consider the transition of the state from one instant of time to the next one: Since the odometry is affected by noise, instead of getting the true robot displacement, you will get a noisy estimate pˆ t : estimate of pose at time t+1 pt: pose at time t pt+1: pose at time t+1 Error accumulates over time leading the estimate to diverge from the true position Try to estimate your position after walking 100 m with you eyes closed Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 13 Robot Localization In Probabilistic Robotics the target is not only to estimate a value of the unknown variable (robot pose), but also to quantify the uncertainty on such estimate: The uncertainty is explicitly modeled by a probability density (belief) Example: Position of the robot in 2D environment: pt : prob(x , y ) Deterministic Approach Basilio Bona - DAUIN - PoliTo probability pt = [x , y ] ROBOTICS 01PEEQW - 2015/2016 Probabilistic Approach 14 Robot Localization EXAMPLE: Localization within a sensor infrastructure Mobile Robot can acquire odometric measurements and distance information from sensors in known positions Planar simulation of mobile robot scenario Belief of the robot bel (pt ) = prob(x , y | u 1:t , z 1:t ) Range Sensors (RFID Tags) Mobile Robot Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 15 Robot Localization Why is the uncertainty important: The robot has to be conscious of the uncertainty otherwise it risks to act in a misleading optimism How to model the problem? The evolution of robot pose can be easily modeled as a dynamic system: pt f pt 1 , ut , t zt h pt , map,t PROCESS MODEL: it describes how the pose evolves in function of the odometric information MEASUREMENT MODEL: it describes what quantities (function of robot pose) are measured by the robot We need to estimate pt: Localization is a state estimation problem But the state is a random variable Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 16 Robot Localization A general algorithm to perform this task is the Bayes Filter: For each time instant compute the current estimate as: PREDICTION A PRIORI ESTIMATE bel( pt ) = ò prob( pt | pt - 1, u t ) ×bel( pt - 1 )dpt - 1 (includes the commands in the belief) D UPDATE bel( pt ) = h ×prob(z t | pt - 1 ) ×bel( pt ) A POSTERIORI ESTIMATE (includes the measurements in the belief) Such approach is based on the Markov assumption, which postulates that future states are independent on past ones, given the current state Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 17 Robot Localization In general the prediction phase of the Bayes filter cannot be computed in closed form, because of the integral and this makes the previous formulation unsuitable for real time computation (the filter needs be iterated at each time step) Effective implementations of Bayes filter requires further assumptions on the representation of the probability densities involved in the estimation: Kalman filter: the probability densities are supposed to be multivariate Gaussians Particle filters: the probability densities are approximated with a set of weighted particles Several other solutions do exist, although they will not be treated in this course (histogram filters, graph-based models, etc) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 18 EKF LOCALIZATION Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 19 Robot Localization – Kalman Filter Localization Preliminaries on Gaussian Densities The general equation for a multivariate density with n support in is given by the following expression: f x 1 2 n/2 1/ 2 Can be parameterized in: T 1 exp x 1 x 2 mean value covariance With probability 1- the random variable x is contained in the ellipsoid described by the following inequality: x T x2 n2 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 1 x g 1 x1 20 Robot Localization – Kalman Filter Localization It is possible to demonstrate that if the dynamic system describing robot motion and measurements is linear, i.e. pt At pt 1 Bt ut t zt Ct pt t PROCESS MODEL MEASUREMENT MODEL the noises t and t are zero mean Gaussian noise, with covariance matrices Rt and Qt respectively, the initial state is p0 N 0 , 0 then the state at time t will be distributed according to pt N t , t for all the future time steps t (Gaussianity is preserved by the linear transformation with additive Gaussian noise) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 21 Robot Localization – Kalman Filter Localization How we can use the process model for estimation? pt At pt 1 Bt ut t If we have an initial estimate pˆ 0 , 0 and we know that the process noise is t N 0, Qt , for all time steps we can iteratively compute the pose estimate: pˆ t , t pˆ t At pˆ t1 Bt ut T A A t t t 1 t Qt pt If we use only odometry for localization the localization error accumulates over time! Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 22 Robot Localization – Kalman Filter Localization pˆ t , t is called a-priori estimate since it only includes the information about the command ut In order to refine our pose estimate we have to take into account also the measurements acquired from sensors, i.e., zt In order to include zt we simply correct the a-priori estimate by using the acquired measurements: a-posteriori estimate a-priori estimate measured quantity quantity that we expect to measure from our a-priori estimate pˆ t pˆ t K t ( zt Ct pˆ t ) t ( I Kt Ct ) t INNOVATION Kt is the Kalman Gain, and weights the innovation in order to have a minimum covariance a-posteriori estimate K t t CtT (Ct t CtT Qt ) 1 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 23 Robot Localization – Kalman Filter Localization The previous equations constitute the fundamental blocks of the Kalman filter: For each time instant compute the current estimate as: PREDICTION A PRIORI ESTIMATE pˆ t At pˆ t1 Bt ut t (includes the commands in the belief) At t 1 A Qt T t UPDATE K t t CtT (Ct t CtT Qt ) 1 A POSTERIORI ESTIMATE pˆ t pˆ t K t ( zt Ct pˆ t ) (includes the measurements in the belief) t ( I Kt Ct ) t Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 24 Robot Localization – Extended Kalman Filter (EKF) The Kalman filter provides the minimum variance solution for linear Gaussian systems (optimality) pt f pt 1 , ut , t In most of the real world problems, zt h pt , map,t however, both the process and measurements models are expressed by non-linear equations Kalman filter can still cope with this case: for filter prediction and update, the models are linearized, and the approach becomes f At pt h Ct pt pt pˆ t 1 pˆ t f pˆ t1 , ut t At t 1 AtT Qt Basilio Bona - DAUIN - PoliTo UPDATE PREDICTION (with no guarantee of optimality!!): pt pˆ t K t t CtT (Ct t CtT Qt ) 1 pˆ t pˆ t K t zt h( pˆ t ) t ( I Kt Ct ) t ROBOTICS 01PEEQW - 2015/2016 25 Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure Fixed Sensors (deployed in the environment in known positions) True position of the mobile robot Mobile Robot can acquire odometric measurements and distance information from sensors in known positions t=0 KF estimate (time zero) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 26 Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure STEP 1: - Acquire odometry t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 27 Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 28 Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 29 Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 30 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 31 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 32 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 33 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 34 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update ... t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 35 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) EXAMPLE: EKF Localization within a sensor infrastructure TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update ... t=0 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 36 Luca Carlone – Politecnico di Torino Robot Localization – Extended Kalman Filter (EKF) Trajectory estimated using EKF Localization Trajectory estimated using measurements only Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 37 Luca Carlone – Politecnico di Torino Robot Localization – EKF Localization summary Extended Kalman Filter is an effective solution for mobile robot localization, but a naïve implementation does not always suffice to cope with the problem at hand: ? zt h pt Linearization can cause filter divergence if the original problem is highly non-linear It can be hard to precisely model the available information according to the process and measurement models Multivariate Gaussian are unimodal distributions, hence they can be unsuitable to model a multimodal probability belief Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 38 Luca Carlone – Politecnico di Torino Robot Localization – EKF Localization summary Several variants do exist that allow to mitigate the issues mentioned before: Multi Hypotheses Tracking Unscented Kalman filter Particle Filters … In the following we will focus on the use of Particle filters for the estimation of mobile robot pose Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 39 Luca Carlone – Politecnico di Torino MONTE CARLO LOCALIZATION Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 40 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) Particle-filters based localization (Monte Carlo Localization) uses a set of weighted random samples for approximating robot pose belief: [i ] ˆ bel pt pt pt n i 1 • [i ] t n [i ] 1 t i 1 where • n is the particle set size • ^ p[i]t is the pose of the i-th particle at time t • ω[i]t is the importance weight of the sample i • δ( · ) is the Dirac delta function Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 41 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) Monte Carlo Localization: Particle-based Representation of position belief Particle-based approximation Gaussian approximation (ellipse: 95% acceptance region) ROBOTICS 01PEEQW - 2015/2016 Basilio Bona - DAUIN - PoliTo 42 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) Using particle filters approximation, the Bayes Filter can be reformulated as follows (starting from the robot initial belief at time zero): 1. PREDICTION: Generate a new particle set given motion model and controls applied 2. UPDATE: Assign to each particle an importance weight according to sensor measurements 3. RESAMPLING: Randomly resample particles in function of their importance weight Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 43 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) 1. PREDICTION: Generate a new particle set given motion model and controls applied For each particle: Given the particle pose at time step t-1 and from the commands , you predict particle pose at time t, using the motion model: pˆ t[ i ] f pˆ [t i]1 , ut , t The variable t is randomly extracted according to the noise distribution We obtain a set of predicted particles Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 t 44 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) 2. UPDATE: Assign to each particle an importance weight according to sensor measurements For each particle: Compare particle’s prediction of measurements with actual measurements zt VS h( pˆ t[ i ] ) Particles whose predictions match the measurements are given a high weight RED: particles with high weights Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 45 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) 3. RESAMPLING: Resample particles based on weight For n times draw a particle from t (with replacement) with probability given by the importance weights and put it in the set t RED: particles with high weights The new set provides the particle-based approximation of the t robot pose at time t Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 46 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure Fixed Sensors (deployed in the environment in known positions) True position of the mobile robot Mobile Robot can acquire odometric measurements and distance information from sensors in known positions Initial particle set Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 47 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 48 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 49 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 50 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 51 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 Filter Prediction Acquire meas. Weights Update Resampling 52 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 53 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 54 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 55 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 56 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 57 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 3: - Acquire odometry Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 58 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 3: - Acquire odometry - Filter Prediction Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 59 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 3: - Acquire odometry - Filter Prediction - Acquire meas. Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 60 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 3: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 61 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure STEP 1: - Acquire odometry - Filter Prediction Acquire meas. Weights Update Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 3: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling 62 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 Luca Carlone – Politecnico di Torino Robot Localization – Particle Filters (MCL) EXAMPLE: Monte Carlo Localization within a sensor infrastructure Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 63 Luca Carlone – Politecnico di Torino Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Robot Localization Mapping and world representations Simultaneous Localization and Mapping (SLAM) Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 64 Luca Carlone – Politecnico di Torino Mapping and world representations Mapping is the task of building a consistent representation of the prob(m | x1:t , z1:t ) environment assuming that we know robot poses Depending on the scenario it is convenient to use different world representations (to mention just a few): Digital elevation map landmarks Point-cloud representation Landmark-based representation (stochastic map) Basilio Bona - DAUIN - PoliTo Occupancy grid-map ROBOTICS 01PEEQW - 2015/2016 65 Luca Carlone – Politecnico di Torino Mapping and world representations We will briefly describe two particular representations which are widespread in the applications of mobile robotics in indoor scenarios Landmark-based maps: stochastic maps that contain a probabilistic description (usually mean+covariance) of the position of salient features of the scenario Occupancy grid maps: high- resolution models of the environment. It is a grid in which each cell contains the probability of the cell being occupied Landmarks (doors, corners, objects..) Free cell Unknow cell Occupied cell Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 66 Luca Carlone – Politecnico di Torino Mapping and world representations Landmark-based map: + compact world representations (low memory occupation) + efficient management + the problem can be solved in analogy with the approaches presented for robot localization - do not directly provide information for navigation (i.e. obstacle avoidance) - How to distinguish features? (data association) Basilio Bona - DAUIN - PoliTo Occupancy grid map: + fast probabilistic update rules + suitable for navigation: widespread path planning algorithms (A*, D*, etc.) can be adapted to deal with grid maps + are intuitive models of the environment - remarkable memory occupation ROBOTICS 01PEEQW - 2015/2016 67 Luca Carlone – Politecnico di Torino Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Robot Localization Mapping and world representations Simultaneous Localization and Mapping (SLAM) Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 68 Luca Carlone – Politecnico di Torino Simultaneous Localization and Mapping (SLAM) Localization is based on the knowledge of a map of the environment, while mapping presupposes the knowledge of robot pose However in real indoor scenarios: It is rare to have a map of the environment No absolute position information is available The robot has to acquire the map of the environment while localizing itself wrt to this map What happens if we simply perform localization using the tools learned so far and we do mapping with the estimated pose? Mapping and localization are not independent, hence a joint solution is needed: Simultaneous Localization and Mapping (SLAM) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 69 Luca Carlone – Politecnico di Torino Simultaneous Localization and Mapping (SLAM) We need to estimate the joint distribution of both robot pose and map representation of the environment: belt p ( x1:t , m | z1:t , u1,t ) The algorithms employed depend mainly on which type of map needs to be estimated: Landmark-based maps EKF-SLAM Sparse Extended Information Filters GraphSLAM Rao-Blackwellized Particle Filters (FastSLAM) Occupancy grid maps Rao-Blackwellized Particle Filters GraphSLAM … Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 70 Luca Carlone – Politecnico di Torino EKF-SLAM Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 71 Luca Carlone – Politecnico di Torino EKF-SLAM Intuition: include the position of the landmarks in the state vector and perform estimation over an augmented state: xt [ xr ,t , yr ,t , r ,t , l1x , l1 y ,..., lNx , lNy ]T localization l1 mapping According to EKF framework the SLAM belief will be described by a multivariate Gaussian in 3 2 N xr xr xr yr xr y r r xr r l x l xt ~ N 1 x , r 1 x t=0 l1 y xr l1 y l Nx xr l Nx x l r Ny l Ny Basilio Bona - DAUIN - PoliTo x y y y y yl yl x y l l yl yl l l r r r r r r r 1x r 1y r Nx r Ny … r r r r r r r 1x r 1y r Nx r Ny x l yl l l l l l xl yl l l l l l r 1x r 1x r 1x 1x 1x 1x 1 y l l 1 x l Nx 1 x l Ny r 1y r 1y r 1y 1x 1 y 1y 1y l l 1 y l Nx 1 y l Ny xr l Nx yr l Nx r l Nx l1 xl Nx l1 y l Nx l Nxl Nx l Nxl Ny ROBOTICS 01PEEQW - 2015/2016 x l yl l l l l l r Ny r Ny 1 x Ny 1 y Ny l Nxl Ny l Nyl Ny r Ny l2 r xr ,t y r ,t r ,t ptr 1 l 1x l1 y ... N ... l Nx l Ny lN 72 Luca Carlone – Politecnico di Torino EKF-SLAM 1 We can simply extend the expression of the dynamic system used for EKF Localization [l1x , l1 y ] N [lNx , lNy ] pt f pt 1 , ut , t zt h pt , map,t PROCESS MODEL ptr f ptr.1 , ut , t 1 1 ... ... N N MEASUREMENT MODEL zt h ptr , 1 t ,..., N t ,t So the estimation procedure can proceed according to EKF, iterating prediction and update phases Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 73 Luca Carlone – Politecnico di Torino EKF-SLAM EKF-SLAM: a typical situation (1) Robot starts; first measurement of feature A (2) Robot drives forwards (uncertainty grows) (3) Robot makes first measurements of B and C (4) Robot drivesvector back towards (6)discovered. Robot re-observes B; also State xt canthe be grown new landmarks (5) Robotasre-observes A (loop are start (uncertainty grows more) the uncertainty of C shrinks closure); uncertainty shrinks Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 74 Luca Carlone – Politecnico di Torino EKF-SLAM summary Effective and easy to implement solution to landmark-based SLAM Have been applied successfully in large-scale environments ISSUES: Complexity is quadratic in the number of landmarks: O(n2) G N LI SCA Can diverge if non linearities are large! Data association: how do we decide if we are observing an already seen landmark? Dynamic environments: EKF-SLAM requires the environment to be static! Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 75 75 Luca Carlone – Politecnico di Torino RBPF-SLAM Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 76 Luca Carlone – Politecnico di Torino Rao-Blackwellized Particle Filters (RBPF-SLAM) Rao-Blackwellized Particle Filters (RBPF) are a sample-based techniques for solving SLAM They are particularly suitable for estimating occupancy gridmaps of an indoor environment Rao-Blackwellized Particle Filters are based on the following factorization of the SLAM belief: Robot trajectory Map of the environment Measurements Commands belt prob( x1:t , m | z1:t , u1,t ) prob(m | x1:t , z1:t ) prob( x1:t | z1:t , u1,t ) SLAM belief Basilio Bona - DAUIN - PoliTo mapping localization 77 ROBOTICS 01PEEQW - 2015/2016 77 Luca Carlone – Politecnico di Torino Rao-Blackwellized Particle Filters (RBPF-SLAM) The previous formula is known as Rao-Blackwell factorization, which gives the name to the corresponding solution to SLAM The underlying math traduces in the following simple concept: The particle filter estimate potential trajectories of the robot and for each hypothesis the SLAM reduces to mapping with known poses Contrarily to localization, each particle is not a pose hypothesis but a trajectory hypothesis! The filter estimates potential trajectories and a map is associated to each trajectory hypothesis 78 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 78 Luca Carlone – Politecnico di Torino RBPF-SLAM summary Rao-Blackwellized Particle Filters (RBPF) have been demonstrated to be an effective solution for the estimation of occupancy grid maps No data association (grid maps does not distinguish elements in the environment) ISSUES: Each particle carries on a complete map hypothesis: a common computer is not able to manage more than few hundreds of particles but: to cope with large uncertainty you need a large number of particles RBPF-SLAM requires the environment to be static! 79 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 79 Luca Carlone – Politecnico di Torino Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Robot Localization Mapping and world representations Simultaneous Localization and Mapping (SLAM) Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 80 Luca Carlone – Politecnico di Torino Path planning After the estimation of a map (occupancy grid-map for instance) it is straightforward to apply common path planning algorithms Without entering into details we just mention few well-known approaches to path planning, common in robotics (and not only..): Dijkstra algorithm, A*, D* Value iteration Vector Field Histogram Wavefronts Markov Decision Processes Potential fields Sampling-based schemes … Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 81 Luca Carlone – Politecnico di Torino Contents Introduction Mobile Robotics Application scenarios Robot perception and action Mobile Robots Navigation Robot Localization Mapping and world representations Simultaneous Localization and Mapping (SLAM) Path Planning Examples and current research effort Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 82 Luca Carlone – Politecnico di Torino Examples and current research effort Rao-Blackwellized Particle Filters SLAM A robot moving in an unknown environment has to perform Simultaneous Localization and Mapping using wheel odometry and a laser scanner RBPF-SLAM Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 Odometry based mapping 83 Luca Carlone – Politecnico di Torino Examples and current research effort Rao-Blackwellized Particle Filters SLAM in multi robot systems Map of Robot 1 Map of Robot 2 Final Map Filter estimation is based on virtual data exchanged among robots when they meet each other Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 84 Luca Carlone – Politecnico di Torino Examples and current research effort Simultaneous Localization and Mapping using omnidirectional camera Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 85 Luca Carlone – Politecnico di Torino Examples and current research effort Simultaneous Localization and Mapping using omnidirectional camera Visual Odometry (filter prediction) Basilio Bona - DAUIN - PoliTo Image segmentation for estimating distance from obstacles (weights update) ROBOTICS 01PEEQW - 2015/2016 86 Luca Carlone – Politecnico di Torino Examples and current research effort Simultaneous Localization and Mapping using omnidirectional camera Simulations Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 87 Luca Carlone – Politecnico di Torino Examples and current research effort Simultaneous Localization and Mapping using omnidirectional camera Real tests Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 88 Luca Carlone – Politecnico di Torino Examples and current research effort Active SLAM and exploration A robot, deployed in an unknown environment, is required to actively control its movements in order to: Maximize the explored areas (minimize time required for exploration) Minimize the uncertainty in Simultaneous Localization and Mapping In order to properly plan its policy, the robot needs reliable metrics for evaluating the expected gain of a motion policy (information gain-based exploration). Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 89 Luca Carlone – Politecnico di Torino Examples and current research effort Active SLAM and exploration Expected Information from a policy Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 90 Luca Carlone – Politecnico di Torino Stereo Vision OBJECTIVE Extract 3D information from stereo devices to support fully autonomous robot navigation Preliminary test with a cheap stereo webcam for computing the depth map of the environment Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 91 Luca Carlone – Politecnico di Torino Stereo Vision System has been tested on an I-Robot Create Platform Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 92 Luca Carlone – Politecnico di Torino LOCALIZATION AND SERVICE Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 93 Federico Tibaldi – Politecnico di Torino Outline Multirobot localization Intrusion detection and tracking Task allocation Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 94 Federico Tibaldi – Politecnico di Torino Multi Robot Localization Estimation of the pose for a team of robots in an highly symmetrical environment ( e.g. logistic area ) the robots are able to spread the knowledge about small asymmetries localization speed-up Robustness Information exchange is also used to: Monitoring the team localization conditions “When can I start to execute my tasks?” Finite state machine: Switch state according to received position estimates Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 95 Federico Tibaldi – Politecnico di Torino Multi Robot Localization Robots can: Detect and recognize another robot (camera + markers) Estimate its position ( camera + laser scan ) Send the estimate ( wi-fi network ) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 96 Federico Tibaldi – Politecnico di Torino Intrusion detection and tracking Detection of unexpected agents Laser range reading shape Omnidirectional camera Tracking KF for each intruder “intuders” can be followed by the robot a video stream can be acquired for situation Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 97 Federico Tibaldi – Politecnico di Torino Target tracking OBJECTIVE Application of the Kalman Filter for the estimation of the position of a target of interest, using OpenCV primitives • Monocular approach Basilio Bona - DAUIN - PoliTo • ST SmartCam (RVS) ROBOTICS 01PEEQW - 2015/2016 • Tracking from stereo vision 98 Federico Tibaldi – Politecnico di Torino Target tracking Master students are currently working on the integration of a pan-tilt stereo camera on a mobile platform for target tracking and following Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 99 Federico Tibaldi – Politecnico di Torino Task allocation Tasks are assigned to the team of robots: Allocation algorithm should be distributed NO single point of failure System is completely autonomous Market based algorithm An auction for each task One auctioneer Different for each task Many bidders Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 100 Federico Tibaldi – Politecnico di Torino THANK YOU Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 101 Robot Localization A general algorithm to perform this task is the Bayes Filter: For each time instant compute the current estimate : PREDICTION bel( pt ) = ò prob( pt | pt - 1, u t ) ×bel( pt - 1 )dpt - 1 A PRIORI ESTIMATE (includes the commands in the belief) D UPDATE bel( pt ) = h ×prob(z t | pt - 1 ) ×bel( pt ) A POSTERIORI ESTIMATE (includes the measurements in the belief) The expressions for the belief cannot be computed in closed form, because of the integral Effective implementations of Bayes filter require further assumptions: Kalman filter: the probability densities are supposed to be multivariate Gaussians Particle filters: the probability densities are approximated with a set of weighted particles Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 102 Robot Localization A general algorithm to perform this task is the Bayes Filter: For each time instant compute the current estimate as: PREDICTION bel( pt ) = ò prob( pt | pt - 1, u t ) ×bel( pt - 1 )dpt - 1 A PRIORI ESTIMATE (includes the commands in the belief) D UPDATE bel( pt ) = h ×prob(z t | pt ) ×bel( pt ) A POSTERIORI ESTIMATE (includes the measurements in the belief) In general the previous expressions cannot be computed in closed form, because of the integral Effective implementations of Bayes filter require further assumptions: Kalman filter: the probability densities are supposed to be multivariate Gaussians Particle filters: the probability densities are approximated with a set of weighted particles 103 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 Luca Carlone – Politecnico di Torino • • • An hyperbolic catadioptric camera is a central catadioptric camera iff it is set in single effective viewpoint condition (SVP) SVP occurs if the camera sensor and the focus of the hyperbola are at a distance equal to twice the hyperbola focal length f along the hyperboloid central axis [Baker and Nayar 98] A calibrated central catadioptric camera could be treated in a similar way to a common perspective camera 7/5/2010 Basilio Bona - DAUIN - PoliTo NOT SVP Other mirror shapes that allow SVP condition are ellipsoidal mirror or parabolic mirror (+ ortographic lens). See [Baker and Nayar 98] Large field of view dioptric camera (i.e. using fisheye lens) could be calibrated using the same model 104 ROBOTICS 01PEEQW - 2015/2016 104 • The central catadioptric camera model that we adopt is the one described in [Scaramuzza et al. 2006] and related Matlab® toolbox • Camera calibration consists in a polynomial estimation of the forward and inverse projection function • Calibration is performed once, then the system intrinsic extrinsic parameters are stored on a file. It takes into account errors induced by non perfect SVP condition and hardware quality. f(u,v) is the forward projection function • Once calibration is performed, a relation between a pixel and a 3D point can be established up to a scale factor. 7/5/2010 Basilio Bona - DAUIN - PoliTo 105 ROBOTICS 01PEEQW - 2015/2016 105