Localização em Robótica Móvel Odometriausers.isr.ist.utl.pt/~mir/cadeiras/robmovel/Odometry.pdf · • A rotação de uma das rodas faz rodar um tambor em torno do contentor

Localização em Robótica Móvel

Odometria

Maria Isabel [email protected]

Instituto Superior Técnico (IST)Instituto de Sistemas e Robótica (ISR)

Av.Rovisco Pais, 11049-001 Lisboa

PORTUGAL

Outubro.1999

All the rights reserved

1999 - © M. Isabel RibeiroRobótica Móvel Odometry

Classificação de Métodos de Localização

■ Localização com Medidas Relativas

Dead-Reckoning◆ Odometria◆ Navegação Inercial

■ Localização com Medidas Absolutas

■ Localização com Medidas Relativas + Medidas Absolutas

◆ Faróis activos◆ Reconhecimento de marcas artificiais◆ Reconhecimento de marcas naturais◆ Emparelhamento com o modelo


Odometria

■ Determinação da localização do robot móvel através da observação e integração consecutiva do movimento das rodas

■ ODÓMETROS - Instrumentos que medem a distância percorrida por veículos.

◆ Encoders - sensores mais usuais■ Vitruvius descreveu o conceito no séc. 1 ac.■ Leonardo da Vinci (1500) construi um aparato que

usava pedras para calcular a distância percorrida.


O odómetro de Leonardo da Vinci

■ Codex Atlanticus - Biblioteca Ambrosiana - Roma, Itália


O odómetro de Leonardo da Vinci• Contentor, fixo à estrutura do

veículo, cheio com pedras e com um orifício na parte inferior.

• A rotação de uma das rodas faz rodar um tambor em torno do contentor.

• Tambor tem um orifício do mesmo tamanho do existente no contentor.

• Depois de um certo número de rotações da roda, os dois orifícios coincidem e uma pedra cai, sendo recolhida numa caixa.

• O número de pedras recolhidas na caixa num intervalo de tempo permite estimar a distância percorrida.


Fontes de erro na odometria

Integração de informação sobre movimentos incrementais acumulação de erros

■ Erros sistemáticos◆ são característicos do robot e/ou dos sensores

✦ desigual diâmetro ou desalinhamento das rodas, ✦ diâmetro das rodas diferente do valor nominal,✦ incerteza sobre o ponto de contacto da roda

◆ em solos regulares contribuem mais fortemente para o erro total do que os erros não sistemáticos

◆ são graves porque são fonte constante de erros aditivos


Fontes de erro na odometria

Integração de informação sobre movimentos incrementais acumulação de erros

■ Erros não sistemáticos◆ são característicos da relação do robot com o ambiente

✦ movimento sobre solos não uniformes (rugosos), ✦ movimento sobre obstáculos inesperados no solo,✦ escorregamento das rodas

• solo escorregadio• grandes acelerações do veículo• rotações rápidas• …….

◆ em solos irregulares estes podem ser os erros dominantes


Irregularidades no pavimento

■ Comando de movimento - velocidade igual em ambas as rodas

■ Robot móvel com controlo diferencial

-1

-0,5

0

0,5

1

1,5

2

2,5

3

0 10 20 30 40 50 60 70

Trajectória real

Trajectória dada pelo modelo odométrico

X(cm)

Y(cm)


NOTAÇÃO

{R})k(y

)k(x

)(kθθθθ

WY

{W}WX

====

)()()(

)(kkykx

kXθθθθ

• A localização refere-se ao ponto médio do eixo entre as duas rodas motoras

• Eixo com comprimento L

posição

e orientação


Modelo da odometria

■ Descreve a evolução temporal da localização do robot móvel como função de

◆ entrada U(k),◆ v(k) - fonte de ruído (deformações e escorregamento das rodas, vibrações,

….. ) - assumida como N(0,Q(k))

)k(v))k(U),k(X(f)k(X ++++====++++1

[[[[ ]]]]T)k()k(D)k(U θθθθ∆∆∆∆∆∆∆∆==== ◆ ∆∆∆∆D(k) - distância percorrida pela origem do referencial do robot

no intervalo◆ ∆θ∆θ∆θ∆θ(k) - variação da orientação no mesmo intervalo

[ [1kk t,t +


■ Dados:

◆ Modelo do sistema

◆ Estimativa da localização no instante k

◆ Incerteza associada

◆ Leituras de odometria (estimativas)

◆ Caracterização do erro

)k(ˆ),k(D θθθθ∆∆∆∆∆∆∆∆

)k(X

Estimativa da Localização por Odometria

)k(XΣΣΣΣ

)k(v))k(),k(D),k(X(f)1k(X ++++θθθθ∆∆∆∆∆∆∆∆====++++

)k(Q

)1k(X ++++

)1k(X ++++ΣΣΣΣ

■ Pergunta:

◆ qual é a estimativa da localização no instante k+1?

◆ qual é a incerteza associada?


Estimativa da Localização por Odometria◆ Dadas as leituras dos odómetros )k(D),k(D de ∆∆∆∆∆∆∆∆

??)k(D),k(D de ∆∆∆∆∆∆∆∆ )k(),k(D θθθθ∆∆∆∆∆∆∆∆

∆−∆=θ∆

∆+∆=∆

L)k(D)k(D)k(

2)k(D)k(D)k(D

ed

ed

)k(v))k(),k(D),k(X(f)1k(X ++++θθθθ∆∆∆∆∆∆∆∆====++++

◆ O que é a função f( ) ?

))k(Q,0(N~)k(v◆ Quais são as características do ruído de estado?

=

)k(Q000)k(Q000)k(Q

)k(Q

32

1


Caracterização do erro

■ Crowley - ICRA92

=

)k(Q)k(Q

)k(Q)k(Q

3322

11

)k(K)k(DK)k(Q)k(sin)k(DK)k(Q)k(cos)k(DK)k(Q

D33

D22

D11

θ∆+∆=

θ∆=

θ∆=

θθ

• KD - coeficiente de drift de translacção de odometria relativo a ∆D (m2/m)

• KDθ - coeficiente de drift de rotação de odometria relativo a ∆D (rad2/m)

• Kθ - coeficiente de drift de rotação de odometria relativo a ∆θ (rad2/rad)

• KD= 0.001 m2/m

• KDθ =0.0003 rad2/m

• Kθ = 0.001 rad2/rad

Valores típicos


Modelo cinemático

{R})t(y

)t(x

)t(θ

WY

{W} WX

vd(t) - velocidade da roda direita

ve(t) - velocidade da roda esquerda

L - comprimento do eixo entre as duas rodas motoras

−=θ

θ+=

θ+=

L)t(v)t(v)t(

))t(sin(2

)t(v)t(v)t(y

))t(cos(2

)t(v)t(v)t(x

ed

ed

ed

!

!

!Relaciona:

• as velocidades das rodas com

• as velocidades (linear e angular) do referencial do veículo no referencial global


Modelo cinemático

∫

∫

∫

τ+θ=θ

ττθ+=

ττθ+=

τ−τ

τ+τ

τ+τ

t

t L)(v)(v

k

t

t 2)(v)(v

k

t

t 2)(v)(v

k

k

ed

k

ed

k

ed

d)t()t(

d))(sin()t(y)t(y

d))(cos()t(x)t(x

• Discretização do modelo contínuo

∫

∫

∫

+

+

+

τ+θ=θ

ττθ+=

ττθ+=

τ−τ+

τ+τ+

τ+τ+

1k

k

ed

1k

k

ed

1k

k

ed

t

t L)(v)(v

k1k

t

t 2)(v)(v

k1k

t

t 2)(v)(v

k1k

d)t()t(

d))(sin()t(y)t(y

d))(cos()t(x)t(x

Para t> tk


Modelo cinemático

■ Como variam, no intervalo [tk,tk+1[ as velocidades vd(t) e ve(t)?

vd(t)

ve(t)tk tk+1

TRANSLAÇÃO seguida de uma ROTAÇÃOsobre o eixo de rotação

TRANSLAÇÃOROTAÇÃO


f(.) - Hipóteses simplificativas

■ TRANSLAÇÃO seguida de uma ROTAÇÃO sobre o eixo de rotação

)k(T )k(θθθθ∆∆∆∆

θθθθ∆∆∆∆++++====∆∆∆∆

θθθθ∆∆∆∆−−−−====∆∆∆∆

2L)k()k(T)k(D

2L)k()k(T)k(D

d

e

θθθθ∆∆∆∆====∆∆∆∆−−−−∆∆∆∆====θθθθ∆∆∆∆

====∆∆∆∆++++∆∆∆∆====∆∆∆∆

)k(L

)k(D)k(D)k(

)k(T2

)k(D)k(D)k(D

ed

ed

e

d


f(.)-Hipóteses simplificativas

)k(y

)k(x

)k(θθθθ

WY

{W} WX)1k(x ++++

)1k(y ++++

)k(θθθθ∆∆∆∆

)k(D∆∆∆∆

■ TRANSLAÇÃO seguida de uma ROTAÇÃO sobre o eixo de rotação

)k(v)k()k(

))k(sin()k(D)k(y))k(cos()k(D)k(x

)1k()1k(y)1k(x

+

θ∆+θθ∆+θ∆+

=

+θ++

f(X(k),∆∆∆∆D(k),∆θ∆θ∆θ∆θ(k))


f(.) - Hipóteses simplificativas■ TRANSLAÇÃO seguida de uma ROTAÇÃO sobre

o eixo de rotação

)k(y

)k(x

WY

{W} WX)1k(x ++++

)1k(y ++++)k(D∆∆∆∆

)k(θ∆


f(.)-Hipótese simplificativa■ TRANSLAÇÃO seguida de uma ROTAÇÃO sobre

o eixo de rotação

)k(v)k()k(

))k(sin()k(D)k(y))k(cos()k(D)k(x

)1k()1k(y)1k(x

+

θ∆+θθ∆+θ∆+

=

+θ++

f(X(k),∆∆∆∆D(k),∆θ∆θ∆θ∆θ(k))

)k(y

)k(x

WY

{W} WX)1k(x ++++

)1k(y ++++)k(D∆

)k(θ∆

)k(θ

)1k( +θ

)k(X

)1k(X ++++


Modelo cinemático

■ Como variam, no intervalo [tk,tk+1[ as velocidades vd(t) e ve(t)?

vd(t)

ve(t)

tk tk+1

Trajectória segundo um ARCO DE CIRCUNFERÊNCIA

wL

)t(v)t(v

wR2

)t(v)t(v

ed

ed

=−

=+R = raio da circunferênciaw = velocidade angular



■ Trajectória = arco de circunferência de raio R

L P

R+L/2

R

R-L/2roda esquerda

roda direita

∆θ∆θ∆θ∆θ

++++θθθθ∆∆∆∆====∆∆∆∆

−−−−θθθθ∆∆∆∆====∆∆∆∆

2LR)k()k(D

2LR)k()k(D

d

e

∆∆∆∆++++∆∆∆∆====θθθθ∆∆∆∆====∆∆∆∆

∆∆∆∆−−−−∆∆∆∆====θθθθ∆∆∆∆

2)k(D)k(DR)k()k(D

L)k(D)k(D)k(

ed

ed


f(.)-Hipóteses simplificativas■ Trajectória = arco de circunferência de raio R

( )

( ) )k(v

)k()k(

)2)k()k(sin(2)k(

2)k(sin)k(D)k(y

)2)k()k(cos(2)k(

2)k(sin)k(D)k(x

)1k()1k(y)1k(x

+

θ∆+θ

θ∆+θθ∆θ∆∆+

θ∆+θθ∆θ∆∆+

=

+θ++

)k(y

)k(x

)k(θ

WY

{W} WX)1k(x +

)1k(y +

)1k( +θ


2)k(θθθθ∆∆∆∆

)(kθθθθ


kP

1kP ++++

R

R

(((( ))))

(((( ))))2)k(

2)k(sin )k(D PP

2)k(

2)k(sinParcP

PP

1kk

1kk1kk

θθθθ∆∆∆∆θθθθ∆∆∆∆∆∆∆∆====

⇓⇓⇓⇓θθθθ∆∆∆∆θθθθ∆∆∆∆====

++++

++++

++++



)k(v)k()k(

)2)k()k(sin()k(D)k(y)2)k()k(cos()k(D)k(x

)1k()1k(y)1k(x

++++

θθθθ∆∆∆∆++++θθθθθθθθ∆∆∆∆++++θθθθ∆∆∆∆++++θθθθ∆∆∆∆++++θθθθ∆∆∆∆++++

≅≅≅≅

++++θθθθ++++++++

)k(y

)k(x

)(kθθθθ

WY

{W} WX)1k(x ++++

)1k(y ++++

)1( ++++kθθθθ


2)k(θθθθ∆∆∆∆

kP

1kP ++++

'1kP ++++

■ Trajectória = arbitrária■ Aproximar Pk+1 por P’k+1 com )k(DPP '

1kk ∆∆∆∆====++++


Estimativa no instante k+1

■ Estimativa

■ Incerteza - matriz de covariância (com aproximações e hipóteses subjacentes)

))k(ˆ),k(D),k(X(f)1k(X θθθθ∆∆∆∆∆∆∆∆====++++

◆ Expansão de em série de Taylor em torno de

◆ Desprezando termos de ordem superior

◆ são incorrelacionados

),D),k(X(f θ∆∆ )k(ˆ ),k(D ),k(X θ∆∆

)k(QdX

dF)k(dXdF)1k(

TXX ++++ΣΣΣΣ≅≅≅≅++++ΣΣΣΣ

)k(ˆ),k(DD),k(XXXf

dXdF

θ∆=θ∆∆=∆=∂∂=

θθθθ∆∆∆∆∆∆∆∆ e D),k(X



■ Estimativa


))k(ˆ),k(D),k(X(f)1k(X θθθθ∆∆∆∆∆∆∆∆====++++

(((( )))) (((( ))))[[[[ ]]]]TX 1k(X)1k(X)1k(X)1k(XE)1k( ++++−−−−++++++++−−−−++++====++++ΣΣΣΣ

◆ Expansão de em série de Taylor

em torno de

◆ Desprezando termos de ordem superior

),D),k(X(f θ∆∆

)k(ˆ ),k(D ),k(X θ∆∆



■ Estimativa


))k(ˆ),k(D),k(X(f)1k(X θθθθ∆∆∆∆∆∆∆∆====++++

ordem 1ª de cruzados termos

ddF)k(

ddF

DddF)k(

DddF)k(Q

dXdF)k(

dXdF)1k(

TTD

TXX

+

+θ∆

Σθ∆

+∆

Σ∆

++Σ=+Σ θ∆∆

◆ são incorrelacionadosθθθθ∆∆∆∆∆∆∆∆ e D),k(X



■ Estimativa


))k(ˆ),k(D),k(X(f)1k(X θθθθ∆∆∆∆∆∆∆∆====++++

θ∆Σ

θ∆+

∆Σ

∆++Σ=+Σ θ∆∆ d

dF)k(ddF

DddF)k(

DddF)k(Q

dXdF)k(

dXdF)1k(

TTD

TXX

são muito pequenos



■ Estimativa


)k(QdX

dF)k(dXdF)1k(

TXX ++++ΣΣΣΣ≅≅≅≅++++ΣΣΣΣ

))k(ˆ),k(D),k(X(f)1k(X θθθθ∆∆∆∆∆∆∆∆====++++

)k(ˆ),k(DD),k(XXXf

dXdF

θ∆=θ∆∆=∆=∂∂=


• Dados e é possível definir o lugar geométrico dos pontos onde, com uma dada probabilidade, se situam os valores da variável aleatória.

• Elipsóide com centro em e semi-eixos proporcionais aos valores singulares de

Significado da incerteza

))k(),k(X(N~)k(X XΣΣΣΣ

)k(XΣΣΣΣ)k(X

)k(X )k(XΣΣΣΣ

(((( )))) (((( )))) γγγγ≤≤≤≤−−−−ΣΣΣΣ−−−− −−−− )k(X)k(X )k()k(X)k(X 1X

T

Distribuição com 3 graus de liberdade2χ


Significado da incerteza))k(),k(X(N~)k(X XΣΣΣΣ

θθθθ====

)k()k(y)k(x

)k(X

θθθθ====

)k(ˆ)k(y)k(x

)k(X

σσσσσσσσσσσσσσσσσσσσσσσσσσσσσσσσσσσσ

====ΣΣΣΣ

θθθθθθθθθθθθ

θθθθ

θθθθ

2yx

y2yxy

xxy2x

X )k(

)k(XΣΣΣΣ

Elipsóide com centro em e semi-eixos proporcionais aos valores próprios de

)k(X

Dados e é possível definir o lugar geométrico dos pontos onde, com uma dada probabilidade, se situam os valores da variável aleatória.

)k(X )k(XΣΣΣΣ

====

)k(y)k(x

)k(P

====

)k(y)k(x

)k(P

σσσσσσσσσσσσσσσσ

====ΣΣΣΣ 2yxy

xy2x

P )k(Posição

)k(θθθθOrientação )k(θθθθ 2θθθθσσσσ


Significado da incerteza))k(),k(P(N~)k(P PΣΣΣΣ

( ) ( ) γ≤−Σ− − )k(P)k(P )k()k(P)k(P 1P

T

====

)k(y)k(x

)k(PPosição

1 λλ2 λγ

α

)k(x

)k(y

Probabilidade = 90%m=2 K=4.61

)k(θ

21 , λλValores próprios de Σp(k)


Significado da incerteza

1 λγ2 λγ

α

)k(x

)k(y)k(θ

))k(),k(P(N~)k(P PΣΣΣΣ

====

)k(y)k(x

)k(PPosição

)k(θθθθOrientação ))k(),k(ˆ(N~)k( 2θθθθσσσσθθθθθθθθ


Odometria - Experiências

Elipses a 90% de probabilidade

Trajectória real = incrementos angulares de 0.2o

Estimativa inicial odometria x=1cm, y=1cm,θ=5o

Trajectória estimada por odometria

Trajectória real

Elipses a 95% de probabilidade

Documents

Localização em Robótica Móvel Odometriausers.isr.ist.utl.pt/~mir/cadeiras/robmovel/Odometry.pdf · • A rotação de uma das rodas faz rodar um tambor em torno do contentor