Learning in Autonomous Systems Proff. Luca Iocchi, Giorgio Grisetti - - PowerPoint PPT Presentation

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

University of Rome “La Sapienza” Master in Artificial Intelligence and Robotics

Learning in Autonomous Systems

• Proff. Luca Iocchi, Giorgio Grisetti

Course web site: http://www.dis.uniroma1.it/∼iocchi/Teaching/las/

Giorgio Grisetti Particle Filters For Localization 1 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Overview

Definition of the problem Characterizationof the state space and the control space Characterization of the control noise Sampled Transition Model Likelihood Efficient computation

Giorgio Grisetti Particle Filters For Localization 2 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Problem Definition

we want to design a particle filter for 2D robot localization

• ut robot will be moving on a plane and have an orientation

we know the map of the environment as a 2D grid, where each cell of the grid can be occupied, free or unknown we will use a laser scanner as a sensor, that reports the distance measured at fixed angular intervals

Giorgio Grisetti Particle Filters For Localization 3 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Problem Definition: state

The state of our system xt ∈ SE2 is a point with orientation It can be represented as a triplet xt =   xt yt θt   (1)

• r in matrix form

Xt =   cos θt − sin θt xt sin θt cos θt yt 1 c   (2) It is clearly possible to pass from one representation to the other. To this extent we introduce the t2v and v2t functions. Xt = v2t(xt) (3) xt = t2v(Xt) (4)

Giorgio Grisetti Particle Filters For Localization 4 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Problem definition: measurements

The laser returns a set of distances measured at fixed angle intervals: zt = {zj}, thus our laser measurement will consist of a set of range measurements. To simplify the further calculations we will consider the endpoints of each beam (wich is a 2D point). zcart

j

= zj cos αj sin αj

• (5)

where αj is known from the laser specs.

Giorgio Grisetti Particle Filters For Localization 5 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Problem definition: controls and transitions

Controls of our system come as offsets measured through the

• dometry. Thus ut ∈ SE2 lives in the same space as the state xt.

If my robot sits in a certain position xt and receives the control ut he will move to the next position xt+1. This can be expressed using the matrix form as Xt+1 = XtUt (6)

• r in vector form as

xt+1 = (xt, ut) (7) = t2v(v2t(xt)v2t(ut)) (8) This is our noise-free transition function that predicts the next state from the previous one

Giorgio Grisetti Particle Filters For Localization 6 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Prediction

Realizing the predict step in a particle filter requires to define a sampler for the transition model x(i)

t+1 ∼ p(xt+1|x(i) t , ut)

(9) To this extent we might consider the transitions perfect, and only the controls affected by noise x(i)

t+1 ∼ p(xt+1|x(i) t , ut + nt)

(10) the idea is then to draw a sample n(i)

t

∼ p(nt) and compute the next sample according to 8 as x(i)

t+1 = t2v(v2t(x(i) t )v2t(ut + n(i) t ))

(11)

Giorgio Grisetti Particle Filters For Localization 7 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Characterizing the noise

the odometry noise grows proportionally with the distance traveled a reasonable implementation would be to draw noise from a Gaussian distribution whose covariance grows with the traveled distance let ut ∼ N(0|Σt) (12) where Σt = diag

• σ2

x, σ2 y, σ2 θ

• higher covariances will correspond to higher noise, thus more

“spread” samples Our goal is to find a heuristic that correlates the control ut with the covariance coefficients σ2

x, σ2 y, σ2 θ.

If the robot does not move, the covariance is zero.

Giorgio Grisetti Particle Filters For Localization 8 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Characterizing the noise

A simplistic implementation   σ2

x

σ2

y

σ2

θ

  =   a11 a12 a13 a21 a22 a23 a31 a32 a33  

• A

  ||ux|| ||uy|| ||uθ||   (13) Here the covariances are obtained as a linear combination of the magnitude of the motion of the system, through the matrix A.

• nce the covariances are generated (see method prepareSampling),

we can generate samples and perturbate our particle vector(see method sample) by applying Eq. 11.

Giorgio Grisetti Particle Filters For Localization 9 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Characterizing the control noise

the odometry noise grows proportionally with the distance traveled a reasonable implementation would be to draw noise from a Gaussian distribution whose covariance deviation grows with the traveled distance let ut ∼ N(0|Σt) (14) where Σt = diag

• σ2

x, σ2 y, σ2 θ

• higher covariances will correspond to higher noise, thus more

“spread” samples Our goal is to find a heuristic that correlates the control ut with the covariance coefficients σ2

x, σ2 y, σ2 θ.

If the robot does not move, the covariance is zero.

Giorgio Grisetti Particle Filters For Localization 10 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

The predict step becomes

compute σ2

x, σ2 y, σ2 θ using eq 13 (see function prepareSampling

  σ2

x

σ2

y

σ2

θ

  = A   ||ux|| ||uy|| ||uθ||   for each particle, compute its successor

sample u(i)

t

∼ N(0|Σt) compute x(i)

t+1 ∼ p(xt+1|x(i) t , ut + n(i) t )

Giorgio Grisetti Particle Filters For Localization 11 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update

define a prediction function ˆ z = h(x) (15) that computes the predicted laser scan ˆ z if the robot was in position

• x. The prediction could be obtained for instance through ray-tracing.

consider a noise distribution for the indivdual range scan (e.g a gaussian centered around the prediction). regarding the individual beams as independant p(z|x) =

• i

p(zj|x) (16) ∝

• j

exp − (zj − ˆ zj)2 σ2

i

• (17)

= exp  −

• j

(zj − ˆ zj)2 σ2

j

  (18)

Giorgio Grisetti Particle Filters For Localization 12 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update: Issues

computing the prediction for one beam requires ray-casting (expensive) this operation has to be repeated for each beam and each particle the weights obtained by the Eq 18 are highly selective Beware of the sigma: they are much larger than what you think.

Giorgio Grisetti Particle Filters For Localization 13 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Distance Map

we can easily calculate a lookup grid that stores in each cell u, v the distance from the closest occupied cell u′, v′ in the grid map. du,v = min

u′,v′ ||(u, v) − (u′, v′)||

(19) this data structure is called distance-map. the calculation of the distance-map can be done by executing a breadth first visit of the grid where the occupied cells are marked. the idea is to expand the search frontier along regions that have the same “distance”. see the file distancemap.h determining the distance between a point and the closest obstacle requires just a lookup in the distance map.

Giorgio Grisetti Particle Filters For Localization 14 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update: Distance Map Based

Given a candidate robot pose x we can easily calculate the predicted endopoints of the laser scan z in the world frame zmap

j

= Xzcart (20) considering a single endpoint in the map frame, we can compute the distance between this point and the closest occupied point in the map dj = min

k (||zmap j

− pk||) (21) here {pk} are all occupied points in the map if the distance is small the remapped endpoint is close to an occupied point, and its distance is small. Otherwise the distance is big.

Giorgio Grisetti Particle Filters For Localization 15 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update: Distance Map Based

exploiting the grid discretization we can compute dj through a simple lookup as u v

• =

world2grid(Xzcart

j

) (22) dj ≃ du,v (23) This is an O(1) operation

Giorgio Grisetti Particle Filters For Localization 16 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update: Likelihood

So far, given a particle x(i), we can quickly retrieve the distances between each endpoint of the current measurement zi, and the closest occupied obstacle We can build an approximation of the ray-casting based likelihood by considering in the exponent the distances dj instead of the difference between measured and predicted beam p(z|x) ∝ exp  −

• j

d2

j

σ2

j

  (24)

Giorgio Grisetti Particle Filters For Localization 17 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update: Likelihood

In addition to that we might render the measure independant from the number of beams of our laser scanner. To this extent, we can normalize the distances by the number of good beams. This is a heuristic p(z|x) ∝ exp  − 1 #beams

• j

d2

j

σ2

j

  (25)

Giorgio Grisetti Particle Filters For Localization 18 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Update: Particle Depletion

In global localization it is good to prevent the filter to suppress particles too quickly. Having more “persistant” particles gives more time to the algorithm to collect evidence to determine the correct position. This is particularly evident when the environments are higly self-similar. To do this one can artificially inflate the weight adding a constant factor wmin to all weights. w(i) = exp  − 1 #beams

• j

d2

j

σ2

j

  + wmin (26)

Giorgio Grisetti Particle Filters For Localization 19 / 20

Sapienza University of Rome, Italy - Learning in Autonomous Systems (2015/16)

Handing Unknown and Other Hacks

The unknown regions in the map usually cover a large region of the

• space. Covering these region with samples would consume a lot of

them In standard localization we do not want these regions to be occupied by the robot. When triggering global localization, we compute in advance the free space of the map and uniformly spread the samples only in the regions that are not unknown and not occupied. See sampleFromFreeSpace. Again, during global localization, we “Reset” all particles that end up in an unknown (gray) region by replacing them with samples drawn from free space. This functionality can be disabled under position tracking, when the pose is known.

Giorgio Grisetti Particle Filters For Localization 20 / 20