Mapeamento. Mapeamento 20/05/2014. IA368-W Métodos Estocásticos em Robótica Móvel LOCALIZAÇÃO E MAPEAMENTO SIMULTÂNEOS

(1)

IA368-W

Métodos Estocásticos em

Robótica Móvel

• Profs. Eleri Cardozo

• Paulo G. Pinheiro

• Leonardo R. Olivi

1 IA368W - Prof. Eleri Cardozo

LOCALIZAÇÃO E MAPEAMENTO

SIMULTÂNEOS

(SLAM - Simultaneous Localization

and Mapping)

Mapeamento

Mapeamento é o processo pelo qual o robô, utilizando seus sensores proprioceptivos (odometria) e extereoceptivos (laser, sonar, câmeras), deriva o mapa do ambiente. Este mapa pode ser:

• contínuo: retas representando obstáculos intransponíveis; • discreto: células livres e ocupada (occupancy grid); • composto de landmarks;

• topológico: lugares interconectados.

Para mapear o ambiente o robô necessita se locomover através do ambiente, permitindo assim que seus sensores cubram toda a área de interesse.

O mapeamento depende da precisão da odometria do robô: imprecisão na pose gera distorções no mapa.

Mapeamento

Um mapeamento contínuo pode ser obtido com o algoritmo Split-And-Merge após o robô executar um "passeio" pelo ambiente. A precisão do sensor e da odometria irá ditar a precisão das retas e dos landmarks.

Um mapeamento topológico pode ser obtido via detecção de elementos de interconexão de ambiente (passagens de ligação e portas). Como podemos detectar uma passagem ou porta ? No mapeamento discreto o ambiente é segmentado em células de tamanho fixo (da ordem do tamanho do robô). Cada célula possui uma probabilidade (ou outro indicativo) de seu estado que pode ser ocupada (por obstáculos), vazia (área navegável) ou indeterminada.

(2)

Mapeamento

Occupancy Grids (A. Elfes, 1989)

Esta técnica de mapeamento supõe que a odometria do robô é precisa, mas seus sensores estereoceptivos estão sujeitos a erros. Supõe também que o ambiente mapeado se mantém estático durante o mapeamento.

O ambiente é discretizado em células. Cada célula possui duas probabilidades associadas: p(V) e p(O), respectivamente, a probabilidade da célula estar vazia (livre de obstáculos) e estar ocupada por obstáculos. Como os estados vazia e ocupada são os únicos estados possíveis para uma célula: p(V) + p(O) = 1.

Mapeamento

Comumente para espaço amostral binário (como livre/ocupada), define-se a relação que expressa a chance (odds) de um estado ocorrer em relação ao outro:

)

(

1 )

(

log

)

(

)

(

log

)

(

x

p

x

p

x

p

x

p

x

l

−

=

¬

=

Onde x = O (ocupada) e ¬x = V (vazia). Considerando que inicialmente não temos nenhuma informação sobre o mapa, p(O) = p(V) = 0.5, ou

l(O) = l(V)= 0. Valores positivos de lindicam que a chance da célula estar ocupada é maior que a chance dela estar livre. Valores negativos indicam o oposto.

Supondo que efetuamos uma leitura do(s) sensor(es) ztdesejamos

computar l(x|zt), ou seja atualizar as chances das células estarem

ocupadas.

Mapeamento

)

|

(

1 )

|

(

log

)

|

(

t t t

z

x

p

z

x

p

z

x

l

−

=

Aplicando a Regra de Bayes (Filtro de Bayes Binário) obtem-se:

)

(

)

|

(

1 )

|

(

log

)

(

)

(

1

l

0

x

z

x

p

z

x

p

x

l

x

l

t t t t

=

−

+

₋

−

Para cada célula do mapa:

Onde p(x|zt) é a probabilidade da célula estar ocupada dado que o

sensor leu zt(modelo inverso do sensor).

))

(

exp(

1

1 )

(

x

l

x

p

+

−

=

Computado l(x) pode-se recuperar p(x):

OBS: Se l(x) → ∞, p(x) →1 e se l(x) →-∞, p(x) →0

Mapeamento

Como computar o modelo inverso do sensor?

Para um sensor ideal (livre de erros) o modelo inverso é dado por:

z 0.5 1 p(x|z) x z p(x|z) = 0 p(x|z) = 0.5 p(x|z) = 1 α 8 IA368W - Prof. Eleri Cardozo

(3)

Mapeamento

Modelo inverso de sensor com erro na determinação da distância:

k é um fator de ponderação para o pico da função ficar entre 0.5 e 1.0. Pmin

1.0

Mapeamento

k = 0.1 Pmin= 0.1

OBS: Esta função não é uma PDF !

z

α

Mapeamento

Alternativamente, podemos definir um modelo inverso de sensor de forma mais simples:

z x p(x|z) 0.5 1 z-d1 z+d1 z+d2 z+d3 Pmin 11 IA368W - Prof. Eleri Cardozo

Mapeamento

Se o sensor possuir erros de orientação (bearing) podemos introduzir esta incerteza no modelo inverso do sensor. Supondo que os erros de distância e orientação sejam não correlacionados:

























+

−

+

=

2 2 2 2

)

(

2

1 exp

)

5 .

0

2 (

)

,

|

(

θ θ

σ

θ

σ

πσ

θ

d d t t

z

x

P

k

P

z

x

p

P = Pmin, se 0 ≤x ≤z 0.5 se x ≥z z θ 12 IA368W - Prof. Eleri Cardozo

(4)

Mapeamento

k = 0.2 Pmin= 0

Mapeamento

Modelo de sonar (Moravec & Elfes, 1985):

[

]

2 )

(

)

(

)

(

1 )

,

|

(

x

z

θ

O

z

V

z

A

θ

p

=

+

−

Ψ: abertura do sonar (10/15o₎ z δ θ

Para uma célula na posição [δ,θ] (coordenadas polares no frame do sonar): , se -Ψ ≤ θ ≤ Ψ, ou 0 caso contrário 2 2 ) (       Ψ = θ θ A 2 1 ) (       − − = δ_εz z O , se z-ε ≤ δ ≤z-ε, ou 0 caso contrário (εé a precisão da distância medida pelo sonar)

2 1 ) (       − = z z V δ , se 0≤ δ ≤z-ε, ou 0 caso contrário 14 IA368W - Prof. Eleri Cardozo

Mapeamento

Algoritmo de mapeamento:

1. Associe a cada célula i l(x) = 0 2. Faça uma leitura do(s) sensor(es) - z,θ.

3. Para as células i no campo de cobertura do sensor compute

lt,i= lt-1,i+ INV_SENSOR(i,z,θ) - l0

4. Se o critério de parada for satisfeito, retorne p(x) para cada célula do mapa

4. Movimente o robô e vá para 2.

Mapeamento

Como a imprecisão do laser é muito menor que o tamanho da célula, pode-se utilizar o número de leituras que detectou obstáculo em uma célula como um indicativo da probabilidade da mesma estar ocupada por um obstáculo. Cada célula possui um contador iniciado como zero:

Decrementa-se 1 nas células atravessadas pelo raio

Incrementa-se 1 nas células onde foram detectados obstáculos

A final da exploração do ambiente:

• Se o valor do contador for maior que N →P(x) =1 • Se o valor do contador for menor que -N →P(x) = 0 • P(x) = 0.5, caso contrário.

(5)

SLAM

Vimos que:

1. Dado um mapa do ambiente, o robô pode corrigir sua pose com base neste mapa (Filtros de Kalman e Partículas).

2. Dada uma odometria livre de erros, o robô pode derivar um mapa do ambiente (Occupancy Grid).

Nenhuma destas hipóteses é totalmente verdadeira na prática: 1. Tal mapa do ambiente raramente existe.

2. A odometria é sempre imprecisa.

SLAM é o processo pelo qual o robô levanta o mapa do ambiente e simultâneamente utiliza este mapa para se localizar.

Problema do ovo e da galinha:

• Para se localizar o robô necessita do mapa. • Para obter o mapa o robô precisa se localizar.

SLAM

Localização X Mapeamento X SLAM

Localização: Calcular p(x | u, z, m) Mapeamento: Calcular p(m | x , z) SLAM: Calcular p(x, m | u , z)

Algoritmos de SLAM podem operar de duas formas:

Online: O mapa e a pose são atualizados com base apenas na

pose anterior do robô.

Full: O mapa e a pose são atualizados com base no caminho

percorrido pelo robô até o momento anterior (todas as poses anteriores).

SLAM

Existem basicamente 3 algoritmos de SLAM baseados em sensores de distância e mapas de landmarks:

1. EKF SLAM (online): utiliza o Filtro de Kalman Estendido. A pose e os landmarks são relacionados no vetor de estado.

2. GraphSLAM (full): utiliza equações lineares que relacionam a pose e os landmarks.

3. FastSLAM (online e full): combina Filtro de Partículas e EKF para relacionar a pose e os landmarks.

SLAM

X

t-1

X

t

X

t+1

X

t+2 Poses do robô (todas ou a última)

u

t-1

u

t

u

t+1

u

t+2

Z

t-1

Z

t

Z

t+1

Z

t+2

m

Controles aplicados ao robô Landmarks detectado Mapa 20 IA368W - Prof. Eleri Cardozo

(6)

Extended Kalman Filter (EKF) SLAM

EKF SLAM

Idéia













m

r

A pose do robô E a posição dos landmarks são estimadas ao mesmo tempo. z

EKF SLAM

Esta estratégia baseada no Filtro de Kalman estima simultaneamente a pose do robô e a posição (x,y) dos landmarks. O vetor de estados é dado por:













yN xN y x t t t

m

y

x

...

1 1

θ

posição do landmark 1 posição do landmark N pose do robô

=













m

r

A dimensão de m cresce a medida que novos landmarks são detectados.

Mapa (N landmarks)

EKF SLAM

Existem 2 variantes do EKF SLAM:

1. Com correspondência entre landmarks: detectado um landmark, é possível identificá-lo (indexá-lo) com precisão.

2. Sem correspondência entre os landmarks: a identificação do landmark é imprecisa, ou seja, o landmark i pode ser identificado incorretamente como o landmark j.

O primeiro caso requer uma caracterização (assinatura) adicional do landmark para distinguí-lo dos demais. Caso não temos tal assinatura (segundo caso) temos que assegurar que os landmarks são suficientemente separados fisicamente para que a correspondência possa se dar por verossimilhança.

(7)

EKF SLAM

                ∆ + ∆ ∆ + ∆ − = − − 1 0 0 ) 2 cos( 1 0 ) 2 sin( 0 1 1 1 , t t t t t t t M s s G

θ

t T t t t t t t t

R

G

m

u

g

+

Σ

=

Σ

=

− − 1 1

,

)

,

(

µ

Predição Matriz 3x3 - linearização do modelo cinemático para um robô diferencial.

EKF SLAM













=

I

G

Mt t

0

, t T t M t t M t

R

I

G

I

G

+













Σ













=

Σ

−

0

_, 1 ,













Σ

=

Σ

mm mr rm rr (3+2N)x(3+2N) A sub-matrix identidade I é de tamanho 2Nx2N. Isto implica que os landmarks são estáticos e independentes.

As sub-matrizes 0 são de dimensão 2Nx2N e implica independência entre a pose do robô e os landmarks do mapa. (3+2N)x(3+2N)

N: No_{de landmarks.}

EKF SLAM

Fase de Atualização

Inicialmente computa-se a previsão das medidas dos sensores em função da pose do robô e da posição dos landmarks presentes no vetor de estado.













−

=













=

y t y k x t x k y x , , , ,

µ

δ

(

)













−

=

θ

µ

δ

,

2 arctan

ˆ

t x y k t

q

z

δ

T

q

=

Landmark estimado em função da pose estimada do robô. pose do robô posição do landmark k 27 IA368W - Prof. Eleri Cardozo

EKF SLAM

                = 0 ... 0 1 0 0 ... 0 0 0 0 0 ... 0 0 1 0 ... 0 0 0 0 0 ... 0 0 0 0 ... 0 1 0 0 0 ... 0 0 0 0 ... 0 0 1 0 0 ... 0 0 0 0 ... 0 0 0 1 ,k x F k 2k -2 2N -2k 3 k+1

Pose do robô Landmark k

y x x y x y y x y x k t

F

q

H

,

0

1 













−

=

δ

[

N

]

t t y x k t z z m m y x H 1 ...                     ∂ ∂∂ ∂ ∂∂ ∂∂ ∂ = _θ

Determinação do Jacobiano do modelo de observação (função h):

(8)

EKF SLAM

1

)

(

Σ

+

−

Σ

=

t T t t t T t t k t

H

Q

K

Ganho do Filtro:













=

2 2

0

θ

σ

d t

Q

(3+2N)x2 (3+2N)x(3+2N) 2x2 2x2 (3+2N)x2 Kk

té contribuição do landmark k para atualizar a pose do robô, o

próprio landmark k e os demais landmarks.

Variânça do sensor em distância e angulação.

EKF SLAM

Inovação:

O robô realiza uma detecção de landmark no instante t a partir de seus sensores (Split-And-Merge).

Caso seja detectado um novo landmark (não presente no vetor de estados), o mesmo é adicionado no vetor de estados (aqui é assumida a correspondência entre landmarks):

[

_k

]

T t k t k t

r

z

=

φ













+













=













)

sin(

)

cos(

, , , , θ θ

µ

φ

µ

φ

µ

t k t k t t k t k t y t x t yk xk

r

m

EKF SLAM

)

ˆ

(

k t k t k t t t

=

µ

+

K

z

−

z

µ

Cômputo das novas médias µe matriz de covariança Σ:

t t t t

=

I

−

K

H

Σ

(

)

Obs: Como não temos a posição exata dos landmarks (Lx, Ly), a

inovação é computada a partir de duas estimativas:

1. Computando as medidas a partir do estado atual do filtro, ou seja, das médias (pose do robô e posição dos landmarks) presentes no vetor de estado.

2. Medindo diretamente a partir da pose atual do robô (estimada pelo modelo cinemático ou pela odometria do robô).

EKF SLAN

Delalhes de Implementação

O robô inicia em uma pose arbitrária, comumente [0 0 0]T_.

A matriz de covariança inicial é iniciada como:

Quando um novo landmark é detectado no instante t, seu desvio padrão na matrix de covariança (σ2

mxe σ2my) são inicializados com σ2

d(desvio padrão da distânvia lida pelo sensor).

          = Σ 0 0 0 0 0 0 0 0 0 0                 = Σ 2 2 2 2 2 2 2 2 2 2 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 d d y x y yy yx x xy xx t σ σ σ σ σ σ σ σ σ σ σ θθ θ θ θ θ

No final do processo os landmarks são obtidos no referencial do robô.

(9)

FastSLAM

Algoritmo introduzido por Montemerlo, Thrun, Koller e Wegbreit (2002).

Idéia

Use Filtro de Partículas para estimar a pose do robô. Cada partícula estima a posição dos landmarks com Filtros de Kalman

FastSLAM

Aplica o teorema de Rao-Blackwell (Rao-Blackwellization) ao problema de SLAM.

p(x

1:t

, m | z

1:t

, u

0:t-1

) = p(x

1:t

| z

1:t

, u

0:t-1

) . p(m | x

1:t

, z

1:t

)

Se mapa consiste de M landmarks: m1, ... mM:

p(x1:t , m1:M| z1:t, u0:t-1) = p(x1:t| z1:t, u0:t-1) . ∏p(mi| x1:t, z1:t) Problema de localização Problema de mapeamento Problema de SLAM i=1 M 35 IA368W - Prof. Eleri Cardozo

FastSLAM

p(x

1:t

| z

1:t

, u

0:t-1

)

FastSLAM utiliza um filtro de partículas para computar este termo. Cada partícula carrega seu próprio mapa (landmarks):

x y θθθθ µµµµ₁ ΣΣΣΣ₁ pose do

robô

posição dos landmarks (x, y) e covariança Partícula µµµµ₂ΣΣΣΣ₂ µµµµ_MΣΣΣΣ_M













=

yk xk k

µ













=

Σ

2 2 2 2 yy yx xy xx k

_σ

σ

média da posição do landmark k covariança da posição do landmark k

...

(10)

FastSLAM

p(m

i

| x

1:t

, z

1:t

)

FastSLAM usa Filtro de Kalman Estendido para calcular este termo. O robô realiza uma leitura de seus sensores e determina a posição e angulação (vetor z) dos landmarks a seu alcance a partir da pose estimada no instante atual.

Para cada landmark detectado j e para todas as partículas k: 1. Se o landmark j ainda não foi inicializado na partícula k, sua

média e covariança relativos à pose da partícula são inicializadas.

2. Caso contrário, a média e a covariança do landmark j são atualizadas na partícula k via EKF.

FastSLAM

0 1 1 ] [ , ] [ , ] [ ] [ 1 ] [ , ] [ , , ] [ , ,

)

(

)

,

(

)

,

(

p

w

H

Q

H

x

h

H

x

z

h

k T t k t j k t j k t k t t k t j k t j y k t j x

=

Σ

′

=













− − −

µ

Predição: para cada partícula k e para cada landmark j ainda não

inicializado nesta partícula:

pose da partícula k leituras obtidas pelo robô













=

2 ₂

0

θ

σ

d t

Q

covariança do sensor 38 IA368W - Prof. Eleri Cardozo

FastSLAM













−

+

−

=

θ

p

l

p

l

p

l

p

l

p

h

z

x x y y y y x x

)

,

(

2 arctan

)

(

)

(

)

,

(

2 2 pose do robô/partícula posição do landmark h-1_{: Dado z e p encontre l} h' : Jacobiano de h (H) 2 ] [ , , ] [ , 2 ] [ , , ] [ , ] [ , ] [ , , ] [ , ] [ , , ] [ , ] [ , , ] [ , ] [ , , ] [ , ] [

)

(

)

(

1

0 )

,

(

k t y j k t y k t x j k t x k t x k t x j k t y k t y j k t y k t y j k t x k t x j k t j k t

x

q

x

q

x

q

x

q

x

H

µ

−

+

−

=













−

=

FastSLAM

r φ r













] [ , , ] [ , , k t y j k t x j

µ













] [ , ] [ , ] [ , k t k t y k t x

x

θ Robô Partícula k

)

sin(

.

)

cos(

.

] [ , ] [ , ] [ , ] [ , ] [ , ] [ ,

φ

µ

φ

µ

θ θ

+

=

+

=

k t k t y k t y k t k t x k t x

x

r

x

r

x

h-1 40 IA368W - Prof. Eleri Cardozo

φ

(11)

FastSLAM

Atualização: Se o landmark j já foi inicializado na partícula k

(

) (

)

(

)



_









−

+

−

=

] [ , , ] [ , , ] [ , , , ] [ , , 2 , , ] [ , , 2 ] [ , , ] [ , ] [ ,

,

2 arctan

ˆ

k t j k t x j k t x j t y j k t y j j t y j k t x j k t x j k t x k t j

x

z

θ

µ

A diferença entre estes dois vetores é a inovação do EKF.

[

]

T t t t

r

z

=

φ

FastSLAM

] [ 1 , ] [ , ] [ ] [ 1 , ] [ , 1 ] [ 1 , ] [ 1 ,

)

(

)

ˆ

(

k t j k t j k t t k t j k t j T k t j t T k t j

KH

I

z

K

Q

H

K

Q

H

Q

− − − − −

Σ

−

=

Σ

−

+

=

Σ

=

+

Σ

=

µ

Atualização da posição do landmark j pela partícula k:

Estas operações são feitas para todo o landmark já inicializado e detectado no instante t pelo robô e para cada partícula k.

FastSLAM

Determinação do peso das partículas













₋

=

₍

_ˆ

₎

−

₍

_ˆ

₎

2

1 exp

2

1

[] 1 [] ] [ k t t T k n t k

z

Q

z

Q

w

π

Q é a covariança da medida do sensor computada anteriormente para a partícula. As partículas cujas estimativas dos landmarks que mais se aproximam da estimativa dos sensores do robô têm um peso maior.

∑

=

M p p k k

w

1 ] [ ] [ ] [

Normalização dos pesos. detectados pelo robô

estimados pela partícula

FastSLAM

Algoritmo

Inicialização:

1. Gere Ninitpartículas na área a ser mapeada.

2. Faça uma leitura do sensor de distância do robô.

3. Determine a posição dos landmarks (por exemplo, cantos com o algotitmo Split-and-Merge). A posição é dada pela distância e angulação em relação ao referencial do robô.

4. Inicie os EKF correspondentes de cada landmark detectado em todas as partículas (µ, Σ) e compute os pesos das partículas.

(12)

FastSLAM

Laço principal

1 . A partir dos sensores do robô compute a distância e orientação (vetor z) dos landmarks.

2. Para cada partícula k:

2.1. Movimente a partícula de acordo com a movimentação do robô (pode-se utilizar o modelo cinemático do robô com as velocidades Vde Veobtidas do robô).

2.2. Obtenha a pose estimada da partícula. 2.3. Atualize a posição dos landmarks via EKF. 2.4. Compute o peso da particula.

3. Reamostre as partículas com maior peso e vá para 1.

FastSLAM

IA368W - Prof. Eleri Cardozo 46 Determinação da pose do robô e da posição dos landmarks:

A convergência do algoritmo pode ser medida pela: 1. Covariança dos landmarks.

2. Covariança da pose do robô.

A pose do robô e a posição dos landmarks pode ser computada pela média dos valores presentes nas partículas, ponderada pelo respectivo peso da partícula.

É interessante que durante o mapeamento e localização o robô detecte diversas vezes os landmarks para permitir a convergência tanto dos Filtros de Kalman quantos do Filtro de Partículas (loop closure).

FastSLAM 2.0

IA368W - Prof. Eleri Cardozo 47 No algoritmo FastSLAM a movimentação das partículas depende unicamente do controle imposto ao robô. Se o modelo utilizado pelas partículas divergir da movimentação real do robô a convergência do algoritmo é prejudicada ou pode não ocorrer. FastSLAM 2.0 utiliza tanto o controle quanto as medidas do robô para movimentar as partículas.

A informação sobre as mediadas é indroduzida no Jacobiano de H: Hx: Jacobiano com relação à pose (3 x 2)

Hm: Jacobiano com relação ao mapa (landmarks) (2 x 2)

FastSLAM 2.0: Idéia básica

IA368W - Prof. Eleri Cardozo 48

r_t-1 φt-1 Landmark j rt-1 φt-1 Landmark j r_t φt

Distribuição das partículas em função da trajetória percorrida pelo robô (linha azul).

Distribuição das partículas em função da trajetória percorrida e das medidas realizadas pelo robô (linhas azul e vermelha).