Robotics: Science and Systems

Zhibin (Alex) LiSchool of Informatics

University of Edinburgh

Localisation and Mapping

Outline❏ Introduction of mapping❏ Occupancy grid map

❏ Basic assumptions❏ Occupancy update rule❏ Log odds representation❏ Algorithm of occupancy grid mapping

❏ Simultaneous Localisation and Mapping (SLAM)❏ Application domains❏ Basic concepts❏ Data association problem❏ Three main SLAM paradigms

*Further reading2

Recap: What Tools Do We Need?

3

Mapping(What does the area look like?)

Localisation and MappingLocalisation: given the map, compute the poses of the robotMapping: given the poses, compute the map of the environment

Goal: an autonomous robot should be ableto construct a map or floor plan, and use it to localize itself in the constructed map.

4

Mapping with known poses

5

Why mapping?❏Learning/building maps is one of the fundamental problems in mobile robotics.❏Maps allow robots to efficiently carry out their tasks, allow localisation.❏Successful robot systems rely on maps for localisation, path planning, activity planning etc., especially towards autonomy.❏Mapping is one of the core competencies of truly autonomous robots.

6

General problem of mapping

What does the environment look like?

7Nobili, Simona and Scona, Raluca and Caravagna, Marco and Fallon, Maurice, “Overlap-based ICP Tuning for Robust Localization of a Humanoid Robot”, IEEE Intl. Conf. on Robotics and Automation (ICRA), 2017

A popular sensor is LiDAR (Light Detection and Ranging).For each laser beam, it detects the distance by

Each reflection represents an occupied point in space. By gathering all the points reflected by the objects and environment, 3D point clouds are formed.

Modern sensing techniques

8

In addition, GPS provides absolute position of the sensor (LiDAR) and an Inertial Measurement Unit (IMU) provides the precise orientation of the sensor. With these two additional information, we can translate the detected point from LiDAR sensor into static points to build a map in the global coordinate, assuming environment is static.

Modern sensing techniques

9

General problem of mapping

10Tartan Rescue from DARPA robotics challenge

Limited data transmission, representation of environment with lower resolution.

Mapping: basics❏Formally, mapping involves, given the sensor data ,

to calculate the map of maximum likelihood

actions

observations

11

Mapping: basics❏For mapping with known poses, is the odometry (true poses ), to are hence the sequence of motions; to are sequence of measurements

to calculate the map of maximum likelihood given all the data d

12

Learning map is a chicken and egg problem❏In previous lecture, we looked at an example of estimating the pose of the vehicle given the measurement data and the map.❏Mapping, however, involves simultaneously estimating the pose of the vehicle and the map. When the robot moves, it the error of odometry accumulates, which gradually makes the pose less certain.❏The general problem is therefore denoted as the simultaneous localisation and mapping problem (SLAM).❏Throughout this section we will describe how to calculate a map given we know the pose of the vehicle, which is relatively easy.

13

Why is mapping hard?❏Size: when environment is large compared to sensor range❏Noise: if robots’ sensors and actuators were noise free, there wouldn’t be that much of a problem❏Perceptual ambiguity and aliasing: when different places look alike, locally, the learning process needs to properly extract global correspondences❏Cycles: straight line paths, e.g., along a corridor are easier than cycles where errors accumulate by the time one returns to the same place

14

Problems in mapping❏Sensor interpretation

❏How do we extract relevant information from raw sensor data?

❏How do we represent and integrate this information over time?

❏Robot locations have to be estimated❏How can we identify that we are at a previously visited place?

❏This problem is the so-called data association problem.

15

Grid maps1. The world is discretized the into cells2. The grid structure is fixed3. Assume each cell is either occupied

or free4. It is a non-parametric model5. It requires significant memory

storage

16

Occupancy grid maps❏Introduced by Moravec and Elfes in 1985❏Represent environment by a grid.❏Estimate the probability that a location is occupied by an obstacle.❏Key assumptions: Occupancy of individual cells (m[xy]) is independent

Robot positions are known!

17

Updating occupancy grid maps❏Idea: Update each individual cell using a binary Bayes’ filter.

❏Additional assumption: Map is static.

18

Assumptions: occupancy state is binaryEach grid cell is either completely free or occupied, ie binary state.

19

Occupied

Free

Unknown (or a prior estimate, eg 0.2)

Assumptions: independencyEach grid cell is independent from each other.

20

No correlation with each neighbors

Occupancy grid maps❏ Each x-y coordinate is assigned with a binary occupancy value which

specifies whether or not a location is occupied by an object.

❏ Easy to find paths through the unoccupied space, ie collision-free.

21

Occupancy update ruleCompute the ratio of occupancy probabilities, we obtain:

22

=

=

Total probability

Unknown p(mi)=0.5 in a simple case

Only by using this ratio, the multiplication of probability growers higher and higher if the cell is occupied, and vice versa.

Log odds representation❏Use the log-odds representation, turning multiplication into summation.

❏Define log odds ratio as

❏Later, given log odds ratio, we can recalculate

Odds are a ratio of probabilities: ; the log odds of event A are = .

How to understand log odds ratio? Whiteboard exercise.

For example, results in , meaning the log-odds ratio is 1.105:(1+1.105); so the probability is 1.105/(1+1.105)=0.525.

23

Occupancy mapping expressed by log odds By substituting in the log-odds form as defined previously, we obtain the recursive equation (i=x,y in a 2D case)

or simply

where l0 is the prior of occupancy represented as a log-odds ratio.

Note: is called inverse sensor model because it uses sensor measurement to rule out back to its causes, ie what map causes this measurement. 24

Pseudo code of binary Bayes’ filter

25

Only sum operation is required, efficient computation in log-odds representation

Example: sonar range sensingSimilar principle, for each sonar beam: First, consider only the optical axis.

26

z+d1 z+d2

z+d3z

z-d1

Occupancy value depending on measured distance

27

Measured distance, 2 meter is the borderline, p=0.5.

Prior

Linear along the optical axis

Key parameters of the modelAssume linear in yt , and Gaussian in .

28

Update by measured distance and deviation from the optical axisHow the density if influenced by the deviation away from the optical axis in a Gaussian-like manner.

29

Higher uncertainty, large variance (flattened Gaussian)

Lower uncertainty, lower variance (tall Gaussian)

Resulting model for occupancy grid mapsCombination of a linear function and a Gaussian (occupancy probability, given position and reading). Two measured distances:

30

Recursive update of occupancy grids

31

Adding measurement recursively by the update rule represented in log odds form.

Map constructed by ultrasound sensors

32

Occupancy and maximum likelihood map

The maximum likelihood map is obtained by clipping the occupancy grid map at a threshold of 0.5. Note, sonar sensor has less accuracy as indicated here.

33

Occupancy grids (laser scan)

34

Tech Museum, San Jose

CAD map Occupancy grid map35

Mapping from Valkyrie humanoid robot

36

SLAM

37

The SLAM problem❏ SLAM stands for simultaneous localisation and mapping❏ The task of building a map while estimating the pose of the robot

relative to this map

❏ Why is SLAM hard?Chicken and egg problem: ❏ a map is needed to localize the robot ❏ a pose estimate is needed to build a map

Previously, localisation is to infer the location given a known map. The mapping we present here builds a map given known locations. SLAM, however, builds a map and locates the the post of the robot simultaneously.

38

The SLAM problemA robot moving through an unknown, static environment:❏Given:

❏The robot’s controls

❏Observations of nearby features

❏Estimate:❏Map of features

❏Path of the robot

39

SLAM applicationsIndoors

Space

Undersea

Underground

40

Graphical model of full SLAM

41In SLAM, the robot seeks to recover the unobservable variables, estimate the posterior over the entire robot path together with map.

Graphical model of online SLAM

42

Only recover the present location rather than the whole path.

Robot path error correlates errors in the map

SLAM: robot path and map are both unknown!

43

Why is SLAM a hard problem?

Why is SLAM a hard problem?

❏In the real world, the mapping between observations and landmarks is unknown❏Picking wrong data associations can have catastrophic consequences❏Pose error correlates data associations

Robot poseuncertainty

44

Is that a new feature, or a previously observed feature?

Data association problem

❏A data association is an assignment of observations to landmarks❏In general there are more than n observations and m landmarks possible associations❏Also called “assignment problem”

45

Is that a new feature, or a previously observed feature?

Localisation vs SLAM❏ A particle filter can be used to solve both problems❏ Localisation: state space❏ SLAM: state space

❏ for landmark maps = ❏ for grid maps =

❏ Problem: The number of particles needed to represent a posterior grows exponentially with the dimension of state space!

46

Three main SLAM paradigms1. Extended Kalman Filters2. Particle Methods3. *Graph-Based Optimization Techniques

47

Extended Kalman FiltersThe EKF algorithm represents the robot estimate by a multivariate Gaussian

The high-dimensional vector contains the robot’s best estimate of its own current location and the location of the features in the environment.

As presented in previous lecture, it requires a motion function and a measurement function. EKF SLAM linearizes the motion and measurement functions using Taylor series expansion.

It is basically the application of the EKF for solving the online SLAM problem.

48

Particle methodsThe second principal SLAM approach is based on particle filters:

❏ Each particle is best guess to the true value of the state, by collecting a set of particles, the particle filter approximates the posterior distribution.

❏ Particle filter approaches the true posterior as the particle set size goes to infinity.

❏ It is a nonparametric representation and represents multimodal distributions.

❏ FastSLAM (fast simultaneous localisation and mapping) [Montemerlo, M.; Thrun, S.; Koller, D.; Wegbreit, B. (2002). "FastSLAM: A factored solution to the simultaneous localization and mapping problem" (PDF). Proceedings of the AAAI National Conference on Artificial Intelligence. pp. 593–598.] 49

Results – Victoria Park❏4 km traverse❏< 5 m RMS position error❏100 particles

Dataset courtesy of University of Sydney

Blue = GPSYellow = FastSLAM

50

Demo (using FastSLAM Algorithm)

51

Demo (using FastSLAM Algorithm) – Loop Closure

52

Extracurricular reading (not required)RI Seminar: John Leonard : Mapping, Localization, and Self-Driving Vehicles

53