1. ROBOTICS 01PEEQW Basilio Bona DAUIN – Politecnico di Torino

2. Mobile Robots Navigation Overview, examples and ongoing research

3.  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 Contents Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 3

4.  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 Contents Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 4

5. Robotics Industrial Robotics Industrial Robotics 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 They can move autonomously in the environment exploiting their perceptions and performing required actions Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 5

6. Application scenarios Domestic Robots Mobile Robotics Personal Robots Elderly and health care Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 6

7. Application Scenarios Military Robots Mobile Robotics Service Robots Science-oriented Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 7

8. Robot perception and action Locomotion Manipulation Other actuators Reasoning Perception Actuation Wheel encoders Laser range finder Camera Gyros Accelero -meters Radio devices GPS Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 8

9. Perception Wheel encoders Laser range finder Camera Gyros Accelero- meters Radio devices GPS They do not measure external variables but quantities related to robot motion These measurements are often referred to as odometry or commands Not available in indoor scenarios  Perception modelling Robot perception and action Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 9

10.  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 Contents Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 10

11.  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 a consistent and meaningful representation of the environment  Path planning: the robot has to plan the motion strategy to reach a given target position Mobile Robot Navigation Target point Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 11

12. 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): p t = [x t y t θ t ] u 1:t motion estimates from odometry (controls) z 1:t measurements from onboard sensors (measurements) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 12

13.  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: 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 Since the odometry is affected by noise, instead of getting the true robot displacement, you will get a noisy estimate p t : pose at time t p t+1 : pose at time t+1 estimate of pose at time t+1 Basilio Bona - DAUIN - PoliTo Robot Localization ROBOTICS 01PEEQW - 2015/2016 13

14.  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: probability Deterministic Approach Probabilistic Approach Basilio Bona - DAUIN - PoliTo Robot Localization ROBOTICS 01PEEQW - 2015/2016 14

15. Planar simulation of mobile robot scenario Belief of the robot  EXAMPLE: Localization within a sensor infrastructure Mobile Robot can acquire odometric measurements and distance information from sensors in known positions Range Sensors (RFID Tags) Mobile Robot Basilio Bona - DAUIN - PoliTo Robot Localization ROBOTICS 01PEEQW - 2015/2016 15

16.  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:  We need to estimate p t : Localization is a state estimation problem But the state is a random variable 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 Robot Localization Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 16

17.  A general algorithm to perform this task is the Bayes Filter: For each time instant compute the current estimate as: Such approach is based on the Markov assumption, which postulates that future states are independent on past ones, given the current state A PRIORI ESTIMATE (includes the commands in the belief) A POSTERIORI ESTIMATE (includes the measurements in the belief) PREDICTION UPDATE Robot Localization Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 17

18.  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) Robot Localization Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 18

19. EKF LOCALIZATION Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 19

20.  Preliminaries on Gaussian Densities The general equation for a multivariate density with support in is given by the following expression: Can be parameterized in: mean value covariance x1x1 With probability 1-  the random variable x is contained in the ellipsoid described by the following inequality: x2x2 Basilio Bona - DAUIN - PoliTo Robot Localization – Kalman Filter Localization ROBOTICS 01PEEQW - 2015/2016 20

21.  It is possible to demonstrate that if the dynamic system describing robot motion and measurements is linear, i.e. the noises and are zero mean Gaussian noise, with covariance matrices R t and Q t respectively, the initial state is then the state at time t will be distributed according to for all the future time steps t (Gaussianity is preserved by the linear transformation with additive Gaussian noise) Basilio Bona - DAUIN - PoliTo PROCESS MODEL MEASUREMENT MODEL Robot Localization – Kalman Filter Localization ROBOTICS 01PEEQW - 2015/2016 21

22.  How we can use the process model for estimation? If we have an initial estimate and we know that the process noise is, for all time steps we can iteratively compute the pose estimate: If we use only odometry for localization the localization error accumulates over time! Basilio Bona - DAUIN - PoliTo Robot Localization – Kalman Filter Localization ROBOTICS 01PEEQW - 2015/2016 22

23.  is called a-priori estimate since it only includes the information about the command  In order to refine our pose estimate we have to take into account also the measurements acquired from sensors, i.e.,  In order to include we simply correct the a-priori estimate by using the acquired measurements:  is the Kalman Gain, and weights the innovation in order to have a minimum covariance a-posteriori estimate a-posteriori estimate a-priori estimate measured quantity quantity that we expect to measure from our a-priori estimate INNOVATION Basilio Bona - DAUIN - PoliTo Robot Localization – Kalman Filter Localization ROBOTICS 01PEEQW - 2015/2016 23

24.  The previous equations constitute the fundamental blocks of the Kalman filter:  For each time instant compute the current estimate as: A PRIORI ESTIMATE (includes the commands in the belief) A POSTERIORI ESTIMATE (includes the measurements in the belief) PREDICTION UPDATE Basilio Bona - DAUIN - PoliTo Robot Localization – Kalman Filter Localization ROBOTICS 01PEEQW - 2015/2016 24

25.  The Kalman filter provides the minimum variance solution for linear Gaussian systems (optimality)  In most of the real world problems, 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 (with no guarantee of optimality!!): PREDICTION UPDATE Robot Localization – Extended Kalman Filter (EKF) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 25

26. t=0 Fixed Sensors (deployed in the environment in known positions) True position of the mobile robot KF estimate (time zero) Robot Localization – Extended Kalman Filter (EKF)  EXAMPLE: EKF Localization within a sensor infrastructure Mobile Robot can acquire odometric measurements and distance information from sensors in known positions Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 26

27. t=0 STEP 1: - Acquire odometry  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 27

28. t=0 STEP 1: - Acquire odometry - Filter Prediction  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 28

29. t=0 STEP 1: - Acquire odometry - Filter Prediction - Acquire meas.  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 29

30. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 30 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

31. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 31 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

32. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 32 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

33. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas.  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 33 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

34. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 34 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

35. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update...  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 35 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

36. t=0 TIME STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update TIME STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Filter Update...  EXAMPLE: EKF Localization within a sensor infrastructure Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 36 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

37. Trajectory estimated using EKF Localization Trajectory estimated using measurements only Robot Localization – Extended Kalman Filter (EKF) Luca Carlone – Politecnico di Torino 37 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

38.  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: 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 ? Robot Localization – EKF Localization summary Luca Carlone – Politecnico di Torino 38 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

39.  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 Robot Localization – EKF Localization summary Luca Carlone – Politecnico di Torino 39 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

40. MONTE CARLO LOCALIZATION Luca Carlone – Politecnico di Torino 40 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

41.  Particle-filters based localization (Monte Carlo Localization) uses a set of weighted random samples for approximating robot pose belief: 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 Robot Localization – Particle Filters (MCL) Luca Carlone – Politecnico di Torino 41 Basilio Bona - DAUIN - PoliTo ^ ROBOTICS 01PEEQW - 2015/2016

42.  Monte Carlo Localization: Particle-based Representation of position belief Particle-based approximation Gaussian approximation (ellipse: 95% acceptance region) Robot Localization – Particle Filters (MCL) Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo 42 ROBOTICS 01PEEQW - 2015/2016

43.  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 r esample particles in function of their importance weight Robot Localization – Particle Filters (MCL) Luca Carlone – Politecnico di Torino 43 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

44. 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:  The variable is randomly extracted according to the noise distribution  We obtain a set of predicted particles Luca Carlone – Politecnico di Torino 44 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

45. 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  Particles whose predictions match the measurements are given a high weight RED: particles with high weights Robot Localization – Particle Filters (MCL) Luca Carlone – Politecnico di Torino 45 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

46. 3.RESAMPLING: Resample particles based on weight For n times draw a particle from (with replacement) with probability given by the importance weights and put it in the set The new set provides the particle-based approximation of the robot pose at time t RED: particles with high weights Luca Carlone – Politecnico di Torino 46 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

47. Fixed Sensors (deployed in the environment in known positions) True position of the mobile robot Initial particle set  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Mobile Robot can acquire odometric measurements and distance information from sensors in known positions Luca Carlone – Politecnico di Torino 47 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

48. STEP 1: - Acquire odometry  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 48 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

49. STEP 1: - Acquire odometry - Filter Prediction  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 49 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

50. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas.  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 50 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

51. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 51 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

52. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 52 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

53. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 2: - Acquire odometry  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 53 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

54. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 2: - Acquire odometry - Filter Prediction  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 54 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

55. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas.  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 55 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

56. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 56 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

57. STEP 1: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling STEP 2: - Acquire odometry - Filter Prediction - Acquire meas. - Weights Update - Resampling  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 57 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

58. 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  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 58 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

59. 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  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 59 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

60. 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.  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 60 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

61. 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  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 61 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

62. 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  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 62 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

63.  EXAMPLE: Monte Carlo Localization within a sensor infrastructure Luca Carlone – Politecnico di Torino 63 Basilio Bona - DAUIN - PoliTo Robot Localization – Particle Filters (MCL) ROBOTICS 01PEEQW - 2015/2016

64.  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 Contents Luca Carlone – Politecnico di Torino 64 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

65.  Mapping is the task of building a consistent representation of the environment assuming that we know robot poses Depending on the scenario it is convenient to use different world representations (to mention just a few): Landmark-based representation (stochastic map) landmarks Occupancy grid-map Digital elevation map Point-cloud representation Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo Mapping and world representations ROBOTICS 01PEEQW - 2015/2016 65

66. 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  We will briefly describe two particular representations which are widespread in the applications of mobile robotics in indoor scenarios Free cell Unknow cell Occupied cell Mapping and world representations Landmarks (doors, corners, objects..) Luca Carlone – Politecnico di Torino 66 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

67. 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) 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 Luca Carlone – Politecnico di Torino 67 Basilio Bona - DAUIN - PoliTo Mapping and world representations ROBOTICS 01PEEQW - 2015/2016

68.  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 Contents Luca Carlone – Politecnico di Torino 68 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

69.  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? Simultaneous Localization and Mapping (SLAM) Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo  Mapping and localization are not independent, hence a joint solution is needed: Simultaneous Localization and Mapping (SLAM) ROBOTICS 01PEEQW - 2015/2016 69

70.  We need to estimate the joint distribution of both robot pose and map representation of the environment:  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 … Luca Carlone – Politecnico di Torino 70 Basilio Bona - DAUIN - PoliTo Simultaneous Localization and Mapping (SLAM) ROBOTICS 01PEEQW - 2015/2016

71. EKF-SLAM Luca Carlone – Politecnico di Torino 71 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

72.  Intuition: include the position of the landmarks in the state vector and perform estimation over an augmented state:  According to EKF framework the SLAM belief will be described by a multivariate Gaussian in localizationmapping t=0 l2l2 l1l1 lNlN r … EKF-SLAM Luca Carlone – Politecnico di Torino 72 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

73.  We can simply extend the expression of the dynamic system used for EKF Localization  So the estimation procedure can proceed according to EKF, iterating prediction and update phases PROCESS MODEL MEASUREMENT MODEL Luca Carlone – Politecnico di Torino 73 Basilio Bona - DAUIN - PoliTo EKF-SLAM ROBOTICS 01PEEQW - 2015/2016

74. State vector x t can be grown as new landmarks are discovered.  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 drives back towards the start (uncertainty grows more) (5) Robot re-observes A (loop closure); uncertainty shrinks (6) Robot re-observes B; also the uncertainty of C shrinks EKF-SLAM Luca Carlone – Politecnico di Torino 74 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

75.  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) 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! SCALINGSCALING EKF-SLAM summary Luca Carlone – Politecnico di Torino 75 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 75

76. RBPF-SLAM Luca Carlone – Politecnico di Torino 76 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

77.  Rao-Blackwellized Particle Filters (RBPF) are a sample-based techniques for solving SLAM  They are particularly suitable for estimating occupancy grid- maps of an indoor environment  Rao-Blackwellized Particle Filters are based on the following factorization of the SLAM belief: Robot trajectory Map of the environment MeasurementsCommands SLAM belief mapping localization Luca Carlone – Politecnico di Torino 77 Basilio Bona - DAUIN - PoliTo Rao-Blackwellized Particle Filters (RBPF-SLAM) ROBOTICS 01PEEQW - 2015/2016 77

78.  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 Luca Carlone – Politecnico di Torino 78 Basilio Bona - DAUIN - PoliTo Rao-Blackwellized Particle Filters (RBPF-SLAM) ROBOTICS 01PEEQW - 2015/2016 78

79.  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! RBPF-SLAM summary Luca Carlone – Politecnico di Torino 79 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 79

80.  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 Contents Luca Carlone – Politecnico di Torino 80 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

81.  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 … Path planning Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 81

82.  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 Contents Luca Carlone – Politecnico di Torino 82 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

83.  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-SLAMOdometry based mapping Examples and current research effort Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 83

84.  Rao-Blackwellized Particle Filters SLAM in multi robot systems Filter estimation is based on virtual data exchanged among robots when they meet each other Map of Robot 1 Map of Robot 2 Final Map Examples and current research effort Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 84

85.  Simultaneous Localization and Mapping using omnidirectional camera Luca Carlone – Politecnico di Torino 85 Basilio Bona - DAUIN - PoliTo Examples and current research effort ROBOTICS 01PEEQW - 2015/2016

86. Visual Odometry (filter prediction) Image segmentation for estimating distance from obstacles (weights update)  Simultaneous Localization and Mapping using omnidirectional camera Examples and current research effort Luca Carlone – Politecnico di Torino Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 86

87.  Simultaneous Localization and Mapping using omnidirectional camera Simulations Examples and current research effort Luca Carlone – Politecnico di Torino 87 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

88.  Simultaneous Localization and Mapping using omnidirectional camera Real tests Examples and current research effort Luca Carlone – Politecnico di Torino 88 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

89. 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). Maximize the explored areas (minimize time required for exploration) Minimize the uncertainty in Simultaneous Localization and Mapping  Active SLAM and exploration A robot, deployed in an unknown environment, is required to actively control its movements in order to: Luca Carlone – Politecnico di Torino 89 Basilio Bona - DAUIN - PoliTo Examples and current research effort ROBOTICS 01PEEQW - 2015/2016

90.  Active SLAM and exploration Expected Information from a policy Luca Carlone – Politecnico di Torino 90 Basilio Bona - DAUIN - PoliTo Examples and current research effort ROBOTICS 01PEEQW - 2015/2016

91. 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 Stereo Vision Luca Carlone – Politecnico di Torino 91 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

92. System has been tested on an I-Robot Create Platform Luca Carlone – Politecnico di Torino 92 Basilio Bona - DAUIN - PoliTo Stereo Vision ROBOTICS 01PEEQW - 2015/2016

93. LOCALIZATION AND SERVICE Federico Tibaldi – Politecnico di Torino 93 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

94. Outline  Multirobot localization  Intrusion detection and tracking  Task allocation Federico Tibaldi – Politecnico di Torino 94 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

95.  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 Multi Robot Localization Federico Tibaldi – Politecnico di Torino 95 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

96.  Robots can: Detect and recognize another robot (camera + markers) Estimate its position ( camera + laser scan ) Send the estimate ( wi-fi network ) Multi Robot Localization Federico Tibaldi – Politecnico di Torino 96 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

97.  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 Intrusion detection and tracking Federico Tibaldi – Politecnico di Torino Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 97

98. Monocular approach OBJECTIVE Application of the Kalman Filter for the estimation of the position of a target of interest, using OpenCV primitives ST SmartCam (RVS) Tracking from stereo vision Federico Tibaldi – Politecnico di Torino Basilio Bona - DAUIN - PoliTo 98 Target tracking ROBOTICS 01PEEQW - 2015/2016

99. Master students are currently working on the integration of a pan-tilt stereo camera on a mobile platform for target tracking and following Federico Tibaldi – Politecnico di Torino Basilio Bona - DAUIN - PoliTo 99 Target tracking ROBOTICS 01PEEQW - 2015/2016

100.  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 Task allocation Federico Tibaldi – Politecnico di Torino 100 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

101. THANK YOU Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 101

102.  A general algorithm to perform this task is the Bayes Filter: For each time instant compute the current estimate : A PRIORI ESTIMATE (includes the commands in the belief) A POSTERIORI ESTIMATE (includes the measurements in the belief) PREDICTION UPDATE Robot Localization  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

103.  A general algorithm to perform this task is the Bayes Filter: For each time instant compute the current estimate as: A PRIORI ESTIMATE (includes the commands in the belief) A POSTERIORI ESTIMATE (includes the measurements in the belief) PREDICTION UPDATE Robot Localization  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 Luca Carlone – Politecnico di Torino 103 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016

104. 1047/5/2010 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 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 Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 104

105. 1057/5/2010 The central catadioptric camera model that we adopt is the one described in [Scaramuzza et al. 2006] and related Matlab® toolboxMatlab® 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. Basilio Bona - DAUIN - PoliTo ROBOTICS 01PEEQW - 2015/2016 105