utilizaciÓn de los determinantes de cayley-menger en...

UTILIZACIÓN DE LOS

DETERMINANTES DE

CAYLEY-MENGER

EN LA LOCALIZACIÓN POR

TRILATERACIÓN ESFÉRICA

Dirigido por:

D. Manuel Mazo Quintas

Realizado por:

D. Ramón Rodríguez Luque

Junio - 2006

Trilateración Esférica

� Introducción.

� Bideterminantes y determinantes de Cayley-Menger.

� Propiedades geométricas de los determinantes.

� Propiedades geométricas de los bideterminantes.

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

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

� Influencia del error de la localización de las balizas.

� Influencia del error de las medidas de distancia a las balizas.

� Resultados de las simulaciones.

Introducción I

Introducción II

( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

−+−+−=

−+−+−=

−+−+−=

2

41

2

41

2

41

2

4

2

31

2

31

2

31

2

3

2

21

2

21

2

21

2

2

zzyyxxl

zzyyxxl

zzyyxxl

Resolución del sistema de ecuaciones no lineal

Introducción III

Sensores a utilizar para la medida de distancias:

•Ultrasonidos

•Radiofrecuencia

•Infrarrojos

Técnicas de medida:

•Tiempo de vuelo

•Desfase

Medidas sometidas a errores

-> Influencia en la estimación de localización.

Bideterminantes y determinantes de Cayley-Menger.

Definición Bideterminante:

Determinante:

Propiedades geométricas de los determinantes I

Para n=2, El cuadrado de la Distancia Euclideaentre los puntos ‘p1’ y ‘p2 ‘

( ) 2

21

2

21

2

12

2

21

2

122

12

2

21

0

2

22

2

12

2

21

0

2

11

2

21

),(),(),(2

1

),(0

11

0),(

11

2

1

0),(1

),(01

110

2

1

),(),(1

),(),(1

110

2

12),(

ppdppdppd

ppdppdppd

ppd

ppdppd

ppdppdppD

=+⋅

=

+−⋅=⋅

=⋅

−⋅=

43421

43421

Propiedades geométricas de los determinantes II

Para n = 3,

El cuadrado del doble del área del triangulo encerrado por ellos

Propiedades geométricasde los determinantes III

Para n = 4, El cuadrado de seis veces el volumen del

tetraedro definido por los puntos.

Propiedades geométricas de los bideterminantes I

Para n = 2, Se relaciona

directamente con el producto escalar entre dos vectores.

Propiedades geométricas de los bideterminantes II

Para n = 2, Si: p1 = q1

Propiedades

geométricas de los bideterminantes III

Para n = 3

Propiedades

geométricas de los bideterminantes IV

Para n = 3, con p1=q1 y p3=q2

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

¿p4 = (x y z)?

Conocido:

� Distancias

l1, l2 y l3

� Balizas

p1, p2 y p3

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

A1, A2 y A3, serán las “áreas con signo” de los triángulos: p2p3p, p3p1p y p1p2p

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

)·cos(),,(

),,();·cos(

),,(

),,(

2

),,(

)·cos(2

),,(

2

),,(

)·cos(

),,()·(4

)·cos(

),,()·(4

34

321

421324

321

4312

321

24431

321

241342

321

2

241342

431

2

134

φφ

φφ

φ

pppD

pppD

A

A

pppD

pppD

A

A

pppD

pppD

pppD

A

A

A

pppDA

AA

pppDA

bb

p

b

b

p

p

==

===

=

=

=

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

)·cos(),,(

),,();·cos(

),,(

),,(34

321

421324

321

4312 φφpppD

pppD

A

A

pppD

pppD

A

A

bb

==

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

>0, cuatro veces el cuadrado del área

Cuando la proyección de p4 (p) está en el triángulo base:

<0

>0

Expresión del cálculode la trilateraciónesférica VI

21

3vv

hk

×=

Expresión del cálculode la trilateraciónesférica VII

Expresión del cálculode la trilateraciónesférica VIII

Indeterminación

Las tres balizas están alineadas

=0

Sin ambigüedad

( ) 0,, 321 =pppD

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

Influencia del error de la localización de las balizas I

incorreladas

entre sí

≠=

==

jisipEpE

jisiIppE

T

ji

pT

ji,0}{}·{

,}{

2

δδ

σδδ

ruido gaussiano

Influencia del error de la localización de las balizas II

( ) ( ) ( )[ ]211

0

22

0

1322111

0

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

∂−∂=∂

∂−∂=∂

132

121

ppv

ppv

A partir de la ecuación general, el error de localización:

Influencia del error de la localización de las balizas III

La media del error de localización:

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

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

0

1

0

2

0

2

0

13

0

22

0

11

0

14p vvEvvEvvEkvEkvEkpEE4342143421321321321

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

{ } 0p 4 =∂E

Asumiendo que de media se conoce la posición cierta, la

posición media obtenida no se ve afectada por dicho error.

Influencia del error de la localización de las balizas IV

La matriz de covarianza del error de estimación de la posición:

{ }[ ] { }[ ]{ } { } { } { } { }TTTTppEpEpEppEpEppEpECs 44

0

4

0

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

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

0

1

0

3

0

3

1

0

31

0

3

2

3

2

0

22

0

2

2

3

3

0

13

0

1

2

3

33

2

222

2

1

11

2

21

:

1

ppvdonde

pvpvEk

pvpvEk

pvpvEk

ppEkppEk

ppEkkCs

T

T

T

TT

T

−=

∂×∂×+

∂×∂×+

∂×∂×+

∂∂+∂∂+

∂∂−−=Desarrollando y eliminando los términos que

involucran productos de variables

incorreladas, se tiene:

Influencia del error de la localización de las balizas V

La matriz de covarianza del error de estimación de la posición:

La matriz de covarianza resulta una matriz isotrópica:

( )

( )

−=

−=

−=

Ι

+++

++

−−

=

14

13

12

2222

3

2

2

2

1

2

21

2 :·

2

1

ppc

ppb

ppa

donde

cbak

kk

kk

Cs pσ

( )

( )

+++

++

−−

=

2222

3

2

2

2

1

2

21

22

2

1

cbak

kk

kk

ps σσ

Influencia del error de la localización de las balizas VI

En forma de términos de longitudes, áreas y volúmenes:

( ) ( )222

4

22

3

2

2

2

12

2

181cba

A

VAAA

A bbp

s +++++=

σ

σ

Si el robot está muyalejado del plano base

Si el robot está muycerca del plano base

( )222

4

22

18cba

A

V

bV

p

s ++≈

∞→

σ

σ( )2

3

2

2

2

12

0

2

1AAA

AbV

p

s ++≈

→

σ

σ

Influencia del error de las medidas

de distancia a las balizas I

ji

i j ji

i

i i

llll

pl

l

pppp ∂∂

∂∂

∂+∂

∂

∂=−=∂ ∑∑∑

= ==

3

1

3

1

4

23

1

40

4442

1

Para errores pequeños de distancias, la localización del robot se puede aproximar

por el desarrollo de Taylor, con solo los términos hasta de segundo orden,

Influencia del error de las medidas

de distancia a las balizas II

{ } { }ji

i j ji

llEll

ppE ∂∂

∂∂

∂=∂ ∑∑

= =

3

1

3

1

4

2

42

1 { } IllE r

T 2σ=∂∂

{ }

∂

∂+

∂

∂+

∂

∂=∂

2

3

4

2

2

2

4

2

2

1

4

22

42 l

p

l

p

l

ppE rσ

Influencia del error de las medidas

de distancia a las balizas III

Sustituyendo p4 por el valor de la expresión general:

{ } ( )( )213

2

22

2

11

22

42

vvkvkvkpE r ×∇±∇+∇=∂σ

2

3

2

2

2

2

2

1

22

l

k

l

k

l

kk iiii

∂

∂+

∂

∂+

∂

∂=∇Donde:

El signo correspondiente al utilizado en la localización

Influencia del error de las medidas

de distancia a las balizas IV

Teniendo en cuenta:•La proyección del error de posición sobre el plano base

puede ser despreciada.•El error de posición del robot se va haciendo más relevante

cuanto más se aproxima al plano base.

{ } ( )

×∇±∇+∇=∂

≈≈

213

2

0

22

2

0

11

22

42

vvkvkvkpE r

321321

σ

{ } ( )213

22

42

vvkpE r ×∇±≈∂σ

Despreciando la proyección sobre el plano base

Influencia del error de las medidas

de distancia a las balizas V

Cuando el robot esté muy cerca del plano base, se puede simplificar como:

{ } ( )213

22

42

vvkpE r ×∇±≈∂σ

{ } ( )213

2

1

2

3

2

2

2

2

2

3

2

12

0454

vvV

AlAlAlpE rV

×++

≈∂→

σm

Conclusión:El error de posición debido a las medidas de distancia no es

de media cero, aunque lo sea el de las medidas de distancia. La componente principal de error es normal al

plano base y su magnitud es más importante cuanto más

cerca del plano base está el robot.

Resultados de las simulaciones I

Varianza del Error relativo al del error de posición de las balizas, en un plano paralelo al base, a 8000 unidades de distancia del plano base

p1=(-500√3, -500, 0)

p2=(0, 1000, 0)

p3=(500√3, -500, 0)

Resultados de las simulaciones II

Varianza del Error relativo a la del error de posición de las balizas, en el plano base. Error máximo cuanto más lejos se está del eje alrededor del

cual están las balizas.

Balizas casi alineadas

Resultados de las simulaciones III

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.

Resultados de las simulaciones IV

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.

Resultados de las simulaciones V

CASO I.- BALIZAS CASI ALINEADASError 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

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

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

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5

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

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

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5

Error de posición en relación al error de las medidas de distancia, z=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

Resultados de las simulaciones VI

CASO II.- BALIZAS NADA ALINEADASError 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

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

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

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5

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

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

-3

-2

-1

0

1

2

3

4

1

1.5

2

2.5

Error de posición en relación al error de las medidas de distancia, z=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