Trayectorias Óptimas con un Cuadricóptero...Trayectorias Optimas con un Cuadric optero Centro de...

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


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


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.


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


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


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


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.


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)


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)


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.


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)


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)


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:


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.


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.


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


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


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


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


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


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


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


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


Estabilizacion con PD

[Video demostrativo]Buen dıa


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


Trayectorias Óptimas con un Cuadricóptero...Trayectorias Optimas con un Cuadric optero Centro de...

Documents

Transcript of Trayectorias Óptimas con un Cuadricóptero...Trayectorias Optimas con un Cuadric optero Centro de...