aula 5 aula 5 – introdução à robótica móvel … na robótica móvel • modelos de movimento...

Aula 5 Aula 5 –– Introdução à Robótica MóvelIntrodução à Robótica Móvel

Lidando com IncertezLidando com IncertezasasLidando com IncertezLidando com Incertezasas

Prof. Dr.Prof. Dr. Marcelo BeckerMarcelo Becker

EESC - USP

•• Introdução Introdução às Incertezas às Incertezas

• Incertezas na Robótica Móvel

• Filtros

Sumário da AulaSumário da Aula

• Filtros

• Bibliografia Recomendada

EESC-USP © M. Becker 2009 2/78

Introdução às Incertezas

� Origem� Inexatidão dos equipamentos de medida (bias

uncertainty)� Variações randômicas na medida (precision

uncertainty)uncertainty)

� Em projetos, a estimativa das incertezas deve ser feita num nível de confiabilidade da ordem de 95%



� Propagação das Incertezas: • A incerteza dos resultados finais depende da

incerteza de cada medida individualmente…

Exemplo:

),( IVPP

PPP

δδδδ

δ

=

±=

III

VVV

IVP

δ

δ

±=

±=

⋅=


),,,( 21 nxxxfR L=

n

nx

Rx

x

Rx

x

RxR

∂

∂++

∂

∂+

∂

∂= δδδδ L

2

2

1

1


• Caso Geral:

nxxx ∂∂∂ 21

ix

R

∂

∂: Coeficiente de Sensibilidade de R com relação a xi

ixδ : incerteza de xi

Rδ Pode ser nulo (caso os termos se cancelem…)


• Caso Geral de Propagação de Incertezas

∑∂

∂=

n

xRx

Rww ixw

Incerteza Máxima:

:incerteza das variáveis


∑= ∂

=i i

xRx

wwi

1

2/1

2

1

)(∑=

∂

∂=

n

i i

xRx

Rww

i

Validação: Todas as variáveis medidas devem ser independentes entre si…

ix

Ou: (RSS: root of the sum of the squares)


• Caso Especial de Propagação de Incertezas:

N

n

baxxCxR K21=


2/1

22

2

22

1

1 )()()(

+++

=n

nR

x

wN

x

wb

x

wa

R

wK

• As maiores incertezas tendem a dominar o resultado…


• Como Estimar Incertezas Aleatórias?

� Assuma que a variável é medida n vezes:n

x

x

n

i

i∑== 1


� Calcule o desvio padrão das medidas:

� A incerteza aleatória na média é obtida por uma distribuição t (t-distribution):

2/1

1

2

)1(

)(

−

−= ∑

=

n

i

ix

n

xxS

n

Stp x

x 2/α±=


• Student’s t-distribution

2/)1(2

)1)(2

(

)2

1(

),(++Γ

+Γ

=ν

ν

ννπ

ν

νt

tfnS

xt

/

µ−=

Student’s t, degree of freedom ν = n-1


� Student’s t, degree of freedom ν = n-1


• Intervalo de Estimativa para uma população média (n < 30)

ααα −=≤≤− 1][ 2/2/ tttP

αµ αα −=+≤≤− 1][ 2/2/n

Stx

n

StxP

nS

xt

/

µ−=


0=t2/αt− 2/αt

2/α=Area2/α=Area

Intervalo de Confidência

2/α=Area

αµ αα −=+≤≤− 1][ 2/2/n

txn

txP

n

Stx 2/αµ ±=

Com nível de Confidência 1-α


• Student's t-Distribution Table



• Componentes Aleatórios da Incerteza

� No caso em que a média e o desvio padrão são obtidos através de diferentes conjuntos de dados, i.e.:

)(Mxx = )(MSS xx′=


xx StpM 2/ :1 α±==

MxxM

i

i∑=

=1

xx

M

MSMtp x

x

)(),1(

′−′±= α

� Como determinar t?

� Se n ≥ 30, 1-α = 95%: t = 2.0

� Se n < 30, 1-α = 95%: use Tabela do slide anterior...


• Incertezas Sistemáticas: BBxx

� Permanecem constantes se o teste é repetido nas mesmas condições;

� São independentes do tamanho da amostra;� Erros conhecidos que não foram eliminados;


� Erros conhecidos que não foram eliminados;� Podem ser estimadas a partir de:

Especificações do fabricante, testes de calibração, modelagem matemática, etc.;


• Ilustração Gráfica das Incertezas Aleatórias e Sistemáticas


População Amostragem


• Incerteza Total:

� Combinação das incertezas sistemática e aleatória usando RSS:


( ) 2/122

xxxPBW +=

( ) 2/122

xxx PBW +=Para uma amostragem

Para múltiplas amostragens


� Prediction:

• Bayes Filters

111 )(),|()( −−−∫= tttttt dxxbelxuxpxbel


� Correction:

∫

)()|()( tttt xbelxzpxbel η=


2

2)(

2

1

2

2

1)(

:),(~)(

σ

µ

σπ

σµ

−−

=x

exp

Nxpµ

Introdução às Incertezas• Gaussianas:

2 σπ-σ σ

Variável única

)()(2

1

2/12/

1

)2(

1)(

:)(~)(

µxΣµx

Σx

Σµx

−−− −

=t

ep

,Νp

dπ

µµµµ

Multi-variável


),(~),(~ 22

2

σµσµ

abaNYbaXY

NX+⇒

+=

• Propriedades de Gaussianas:


),(~ σµ abaNYbaXY

+⇒

+=

+++

+⋅⇒

−− 2

2

2

1

22

2

2

1

2

112

2

2

1

2

2212

222

2

111 1,~)()(

),(~

),(~

σσµ

σσ

σµ

σσ

σ

σµ

σµNXpXp

NX

NX


),(~),(~

TAABANY

BAXY

NXΣ+⇒

+=

Σµ

µ

• Gaussianas Multi-variáveis


• Vamos permanecer no “Gaussian world” enquanto pudermos iniciar com Gaussians e for possível realizar apenas transformações lineares.

BAXY +=

Σ+ΣΣ+Σ

Σ+

Σ+Σ

Σ⋅⇒

Σ

Σ−− 1

2

1

1

2

21

11

21

221

222

111 1,~)()(

),(~

),(~µµ

µ

µNXpXp

NX

NX


• Elipses de Erro:• Como a matriz de covariância de uma

variável aleatória determina o formato da elipse de erro e vice-versa?


elipse de erro e vice-versa? – Matriz de Covariância;– Elipse de Erro;– Autovalores;– Coeficiente de Correlação.

[Cortesia de Kai Arras]


Matriz de Covariância Elipse de Erro


[Cortesia de Kai Arras]

Auto

-valo

res

Coeficiente de Correlação



•• Incertezas na Robótica MóvelIncertezas na Robótica Móvel

• Filtros


• Filtros



Incertezas na Robótica MóvelIncertezas na Robótica Móvel

• Movimento de Robôs Móveis– Inerentemente sujeitos a incertezas– Como é possível modelar as incertezas?



• Rede Dinâmica Bayesiana • Dynamic Bayesian Network



• Modelos Probabilísticos para Movimento- Para implementar Bayes Filters, é necessário o

modelo de transição p(x | x’, u)

- O termo p(x | x’, u) especifica a probabilidade - O termo p(x | x’, u) especifica a probabilidade posterior que a ação u causa no robô de x a x’

- Como é possível modelar p(x | x’, u) baseado nas equações de movimento...



• Sistema de Coordenadas- A configuração do robô pode ser descrita por 6

parâmetros: 3 coordenadas cartesianas e 3 ângulos (ângulos de Euler – roll, pitch e yaw ou tilt)tilt)

- Em superfícies planares: (x,y,θ)



• Modelos de Movimento - Na prática, tem-se 2 tipos de modelos:

• Baseados em odometria (Odometry-based)• Baseados na Velocidade (Velocity-based ou dead

reckoning)

- Modelos do tipo Odometry-based são empregados quando o robô móvel tem encoders nas rodas

- Modelos do tipo Velocity-based são aplicados quando o robô não possui um sistema de odometria• A nova pose (posição e orientação) é baseada nas

velocidades e no intervalo de tempoEESC-USP © M. Becker 2009 27/78


• Dead Reckoning

- Originário do “deduced reckoning”- Procedimento matemático para

determinar a posição atual do veículo- O cálculo da pose atual do veículo é

baseado em suas velocidades e no intervalo de tempo



• Razões para a presença de erros

Caso Ideal Rodas com Diâmetros Diferentes

Obstáculos Carpete, etc...



• Modelo de Odometria� O robô move-se de para� Informação de Odometria:



• Modelo de Odometria



• A função Atan2– Estende a tangente inversa e acerta os

sinais de x e y



• Modelo de Ruído em Odometria– A medida de movimento é dada por um

movimento sujeito a ruído



• Distribuições Típicas para Ruído em OdometriaDistribuição Normal Distribuição Triangular



• Cálculo da Probabilidade (zero-centered)– Para Distribuição Normal

– Para Distribuição Triangular


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Cálculo da Probabilidade (dados x, x’, u)

Valores de odometria (u)

Valores de interesse (x , x’)


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Aplicação

– Aplicação do modelo do sensor para pequenos deslocamentos

– Distribuições típicas com formato de banana (“banana-shaped distributions”) obtidas para a previsão no plano 2D.previsão no plano 2D.


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Sample-based Density Representation


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Como?

– Distribuição Normal

– Distribuição Triangular


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Distribuição Normal (106 Samples)


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Distribuição Triangular


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Rejeição do Sampling

– Sampling de distribuições arbitrárias


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Rejeição do Sampling

– Exemplo: Sampling de


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Modelo de Movimento baseado em

Sample Odometry


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Exemplos de Sample Odometry


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Modelo baseado em Velocidade


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Equações para o Modelo baseado em

Velocidade:– Centro do círculo:

Com:


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Cálculo da Probabilidade:


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Sampling para o Modelo de

Velocidade:


Incertezas na Robótica MóvelIncertezas na Robótica Móvel• Exemplos de Sample Velocity:




•• FiltrosFiltros





Filtro de Filtro de KalmanKalman DiscretoDiscreto

tttttt uBxAx ε++= −1

• Estima o estado x de um processo controlado em tempo

discreto que é governado por uma equação diferencial

linear estocástica (linear stochastic difference equation).


tttt xCz δ+=

Com a medida:


Matriz (n x n) que descreve como o estado evolui

de t a t-1 sem controle ou ruído;tA

Matriz (n x l) que descreve como o controle ut

muda o estado de t a t-1;tB

Filtro de Filtro de KalmanKalman DiscretoDiscreto

tε

Matriz (k x n) que descreve como mapear o estado

xt para uma observação zt.tC

tδ

Variáveis aleatórias representando os ruídos do

processo e da medida que são assumidos como

sendo independentes e distribuídas normalmente com covariâncias Rt e Qt respectivamente.


Atualização do Atualização do KalmanKalman FilterFilter em 1Dem 1D

Crença Inicial

Medida e Incerteza Associada

Crença após a aplicação

do Filtro de Kalman


1)( com)(

)()( −+ΣΣ=

Σ−=Σ

−+== t

T

ttt

T

ttttttt

tttttt

t QCCCKCKI

CzKxbel

µµµ

2

,

2

2

22 c

)1(

)()(

tobst

tt

ttt

ttttt

t KomK

zKxbel

σσ

σ

σσ

µµµ

+=

−=

−+==



do Filtro de Kalman


+Σ=Σ

+==

−

−

t

T

tttt

ttttt

tRAA

uBAxbel

1

1)(

µµ

+=

+==

−

2

,

222

1)(

tactttt

ttttt

ta

ubaxbel

σσσ

µµ


+Σ=Σ − ttttt RAA 1


do Filtro de Kalman Nova Crença após

Movimento para a Direita



do Filtro de KalmanNova Crença após

Movimento para a Direita


Nova Medida e

Incerteza Associada

Nova Crença após a

Aplicação do Filtro de Kalman


Sistemas Gaussianos Lineares: Sistemas Gaussianos Lineares: InicializaçãoInicialização

• A crença inicial é uma distribuição normal:

( )0000 ,;)( Σ= µxNxbel


• A dinâmica é uma função linear do estado e controle adicionada do ruído:


Sistemas Gaussianos Lineares: Sistemas Gaussianos Lineares: DinâmicaDinâmica

( )ttttttttt RuBxAxNxuxp ,;),|( 11 += −−

( ) ( )1111

111

,;~,;~

)(),|()(

−−−−

−−−

Σ+

⇓⇓

= ∫

ttttttttt

tttttt

xNRuBxAxN

dxxbelxuxpxbel

µEESC-USP © M. Becker 2009 60/78

( ) ( )

⇓

Σ+

⇓⇓

=

−−−−

−−−∫

ttttttttt

tttttt

xNRuBxAxN

dxxbelxuxpxbel

1111

111

,;~,;~

)(),|()(

µ

Sistemas Gaussianos Lineares: Sistemas Gaussianos Lineares: DinâmicaDinâmica

+Σ=Σ

+==

−Σ−−

−−−−−=

⇓

−

−

−−−−−−−

−−

−∫

t

T

tttt

ttttt

t

tttt

T

tt

tttttt

T

tttttt

RAA

uBAxbel

dxxx

uBxAxRuBxAxxbel

1

1

111

1

111

1

1

1

)(

)()(2

1exp

)()(2

1exp)(

µµ

µµ

η


• As Observações também são funções lineares adicionadas do ruído:

tttt xCz δ+=

( )QxCzNxzp ,;)|( =

Sistemas Gaussianos Lineares: Sistemas Gaussianos Lineares: ObservaçõesObservações

( )tttttt QxCzNxzp ,;)|( =

( ) ( )ttttttt

tttt

xNQxCzN

xbelxzpxbel

Σ

⇓⇓

=

,;~,;~

)()|()(

µ

η


( ) ( )

11

,;~,;~

)()|()(

⇓

Σ

⇓⇓

=

ttttttt

tttt

xNQxCzN

xbelxzpxbel

µ

η

Sistemas Gaussianos Lineares: Sistemas Gaussianos Lineares: ObservaçõesObservações

1

11

)(with )(

)()(

)()(2

1exp)()(

2

1exp)(

−

−−

+ΣΣ=

Σ−=Σ

−+==

−Σ−−

−−−=

t

T

ttt

T

ttttttt

tttttt

t

ttt

T

tttttt

T

tttt

QCCCKCKI

CzKxbel

xxxCzQxCzxbel

µµµ

µµη


Algoritmo do Filtro de KalmanAlgoritmo do Filtro de Kalman1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):

2. Prediction:3.4.

ttttt uBA += −1µµ

t

T

tttt RAA +Σ=Σ −1

5. Correction:6.7.8.

9. Return µt, Σt

1)( −+ΣΣ= t

T

ttt

T

ttt QCCCK

)( tttttt CzK µµµ −+=

tttt CKI Σ−=Σ )(


CicloCiclo PrediçãoPredição -- CorreçãoCorreção

+Σ=Σ

+==

−

−

t

T

tttt

ttttt

tRAA

uBAxbel

1

1)(

µµ

+=

+==

−

2

,

222

1)(

tactttt

ttttt

ta

ubaxbel

σσσ

µµ

Prediction



1)(,

)(

)()(

−+ΣΣ=

Σ−=Σ

−+== t

T

ttt

T

ttttttt

tttttt

t QCCCKCKI

CzKxbel

µµµ

2

,

2

2

22 ,

)1(

)()(

tobst

tt

ttt

ttttt

t KK

zKxbel

σσ

σ

σσ

µµµ

+=

−=

−+==

Correction


2

,)(

)( ttttttK

zKxbel

σµµµ=

−+==

+= − 1 ttttt uba µµ

Prediction


1)(,)(

)()( −+ΣΣ=

Σ−=Σ

−+== t

T

ttt

T

ttttttt

tttttt

t QCCCKCKI

CzKxbel

µµµ

2

,

222 ,

)1(

)()(

tobst

tt

ttt

ttttt

t KK

zKxbel

σσ

σ

σσ

µµµ

+=

−=

−+==

+Σ=Σ

+==

−

−

t

T

tttt

ttttt

tRAA

uBAxbel

1

1)(

µµ

+=

+==

−

2

,

222

1)(

tactttt

ttttt

ta

ubaxbel

σσσ

µµ

Correction


Sumário sobre Sumário sobre KalmanKalman FilterFilter• Alta Eficiência: medidas (polinômio) de

dimensão k e estado de dimensão n: O(k2.376 + n2)

• Ótimo para Sistemas Gaussianos Lineares!

• A maioria dos Sistemas Robóticos são NÃO LINEARESNÃO LINEARES!


Sistemas Dinâmicos NãoSistemas Dinâmicos Não--LinearesLineares• A grande maioria dos problemas mais

realísticos relacionados à robótica são NÃO-LINEARES

),( 1−= ttt xugx

)( tt xhz =


Hipótese de Hipótese de LinealizaçãoLinealização


FunçãoFunção NãoNão--LinearLinear


LinealizaçãoLinealização parapara o EKF (o EKF (1)1)


LinealizaçãoLinealização parapara o EKF (2)o EKF (2)


• Prediction:)(

),(),(),( 11

1

111 −−

−

−−− −

∂

∂+≈ tt

t

tttttt x

x

ugugxug µ

µµ

First Order Taylor Series Expansion


• Correction:

)(),(),( 1111

1

−−−−

−

−+≈ ttttttt

t

xGugxug µµ

)()()(

)()(

)()(

ttttt

tt

t

ttt

xHhxh

xx

hhxh

µµ

µµ

µ

−+≈

−∂

∂+≈


EKF Algoritmo EKF Algoritmo 1. Extended_Kalman_filter( µt-1, Σt-1, ut, zt):

2. Prediction:3.4.

),( 1−= ttt ug µµ

t

T

tttt RGG +Σ=Σ −1

ttttt uBA += −1µµ

t

T

tttt RAA +Σ=Σ −1

5. Correction:6.7.8.

9. Return µt, Σt

1)( −+ΣΣ= t

T

ttt

T

ttt QHHHK

))(( ttttt hzK µµµ −+=

tttt HKI Σ−=Σ )(

1

1),(

−

−

∂

∂=

t

ttt

x

ugG

µ

t

tt

x

hH

∂

∂=

)(µ

1)( −+ΣΣ= t

T

ttt

T

ttt QCCCK

)( tttttt CzK µµµ −+=

tttt CKI Σ−=Σ )(







•• Bibliografia RecomendadaBibliografia Recomendada


Bibliografia RecomendadaBibliografia Recomendada

• Siegwart, R. and Nourbakhsh, I.R., 2004, Introduction to Autonomous Mobile Robots, 1st Edition, MIT Press, ISBN 0-262-19502-X

• http://www.mobilerobots.org• Sandin, P. E., 2003, Robot Mechanisms and


• Sandin, P. E., 2003, Robot Mechanisms and Mechanical Devices Illustrated, McGraw-Hill, ISBN 0-07-141200-X

• Trhun, S., Burgard, W., and Fox, D., 2005, Probabilistic Robotics, The MIT Press, ISBN 0-262-20162-3.

aula 5 aula 5 – introdução à robótica móvel … na robótica móvel • modelos de movimento...

Documents