Localizacion de vehıculos aereos basada en 3DRO-SLAM con inicializacion no retardada

empleando mezcla de Gaussianas

Felipe R.Fabresse, Fernando Caballero, Ivan Maza y Anıbal Ollero

Grupo de Robotica, Vision y Control, Universidad de Sevilla,felramfab@us.es,fcaballero@us.es,imaza@us.es,aollero@us.es

Resumen Este artıculo presenta un metodo de localizacion y mapeo si-multaneo basado en filtro de Kalman extendido (EKF) con aplicacion avehıculos aereos, empleando para ello unicamente medidas de distancia.La apoximacion utilizada ha sido optimizada para funcionar en entor-nos tridimesnionales, reduciendo la carga computacional del mismo enun doble sentido: primero, gracias al uso de una parametrizacion esfericareducida del vector de estado y, segundo, proponiendo un nuevo esquemade actualizacion para el EKF empleado. El metodo utiliza un vector deestado cuya parametrizacion esta basada en una mezcla de Gaussianaspara evitar la inicializacion retardada de cada uno de los elementos delmapa (sensores de distancia), modelando ası la naturaleza multimodaldel modelo de obsevacion de este tipo de sensores y, por otro lado, basadaen una representacion esferica reducida que permite disminuir el nume-ro de parametros necesarios para representar un numero determinadode hipotesis. El metodo es validado mediante una serie de simulacionesque tienen en cuenta el modelo dinamico del vehıculo y el modelo deobservacion de las medidas de distancia.

Keywords: SLAM, medidas de distancia, mezcla de Gaussianas, vehıcu-los aereos, parametrizacion esferica, EKF

1. Introduccion

La localizacion y mapeo simultaneo empleando unicamente medidas de dis-tancia (range-only simultaneous localization and mapping o RO-SLAM, en len-gua anglosajona) es un caso particular de SLAM que trata de localizar a unrobot mobil, al mismo tiempo que se construye un mapa con la posicion de unconjunto de sensores de distancia. Comparado con otras tecnicas de SLAM ba-sadas en LIDAR o camaras, RO-SLAM tiene la ventaja de integrar medidas queno requieren de una linea de vision directa entre el robot y el sensor de distanciacuando estos estan basados en dispositivos de radio. Ademas, el problema de laasociacion de datos esta implıcitamente resuelto gracias al uso de identificadoresunicos integrados en la senal emitida por los sensores de distancia. RO-SLAMtiene asociadas algunas complejidades destacadas relacionadas principalmente

con la poca cantidad de informacion aportada por estos sensores (unicamen-te informacion de distancia entre cada par de sensores) que induce a multipleshipotesis en la localizacion de cada elemento del mapa, las cuales, no son facil-mente integrables en las tıpicas aproximaciones lineales/Gaussianas, razon porla cual los investigadores han explorado diferentes aproximaciones para resolvereste problema: redes neuronales, filtros de partıculas, algoritmos espectrales, etc.

Ası, en [9] se presenta una solucion basada en redes neuronales para apren-der el modelo de observacion de un conjunto dado de sensores de distancia conel objetivo de localizar un robot movil de tierra, haciendo uso de tecnicas deaprendizaje automatico que asocien la potencia de la senal recibida de cada sen-sor de distancia en cada posicion del entorno que pueda ocupar el robot (metodoconocido como fingerprinting). El metodo propuesto no resuelve el problema delmapeo de los sensores. Las entradas de este sistema neuronal son las medidasde distancia recibidas por cada sensor, y la salida es la localizacion del vehıculoutilizando una representacion 2D. El metodo es bastante preciso pero requierede una fase de entrenamiento previo para un conjunto concreto de sensores dedistancia fijados en el entorno donde se debe localizar el robot de modo que nose contempla la posibilidad de anadir de forma dinamica nuevos sensores de dis-tancia ademas, tambien es necesario emplear un sistema de localizacion precisodurante dicha fase de entrenamiento. En [5] se propone un metodo de mapeotridimensional y distribuido empleando filtros de partıculas independientes paraobtener una estimacion inicial de la posicion de cada uno de los sensores del ma-pa. El metodo utiliza las medidas de distancia proporcionadas por un conjuntode sensores de distancia y un robot movil, cuya posicion es registrada medianteel uso de un GPS diferencial (DGPS). Posteriormente, un filtro de Kalman (EKFo UKF) puede ser utilizado cuando el filtro de partıculas de uno de los sensoresde distancia ha convergido a una distribucion Gaussiana. Este metodo no tieneen cuenta la posibilidad de integrar medidas entre cada par de elementos delmapa (solo entre el robot y un sensor), las cuales podrıan ser utilizadas parareducir la incertidumbre asociada a la estimacion de cada elemento del mapa,ademas, el metodo presenta un metodo retardado para la inicializacion de loselementos del mapa en el filtro de Kalman, de modo que las medidas de distanciano pueden ser integradas en el filtro de Kalman hasta que el filtro de partıculasde cada elemento converja a una distribucion Gaussiana.

Multiples metodos han sido propuestos en la ultima decada para resolverel problema de RO-SLAM en espacios bidimensionales. Ası, en [3] se presentauna solucion novedosa, basada en el uso de algoritmos espectrales, que permiteobtener la posicion del robot y de los elementos del mapa realizando una des-composicion SVD de la matriz de observacion. La solucion ofrece una precisionsimilar a la de los metodos clasicos de estimacion y reduce considerablemen-te la carga computacional requerida. No obstante, se trata de un metodo deoptimizacion por lotes que no permite la ejecucion del mismo en tiempo real.

Una comparativa de los diferentes esquemas empleados para resolver el pro-blema de SLAM en la ultima decada es presentada en [11]. El artıculo demuestraque el metodo FastSLAM presenta mejores resultados que otros metodos basados

en EKF o UKF. Como ejemplo, en [13] y [12] se presenta una solucion basada enel esquema FastSLAM, de modo que el vehıculo es localizado mediante el empleode un filtro de partıculas independiente del filtro de partıculas utilizado por cadaelemento del mapa. En [13], el filtro de partıculas empleado esta optimizado demodo que se reduce el numero de partıculas necesario para estimar la posiciondel sensor de distancia reduciendo de este modo el tiempo de computo del meto-do. Por otro lado, [12] optimiza el problema utilizando un metodo adaptativo demuestreo de las partıculas en la fase de correccion del filtro de partıculas. Otroalgoritmo basado en el esquema FastSLAM es empleado en [10] y en [2], en el quelos autores un filtro de partıculas para inicializar la estimacion inicial del filtroEKF empleado por cada elemento del mapa. El empleo de este filtro EKF reducela carga computacional con respecto a las versiones de FastSLAM presentadasen [13] y [12]. El principal inconveniente de este metodo es la perdida de infor-macion en el filtro EKF ocasionado durante la fase de inicializacion realizadapor el filtro de partıculas de cada sensor de distancia. Este es un factor clave enRO-SLAM dado que la convergencia de los elementos del mapa a distribucionesGaussianas tiene una importante dependencia de la trilateracion realizada porel vehıculo.

Un metodo de inicializacion no retardada mediante el empleo de un modelode mezcla de Gaussianas para incorporar la multimodalidad del modelo de ob-servacion en un filtro de Kalman extendido, es el presentado en [6]. Este modelode mezcla de Gaussianas permite la integracion de distribuciones Gaussianasmultimodales en filtros unimodales como el filtro de Kalman extendido (EKF),haciendo posible la incorporacion de todas las medidas de distancia en el filtrodesde el primer momento. Del mismo modo que en [7], [6] utiliza una parametri-zacion polar para la representacion en 2D de los elementos del mapa, la cual seadapta mejor a este tipo de problemas en los que solo se dispone de informacionde distancia. El empleo de un modelo de mezcla de Gaussianas es tambien utili-zado en [1], utilizando en este caso un esquema FastSLAM en lugar del esquemaEKF-SLAM de [6]. En [1], cada sensor de distancia es inicializado de modo quecada hipotesis queda representada por un filtro EKF independiente, lo cual nopermite la incorporacion de medidas de distancia entre cada par de elementosdel mapa guardando la correlacion existente entre cada par de elementos delmapa.

Este artıculo propone una extension del metodo 2D propuesto en [6] al proble-ma RO-SLAM en 3D. Entre las ventajas de el metodo propuesto, cabe destacarla capacidad de integrar las medidas entre cada par de elementos del mapa (nosolo entre robot y sensor), una caracterıstica muy importante en RO-SLAM taly como senala [8]. Otra caracterıstica importante de este metodo con respec-to a una extension directa del metodo presentado en [6], es la parametrizacionesferica reducida del vector de estado que permite reducir no solo la longitud delvector de estado, sino que tambien permite reducir la carga computacional de lafase de correccion del filtro EKF empleado.

El resto de este documento se estructura del siguiente modo. La seccion 2introduce los conceptos basicos del metodo propuesto para resolver el problema

de RO-SLAM en 3D. La seccion 3 detalla cada una de las fases del algoritmoRO-SLAM desarrollado. Finalmente, la seccion 4 muestra algunos resultadosexperimentales en simulacion que permiten validar el metodo desarrollado y seconcluye en la seccion 5 proponiendo algunas mejoras al metodo.

2. Resumen del metodo

Al igual que en [6], este artıculo utiliza un EKF centralizado, resolviendo elproblema del mapeo con un par de modelos de mezcla de Gaussianas (GMMson las siglas anglosajonas). Los modelos de mezcla de Gaussianas permitenmodelar la multimodalidad del modelo de observacion asociado a las medidas dedistancia, integrando distribuciones no-Gaussianas en un filtro Gaussiano comoel filtro de Kalman extendido (EKF).

Los modelos de mezcla de distribuciones de probabilidad son distribuciones deprobabilidad formados por la combinacion de un conjunto de otras distribucionesde probabilidad, formando una alternativa semiparametrica a las convencionalesdistribuciones no parametricas como las utilizadas en los filtros de partıculas,dando una mayor flexibilidad y precision cuando se pretende modelar el modeloestadıstico intrınseco de las medidas de distancia. En el caso particular de lamezcla de Gaussianas, cada componente de la mezcla i es una distribucion nor-mal N (µi, σi) ponderado por un peso ωi, tal que 0 ≤ ωi ≤ 1 y

∑ki=1 ωi = 1.

De modo que, dada una variable aleatoria X con k componentes, su funcion deprobabilidad fX(x) tendra la siguiente forma:

fX(x) =

k∑i=1

ωiN (µi, σi) (1)

Tal y como muestra la Figura 1, este artıculo utiliza una parametrizacionesferica para representar la posicion de cada elemento del mapa, la cual extiendela parametrizacion polar utilizada en el caso de RO-SLAM en [6,7]. Para modelarlos parametros de esta representacion esferica se emplean un par de mezclas deGaussianas, una para el angulo de azimuth θi y otra para el angulo de elevacionφi, de modo que solo con la primera medida de distancia ri se conocen losparametros de distancia ρi = ri y el centro de la esfera [xi, yi, zi]

T (posicion delrobot aereo en el momento de la recepcion de la primera medida de distancia).

A pesar de que una extension directa del metodo propuesto en [6] requiereutilizar un vector de estado muy grande para incluir todas las posibles hipote-sis acerca de la posicion del sensor de distancia, lo cual es ineficiente cuandose emplea un EKF, este artıculo propone una parametrizacion reducida que sebasa en considerar los angulos de azimuth θi y elevacion φi completamente in-dependientes. Esta consideracion implica a su vez una optimizacion en la fasede actualizacion del filtro de Kalman que reduce considerablemente la cargacomputacional.

Al igual que en otras aproximaciones como [1], este artıculo propone unainicializacion de los parametros de los sensores de distancia automatizada que

X

[xi, yi, zi]

σρi

ρi

θi

Φi

Figura 1. Parametrizacion esferica de los elementos de los sensores de distancia. El areaamarilla representa la distribucion esferica y uniforme en la que se puede encontrar elsensor de distancia una vez que se recibe la primera medida de distancia. El objetoverde representa la posicion real del sensor de distancia. El centro de la esfera es laposicion desde la cual el vehıculo aereo recibio la primera medida de distancia delsensor.

mejora la estimacion inicial de los sensores de distancia. Esta inicializacion au-tomatizada generaliza el metodo de inicializacion propuesto en [6], haciendo quecada mezcla de Gaussianas se adapte mejor a la distribucion uniforme y esfericareal.

Por otro lado, el empleo de un filtro de Kalman centralizado para la locali-zacion del vehıculo y de los diferentes elementos del mapa permite mantener lacorrelacion existente entre todas las variables del sistema, haciendo posible la in-tegracion de medidas de distancia entre cada par de elementos del mapa. Esta esuna caracterıstica que no puede ser tenido en cuenta en un esquema FastSLAM,dado que cada elemento del mapa en este esquema es considerado completamen-te independiente del resto de elementos del mapa y de la posicion del vehıculoaereo. La correlacion entre elementos del mapa es especialmente interesante enaquellas aplicaciones en las que los sensores de distancia se encuentran integra-dos en elementos estructurales, de modo que exista una restriccion asociada a laposicion relativa de los sensores de distancia, haciendo que exista entre dichoselementos una correlacion elevada.

3. Descripcion del filtro EKF para 3D RO-SLAM

3.1. Vector de estado: Parametrizacion esferica reducida

El vector de estado utilizado para el filtro EKF esta compuesto por el estadodel vehıculo aereo xr (por ejemplo, xr = [xr, yr, zr]

T para una representacionCartesiana en 3D de la posicion del vehıculo) y el estado de los elementos delmapa fi, de modo que, por cada intante t, el vector de estado sera de la forma:

x = [xtr, ft1, f

t2, . . . , f

tm]T (2)

En este vector de estado, la posicion de cada elemento del mapa esta ex-presado en una representacion esferica reducida con respecto a la posicion xi =[xi, yi, zi]

T desde la cual el vehıculo aereo recibio la primera medida de distancia.Ası, si los parametros de la posicion de un elemento del mapa i son completa-mente conocidos, dicha posicion quedara expresada como fi = [xti, ρi, θi, φi]

T ,siendo ρi la primera medida de distancia recibida entre un sensor de distancia iy el vehıculo aereo y, θi y φi los angulos de azimuth y elevacion respectivamente.Dado que con una sola medida de distancia ri no conocemos la informacion dela orientacion relativa al vehıculo en la que se encuentra el sensor, los valoresθi y φi no pueden ser inicializados. Este artıculo propone muestrear de manerauniforme el espacio de posibles valores θi y φi en nθ y nφ muestras independien-tes de modo que la combinacion de esta muestras {θijθ , φijφ} forme el espaciocompleto de n = nθnφ hipotesis, mientras que una extension directa del metodopropuesto en [6] incluirıa todos las hipotesis en el vector de estado. Es decir,la paremetrizacion propuesta en este artıculo utiliza un vector de estado parainicializar cada elemento del mapa de 4 + nθ + nφ parametros, mientras queuna extension directa del metodo presentado en [6] emplearıa 4 +nθnφ parame-tros para representar la misma cantidad de hipotesis. Con la parametrizacionesferica reducida propuesta, el vector de estado de cada elemento del mapa serıainicialmente de la forma:

fi = [xti, ρi, θi1, θi2, . . . , θinθ , φi1, φi2, . . . , φinφ ]T (3)

En esta parametrizacion, el conjunto de muestras θijθ junto con su respec-tivo peso (o probabilidad) asociado ωijθ forma las medias y los pesos de unamezcla de Gaussianas, y el conjunto de muestras φijφ junto con su respectivopeso (o probabilidad) asociado ωijφ formarıa otra segunda mezcla de Gaussianas.La funcion de probabilidad de cada mezcla de Gaussianas fθi(x) y fφi(x) mode-lan un par de distribuciones uniformes en los intervalos [0, 2π) y (−π/2, π/2)respectivamente, siendo su expresion final de la forma:

fθi(x) = U(0, 2π) ≈∑nθjθ=1 ωθijθN (θijθ , σθijθ ) (4)

fφi(x) = U(-π2 ,π2 ) ≈

∑nφjφ=1 ωφijφN (φijφ , σφijφ ) (5)

Con esta representacion reducida del estado para la inicializacion de cadaelemento del mapa fi, la combinacion de todos los posibles pares de muestras{θijθ , φijφ} junto con la desviacion asociada a cada hipotesis, modelarıa el espaciocompleto de hipotesis de la distribucion uniforme esferica dada por el modelo deobservacion de las medidas de distancia. Un ejemplo de las hipotesis generadaspor esta parametrizacion se muestra en la Figura 2, donde los valores de azimuthhan sido muestreados en 5 muestras θijθ y los valores de elevacion han sidomuestreados con 3 muestras φijφ , formando un total de 15 hipotesis (marcasamarillas de la Figura 2). Los siguientes apartados describen los procesos de

−2−1

01

2

−3

−2

−1

0

1−1.5

−1

−0.5

0

0.5

1

1.5

X (meters)

← Beacon 1: 1.5573m

Y (meters)

Z (

met

ers)

Figura 2. Inicializacion de un sensor de distancia en el filtro EKF para una medidade distancia de 1 metro.

inicializacion de parametros del filtro, ası como la actualizacion y la poda dehipotesis del filtro.

3.2. Inicializacion del filtro

Una vez que se recibe la primera medida de distancia ri de un sensor i, ladistribucion de los posibles valores de azimuth θi y elevacion φi forman unadistribucion uniforme alrededor la posicion del vehıculo aereo [xi, yi, zi]

T en elmomento de la recepcion de dicha medida de distancia tal y como se muestraen la Figura 1. Esta distribucion uniforme y esferica puede ser representada porun par de GMMs utilizando las expresiones (4) y (5) respectivamente. De modoque los parametros de las posibles hipotesis serıan xi, yi, zi, ρi y un conjunto depares {θijθ , φijφ} formado a partir de los valores muestreados en θi y φi.

Con esta parametrizacion, el numero de hipotesis generadas h para modelarla distribucion uniforme real esta relacionado con el numero de muestras en θi(nθ) y el numero de muestras en φi (nφ) por la expresion h = nθnφ, siendo

nθ = d√

2h∗e y nφ = dnθ/2e. Siendo el numero de hipotesis optimo h∗ un valorinicializado a partir de la primera medida de distancia ri como h∗ = 4πr2i d,donde d representa la densidad de hipotesis que queremos obtener. Diferentesexperimentos realizados en simulacion, muestran que una densidad optima dehipotesis esta en torno a d = 0,18. Este valor fue elegido de forma empıricarealizando diferentes simulaciones donde un vehıculo aereo seguıa una trayectoriacircular en el plano XY y un movimiento senoidal en el eje Z, por otro ladoel sensor de distancia se coloco en el centro de dicha trayectoria circular y seemplearon diferentes distancias entre el vehıculo y la posicion del sensor (radiode la trayectoria circular) y diferentes numeros de hipotesis. Con los resultados

de estos experimentos, la densidad optima d se selecciono teniendo en cuenta lacarga computacional requerida por cada valor de densidad y el error absolutoproducido por la hipotesis mas probable. El Cuadro 1 muestra algunos de losvalores obtenidos durante los experimentos.

Cuadro 1. Resultados experimentales obtenidos para la seleccion de la densidadoptima de hipotesis utilizando un sensor de distancia con una desviacion tıpica deσρ = 50cm

ρ (metros) Num.Hipotesis Error (cm) Densidad de Hipotesis

5 62 3.8 0.1910 240 9.3 0.1820 1200 12.5 0.2340 3000 11.56 0.1560 6500 24.2 0.15

Una vez que se conoce el numero de muestras a utilizar en θi y φi, y por tantoel numero de hipotesis h que se van a generar, el siguiente paso es definir el valorde los pesos ωijθ y ωijφ asociados a cada muestra, ası como la media θijθ y φijφde la distribucion normal formada por cada muestra y la desviacion asociadaσθijθ y σφijφ . Para inicializar estos valores, teniendo en cuenta la distribucion

uniforme real de las muestras, el valor de los pesos ωijθ y ωijφ debe ser uniforme,es decir su valor sera ωijθ = 1/nθ y ωijφ = 1/nφ.

La media de las nθ muestras en θi debera del mismo modo estar uniforme-mente distribuida en el intervalo [0, 2π), al igual que la media de las nφ muestrasen φi debera estar uniformemente distribuida en el intervalo (−π/2, π/2) tal ycomo se muestra en la Figura 2 para 15 hipotesis (5 valores de azimuth y 3 deelevacion).

Finalmente, para seleccionar el valor de desviacion asociado a cada muestrade forma que la mezcla de Gaussianas modele de la forma mas exacta posible elespacio de probabilidad uniforme inicial, estos valores son inicializados de formaidentica acorde a las siguientes expresiones:

σθijθ =2π

kθnθjθ = 1, . . . , nθ (6)

σφijφ =π

kφnφjφ = 1, . . . , nφ (7)

Los valores kθ y kφ de estas expresiones han sido seleccionados empıricamenterealizando diferentes simulaciones en las que se comparaba una GMM con la dis-tribucion uniforme deseada. En dichas simulaciones se han empleado diferentesnumeros de Gaussianas para formar cada GMM probada en el distancia [0, 2π)y (−π/2, π/2) , ademas, por cada GMM, se emplearon diferentes valores de des-viacion para inicializar las componentes de cada GMM. Todas las GMM fueron

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6−200

0

200

400

600

800

1000

StandardkdeviationkforkeachkgaussiankofkthekGMMk(σθ)

Kul

lbac

kkLe

ible

rkdi

stan

cek(

KL)

N.Hyp:k 1N.Hyp:k 5N.Hyp:k 10N.Hyp:k 20N.Hyp:k 40N.Hyp:k 80N.Hyp:k160N.Hyp:k220

(a) Resultados experimentales parakθ

0 0.1 0.2 0.3 0.4 0.5 0.6

−500

0

500

1000

1500

2000

StandardbdeviationbforbeachbgaussianbofbthebGMMb8σφ)

Kul

lbac

kbLe

ible

rbdi

stan

ceb8

KL)

N.Hyp:b 1N.Hyp:b 5N.Hyp:b 10N.Hyp:b 20N.Hyp:b 40N.Hyp:b 80N.Hyp:b160N.Hyp:b220

(b) Resultados experimentales parakφ

Figura 3. Experimentos realizados para la seleccion optima de kθ y kφ. El eje Yrepresenta el factor de divergencia de Kullback-Leibler asociado a cada GMM probada yel eje X representa la desviacion tıpica con la que se inicializa cada componente de cadaGMM simulada. Cada linea representa una GMM simulada para un numero concreto decomponentes (hipotesis). (a) muestra los resultados para GMM en el intervalo [0, 2π)(espacio de θi) y (b) muestra los resultados para GMM en el intervalo (−π/2, π/2)(espacio de φi).

comparadas con la distribucion uniforme asociada utilizando el factor de diver-gencia de Kullback-Leibler. Los resultados de estos experimentos se muestran enla Figura 3, donde el eje X representa el valor de desviacion tıpica utilizado y eleje Y representa el valor de divergencia de Kullback-Leibler obtenido por cadauna de las GMM utilizadas (cada linea representa una GMM con un numerodiferente de hipotesis). Los valores de kθ y kφ que ofrecen una mejor adaptacionde la distribucion uniforme para un numero cualquiera de hipotesis es kθ = 1,9y kφ = 2,5.

Las principales ventajas de este proceso de inicializacion comparado con otrosmetodos de aproximacion comentados en la introduccion de este artıculo sontres: la inicializacion retardada de los parametros de cada elemento del mapapermite la integracion de todas las medidas de distancia recibidas en el filtroEKF desde el principio, segundo, permite la integracion de nuevos sensores dedistancia en cualquier instante sin tener que establecer el numero de hipotesisnecesario de forma fija y finalmente, adapta el valor de los parametros de lasmezclas de Gaussianas de forma automatica a partir de la primera medida dedistancia recibida.

3.3. Actualizacion del filtro

Tras la recepcion de la primera medida de distancia del elemento fi y su con-secuente inicializacion directa del vector de estado en el filtro EKF, las siguientesmedidas recibidas ri son utilizadas para actualizar la estimacion del elemento fien el filtro y la de los pesos asociados a las GMMs. No obstante, en lugar deutilizar nθnφ ecuaciones por cada medida ri para actualizar todas las hipotesis

recibida, tal y como se harıa en [1], este artıculo propone una estrategia de ac-tualizacion que solo requiere n = nθ + nφ ecuaciones, una por cada muestra enθi y φi. De modo que, al recibir una nueva medida de distancia ri del sensor fi,la ecuacion utilizada para actualizar los parametros del elemento fi en el vectorde estado (2) serıa la siguiente:

ri =√

(xfi − xr)2 + (yfi − yr)2 + (zfi − zr)2 (8)

Donde xfi , yfi y zfi se calculan como:

xfi = xi + ρicos(θi)cos(φi) (9)

yfi = yi + ρisin(θi)cos(φi) (10)

zfi = zi + ρisin(φi) (11)

La cuestion ahora es como integrar (8) en el filtro EKF por cada muestraen θi y φi de forma independiente, teniendo en cuenta que (8) depende de am-bos parametros θi y φi. La solucion propuesta en este artıculo plantea integrarel estado actual de la GMM en φi a la hora de actualizar una sola muestraθijθ , obteniendo un valor medio de elevacion φi tal y como describe la siguienteexpresion.

φi =

nφ∑jφ=1

φijφωφijφ (12)

Del mismo modo, para actualizar una sola muestra φijφ , se empleara un valor

medio φi calculado de forma analoga a partir del estado actual de la GMM enθi como:

θi =

nθ∑jθ=1

θijθωθijφ (13)

El resto de parametros son actualizados por cada una de las n = nθ + nφecuaciones empleadas. Aunque, en este sentido, es importante tener en cuentaque, tal y como se indica en [6], no es posible integrar la desviacion σri (equi-valente a la desviacion σρi representada en la Figura 1) de la medida ri n vecesdado que, en caso contrario, serıa como si se integrasen n medidas ri iguales en elfiltro, haciendo que el filtro EKF tienda a diverger con el paso del tiempo. Paraevitar este problema, este artıculo, al igual que [6], propone dividir la varianzaσ2ri asociada a cada medida ri en n varianzas σ2

rijde modo que

∑nj=1 σ

2rij

= σ2ri .

Para ello, cada varianza es calculada como σ2rij

= σ2ri/λij , donde λij is un factor

proporcional de probabilidad lij asociada a cada una de las n ecuaciones em-pleadas para actualizar el filtro, tal que

∑nj=1 λij = 1. Ası, la probabilidad lij

asociada de cada ecuacion de actualizacion se calcularıa como:

lij =

{p(ri|xtr,xti, ρi, θij , φi) j ∈ [1, nθ]

p(ri|xtr,xti, ρi, θi, φi(j-nθ)) j ∈ [nθ+1, nθ+nφ](14)

λij = lij/

n∑k=1

lik (15)

Estas probabilidades siguen una distribucion Gaussiana cuya media es obte-nida del resultado de aplicar (8) por cada par de valores {θijθ , φi} o {θi, φijφ},y propagando la correspondiente covarianza del estado actual a traves del Jaco-biano de dicha ecuacion.

El ultimo paso a realizar cuando se recibe una nueva medida de distanciari es actualizar los pesos ωθijθ y ωφijφ asociados a cada muestra en θi y φi

respectivamente. En este caso, se empleara la probabilidad p(ri|xtr,xti, ρi, θi, φi)para actualizar cada peso ωθijθ y ωφijφ , pero en este caso, en lugar de emplear

un valor medio de azimuth y elevacion obtenido de cada una de las GMMs, lospesos se actualizan empleando el valor de probabilidad mas alto por cada par{θijθ , φijφ} tal y como se indica a continuacion:

ωθijθ = ωθijθmax(p(ri|xtr,xti, ρi, θijθ , φijφ)|jφ = 1..nφ) (16)

ωφijφ = ωφijφmax(p(ri|xtr,xti, ρi, θijθ , φijφ)|jθ = 1..nθ) (17)

Finalmente, los valores de los pesos deben ser normalizados. El empleo delvalor maximo de probabilidad en (16) y (17), al igual que el empleo de los valoresmedios θi y φi, hace que cada actualizacion sea completamente independientedel resto de las muestras. El empleo de la probabilidad mayor en lugar de re-utilizar el valor medio, se debe a que un valor medio puede dar lugar a unaprobabilidad p(ri|xtr,xti, ρi, θi, φi) pequena como consecuencia del estado actualde cada GMM, haciendo que el peso de una muestra en θi o φi sea altamentedependiente del estado de dichas GMMs.

3.4. Poda de hipotesis

Para reducir la carga computacional del metodo presentado, a medida que lospesos de cada muestra en un determinado elemento fi del vector de estado vanactualizando, este artıculo propone un metodo de poda de hipotesis de formaque cada muestra del vector de estado es eliminado si satisface alguna de lassiguientes condiciones:

El peso ωij de la muestra j de cualquiera de las GMMs del elemento fiesta por debajo de un umbral:

ωθij ≤ 10−11/h (18)

ωφij ≤ 10−11/h (19)

Donde h = nθnφ es el numero actual de hipotesis. El valor de este umbralha sido escogido de forma experimental tras realizar diferentes simulaciones,quedando el umbral que ofrecıa mejores resultados de forma independientea la posicion relativa del sensor de distancia con respecto al vehıculo.

La longitud del arco que separa una muestra αij1 de otra αij2 con j1 6= j2en cualquiera de las GMMs es inferior al 3 % del valor del parametro ρi paraun elemento fi dado. Se conserva aquella muestra que posea un peso mayor,eliminando la otra muestra.

Cabe destacar de este metodo que, por ejemplo, al eliminar una muestraen θi, en realidad se estan eliminando nφ hipotesis (nθ en el caso de eliminaruna muestra en φi) como consecuencia de la parametrizacion esferica reducidaempleada en este artıculo.

4. Resultados experimentales

Para validar el metodo propuesto esta seccion muestra los resultados obteni-dos mediante tres tipos de simulaciones diferentes, donde cada tipo de simulacionpersigue demostrar diferentes aspectos del algoritmo desarrollado. Las simula-ciones estan basadas en el modelado de un vehıculo aereo de 4 helices y una redde sensores inalambrica empleada como dispositivo para medir la distancia entrecada par de nodos de la red. En estas simulaciones se asume que el vehıculo aereointegra uno de los nodos de la red de sensores (nodo base) que permita medir ladistancia con el resto de nodos de la red (beacons).

La red de sensores utilizada en este simulador esta basada en la caracterısti-ca estatica obtenida de una red de sensores real, la cual realiza las medidas dedistancia a traves de un metodo optimizado basado en el calculo del tiempo devuelo de la senal de radio, en lugar de otros metodos mas convencionales comoson RSSI. De modo que la desviacion de las medidas de este sensor han sido ca-racterizadas considerando un ruido Gaussiano de media 0 y desviacion σri = 1,5metros. Cada mensaje de la red de sensores simulada contiene un identificadorunico, tipo RFID, para conocer el sensor que produjo la medida. Ademas, paraser lo mas realistas posible, el simulador tiene en cuenta el intervalode transmi-sion de los dispositivos y el radio de alcance de los mismos.

En cuanto al vehıculo aereo empleado, se ha considerado el modelo dinamicode un vehıculo aereo de 4 helices [4] donde los parametros del vector de estadoserıan los siguientes:

xr = [xr, yr, zr, αr, θr, ψr, xr, yr, zr, αr, θr, ψr]T (20)

Es decir, el vector de estado incluye la posicion tridimensional en repre-sentacion Cartesiana del vehıculo [xr, yr, zr], la orientacion en formato RPY[αr, θr, ψr] y las velocidades lineales [xr, yr, zr] y angulares asociadas [αr, θr, ψr].

4.1. Trayectoria circular

En el primer experimento realizado, solo se ha considerado el problema delmapeo de la red de sensores, suponiendo completamente conocida la posicion delvehıculo aereo. Este experimento en simulacion tiene como objetivo validar el

esquema de mapeo propuesto en este artıculo, empleando para ello una trayec-toria optima que permita una correcta trilateracion del sensor de distancia. Paraello, se ha empleado una trayectoria circular de 20 metros de radio en el planoXY y con una comportamiento senoidal en el eje Z que oscila entre [−15, 15]metros, haciendo posible de este modo la trilateracion del beacon (Beacon 1)que esta situado en el centro de esta trayectoria circular. En este caso, se hasimulado que el radio de alcance del emisor de radio sea superior al radio de latrayectoria circular.

Los resultados de este experimento se encuentran resumidos en la Figura4. En esas graficas, se muestra como el simulador coloca una etiqueta junto albeacon (rombo azul) que indica no solo el identificador del mismo si no tambienla medida de distancia producida en cada instante. La trayectoria que debe seguirel vehıculo aereo esta representada por una linea discontinua azul, mientras quela posicion actual del vehıculo y la trayectoria (groundtruth) seguida hasta elinstante actual estan representados con una marca en forma de cruz magenta yuna linea magenta respectivamente.

En estos experimentos el beacon es inicializado practicamente al inicio de latrayectoria desde la recepcion de la primera medida tal y como muestra la figura4(a). En estas figuras, el simulador muestra el conjunto de hipotesis en formade cruces (el intervalo de confianza de las hipotesis no se ha representado porclaridad) cuyo color varıa de rojo a verde en funcion de si el peso va de 0 a 1respectivamente (ver leyenda en Figura 4(a)). Inicialmente todas las hipotesistienen el mismo color (mismo peso) y estan distribuidas de forma uniforme. Amedida que se reciben nuevas medidas, el filtro hace converger cada una de lashipotesis a la solucion correcta, modificando su peso y, por otro lado, el algoritmode poda elimina aquellas hipotesis que cumplen con algunos de los requisitosmencionados anteriormente, tal y como demuestra la Figura 4(b). Finalmente,en la figura 4(c) se muestra como el metodo converge a una sola hipotesis depeso 1 (color verde).

En la Figura 4(d) se muestra el error cometido por la hipotesis a la cualconverge el sistema al final de la simulacion (hipotesis mas probable) desde elmomento de su inicializacion. El error se muestra en centımetros en el eje deordenadas, mientras que el eje de abscisas representa el numero de medidasrecibidas por el vehıculo procedentes de ese beacon. Como se pude observar elerror cometido por el metodo es inferior a los 25 centımetros empleando unsensor con una desviacion de σri = 1,5 metros.

4.2. Trayectoria rectilınea

Este experimento pretende reflejar el comportamiento del metodo en aquelloscasos en los que el vehıculo no realiza una trayectoria optima para poder trilateralla posicion del beacon. En este caso la trayectoria seguida por el vehıculo aereoes en forma de linea recta en el plano XY y un movimiento senoidal en el eje Z,lo cual debe provocar una situacion ambigua en la solucion del metodo a amboslados de la lınea recta. Ademas, en este caso se ha considerado un radio dealcance de unos 50 metros para demostrar el comportamiento dinamico del vector

(a)

−30−20

−100

1020

30

−20

−10

0

10

20−15

−10

−5

0

5

10

15

X (meters)

← Beacon 1: 20.0066

Y (meters)

Z (

met

ers)

(b)

(c)

0 100 200 300 400 500 600 700 8000

50

100

150

200

250

300

350

Samples

Err

or (

cm)

H106

(d)

Figura 4. Evolucion de las hipotesis de un beacon (rombo azul) en una trayectoriacircular (linea azul discontinua) seguida por un vehıculo aereo (cruz y linea magentarepresentan el groundtruth) cuya posicion es completamente conocida (a) - (c). Lashipotesis del beacon estan representadas con cruces cuyo color varıa de rojo a verdesegun si el peso va de 0 a 1 respectivamente, tal y como se indica en la leyenda de laFigura (a). La Figura (d) representa el error absoluto cometido a lo largo de toda latrayectoria por la hipotesis mas probable (la que queda al final de la trayectoria).

de estado a la hora de incluir nuevos nodos en tiempo real y sin retardos conrespecto a la primera medida de distancia. Los resultados de este experimentose muestran en la Figura 5.

Como se puede apreciar, debido a la limitacion en el radio de alcance delnodo emisor, el beacon 1 no es detectado hasta que la distancia entre el vehıculoaereo y el beacon es inferior a dicha distancia, momento en el cual el beacones inicializado de forma instantanea tal y como demuestra la Figura 5(a). aligual que en el caso anterior, las hipotesis son inicializadas con una distribucionuniforme en el espacio y con un peso equivalente que, en este caso, es inferior(color rojo) al peso de inicializacion de la figura 4(a) debido a que el numero dehipotesis utilizado en este caso es mayor como consecuencia de la medida inicialrecibida para la inicializacion de hipotesis. A medida que se integran nuevas me-didas de distancia, el filtro tiende a converger a dos hipotesis como consecuencia

(a) (b)

0 100 200 300 400 500 600 7000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

Samples

hypo

thes

es w

eigh

t

(c)

0 100 200 300 400 500 600 7000

500

1000

1500

2000

2500

Samples

Err

or (

cm)

H219

H389

(d)

Figura 5. Evolucion de las hipotesis de un beacon (rombo azul) para una trayectoriarectilınea en el plano XY (linea azul discontinua) la cual es seguida por un vehıculoaereo (cruz y linea magenta representan el groundtruth) cuya posicion es completa-mente conocida (a) - (b). Las hipotesis del beacon estan representadas con cruces cuyocolor varıa de rojo a verde segun si el peso va de 0 a 1 respectivamente, tal y comose indica en la leyenda de la Figura (a). La Figura (c) representa la evolucion de lospesos de las hipotesis. La Figura (d) representa el error absoluto cometido a lo largode toda la trayectoria por las hipotesis mas probables (las que quedan al final de latrayectoria).

de un problema de observabilidad del estado asociado a la trayectoria rectilıneaen el plano XY (ver Figura 5(b)). La Figura 5(c) muestra la evolucion de lashipotesis desde el momento de la inicializacion del nodo, siendo el eje de abscisasel numero de medidas recibidas y el eje de ordenadas el peso asociado a cadahipotesis. Como se puede observar en la Figura 5(d), en este caso, la hipotesis demayor probabilidad al final de la simulacion coincide con la hipotesis que tieneun error absoluto menor.

4.3. Multiples sensores de distancia

En este ultimo experimento se pretende demostrar el funcionamiento delmetodo incluyendo la localizacion del vehıculo aereo, ası como el empleo de

nodos ((Anchor)) (nodos de los que se conoce su posicion) y multiples ((beacons))(nodos que deben ser mapeados) y una IMU. Los nodos ((Anchor)) permitencorregir la estimacion de la posicion del robot aereo mediante la trilateraciondel mismo a partir de la posicion conocida de los nodos ((Anchor)), mientrasque la IMU permite corregir la estimacion en la velocidad lineal y angular delmismo. El empleo de los nodos ((Anchor)) ası como la IMU es necesario debidoa la alta incertidumbre introducida por el modelo predictivo del vehıculo aereoempleado. Los resultados de este experimento se pueden apreciar en la Figura6(a) y la Figura 6(b), en los que se muestra el groundtruth del robot aereo comouna linea y cruz (posicion actual) en color magenta, la posicion de los nodos((Anchor)) como rombos magenta y la posicion real de los nodos ((Beacon)) comorombos azules. La lınea y cruz (posicion actual) celeste muestran la estimacion dela localizacion del robot aereo a lo largo de todo el recorrido simulado, mientrasque la estimacion final de los nodos ((Beacon)) se muestra con unas cruces verdes(hipotesis mas probable que no ha sido podada). Las elipses amarillas indican elintervalo de confianza de cada estimacion tanto en localizacion como en mapeo.

Tal y como muestran los resultados, el error cometido tanto en localizacioncomo en mapeo es de aproximadamente 20 centımetros, empleando una red desensores cuya desviacion tıpica en las medidas de distancia es de σri = 1,5metros. Este error puede variar en funcion del numero de medidas erroneasrecibidas y de la trayectoria del robot aereo, tal y como ocurre con el ((Beacon 1)),cuyo error es de 0.7 metros aproximadamente al final de la trayectoria simulada.

5. Conclusiones y desarrollos futuros

Este artıculo a presentado un sistema de localizacion de vehıculos aereos ba-sado 3D RO-SLAM utilizando una parametrizacion esferica reducida por cadaelemento del mapa la cual permite reducir la carga computacional del proble-ma. La solucion propuesta utiliza el esquema EKF-SLAM incluyendo tanto laposicion del vehıculo aereo como la posicion de cada elemento del mapa en unmismo vector de estado, haciendo posible la integracion de medidas producidastanto entre robot y sensor, como entre sensor y sensor y manteniendo al mismotiempo la posible correlacion que pudiera existir entre cada par de elementos delmapa. al mismo tiempo esta solucion permite la inicializacion directa del vectorde estado de cada elemento del mapa al recibir una unica medida de distancia,gracias a la integracion de un par de modelos probabilısticos de mezclas de Gaus-sianas (GMM) independientes entre sı en el vector de estado para representar lamultimodalidad del modelo de observacion asociado a las medidas de distancia(se emplea una GMM por cada parametro de orientacion θi y φi). Esta solucionno solo permite la inicializacion directa de los elementos del mapa con una solamedida de distancia, sino que tambien propone un metodo para inicializar losparametros de cada elemento del mapa de forma automatica dependiendo de laprimera medida de distancia recibida. Ademas, al considerar los parametros θiy φi de forma totalmente independiente, es posible actualizar el filtro de Kal-

man de forma eficiente, reduciendo la carga computacional requerida por otrosmetodos 3D RO-SLAM presentados hasta el momento.

Para la validacion del metodo, este artıculo presenta una serie de resultadosde experimentos en simulacion en los que se simula tanto el modelo dinamicode una vehıculo aereo de 4 helices, como el modelo de observacion de un sensorde distancia basado en una red de sensores inalambrica (tecnologıa de radio fre-cuencia). El simulador no solo ha tenido en cuenta la caracterizacion Gaussianade las medidas, sino tambien el radio de alcance de cada uno de los nodos dela red de sensores y la frecuencia de emision de los mismos. La caracterizacionde las medidas de estos sensores de distancia ha sido basada en un modelo desensores que es capaz de calcular la distancia entre cada par de nodos de la redutilizando una tecnica avanzada de estimacion basada en el tiempo de vuelo dela senal.

En el futuro se considerara la inclusion de experimentos en los que se intro-duzcan medidas entre pares de sensores y no solo entre robot y sensor, teniendoen cuenta las posibles restricciones existentes en la posicion relativa entre multi-ples sensores de distancia que estan integrados en un elemento estructural (comopueden ser barras, bloques, etc) como es el caso del proyecto ARCAS que hafinanciado parte de este trabajo. El metodo sera ademas probado con experi-mentos reales que permitan la validacion del metodo en situaciones reales convehıculos aereos.

Agradecimientos

Este trabajo ha sido parcialmente financiado por el proyecto ARCAS (ICT-2011-287617) del Septimo Programa Marco de la Comision Europea y los proyec-tos nacionales RANCOM (P11-TIC-7066) y CLEAR (DPI2011-28937-C02-01).

Ademas, el trabajo de Fernando Caballero esta parcialmente financiado porel proyecto FROG (FP7-ICT-2011.2.1) del Septimo Programa Marco de la Co-mision Europea.

Referencias

1. J.-L. Blanco, J.-A. Fernandez-Madrigal, and J. Gonzalez. Efficient probabilisticrange-only SLAM. In IEEE/RSJ International Conference on Intelligent Robotsand Systems, 2008. IROS 2008, pages 1017 –1022, September 2008.

2. Jose-luis Blanco, Javier Gonzalez, and Juan-antonio Fernandez-madrigal. A pureprobabilistic approach to range-only SLAM. Pasedena Conference Center, Califor-nia, USA, May 2008.

3. Byron Boots and Geoffrey J. Gordon. A spectral learning approach to range-onlySLAM. arXiv:1207.2491, July 2012.

4. S. Bouabdallah and R. Siegwart. Full control of a quadrotor. In IEEE/RSJ Inter-national Conference on Intelligent Robots and Systems, 2007. IROS 2007, pages153–158, 2007.

5. F. Caballero, L. Merino, I. Maza, and A. Ollero. A particle filtering method forwireless sensor network localization with an aerial robot beacon. In IEEE Interna-tional Conference on Robotics and Automation, 2008. ICRA 2008, pages 596 –601,May 2008.

6. F. Caballero, L. Merino, and A. Ollero. A general gaussian-mixture approachfor range-only mapping using multiple hypotheses. In Robotics and Automation(ICRA), 2010 IEEE International Conference on, pages 4404–4409, 2010.

7. J. Djugash, S. Singh, and B. Grocholsky. Decentralized mapping of robot-aidedsensor networks. In IEEE International Conference on Robotics and Automation,2008. ICRA 2008, pages 583 –589, May 2008.

8. Joseph Djugash. Geolocation with Range: Robustness, Efficiency and Scalability.PhD thesis, November 2010.

9. M. Gholami, N. Cai, and R.W. Brennan. An artificial neural network approachto the problem of wireless sensors network localization. Robotics and Computer-Integrated Manufacturing, 29(1):96–109, February 2013.

10. Dan Hai, Yong Li, Hui Zhang, and Xun Li. Simultaneous localization and mappingof robot in wireless sensor network. In 2010 IEEE International Conference onIntelligent Computing and Intelligent Systems (ICIS), volume 3, pages 173 –178,October 2010.

11. Z. Kurt-Yavuz and S. Yavuz. A comparison of EKF, UKF, FastSLAM2.0, andUKF-based FastSLAM algorithms. In 2012 IEEE 16th International Conferenceon Intelligent Engineering Systems (INES), pages 37 –43, June 2012.

12. Zhong Min Wang, De Hua Miao, and Zhi Jiang Du. Simultaneous localizationand mapping for mobile robot based on an improved particle filter algorithm. InInternational Conference on Mechatronics and Automation, 2009. ICMA 2009,pages 1106 –1110, August 2009.

13. P. Yang. Efficient particle filter algorithm for ultrasonic sensor-based 2D range-onlysimultaneous localisation and mapping application. IET Wireless Sensor Systems,2(4):394 –401, December 2012.

(a)

(b)

Figura 6. Resultado final RO-SLAM con experimento en simulacion empleando multi-ples nodos ((Beacon)) y nodos ((Anchor)) (nodos de la red de sensores cuya posicion esconocida) y una IMU. En estos experimentos se muestra el groundtruth del robotaereo como una linea y cruz (posicion actual) en color magenta, la posicion de losnodos ((Anchor)) como rombos magenta y la posicion real de los nodos ((Beacon)) comorombos azules. La lınea y cruz (posicion actual) celeste muestran la estimacion de lalocalizacion del robot aereo a lo largo de todo el recorrido simulado, mientras que laestimacion final de los nodos ((Beacon)) se muestra con unas cruces verdes (hipotesismas probable que no ha sido podada). Las elipses amarillas indican el intervalo deconfianza de cada estimacion tanto en localizacion como en mapeo. La leyenda de laFigura (b) es la misma que la empleada en la Figura (a).