Control de Sistemas Multiagente

Parte I. Modelado y control de vehı́culos aéreos

Dr. Hugo Rodrı́guez Cortés

Sección de Mecatrónica, Departamento de Ingenierı́a Eléctrica
CINVESTAV-IPN

Junio 2023
MCVA 1 / 64
 Contenido

a Sensores disponibles.

b Estimación de la orientación.

c Estimación de la posición traslacional.

MCVA 2 / 64
 Sensar y estimar

■ Controlar el movimiento de un robot móvil serı́a una tarea muy

sencilla si se tuviera un modelo preciso del espacio de trabajo y si los
actuadores ejecutaran los comandos perfectamente.

■ Los procesos de sensar y estimar son vitales para compensar la falta

de conocimiento del espacio de trabajo y las imperfecciones de los
actuadores.

■ Sensar y estimar tienen el rol de proporcionar información sobre el

espacio de trabajo y el estado de robot para la toma de decisiones.

■ Sensar y estimar pueden interpretarse como la transformación de una

cantidad fı́sica en una representación numerica interpetable por una
computadora.
MCVA 3 / 64
 ■ La información necesaria para sensar y estimar viene de los datos
extraı́dos por los sensores y del conocimiento parcial de modelos
cinemáticos o dinámicos.

■ Un sensor es un dispositivo cuyo propósito es detectar eventos o cam-

bios en su espacio de trabajo y enviar la información a otro dispositivo.

■ La estimación es un algoritmo que permite fusionar la información de

uno o varios sensores con el conocimiento fı́sico de un sistema para
extraer información relevante.

■ La estimación está estrechamente relacionada con el diseño de

observadores de estado. Un paso importante es identifica si cierta in-
formación es observable desde las mediciones de un grupo de sensores.

MCVA 4 / 64
 Estimación de la orientación

■ {i} Marco de Referencia Inercial.

■ {b} Marco de Referencia Cuerpo.

Figure 1: Sistemas que requieren estimar la ori-
entación.

MCVA 5 / 64
 Representaciones de la orientación

Representaciones de orientación Global Única

a) Ángulos de Euler No No
b) Parámetros de Rodrigues No No
c) Parámetros de Rodrigues Modificados No No
d) Cuaternos Si No
e) Eje - Ángulo Si No
f) Matriz de Cosenos Directores (MCD) Si Si

Table 1: Representaciones de la orientación.

Ángulos de Euler:
φ, θ, ψ (1)
Cuaternos:

q = [ η, ǫ1 , ǫ2 , ǫ3 ] (2)

MCVA 6 / 64
 Representaciones de la orientación

Matriz de rotación
 
cos(θxb ,xi ) cos(θxb ,yi ) cos(θxb ,zi )
R =  cos(θyb ,xi ) cos(θyb ,yi ) cos(θyb ,zi )  (3)
cos(θzb ,xi ) cos(θzb ,yi ) cos(θzb ,zi )

Parametrizada con los ángulos de Tait-Bryan en la secuencia ZY X

 
c ψ c θ c ψ sθ sφ − s ψ c φ c ψ sθ sφ + sψ sφ
R =  sψ c θ sψ sθ sφ + c ψ c φ sψ sθ sφ − c ψ sφ  (4)
−sθ c θ sφ cθ cφ

MCVA 7 / 64
 Cinemática rotacional

Ángulos de Euler:
    
φ̇ c θ sθ sφ sθ c φ p
 θ̇  = 1  0 cθ cφ −cθ sφ   q  (5)
cθ
ψ̇ 0 sφ cφ r

Cuaternos:
    
η̇ 0 −p −q −r η
 ǫ˙1  1  p 0 r −q   
 =    ǫ1  (6)
 ǫ˙2  2  q −r 0 p   ǫ2 
ǫ˙3 r q −p 0 ǫ3

MCVA 8 / 64
 Cinemática rotacional

Matriz de rotación:
 
0 −r q
Ṙ = R S(Ω) = S(RΩ)R , S(Ω) =  r 0 −p  (7)
−q p 0

MCVA 9 / 64
 Sensores inerciales
■ Los sensores inerciales se basan en la medición de aceleraciones o
velocidades angulares a partir de fuerzas y momentos inerciales.
■ Para una partı́cula de masa m montada sobre un sistema de referencia
no inercial aparece la fuerza inercial siguiente


i b b
ma = ma + ma + 2mΩ × V + mΩ × Ω × r + mΩ̇ × rb b

con
◆ ai la aceleración inercial, a la aceleración entre los sistemas de
referencia,
◆ ab la aceleración de la partı́cula con respecto al sistema no inercial,

◆ Ω la velocidad de rotación del sistema no inercial, V b la velocidad

de la partı́cula con respecto al sistema no inercial,
◆ y rb la posición de la partı́cula con respecto al sistema no inercial.
MCVA 10 / 64
 Mediciones disponibles (acelerómetros)

■ El modelo simplificado de un acelerómetro se muestra en la siguiente

Figura

k
xr
r rb

xb
mp
d

Sobre el dispositivo se tiene

d2 r b

b b b
mp 2 = mp a + 2mp Ω × v + mp Ω × Ω × r + mp Ω̇ × rb
dt
Al asumir que el dispositivo se aisla para no detectar la velocidad
angular,
d2 r b
m p 2 = m p ab
dt

MCVA 11 / 64
 Mediciones disponibles (acelerómetros)

■ Las fuerzas que actúan sobre la masa son

F = −dv b − krb + mp e⊤
b g b

con eb un vector unitario en la dirección del eje xb y gb el vector de

aceleración gravitacional en xb .
■ De tal forma que

mp ab = −dv b − krb + mp e⊤
b g b

Al definir la fuerza especifica como f = − mkp rb se obtiene

b ˙ b ⊤ b


f = −f + a − eb g
k
k
con b
la constante de tiempo del acelerómetro.

MCVA 12 / 64
 Mediciones disponibles (acelerómetros)

k
■ Si ≈0
b
f = ab − e ⊤
b g b

es la salida del acelerómetro.

■ Acelerómetro capacitivo

masa

sensores
◆ Dimensiones pequeñas y fácil
electrodo montaje.
◆ Baja precisión.
substrato

MCVA 13 / 64
 ■ Acelerómetro piezoresistivo

resistores piezoeléctricos
vigas felxibles

◆ Amplio rango de medición.

masa

◆ Baja sensibilidad, requieren

compensación de temper-
atura.

MCVA 14 / 64
 ■ Acelerómetro piezoeléctrico
carcasa

señal (+)
◆ Amplio rango de respuesta,
alta sensitividad, respuesta
masa rápida.
tierra (-) ◆ Alta impedancia, difı́ciles de
producir.

material piezoeléctrico

MCVA 15 / 64
 ■ Acelerómetro basado en efectos piezoeléctricos.

Acelerómetro LSM303DLHC

■ En tres dimensiones, en un vehı́culo de masa m, se tiene

b 1 b ⊤ i


f = ma − R Fg + ba + µa (8)
m
alternativamente,

b 1 b ⊤ i


f = F T − R F g + b a + µa (9)
m
con ba una desviación en la medición y µa ruido de medición.
MCVA 16 / 64
 ■ En los desarrollos siguientes se considera ba = µa = 0.

■ Si la suma de las fuerzas externas FTb es cero, el acelerométro medira

1 ⊤ i
b
f = − R Fg (10)
m
i
 ⊤
Se tiene que Fg = 0 0 mg , por lo tanto
   
fxb gsθ
 fyb  =  −gcθ sφ  (11)
fzb −gcθ cφ

de donde !  b
fyb fx
φ̂ = arctan , θ̂ = arcsin
fzb g
MCVA 17 / 64
 ■ Si el acelerómetro se monta en el centro de gravedad de un vehı́culo
que se encuentra sobre un plano inclinado,

mg

1f
2 f 1f
2 n

por lo tanto,
 
ff
FTb = R⊤ Fgi + FR , FR =  0 
−fn

con ff la fuerza de fricción y fn la fuerza normal.

MCVA 18 / 64
 ■ En este caso la medición del acelerométro es
 b   
fx ff
1
f b =  fyb  =  0 
m
fzb −fn

■ Geométricamente fn
ff

mg

de tal forma que ff = mgsθ y fn = mgcθ . Por lo tanto,

 
gsθ  b
b   fx
f = 0 → θ̂ = − arctan b
fz
−gcθ
MCVA 19 / 64
 ■ De forma alternativa, de la ecuación (9) se tiene
   b 
ff fx
fb =  0  =  0 
−fn fzb

■ Al utilizar la ecuación (8) considerando que el vehı́culo está estático

se obtiene
   b 
mgsθ fx  b
⊤ i     fx
−R Fg = FR → 0 = 0 → θ̂ = − arctan b
fz
−mgcθ fzb

MCVA 20 / 64
 ■ Cuando el vehı́culo está volando, se tiene
Fpb

Fab
θ

Fgi = mge3

■ De la ecuación (9), la fuerza especı́fica es

b 1 b b


f = Fp + Fa
m
ya que en este caso FTb = Fpb + Fab + R⊤ Fgi .

MCVA 21 / 64
 ■ A partir de las ecuaciones dinámicas traslacionales de un cuerpo rı́gido
en ejes cuerpo se obtiene

V̇ b + Ω × V b − R⊤ gi = f b
i
 ⊤
con g = 0 0 g . De forma equivalente

b b 1 b
⊤ i b


V̇ + Ω × V − R g = Fp + Fa
m

MCVA 22 / 64
 Mediciones disponibles (Giróscopo)
■ El término giróscopo se deriva del Griego y se refiere al movimiento
de precesión.

■ Los giróscopos son dispositivos que se montados en una estructura

con la capacidad de sensar su velocidad angular.
Z

Y El disco gira alrededor del eje X a

una velocidad p̄. Además,

X Ixx >> Iyy , Izz

las ecuaciones de movimiento son

Iyy q̇ + Ixx p̄r = τy
Izz ṙ − Ixx p̄q = τz

MCVA 23 / 64
 Mediciones disponibles (Giróscopo)

■ Las inercias Iyy e Izz son muy pequeñas, por lo tanto

Ixx p̄r = τy
−Ixx p̄q = τz

Notar que si se aplica un momento τ̄y , aparece una rotación alrededor

del eje Z, esto es,
Ixx p̄r̄ = τy

■ De forma analoga, si la estructura gira con una velocidad r̄ aparece

un momento alrededor del eje Y .

■ Las cantidades Ixx , p̄ son conocidas, por lo que al medir a τy se puede

conocer el valor de r̄.

MCVA 24 / 64
 ■ Los avances en la tecnologı́a ha permitido su uso extensivo al diseñarse
como Sistemas Micro Electro-Mecánicos (MEMS)

Giróscopo L3GD20

■ Los giróscopos MEMS utilizan un elemento mecánico vibratorio que

detecta la velocidad angular. No tienen elementos rotatorios para
evitar el uso de cojinetes.

■ Los giróscopos MEMS con elementos vibratorios se basan en la trasfer-

encia de energı́a entre dos modos de movimiento causados por la acel-
eración de Coriolis.
ac = 2Ω × v b
MCVA 25 / 64
 ■ El modelo simplificado de un giróscopo se muestra en la siguiente
Figura
Z
FZ

ky
vy
mp

dy

kz dz

p
X

El giróscopo detecta el movimiento rotacional alrededor del eje X al

transferir la acción de la velocidad vy sobre el eje impulsor Y , al eje
sensor Z.
MCVA 26 / 64
 ■ Se tiene
mp ÿ = −ky y − dy ẏ + fi
mp z̈ = −kz z − dz ż + 2mp vy p
con fi la fuerza impulsora. El sistema masa-resorte-amortiguador so-
bre el eje Y tiene muy poco amortiguamiento para poder detectar la
velocidad, por lo tanto

mp ÿ = −ky y + fi

de donde
y(s) 1 1
= 2 2
⇒ y(t) = sin(ωy t)fi (t)
fi (s) s + ωy ωy
p
con ωy = ky /mp . La efectividad en la detección de la velocidad se
cuantifica con el factor de calidad mecánica Qm .
MCVA 27 / 64
 ■ Por lo tanto
vy = Qm cos(ωy t)fi (t)

■ En el sistema masa-resorte-amortiguador sobre el eje Z se tiene

mp z̈ = −kz z − dz ż + 2mp Qm cos(ωy t)fi p

p
■ Al definir f = −ωz2 z
con ωz = kz /dz se tiene
 
dz ˙ dz
f = −f + az − f + 2Qm cos(ωy t)fi p
kz kz

La calidad del giróscopo depende de la detección de vy y del ais-

lamiento con respecto a az .

MCVA 28 / 64
 Mediciones disponibles (Giróscopo)

■ En resumen, un arreglo tridimensional de giróscopos tiene la capacidad

de sensar la velocidad angular
 ⊤
p
Ωb =  q  (12)
r
Ωb = Ω + b Ω + µΩ (13)

con bΩ y µΩ desviaciones y ruidos de medición, respectivamente.

MCVA 29 / 64
 Mediciones disponibles (Magnetómetro)

■ Es un dispositivo capaz de medir la magnitud del campo magnético

de la Tierra en la posición en la que se encuentra.

■ Existen magnetómetros basados en tecnologı́a de Sistemas Micro

Electro-Mecanicos

Magnetómetro BM1422AGMV-ZE2

■ Los magnetómetros miden

mb = [mx , my , mz ]⊤ (14)
m b = R ⊤ m i + b m + µm (15)

con bm y µm desviaciones y ruidos de medición, respectivamente.

MCVA 30 / 64
 Mediciones disponibles

■ Una central de medición inercial (IMU) es un conjunto de sensores

integrado por giróscopo, acelerómetro y magnetómetro.

■ Es común encontrar que los sensores que integran a la IMU son

tridimensionales.

■ IMU de tecnologı́a MEMS

Figure 2: IMU MiniIMU9v2

MCVA 31 / 64
 Central de medición inercial (IMU)
3
GiroscopioX
GiroscopioY
2 GiroscopioZ 0.37 1400
Giroscopio ( °/s )

1 1200
0.365

Magnetómetro eje X (gauss)

Densidad de Probabilidad
0 1000
0.36

800
−1
0.355
600
−2
0.35
400
−3
0 10 20 30 40 50 60 0.345
Tiempo ( S ) 200

0.34 0
0 2000 4000 6000 8000 10000 0.34 0.35 0.36 0.37
Muestras Magnetómetro eje X (gauss)

Figure 3: Mediciones del giróscopo Figure 5: Medición del magnetómetro

2.5 4000 2.5 1500

3500 2
2
Acelerómetro eje X (m/s2)

Densidad de Probabilidad

1.5

Densidad de Probabilidad
3000

Giroscopio eje X (°/s)

1.5
1
1000
2500
1 0.5
2000 0
0.5
1500 −0.5
500
0 −1
1000
−1.5
−0.5 500
−2
−1 0 −2.5 0
0 2000 4000 6000 8000 10000 −1 0 1 2 3 0 2000 4000 6000 8000 10000 −4 −2 0 2 4
Muestras Acelerómetro eje X (m/s2) Muestras Giroscopio eje Y (°/s)

Figure 6: Medición del giroscopo

Figure 4: Medición del acelerométro
MCVA 32 / 64
 Métodos de Estimación

MCVA 33 / 64
 Determinı́sticos Algebraicos

Algoritmo 1 Método TRIAD

Entrada: ab = [ax ; ay ; az ]T , mb = [mx ; my ; mz ]T
ai = [0; 0; 9.81]T , mi = [0.377; −0.499; −1.124]T
Salida: Rib Matriz de orientación.

1: for i = 1, 2, ..., N do
ab mb ai mi
2: ab = , mb = , ai = , mi =
|ab | |mb | |ai | |mi |
3: t1b = ab , t1i = ai
a b × mb a i × mi
4: t2b = , t2i =
|ab × mb | |ai × mi |
5: t3b = t1b × t2b , t3i = t1i × t2i
6: Rib (i) = [t1b , t2b , t3b ][t1i , t2i , t3i ]T
7: end for

MCVA 34 / 64
 Algoritmo 2 Método QUEST
Entrada: ab = [ax ; ay ; az ]T ,mb = [mx ; my ; mz ]T
ai = [0; 0; 9.81]T , mi = [0.377; −0.499; −1.124]T
Salida: q = [η, ǫT ]T = [η, ǫ1 , ǫ2 , ǫ3 ]T Cuaterno de orientación.
1: for k = 1, 2, ..., N do
mi mb ai mi
2: v1b = , v2b = v1i = , v2i =
|mi | |mb | |ai | |mi |
N
X
3: Definimos: ρk = [ρ1 , ρ2 ]T , λopt ≈ ρk = ρ1 + ρ2
k=1
T T
4: B = ρ1 (v1b v1i ) + ρ2 (v2b v2i ), S = B + BT
5: Z = [B23 − B32 , B31 − B13 , B12 − B21 ]T , σ = tr[B]
6: p = [(λopt + σ)I3 − S]−1 Z
 
1
7: q(k) = √ 1 T = [η, ǫ1 , ǫ2 , ǫ3 ]T
1+p p p
8: end for

MCVA 35 / 64
 Estocásticos
3.- Método de Fusión Sensorial
φg k+1 = φg k + ∆t pk
θg k+1 = θg k + ∆t qk (16)

Modelo del sistema

φg k+1 = φg k + ∆tpk ; −ζ1k
θg k+1 = θg k + ∆tqk − ζ2k (17)

Modelo de medición Figure 7: Inclinación eje X - Alabeo.

 
ax
φa k = arctan  q  + ξ1 k
 
ay 2 + az 2
!
ay
θa k = arctan p + ξ2 k (18)
ax 2 + az 2

Modelo para el Filtro de Kalman

xk = A xk−1 + B uk + Hk−1 ζk−1 (19) Figure 8: Inclinación eje Y - Cabeceo.
yk = C xk + ξk−1 (20)

MCVA 36 / 64
 Estocásticos
 φg k+1
  1 −∆t 0 0
 φg k
  ∆t pk

ζ1 k+1 0 1 0 0 ζ1 k 0
xk = θg k+1 = 0 0 1 −∆t θg k + ∆t qk (21)
ζ2 k+1 0 0 0 1 ζ2 k 0

 1 0 0 0
 φa k
  ξ1 k

0 0 0 0 ζ1 k 0
yk+1 = 0 0 1 0 θa k + ξ2 k (22)
0 0 0 0 ζ2 k 0

Algoritmo 3 Fusión Sensorial usando el Filtro de Kalman

Entrada: ab = [ax , ay , az ]T , Ωb = [p, q, r]T , P0,0 = var(x0 ) , x̂0 = E(x0 )
1: for k = 1, 2, ... do
2: Q = I4 var(ζ), R = I4 var(ξ)
3: Pk|k−1 = A Pk−1,k−1 AT + Hk−1 Qk−1 Hk−1
T

4: x̂k|k−1 = A x̂k−1|k−1 + Bk−1 uk−1

5: Gk = Pk|k−1 C T (C Pk|k−1 C T + Rk )−1
6: Pk|k = [I − Gk C] Pk,k−1
7: x̂k|k = x̂k|k−1 + Gk (yk − C x̂k|k−1 )
8: end for

MCVA 37 / 64
 Estocásticos

4.- Filtro de Kalman

Figure 9: Diagrama del método del filtro de Kalman.

xk = A xk−1 + B uk + Hk−1 ζk−1 (23)

yk = C xk + ξk−1 (24)

2
   
ηk+1 −pk −qk −rk ηk

h ξηk
 
h
 ǫ1k+1  h  pk 2 rk −qk  ǫ
  1k   h ξǫ 

=
  h + 1k  (25)
 ǫ2k+1  qk

−rk 2 pk
 ǫ  h ξǫ
  2 h  2k 
2k

ǫ3k+1 rk qk −pk 2 ǫ3k h ξǫ3k
h
h iT h iT
y = [ I 4 ] η k ǫ1k ǫ2k ǫ3k + ξη ξǫ1 ξǫ2 ξǫ3 (26)
k k k k

MCVA 38 / 64
 Observadores no lineales
5.- Filtros Complementarios

a m S(·) : R3 → so(3)
va = , vm =
|a| |m| !
0 −Ω3 Ω2
Calculo algebraico instantáneo de R: S(Ω) = Ω3 0 −Ω1
∗
−Ω2 Ω1 0
Ry = min(λ1 |e3 − Rva |2 + λ2 |vm − Rvm |2 )

Criterio de error:
(·)∧ : so(3) → R3
R̃ = R̂⊤ R S(Ω)∧ = Ω, Ω ∈ R3
Objetivo

R̃(t) → I3

Modelo cinématico:

Ṙ = R S(Ω) = S(R Ω) R

MCVA 39 / 64
 Observadores no lineales

FC. Directo
ḃ = S(Ry Ωy + kp R̂ρ)R̂
R
Figure 10: Filtro complementario di-
ρ = Pa (R̃)∧ , R̃ = R̂⊤ Ry recto.
R̂(0) = R̂0 (27)

FC. Pasivo
˙
R̂ = S(R̂Ωy + kp R̂ρ)R̂
ρ = Pa (R̃)∧ , R̃ = R̂⊤ Ry
R̂(0) = R̂0 (28)

Figure 11: Filtro complementario pa-

sivo.
MCVA 40 / 64
 Observadores no lineales

6. Filtro de Inmersión e Invarianza

ṙ3 = S(Ωb )r3 + ξ

ν̇ = −r3 + υ̇ + µa
y = ν (29)

∂β1
    
ξ̂+β2 ζ̂a +β3
r̂˙ 3 = S(Ωb )(r̂3 − β1 ) + ξ̄ tanh + −r̂3 + β1 + ζ̄a tanh
ξ̄ ∂ν ζ̄a

∂β2
  
˙ ζ̂a +β3
ξ̂ = − −r̂3 + β1 + ζ̄a tanh
∂ν ζ̄a

∂β3
  
˙ ζ̂a +β3
ζ̂a = − −r̂3 + β1 + ζ̄a tanh (30)
∂ν ζ̄a

∂βi
β1 = γ1 ν = diag{γi } , γi > 0
∂ν
β2 = γ2 ν T
ν̇ = [ax , ay , az ]
β3 = γ3 ν
ξ̄, µ̄ ∴ cte.

MCVA 41 / 64
 Estimación de la posición

■ Al igual que para la orientación no existe un sensor que directamente

detecte la posición de un vehı́culo móvil.

■ La posición de un vehı́culo móvil se obtiene fusionando mediciones de

varios sensores mediante un algoritmo de estimación.

■ En vehı́culos terrestres se utiliza odometrı́a, una contracción de las

palabras griegas hodos, viaje, y metron medir. La odometrı́a se ha
estudiando por más de 2000 años.

■ En robots terrestres se utiliza la información sobre el movimiento de

las ruedas para estimar la posición del vehı́culo.

■ Al proceso de estimación se le conoce como dead reckoning.

MCVA 42 / 64
 ■ En el robot móvil (2, 0) el proceso dead reckoning toma la forma
siguiente. Cualquier movimiento del robot puede interpretarse como
una rotación alrededor del punto instantaneo de rotación.
■ Si el punto instantaneo de rotación se encuentra sobre la lı́nea que
une a las dos ruedas, a una distancia d, se tiene
l


ur = r d + 2
l


ul = r d − 2

con ur y ul las velocidades de las ruedas derecha e izquierda respec-

tivamente.
■ Resolviendo para r y d se obtiene
ur − ul ur + ul
r= , d=
l ur − ul
MCVA 43 / 64
 ■ La velocidad lineal instantanea es u = rd, por lo tanto, a partir del
modelo se obtiene
Z t Z t
ur (τ ) − ul (τ ) ur (τ ) − ul (τ )
x= cψ(τ ) dτ , y = sψ(τ ) dτ
0 l 0 l
Z t
ur (τ ) − ul (τ )
ψ= dτ
0 l

■ Condiciones iniciales, imperfecciones en las ruedas, patinado son

fuentes de error en la determinación de la posición.

MCVA 44 / 64
 ■ El Sistema de Posicionamiento Global Satélital (GNSS) comúnmente
conocido como Sistema de Posicionamento Global (GPS) es el
mecánismo más usado para estimar la posición.

■ El GPS proporciona una estimación de la posición en tres dimensiones,

en coordenadas absolutas, ası́ como la hora y fecha en cualquier
punto sobre la superficie terrestre.

■ El GPS estándar proporciona un estimado de la posición en el plano

Cartesiano con una posible variación de 20m.

■ Originalmente, el GPS, se desarrolló para aplicaciones militares.

MCVA 45 / 64
 ■ El GPS se basa en la recepción de señales de radio transmitidas por
un conjunto de satélites orbitando la tierra.

■ Al comparar los retrazos de tiempo en la recepción de las señales de

los diferentes satélites se puede obtener la posición.

■ El sistema GPS más aceptado es el que se basa en los satélites de la

constelación NAVSTAR operado por la fuerza aérea de EUA.

■ El gobierno ruso opera la constelación GLONASS para dar servicio

de GPS. La constelación Galileo es la opción propuesta por la Unión
Europea.

MCVA 46 / 64
 ■ Históticamente NAVSTAR proveı́a dos tipos de servicios. El sistema
de posicionamiento preciso (PPS), para usos militares, y el sistema
de posicionamiento estándar (SPS), con menor precisión.

■ La diferencia en precisión se introduce artificialmente por razones

estrategicas.

■ El desempeño del GPS depende de varios factores: precisión de

la transmisión satélital, condiciones ambientales, interaccuón con
obstáculos, y las propiedades del receptor.

■ El sistema de posicionamiento global diferencial (DGPS) es una

técnica para corregir la señal del GPS utilizando un GPS instalado
en una posición conocida.
MCVA 47 / 64
 Figure 12: Sistema de posicionamiento con cámaras infrarrojas.

■ Triangulación de la posición de un centro geométrico definido por

marcadores.
MCVA 48 / 64
 Fusión de sensores visuales e inerciales

■ Una solución prometedora es la fusión de sensores inerciales y visuales.

■ Al asumir que la cámara se encuentra en el centro de gravedad del

vehı́culo se puede considerar que la posición de la cámara es igual a
la posición del vehı́culo.

■ Con sensor visual se detectan caracterı́sticas en la imágen, tales

como, puntos, lı́neas, áreas.

■ Al modelar los cambios que se detectan en las caractarı́sticas entre

dos imágenes consecutivas es posible determinar el cambio en la
orientación y posición de la cámara.

■ También es posible determinar la velocidad traslacional de la cámara

utilizando el flujo óptico.
MCVA 49 / 64
 ■ La posición y orientación de un sensor visual puede representarse con
el grupo especial Euclidiano
   
R T
SE(3) = g = , | R ∈ SO(3), T ∈ R3
0 1

con T el vector de traslación.

■ El espacio tangente en la identidad de SE(3) es

   
Ω̂ V
se(3) = ζ̂ = , | Ω̂ ∈ so(3), V ∈ R3
0 0
con 
so(3) = S ∈ R3×3 | S + S ⊤ = 0

MCVA 50 / 64
 ■ Se requiere saber como cambian la posición y velocidad de un punto
cuando la cámara se mueve.

■ Se escoge a la cámara como el marco de referencia inicial y se de-

scriben sus movimientos posteriores, y de caracterı́sticas geométricas
3D, relativos al marco inicial.

■ Es necesario saber como transformar las posiciones y velocidades de

un marco de referencia en la cámara a otro.

■ Saber como expresar la posición y velocidad de un punto con respecto

a una cámara que se mueve.

MCVA 51 / 64
 ■ Suponga que en t = 0, los sistemas de referencia y de la cámara
coinciden, entonces
g(0) = I ∈ R4×4
Además, si en t = 0, X0 son las coordenadas de un punto con respecto
al marco de referencia global. La posición del punto en t1 con respecto
a la cámara es
X(t1 ) = R(t1 )X0 + T (t1 )
con T (t1 ) la posición de la cámara en t1 . En la representación homo-
genea
X(t1 ) = g(t1 )X0
Si la cámara se encuentra en g(t1 ), g(t2 ), · · · , g(tm ) en t1 , t2 , · · · tm
las coordenadas del punto son

X(ti ) = g(ti )X0

MCVA 52 / 64
 p

z X(t1 ) y
X(t3 )
X(t2 )

t2 z
y
z
x
x y
g(t3 , t2 )
g(t2 , t1 )

t1 x t3

g(t3 , t1 )

Se tiene

X(t3 ) = g(t3 , t2 )X(t2 ) = g(t3 , t2 )g(t2 , t1 )X(t1 )

MCVA 53 / 64
 La matriz esencial
p

y x

z z

z
o o

x
y

(R, T )

A cada cámara se asocia un marco de referencia con origen en el centro

óptico y eje z alineado con el eje óptico.

Se asume que el sistema de referencia inercial está en una de las cámaras,

su ubicación posterior se obtiene de g(R, T ).

MCVA 54 / 64
 p

y x1 x2 x

z z
ℓ1 ℓ2
z
o1 o2
e1 e2

x
y

(R, T )

Sean x1 , x2 ∈ R3 las coordenadas homogeneas de la proyección del punto

p en los planos de imágen, se tiene

λ2 x2 = Rλ1 x1 + T

MCVA 55 / 64
 Considere dos proyecciones homogeneas x1 y x2 del mismo punto p
obtenidas en dos posiciones de una cámara con ubicación relativa (R, T ).
Entonces,
x⊤ bRx1 = 0, X
T b = S(X) ∀X ∈ R3
2

La matriz
E = TbR
se conoce como matriz esencial contiene información sobre la ubicación
relativa de las posiciones de la cámara.

Los vectores o1 o2 , o1 p y o2 p forman un triangulo por lo que se encuentran

en el mismo plano. Su triple producto, el volumen del paralelepı́pedo
formado por los tres vectores, es cero.

MCVA 56 / 64
 Matriz de Homografı́a

■ En varias aplicaciones, la escena de interés puede ser aproximada-

mente planar, por ejemplo, un helipuerto.

■ A cierta altura se puede considerar que una cámara montada debajo

del drone observa una imágen planar.

■ La escena puede ser también plana por pedazos, por ejemplo, los
pasillos dentro de un edificio.

■ En un laboratorio se puede validar la navegación de un drone sobre un

tapete acondicionado.

MCVA 57 / 64
 (R, T )

x1 x2

η p

X1 X2

Las proyecciones homogeneas x1 , x2 de las coordenadas tridimensionales

X1 y X2 del punto p, satisfacen la restricción epipolar

X2 = RX1 + T, x2 ⊤ Ex1 = x2 ⊤ TbRx1 = 0

MCVA 58 / 64
 ■ Sea el vector unitario η ∈ S2 normal al plano P con respecto a la
primera posición de la cámara, y d la distancia de P al centro óptico
en la primera posición de la cámara, se tiene,

η ⊤ X1 = d

■ Por lo tanto,
 
1 ⊤ 1 ⊤
X2 = RX1 + T = RX1 + T η X1 = (R + T η )X1
d d

■ La matriz
1 ⊤
H = (R + T η ) ∈ R3×3
d
se conoce como matriz de Homografı́a.

MCVA 59 / 64
 ■ A partir de x1 = λ1 X1 y x2 = λ2 X2 se obtiene
λ2
x2 = Hx1
λ1

■ b2 se tiene
Multiplicando la ecuación anterior por x
λ2 λ2
0= x b2 Hx1 , ya que b2 Hx1 = 0
6= 0, → x
λ1 λ1

■ Al definir,
s
 ⊤
H = H11 H21 H31 H12 H22 H32 H13 H23 H33

se puede escribir

b2 ∈ R3×9
a⊤ H s = 0, con a = x1 ⊗ x
MCVA 60 / 64
 ■ b2 tiene rango igual a dos, la matriz a tiene el mismo
Ya que la matriz x
rango.
■ Dados n pares de proyecciones homogeneas de puntos sobre el plano
j j
 1 2 
n ⊤
P , (x1 , x2 ), j = 1, · · · , n se define χ = a a · · · a , por lo
tanto
χH s = 0

■ Para tener una solución única se requiere rango(χ) = 8.

Proposición. Se tiene rango(χ) = 8 si y solo si de los n puntos existen
cuatro puntos con tres de ellos no colineales.

MCVA 61 / 64
 ■ Si existen más de cuatro puntos con al menos tres no colineales se
puede utilizar estimación por mı́nimos cuadrados lineal para encontrar

mı́nimokχH 2 k2

■ La solución genera a H modulo un factor de escala. Esto es,

HL = λH

Lema. Para una matriz de la forma HL = λ(R + d1 T η ⊤ ), se tiene

|λ| = σ2 (HL )

con σ2 (HL ) ∈ R el segundo valor caracterı́stico más grande de HL .

MCVA 62 / 64
 Dada una matriz
1 ⊤
H = R + Tη
d
existen como máximo dos soluciones fı́sicamente posibles dadas por

R1 = W1 U1⊤ R2 = W2 U2⊤
η1 = vb2 u1 η2 = vb2 u2
T1 = d(H − R1 )η1 T2 = d(H − R2 )η2
R3 = R1 R4 = R2
η3 = −η1 η4 = −η2
T3 = −T1 T4 = −T2

donde
h i h i
W1 = d2 Hu1 , W2 = Hv2 Hu2 Hv
Hv2 Hu1 Hv d2 Hu2
   
U1 = v2 u1 vb2 u1 , U2 = v2 u2 vb2 u2

MCVA 63 / 64
 con
 
V = v1 v2 v3 ,
√ 2 √ 2 √ 2 √ 2
1−σ3 σ1 −1 1−σ3 σ1 −1
u1 = √ 2
v +
2 1
√ 2
v , u2 =
2 3
√ 2
v −
2 1
√ 2
v
2 3
σ1 −σ3 σ1 −σ3 σ1 −σ3 σ1 −σ3

y
SV D(H ⊤ H) = V ΣV ⊤ , Σ = diag{σ12 , 1, σ32 }, σ12 > 1 > σ32

MCVA 64 / 64