1. Cinemática de drones
Sistemas de referencia
-
Sistema tierra (W) ENU:
o Fijo a tierra
-
Sistema Cuerpo (B)
o Sistema de referencia del dron
o X suele ser hacia a donde apunta el morro
o Regla de la mano derecha para saber x y z
-
NED (North- East- Down)
o Usado en aeronáutica (nosotros no lo usaremos)
Orientación de la aeronave – Me he perdido un
Para saber la orientación de la nave (Sistema local B) respecto
al sistema global W.
Se utilizan los ángulos de Euler
Orden: X: Pitch(θ) → Y: Roll(ϕ) → Z: Yaw(ψ)
Matriz de orientación
Esta matriz
sirve para
saber la
orientación
respecto del
sistema global
Dw = R·db
Rotaciones
Para conocer las velocidades en cada eje R·vector_velocidad
Proyección de la fuerza de empuje es siempre vertical porque es generada por
las hélices [0 0 Ft]
Ejemplo 1: Cuadricóptero m =1 kg, Empuje total rotores T=m·g = 9.8 (N) apunta
arriba, Φ=0°, θ =10°, ψ=0°, una velocidad de Vx=3 (m/s) en {B}
Ejercicio2: Cuadricóptero m =1 kg, Empuje total rotores T=m·g = 9.8 (N)
(apunta arriba), Φ=0°, θ =10°, ψ=90°, una velocidad de Vx=3 (m/s) en {B}
Modelo cinemático de rotación
Descripción matemática de cómo cambia la orientación del dron en el espacio,
centrándose exclusivamente en la geometría del movimiento sin tener en
cuenta las fuerzas o los motores que lo producen.
Formas de medir la rotación:

Velocidades angulares
o En el sistema cuerpo (B)
𝑇
 ω𝑏 = [ω𝑥 , ω𝑦 , ω𝑧 ]
 Medida por la IMU (sensor del dron)
 Es medido respecto a los ejes del dron
o Sistema tierra (W)
𝑇
 ω𝑤 = [ϕ̇, θ̇, ψ̇]


Es la derivada de los ángulos de Euler (Alabeo, Cabeceo y
Guiñada)
Es la orientación "real" respecto al mundo (dron visto
desde el suelo)
Para convertir las lecturas del sensor interno en cambios de orientación reales
para el observador externo se utiliza la matriz de transformación E
ω𝑏 = 𝑇 · ω𝑤
𝜔𝑤 = 𝐸 · 𝜔𝑏
De manera explícita:
ϕ̇ = ω𝑥 + ω𝑦 sin ϕ tan θ + ω𝑧 cos ϕ tan 𝜃
θ̇ = ω𝑦 cos ϕ − ω𝑧 sin 𝜙
ψ̇ =
1
(ω sin ϕ + ω𝑧 cos ϕ)
cos θ 𝑦
Aquí aparece el término 𝑡θ = tan(θ)Fíjate que
si el dron se inclina 90º, la tangente tiende a
infinito.
Esto es lo que se conoce como Gimbal Lock o
bloqueo de cardán, una limitación matemática
de usar ángulos de Euler
Ejemplo3: Rotación pura de cabeceo (Pitch).


Ángulos actuales: 𝜙 = 0°, θ = 10°, ψ = 0°.
Queremos: una variación de pitch de 20°/s = 0.349 rad/s, sin alabeo ni
guiñada
Ejemplo 4: Rotación de guiñada


Ángulos actuales: 𝜙 = 0°, θ = 10°, ψ = 0°.
Queremos: una variación de pitch de 20°/s = 0.349 rad/s, sin alabeo ni
guiñada
2. Modelo dinámico
La cinemática describe como se mueve el dron geométricamente
La dinámica explica por qué se mueve (Fuerzas y momentos aplicados)

Dinámica de traslación
o El movimiento se rige por la segunda ley de Newton (F=m·a),
pero escrita para el sistema inercial
o

Ft (Empuje total): Suma de los 4 motores (f1+f2+f3+f4)
 Esta fuerza siempre apunta hacia arriba
 R (Matriz de rotación): Proyecta el empuje hacia donde
esta inclinado el dron
 Si el dron está inclinado, parte del empuje se usa para
avanzar horizontalmente y otra parte para luchar contra la
gravedad.
En vuelo estacionario
o Fórmula desglosada:
(Hovering), los ángulos
 Eje X: 𝑚𝑥̈ = 𝐹𝑡 ⋅ (cos ϕ sin θ cos ψ + sin ϕ sin ψ)
son 0 y el empuje Ft debe
 Eje Y: 𝑚𝑦̈ = 𝐹𝑡 ⋅ (cos ϕ sin θ sin ψ − sin ϕ cos ψ) ser exactamente igual al
 Eje Z: 𝑚𝑧̈ = 𝐹𝑡 ⋅ (cos ϕ cos θ) − 𝑚𝑔
peso mg para que el dron
no suba ni baje (𝑧̈ = 0)
Ejemplo 5: Ascenso vertical desde un Hover.


Dron de masa m =1kg, Empuje total Ft= 13 (N) (> al peso del dron),
Calcular la aceleración.
Para 𝜙 = 0°, θ =0°y ψ=0°, la matriz R = I (Identidad)
Ejemplo 6: Desplazamiento horizontal.


Dron de masa m =1kg, Empuje total Ft= 10 (N) (> al peso del dron),
Calcular la aceleración.
Para 𝜙 = 0°, θ =10°y ψ=45°, la matriz R = I (Identidad)
Dinámica rotacional
Explicación de como el dron consigue girar sobre sus propios ejes (Orientación)

Dron como solido rígido: Considerando el cuadricóptero un cuerpo
simétrico nos permite definir la matriz inercia (I)
𝐼𝑥
𝐼 = [0
0
0
𝐼𝑦
0
0
0]
𝐼𝑧
Ecuación de Euler
𝐼ω𝑏̇ + ω𝑏 × (𝐼ω𝑏 ) = 𝜏



τ = [τ𝑥 , τ𝑦 , τ𝑧 ]: Son los pares o momentos (torques) que actúan sobre
cada eje.
𝛚𝒃 : Son las velocidades angulares que ya conoces del modelo
cinemático.
𝛚̇𝒃 : Representa la aceleración angular (qué tan rápido cambia el giro).
Al resolver 𝐼𝜔𝑏̇ + 𝜔𝑏 × (𝐼𝜔𝑏 ) = 𝜏 se obtiene 3 ecuaciones, una para cada eje
τ𝑥 +(𝐼𝑦 −𝐼𝑧 )ω𝑦 ω𝑧

Aceleración en X (Roll): ω̇𝑥 =

Aceleración en Y (Pitch): ω𝑦̇ =

Aceleración en Z (Yaw): ω̇𝑧 =
𝐼𝑥
τ𝑦 +(𝐼𝑧 −𝐼𝑥 )ω𝑧 ω𝑥
𝐼𝑦
τ𝑧 +(𝐼𝑥 −𝐼𝑦 )ω𝑥 ω𝑦
𝐼𝑧
Los términos como (𝐼_𝑦 − 𝐼_𝑧)|ω𝑦 ω𝑧 se llaman términos giroscópicos.
Representan acoplamientos: si el dron está girando sobre dos ejes a la vez,
aparece una fuerza natural que intenta hacerlo girar sobre el tercero
Relación de velocidades de Euler en world (W) y ángulos locales
3. Ecuaciones de Mezclador
Las Ecuaciones del Mezclador (o Mixer) son el "traductor" final de tu dron. Su
función es convertir los 4 comandos globales de vuelo que decide el
controlador en las velocidades específicas a las que debe girar cada motor.


Entrada: 4 comandos de control
o Throttle (𝐹𝑡 ): Empuje vertical para controlar la altura.
o Roll (τ𝑥 ): Par de rotación para inclinarse a izquierda o derecha.
o Pitch (τ𝑦 ): Par de rotación para inclinarse adelante o atrás.
o Yaw (τ𝑧 ): Par de rotación para girar sobre el eje vertical.
Procesamiento
o Primero se debe entender la ubicación de los motores
M1 (Frontal): Situado adelante.
M3 (Posterior): Situado atrás.
M2 (Izquierdo): Situado a la izquierda.
M4 (Derecho): Situado a la derecha
o Las 4 ecuaciones fundamentales
 Empuje vertical: Es la suma de las fuerzas de los 4
motores
 (Ft) 𝐹𝑡 = 𝑘𝑓 (ω12 + ω22 + ω23 + ω24 )

Roll (𝜏 x): Se genera por la diferencia de empuje entre los
motores laterales (M2 y M4)
 ir hacia la derecha (M2↓, M4↑)
 τ𝑥 = 𝑑 ⋅ 𝑘𝑓 (ω24 − ω22 )

Pitch(𝜏 y): Se genera por la diferencia entre los motores
frontal y posterior (M1 y M3)
 ir hacia atrás (M1↑, M3↓)
 τ𝑦 = 𝑑 ⋅ 𝑘𝑓 (ω12 − ω23 )

Yaw(𝜏 z): Se basa en el par de reacción (giro sobre si
mismo)
 girar sentido antihorario (M1↑ = M3↑, M2↓ = M4↓)
 τ𝑧 = 𝑘𝑟 (ω22 + ω24 − ω12 − ω23 )
o Matriz de asignación: Todas las relaciones se agrupan en una
única matriz que permite al software calcularlo todo de golpe
Para saber la
velocidad M^-1 (la
inversa de la
matriz)

Salida: El resultado final del procesamiento son las velocidades
específicas a las que debe girar cada una de las 4 hélices
o Velocidades angulares (𝜔1 , 𝜔2 , 𝜔3 , 𝜔4)
o Fuerza y par real: Al girar a esas velocidades, los motores
generan físicamente el empuje y los momentos que el dron
necesitaba para moverse
 Fuerza de empuje vertical: 𝑓 𝑘 𝜔 , 𝑘 = coeficiente de
empuje
 Par de reacción:
Ejemplos
Caso1: Ascenso vertical: Si quieres subir sin inclinarte ni girar, todos los
motores deben empujar con la misma fuerza
o Lógica
 La fuerza de empuje (Ft) es positiva
 Los pares de rotación son cero (𝜏𝑥 , 𝜏𝑦 , τ𝑧 )
o Resultado: Todos los motores reciben la misma fuerza

𝐹
ω12 = ω22 = ω23 = ω24 = 4𝑘𝑡
𝑓
Caso2: Cabeceo / Pitch: Para inclinar el dron adelante o atrás, necesitas crear
una diferencia de fuerza entre el motor frontal (M1) y el posterior (M3).
o Lógica: Queremos un par de cabeceo positivo (𝜏𝑦 > 0)
o Resultado
τ𝑦
o M1 acelera añadiendo par + 2𝑘 𝑑
𝑓
τ𝑦
o M3 desacelera restando par - 2𝑘 𝑑
𝑓
o Como el de delante empuja más que el de atrás, el morro sube y
el dron se inclina hacia atrás
Caso 3: Alabeo / Roll: Funciona igual que el cabeceo, pero usando los motores
laterales: el izquierdo (M2) y el derecho (M4)
o Lógica: Queremos un par de cabeceo positivo (𝜏𝑥 > 0)
o Resultado
𝜏
o M4 acelera añadiendo par + 2𝑘𝑥 𝑑
𝑓
𝜏
o M2 desacelera restando par - 2𝑘𝑥 𝑑
𝑓
o El lado derecho sube más que el izquierdo, provocando una
inclinación hacia la derecha.
4. Control jerárquico de drones multirrotor
Explica como el “cerebro” del dron organiza las tareas. El control Jerárquico
divide el problema en varias capas o lazos que trabajan a distintas velocidades
o Lógica de los lazos: El control va por prioridades
o Lazos internos (Rápidos):
 Se encargan de la estabilidad.
 Reaccionan en milisegundos para que el dron no se caiga
si hay una ráfaga de aire
o Lazos externos (Lentos):
 Se encargan de la navegación.
 Son los que deciden ir del punto A al punto B y no
necesitan ser tan frenéticos
Funcionamiento de cada lazo
o Lazo de altitud
o Objetivo: Mantiene la altura deseada (Zs)
o Es independiente del movimiento horizontal
o es el encargado de decidir cuánta fuerza vertical neta deben
hacer los motores en conjunto para que el dron suba, baje o se
mantenga quieto en el aire
o Método: Utiliza un control PID + Feedforward: Aade
dir4eectamente el valor de la gravedad (mg) para que el dron
sepa de antemano cuánta fuerza mínima necesita para no caerse
o Lazo de posición
o Entrada: Coordenadas deseadas (Xs, Ys)
o Procesamiento: Usa controladores tipo PD para calcular la
aceleración necesaria
o Traducción: Convierte esa aceleración en ángulos de inclinación
o Limitación: En drones normales, estos ángulos se suelen limitar
(saturar) a 20°-30° por seguridad.
(Traductor): Convierte una aceleración en la inclinación que tiene que tener para
conseguirlo
1º Ecuación dinámica traslacional: La aceleración del dron depende de la fuerza
de los motores
2ºPara realizar los cálculos se hacen suposiciones
- Vuelo nivelado (Ft=mg): Se asume que el empuje total es igual al peso
para centrarnos Oslo en el movimiento horizontal
- Ángulos pequeños: Se asume que el dron no se va a inclinar mucho
3º Las ecuaciones de movimiento se simplifican
4º Ecuaciones usadas por el codigo
1
ϕ = (𝑎𝑥 sin ψ − 𝑎𝑦 cos ψ)
𝑔
1
θ = (𝑎𝑥 cos 𝜓 − 𝑎𝑦 sin 𝜓)
𝑔
o Lazo de actitud
o Objetivo: Estabilizar la orientación del dron
o Controladores: Utiliza tres controladores PD independientes (uno
para cada eje) que
o generan los torques necesarios para corregir el error de ángulo
**** Teoria ****
Matlab
Ejercicio 1 pagina 40
Ejercicio-1: Implementación del controlador del dron
A partir del archivo SimDron.m, completa el código de la función control_quad()
siguiendo las indicaciones de las diapositivas.
La función recibe dos parámetros de entrada: x: vector de estado del dron, y
Ref: vector de referencias (valores deseados).
%% Señal de Control
function Tau = quad_control(t, x, Ref)
global m g
global Kpx Kdx Kpy Kdy Kpz Kdz
global Kp_phi Kd_phi Kp_theta Kd_theta Kp_psi Kd_psi
pos = x(1:3); vel = x(4:6); phi = x(7) ; theta = x(8); psi =x(9); omega =
x(10:12);
xref = Ref(1); yref = Ref(2); zref = Ref(3); psiref = Ref(4);
%Control de altitud
Ft = Kpz*(zref - pos(3)) - Kdz*vel(3) + m*g;
%Control de X e Y
ax = Kpx*(xref - pos(1)) - Kdx*vel(1);
ay = Kpy*(yref - pos(2)) - Kdy*vel(2);
sp = sin(psi); cp = cos(psi);
phi_ref = (ax*sp - ay*cp)/g;
theta_ref = (ax*cp - ay*sp)/g;
tau_x = Kp_phi*(phi_ref-phi) - Kd_phi*omega(1);
tau_y = Kp_theta*(theta_ref-theta) - Kd_theta*omega(2);
tau_z = Kp_psi*(psiref-psi) - Kd_psi*omega(3);
Tau = [Ft; tau_x; tau_y; tau_z];
end
Guiñada apenas cambia, pitch el rojo.
¿ Que pasaría si se aumenta la p?
Roll y pitch iguales porque es simétrico, la guiñada si que va a parte
Kpz=25
Según z dice que se ha pasado, y en la trayectoria del dron también puedes
verlo
Ejercicio-2: Plan de vuelo
Modifica la función entrada() para que el dron ejecute el siguiente plan de
vuelo:



Fase-1: Despegue y ascenso a una altura z = 5m 0 < t ≤ 2 s
Fase-2: Desplazamiento horizontal hasta (x, y)=(5, 5) m 2 < t ≤ 5 s
Fase-3: Cambio de guiñada de 45° 5 < t < 8 s
%% FUNCIONES ------------------------------------------%Generacion de la entrada
function Ref = entrada(t)
%Ref = [xref, yref, zref, psiref];
%EJERCICIO 2
if t <= 2.
Ref = [0, 0, 5, 0];
elseif t > 2 && t <= 5
Ref = [5, 5, 5, 0];
else
Ref = [5, 5, 5, pi/4];
end
end
IMPORTANTE: DINAMICA Y CONTROL
Programar drones con matlab en la proxima clase
Exmane para después de semana santa, la primera semana no sta