1/50
PERCEPCIÓN AVANZADA
Introducción a la Percepción Cooperativa
MASTER EN Automática, Robótica y Telemática
Dr. J. RAMIRO MARTINEZ DE DIOS
2/50
• Introducción
• Filtros Bayesianos
• Filtro de Kalman
• Filtro de Información
• Filtro de partículas
Introducción a la Percepción Cooperativa
3/50
• Objetivos:
– Explotar las sinergias entre entidades que cooperan con un objetivo común
– Se sirve de métodos de fusión sensorial (lección anterior)
• Consideraciones:
– Asociación: determinar si datos de dos o más sensores provienen del mismo fenómeno
– Filtrado de datos: evitar datos contradictorios
– Carga computacional: velocidad de repuesta necesaria
Percepción Cooperativa
4/50
Detección y localización cooperativa Monitorización cooperativa
Robots heterogéneos– Plataformas: varios UAVs o robots
terrestres– Sensores
Percepción Cooperativa en Entornos Multirrobot
5/50
Misión general:1) Vigilancia y detección 2) Confirmación 3) Monitorización
Percepción Cooperativa en Entornos Multirrobot
Vigilancia y detección– Área se divide dependiendo de
características de UAVs y sus sensores– Cada UAV recorre su área buscando
aplicando técnicas de detección
6/50
Demostración del sistema para la detección de fuegos– Sensores: cámara IR, cámara visual, sensor de fuego
Vigilancia y detección
Percepción Cooperativa en Entornos Multirrobot
560 570 580 590 600 610 620 630 640 650 6600
20
40
60
80
100
120
140
7/50
Confirmación
Percepción Cooperativa en Entornos Multirrobot
460 480 500 520 540 560 580 600 6200
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Pro
babi
lity
0.2821.5Desviación estándar estimada
200.044443961.4564628.9Posición estimada (fusión)
2004443961564627Posición real del fuego
AlturaLatitudLongitud
8/50
• Introducción
• Filtros Bayesianos
• Filtro de Kalman
• Filtro de Información
• Filtro de partículas
Introducción a la Percepción Cooperativa
9/50
Fusión sensorial para estimación
• Determinar un valor desconocido a partir de observaciones
– Cantidad desconocida: estado de un sistema dinámico: xk
– Observaciones o medidas
Incertidumbre
modelos probabilísticos
Filtros Bayesianos
10/50
• Modelo de un sistema (o proceso) dinámico
• Caso lineal:
• xk: vector de estados del sistema en el instante k
– Estado es completo: contiene toda la información sobre el pasado del sistema que
es útil para predecir su fututo (Hipótesis de Markov)
• wk: incertidumbre del modelo (desconocido modelo probabilista)
– Perturbaciones sobre el sistema
– Imperfecciones en el modelo
kkkkkkk wGuBxAx ++=+1
),,(1 kkkkk vuxfx =+
Filtros Bayesianos
11/50
• Modelo de medida
• Caso lineal:
• zk: vector de medidas en el instante k
• rk: ruido de medida (desconocido modelo probabilista)
– Inexactitud de la observación
– Imperfecciones del modelo de medida
kkkk rxhz += )(
kkkk rxCz +=
Filtros Bayesianos
12/50
Filtros iterativos de Bayes
La estimación del estado en el instante k, se obtiene a partir de la
estimación en el instante k-1 y la nueva observación:
Se basa en aplicar el Teorema de Bayes:
))|(),|(()|( 1 xzpZxpfZxp kkk −=
)|(
)|()|()|(
1
1
−
−=kk
kkk
Zzp
ZxpxzpZxp
)|()|()|( 1−∝ kkk ZxpxzpZxp
Estimación para k
función de verosimilitudrelaciona la observación actual con el estado
Estimación para k-1
Filtros Bayesianos
13/50
• Introducción
• Filtros Bayesianos
• Filtro de Kalman
• Filtro de Información
• Filtro de partículas
Introducción a la Percepción Cooperativa
14/50
Filtros iterativos de Bayes. Filtro de Kalman
Sistema dinámico lineal pero no necesariamente TI:
Ruidos blancos e independientes:
Ruidos Gaussianos
kkkkkkk wGuBxAx ++=+1
kkkk rxCz +=
[ ] QwwE iT
i = [ ] RrrE iT
i = [ ] 0=iT
iwrE[ ] [ ] 0== ii rEwE
( )QNwi ,0 ⋯ ( )RNri ,0 ⋯
Teorema central del límite
Filtro de Kalman
15/50
Fases:
– Predicción: predice estado del sistema• Modelo de predicción
– Actualización: compara predicción con medidas y corrige• Modelo de observación
En cada paso estima:-
-
kkx /
kkP /
medidasyk
Filtro
Predicción
Actualización
Filtro de Kalman
16/50
• Fase de predicción:
• Fase de actualización
Predicción del estado
Matriz de covarianza de la estimación del estado
kkkkk uBxAx +=+ //1
QAPAP T
kkkk +=+ //1
Corrección de la estimación del estado futuro
Ganancia del filtro de Kalman
[ ]kkkkkkkk xCyLxx /111/11/1 ++++++ −+=
kkkkkkk PCLPP /11/11/1 +++++ −=
[ ] 1
/1/11
−+++ += RCPCCPL T
kk
T
kkk
Filtro de Kalman
17/50
Modelos no lineales: Filtro de Kalman extendido (EKF)
• Modelos no lineales: linealizar
• Resto queda igual
( )( )
( )( )
( )( )
,
,
k k k
k
k k k
k
k k
k
A F x ux
B F x uu
C G xx
∂=∂∂=
∂∂=
∂
kkkk wBuAxx ++=+1
kkk rCxy +=
( )1 ,k k k kx F x u w+ = +
( )k k ky G x r= +
F o G son no lineales
Filtro de Kalman Extendido
18/50
Vista frontalVista aérea
Vista lateral B
Vista lateral A
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
19/50
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
Enfoque:
– Fusionar las medidas para mejorar estimación
– Método: Filtro de Kalman
• Modelo lineal de propagación de incendio
• Ruidos Gaussianos
• Ventajas:
– Implantación sencilla
– Carga computacional asequible tiempo real
• Desventajas:
– Modelo lineal
20/50
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
Esquema:
• Funciones del filtro de Kalman:
– Diagnosis de sensores
– Fusión sensorial
medidask
Diagnósticoj (FK)
FK
Estimación
estadoPredicción
AsociaciónActualización
Se integran las medidas cuyo sensor
ha sido diagnosticado como correcto
21/50
Vector de estados
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
Sentido de avance
(xj, yj, hj, anchoj)
[ ]kNkNkNkNkkkkk anchohyvelanchohyx ,,,,,1,1,1,1 vel⋯=
Posición del frente en el instanteAltura de llamas en el corteAncho del frente en el corteVelocidad local del frente
Un único vector de estados para representar todo el frente en un instante
22/50
Modelo del proceso
- Modelo lineal en cada punto- Posición evoluciona con velocidad constante
- Altura de llamas y ancho constante
- Matricialmente
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
kkk wAxx +=+1
=
10000000
01000000
00100000
10010000
00001000
00000100
00000010
00001001
⋯
⋯
⋯
⋯
⋮⋮⋮⋮⋮⋮⋮⋮
⋯
⋯
⋯
⋯
A
kzkz xCy ,, =
=
01000000
00100000
00010000
00000100
00000010
00000001
⋯
⋯
⋯
⋮⋮⋮⋮⋮⋮⋮⋮
⋯
⋯
⋯
C
+=
+=
+=
+=
+
+
+
+
3,1,
2,1,
1,1,
,,1,
kkzkz
kkzkz
kkzkz
kzkzkz
wvelvel
wanchoancho
whh
velyy
Otros efectos, topografía, vegetación
23/50
- N sensores que observan el frente- Modelo de observación:
- Fase de actualización
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
ikk
iik rxCy += iTi
kik RrrE =
∑∈
++++++
−+=
kSi
kkii
kikkkkk xCyLxx /111/11/1
Solo se consideran sensores válidos
Diagnosis de sensores
24/50
Diagnosis de sensores
- Errores del sensor: e.g. humo oculta imágenes visuales
- Ruptura súbita de la tendencia: en un solo intervalo de tiempo
- Este tipo de rupturas no puede estar originado por fenómeno físico- El fuego no se apaga con tanta rapidez
- No se trata de ruido Gaussiano- Incumple hipótesis del FK- Será muy fácil de detectar con FK
- Objetivo: identificar rupturas súbitasde tendencias mediante FK
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
0 50 100 150 200 250 300820
830
840
850
860
870
880
890
900
Experimento de campo
25/50
Diagnosis de sensores
Considera los cortes con cada recta z de la malla observado por el sensor i
El vector de estados es:
Supongamos proceso con modelo lineal:
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
[ ]Tikz
ikz
ikz
ikz anchohyy ,,,, =
[ ]Tkzkzkzkzkz velanchohyx ,,,,, =
kkk wAxx +=+1
=
1000
0100
0010
1001
Akz
ikz xCm ,, =
=0100
0010
0001
C
A efectos de diagnosis cada punto es independiente
+=
+=
+=
+=
+
+
+
+
3,1,
2,1,
1,1,
,,1,
kkzkz
kkzkz
kkzkz
kzkzkz
wvelvel
wanchoancho
whh
velyy
(xj, yj, hj, anchoj)
26/50
Errores en el sensor originan errores de predicción altos:
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
kkk xCye /11 ++ −=
0 50 100 150 200 250 300820
830
840
850
860
870
880
890
900
0 50 100 150 200 250 300−1000
−500
0
500
1000
1500
Experimento de campo e
)max(RkT =Umbral depende del ruido del sensor:
k=1.96 Se toman 95% de puntos
27/50
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
28/50
360 380 400 420 440 460 480 500
480
500
520
540
560
580
X
Y
Fire front shape G520 Aerial
Vista IR
Se pierden datos
360 380 400 420 440 460 480 500
480
500
520
540
560
580
X
Y
Fire front shape G520 AerialVista aérea
360 380 400 420 440 460 480 500
480
500
520
540
560
580
X
Y
Fire front shape G520 AerialVista frontal
Fusión sensorial mediante Filtro de Kalman con diagnosis de sensores
Frentes interpolados
Fusión con KF con diagnosis
29/50
• Introducción
• Filtros Bayesianos
• Filtro de Kalman
• Filtro de Información
• Filtro de partículas
Introducción a la Percepción Cooperativa
30/50
Filtro de Información
• Versión dual del Filtro de Kalman
• Supone incertidumbre Gaussina
• La principal diferencia es la parametrización:
– FK: representación de probabilidad
– IF: representación canónica
• Parametrización canónica (IF):
– Ω = Σ-1 (matriz de información)
– ξ = Σ-1 * µ (vector de información)
• Representación de probabilidad (KF):
– p(x) = η * exp -1/2*xT*Ω*x – xT*ξ
31/50
Filtro de Información
• Filtro de Información( ξ(t-1), Ω(t-1), u(t),z(t) )
– Ω(t)' = (A(t)*Ω(t-1)-1*A(t) + R(t))-1
– ξ(t)' = Ω(t)'*(A(t)*Ω(t-1)-1*ξ(t-1) + B(t)*u(t))
– Ω(t) = C(t)'*Q(t)-1*C(t) + Ω(t)'
– ξ(t) = C(t)'*Q(t)-1*z(t)+ ξ(t)'
– Return ξ(t), & Ω(t);
• Complejidad computacional:
– Predicción: KF es poco costoso, IF es costoso
– Actualización: KF es costoso, IF es poco costoso
actualización
predicción
32/50
Filtro de Información
• Ventajas del IF frente al FK:
– Permite modelar la incertidumbre total: Ω = 0
– IF es numéricamente más estable
– IF permite integrar información de forma directa sin necesidad de
convertirla a probabilidades
– Menos carga computacional que el FK al aumentar el número de medidas
• Desventajas:
– Más carga computacional en la actualización
– Kalman filter es más conocido
EIF: Filtro de Información Extendido: modelo no lin eal
KF e IF requieren modelos de ruido Gaussiano
33/50
• Introducción
• Filtros Bayesianos
• Filtro de Kalman
• Filtro de Información
• Filtro de partículas
Introducción a la Percepción Cooperativa
34/50
• Limitaciones del FK y IF:
– Emplean modelos paramétricos lineales o no lineales
– Ruido Gaussiano
– Es un sistema mono-hipótesis: 1 sola gaussiana
• Filtro de partículas: aproxima FDP por sus partículas
– Es no paramétrico:
• Admite cualquier modelo y cualquier tipo de ruido
• Permite modelar multi-hipótesis
Filtro de partículas
35/50
• Representa la Función de Densidad de Probabilidad por un
conjunto suficientemente grade de partículas con pesos
Particle Filter
Each with weight 1/N,
N = Nr. Of particles
Filtro de partículas
36/50
• En cada iteración aplica el filtro Bayesiano sobre cada partícula
aumentando el peso de las partículas más probables
Filtro de partículas
37/50
• Se permite la creación y eliminación de partículas
• Paso “Importance sampling”:
– Se escoge un nuevo conjunto de partículas a partir del conjunto
existente
– Se utiliza el peso para dicha elección. Partículas con peso mayor
tienen más probabilidad de ser escogidas
– Las partículas con peso menor tienden a desaparecer
– El peso de las partículas del nuevo conjunto se ponen todos iguales.
– Es computacionalmente pesado. Se hace con cierta frecuencia, no en
cada paso
• Cuando el conjunto contiene pocas partículas efectivas
Filtro de partículas
38/50
• Obtención de nuevas partículas
• Actualización de pesos
• Resampling
Filtro de partículas
39/50
One Cycle…of the basic algorithm …
Filtro de partículas
40/50
Filtro de partículas
One Cycle…of the basic algorithm …
41/50
No requiere hipótesis de sistema lineal ó Gaussiano
Conduce a una proximación de la FDP completa
No obtiene estado sino la FDP completa. Es necesario obtenerestado de la FDP
La versión básica es muy sencilla de implantar: modularidad
Filtro de partículas
42/50
• Vuelo de UAV afectado por perturbaciones
• Objetivo: – generar maypoints intermedios para asegurar:
• Probabilidad de pasar por zonas de paso >PwzMIN
• Probabilidad de pasar por zonas prohibidas<PfzMAX
• Entradas: – conjunto de zonas de paso y zonas prohibidas
• Salida: – conjunto de waypoints de la trayectoria
Planificación de trayectorias de UAVs
43/50
Filtros de partículas para planificación de trayectorias de UAVs
44/50
Simulación de trayectorias de UAV mediante Filtros de partículas
Simulación de trayectorias de UAV
- Modelo de un UAV en B.C.- Modelo de viento
Particle Filters- Modelos de complejidad arbitraria
- ruido no-Gaussian
- Esquemas multihipótesis
45/50
Trajectory simulation
10099,110099,510096,299,410098,910098,3Pwz,i(%)
WZ11
WZ10
WZ9
WZ8
WZ7
WZ6
WZ5
WZ4
WZ3
WZ2
WZ1
Way-zone
Experimentos
46/50
Real experiments- Bollullos (Sevilla, Spain) 2008 and 2009
- 28 XBow Mica2 with MTS400 sensorboards
Experimentos reales
47/50
Experiment 1: Waypoints are cluster centroids (no intermediate waypoints)
Error=28 m
Error=25 m
Intermediate waypoints are required
Experimentos reales
48/50
Experiment 2: Proposed method
UAV received 91,8% of the messages
UAV passed by wayzones with Prob.>95%
96,997,199,298,5Pwz,i(%)
WZ4
WZ3
WZ2
WZ1
Way-zone
Receivedmessages
Experimentos reales
49/50
PERCEPCIÓN AVANZADA
Introducción a la Percepción Cooperativa
MASTER EN Automática, Robótica y Telemática
Dr. J. RAMIRO MARTINEZ DE DIOS