herramienta para la simulacion integradas de subsistemas en...

Capítulo 4

Control

En este capítulo se presentan las alternativas de control planteadas, tanto para el lazo internocomo para el lazo externo de control. El interno se ocupará de la estabilización del quadrotor,primero se verá la descripción de un control lineal ya sea mediante PDs o PIDs y en segundo lugarse verá un controlador no lineal como es el H∞. El lazo externo se ocupará de la posición delquadrotor y en esa sección se verán dos controladores no lineales, el primero de ellos, Backsteppingaprovecha las características especiales del sistema no lineal, que se verá en la explicación teóricadel mismo, y el segundo controlador es un controlador predictivo, MPC, un controlador basado enel modelado del quadrotor. Los controladores H∞, Backsteppig y predictivo, son los expuestos enlos artículos [4] y [5], realizados por Guilherme Vianna Raffo, y expuestos aquí para que el presentedocumento quede autocontenido.

Se trata de un sistema complicado para controlar ya no solo porque es no lineal si no porqueademás es subactuado, se controlan seis grados de libertad con tan solo cuatro señales, tres paralos pares y una para el empuje total.

4.1. Controladores de orientación / lazo interno

En esta sección se verán los controladores encargados de la orientación del quadrotor, se empe-zará viendo un control lineal, tanto PD como PID para dos de los tres ángulos y posteriormente severá un controlador no lineal, H∞.

4.1.1. Controladores básicos lineales PID

4.1.1.1. Modelo lineal aproximado

En primer lugar se va a justificar la obtención de un modelo linealizado muy sencillo G(s) =K

s2,

y como los ángulos roll pitch y yaw se corresponden con él.

Transformando las ecuaciones vistas en el capítulo 2, para darle la propiedad de antisimetríaobtenemos:

M(η)η + C(η, η)η = τη

41

42 CAPÍTULO 4. CONTROL

η =

φθψ

τη =

τφτθτψ

M(η) =

[

Ixx 0 −Ixx sin θ

0 Iyy cos2 φ + Izz sin2 φ (Iyy − Izz) cos φ sin φ cos θ

−Ixx sin θ (Iyy − Izz) cos φ sin φ cos θ Ixx sin2 θ + Iyy sin2 φ cos2 θ + Izz cos2 φ cos2 θ

]

La matriz C(η, η) antisimétrica:

C(η, η) =

c11 c12 c13

c21 c22 c23

c31 c32 c33

donde:

c11 = 0

c12 = (Iyy − Izz)(

θ cosφ sinφ+ 12 ψ sin2 φ cos θ

)

+ 12(Izz − Iyy)ψ cos2 φ cos θ

−12Ixxψ cos θ

c13 = (Izz − Iyy)(ψ cosφ sinφ cos2 θ + 12 θ cos θ cos2 φ) + 1

2(Iyy − Izz)θ cos θ sin2 φ

−12Ixxθ cos θ

c21 = (Izz − Iyy)(

θ cosφ sinφ+ 12 ψ sin2 φ cos θ

)

+ 12(Iyy − Izz)ψ cos2 φ cos θ

+12Ixxψ cos θ

c22 = (Izz − Iyy)φ cosφ sinφ

c23 = −Ixxψ sin θ cos θ + Iyyψ sin2 φ cos θ sin θ + Izzψ cos2 φ sin θ cos θ

+12(Iyy − Izz)φ cos θ cos2 φ+ 1

2(Izz − Iyy)φ cos θ sin2 φ− 12Ixxφ cos θ

c31 = (Iyy − Izz)(ψ cos2 θ sinφ cosφ+ 12 θ cos θ cos2 φ) + 1

2(Izz − Iyy)θ cos θ sin2 φ

−12Ixxθ cos θ

c32 = (Izz − Iyy)(

θ cosφ sinφ sin θ + 12 φ sin2 φ cos θ

)

+ 12(Iyy − Izz)φ cos2 φ cos θ

−12Ixxφ cos θ + Ixxψ sin θ cos θ − Iyyψ sin2 φ sin θ cos θ − Izzψ cos2 φ sin θ cos θ

c33 = (Iyy − Izz)φ cosφ sinφ cos2 θ − Iyy θ sin2 φ cos θ sin θ − Izz θ cos2 φ cos θ sin θ

+Ixxθ cos θ sin θ

Nos centramos en lo que sería el punto de operación, φ = 0 y θ = 0, es decir, en la estabilizacióndel quadrotor. Usando la aproximación de pequeño ángulo:

4.1. Controladores de orientación / lazo interno 43

sinα = 0

cosα = 1

Los parámetros de la matriz nos quedarán:

M(η) =

Ixx 0 00 Iyy 00 0 Izz

C(η, η) =

c11 c12 c13

c21 c22 c23

c31 c32 c33

donde:c11 = 0

c12 = 12(Izz − Iyy)ψ − 1

2Ixxψ

c13 = 12(Izz − Iyy)θ − 1

2Ixxθ

c21 = 12(Iyy − Izz)ψ + 1

2Ixxψ

c22 = 0

c23 = 12(Iyy − Izz)φ+ 1

2Ixxφ

c31 = 12(Iyy − Izz)θ − 1

2Ixxθ

c32 = 12(Iyy − Izz)φ− 1

2Ixxφ

c33 = 0

Sustituyendo y agrupando valores en:

M(η)η + C(η, η)η = τη

obtendremos:

Ixxφ+

[

1

2(Izz − Iyy − Ixx)ψ

]

θ +

[

1

2(Izz − Iyy − Ixx)θ

]

ψ = τφ

Ixxθ +

[

1

2(Iyy − Izz + Ixx)ψ

]

φ+

[

1

2(Iyy − Izz + Ixx)φ

]

ψ = τθ

Ixxψ +

[

1

2(Iyy − Izz − Ixx)θ

]

φ+

[

1

2(Iyy − Izz − Ixx)φ

]

θ = τψ

Aplicando transformada de Laplace:

Ixxs2φ(s) +

[

1

2(Izz − Iyy − Ixx)sψ(s)

]

sθ(s) +

[

1

2(Izz − Iyy − Ixx)sθ(s)

]

sψ(s) = τφ(s)


Ixxs2θ(s) +

[

1

2(Iyy − Izz + Ixx)sψ(s)

]

sφ(s) +

[

1

2(Iyy − Izz + Ixx)sφ(s)

]

sψ(s) = τθ(s)

Ixxs2ψ(s) +

[

1

2(Iyy − Izz − Ixx)sθ(s)

]

sφ(s) +

[

1

2(Iyy − Izz − Ixx)sφ(s)

]

sθ(s) = τψ(s)

siendo φ(s) = L [φ(t)], θ(s) = L [θ(t)], ψ(s) = L [ψ(t)], τi(s) = L [τi(t)], manteniendose la letraminuscula por la aproximación de pequeño ángulo. Si despejamos φ, θ y ψ:

Ixxs2φ(s) = τφ(s) −

[

1

2(Izz − Iyy − Ixx)sψ(s)

]

sθ(s) −

[

1

2(Izz − Iyy − Ixx)sθ(s)

]

sψ(s)

Ixxs2θ(s) = τθ(s) −

[

1

2(Iyy − Izz + Ixx)sψ(s)

]

sφ(s) −

[

1

2(Iyy − Izz + Ixx)sφ(s)

]

sψ(s)

Ixxs2ψ(s) = τψ(s) −

[

1

2(Iyy − Izz − Ixx)sθ(s)

]

sφ(s) −

[

1

2(Iyy − Izz − Ixx)sφ(s)

]

sθ(s)

Ixxs2φ(s) = τφ(s) − (Izz − Iyy − Ixx)s2ψ(s)θ(s)

Ixxs2θ(s) = τθ(s) − (Iyy − Izz + Ixx)s2ψ(s)φ(s)

Ixxs2ψ(s) = τψ(s) − (Iyy − Izz − Ixx)s2θ(s)φ(s)

Cómo estamos en la hipótesis de pequeño ángulo podemos despreciar los términos que implicanángulos al cuadrado:

Ixxs2φ(s) ≈ τφ(s) → φ(s) ≈

Kφ

s2τφ(s)

Ixxs2θ(s) ≈ τθ(s) → θ(s) ≈

Kθ

s2τθ(s)

Ixxs2ψ(s) ≈ τψ(s) → ψ(s) ≈

Kψ

s2τψ(s)

4.1.1.2. Indentificación del modelo

Para identificar el sistema, ponemos un controlador proporcional, con ganancia igual a Kc, porlo que la función de transferencia en bucle cerrado sea la correspondiente a un oscilador:

Figura 4.1: Sistema identificado


GBC =KKc

s2 + ω2d

Esto quiere decir, que el sistema en bucle cerrado que resulta con un proporcional es un oscilador.Observando experimentalmente el período de oscilación, T , podemos calcular ωd, ya que:

ωd =2π

T

Para el cálculo se hicieron dos experimentos, el primero de ellos para obtener los valores de roll,y el segundo de ellos para obtener los valores de pitch.

Como cabría esperar, las oscilaciones no son demasiado regulares. Por ello, promediamos a lolargo de varios ciclos.

Con el primer experimento:

Haciendo un zoom y anotando las medidas usadas, tenemos:


T =45′2 − 42′1

3≈ 1′03

ωd =2π

T=

2π

1′03= 6′1rad/s

ω2d = KKc → K =

ω2d

Kc=

37′21

1= 37′21

Luego la función de transferencia para el ángulo roll será:

Groll(s) =37′21

s2

Del segundo experimento obtenemos:

Haciendo un zoom y anotando las medidas usadas, tenemos:


T =103′3 − 100′6

3= 0′9

ωd =2π

T=

2π

0′9= 6′98rad/s

ω2d = KKc → K =

ω2d

Kc=

48′74

1= 48′74

Luego la función de transferencia para el ángulo pitch será:

Gpitch(s) =48′74

s2

Actualmente los experimentos realizados para la identificación del ángulo yaw están pendientesde llevarse a cabo, debido a contratiempos con la parte hardware del equipo, por tanto el diseño delcontrol lineal sólo abarca los ángulos roll y pitch. A partir de los resultados obtenidos, se desarrollaun PD y un PID para los ángulos roll y pitch.

4.1.1.3. Diseño de control lineal para el subsistema de rotación

Para el cálculo de los controladores fijamos ts, ωc y Mf para obtener los distintos parámetros.Definimos el tiempo de subida como una décima de segundo con lo cual obtenemos una frecuenciade corte de 15′7 rad/s. Como el periodo de muestreo es de 20ms se obtiene una frecuencia demuestreo ωs = 100π rad/s que redondeando son unos 300 rad/s por lo que la frecuencia de Nyquistes de 150 rad/s aproximadamente, está a casi una decada de distancia de la frecuencia de corte denuestro diseño, por lo que tenemos suficiente margen.

PD

Fijamos ts, ωc y Mf para obtener Td y Kc.

Para el ángulo roll:

Haciendo uso del fichero disenoPDRoll.m con G(s) = 37′21s2 , tBCs ≈ 0′1s, ωc = 15′7 rad/s y

Mf = 75º obtenemos:

C(s) = Kc (1 + Tds)

Td = 0′24Kc = 1′71


Figura 4.2: Diagramas de Bode del PD para el ángulo roll

Para el ángulo pitch:Haciendo uso del fichero disenoPDPitch.m con G(s) = 48′74

s2 , tBCs ≈ 0′1s, ωc = 15′7 y Mf = 75ºobtenemos:

C(s) = Kc (1 + Tds)

Td = 0′24Kc = 1′31

Figura 4.3: Diagramas de Bode del PD para el ángulo pitch

Como el sistema es un doble integrador los polos están en el mismo sitio, solo cambia la gananciapor eso los Td de ambos controladores salen iguales y solo cambia la ganancia de los controladores.


El PD es realizable gracias a la discretización mediante Euler II, usando un tiempo de muestreode 20 ms, Tm = 0′02s, obtenemos:

C(s) = Kc(1 + Tds)

s =z − 1

Tmz

C(z) = Kc

(

1 + Tdz − 1

Tmz

)

C(z) = Kc(Td + Tm)z − Td

Tmz

C(z−1) =Kc

Tm

[

(Td + Tm) − Tdz−1

]

uk = Kc

[(

1 +TdTm

)

ek −TdTm

ek−1

]

uk = Kc

[

ek +TdTm

(ek − ek−1)

]

PID

Con el PD no conseguimos error nulo en régimen permanente ante perturbaciones mantenidas,ya que el sistema al ser un doble integrador, sólo nos proporciona error nulo en régimen permanenteante la referencia incluso en rampa. Por ello añadimos el término integral para conseguir error nuloen régimen permanente ante perturbaciones mantenidas.

Optamos por el diseño del PID con los dos ceros coincidentes, lo que dará lugar a una curva demagnitud asintótica en forma de “V”. Se usarán las mismas especificaciones que los PD, tBCs ≈ 0′1s,ωc = 15′7 y Mf = 75º.

Para el ángulo roll:

Haciendo uso del fichero disenoPIDRoll.m con G(s) = 37′21s2 , tBCs ≈ 0′1s, ωc = 15′7 rad/s y


C(s) = K ′

c

(1 + τs)2

τs

τ = 0′483(

1τ

= 2′07rad/s)

K ′

c = 0′86

C(s) = Kc

(

TiTds2 + Tis+ 1

)

Tis

Ti = 2τ = 0′9662Td = τ

2 = 0′2415Kc = 2K ′

c = 1′72


Figura 4.4: Diagramas de Bode del PID para el ángulo roll

Para el ángulo pitch:

Haciendo uso del fichero disenoPIDPitch.m con G(s) = 48′74s2 , tBCs ≈ 0′1s, ωc = 15′7 rad/s y


C(s) = K ′

c

(1 + τs)2

τs

τ = 0′483(

1τ

= 2′07rad/s)

K ′

c = 0′65

C(s) = Kc

(

TiTds2 + Tis+ 1

)

Tis

Ti = 2τ = 0′9662Td = τ

2 = 0′2415Kc = 2K ′

c = 1′3

Figura 4.5: Diagramas de Bode del PID para el ángulo pitch


El procedimiento empleado es el siguiente, una vez se han obtenido los parámetros con los ceroscoincidentes, pasamos a los forma nominal del PID:

C(s) = K ′

c

(1 + τs)2

τs→ C(s) = Kc

(

TiTds2 + Tis+ 1

)

Tis

K ′

c

(1 + τ1s)(1 + τ2s)

τ1s= K ′

c

[

1 + (τ1 + τ2)s+ τ1τ2s2]

τ1s

Kc

(

TiTds2 + Tis+ 1

)

Tis

Identificando términos obtenemos:

Kc

Ti=K ′

c

τ1

TiTd = τ1τ2

Ti = τ1 + τ2

Despejando y sustituyendo:

Td =τ1τ2

Ti=

τ1τ2

τ1 + τ2

Kc = TiK ′

c

τ1= K ′

c

τ1 + τ2

τ1

Como tenemos los dos ceros en el mismo sitio, τ1 = τ2 = τ :

Ti = 2τ

Td =τ2

2τ=τ

2

Kc = 2K ′

c

Para discretizar los PID podemos usar Euler II o Bilineal.

Euler II:

C(z) =q0 + q1z

,1 + q2z−2

1 − z−1

q0 = Kc

(

1 + Td

Tm+ Tm

Ti

)

q1 = Kc

(

−1 − 2 Td

Tm

)

q2 = KcTd

Tm

Bilineal:

C(z) =q0 + q1z

,1 + q2z−2

1 − z−1


q0 = Kc

(

1 + Tm

2Ti+ Td

Tm

)

q1 = Kc

(

−1 + Tm

2Ti− 2 Td

Tm

)

q2 = KcTd

Tm

Se usará la aproximación Euler II ya que la bilineal aplicada a un PID impropio da el polo enz = −1, característica indeseable y que evitaremos.

Tal y como pasaba antes, la dinámica va en el mismo lugar y solo cambia la ganancia entreambos controladores.

Figura 4.6: Esquema PID

Diseño alternativo

En lugar de implementar el controlador PID en un solo bloque, lo dividimos en tres términos,

cuya particularización de la ganancia del término integral Ki =Kc

Ti= 0, permite la implementación

de un PD. La ventaja de esta segregación es la posibilidad de usar directamente la medida de laderivada de la salida que da la IMU, en lugar de la aproximación numérica a partir de la propiasalida.

uk = Kc

[

e(t) +1

Ti

ˆ

e(t)dt+ Tdde(t)

dt

]

uk = Kpe(t) +Ki

ˆ

e(t)dt+Kd

de(t)

dt

siendo:

Kp = Kc, Ki =Kc

Ti, Kd = KcTd

Se ha comprobado que la medida de la derivada de los ángulos que se puede obtener a través dela IMU es bastante buena, apartado 3.3.2, por lo que no resultaría imprescindible aplicar el filtro.En la figura 4.7 se aprecia como quedaría el nuevo esquema del PID con el filtro incorporado.


Figura 4.7: Esquema PID separado

Para la implementación de dicho filtro, se va a usar un filtro paso baja de primer orden:

GPB(s) =1

1 + τPBs

Como el tiempo de muestreo es de 0’02 segundos, tendremos una frecuencia de muestreo deunos 300 rad/s, con lo que la frecuencia de Nyquist será la mitad:

ωN =ωs2

= 150 rad/s

Para quedarnos en el lado de la seguridad elegiremos un τPB conservador, pondremos el poloentre la frecuencia de corte y la frecuencia de Nyquist, por lo que 1

τP B≈ 80 ÷ 100. Finalmente nos

quedamos con 1τP B

= 80 por lo que despejando obtenemos τPB = 0′0125.

Aproximación mediante Euler II, s =z − 1

Tmz:

GPB(z) =Tmz

(Tm + τPB)z − τPB

Quedando una expresión en diferencias tal que:

yk = ayk−1 + buk

donde:

a =τPB

Tm + τPB

b =Tm

Tm + τPB


4.1.2. Controlador H∞ no lineal

Se usarán las ecuaciones de Lagrange-Euler obtenidas en el capítulo 2. Información obtenidadel trabajo realizado en los artículos [4] y [5], incluida para que el presente documento sea auto-contenido.

Se desarrollará un controlador H∞ para el subsistema rotacional, aunque en primera instanciase verá la teoría que rodea a dicho controlador.

4.1.2.1. Formulación teórica de control H∞ no lineal

Un sistema suave no lineal de orden n con perturbación tiene la siguiente expresión:

x = f(x, t) + g(x, t)u + k(x, t)d

donde u ∈ Rp es el vector de control de entrada, d ∈ R

q es el vector de perturbaciones externas,x ∈ R

n es el vector de estados. Además, definimos una función de coste, ζ ∈ R(m+p) tal que:

ζ = W

[

h(x)u

]

donde h(x) ∈ Rm es función del vector de errores y W ∈ R

(m+p)×(m+p) es una matriz deponderación. Si el vector de estados es medible, la formulación del H∞ es la siguiente:

Encontrar el menor valor de γ∗ ≥ 0 , tal que para cualquier γ ≥ γ∗ exista un estado derealimentación u = u(x, t) tal que la ganancia L2 de d a ζ sea menor o igual que γ, es decir:

ˆ T

0‖ζ‖2

2 dt ≤ γ2

ˆ T

0‖d‖2

2 dt

Si desarrollamos el término de la integral de la izquierda de la desigualdad, tenemos:

‖ζ‖22 = ζT ζ =

[

hT (x) uT]

W TW

[

h(x)u

]

Podemos reescribir la matriz W TW definida positiva como sigue:

W TW =

[

Q S

ST R

]

Las matrices Q y R son definidas positivas, y como W TW es definida positiva,W TW > 0,tenemos una desigualdad matricial, LMI, sobre la que podemos aplicar el complemento de Schur,por lo que:

Q − SR−1ST > 0

Bajo estas hipótesis, se calcula un control óptimo u∗(x, t) si se encuentra una solución V (x, t),con V (x0, t) ≡ 0 para t ≥ 0, para la siguiente ecuación HJBI ( Hamilton-Jacobi-Bellman-Isaacs).

∂V

∂t+∂TV

∂xf(x, t) +

1

2

∂TV

∂x

[

1

γ2k(x, t)kT (x, t) − g(x, t)R−1gT (x, t)

]

∂V

∂x

−∂TV

∂xg(x, t)R−1STh(x) +

1

2hT (x)

(

Q − SR−1ST)

h(x) = 0


Para cada γ >√

σmax(R) ≥ 0, donde σmax es el máximo autovalor. En tal caso, la ley decontrol óptima es de la forma:

u∗ = R−1(

STh(x) + gT (x, t)∂V (x, t)

∂x

)

4.1.2.2. Diseño del controlador H∞ no lineal para el subsistema rotacional

Recordar que en las ecuaciones vistas en el capítulo segundo, se tiene los pares τ η, que incluyetanto a los pares de control como a las perturbaciones externas:

M(η)η + C(η, η)η = τ η

τ η = τ ηa + τ ηd

donde τ ηa son los pares aplicados y τ ηdrepresenta los errores de modelado y las perturbaciones

externas.En primer lugar se define un nuevo vector de estados:

xη =

˙ηη´

ηdt

=

η − ηrη − ηr

´

(η − ηr)dt

donde ηr, ηr ∈ Rn son la trayectoria y velocidad de referencia respectivamente. Se observa que

el término integral se ha incluido en el vector de estados, para poder conseguir error nulo en régimenpermanente cuando haya perturbaciones externas de acción prolongada.

Se propone como acción de control para el subsistema de rotación la siguiente ley:

τ ηa = M(η)η + C(η, η)η − T −11 (M(η)T xη + C(η, η)T xη) + T −1

1 u

Dicha ley se puede descomponer en tres partes, la primera de ellas incluye a los dos primerostérminos de la ecuación, M(η)η +C(η, η)η, y está orientada a compensar la dinámica del sistema.La segunda parte son los términos que acompañan al vector de estado, xη, y a su derivada, xη, esdecir, −T −1

1 (M(η)T xη + C(η, η)Txη). Si el modelado fuera perfecto y no hubiese perturbacionesexternas, esto es τ ηd

= 0, tendríamos un seguimiento perfecto de la referencia con estos términos,pero como no se puede asegurar ninguna de ambas situaciones, se añade una tercera parte. Dichaparte está formada por el término que va con u y representa la acción necesaria para rechazar lasperturbaciones.

La matriz T de la ley de control se puede expresar como sigue:

T = [T 1 T 2 T 3]

cuya única restricción es que T 1 = ρI, siendo ρ un escalar positivo y I la matriz identidad deorden n.

Sustituyendo la ley de control en las ecuaciones del sistema y definiendo d = M(η)T 1M−1(η)τ ηd

4.1,se obtiene:

M(η)T xη + C(η, η)T xη = u + d

que representa la ecuación dinámica del error del sistema. Con ello se puede definir el problemadel control H∞ como:

4.1T 1 es un escalar por la matriz identidad se puede sacar fuera y M(η)M−1(η) = I


Encontrar una ley de control u(t) tal que el cociente entre la variable de energía de coste

ζ = W[

hT (xη) uT]T

y la señal de energía de pertubación d es menor que un nivel de atenuaciónγ.

Dadas las expresiones de xη y ζ, las matrices Q y S, vistas en el apartado teórico, adoptan laforma:

Q =

Q1 Q12 Q13

Q12 Q2 Q23

Q13 Q23 Q3

, S =

S1

S2

S3

Una vez llegados a este punto, para aplicar los procedimientos presentados en la sección anteriordonde veíamos la teoría del H∞ tenemos que reescribir la ecuación del sistema:

xη = f(xη, t) + g(xη, t)u + k(xη, t)d

f(xη, t) = T −10

−M(η)−1C(η, η) 0 0

T −11 I − T −1

1 T 2 I + T −11 (T 2 − T 3)

0 I −I

T 0xη

g(xη, t) = k(xη, t) = T −10

M(η)−1

0

0

donde I y 0 son la matriz identidad y la matriz cero respectivamente, ambas de orden n, yademás:

T 0 =

T 1 T 2 T 3

0 I I

0 0 I

La solución de la ecuación HJBI depende de la función de coste ζ y de h(x). Siguiendo loexpuesto en [4] y [5], se iguala dicho vector al vector de errores, es decir, h(x) = xη. Con esto, paracalcular la ley de control u se debe resolver la ecuación HJBI, pero antes de ello se enunciará unteorema que servirá de ayuda a dicha resolución:

Teorema: sea V (xη, t) una función escalar tal que:

V (xη, t) =1

2xTη T T

0

M(η) 0 00 Y X − Y

0 X − Y Z + Y

T 0xη

donde, X,Y ,Z ∈ Rn×n son constantes, simétricas y definidas positivas tal que

Z − XY −1 + 2X > 0. Si se verifica la ecuación matricial:

0 Y X

Y 2X Z + 2X

X Z + 2X 0

+ Q +

1

γ2T ′T − (S′ + T )TR−1(ST + T ) = 0

entonces la función V (xη, t) cumple la ecuación HJBI para un valor de γ suficientemente alto.

La matriz T =[

T 1 T 2 T 3

]

se puede calcular mediante las ecuaciones algebraicas de Ricca-

ti, y sustituyendo en la expresión de la ley de control el valor de V (xη, t) correspondiente al índiceγ óptimo, se obtiene la siguiente ley de control:

4.2. Controladores de posición / lazo externo 57

u∗ = −R−1(ST + T )xη

Se sustituye el valor de la acción óptima en la ley de control propuesta para el subsistemarotacional:

τ ∗

ηa= M(η)ηr + C(η, η)η − M(η)

(

KD˙η + KP η + KI

ˆ

ηdt

)

se puede observar la forma tipo PID que adopta el último término de la ecuación.

KD = T −11

(

T 2 + M(η)−1C(η, η)T 1 + M(η)−1R−1(S′

1 + T 1))

KP = T −11

(

T 3 + M(η)−1C(η, η)T 2 + M(η)−1R−1(S′

2 + T 2))

KI = −T −11

(

M(η)−1C(η, η)T 3 + M(η)−1R−1(S′

3 + T 3))

Un caso particular se da cuando la matriz W TW cumple las siguientes condiciones:

Q1 = ω21I , Q2 = ω2

2I , Q3 = ω23I , R = ω2

uI

Q12 = Q13 = Q23 = 0 , S1 = S2 = S3 = 0

En este caso, las ganancias descritas con anterioridad adoptan la siguiente expresión:

KD =

√

ω22 + 2ω1ω3

ω1I + M(η)−1

(

C(η, η) +1

ω2u

I

)

KP =ω3

ω1I +

√

ω22 + 2ω1ω3

ω1I + M(η)−1

(

C(η, η) +1

ω2u

I

)

KI =ω3

ω1M(η)−1

(

C(η, η) +1

ω2u

I

)

Se puede observar cómo estas ganancias no dependen del parámetro γ, propiedad muy impor-tante ya que gracias a ello se pueden calcular con un procedimiento de resolución de ecuacionesalgebraicas y obtener el óptimo.

Para ajustar el controlador, como las ganancias dependen de parámetros que varían con eltiempo, los valores que se fijarán serán, ω1, ω2, ω3 y ωu:

ω1 = 1 , ω2 = 3

ω3 = 9 , ωu = 1′5

4.2. Controladores de posición / lazo externo

Información obtenida del trabajo realizado en los artículos [4] y [5], incluida para que el presentedocumento sea autocontenido.

En un lazo de control interno para el control del subsistema de rotación se propone un con-trolador H∞ que se encargará de la estabilización del quadrotor. La posición y velocidad angularse controla en este lazo, a través de los pares aplicados τ ηa

= [τφaτθa

τψa]T , que son las variables

manipulables.


Controlador usado para seguir la trayectoria de referencia. Se desarrolla en dos pasos, en elprimero se controla la altitud z, con el empuje total, U1. En el segundo, los ángulos de referencia,balanceo, φr, y cabeceo, θr, se calculan para seguir el movimiento xy deseado. En éste último pasola variable U1 se usa como parámetro que varía con el tiempo.

Se usarán las ecuaciones de Lagrange-Euler obtenidas en el capítulo 2 para la dinámica trans-lacional:

x =1

m(cosψ sin θ cosφ+ sinψ sinφ)U1 +

Axm

y =1

m(sinψ sin θ cosφ− cosψ sinφ)U1 +

Aym

z = −g +1

m(cos θ cosφ)U1 +

Azm

Usaremos un vector de estado ampliado, que defina el siguiente sistema:

˙ξ = f

(

ξ,uξ)

donde ξ = [x u0 y v0 z w0]T siendo [x, y, z]T y [u0, v0, w0]T la posición del centro de masasrespecto al sistema inercial y las componentes de la velocidad lineal del centro de masas del quadro-tor expresadas en el sistema de referencia inercial. En este caso uξ no se desarrolla pero se muestrapara darle generalidad, ya que en el caso del controlador predictivo si se verá.

Con el nuevo vector de estado definido, se puede expresar la dinámica del sistema de la siguienteforma:

ξ =

xu0

yv0

zw0

→ ˙ξ =

xu0

yv0

zw0

˙ξ = f

(

ξ,uξ)

=

u0

uxU1

m+Axm

v0

uyU1

m+Aym

w0

−g + (cosφ cos θ)U1

m+Azm

(4.1)

donde Ax, Ay, Az son las fuerzas aerodinámicas que actuan sobre el helicóptero y además, sedefinen dos variables para simplificar la expresión:

ux.= cosψ sin θ cosφ+ sinψ sinφ

uy.= sinψ sin θ cosφ− cosψ sinφ

(4.2)

Para ambos controladores se usará el mismo punto de partida, y el objetivo en ambos es en-contrar una actuación virtual, ucx y ucy, que al igualarla con urx y ury se calcularán los ángulos φr yθr que serán pasados al controlador de estabilización. Para ello, usando como base la ecuación 4.2podemos decir:


urx(t) = cosψ(t) sin θr(t) cosφr(t) + sinψ(t) sinφr(t)

ury(t) = sinψ(t) sin θr(t) cosφr(t) − cosψ(t) sinφr(t)(4.3)

de donde despejaremos los valores de referencia de los ángulos.

4.2.1. Backstepping

El controlador Backstepping en particular ha sido obtenido del artículo [5].

Este controlador va acompañado del controlador H∞, el esquema de control del conjunto es elmostrado en la figura 4.8.

Figura 4.8: Estructura de control

4.2.1.1. Formulación teórica de control Backstepping con acción integral

Técnica diseñada para sistemas no lineales que pueden ser divididos en n subsistemas, los cualessólo dependen de ellos mismos y de los subsistemas anteriores y del subsistema posterior, salvo eln-ésimo subsistema que además depende tambien de la acción de control del sistema no lineal. Tieneuna estructura recursiva ya que va controlando poco a poco, empezando por el primer subsistemay en cada paso va incorporando al control un nuevo subsistema, hasta que controla con la últimaley los n subsistemas y por consiguiente el sistema no lineal.

El quadrotor puede verse sometido a acciones externas, perturbaciones como rachas de viento,o viento continuado, por eso se considera la acción integral en esta aproximación, para evitar tenerun error en régimen permanente debido a dichas perturbaciones cuando estemos en bucle cerrado.

Primero se presentará la formulación de Backstepping para luego aplicarla al sistema en cuestión.Dado un sistema no lineal:

x1 = f1(x1) + g1(x1)x2


x2 = f2(x1,x2) + g2(x1,x2)u + d (4.4)

donde x1 y x2 son las componentes del vector de estado, u es el vector de control, y d es unvector de perturbaciones.

Lo primero es definir mediante un cambio de variables un nuevo vector de estado:

e1 = x1 − xr(t)

e2 = x2 − α(e1, t)

donde α(e1, t) es un control virtual y xr(t) es la señal de referencia. El objetivo de este cambioes conseguir que no tengamos error en régimen permanente, es decir, lımt→∞ e1(t) = 0.

El término integral, ξ(t) =´ t

0 e2(τ)dτ , se meterá en el segundo paso, ya que si se hace en elprimero no se garantiza convergencia con señales de referencia, ya sean éstas constantes o variables.

Para el sistema definido anteriormente, en la ecuación 4.4, aplicando el primer paso obtenemos:

e1 = f1 (e1 + xr(t)) + g1 (e1 + xr(t)) e2 + g1 (e1 + xr(t)) α(e1, t) − xr(t)

Se define una función de Lyapunov definida positiva, V1(e1, t) tal que:

V1(e1, t) =1

2eT1 e1

Y su derivada:

V1(e1, t) = eT1 e1

La acción de control virtual se define como:

α(e1, t) = g1 (e1 + xr(t))−1 [−f1 (e1 + xr(t)) − C1e1 + xr(t)]

donde C1 = CT1 > 0.

Por lo que la función de Lyapunov al sustituir queda:

V1(e1, t) = −eT1 C1e1 + eT1 g1 (e1 + xr(t)) e2

Se observa que cuando e2 = 0, la derivada es definida negativa.

Con el segundo paso del controlador, se introduce la acción integral y el sistema que se consideraahora es el siguiente:

ξ = e2

e2 = f2 (e1 + xr(t), e2 + α(e1, t)) + g2 (e1 + xr(t), e2 + α(e1, t)) u − α(e1, t) + d

donde:

α(e1, t) =∂α

∂e1e1 +

∂α

∂t


∂α

∂e1= g−1

1

[

−g−11 (−f1 − C1e1 + xr(t))D(e1+xr(t))g1 −D(e1+xr(t))f1 − C1

]

4.2

∂α

∂t= g−1

1

[

−g−11 (−f1 − C1e1 + xr(t))D(e1+xr(t))g1xr(t)

−D(e1+xr(t))f1xr(t) − C1xr(t)]

Se elige una segunda función de Lyapunov:

V2(ξ, e1, e2, t) = V1 +1

2ξTKξξ +

1

2eT2 e2

donde Kξ = KTξ > 0.

Al derivar la segunda función, se obtiene:

V2(ξ, e1, e2, t) = V1(e1, t) + ξTKξξ + eT2 e2

= −eT1 C1e1 + eT1 g1e2 + ξTKξe2 + eT2 [f2 + g2u − α(e1, t) + d]

Se define una nueva ley de control con la siguiente expresión:

u = g−12

[

−f2 + α(e1, t) − gT1 e1 − Kξξ − C2e2

]

con C2 = CT2 > 0.

Se puede reescribir la derivada de la primera función de Lyapunov:

V1(ξ, e1, e2, t) = −eT1 C1e1 − eT2 C2e2 + eT2 d

Obteniéndose una ley de control expresada de forma diferente, en términos de un PID no linealcon acción de feedforward

u = −KIξ − KP [x1 − xr(t)] − KD

[

x2 − gT1 (xr(t) − f1)]

+ F F

donde:KI = gT2 Kξ

KP = gT2

(

gT1 + C2gT1 C1

)

KD = gT2 C2

F F = gT2 [−f2 + α(e1, t)]

4.2.1.2. Control de altitud

Se usarán las dos últimas ecuaciones del sistema 4.1, que son las correspondientes a z y w0

respectivamente. Además se considerará el término de perturbaciones Az 6= 0 y debido a la estruc-tura en cascada presentada en la figura 4.8, los valores de los ángulos de Euler serán parámetrosvariables con el tiempo.

4.2D(e1+xr(t))g1 =∂g1

∂ (e1 + xr(t)), es la derivada de g1 respecto a (e1 + xr(t))


Empezaremos con el primer paso del controlador, para ello haremos el cambio de variables:

e1z = z − zr(t)

e2z = z − αz(e1z ,t)

El término integral es como ya se vio en el desarrollo teórico, ξz(t) =´ t

0 e2z (τ)dτ , al igual queel control virtual, α1(e1z , t), que al sustituir en la expresión vista antes los valores de las ecuacionespara la altura, obtenemos:

αz(e1z , t) = −c1ze1z + zr(t)

Con todo esto, el sistema para el control de la altura usando Backstepping se queda con lasiguiente forma:

e1z = e2z + α(e1z , t)

ξz = e2z

e2z = −g + (cosφ cos θ)U1

m+Azm

− αz(e1z , t)

Con el empuje total calculado a partir de la acción de control del Backstepping como:

U1 =cosφ cos θ

m[−kξzξz − (1 + c2zc1z )(z − zr) − (c1z + c2z )(z − zr) + g + zr]

siendo:kξz = 1, c1z = 1, c2z = 1.

4.2.1.3. Control movimiento longitudinal y lateral

Una vez obtenido el valor de U1 se pasa al cálculo de ucx y ucy , en esta ocasión no se consideranperturbaciones externas, es decir Ax = 0 y Ay = 0. Para el cálculo de la actuación virtual usaremosla misma aproximación que en el caso de la altura, se empezará por el cambio de variables:

e1xy = x1xy − xrxy (t)

e2xy = x2xy − αxy(e1xy ,t)

donde

x1xy =

[

xy

]

, x2xy =

[

xy

]

, ξxy =

[

´ t

0 e2x(τ)dτ´ t

0 e2y (τ)dτ

]

.

Y la señal de control virtual en esta ocasión es un vector con dos componentes, que al igual quecon la altura, se calcula como se ha visto en el apartado teórico:

αxy(e1xy , t) =[

αx(e1x , t) αy(e1y , t)]T


Con todo, la señal de control obtenida es la siguiente:

[

ucxucy

]

=m

U1

[

−Kξxyξxy − (I + C2xy C1xy )(x1xy − xrxy )

−(C2xy + C1xy )(x2xy − xrxy ) + xrxy

]

donde:

C1xy =

[

c1x 00 c1y

]

, C2xy =

[

c2x 00 c2y

]

, Kξxy=

[

kξx0

0 kξy

]

.

siendo:

kξx= 1 c1x = 1, c2x = 1.

kξy= 1 c1y = 1, c2y = 1.

Mencionar que ucx y ucy se encargarán de que se cumpla el movimiento en el plano xy, obtenién-dose los valores de φ y θ a través de la ecuación 4.3 sustituyendo y despejando.

Se observa que las ecuaciones 4.2 constituyen el sistema a controlar, por lo que estos valoresno se pueden ajustar directamente, ya que esos dos ángulos son salidas del sistema rotacional,usaremos el lazo de control interno, con el controlador H∞ para calcularlos.

Para ello se definirá una referencia ,urxy(k) =[

urx ury

]T, siendo urx y ury las definidas en 4.3.

Así que haciendo que la referencia coincida con los valores calculados, ucxy = urxy, usando laecuación 4.3 se calculan los ángulos de balanceo y cabeceo de referencia, φr y θr respectivamente,ya que dichas referencias son necesarias para el lazo del subsistema rotacional.

4.2.2. Control predictivo basado en modelo

El controlador predictivo ha sido obtenido del artículo [4].

Denominado por sus siglas E-SSPC, provenientes de state-space predictive controller based on

the error model, que significa controlador predictivo espacio de estado basado en error del modelo.

Al igual que con el Backstepping, al controlador basado en modelo le acompaña para cerrar ellazo externo el controlador H∞. El esquema de control del conjunto es el siguiente:


Figura 4.9: Estructura de control

Se propone un controlador predictivo en un lazo de control externo para el movimiento trans-lacional usando las referencias que vienen del generador de trayectoria. De nuevo, se implementaen dos pasos, en el primero de ellos, la altura z del helicóptero es controlada y el empuje U1 es lavariable manipulada. En el segundo paso, la referencia de balanceo y cabeceo, φr y θr respectiva-mente, se calculan para que se cumpla el movimiento xy deseado, la variable U1 es usada como unparámetro variable en el tiempo en este paso.

Ahora, a diferencia del controlador Backstepping, tenemos que definir una referencia con lamisma expresión matemática que el modelo, pues es un controlador predictivo basado en el modelo.

˙ξr(t) = f

(

ξr(t),uξr(t)

)

donde ξr(t) = [xr(t) u0r (t) yr(t) v0r (t) zr(t) w0r (t)]Ty uξr(t) = [uxr uyr uzr ]T son el vector de

estados de referencia y las entradas de control respectivamente. En este caso no se consideranperturbaciones externas en el modelado de la referencia. Los valores de referencia usados son:

U1r = m (zr + g) , uxr =xrm

U1r

, uyr =yrm

U1r

Si restamos al vector de estados el vector de referencias virtuales, es decir, ˜ξ(t) = ξ(t) − ξr(t)

y lo mismo con la señal uξ, uξ(t) = uξ(t) − uξr(t), se obtiene:

˙ξ(t) = A(t)˜ξ(t) + B(t)uξ(t)

Las matrices A(t) y B(t) son el jacobiano del sistema descrito en 4.1 en relación a ξ(t) y uξ(t)respectivamente.

Para subsanar el error debido a las perturbaciones, en el vector de estado introducimos untérmino integral:


xξ(t) =

x(t)u0(t)´

x(t)dty(t)v0(t)´

y(t)dtz(t)w0(t)´

z(t)dt

=

x(t) − xr(t)u0(t) − u0r (t)´

(x(t) − xr(t)) dty(t) − yr(t)v0(t) − v0r (t)´

(y(t) − yr(t)) dtz(t) − zr(t)w0(t) − w0r (t)´

(z(t) − zr(t)) dt

Discretizando el vector de estados mediante la aproximación de Euler, se tiene:

xξ(k + 1) = A xξ(k) + B(k) uξ(k) (4.5)

Dicho subsistema de translación se descompone en dos partes, una para el control de altura yotra para el control del plano xy. Para cada una de ellas, las matrices A y B son:

Az =

1 ∆t 00 1 0

∆t 0 1

, Bz =

0∆t

mcos θ(k) cosφ(k)

0

Axy =

1 ∆t 0 0 0 00 1 0 0 0 0

∆t 0 1 0 0 00 0 0 1 ∆t 00 0 0 0 1 00 0 0 ∆t 0 1

, Bxy =

0 0∆t

mU1(k) 0

0 00 0

0∆t

mU1(k)

0 0

donde ∆t es el tiempo de muestreo que hay que elegirlo con cuidado, ya que debe ser sufi-cientemente pequeño para percatarse de la dinámica del error translacional y lo suficientementegrande como para considerar, en estado de equilibrio, imperceptible la dinámica del bucle cerradodel sistema rotacional.

Hay que calcular una ley de control que se encuentre en un conjunto acotado de posibles valores,tales que hagan que para el estado definido en 4.5 partiendo de una posición inicial xξ0 lleguemosa lımt→∞xξ = 0.

Por lo que, tanto para la altura como para el movimiento lateral en ambas direcciones, x e y,se diseñará una ley de control que fuerce al sistema a seguir la trayectoria de referencia. Para elprimer paso, el diseño del controlador para la altura, se minimizará la siguiente función de coste:

Jz = [xξz − xξrz]T Qz [xξz − xξrz] +

[

ûξz − ûξrz]T

Rz

[

ûξz − ûξrz]

+ Ω (xξz(k +N2z |k) − xξrz(k +N2z |k)) (4.6)

donde Qz y Rz son matrices diagonales de ponderación definidas positivas, N2z es el horizontede predicción y Ω es el coste del estado cuya expresión es:


Ω (xξz(k +N2z |k) − xξrz(k +N2z |k))

= [xξz(k +N2z |k) − xξrz(k +N2z |k)]T

×P z [xξz(k +N2z |k) − xξrz(k +N2z |k)]

con P z ≥ 0.

La predicción del estado, xξz(k+ j|k), usando la ecuación 4.6 sustituyendo las matrices corres-pondientes para la altura, z, da como resultado:

xξz = P z(k|k) xξz(k|k) + Hz(k|k) ûξz

donde uξz(k|k) = U1(k) − U1r (k), y xξz(k) es el vector de estado para la altura. Los vectoresde referencia que se usarán:

xξrz.=

xξrz(k + 1|k) − xξrz(k|k)...

xξrz(k +N2z − 1|k) − xξrz(k|k)

, ûξrz

.=

U1r (k|k) − U1r (k − 1|k)...

U1r (k +Nuz − 1|k) − U1r (k − 1|k)

donde Nuz es el horizonte de control.Minimizando la ecuación 4.6, la de la función de coste, sin tener en cuenta las restricciones se

obtiene la siguiente ley de control:

ûξz =[

HTz QzHz + Rz

]

−1 [

HTz Qz (xξrz − P zxξz(k)) + Rz

ûξrz]

En realidad solo es necesario aplicar ûξz(k|k) en cada instante k, sin embargo, la señal que seaplica es: U1(k) = ûξz(k|k) + U1r (k).

Una vez obtenida la ley de control para la altura, se pasa a calcular la ley de control para elmovimiento en el plano xy. Para dicha ley, se llevará a cabo el mismo razonamiento, pero estavez en el sistema definido por 4.5 sustituimos las matrices Axy y Bxy, obteniendo finalmente lasiguiente ley de control:

ûξxy =[

HTxyQxyHxy + Rxy

]

−1×

[

HTxyQxy (xξrxy − P xyxξxy(k)) + Rxy

ûξrxy]

donde ûξxy(k|k) = [ux(k|k) uy(k|k)]T y uξxy(k) = uξrxy(k) + ûξxy(k|k).Otra analogía con el controlador de altura, es el caso de las referencias, el vector de errores de

referencia, xξrxy y el de control de error, ûξrxy son obtenidos como en aquel caso.Al igual que pasaba en el Backstepping, para calcular el movimiento en xy hace faltan los valores

de los ángulos de referencia, tanto del balanceo como del cabeceo, φ y θ respectivamente, pero ambos

son salidas del subsistema rotacional, para ello creamos una actuación virtual, ucξxy(k) =[

ucx ucy

]T,

cuyas componentes están definidas por la ecuación 4.3.Obteniéndose sustituyendo en las ecuaciones previas:

[

ucx(k)ucy(k)

]

=

[

ucx(k|k)ucy(k|k)

]

+

[

uxr (k)uyr (k)

]

Finalmente, cuando se tienen las entradas virtuales de referencia, urξxy(k) = ucξxy(k), usandolas ecuaciones 4.3, obtenemos los ángulos φr y θr que le pasaremos al controlador del subsistemarotacional, en este caso al controlador H∞.


El ajuste de lo controladores se lleva a cabo mediante los parámetros que definen la función decoste:

N2z = 3 , Nuz = 3

Qz =

1 0 00 0′8 00 0 10

, Rz =

0′5 0 00 0′5 00 0 0′5

N2xy = 6 , Nuxy = 3

Qxy =

5 0 0 0 0 00 5 0 0 0 00 0 10 0 0 00 0 0 5 0 00 0 0 0 5 00 0 0 0 0 10

, Rxy =

[

150 00 150

]

herramienta para la simulacion integradas de subsistemas en...

Documents