utilizaciÓn de los determinantes de cayley-menger en...
TRANSCRIPT
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