Você está na página 1de 34

Localizao em Robtica Mvel

Odometria

Maria Isabel Ribeiro


mir@isr.ist.utl.pt
Instituto Superior Tcnico (IST)
Instituto de Sistemas e Robtica (ISR)
Av.Rovisco Pais, 1
1049-001 Lisboa
PORTUGAL

Outubro.1999
All the rights reserved

Classificao de Mtodos de Localizao

Localizao com Medidas Relativas

Robtica Mvel

Dead-Reckoning

Localizao com Medidas Absolutas

Odometria
Navegao Inercial

Faris activos
Reconhecimento de marcas artificiais
Reconhecimento de marcas naturais
Emparelhamento com o modelo

Localizao com Medidas Relativas + Medidas


Absolutas
1999 - M. Isabel Ribeiro

Odometry

Odometria

Determinao da localizao do robot mvel


atravs da observao e integrao consecutiva
do movimento das rodas
ODMETROS - Instrumentos que medem a distncia
percorrida por veculos.

Robtica Mvel

Encoders - sensores mais usuais

Vitruvius descreveu o conceito no sc. 1 ac.


Leonardo da Vinci (1500) construi um aparato que
usava pedras para calcular a distncia percorrida.

1999 - M. Isabel Ribeiro

Odometry

O odmetro de Leonardo da Vinci

Robtica Mvel

Codex Atlanticus - Biblioteca Ambrosiana - Roma, Itlia


1999 - M. Isabel Ribeiro

Odometry

O odmetro de Leonardo da Vinci

Robtica Mvel

1999 - M. Isabel Ribeiro

Contentor, fixo estrutura do


veculo, cheio com pedras e
com um orifcio na parte
inferior.
A rotao de uma das rodas
faz rodar um tambor em
torno do contentor.
Tambor tem um orifcio do
mesmo tamanho do existente
no contentor.
Depois de um certo nmero
de rotaes da roda, os dois
orifcios coincidem e uma
pedra cai, sendo recolhida
numa caixa.
O nmero de pedras
recolhidas na caixa num
intervalo de tempo permite
estimar a distncia
percorrida.

Odometry

Fontes de erro na odometria


Integrao de informao sobre movimentos
incrementais
acumulao de erros

Erros sistemticos

so caractersticos do robot e/ou dos sensores

Robtica Mvel

desigual dimetro ou desalinhamento das rodas,


dimetro 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 no sistemticos
so graves porque so fonte constante de erros aditivos

1999 - M. Isabel Ribeiro

Odometry

Fontes de erro na odometria


Integrao de informao sobre movimentos
incrementais
acumulao de erros

Erros no sistemticos

so caractersticos da relao do robot com o ambiente

movimento sobre solos no uniformes (rugosos),


movimento sobre obstculos inesperados no solo,
escorregamento das rodas
solo escorregadio
grandes aceleraes do veculo
rotaes rpidas

Robtica Mvel

em solos irregulares estes podem ser os erros dominantes

1999 - M. Isabel Ribeiro

Odometry

Irregularidades no pavimento

Comando de movimento - velocidade igual em


ambas as rodas
Robot mvel com controlo diferencial

Y(cm)

3
2,5
2

Trajectria real

1,5
1

Trajectria dada pelo modelo


odomtrico

0,5
0
-0,5
Robtica Mvel

10

20

30

40

50

60

70

X(cm)

-1

1999 - M. Isabel Ribeiro

Odometry

NOTAO
YW
x (k )
X (k ) = y (k )

(k )

(k )
y(k )
{R}

posio

{W}

x (k )

XW

e
orientao

A localizao refere-se ao ponto mdio do eixo


entre as duas rodas motoras
Eixo com comprimento L
Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Modelo da odometria
Descreve a evoluo temporal da localizao do
robot mvel como funo de

entrada U(k),
v(k) - fonte de rudo (deformaes e escorregamento das rodas, vibraes,
.. ) - assumida como N(0,Q(k))

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


U(k ) = [D(k ) (k )]T

Robtica Mvel

D(k) - distncia percorrida pela origem do referencial do robot


no intervalo tk , t k +1
(k)
- variao da orientao no mesmo intervalo

1999 - M. Isabel Ribeiro

Odometry

Estimativa da Localizao por Odometria

Robtica Mvel

Dados:
X(k + 1) = f (X(k ), D(k ), (k )) + v(k )

Modelo do sistema

Estimativa da localizao no instante k

X(k )

Incerteza associada

X (k )

Leituras de odometria (estimativas)

Caracterizao do erro

(k ), (k )
D

Q(k )

Pergunta:

qual a estimativa da localizao no instante k+1?

X(k + 1)

qual a incerteza associada?

X (k + 1)

1999 - M. Isabel Ribeiro

Odometry

Estimativa da Localizao por Odometria

Dadas as leituras dos odmetros


D e (k ), D d (k )

De (k ), Dd (k )
D (k ), (k )

D d (k ) + D e (k )

=
D
(
k
)

D d (k ) D e (k )
(k ) =

Quais so as caractersticas do rudo de estado?

v(k ) ~ N(0, Q(k ))

0
0
Q1(k )
Q2 (k )
0
Q(k ) = 0

0
0
Q3 (k )

O que a funo f( ) ?

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


Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Caracterizao do erro

Crowley - ICRA92

Q11(k )

Q(k ) =
Q22 (k )

Q33 (k )

Q11(k ) = KD D(k ) cos (k )


Q22 (k ) = KD D(k ) sin (k )
Q33 (k ) = KD D(k ) + K (k )

KD - coeficiente de drift de translaco de odometria relativo a D (m2/m)

KD - coeficiente de drift de rotao de odometria relativo a D (rad2/m)


K - coeficiente de drift de rotao de odometria relativo a (rad2/rad)
KD= 0.001 m2/m
Valores tpicos

KD =0.0003 rad2/m
K = 0.001 rad2/rad

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Modelo cinemtico
YW

vd(t) - velocidade da roda direita


(t )

ve(t) - velocidade da roda esquerda

y(t )

L - comprimento do eixo entre as


duas rodas motoras

{R}

{W}

x(t )

XW

x! ( t ) = v d ( t ) + v e ( t ) cos(( t ))
2

v d(t) + v e (t)

!
y
(
t
)
sin(( t ))
=

v (t) v e (t)
! ( t ) = d
L

Robtica Mvel

1999 - M. Isabel Ribeiro

Relaciona:

as velocidades das
rodas com

as velocidades
(linear e angular) do
referencial do veculo
no referencial global
Odometry

Modelo cinemtico
Discretizao do modelo contnuo

Para t> tk

t v ( )+ v ( )
x( t ) = x( t k ) + d 2 e cos(( ))d
tk
t v ( )+ v ( )
y( t ) = y( t k ) + d 2 e sin(( ))d
tk
t v ( ) v ( )
( t ) = ( t k ) + d L e d
tk
t k +1

x( t k +1) = x( tk ) +

tk

t k +1

y( t k +1) = y( tk ) +

tk

t k +1

( tk +1) = ( t k ) +

tk

Robtica Mvel

1999 - M. Isabel Ribeiro

v d ( )+ v e ( )
cos(( ))d
2
v d ( )+ v e ( )
sin(( ))d
2
v d ( ) v e ( )
d
L
Odometry

Modelo cinemtico

Como variam, no intervalo [tk,tk+1[ as velocidades


vd(t) e ve(t)?

vd(t)

ve(t)
tk

tk+1

TRANSLAO seguida
de uma ROTAO
sobre o eixo de rotao

TRANSLAO
ROTAO
Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

f(.) - Hipteses simplificativas

TRANSLAO seguida de uma ROTAO sobre


o eixo de rotao
e

d
T (k )
L

De (k ) = T (k ) (k ) 2

Dd (k ) = T (k ) + (k ) L

2
Robtica Mvel

1999 - M. Isabel Ribeiro

(k )
Dd (k ) + De (k )

= T(k )
D(k ) =
2

(k ) = Dd (k ) De (k ) = (k )

L
Odometry

f(.)-Hipteses simplificativas

TRANSLAO seguida de uma ROTAO sobre


o eixo de rotao
YW

(k )

y (k + 1)

D (k )
(k )

y (k )

{W}

x (k )

x (k + 1)

XW

x(k + 1) x(k ) + D(k ) cos((k ))


y(k + 1) = y(k ) + D(k ) sin((k )) + v(k )

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


D(k),
(k))
f(X(k),

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

f(.) - Hipteses simplificativas

TRANSLAO seguida de uma ROTAO sobre


o eixo de rotao
YW
y (k + 1)

D (k )

y (k )

(k )

{W}

Robtica Mvel

1999 - M. Isabel Ribeiro

x (k )

x (k + 1)

XW

Odometry

f(.)-Hiptese simplificativa

TRANSLAO seguida de uma ROTAO sobre


o eixo de rotao
X(k + 1)

YW

(k )

y (k + 1)

D(k )
( k )

y (k )

{W}

( k + 1)

X(k )

x (k )

x (k + 1)

XW

x(k + 1) x(k ) + D(k ) cos((k ))


y(k + 1) = y(k ) + D(k ) sin((k )) + v(k )

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


Robtica Mvel

1999 - M. Isabel Ribeiro

D(k),
(k))
f(X(k),

Odometry

Modelo cinemtico

Como variam, no intervalo [tk,tk+1[ as velocidades


vd(t) e ve(t)?

vd(t)

Trajectria segundo
um ARCO DE
CIRCUNFERNCIA

ve(t)
tk

tk+1

v d(t) + v e (t)
= wR
2
v d(t) v e (t)
=w
L
Robtica Mvel

1999 - M. Isabel Ribeiro

R = raio da circunferncia
w = velocidade angular
Odometry

f(.)-Hipteses simplificativas

Trajectria = arco de circunferncia de raio R


R+L/2
R

roda
esquerda

R-L/2

roda direita

D
(
k
)
(
k
)
R

D (k ) = (k ) R + L
d

2
Robtica Mvel

1999 - M. Isabel Ribeiro

Dd (k ) De (k )

(
k
)

D(k ) = (k )R = Dd (k ) + De (k )

2
Odometry

f(.)-Hipteses simplificativas

Trajectria = arco de circunferncia de raio R


YW

(k + 1)

y (k + 1)

(k )
(k )

(k )

PkPk +1
sin((k ) 2)
=
(k ) 2
arcPkPk +1

sin((k ) 2)
PkPk +1 = D(k )
(k ) 2

Pk + 1

(k ) 2

(k )

y (k )

Pk

{W}

x (k )

x (k + 1)

XW

sin((k ) 2)

x
(
k
)
D
(
k
)
cos(
(
k
)
(
k
)
2
)
+

(k ) 2

x(k + 1)
(
)
sin
(
k
)
2

y(k + 1) = y(k ) + D(k )


sin((k ) + (k ) 2) + v(k )

(k ) 2
(k + 1)

(k ) + (k )

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

f(.)-Hipteses simplificativas

Trajectria = arbitrria
Aproximar Pk+1 por Pk+1 com PkPk' +1 = D(k)
YW

(k + 1)

y(k + 1)

Pk' + 1

(k )

Pk + 1

(k ) 2

(k )
y(k )

{W}

Pk

x (k )

x(k + 1)

XW

x(k + 1) x(k ) + D(k ) cos((k ) + (k ) 2)


y(k + 1) y(k ) + D(k ) sin((k ) + (k ) 2) + v(k )

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

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Estimativa no instante k+1


Estimativa

(k ), (k ))
X(k + 1) = f (X (k ), D

Incerteza - matriz de covarincia (com aproximaes e

hipteses subjacentes)

(k ), D
(k ), (k )
Expanso de f ( X(k ), D, ) em srie de Taylor em torno de X

Desprezando termos de ordem superior

X(k ), D e so incorrelacionados

dF
dFT
X (k + 1)
X (k )
+ Q(k )
dX
dX
dF f
=
dX X X = X(k ),D= D(k ),= (k )
Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Estimativa no instante k+1

Estimativa
(k ), (k ))
X(k + 1) = f ( X(k ), D

Incerteza - matriz de covarincia (com aproximaes e


hipteses subjacentes)

[(

)(

X (k + 1) = E X(k + 1) X(k + 1) X(k + 1) X(k + 1 T

Expanso de f ( X(k ), D, ) em srie de Taylor

(k ), D
(k ), (k )
em torno de X

Robtica Mvel

Desprezando termos de ordem superior

1999 - M. Isabel Ribeiro

Odometry

Estimativa no instante k+1

Estimativa
(k ), (k ))
X(k + 1) = f ( X(k ), D

Incerteza - matriz de covarincia (com aproximaes e


hipteses subjacentes)

dF
dFT
dF
dFT
dF
dFT
X (k + 1) =
X (k )
+ Q(k ) +
D (k )
+
(k )
+
dX
dX
dD
dD d
d
+ termos cruzados de 1 ordem

Robtica Mvel

X(k ), D e so incorrelacionados

1999 - M. Isabel Ribeiro

Odometry

Estimativa no instante k+1

Estimativa
(k ), (k ))
X(k + 1) = f ( X(k ), D

Incerteza - matriz de covarincia (com aproximaes e


hipteses subjacentes)

dF
dFT
dF
dFT
dF
dFT
X (k + 1) =
X (k )
+ Q(k ) +
D (k )
+
(k )
dX
dD
dD d
d
dX
so muito pequenos

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Estimativa no instante k+1

Estimativa
(k ), (k ))
X(k + 1) = f ( X(k ), D

Incerteza - matriz de covarincia (com aproximaes e


hipteses subjacentes)

dF
dFT
X (k + 1)
X (k )
+ Q(k )
dX
dX
dF f
=
dX X X = X(k ),D= D(k ),= (k )

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Significado da incerteza
X(k ) ~ N( X(k ), X (k ))
Dados X(k ) e X (k ) possvel definir o lugar geomtrico dos
pontos onde, com uma dada probabilidade, se situam os
valores da varivel aleatria.
(k ) e semi-eixos proporcionais aos
Elipside com centro em X
valores singulares de X (k )

Distribuio

Robtica Mvel

T
1

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

1999 - M. Isabel Ribeiro

com 3 graus de liberdade

Odometry

Significado da incerteza
X(k ) ~ N( X(k ), X (k ))
x(k )
X(k ) = y(k )

(k )

x(k )
X(k ) = y(k )

(k )

2x

X (k ) = xy

xy
2y
y

y
2

Dados X(k ) e X (k ) possvel definir o lugar geomtrico dos


pontos onde, com uma dada probabilidade, se situam os
valores da varivel aleatria.
(k ) e
Elipside com centro em X
semi-eixos proporcionais aos
valores prprios de X (k )

Posio
Orientao
Robtica Mvel

x(k )
P(k ) =

y(k )
(k )
1999 - M. Isabel Ribeiro

x(k )

P(k ) =

y(k )
(k )

2x
P (k ) =
xy

xy

2y
2
Odometry

Significado da incerteza
x(k )
Posio P(k ) =

y(k )

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

( P(k ) P(k ))T P (k )1( P(k ) P(k ))


Probabilidade = 90%
m=2 K=4.61

y ( k )

1, 2
Valores prprios de p(k)
Robtica Mvel

1999 - M. Isabel Ribeiro

( k )

x ( k )
Odometry

Significado da incerteza
x(k )
Posio P(k ) =

y(k )

P(k ) ~ N(P(k ), P (k ))
2
(k ) ~ N( (k ), (k ))

(k )

Orientao

y ( k )
( k )

x ( k )

Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry

Odometria - Experincias

Trajectria estimada por odometria

Trajectria real

Elipses a 90% de probabilidade

Elipses a 95% de probabilidade

Trajectria real = incrementos angulares de 0.2o


Estimativa inicial odometria x=1cm, y=1cm,=5o
Robtica Mvel

1999 - M. Isabel Ribeiro

Odometry