1
FUSIÓN SENSORIAL 1 INTEGRACIÓN SENSORIAL : FUSIÓN SENSORIAL :
Usa la información de distintos sensores para hacer algo útil.
Combina lecturas de distintos sensores en una estructura de datos uniforme.
OBJETIVO : Estimar la posición del robot a partir de medidas proporcionadas por diferentes sensores.
Normalmente un sensor no es suficiente porque las medidas que proporciona proporciona son : Ruidosas
imprecisas
inciertas
inexactas
1
Las medidas son proporcio proporcionadas nadas con ruido.
2
La precisión está limitada. limitada.
3
No son totalmente fiables.
4
La información que proporc proporcionan ionan del entorno es limitada e incompleta. incompleta.
5
La elección de un sensor puede resultar resultar más cara que la combinación combinación de 2 o más sensores más baratos.
Modelo de ruido empleado : Ruido : Ruido blanco Gausiano aditivo de media nula ¿ Por qu un modelo gausiano ! gausiano ! " MOTIVOS : 1
Es bastante realista, sobre sobre todo si el error error tiene diversas componentes.
2
odelado sencillo ! edia " varian#a lo definen totalmente. totalmente.
3
$onduce a expresiones matemáticas sencillas # sencillas # mane%ables $. mane%ables $.
MODELO DEL SENSOR :
El modelo de un sensor pro%ee una relación matem&tica entre la propiedad de inters # e $ ' la lectura del sensor # r $ $ : r & f 'e( El modelo debe incluir la relación del dispositi%o f(sico ' el ruido debido al sensor mismo # interno $ ' al medio ambiente # e)terno $.
*as limitaciones de los diferentes tipos de sensores ' la incertidumbre+ siempre presente tanto interna como e)terna+ moti%an la combinación de %arios sensores # 'a sean del mismo o diferente tipo $ mediante la Fusión Sensorial ,ormas de fusión sensorial en robótica mó%il : edidas de varios sensores sensores - Promedio ponderado # ,orma simple de combinar %arios sensores # del mismo tipo $. e toma el promedio de las
2 mediciones $ / Clasificador ba'esiano simple / 012 3 probabil(stico 4 / edidas en diferentes posiciones)tiempos # ,iltro de 5alman $# tiene en cuenta la incertidumbre en la fuente de los datos $.
ean n medidas
de una misma cantidad desconocida x :
*e desea estimar el valor de x que proporcione el m+nimo error cuadrático. *olución :
TEOREMA DE FUSIÓN SENSORIAL :
medida
*ean n sensores proporcionando cada uno de ellos una
, respecto a una misma referencia, de una cantidad desconocida X , con ruido aditivo
insesgado e independiente, siendo
todos vectores # p ) 6 $.
$ada error está caracteri#ado por su matri# # p ) p$ de covarian#a . La estimación óptima *E # inimum *quare Estimation $ para la cantidad desconocida X " su matri# de covarian#a C asociada son :
demás, la estimación óptima *E implica que el error de la estimación es insesgado " es m+nimo en sentido cuadrático medio cuando se emplea como norma la distancia de a-alanobis ESTIMACIÓN : •
ea X la cantidad %ectorial desconocida+ de dimensión # n ) 6 $+ ' sea Y un %ector de m medidas #
[
m)6$: •
]
e trata de estimar el %ector X que minimi7a el error cuadr&tico : n 8 m # C es de rango m&)imo $ :
n 9 m
olución única
soluciones
solución
olución de m(nima norma :
# istema indeterminado $ :
n;m
olución con m(nimo error cuadr&tico :
olución ponderada según la incertidumbre de las medidas+ siendo :
# istema sobredeterminado o incompatible $ :
3
2
ESTIMACIÓN EN SISTEMAS DINÁMICOS : FILTRO DE KALMAN
2.1
INTRODUCCIÓN
?@ $ en 6>A@. /todo recursivo para estimar el estado de un sistema dinámico basado en medidas contaminadas con ruido. Proporciona una solución recursi%a eficiente del mtodo de m(nimos cuadrados+ calculando una estimación óptima del estado de un sistema en cada momento+ usando la información disponible en el instante #t/6$ ' actuali7&ndola con la información adicional disponible en el instante t. e basa en B modelos : 1 2
odelo de la planta # lineal $ contaminado con ruido # errores de modelado $. odelo del proceso de medida # los sensores $ tambi/n con ruido.
sume : Ruido blanco, de distribución gausiana " media nula, incorrelado en el tiempo, " el ruido en el sistema es independiente e incorrelado con el ruido en las medidas. uposiciones nunca estrictamente ciertas pero habitualmente asumibles.
Utilidad pr&ctica : 2eside en su habilidad para estimar el estado # por eDemplo la pose de un robot mó%il $+ bas&ndose en un n de medidas que pueden ser : 1 Incompletas 2 Indirectas
3 Intermitentes 4 Inexactas
2.2
Relacionadas con algunas pero no todas las variables de inter/s. Relacionadas indirectamente con las variables de inter/s. 0isponibles a intervalos no necesariamente regulares. $ontaminadas por distintas fuentes de error.
MODELO DEL SISTEMA Y MODELO DE MEDIDA
Modelo de un sistema lineal en tiempo discreto :
# ruido en el sistema $ ' ' de media nula :
2.3
# ruido en la medida $ son ruidos blancos gausianos+ incorrelados
ECUACIONES DEL FILTRO DE KALMAN
Para propagar el estado ' su co%arian7a.
Para corregir el estado con la información procedente de las medidas.
CICLO DE PROPAGACIÓN
4 CICLO DE ACTUALIACIÓN
2.4
FILTRO DE KALMAN E!TENDIDO " O GENERALIADO #
EKF
SISTEMAS NO LINEALES : i el sistema es no lineal+ 'a sea en su ecuación de estado+ en su ecuación de medidas+ o en ambas *ineali7ación del modelo del sistema ' del modelo de medida en torno a la estimación actual.
•
ea el sistema no lineal :
•
Es necesario calcular los Facobianos :
ECUACIONES :
CICLO DE PROPAGACIÓN
CICLO DE ACTUALIACIÓN
2.5
PROCESADO SECUENCIAL DE LAS MEDIDAS
i los errores en las medidas indi%iduales son incorrelados # por eDemplo porque se obtienen a partir de sensores independientes $+ puede demostrarse que procesar todas las medidas simult&neamente en un único bloque es equi%alente a procesar cada una por separado.
VENTAJAS :
1
2
En sistemas en tiempo real que disponen de las medidas de modo intermitente o as+ncronamente ! Es posible implementar el filtro en tiempo real de modo que se adapte a un n1 variable de medidas o incluso a su presencia o ausencia en determinados instantes.
*e evita la complicación de tener que reestructurar las matrices para acomodar un n1 variable de medidas.
5
3
enta%as computacionales porque es muc-o más eficiente invertir 2 matrices de orden # n ) 2 $ que invertir una de orden n
2.$
CONSIDERACIONES ADICIONALES
ELECCIÓN DE LAS MATRICES DE COVARIANA DEL RUIDO : 1
Las propiedades del ruido en las medidas pueden obtenerse a partir de las especificaciones del sensor " por experimentación. El ruido en el sistema modela la fidelidad del modelo respecto al proceso real.
2
Es el dise3ador quien decide, en base a su intuición " experiencia, un modelo de comportamiento del error, que -a de ser validado mediante experimentación.
INICIALIACIÓN DEL FILTRO : 1
2
*i el estado es conocido
3
*i el estado es desconocido
4ara comen#ar a iterar las ecuaciones del filtro es necesario una estimación inicial del vector de estado " de su matri# de covarian#a.
OBSERVABILIDAD : 1
2
4ara garanti#ar la convergencia del filtro de alman es necesario medidas que -agan observable el sistema. *i las medidas disponibles en un instante o per+odo de tiempo determinado no son suficientes para garanti#ar que el sistema sea observable, la covarian#a 4 de la estimación se -ará cada ve# ma"or " el filtro diverge.
AMPLIACIÓN DE ESTADO : 1
2
Es fundamental para la convergencia del filtro que los ruidos sean blancos, incorrelados " de media nula. En la realidad, rara ve# los ruidos presentan este comportamiento ideal. Los parámetros que definen este comportamiento correlado del ruido pueden incluirse en el vector de estado " ser, a su ve#, estimados.
INTERPRETACIÓN DEL FILTRO DE KALMAN : 1
En la fase de propagación utili#a el modelo para predecir el estado " su varian#a ' su
6 confian#a ( en el instante siguiente. 2
2.%
En la fase de actualización incorpora las medidas pro"ectándolas sobre el espacio de estados, de acuerdo con su confian#a o valide# : El resultado no es lo que predice el modelo ni lo que indican las medidas, sino una suma ponderada de ambos de acuerdo con su confian#a relativa. ELIMINACIÓN DE MEDIDAS ESP&REAS UTILIANDO LA DISTANCIA DE MA'ALANOBIS
e llama predicción a las medidas que predicen el modelo de medida cuando se e%alúa en el estado estimado+ es decir : Para medidas lineales
Para medidas no lineales
Como la co%arian7a de la estimación es ' la co%arian7a de las medidas es tenemos que la distancia de Mahalanobis entre la predicción ' la medida es :
+
6$ En sistemas lineales :
B$ En sistemas no lineales :
?$
En sistemas lineales En sistemas no lineales
*a probabilidad de que una medida correcta # con error normal $ est a m&s de ? unidades de distancia de Mahalanobis de la predicción es realmente mu' baDa # ; 6@G $. i se da este caso+ basta eliminar dicha medida asumiendo que es un espúreo+ es decir+ es una medida que presenta errores ma'ores ' distintos que los contemplados en su distribución gausiana del error.
7
APLICACIÓN DEL FILTRO DE KALMAN AL POSICIONAMIENTO " FILTRO DE KALMAN PARA NAVEGACIÓN DE ROBOTS MÓVILES #
3 3.1
INTRODUCCIÓN
MODELO DEL SISTEMA : Propaga el estado.
Elección del vector de estado
Ecuaciones del modelo dinámico
odelo del ruido
Lineali#ación en torno a la tra"ectoria
4osibilidad de incluir parámetros de calibración
MODELO DE MEDIDA : Predice las medidas de los sensores a partir del estado estimado a priori. $ompás magn/tico Giroscopio # medida de &ngulo o %elocidad angular $ Goniómetro # triangulación $ isión artificial 5ransponder o G4* # trilateración $ Medidores de rango *onar, Láser
3.2
SIMPLIFICACIONES : 1
ROBOT MOVIL : ELECCIÓN DE LAS VARIABLES DE ESTADO
4rincipal otion asumption :
Helocidad lineal según el eDe longitudinal del %eh(culo.
Helocidad angular según el eDe
al suelo.
2
En entornos interiores : uelos planos ' hori7ontales.
3
0inámica despreciable # $adencia de muestreo mu" alta $ : Con el sistema libre+ en cada inter%alo de muestreo la %elocidad lineal ' la angular se pueden considerar ctes.
ELECCIÓN DEL VECTOR DE ESTADO :
8 1 2
•
4osición - )+ '+ # 7 8 @ $ 4 del robot respecto a una referencia fi%a.
•
6rientación I medida sobre el e%e vertical # suelos -ori#ontales $.
3
•
elocidad lineal del robot v
4
•
elocidad angular 7 medida sobre el e%e vertical.
3.3
MODELO DE LA BASE MÓVIL
A
SISTEMA LIBRE
Modelo din&mico no lineal :
Modelo din&mico discreti7ado :
B B.1
CON PROPULSIÓN Configuración
diferencial :
Modelo discreti7ado :
Modelo discreto no lineal
9
B.2
Facobiano :
Configuración
triciclo ' Ackerman :
Modelo discreti7ado :
B.3 Configuración
síncrona :
Modelo discreti7ado :
10
3.4
MODELO DE SENSORES PARA POSICIONAMIENTO RELATIVO
A
CODIFICADORES ÓPTICOS INCREMENTALES " ENCODERS #
Considerando la e)istencia de errores :
con
B
GIROSCOPIO DE ESTADO SÓLIDO "
RATE
GYRO #
Considerando la e)istencia de errores :
con puede considerarse una co%arian7a cte.
C
COMPÁS MAGN(TICO DE N&CLEO SATURABLE " FLUXGATE #
11
Considerando la e)istencia de errores :
con
puede considerarse una co%arian7a cte. MODELO DE MEDIDA DE POSICIONAMIENTO RELATIVO
con
4
ECUACIONES CINEMÁTICAS DE ROBOTS MÓVILES
A
CONFIGURACIÓN DIFERENCIAL ECUACIONES CINEMÁTICAS
B
MODELO DISCRETO APRO!IMADO
CONFIGURACIÓN TRICICLO
12
C
ECUACIONES ODOMÉTRICAS
CONFIGURACIÓN S)NCRONA
13
Y el cambio de dirección realizado será ∆θ = (Ld - Li ) / B donde B es la distancia entre ruedas.
Si x (0), y (0), θ(0) e!" #!$ %&&'e"!'!$ ei!$ 'e# &*&+, #&$ !#&e$ #e-& 'e# .&i.ie"+& $&" θ(+) / θ(0) ∆θ x (t ) / x (0) LC %&$ θ(+) y (t ) / y (0) LC %&$ θ(+)
