Post on 24-Dec-2018
Localização em Robótica Móvel
Odometria
Maria Isabel Ribeiromir@isr.ist.utl.pt
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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.
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
O odómetro de Leonardo da Vinci
■ Codex Atlanticus - Biblioteca Ambrosiana - Roma, Itália
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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.
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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)
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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 +
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
■ 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?
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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))
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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(θ∆
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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 ++++
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
f(.)-Hipóteses simplificativas
■ 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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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( +θ
)k(θθθθ∆∆∆∆
2)k(θθθθ∆∆∆∆
)(kθθθθ
)k(θθθθ∆∆∆∆
kP
1kP ++++
R
R
(((( ))))
(((( ))))2)k(
2)k(sin )k(D PP
2)k(
2)k(sinParcP
PP
1kk
1kk1kk
θθθθ∆∆∆∆θθθθ∆∆∆∆∆∆∆∆====
⇓⇓⇓⇓θθθθ∆∆∆∆θθθθ∆∆∆∆====
++++
++++
++++
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
f(.)-Hipóteses simplificativas
)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θθθθ
)k(θθθθ∆∆∆∆
2)k(θθθθ∆∆∆∆
kP
1kP ++++
'1kP ++++
■ Trajectória = arbitrária■ Aproximar Pk+1 por P’k+1 com )k(DPP '
1kk ∆∆∆∆====++++
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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 θθθθ∆∆∆∆∆∆∆∆====++++
(((( )))) (((( ))))[[[[ ]]]]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 θ∆∆
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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 θθθθ∆∆∆∆∆∆∆∆====++++
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
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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 θθθθ∆∆∆∆∆∆∆∆====++++
θ∆Σ
θ∆+
∆Σ
∆++Σ=+Σ θ∆∆ d
dF)k(ddF
DddF)k(
DddF)k(Q
dXdF)k(
dXdF)1k(
TTD
TXX
são muito pequenos
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
Estimativa no instante k+1
■ Estimativa
■ Incerteza - matriz de covariância (com aproximações e hipóteses subjacentes)
)k(QdX
dF)k(dXdF)1k(
TXX ++++ΣΣΣΣ≅≅≅≅++++ΣΣΣΣ
))k(ˆ),k(D),k(X(f)1k(X θθθθ∆∆∆∆∆∆∆∆====++++
)k(ˆ),k(DD),k(XXXf
dXdF
θ∆=θ∆∆=∆=∂∂=
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
• 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χ
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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θθθθσσσσ
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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)
1999 - © M. Isabel RibeiroRobótica Móvel Odometry
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θθθθσσσσθθθθθθθθ