SIMULADOR FASTLAM Software para Matlab que permite observar el comportamiento de un Robot que ejecuta el algoritmo FastSlam.

Manual de Uso

1.- INTRODUCCION El presente documento describe el uso y funcionamiento de un software desarrollado para ser ejecutado en ambiente Matlab, que presenta al usuario una animacin del comportamiento de un robot que trata de determinar su posicin y de reconstruir el mapa del escenario que lo rodea, mediante la implementacin del algoritmo Fast SLAM, propuesto por Montemerlo et al [1]. El objetivo principal del trabajo desarrollado fue implementar una interfaz de usuario que permitiese experimentar con los parmetros principales involucrados en la resolucin de SLAM (Simultaneous Localization And Mapping). De esta manera el usuario podr definir sus propios escenarios compuestos por un nmero arbitrario de landmarks y una ruta arbitraria para ser seguida por un robot, pudiendo definir y ajustar un gran numero de variables que buscan representar el comportamiento de un robot real. Se observar una animacin del comportamiento del robot, sus observaciones y estimaciones de posicin y mapa, para finalmente disponer de representaciones graficas de los resultados del proceso, cuyos datos pueden ser almacenados para posterior uso o estudio. Tanto el presente documento como el software estn orientados a estudiantes que cursando asignaturas en las que se estudien este tipo de algoritmos.

2.-Definicin del problema El problema de localizacin y mapeo simultaneo (SLAM, por su sigla en ingles), tiene como objetivo que un robot pueda determinar tanto su posicin como la ubicacin de un conjunto de landmarks (marcas observables e identificables por el robot) mientras se desplaza en un determinado escenario. Supngase un robot que se desplaza mediante ruedas, como se muestra en la figura 1, en que se puede controlar la velocidad lineal con que se desplaza y el Angulo de la rueda de control de direccin. Ya que los mecanismos que permiten su desplazamiento estn susceptibles a errores de control y accionamiento, toda prediccin del desplazamiento del robot y por tanto de su pose futura, tendr un error. Este efecto es acumulativo y aumenta montonamente el error que el robot comete en la determinacin de su posicin.

Figura 1: Representacion simple del robot Se supondr que este robot cuenta con un sistema de visin que le permite observar landmarks que se encuentren en su campo visual (180 frontales) determinando su posicin angular y distancia relativa a la pose del robot. Toda medicin esta afectada por ruido sensorial que agrega un error a estas mediciones. Este error en las observaciones del robot, tendr como consecuencia un error en la construccin del mapa de su entorno. Existen una serie de algoritmos y tcnicas que permiten corregir estos errores, principalmente basndose en la informacin sensorial que puede recopilar el robot, cada vez que un nuevo landmark es observado, permite corregir tanto la pose del robot como la representacin de su entorno.

Dentro de los algoritmos ms empleados se encuentran aquellos que emplean filtros Kalman KF, versiones mejoradas de estas tcnicas como EKF. Basandose precisamente en este tipo de algoritmos de SLAM, es que Michael Montemerlo, Daphne Koller y Ben Wegbreit proponen un algoritmo denominado FastSlam, como una solucin factorizada para el problema de mapeo y localizacin simultanea. El problema que tienen los algoritmos basados en EKF, es que al aumentar el nmero de landmarks aumenta fuertemente el costo y complejidad computacional. La figura 2 muestra un modelo probabilstico degenerativo (red dinmica de Bayes).

Figura 2: El problema SLAM s1, s2, st : Pose del robot para un tiempo t u1, u2,ut : Controles del robot z1, z2,zt : Observaciones del robot 1, 2, k : Posicion de los K landmarks observados La pose s del robot varia en el tiempo dependiendo de los controles u, y las observaciones z dependen de las posiciones de los landmarks. Por ejemplo, en t=1 el robot observa el landmark 1, lo que constituye la observacion z1, mientras en t=2 y en t=T el robot detecta el landmark 2 , lo que genera las observaciones z1 y zT respectivamente. El problema de SLAM radica en determinar la ubicacin de los landmarks y la ruta (sucesin de poses s) del robot a partir de los controles u y las observaciones z.

En esta situacin existen importantes independencias condicionales. Particularmente el hecho de que el conocimiento de la ruta del robot hace independiente la medicin individual de los landmarks. Por ejemplo si se conociese en forma exacta la ruta del robot, el problema de determinar la ubicacin de los landmarks puede ser desacoplada en K problemas de estimacin independientes, uno para cada landmark. Basandose en este hecho es que se propuso un algoritmo eficiente llamado FastSLAM, este descompone el problema de SLAM en un problema de localizacin del robot y en un conjunto de problemas de estimacin de ubicacin de landmarks que estn condicionados a la estimacin de la pose del robot. La pose del robot evoluciona segn una ley de probabilidades referida al modelo de movimiento:

p (st ut , st 1 )
Que dice bsicamente que la pose del robot en un momento t, estar determinada por una funcin probabilstica de los controles ut, y la pose anterior del robot. Este modelo suele basarse en la cinemtica del robot. Las observaciones del robot responden a un modelo probabilstico que se denomina modelo de mediciones

p (zt st , , nt )
Aqu

= { 1 , 2 ,..., k } es el conjunto de todos los landmarks y

el ndice del landmark percibido en el instante t, llamado

nt { ,2,..., K }es 1

tambin correspondencia, correspondencia conocida significa que los landmarks son distinguibles entre si. Si la correspondencia es conocida, el problema se simplifica a:

p s t , z t , u t , n t

El super ndice t, indica que se trata de un conjunto de variables para el intervalo de tiempo 1 hasta t. LA independencia condicional implica que esa expresin puede ser factorizada como sigue:

p s t z t , u t , nt

) p(
k

z t , u t , nt

Donde claramente se observa que el problema puede ser descompuesto en K+1 problemas de estimacin, un problema de estimacin de pose del robot y K problemas de estimar la ubicacin de los K landmarks condicionados a la estimacin de la ruta. La estimacin de la ruta, o conjunto de poses del robot, se implementa mediante un filtro de partculas. Mientras que la estimacin de la posicion de los landmarks se consigue mediante filtros Kalman, usando filtros distintos para cada landmark. Ya que la estimacin de los landmarks estn condicionadas ala estimacin de la ruta, cada partcula en el filtro de partculas tiene su propia estimacin local de landmark. Asi, si existen M particulas, habr un total de KxM filtros kalman, de dimensin 2 para un plano. 2.1.-Estimacin de la pose por filtro de partculas FastSlam emplea un algoritmo de filtro de particulas para la determinacin de la ruta del robot que es muy similar al algoritmo de Monte Carlo (MCL).
t t t t Sea St el conjunto de partculas que representan la pose posterior p s z , u , n

t ,[m ] S t , representa una estimacin para un instante de tiempo t, cada particula S

de la ruta del robot, el superndice m denota la m-sima partcula de un total de M partculas en el conjunto St. El conjunto St es calculado incrementalmente a partir de St-1, un contrtol ut del robot y una medicion zt. cada particula en St-1 es usada para generar una estimacin probabilstica de la pose del robot en el instante t, obtenido del modelo de movimiento probabilstico.

st[ m ] p (st ut , st 1 )
Esta estimacin es agregada a un conjunto temporal de partculas, a continuacin
t 1,[ m ] de S . Bajo el supuesto de que el conjunto de partculas St-1 esta distribuido

t 1 t 1 t 1 t 1 segn p s z , u , n (que es una aproximacin asintticamente correcta), la t t t t partcula nueva se distribuye segn p s z , u , n Luego de generar M partculas,

el nuevo conjunto St es obtenido por muestreo del conjunto temporal de partculas.

t ,[ m ] Cada partcula S es tomada con remplazo con una probabilidad proporcional a

[m ] un peso wt asociado a cada partcula:

[ m] t

p s t ,[ m ] z t , u t , n t

p s t ,[ m ] z t 1 , u t , n t 1

)
( )

Que es bsicamente el cociente entre la distribucin objetivo y la distribucin propuesta. El conjunto de muestras resultantes esta distribuido de acuerdo a una aproximacin a la pose posterior deseada p s t z t , u t , n t , que es mas exacta a medida que el nmero de partculas M aumenta. Notese que para cada estimacin, solo se considera la pose anterior y no las anteriores a esta ultima. 2.2.- Estimacin de posicin de los landmarks Las estimaciones se realizan sobre

p k z t , u t , nt

implementando filtros

Kalman. Como cada estimacin esta condicionada a la pose del robot, los filtros Kalman estn asociados a las partculas individuales en el conjunto St. Especficamente, la ruta posterior completa y la ubicacin de los landmarks en el algoritmo FastSlam estn representadas por el conjunto de muestras
[ [ m S t = s t ,[ m ] , 1 m ] , 1m ] ,..., [Km ] , [K ]

k[m] y k[m] , son respectivamente la media y covarianza de la distribucin Gausiana de la estimacin del k-simo landmark, asociado a la m-sima particular. En un escenario bidimensional k[m] es un vector de 2 elementos y k[m] es una matriz de 2x2. En el caso de FastSlam, la estimacin se realiza empleando EKF(Extended Kalman Filter) con un modelo perceptual linealizado p (zt st , , nt ) , de esta manera el EKF en FastSlam es muy similar al EKF en el Slam tradicional. Una diferencia significativa del uso de filtros Kalman entre FastSlam y Slam tradicional es que las actualizaciones en FastSlam involucran solo una gausiana de dimensin 2 (para los parmetros de ubicacin de 2 landmark), mientras los dems emplean una gausiana de 2K+3 (K landmarks y 3 parmetros de pose del robot), lo que incrementa notablemente el costo computacional. Para la determinacin de k[m] y k[m] en cada partcula se emplea una representacin de rbol binario balanceado, como se muestra en la figura 3.

Figura 3: Arbol que representa la estimacin de K=8 landmarks en una partcula individual As los parmetros gausianos k[m] y k[m] se ubican en las hojas del rbol, reduciendo el tiempo necesario para encontrar cada valor. Supngase que se activa un nuevo control ut, y una nueva medicin ut ,cada nueva partcula en St ser diferente de St-1 en dos aspectos, primero tendr una diferente estimacin de ruta de acuerdo al modelo de movimiento y en segundo lugar los parmetros gausianos con ndice nt seranm diferentes que su predecesora, todos los dems parmetros gausianos sern equivalentes. En la figura 4 se muestra un diagrama de flujo global del algoritmo implementado.

INICIO

Inicializacion variables y parmetros de

Creacin poblacin inicial de partculas y pose inicial

SI FIN

Ruta cumplid

NO Calculo: pose real, controles. Suma ruido

Prediccin Pose NO OBS?

SI Calcula observaciones y les aade ruido

Asociacin de datos

Re muestreo de partculas

Figura 4: Diagrama de flujo general del algoritmo implementado

3.-SIMULADOR Para la implementacin del simulador, se ofrece una interfaz de usuario como la que se muestra en la figura 5. Esta se ejecuta desde el archivo arena.m

Figura 5: Apariencia del simulador Esta interfaz permite al usuario crear nuevos escenarios, guardarlos o recuperar escenarios anteriores, estos escenarios compuestos por un conjunto de landmarks y una ruta de desplazamiento del robot, se denominan arena. 3.1-Creacin de una nueva arena Para crear una nueva arena, deber seguir los siguientes pasos 1. 2. 3. 4. Definir el ancho del escenario: esta distancia se especifica en metros Definir el alto del escenario: esta distancia se especifica en metros Definir el numero de landmarks que contendr la arena Definir el nmero de puntos de ruta o checkpoints que en sucesin describirn el desplazamiento del robot a lo largo de la arena. 5. Hacer click en el botn CREAR. Luego de esto se corregirn las escalas de los ejes de la imagen central a los parametors ajustados y en la parte superior se actualizaran los nmeros de landmarks y puntos de ruta que falta por definir. 6. A continuacin, haciendo uso del mouse, el usuario deber hacer click en los puntos en donde desee ubicar los landmarks, repitiendo este proceso

hasta ubicar todos los landmarks. Podr observar que el contador de landmarks de la parte superior se decrementa progresivamente, permitindole saber cuantos restan para terminar. 7. Una vez ubicados todos los landmarks y con el mismo mtodo el usuario ubicara la posicin de los puntos de ruta. Tenga en cuenta que el robot tiene limitaciones en el control de giro, por lo que definir puntos de ruta muy cercanos y con ngulos muy pronunciados, har que el robot quede dando vueltas en crculos. Al igual que con los landmarks, vera decrementar el contador de puntos de ruta a medida que hace click sobre la arena para definir su ubicacin. 8. Si lo desea puede guardar la arena creada haciendo click en el botn GUARDAR. Tenga en cuenta que puede guardar la arena antes o despus de haber realizado simulaciones con ella. 9. A este punto ya puede ejecutar una simulacin, empleando el conjunto de parmetros por defecto en la arena recin definida, para ello haga click en el botn EJECUTAR.

Figura 6: Apariencia que tiene una arena nueva.

3.2-DEFINICION DE CARACTERISTICAS DEL ROBOT En la figura adjunta, observara un detalle del panel para definicin de parmetros del vehculo, que se encuentra a la derecha de la interfaz. Apreciara una representacin del robot que destaca el ngulo , que representa el mximo ngulo que puede tener la rueda de control de direccin, adems se muestra el parmetro d, que es la distancia que existe entre el eje de la rueda frontal y el eje de las ruedas traseras. Estos parmetros son empleados para el clculo de cinemtica del modelo de movimiento en la prediccin de la pose del robot. Adems de otros que definen las observaciones y control del robot. La interfaz sugiere un conjunto de parmetros por defecto que pueden ser modificados: Max ngulo theta: ngulo mximo de la rueda de direccin Distancia d: distancia entre los ejes Velocidad: velocidad lineal del robot Velocidad giro rueda: mxima velocidad con que se puede corregir el ngulo de la rueda de direccin. Rango visin: Distancia mayor a la cual el robot detectara un landmark Vel sensores: velocidad con que los sensores del robot son ledos por el simulador. Ruido Actuadores o Velocidad: desviacin estndar del ruido gausiano de media cero que alterar el control de la velocidad o Angulo: desviacin estndar del ruido gausiano de media cero que alterar el control de la inclinacin de la rueda de control de giro. Ruido Observaciones o Distancia: desviacin estndar del ruido gausiano de media cero que alterar la medicin de la distancia relativa al robot a la que se encuentra un landmark observado. o Angulo: desviacin estndar del ruido gausiano de media cero que alterar la medicin del angulo relativo al robot al que se encuentra un landmark observado.

3.3-DEFINICION DE PARAMETROS DE SIMULACION A la izquierda del simulador, en la parte inferior, se encuentra el panel de parmetros de simulacin, denominado partculas, mostrado en la figura 8.

Figura 8: Panel de parmetros de simulacin. En este panel podr definir: Poblacin: corresponde al nmero M de partculas que sern empleadas en el proceso de estimacin de la pose del robot, empleando filtros de partculas. % renovacin: indica el porcentaje mnimo de partculas efectivas antes del proceso de re muestreo.

3.4.-COMPORTAMIENTO DEL ROBOT 3.4.1.-Pose inicial La pose inicial del robot ser el primer punto de ruta y con ngulo que apunta directamente al segundo punto de ruta. 3.4.2.-Desplazamiento El robot dirigir sus controles de manera tal de llegar a los puntos de ruta en forma secuencial y considerara que ha llegado a un cuando se encuentre a menos de 1,5mts de distancia. Entonces se dirigir al siguiente punto de ruta y as sucesivamente. Como el robot no se detiene y no puede girar instantneamente ni retroceder, cuando llegue a un punto de ruta habr un pequeo sobrepaso mientras corrige su rueda de control de direccin hacia el nuevo punto de ruta, esto representa con bastante fidelidad la situacin real de un vehculo de estas caractersticas. Si un giro hace que el robot salga de la arena, se har momentneamente invisible, pero eventualmente volver a aparecer en direccin a su siguiente punto de ruta.

3.5.-SIMULACION Antes de iniciar una simulacin, deber crear una nueva arena o cargar alguna arena existente. A continuacin debe pulsar el botn EJECUTAR, y se iniciara la animacin. La pose inicial del robot es el primer punto de ruta. Usted vera como aparecen en la pantalla los siguientes objetos que se aprecian en la figura 10:

Figura 10: Simulacin en proceso Robot Real. Un triangulo con dos rectngulos vedes que representan chasis y ruedas respectivamente, representan la posicin real del robot. Robot Estimado: un triangulo de color rojo, representa la mejor estimacin, en funcin del peso w, de la pose del robot. Observaciones: lneas azules representan las observaciones que el robot realiza de los landmarks que se encuentran en su campo receptivo. Estimacin landmarks: elipses de color rojo se ubican en la posicin en que se estiman las ubicaciones de los landmarks observados. Notese que no aparecern estimaciones de landmarks que no sean detectados por el robot.

Podr observar que la superposicin de la representacin del robot real y el robot estimado aparece de color azul. Una vez que el robot llega al punto final de la ruta, aparecer en pantalla una interfaz que le permite guardar el resultado de la simulacin. Podr

escoger la ruta y nombre del archivo que contendr la evolucin del error en la estimacin de la pose del robot y la evolucin de la estimacin de los landmarks observados durante el proceso. Luego de que guarde el resultado de la simulacion o decida no guardarlo haciendo click en cancelar, se desplegara en pantalla una figura que contiene dos graficas, la evolucin de la estimacin del error en la pose y las covarianzas de las estimaciones de la posicin de los landmarks, si lo desea tambin puede guardar esta figura o puede recuperarla a partir del archivo almacenado en el punto anterior. En la figura 11 se muestra un ejemplo de este resultado

Figura 11: Evolucin de la simulacin. En la figura de la izquierda, se puede apreciar como evoluciona el error en la estimacin de la pose, si no existiesen observaciones, este error crecera montonamente de manera indefinida, sin ambargo se aprecia como la ocurrencia de observaciones hace decrecer este error, mientras mas observaciones son agregadas al modelo, mayor es la disminucin de este error. En la imagen de la derecha, se observan las curvas de la covarianza de las particulas que estiman la posicin de cada uno de los landmarks, se aprecia como a medida que avanza el robot y observa mas landmarks, se produce una convergencia evidente en las estimaciones y nuevamente, a medida que mas landmarks son observados, el conjunto completo es mejor estimado.

4.-LIMITACIONES DEL SIMULADOR 4.1.-Del nmero de landmarks El algoritmo empleado no tiene limitaciones en cuanto al numero de landmarks o al numero de puntos de ruta, sin embargo se debe tener en cuenta que este simulador agrega una importante carga computacional a la simulacion ya que realiza clculos necesarios para la representacin grafica y animacin, no solo de la pose del robot, si no tambin de las observaciones y las distribuciones probabilsticas de las estimaciones de los landmarks, por lo que un aumento excesivo principalmente en el numero de landmarks puede hacer que la simulacin se vuelva lenta. 4.2.-De los puntos de ruta Teniendo en cuenta que el simulador hace una aproximacin al desplazamiento real de un robot con las caractersticas antes descritas, tendr las mismas limitaciones que tiene un vehculo real, especficamente respecto al ngulo de giro y su velocidad lineal. Si el usuario define puntos de ruta que exigen giros muy cerrados, el robot no podr seguir fielmente la ruta deseada, incluso pudiendo llegar a caer en la situacin de dar vueltas en crculos tratando de llegar a un punto. Para evitar esta situacin se pueden tomar las siguientes medidas ordenadas segn relevancia importancia: Evitar definir rutas que contemplen giros bruscos y puntos de ruta muy cercanos entre giros. Por ejemplo evitar rutas tipo zigzag muy pronunciado. Evitar definir distancias muy grandes para el parmetro d, que es la distancia entre ejes, si es muy grande, el radio de giro es tambin muy grande por lo que al robot se le dificultara maniobrar. Evitar restringir excesivamente el ngulo mximo para el giro de la rueda de direccin, en este caso un valor pequeo implica radios de giro muy grandes. Evitar restringir excesivamente la velocidad de giro de la rueda de direccin, esto limita la capacidad del robot de corregir rpidamente su trayectoria y por ende, seguir la ruta deseada con precisin. Evitar aumentar excesivamente la velocidad lineal. Esta accin limita la capacidad de controlar con precisin el.

4.3.-Del campo visual El campo visual del robot esta limitado al semiplano anterior, vale decir que desde su eje directriz hacia el frente, puede ver 90 a la izquierda y 90 a la derecha, por lo que aquellas rutas que ubiquen landmarks fuera de este campo visual pueden volverlos invisibles al robot y producir un desmedro en el resultado. 5.- Ejemplos Se han incluido un conjunto de escenarios de ejemplo que permiten observar distintas caractersticas, tanto de la interfaz como del algoritmo empleado.

Figura 12: arena04 La figura 12, muestra la trayectoria seguida por el robot en el escenario del archivo de ejemple arena04, se observa el robot en su posicin final, rodeado por la nube de puntos que corresponden a las partculas que estiman su pose. Adems se pueden apreciar las aproximaciones a la posicin de los landmarks. Este ejemplo resulta interesante ya que el robot parte su trayectoria en el centro de la pantalla, avanza, da un giro a su izquierda para seguir avanzando, recorre poco mas de 100 metros antes de avistar el primer landmark, lo que le suma un gran error de edometra. Entra a la nube de landmarks donde comienza el proceso de correcciones de posicin y mapa en base a las observaciones. Para luego recorrer otro tramo sin observaciones. La figura 13 muestra la evolucin de este proceso. En ella se aprecia claramente el aumento en el error de la odometria y comoo este se acota con las mediciones, luego que ya no hay observaciones el error sigue aumentando, tambin se observan las rpidas convergencias de las covarianzas en las estimaciones de la posicin de los landmarks.

error estimacion pose 3.5 3 2.5 2 1.5 1 0.5 0 1 covarianza [mts] 4 error [mts] 6

covarianzas de los landmarks estimados

1000

2000

3000 iteraciones

4000

5000

6000

1000

2000

3000 iteraciones

4000

5000

6000

Figura 13: evolucin del algoritmo FSlam en la arena04. En el archivo arena10, se construyo un escenario que contiene 200 landmarks.

Figura 14: escenario arena10

error estimacion pose 1.6 1.4 1.2 covarianza [mts] 1 error [mts] 0.8 0.6 0.4 0.2 0 0.07 covarianzas de los landmarks estimados

0.06

0.05

0.04

0.03

0.02

0.01

500

1000

1500

2000 2500 iteraciones

3000

3500

4000

500

1000

1500

2000 2500 iteraciones

3000

3500

4000

Figura 15: resultado del algoritmo en arena10

Este escenario de 100 x 100 metros y que contiene 200 landmarks fue resuleto por el algoritmo con un error mximo menor a 1.6mts en la estimacin de la pose y con rpida convergencia en la localizacin de los landmarks. 6.- Bibliografia [1] M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM: a factorized solution to the simultaneous localization and mapping problem. [2] H. Durranr-Whyte, T. Bailey. Simultaneous Localization an Mapping (SLAM) Part I, Part II