memoria descriptiva del trabajo de investigación (33 páginas

Doctorado en Electrónica.

“Captación Multisensorial y Sistemas Robóticos”

“UTILIZACIÓN DE LOS DETERMINANTES DE CAYLEY-

MENGER EN LA LOCALIZACIÓN POR TRILATERACIÓN ESFÉRICA”

TRABAJO DE INVESTIGACIÓN PARA EL CURSO: “CAPTACIÓN Y MODELADO DEL ENTORNO MEDIANTE MEDIDORES DE DISTANCIA”

(Curso 2005-2006)

Dirigido por:

D. Manuel Mazo Quintas

Realizado por:

D. Ramón Rodríguez Luque

“Trilateración mediante los determinantes de Cayley-Menger”

- 2 -

ÍNDICE:

1 INTRODUCCIÓN A LA LOCALIZACIÓN POR TRILATERACIÓN ESFÉRICA. 3

2 BIDETERMINANTES Y DETERMINANTES DE CAYLEY-MENGER. 5

2.1 PROPIEDADES GEOMÉTRICAS DE LOS DETERMINANTES DE CAYLEY-MENGER. 5 2.2 PROPIEDADES GEOMÉTRICAS DE LOS BIDETERMINANTES DE CAYLEY-MENGER. 6

3 EXPRESIÓN DEL CÁLCULO DE LA TRILATERACIÓN ESFÉRICA. 10

4 ANÁLISIS DEL ERROR DE POSICIÓN (GDOP). 15

4.1 INFLUENCIA DEL ERROR DE LA LOCALIZACIÓN DE LAS BALIZAS, EN LA POSICIÓN

DEL ROBOT. 15 4.2 INFLUENCIA DEL ERROR DE LAS MEDIDAS DE DISTANCIA A LAS BALIZAS, EN LA POSICIÓN DEL ROBOT. 19

5 RESULTADOS DE LAS SIMULACIONES. 23

5.1 CASO I.- BALIZAS CASI ALINEADAS. 23 5.2 CASO II.- BALIZAS NADA ALINEADAS. 24

6 FUNCIONES MATLAB IMPLEMENTADAS. 26

6.1 FUNCIÓN: [PUNTO1 PUNTO2] = TRILATERACION(P1,P2,P3,L1,L2,L3) 26 6.2 FUNCIÓN: BIDETERMINANTE_TRILAT(MATRIZ) 27 6.3 FUNCIÓN: D(P,Q) 27 6.4 FUNCIÓN: DETERMINANTE(P) 28 6.5 FUNCIÓN: BIDETERMINANTE(P,Q) 29 6.6 FUNCIÓN: AREAS(P1, P2, P3, P4) 29 6.7 FUNCIÓN: ERROR_DISTANCIA(P1, P2, P3, P4) 30 6.8 FUNCIÓN: ERROR_POSICION(P1, P2, P3, P4) 30 6.9 SCRIPT: CÁLCULO Y REPRESENTACIÓN DE LOS ERRORES. 31

7 REFERENCIAS. 33


- 3 -

1 Introducción a la Localización por Trilateración Esférica.

El problema de la localización es de gran importancia en nuestros tiempos. En múltiples aplicaciones es preciso averiguar la posición absoluta de un móvil o robot con respecto de algún sistema de referencia.

Es muy habitual realizar la localización basándose en la medición de distancias a ciertos puntos de referencia, como por ejemplo en el caso de la localización absoluta realizada en la técnica GPS (global position system).

Para realizar la localización de un móvil se puede realizar la medida de los tiempos de vuelo de una señal, medida indirecta de distancia, desde unos puntos de referencia de localización conocida o balizas, al punto de interés.

El proceso para obtener la posición de un móvil a partir de la información de distancias a posiciones de localización conocidas se denomina trilateración esférica.

Figura 1-1.- Representación de la localización mediante de un móvil mediante trilateración esférica.

En la figura 1-1 se muestra un caso de aplicación en el que los puntos de referencia de posición conocida serían las torretas ubicadas en 432 ,, xxx

((( y se miden

las distancias desde el móvil a dichos puntos por algún procedimiento, como por ejemplo, de medida de tiempos de vuelo.

Para resolver el problema de la localización basta con resolver el siguiente sistema de ecuaciones que representa la solución a la intersección de las tres esferas


- 4 -

definidas mediante sus centros en las posiciones de referencia y sus radios como las distancias medidas al móvil.

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

−+−+−=

−+−+−=

−+−+−=

241

241

241

24

231

231

231

23

221

221

221

22

zzyyxxr

zzyyxxr

zzyyxxr

Dado que dicho sistema de ecuaciones no es un sistema lineal, su resolución no es inmediata. Tradicionalmente se ha resuelto, mediante métodos algebraicos o métodos numéricos.

Algunas soluciones pasan por linealizar el sistema de ecuaciones mediante la inclusión de una nueva variable, lo que obliga a utilizar la medida de distancia a otra baliza más, con lo que hacen falta cuatro para localizar en 3D. El sistema conseguido se puede luego resolver mediante resolución directa, mínimos cuadrados, descomposición en valores singulares, etc.

Otras soluciones pasan por métodos que implican conocer una estimación previa de la localización para iterativamente aproximarse al valor más acertado.

En la solución propuesta basta la información de tres balizas para hacer la localización en 3D, y se obtiene una expresión directa a aplicar para obtener la localización.

Los sensores a utilizar para la medida de distancias suelen ser señales de ultrasonidos, radiofrecuencia e infrarrojos, mientras que las técnicas de medida más habituales son el tiempo de vuelo o el desfase entre la señal emitida y recibida. En cualquiera de los casos las medidas estarán sometidas a errores y será necesario averiguar como influyen dichos errores en la estimación de localización realizada.

Este trabajo se dedica al estudio de aplicación de los bideterminantes de Cayley-Menger a la resolución del problema de la obtención de la posición por trilateración esférica [1], así como, al modelado e implementación mediante la herramienta MATLAB del mismo, estudiando la dilución de la precisión de la localización en función de la ubicación de las balizas (puntos de referencia) y la localización del móvil, debida al error de los valores estimados, tanto de las medidas de distancia del robot a las balizas, como de las coordenadas de las balizas (los puntos conocidos).

La solución aquí descrita se basa en construir una expresión para el cálculo que se apoya en modelos geométricos, y características geométricas de los bideterminantes y determinantes de Cayley-Menger.


- 5 -

2 Bideterminantes y determinantes de Cayley-Menger.

Dado que en la resolución de la trilateración propuesta se van a utilizar tanto los determinantes, como los bideterminantes de Cayley-Menger, se procede en este punto a describir en que consisten, como se calculan y las implicaciones geométricas que tiene su interpretación.

Se define el bideterminante de Cayley-Menger de dos secuencias de puntos como:

Ecuación 2-1.- Bideterminante de Cayley-Menger.

Donde D(pi,qj) se corresponde con el cuadrado de la distancia entre los puntos ‘pi ’ y ‘qj ‘.

Cuando las dos secuencias de puntos son la misma se representará como:

Ecuación 2-2.- Cuando las dos secuencias son la misma se denomina determinante de Cayley-Menger.

Y se denominará determinante de Cayley-Menger.

2.1 Propiedades geométricas de los determinantes de Cayley-Menger.

En el caso de n=2:

Ecuación 2-3.- Determinante de dos puntos corresponde con el cuadrado de la distancia entre ellos.

El determinante corresponde con el cuadrado de la distancia euclidea entre los puntos ‘p1’ y ‘p2 ‘, lo que es consecuente con la propia definición de determinante.

En el caso de n=3 y siendo A el área del triángulo definido por los puntos:


- 6 -

Ecuación 2-4.- Determinante de tres puntos corresponde con el cuadrado del doble del área del triangulo encerrado por ellos.

El determinante de Cayley-Menger de los puntos se corresponde con el cuadrado del doble del área del triangulo encerrado por ellos, o bien, con el cuadrado del área del paralelogramo definido por los dos vectores formados con los tres puntos (figura 2-1).

Figura 2-1.- En el lado izquierdo el mencionado paralelogramo definido por los dos vectores, y a la derecha el triángulo mencionado en el texto.

En el caso de n=4 y siendo V el volumen del tetraedro definido por los puntos:

Ecuación 2-5.- Determinante de cuatro puntos corresponde con el cuadrado de seis veces el volumen de tetraedro definido por los puntos.

Corresponde con el cuadrado de seis veces el volumen de tetraedro definido por los puntos.

Se puede hacer la interpretación geométrica de los determinantes de Cayley-

Menger como veces el cuadrado del hipervolumen ocupado por la serie de puntos.

Si n>4 dado que el determinante da esencialmente el volumen de un simplex en R n-1, pero, como es degenerado a R3, su volumen es cero.

2.2 Propiedades geométricas de los bideterminantes de Cayley-Menger.

En el caso de n=2 (figura 2-2) se relaciona directamente con el producto escalar entre dos vectores, expresado en la ecuación 2-6.


- 7 -

Figura 2-2.- Puntos a los que se hace referencia en al cálculo del bideterminante de n=2.

Ecuación 2-6.- Expresión del bideterminante para n=2.

Además en el caso de que p1 = q1 (figura 2-3) se puede expresar directamente el coseno del ángulo que forman los vectores formados por p1 p2 y q1 q2, mediante una expresión basada en bideterminantes como se hace en la ecuación 2-7.

Figura 2-3.- Puntos a los que se hace referencia en al cálculo del bideterminante de n=2 cuando p1 = q1

Ecuación 2-7.- Expresión del teorema del coseno expresado en forma de bideterminantes de Cayley-Menger.

En el caso de n=3 (figura 2-4) se relaciona directamente con el producto escalar entre los dos vectores que definen la superficie de cada uno de los triángulos, y que se obtienen del producto vectorial de los dos vectores que se pueden componer mediante los tres puntos de cada uno de los dos triángulos.

En la ecuación 2-8 se expresa el bideterminante de dos conjuntos de tres puntos, y tal como se indicaba anteriormente, como el producto escalar de dos productos vectoriales. También están indicadas las expresiones para el cálculo de la superficie de los dos triángulos en forma de la mitad del módulo del producto vectorial correspondiente, basándose en el resultado de la ecuación 2-4.


- 8 -

Figura 2-4.- Representación de los puntos usados en el bideterminante para n=3

Ecuación 2-8.- Expresión del bideterminante para n=3, y el área de los triángulos formados por cada conjunto de tres puntos.

En el caso de que p1 = q1 y p3 = q2 , como se muestra en la figura 2-5, se puede expresar directamente el coseno del ángulo que forman los planos formados por p1 p2 p3 y q1 q2 q3, como se hace en la ecuación 2-9 y 2-10.

Figura 2-5.- Caso de n=3 en que coincide uno de los lados de cada triángulo, es decir, p1 = q1 y p3 = q2.


- 9 -

Ecuación 2-9.-Relación entre el ángulo formado por la superficie compuesta por los puntos ‘p’ y la de los puntos ‘q’, en forma de bideterminantes.

Ecuación 2-10.- Ecuación 2-9, elevando al cuadrado y expresándola en función de los cuatro puntos que influyen en la trilateración. Teorema del coseno generalizado al tetraedro.

Como se demuestra en [4] se puede expresar el cos(Φ)2 de la forma de la ecuación 2-11, cosa que es útil para el posterior desarrollo del cálculo de la trilateración.

Ecuación 2-11.- Otra forma de expresar el cos(Φ)2 para relacionarla con la ecuación 2-10.

E igualando el término derecho de ambas ecuaciones 2-10 y 2-11, se obtiene la ecuación 2-12 de utilización directa en el posterior desarrollo del cálculo de la trilateración.

Ecuación 2-12.- Expresión fruto de igualar la 2-10 con la 2-11, y que será utilizada en el próximo apartado dedicado al cálculo de la trilateración.


- 10 -

3 Expresión del cálculo de la trilateración esférica.

El problema a resolver se puede plantear en base a la figura 3-1 en la que se muestra el móvil a localizar en la posición ‘p4 = (x y z)’ que es el objetivo del problema, dados l1, l2 y l3 distancias del móvil a cada uno de los puntos de referencia: p1, p2 y p3 de coordenadas conocidas (balizas).

Figura 3-1.- Representación de la base del problema de la trilateración.

Para desarrollar la expresión para el cálculo de ‘p4’ se procede a componer de forma geométrica como se muestra a continuación.

En primer lugar se calculan las coordenadas de la proyección de punto ‘p4’ sobre el plano que incluye los tres puntos de referencia conocidos, esto es el punto ‘p’, con lo que a continuación bastará con calcular la distancia del punto buscado a dicha superficie que al multiplicarla por el vector unitario normal a dicha superficie se pueda sumar con el vector de posición del punto ‘p’.

En la figura 3-2 se puede ver la representación de dicho punto ‘p’ además de las tres superficies triangulares en las que se divide el triángulo formado por los tres puntos de referencia, que son utilizadas para el cálculo, y cuyo área viene expresado por A1, A2 y A3.


- 11 -

Figura 3-2.- Representación del punto ’p’.

Se pueden expresar las coordenadas de ‘p’ como sigue:

Ecuación 3-1.- Cálculo de las coordenadas de la proyección del punto p4 sobre el plano que incluye los puntos de referencia.

Se puede interpretar la anterior expresión como el centro de masas del triángulo formado por los puntos de referencia, donde la masa de cada una de las esquinas sería el área que se encierra entre el lado opuesto y el punto ‘p’, y Ab es la suma de todas ellas, es decir, el área del triángulo p1 p2 p3.

En cuanto a A1, A2 y A3, serán las “áreas con signo”1 de los triángulos: p2p3p,

p3p1p y p1p2p.

1 Para un triángulo ‘abc’ en el plano euclídeo con una superficie de valor A, el área con signo se define como +A si el punto ‘b’ está a la derecha de la línea ‘ac’ cuando vamos de ‘a’ a ‘c’. Será por tanto negativo ‘-A’ si el punto ‘b’ está a la izquierda de la línea ‘ac’ cuando vamos de ‘a’ a ‘c’.


- 12 -

Descomponiendo y simplificando la expresión anterior se puede poner:

Ecuación 3-2.- Simplificación de la expresión del cálculo de ‘p’.

Como se puede observar en la figura 3-2 el área A2 se puede obtener proyectando el triángulo formado por los puntos p3p4p1 sobre el plano base p1p2p3, basta con multiplicar dicho área de p3p4p1 por el ángulo formado por las dos superficies Φ24.

Como el área del triángulo está relacionado con el determinante de Cayley-Menger de los puntos que definen sus esquinas de la forma tratada en la ecuación 2-4 se puede expresar que:

),,()·(4

)·cos(

),,()·(4

3212

241342

4312

134

pppDA

AA

pppDA

b

p

p

=

=

=

φ

Ecuación 3-3.- Relación entre áreas y determinantes de Cayley-Menger.

Que se puede simplificar en:

Ecuación 3-4.- Relación de áreas obtenida como la proyección del área de interés sobre el plano base (referencias).

Aprovechando la propiedad geométrica de los bideterminantes que se expresó en la ecuación 2-9 y 2-10 se puede expresar el punto ‘p’ como:


- 13 -

Ecuación 3-5.- Expresión final de cálculo del punto ‘p’.

Una vez obtenida una expresión para el cálculo de la proyección de p4 se puede escribir como se muestra en la ecuación 3-6.

Ecuación 3-6.- Expresión para el cálculo de p4.

En la ecuación 3-6 aparece una constante k3 que corresponde a la relación entre la altura ‘h’ del tetraedro inicialmente planteada y el módulo del producto vectorial de v1 y v2. En cuanto al signo que precede a k3 tiene que ver con si el punto buscado está a un lado o al otro del plano base definido por los puntos de referencia. En el caso particular de que el módulo del producto vectorial fuese uno, entonces k3 coincidiría con ‘h’, la altura del tetraedro.

En la ecuación 3-7 se expresa como calcular ‘h’:

Ecuación 3-7.- Cálculo de ‘h’ la altura del tetraedro formado por todos los puntos.

Y dado que el módulo del vector resultado del producto vectorial de v1 y v2 se puede expresar en forma de determinante de Cayley-Menger como se muestra en la ecuación 3-8, se tiene que combinándolo con el resultado de la ecuación 3-7 el valor del parámetro k3 es el que se muestra en la misma.


- 14 -

Ecuación 3-8.- Cálculo de K3.

Y combinando todos los resultados anteriores se puede obtener una expresión final expresada en la ecuación 3-9 en la que se puede obtener las coordenadas del móvil (p4) de forma directa teniendo como parámetros de entrada las distancias medidas desde el móvil a los puntos de referencia, las distancias entre estos, las coordenadas del primero (p1) y los vectores que definen entre los otros dos puntos de referencia y el primero.

Como ya se describió anteriormente existe una cierta ambigüedad en la obtención del punto buscado debido a que no se puede distinguir con la medida de distancias si el punto p4 se encuentra a un lado o al otro del plano base que contiene a las balizas, lo que queda de manifiesto en la expresión final mediante el signo ± que aparece en su interior.

Ecuación 3-9.- Expresión final para el cálculo de la posición del móvil por trilateración.

Como se puede deducir del análisis de la expresión de la ecuación 3-9, solo estará unívocamente determinada la localización del móvil cuando D(p1p2p3p4) = 0, esto es, cuando el robot esté localizado en el plano base formado por p1, p2 y p3 .

Por otro lado si las tres balizas están alineadas, entonces D(p1p2p3) = 0 y la posición del móvil está indeterminada.


- 15 -

4 Análisis del error de posición (GDOP).

En los sistemas reales las medidas realizadas, tanto de las distancias del robot a las balizas, como la propia localización de las balizas, están afectadas de inexactitud debida a diversos factores. La suma de todos esos factores de influencia al aplicar la trilateración van a dar inexactitud en la estimación de la localización realizada.

El objetivo de este apartado es el análisis de la influencia de los distintos errores en la localización estimada, además de como el propio lugar donde el robot esté, influye en el error de su estimación de localización.

Asumiendo que ambos tipos de medidas, las localizaciones de las balizas y las distancias a las balizas, están influidas por un ruido blanco de densidad de probabilidad gausiana, con media cero, en este apartado se procede a analizar la influencia de dichos ruidos en la localización final del robot.

4.1 Influencia del error de la localización de las balizas, en la posición del robot.

Dados δpi y pi0 que representan el ruido y la posición real (desconocida),

respectivamente, de las balizas, pi , i = 1,2 y 3. Entonces se denomina pi = pi0 + δpi , a

las posiciones conocidas de las balizas. Se asume que dicho ruido es gausiano de media cero, es decir, la esperanza de δpi es cero, E{ δpi } = 0.

Se asume también que las tres coordenadas de posición de cada baliza están incorreladas entre sí, y con la misma varianza σp

2 para las tres balizas. Es decir:

≠=

==

jisipEpE

jisiIppE T

ji

pTji ,0}{}·{

,}{

2

δδ

σδδ

Ecuación 4-1.- Expresión de la incorrelación que existe entre los ruidos que afectan a la localización de las tres balizas.

A partir de la ecuación 3-9 se puede comprobar que:

( ) ( ) ( )[ ]211022

01322111

0444p vvvvvvkvkvkppp ∂×∂+∂×+∂×±∂+∂+∂=−=∂

Ecuación 4-2.- Expresión del error de localización en función del error de posición de las balizas.

Donde:

∂−∂=∂

∂−∂=∂

132

121

ppv

ppv


- 16 -

Luego el error medio de localización del robot debido a la posición de las balizas sería:

{ } { } { } { } ( ){ } ( ){ } ( ){ }

∂×∂+∂×+∂×±∂+∂+∂=∂ 21

0

102

0

2013

0

22

0

11

0

14p vvEvvEvvEkvEkvEkpEE4342143421321321321

{ } ( ){ } ( ) ( ) ( ){ }13322132134p ppppppEkvvEkE ∂×∂+∂×∂+∂×∂±=∂×∂±=∂

Y dado que los ruidos de la posición de las balizas están incorrelados entre sí, siendo sus medias cero, la esperanza de los productos dos a dos de la expresión anterior es cero. Y por tanto:

{ } 0p4 =∂E

Ecuación 4-3.- Valor medio del error de localización del robot debido al error de posición de las balizas.

Luego en las condiciones planteadas de error de posición de las balizas, en las que se asume que de media se conoce la posición cierta, la posición media obtenida no se ve afectada por dicho error.

La matriz de covarianza del error de estimación de la posición del robot se puede evaluar como:

{ }[ ] { }[ ]{ } { } { } { } { }TTTT ppEpEpEppEpEppEpECs 44

0

4

0

4444444 · ∂∂=∂∂−∂∂=∂−∂∂−∂=321321

Ecuación 4-4.- Matriz de covarianza del error de posición del robot, debido al error de posición de las balizas.

Desarrollando la ecuación 4-4 y eliminando los términos que involucran productos de variables incorreladas, se tiene:

( ) { }{ } { }( )( ){ }( )( ){ }( )( ){ }

01

03

03

1031

03

23

2022

02

23

3013

01

23

332

2222

1

112

21

:

1

ppvdonde

pvpvEk

pvpvEk

pvpvEk

ppEkppEk

ppEkkCs

T

T

T

TT

T

−=

∂×∂×+

∂×∂×+

∂×∂×+

∂∂+∂∂+

∂∂−−=

Ecuación 4-5.- Matriz de covarianza del error de posición, desarrollo de la ecuación 4-4.

Que se puede poner de la forma:


- 17 -

( )

( )

−=

−=

−=

Ι

+++

++

−−

=

14

13

12

22223

22

21

221

2 :·

2

1

ppc

ppb

ppa

donde

cbak

kk

kk

Cs pσ

Ecuación 4-6.- Desarrollo de la ecuación 4-5.

Dicha matriz de covarianza resulta una matriz es isotrópica, esto es, la varianza de la localización del robot es la misma en todas las direcciones, de valor σs. Por tanto:

( )

( )

+++

++

−−

=

22223

22

21

221

22

2

1

cbak

kk

kk

ps σσ

Que se puede poner en forma de términos de longitudes, áreas y volúmenes como:

( ) ( )2224

223

22

212

2

181cba

A

VAAA

A bbp

s +++++=

σ

σ

Ecuación 4-7.- Expresión final de la varianza del error de posición del robot en función de la varianza del error de posición de las balizas, expresada en términos de longitudes, áreas y volúmenes.

La expresión de la ecuación 4-7 se puede simplificar en los supuestos de que, suponiendo que las balizas no están alineadas (como se estudió en el apartado anterior en el supuesto de alineación de las balizas la posición del robot no estaría determinada), si el robot está alejado del plano base que contiene a las balizas se puede simplificar como la ecuación 4-8, y si está muy cercano a dicho plano como la ecuación 4-9.

( )222

4

22

18cba

A

V

bV

p

s ++≈

∞→

σ

σ

Ecuación 4-8.- Si el robot está muy alejado del plano base.

( )23

22

212

0

2

1AAA

AbV

p

s ++≈

→

σ

σ

Ecuación 4-9.- Si el robot está muy cerca del plano base.

En la figura 4-1 y 4-2 se puede observar el error de posición relativo al de las medidas de posición de las balizas, en dos casos representativos.


- 18 -

El primero de los casos, en la figura 4-1, se dispone de tres balizas en posiciones que forman un triángulo equilátero en el plano XY, inscrito en un círculo centrado en el origen, de radio 1000 unidades de distancia. Sus coordenadas: p1=(-500√3, -500, 0)T, p2=(0, 1000, 0)T y p3=(500√3, -500, 0)T.

El análisis se realiza sobre un plano paralelo al de base, y a una distancia de él de 8000 unidades de distancia. Como se puede observar el error es máximo cuanto más lejos se está del las balizas, fuera del perímetro cubierto por ellas.

El segundo de los casos en la figura 4-2, se dispone de tres balizas en posiciones que están bastante alineadas alrededor del eje X (sin llegar a estarlo, pues si no la localización sería imposible con la expresión presentada) en el plano XY. Sus coordenadas: p1=(-500, -5, 0)T, p2=(0, 5, 0)T y p3=(500, -5, 0)T.

Figura 4-1.- Varianza del Error relativo a la del error de posición de las balizas, en un plano paralelo al base, a 8000 unidades de distancia del plano base.

El análisis se realiza sobre el propio plano base. Como se puede observar el error es máximo cuanto más lejos se está del eje alrededor del cual están las balizas.


- 19 -

Figura 4-2.- Varianza del Error relativo a la del error de posición de las balizas, en el plano al base.

4.2 Influencia del error de las medidas de distancia a las balizas, en la posición del robot.

Al igual que ocurre con la inexactitud del conocimiento que se tiene de la posición de las balizas, con mayor importancia se tiene inexactitud en la medida de las distancias a las balizas.

Independientemente del procedimiento utilizado para medir dichas distancias, se introducirá un ruido que para su análisis se va a suponer que es ruido blanco con una función de distribución de probabilidad gausiana de media cero.

Se representa el ruido aditivo gausiano de media cero las distancias medidas del robot a las balizas como, δl = (δl1 , δl2, δl3)

T , y la medida del valor real de distancias como, l0=(l01, l

02, l

03)

T, entonces las distancias medidas serán las sumas de ambas magnitudes, l = l0 + δl.

Suponiendo que los errores en las medidas de distancia están incorrelados entre ellas, y tienen una varianza de σr

2

{ } IllE rT 2σ=∂∂

Para errores pequeños de distancias, la localización del robot se puede aproximar por el desarrollo de Taylor de la ecuación 3-6 con solo los términos hasta de segundo orden,


- 20 -

jii j ji

ii i

llll

pl

l

pppp ∂∂

∂∂

∂+∂

∂

∂=−=∂ ∑∑∑

= ==

3

1

3

1

423

1

40444 2

1

La esperanza del error es

{ } { }jii j ji

llEll

ppE ∂∂

∂∂

∂=∂ ∑∑

= =

3

1

3

1

42

4 2

1

Y como { } IllE rT 2σ=∂∂ ,

{ }

∂

∂+

∂

∂+

∂

∂=∂

23

42

22

42

21

422

4 2 l

p

l

p

l

ppE rσ

Finalmente sustituyendo el valor de ‘p4’ por la expresión de la ecuación 3-9,

{ } ( )( )2132

222

112

2

4 2vvkvkvkpE r ×∇±∇+∇=∂

σ

Donde,

23

2

22

2

21

22

l

k

l

k

l

kk iii

i∂

∂+

∂

∂+

∂

∂=∇

Y se usa el signo ‘±’ en función del que fue utilizado en el cálculo de la posición en la ecuación 3-9.

Integrando las dos conclusiones principales del estudio realizado en [5]:

1. La proyección del error de posición sobre el plano base puede ser despreciada.

2. El error de posición del robot se va haciendo más relevante cuanto más se aproxima al plano base.

Atendiendo a la conclusión primera,

{ } ( )

×∇±∇+∇=∂

≈≈

2132

0

222

0

112

2

4 2vvkvkvkpE r

321321

σ


- 21 -

{ } ( )2132

2

4 2vvkpE r ×∇±≈∂

σ

Ecuación 4-10.- Expresión del error de posición despreciando su proyección sobre el plano base.

Y atendiendo a la segunda conclusión, cuando el robot esté muy cerca del plano base, la ecuación 4-10 se puede simplificar como,

{ } ( )213

21

23

22

22

23

212

0454

vvV

AlAlAlpE rV

×++

≈∂→

σm

Ecuación 4-11.- Expresión del error de posición estando el robot muy cerca del plano base.

Como consecuencia de este error, cuando el robot se desplaza sobre un plano paralelo al base y muy cercano a él, la estimación de posición informa erróneamente de aumento y disminución de su distancia al plano base, según el robot se aleja o acerca del baricentro del triángulo formado por las tres balizas.

En la figura 4-3 se observa como crece el error al alejarse del baricentro de las balizas. El caso representado dispone de sus balizas en las mismas ubicaciones que el ejemplo de la figura 4-1. La distancia del robot al plano base es de 4000 unidades. El valor máximo en todo el área de estudio (unas ocho veces mayor que el área ocupada por las balizas), es de -0.03σr

2.

Figura 4-3.- Componente ortogonal al plano base del error de posición debida al error de las medidas de distancia, cuando el robot se desplaza en un plano paralelo al plano base, y a una distancia de 4000 unidades.

En la figura 4-4 se observa, al igual que en la anterior, como crece el error al alejarse del baricentro de las balizas, pero con la distancia del robot al plano base de 40 unidades. El valor máximo en todo el área de es de -11842σr

2.


- 22 -

Como se puede comprobar el error crece exponencialmente al alejarse de las balizas.

Figura 4-4.- Componente ortogonal al plano base del error de posición debida al error de las medidas de distancia, cuando el robot se desplaza en un plano paralelo al plano base, y a una distancia de 40 unidades.


- 23 -

5 Resultados de las simulaciones.

Mediante la implementación de las ecuaciones deducidas de los errores producidos en la localización en Matlab, se han calculado y representado dichos errores en distintas situaciones de colocación de las balizas, y estudiado a distintas distancias del plano base. Por simplicidad se utiliza en todo caso el plano XY como plano base.

5.1 Caso I.- Balizas casi alineadas.

A continuación se observa la representación de tres balizas en coordenadas cercanas al alineamiento, indicadas como aspas azules.

Las posiciones de las balizas son:

( )( )( )0,1,0

0,0,1

0,25.0,25.0

3

2

1

=

=

=

p

p

p

En sucesivas imágenes la diferencia radica en la altura ‘z’ del plano a la que se estudia el error de posición relativo ala varianza de las medidas de distancias utilizadas para la trilateración.

Se han simulado las situaciones de z de valor 0.5, 1, 2, 3 y 5.

La barra de color representa el valor del número de veces mayor que es el error de posición comparado con la varianza de las medidas de distancia, y se representan las curvas de igual valor de error, en sucesivos colores.

Error de posición en relación al error de las medidas de distancia, z=0.5

-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

2.1

2.2

2.3

2.4

2.5

2.6

2.7

2.8

2.9

Error de posición en relación al error de las medidas de distancia, z=1

-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1.2

1.4

1.6

1.8

2

2.2

2.4

2.6

2.8


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5


- 24 -

Se puede apreciar como a medida que la experiencia se hace más alejado de la superficie menor es el error, dado que en la primera apenas se aprecian las curvas, dado que solo se tiene los valores de error representados justo en la zona interior y central de las balizas.

En la representación adjunta, donde z=3, se aprecia claramente una elipse que pasando aproximadamente por las balizas viene a indicar que su zona interior es de error menor a la varianza de las medidas.

En la última figura se representa el error para z=5.

5.2 Caso II.- Balizas nada alineadas.

A continuación se observa la representación de tres balizas en coordenadas al máximo alejamiento del alineamiento, indicadas como aspas azules.

Las posiciones de las balizas son:

+=

−+=

−−=

02

75.0,0

02

75.0,5.0

02

75.0,5.0

3

2

1

p

p

p


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5


- 25 -

Al igual que ocurría en el caso anterior, el error es mucho mayor cuanto mas cerca del plano base está el robot.

A diferencia del caso anterior se muestra una cierta simetría de las curvas de error constante, alrededor de la zona cubierta por las balizas.

Error de posición en relación al error de las medidas de distancia, z=0.5

-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

2.1

2.2

2.3

2.4

2.5

2.6

2.7

2.8

2.9


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1.2

1.4

1.6

1.8

2

2.2

2.4

2.6

2.8


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5


-4 -3 -2 -1 0 1 2 3 4-4

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5


- 26 -

6 Funciones MATLAB implementadas.

A continuación se incluye el código de MATLAB implementado para el cálculo de la trilateración esférica.

6.1 Función: [punto1 punto2] = trilateracion(p1,p2,p3,l1,l2,l3)

Con esta función se obtienen los dos posibles puntos fruto de la trilateración recibiendo como parámetros las coordenadas de los tres puntos de referencia, y las tres distancias medidas a los mismos desde el móvil.

Es responsabilidad de la aplicación que recibe los puntos determinar cual de los dos es el correcto, por ejemplo si las referencias están en el plano de ‘z=0’ y el móvil se mueve por encima de la superficie bastará con dar por válido aquel cuya coordenada ‘z’ sea positiva.

function [punto1 punto2] = trilateracion(p1,p2,p3,l1,l2,l3)

% Se elevan al cuadrado las distancias por comodidad

l1 = l1^2;

l2 = l2^2;

l3 = l3^2;

% Se calculan parcialmente los elementos de la expresión final

v1 = p2-p1;

v2 = p3-p1;

A = 1/(Determinante([p1 ; p2; p3]));

% Producto vectorial de v1 con v2

B = cross(v1,v2);

% C = D(p1,p2,p3;p1;p3,p4)

matriz = [0 D(p1,p3) l1; D(p1,p2) D(p2,p3) l2; D(p1,p3) 0 l3];

C = Bideterminante_trilat(matriz);

% D = D(p1,p2,p3;p1;p2,p4)

matriz = [0 D(p1,p2) l1; D(p1,p2) 0 l2; D(p1,p3) D(p2,p3) l3];

DD = Bideterminante_trilat(matriz);

% E = D(p1,p2,p3,p4)

matriz = [0 D(p1,p2) D(p1,p3) l1;

D(p1,p2) 0 D(p2,p3) l2;

D(p1,p3) D(p2,p3) 0 l3;

l1 l2 l3 0 ];

E = Bideterminante_trilat(matriz);

punto1 = p1 + A * (-C * v1 + DD * v2 + sqrt(E) * B );

punto2 = p1 + A * (-C * v1 + DD * v2 - sqrt(E) * B );


- 27 -

6.2 Función: Bideterminante_trilat(matriz)

Con esta función se obtiene el bideterminante de Cayley-Menger, para lo que previamente hay que haber construido la variable matriz de la forma de la figura 6-1.

Figura 6-1.- Composición de la variable matriz para calcular con la función correspondiente el bideterminante de Cayley-Menger.

Únicamente se obtiene como resultado el valor del bideterminante de Cayley-Menger.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calcula el Bideterminante de Cayley-Menger de p y q %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% p y q son matrices compuestas en sus filas de los vectores p1,...pn

% y q1,... qn

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [resultado] = Bideterminante_trilat(matriz)

n = size(matriz,1);

matriz = [[0 ones(1,n)]; [ones(n,1) matriz]];

resultado = 2 * (-0.5)^n * det(matriz);

6.3 Función: D(p,q)

Calcula el parámetro fundamental sobre el que se construyen los determinantes de Cayley-Menger, esto es, el cuadrado de la distancia euclidea entre dos puntos.

Esta es una función utilizada en la función ‘trilateración’.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calcula el cuadrado de la distancia euclidea entre dos puntos en 3D %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 'p' y 'q' son sendos vectores que apuntan dichos puntos desde el origen

% de coordenadas y estan compuestos de un estructura con tres campos x,y

% y z.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [distancia] = D(p,q)

delta = p - q ;

% distancia = sqrt(sum((delta).^2));

distancia = sum((delta).^2);


- 28 -

6.4 Función: Determinante(p)

Calcula el determinante de Cayley-Menger dados los puntos en forma de matriz donde las columnas sean las componentes de los puntos, y las filas indiquen cada uno de los puntos.

Esta es una función utilizada en la función ‘trilateración’.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calcula el Determinante de Cayley-Menger de p %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% p es una matriz compuesta en sus filas de los vectores p1,...pn

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [resultado] = Determinante(p)

% n = Numero de filas, ya que las columnas son las componentes

% 'x','y' y 'z'

n = size(p,1);

for i = 1:n

for j=1:n

matriz(i,j) = D(p(i,:),p(j,:));

end;

end;




- 29 -

6.5 Función: Bideterminante(p,q)

Con esta función se puede calcular el bideterminante de Cayley-Menger dados los puntos en forma de matrices donde las columnas sean las componentes de los puntos, y las filas indiquen cada uno de los puntos.

Esta función se implementó para el algoritmo a partir de las expresiones teóricas, pero pudo utilizarse debido a que en el algoritmo todos los bideterminantes implican al punto objetivo, que es desconocido, y en la función es un parámetro de entrada. Su única utilidad está en dado el punto conocido averiguar el valor de los bideterminantes de la expresión general.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calcula el Bideterminante de Cayley-Menger de p y q %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% p y q son matrices compuestas en sus filas de los vectores p1,...pn

% y q1,... qn

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [resultado] = Bideterminante(p,q)

% n = Numero de filas, ya que las colu

n = size(p,1);

for i = 1:n

for j=1:n

matriz(i,j) = D(p(i,:),q(j,:));

end;

end;



6.6 Función: Areas(p1, p2, p3, p4)

Permite calcular como paso intermedio para el análisis de errores el valor de las áreas descritas en la teoría, y que forman parte de las expresiones de cálculo de errores.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calcula de las áreas de subdivisión del triángulo %

% formado por las balizas, y la de la base %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Como parametro de entrada los puntos de las balizas y %

% del robot. %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [A1, A2, A3, Ab] = Areas(p1, p2, p3, p4)

p = [p1 ; p2; p3];

Ab = sqrt( Determinante(p) ) / 2;

q = [p1 ;p3 ;p4 ];

A2 = -Bideterminante(p,q) / (4 * Ab);

q = [p1 ;p2 ;p4 ];

A3 = Bideterminante(p,q) / (4 * Ab);

A1 = Ab -A2 -A3;


- 30 -

6.7 Función: error_distancia(p1, p2, p3, p4)

Implementa la expresión 4-11 que indica el error medio producido en la localización en relación al error de las medidas de distancia.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculo del error de posición relativo al de las %

% distancias a las balizas %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% del robot. %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [error] = error_distancia(p1, p2, p3, p4)

[A1, A2, A3, Ab] = Areas(p1, p2, p3, p4);

A1_2 = A1^2;

A2_2 = A2^2;

A3_2 = A3^2;

l1_2 = sum((p4-p1).^2);

l2_2 = sum((p4-p2).^2);

l3_2 = sum((p4-p3).^2);

V = sqrt(Determinante([p1;p2;p3;p4])) / 6;

V_3 = V^3;

v1 = p2-p1;

v2 = p3-p1;

modulo = norm( cross(v1,v2) );

error = ( l1_2 * A3_2 + l2_2 * A2_2 + l3_2 * A1_2 ) * modulo / 54 / V_3;

6.8 Función: error_posicion(p1, p2, p3, p4)

Implementa la expresión 4-7 que indica la relación entre varianzas del error producido en la localización a causa del error de las medidas de distancia.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calculo del error de posición relativo al de la posicion%

% de las balizas %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% del robot. %

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [error] = error_posicion(p1, p2, p3, p4)

[A1, A2, A3, Ab] = Areas(p1, p2, p3, p4);

a2 = sum((p2-p1).^2);

b2 = sum((p3-p1).^2);

c2 = sum((p4-p1).^2);

V2 = Determinante([p1;p2;p3;p4]) / 36;

Ab2 = Ab^2;

error = (A1^2+A2^2+A3^2)/ Ab2 + 18 * V^2 * ( a2 + b2 + c2) / Ab2^2;


- 31 -

6.9 Script: Cálculo y representación de los errores.

Se utiliza el mismo código para todas las simulaciones, solo que cambiando las coordenadas de las balizas y la función de error en el script.

clear all;

close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Prueba influencia error de medida de tiempos de

% vuelo en la trilateración.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

min_x = -4;

max_x = 4;

paso_x = 0.25;

min_y = -4;

max_y = 4;

paso_y = 0.25;

% Numero de iteraciones en cada posicion

numero_iteraciones = 10;

% Error de medida de distancia desviacion estandar

error_l = 0.1;

% Base de estaciones de referencia

p1 = [-0.5 -sqrt(0.75)/2 0];

p2 = [0.5 -sqrt(0.75)/2 0];

p3 = [0 sqrt(0.75)/2 0];

rango_x = min_x:paso_x:max_x;

rango_y = min_y:paso_y:max_y;

[X,Y] = meshgrid(rango_x,rango_y);

for z = [0.5 1 2 3 4 5 6 7]

z

max_z = 0;

min_z = 0;

for j = 1:size(rango_x,2)

for k = 1:size(rango_y,2)

x = rango_x(j);

y = rango_y(k);

p4 = [x y z];

error_medio(j,k) = error_posicion(p1, p2, p3, p4);

if (error_medio(j,k) > max_z)

max_z = error_medio(j,k);

end;

if (error_medio(j,k) < min_z)

min_z = error_medio(j,k);

end;

end;

j

end;

%surf(rango_x,rango_y,error_medio);

%meshc(X,Y,error_medio);

%axis([min_x max_x min_y max_y min_z max_z]);


- 32 -

contour(X,Y,error_medio,[0.5 1 1.5 2 2.5 3]);

colorbar;

title(strcat('Error de posición en relación al error de la ubicación de las balizas,

z=',num2str(z)))

hold on;

plot(p1(1),p1(2),'+');

plot(p2(1),p2(2),'+');

plot(p3(1),p3(2),'+');

hold off;

saveas(gcf, strcat('simula_error_posicion', num2str(z),'.fig'), 'fig');

nombre = strcat('simula_error_posicion_datos_', num2str(z),'.mat')

save( nombre , 'error_medio');

end;


- 33 -

7 Referencias.

[1] F. Thomas, E. Ottaviano, L. Ros y M. Ceccarelli, “Revisiting Trilateration for Robot Localization”, IEEE Trans. on Robotics and Automation, febrero-2005.

[2] F. Thomas, E. Ottaviano, L. Ros y M. Ceccarelli, “Coordinate-free formulation of a 3-2-1 Wire-based Tracking Device using Cayley-Menger Determinant”, IEEE Trans. on Robotics and Automation, Taipei, Taiwán, 2003.

[3] M. Mazo, “Posicionamiento 3D de Robots móviles utilizando Balizas Activas (“Satélites”)”, Curso de Doctorado de Captación y modelado del entrono mediante medidores de distancia, Departamento de Electrónica de la Universidad de Alcalá, 2006.

[4] I.D. Coope, “Reliable computation of the points of intersection of n spheres in Rn", Australian and New Zealand Industrial and Applied Mathematics Journal, Part C, Vol. 42, pp. 461-477, 2000.

[5] D. E. Manolakis, “Eficient solution and performance análisis of 3-D position estimation by trilateration", IEEE Trans. On Aerospace and Electronic Systems, Vol. 32, No. 4, pp. 1239-1248, 1996.

memoria descriptiva del trabajo de investigación (33 páginas

Documents