Post on 11-Aug-2020
Trayectorias Optimas con un Cuadricoptero
Centro de Investigacion y de Estudios Avanzados del InstitutoPolitecnico Nacional
Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea ySubmarina
Orlando Garcıa PerezAsesores:
Dr. Salazar Cruz SergioDr. Santos Sanchez Omar Jacobo
Agosto, 2014
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 1 / 27
Contenido
1 Planteamiento del problemaJustificacionObjetivosBreve descripcion de resultados previos
2 ControlDiseno de la trayectoriaResultados
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 2 / 27
Planteamiento del problema
Encontrar la trayectoria mas corta que debe recorrer uncuadricoptero, minimizando un ındice de desempeno, y que pueda serseguida por un control de manera optima
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 3 / 27
Justificacion
El principal motivo de este proyecto es que no hay resultadospublicados que utilicen el enfoque de programacion dinamica con unvehıculo de cuatro rotores.
Utilizando un control optimo se logra un ahorro de energıa factorimportante en este tipo de vehıculos.
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 4 / 27
Justificacion
El principal motivo de este proyecto es que no hay resultadospublicados que utilicen el enfoque de programacion dinamica con unvehıculo de cuatro rotores.
Utilizando un control optimo se logra un ahorro de energıa factorimportante en este tipo de vehıculos.
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 4 / 27
Objetivos
Objetivo General: Encontrar la trayectoria optima que debe seguir unvehıculo aereo de cuatro rotores, con restricciones especificas.
Objetivos Especificos-Disenar una trayectoria con cierta restriccion.-Obtener un control LQR tal que minimice un ındice de desempeno,que sea capaz de seguir la trayectoria encontrada
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 5 / 27
Objetivos
Objetivo General: Encontrar la trayectoria optima que debe seguir unvehıculo aereo de cuatro rotores, con restricciones especificas.
Objetivos Especificos-Disenar una trayectoria con cierta restriccion.-Obtener un control LQR tal que minimice un ındice de desempeno,que sea capaz de seguir la trayectoria encontrada
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 5 / 27
Breve descripcion de resultados previos
Trabajos representativos que utilizan el control optimo en el seguimientode trayectorias y/o estabilizacion.-Discrete Optimal Control for a Quadrotor UAV: Experimental Approach.O. Santos, H. Romero, S. Salazar and R. Lozano-Seguimiento de trayectoria de un mini-helicoptero de cuatro rotoresbasado en metodos numericos.Claudio Rosales, Gustavo Scaglia, Ricardo Carelli y Mario Jordan-Optimal Trajectory Planning and LQR Control for a Quadrotor UAV IanD. Cowling James F. Whidborne Alastair K. Cooke
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 6 / 27
Control con LQR
La solucion al problema de regulador cuadratico optimo, para un sistemaestandar de la forma:
x(t) = Ax(t) + Bu(t) (1)
se obtiene una entrada de control optima
u∗(t) = −R−1BTp(t) (2)
Estabiliza (1) y minimiza un ındice de desempeno
J =
∫ ∞0
(x(t)TQx(t) + u(t)TRu(t))dt (3)
donde Q ≥ 0, R > 0, P ≥ 0
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 7 / 27
Modelo
El consumo de energıa es un factor importante para los UAV´s. En estecaso se trata de un vehıculo de cuatro rotores el cual puede estabilizarse ensubsistemas como en [1]. Se considera el modelo reducido propuesto en [4]
mx = −u sin θmy = u cos θ sinφ
mz = u cos θ cosφ−mgϕ = τφθ = τθψ = τψ
(4)
Este modelo se puede estabilizar de manera optima usando un controlLQR (2) combinado con una linealizacion exacta con el objetivo decancelar no linealidades.
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 8 / 27
Estabilizacion por subsistemas
Estabilizacion del subsistema Z:
mz = ucosφ−mg (5)
Del sistema anterior se utiliza la aproximacion de Euler para obtener larepresentacion de estado en forma discreta y observamos que el sistemaanterior puede linealizarse exactamente con una ley de control propuestoen [3]
u(k) = m(u1(k) + g)(cosθ(k)cosφ(k))−1 (6)
con cosθ(k)cosφ(k) 6= 0 si θ, φ ∈ (-π
2,π
2)
Al sustituir la ley de control (6) el subsistema (5) discretizado queda de lasiguiente forma:
xz(k + 1) = Azxz(k) + Bzu1(k) (7)
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 9 / 27
donde Az =
[1 T0 1
], Bz =
[0T
], T = Tiempo de muestreo.
Si el par Az , Bz es controlable podemos hallar una ley de control optimade horizonte infinito para (7) el cual debe minimizar un ındice dedesempeno
Jz =1
2
∞∑k=0
(xTz (k)Qzxz(k) + u21(k)Rz) (8)
donde Hz ,Qz ≥ 0 , Rz > 0, estan dadas y penalizan el estado y elconsumo de energıa respectivamente; N ∗ k = tf . Entonces se desea hallaruna ley de control u1(k) tal que minimice (8). La solucion es presentadacomo sigue:
u∗1(k) = −(Rz + BTz PzBz)−1BT
z PzAzx∗z (k), ∀k ≥ 0 (9)
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 10 / 27
Si el par Az , Bz son controlables, Pz cumple con la ecuacion algebraicadiscreta de Ricati.
Pz = ATz PzAz − AT
z PzBz(Rz + BTz PzBz)−1BT
z PzAz + Qz (10)
De acuerdo con la teoria de control el sistema en lazo cerrado con la ley decontrol (9) es estable y minimiza el ındice de desempeno (8).Para los siguientes subsistemas se sigue la misma mecanica lo cualconduce a lo siguiente.
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 11 / 27
Estabilizacion del subsistema ψ, cuya representacion en forma discreta es:
xψ(k + 1) = Aψxψ(k) + Bψτψ(k) (11)
Del cual se obtiene el siguiente control:
τ∗ψ(k) = −(Rψ + BTψ PψBψ)−1BT
ψ PψAψx∗ψ(k), ∀k ≥ 0 (12)
Estabilizacion del subsistema y − ϕ:Forma discretizada, tomando en cuenta (6) y que el control linealizantetiende a cero cuando k →∞.
xy ,φ(k + 1) = Ay ,φxy ,φ(k) + By ,φτ∗y ,φ(k) (13)
El control queda definido como:
τ∗y ,φ(k) = −(Ry ,φ + BTy ,φPy ,φBy ,φ)−1BT
y ,φPy ,φAy ,φx∗y ,φ(k),∀k ≥ 0 (14)
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 12 / 27
Finalmente se estabiliza el subsistema x − θ el cual esta definido en formadiscreta como sigue:
xx ,θ(k + 1) = Ax ,θxx ,θ(k) + Bx ,θτ∗θ (k) (15)
El control queda definido como:
τ∗x ,θ(k) = −(Rx ,θ + BTx ,θPx ,θBx ,θ)−1BT
x ,θPx ,θAx ,θx∗x ,θ(k),∀k ≥ 0 (16)
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 13 / 27
Diseno de la trayectoria
En este apartado se considera la representacion en tiempo discreto con elenfoque de programacion dinamica, se obtiene la trayectoria optima y uncontrol no lineal.Se utiliza el modelo reducido (4) y se divide por subsistemas:Control para el subsistema z − ψ
mz (t) = u cos θ (t) cosϕ (t)−mg
ψ (t) = τψ,
Su representacion en forma afın queda como sigue:
xz,ψ = f0,z,ψ(xz,ψ) + f1,z,ψ(θ, ψ)U,
Donde:
f0,z,ψ (xz,ψ) =
x2,z
−mgx4,ψ
0
, f1,z,ψ (θ, ϕ) =
0 0
cos θ cosϕ 00 00 1
.Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 14 / 27
Se discretiza con la aproximacion de Euler
xz,ψ (k + 1) = f0,zψ (xz,ψ (k)) + f1,zψ (θ (k) , ϕ (k))U (k) (17)
Se desea encontrar un control u(k) tal que el ındice de desempeno:
Jz,ψ =1
2xTz,ψ(N)H0xz,ψ (N) +
1
2
N−1∑k=0
{xTz,ψ(k)Qz,ψxz,ψ + UT (k)Rz,ψU(k)
}(18)
dondeH0, Qz,ψ ∈ R4×4, Rz,ψ ∈ R2×2, H0, Qz,ψ ≥ 0, Rz,ψ > 0Sea minimizado sujeto a (17) y finalmente se encuentra un control comosigue:
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 15 / 27
u∗(N − k) = −F1 (N − k)
[x1,z (N − k)x2,z (N − k)
]+ F2 (N − k) (19)
y la entrada de control:
τ∗ψ (N − k) = −G1
[x3,ψ (N − k)x4,ψ (N − k)
](20)
Donde:
F1 (N − k) =
[mts cos (θ (N − k)) cos (ϕ (N − k))
r11m2 + E22t2s cos2 θ (N − k) cos2 ϕ (N − k)
][
E21 (E21ts ,+E22)]
,
F2 (N − k) =[
mts cos(θ(N−k)) cos(ϕ(N−k))r11m2+E22t2
s cos2 θ(N−k) cos2 ϕ(N−k)
]E22gts ,
G1 =(
tsE44t2
s +r22
) [E43 (E43ts + E44)
].y
Eij =
{hij , for k = 1,qij , for k > 1.
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 16 / 27
Los subsistemas x − θ y y − φ difieren en el ındice de desempeno aminimizar pues aquı es donde la trayectoria influye:Se desea minimizar un ındice de desempeno
J =1
2xTz (N)H0xz (N) +
1
2
N−1∑k=0
g0 + UT (k)RzU(k) (22)
donde:g0 = (xTz (k)Qzxz − r)2
Y r es un escalar que corresponde a el radio mınimo del obstaculo quedebe evitar el vehıculo.Se halla los controles para τφ y τθ pero el punto importante es que en ellosse encuentra la trayectoria deseada para x y y. Y estos controles obtenidostambien pueden implementarse para seguir la trayectoria, no obstante laidea es utilizar el control arriba propuesto.
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 17 / 27
Resultados de simulacion con el control optimo
Posicion en los ejes x,y,z. Condiciones iniciales: x = 0m, y = 0m, z = 2m
0 5 10 15 20 25 30−0.5
0
0.5
1
1.5
2
2.5
t [s]
met
ros
Pos
x
Posy
Posz
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 18 / 27
Orientacion en pitch, roll y yaw.
0 5 10 15 20 25 30−0.02
0
0.02
0.04
0.06
0.08
0.1
t [s]
Rad
PitchRollYaw
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 19 / 27
Controles uz , τphi , τθ, τψ.
0 10 20 30 409.7
9.8
9.9
10
10.1
10.2
10.3
t [s]
Nm
Control Uz
0 10 20 30 40−0.4
−0.2
0
0.2
0.4
t [s]
Nm
Control thao phi
0 10 20 30 40−0.4
−0.3
−0.2
−0.1
0
0.1
t [s]
Nm
Control thao theta
0 10 20 30 40−0.15
−0.1
−0.05
0
0.05
t [s]
Nm
Control thao psi
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 20 / 27
Seguimiento de la trayectoria
0 0.5 1 1.5 2 2.5−0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
x [metros]
y [m
etro
s]
ReferenciaSalida
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 21 / 27
Resultados de simulacion con un PID
Posicion en los ejes x,y,z. Condiciones iniciales: x = 0m, y = 0m, z = 2m
0 10 20 30 40 50 60 70 80-0.5
0
0.5
1
1.5
2
2.5
x [segundos]
y [
metr
os]
pos x
pos y
pos z
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 22 / 27
Orientacion en pitch, roll y yaw.
0 10 20 30 40 50 60 70 80-0.04
-0.02
0
0.02
0.04
0.06
0.08
0.1
x [segundos]
y [
rad]
pith
roll
yaw
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 23 / 27
Controles uz , τphi , τθ, τψ.
0 10 20 30 40 50 60 70 809.8
9.802
9.804
9.806
9.808
9.81
9.812
9.814
9.816
9.818
9.82
t [s]
Nm
Control Uz
0 10 20 30 40 50 60 70 80-70
-60
-50
-40
-30
-20
-10
0
10
t [s]
Nm
Control thao phi
0 10 20 30 40 50 60 70 80-45
-40
-35
-30
-25
-20
-15
-10
-5
0
5
t [s]
Nm
Control thao theta
0 10 20 30 40 50 60 70 80-0.1
-0.08
-0.06
-0.04
-0.02
0
0.02
t [s]
Nm
Control thao psi
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 24 / 27
Seguimiento de la trayectoria con PID
0 0.5 1 1.5 2 2.5-0.1
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Salida
Referencia
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 25 / 27
Estabilizacion con PD
[Video demostrativo]Buen dıa
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 26 / 27
Referencias I
R. Lozano, Ed.Unmanned Aerial Vehicles: Embedded Control.Hoboken, NJ: John Wiley & Sons, 2010.
Kirk, D.E.Optimal control Theory and introduccion.Prentice Hall, 1970.
O. Santos, H. Romero, S. Salazar and R Lozano.Discrete Optimal Control for a Quadrotor UAV: ExperimentalApproach.Unmanned Aircraft Systems (ICUAS), 2014 International Conference.
P. Castillo, P. Garcıa, R. Lozano and P. Albertos.Modelado y estabilizacion de un helicoptero de cuatro rotores.Revista Iberoamericana de automatica e informatica industrialvol.4(1), pp.41-57, 2007
Centro de Investigacion y de Estudios Avanzados del Instituto Politecnico Nacional Maestria en Ciencias en Sistemas Autonomos de Navegacion Aerea y Submarina (Cinvestav)Trayectorias Optimas con un Cuadricoptero Agosto, 2014 27 / 27