CONTROL PREDICTIVO GENERALIZADO APLICADO A UN

Memorias del XVIII Congreso Mexicano de Robotica 2016Universidad Autonoma de Sinaloa y Asociacion Mexicana de Robotica e Industria AC

XVIII COMRob 2016, ISBN: En tramite9–11 de Noviembre, 2016, Mazatlan, Sinaloa, Mexico

XVIIICOMRob2016/ID-000

CONTROL PREDICTIVO GENERALIZADO APLICADO A UNSISTEMA MULTIROTOR†∗

Jose Luis Mendoza-SotoFacultad de Ingenierıa

Universidad Nacional Autonoma deMexico

Ciudad de Mexico, 04510Email: [email protected]

H. Rodrıguez Cortes†

Centro de Investigacion y deEstudios Avanzados

Instituto Politecnico NacionalCiudad de Mexico, D.F., 07360

[email protected]

Victor H. Cruz SanchezCentro de Investigacion y de

Estudios AvanzadosInstituto Politecnico Nacional

Ciudad de Mexico, D.F., [email protected]

ABSTRACTSe propone una estrategia de control para seguimiento de

trayectorias en un sistema multirotor basada en el enfoque decontrol predictivo generalizado en combinacion con una estrate-gia de control de orientacion en el espacio SO(3). El enfoque decontrol predictivo permite realizar el seguimiento de trayectoriasutilizando acciones de control optimo para cumplir restriccionessobre la trayectoria deseada. Se presentan resultados en sim-ulacion que muestran el desempeno del controlador propuestoy se comparan con los resultados obtenidos en un controladorbasado en control total de energıa.

INTRODUCCIONLos vehıculos aereos no tripulados (UAV’s) han tenido un

auge en su produccion durante los ultimos anos debido a la grancantidad de aplicaciones que se han encontrado.

Los UAV’s cuatrirotor son una excelente plataforma para in-vestigacion y analisis de algoritmos de control debibo a su nat-uraleza no lineal subactuada. En [1] se presenta un resumendonde se analizan algoritmos de control aplicados a cuatriro-tores como: PID, regulador cuadratico lineal (LQR), modosdeslizantes, Backstepping, linealizacion por reatroalimentacion,control adaptable, control robusto y optimo, L1, H∞, logica di-fusa y redes neuronales artificiales. Se describen las ventajas y

∗†TRABAJO REALIZADO BAJO EL PATROCINIO DE CONACYT.†Corresponsal.

desventajas de las diferentes estrategias de control. Ademas, losautores proponen una estrategia de control basada en la teorıa desistemas hıbridos.

Durante el seguimiento de trayectorias es deseable que unUAV tenga la capacidad de evadir obstaculos. La evasion deobstaculos puede incluirse en forma de restriccion al disenar al-goritmos de control basados en procesos de optimizacion.

La implementacion de algoritmos de evasion de obstaculosy de control para un vehıculo aereo es costosa desde el punto devista computacional por lo que debe implementarse utilizandoalgoritmos eficientes.

En [2] se presenta un enfoque de control en tiempo realpara un cuatrirotor donde se combina realimentacion completade estados y control predictivo basado en modelos (MPC) no lin-eal sin restricciones. El metodo propuesto utiliza un algoritmode control optimo iterativo Cuadratico Lineal Secuencial (SLQ).Los autores presentan pruebas experimentales del esquema prop-uesto.

En [3] y en [4] se utiliza un enfoque de control predictivojerarquico hıbrido en dos niveles de control para un cuatrirotor.Un primer nivel de control se encarga de generar trayectoriaslibres de obstaculos mientras que el segundo nivel se encargade estabilizar el cuatrirotor y de seguir trayectorias. El segundonivel de control se disena utilizando MPC lineal para estabilizarel vehıculo alrededor de puntos de operacion generados por elprimer nivel de control a una frecuencia de muestreo menor enel primer nivel de control. En el primer trabajo la estrategia de

Copyright c⃝ 2016 by AMRob

control se aplica a un solo cuatrirotor, mientras que en el segundotrabajo el control se utiliza para un conjunto de cuatrirotores.

En [5] los autores presentan una estrategia de control paraguiar un grupo de cuatrirotores en tareas de cooperacion us-ando MPC. La estrategia hace uso del modelo no lineal de losvehıculos y toma en cuenta los movimientos de los otros cua-trirotores para evitar colisiones y obstaculos. Se presentan simu-laciones numericas.

En [6] se presenta un metodo para generar trayectorias entiempo real para un cuatrirotor tomando en cuenta restriccionesdel estado inicial y final para la posicion, velocidad y aceleracionsobre un periodo de tiempo especificado.

En [7] se presenta una estrategia de control integrado pararesolver el problema de control de vuelo de una formacion lıder-seguidor para dos cuatrirotores. El control se disena para el cua-trirotor seguidor con el objetivo de mantener la formacion y evi-tar obstaculos durante el vuelo utilizando dos niveles de control.Un nivel de control se basa en MPC y el otro nivel en lineal-izacion por realimentacion de estados. El controlador MPC gen-era una trayectoria de referencia optimizada libre de colisionesy es robusta a las perturbaciones de entrada. El controlador delinealizacion por realimentacion da seguimiento a la referenciagenerada en el MPC y compensa cualquier error de seguimientodurante el intervalo de actualizacion del MPC. Los autores pre-sentan experimentos de la formacion de vuelo con el entorno decaptura de movimiento de Vicon.

En [8] se presenta una estrategia MPC para la estabilizaciony seguimiento de trayectorias de un cuatrirotor con empuje re-stringido para considerar condiciones de operacion realistas. Losautores utilizan linealizacion aproximada alrededor de los puntosde operacion y presentan resultados en simulacion.

En este artıculo se propone un esquema de control predictivobasado en modelos. Se utiliza el enfoque de control predictivogeneralizado (GPC) con restricciones y un control geometrico deorientacion. Para el control de la traslacion del cuatrirotor se uti-liza el metodo GPC aprovechando las caracterısticas dinamicasdel cuatrirotor en combinacion con el control de orientacion uti-lizado en [9] basado en el analisis de la orientacion del cuatriro-tor en el espacio SO(3). El objetivo es obtener la secuencia decontrol optima para que el cuatrirotor siga una trayectoria de ref-erencia deseada mientras satisface restricciones sobre la trayec-toria. Ademas, se presenta una comparacion de los resultados ensimulacion con el algoritmo de control total de energıa (TECS)presentado en [10].

0.1 Control predictivo generalizado (GPC)El GPC es un esquema de control con algunas diferencias re-

specto al MPC convencional, por ejemplo en el caso de ausenciade restricciones se puede obtener una solucion analıtica. Tiene lacapacidad para tratar sistemas inestables o de fase no mınima,ademas aparece un parametro de ajuste adicional llamado de

horizonte de control. El enfoque de GPC utiliza un modeloCARIMA (Controlador Auto Regresivo de Promedio Movil In-tegrado) con n salidas y m entradas [11] de la forma

A(z−1)y(t) = B

(z−1)u(t −1)+

1∆

C(z−1)e(t) (1)

donde A(z−1

)y C

(z−1

)son matrices polinomiales monicas de

orden n× n y B(z−1

)es una matriz polinomial de orden n×m.

El operador ∆ queda definido como ∆ = 1− z−1. Las variablesy(t), u(t) y e(t) son vectores de salida, de control y de ruido,con dimension n×1, m×1 y n×1, respectivamente. Es comunconsiderar por simplicidad e(t) como ruido blanco.

El objetivo del GPC es encontrar la secuencia de control queminimiza el siguiente criterio cuadratico con horizonte finito

Jc (Np,Nu) =Np

∑j=1

∥y(t + j|t)−w(t + j)∥2R

+Nu

∑j=1

∥∆u(t + j−1)∥2Q (2)

donde y(t + j|t) es una prediccion optima j pasos adelante de lasalida del sistema calculada en el tiempo t, que es posible cal-cular si se conocen los vectores de la entrada y salida pasados yla secuencia de referencia futura. Np y Nu son los horizontes deprediccion y de control, respectivamente. w(t + j) es el vectorde la secuencia de referencia para el vector de salida. R y Q sonmatrices de ponderacion positivas definidas.

En el caso comun la matriz que modela el ruido se definecomo C

(z−1

)= In×n. La prediccion para la salida se obtiene

como

y(t + j) = F j(z−1)y(t)+E j

(z−1)e(t + j)

+E j(z−1)B

(z−1)∆u(t + j−1) (3)

donde E j(z−1

)y F j

(z−1

)se obtienen al resolver en forma re-

cursiva la ecuacion Diofantina matricial

In×n = E j(z−1) A

(z−1)+ z− jF j

(z−1) (4)

Haciendo E j(z−1

)B(z−1

)= G j

(z−1

)+ z− jG jp

(z−1

)con

grado(G j

(z−1

))< j, la ecuacion de prediccion puede escribirse

como

y(t + j|t) = G j(z−1)∆u(t + j−1)

+G jp(z−1)∆u(t −1)+F j

(z−1)y(t) (5)


La ecuacion (5) puede expresarse en forma compacta como

y = Gu+ f (6)

con f j =G jp(z−1

)∆u(t −1)+F j

(z−1

)y(t) donde no se consid-

eran los terminos que aparecen con adelanto en el tiempo.La ecuacion (2) se puede reescribir como

Jc = (Gu+ f−w)T R(Gu+ f−w)+uT Qu (7)

donde R = diag(Rp, · · · ,Rp) y Q = diag(Q, · · ·Q). Si no hayrestricciones, el control optimo puede expresarse como

∆u(t) = K (w− f) (8)

donde K =(GT R G+ Q

)−1 GT R. Debido a que la estrategiaGPC utiliza el enfoque de horizonte movil, solamente es nece-saria la componente ∆u(t) en el instante t y se descartan lasdemas componentes de ∆u(t).

En el caso de control restringido es necesario resolver unproblema de optimizacion cuadratica en cada paso de muestrapara obtener la secuencia de control optima y no hay una ex-presion analıtica para el control. Las restricciones tomadas encuenta en este artıculo limitan las senales de control u(t) y latrayectoria del cuatrirotor. Por lo que se establece un problemade optimizacion cuadratica de la funcion de costo con las restric-ciones

Rru ≤ c (9)

Rr =

IN×N−IN×N

G−G

; c =

Lu−Lu

Ly− f−Ly+ f

donde N =Np, L es una matriz de orden (N ×n)×m formada porN de matrices identidad de orden m×m. Las variables con barrassuperior e inferior representan los lımites superior e inferior delas entradas y salidas, respectivamente.

MODELOPara describir el modelo dinamico del cuatrirotor es nece-

sario definir dos marcos de referencia, ver figura 1. El marco dereferencia cuerpo y el marco de referencia inercial.

La dinamica rotacional esta dada por

JΩ+Ω× JΩ = Me (10)

FIGURE 1. MARCOS DE REFERENCIA

donde Ω =[

p q r]

es el vector de velocidad angular alrededorde los ejes xb, yb y zb. J es la matriz de inercia, y Me es el vectormomentos de entrada

Me =

LMN

=

(T2 −T4) l(T1 −T3) l

−Q1 +Q2 −Q3 +Q4

(11)

donde Qi son los momentos reactivos y Ti los empujes produci-dos por cada motor definidos como

Ti =CTi π r4i ρω2

i (12)

Qi =CQiπ r5i ρω2

i (13)

con CTi y CQi los coeficientes aerodinamicos propios de cadahelice, ri el radio de cada helice, ρ la densidad del aire y ωison las velocidades de cada uno de los motores.

La dinamica traslacional en un marco de referencia inerciales

X = ge3 −1

maTT R(Φ)e3 (14)

TT =4

∑i=1

Ti (15)

con X =[

x y z]T el vector de posicion traslacional, e3 =[

0 0 1]T , ma es la masa del cuatrirotor, g es la constante de

gravedad, TT es el empuje total de los motores del cuatrirotor.La matriz de rotacion R(Φ) del marco de referencia inercial almarco de referencia cuerpo en terminos de los angulos de alabeoϕ , cabeceo θ y guinada ψ es

R(Φ) =

cψ cθ cψ sθ sϕ − sψ cϕ cψ sθ cϕ + sψ sϕcθ sψ sψ sθ sϕ + cψ cϕ sψ sθ cϕ − cψ sϕ−sθ cθ sϕ cθ cϕ

(16)


FORMULACION DEL CONTROLEn este artıculo se utiliza uno de los enfoques mas utilizados

para el diseno de control de sistemas subactuados que es disenarcontroladores en cascada con el objetivo de que los grados delibertad no actuados sean controlados por los grados de libertadque si son actuados. Entonces se disenan las variables de controlpara obtener el comportamiento deseado de los grados de liber-tad actuados. En [9] se propone un esquema de control para larotacion del cuatrirotor utilizando las caracterısticas del espacioSO(3). Se definen los errores de angulo eR y velocidad angulareΩ como

eR =12(RT

d R−RT Rd)∨

(17)

eΩ = Ω−RT RdΩd (18)

donde Rd y Ωd son los valores deseados para la matriz derotacion y el vector de velocidad angular, respectivamente. Elmapeo ∨ : so(3)→ R3 es el mapeo inverso del mapa

∧· : R3 →so(3) definido como x y = x× y. Entonces se propone el controlcomo

Me =−kReR − kΩeΩ +Ω× JΩ

− J(ΩRT RdΩd −RT RdΩd

)(19)

con kR, kΩ > 0 y donde la matriz de rotacion deseada se obtienecomo Rd =

[b2d (t)× b3d (t) ,b2d (t) ,b3d (t)

]∈ SO(3). donde

b2d (t) =b3d × b1d∥∥∥b3d × b1d

∥∥∥ (20)

y donde b3d puede obtenerse a partir del control de la dinamicatraslacional como

b3d =u∥u∥

(21)

donde u=[

ux uy uz]. Entonces la ecuacion 14 puede expresarse

como

maX = mage3 −u−Γ (22)

con

Γ =TT

e3RdRe3

((eT

3 RTd Re3

)b3 − b3d

)(23)

donde se considera que la dinamica rotacional es mucho masrapida que la dinamica traslacional entonces el termino Γ → 0y el sistema puede expresarse como

maX = mage3 −u (24)

Entonces el diseno del control predictivo para la dinamica trasla-cional se disena con la estructura lineal

x = vx y = vy z = vz

vx =− 1ma

ux vx =− 1ma

uy vz = g− 1ma

uz

(25)

de donde se obtienen las funciones de transferencia:

X (s)Ux (s)

=Y (s)Uy (s)

=Z (s)Uz (s)

=− 1ma

1s2 (26)

donde la fuerza de gravedad g se considera como una pertur-bacion desconocida que compensa el controlador.

Para el diseno del control predictivo se han tomado losparametros ma = 1.12 y g = 9.81 con un tiempo de muestreode 0.0083 segundos de donde resultan las funciones de transfer-encia

X(z−1

)Ux (z−1)

=Y(z−1

)Uy (z−1)

=Z(z−1

)Uz (z−1)

=−0.00003098z−1 −0.00003098z−2

1−2z−1 + z−2 (27)

Se obtienen ahora las matrices polinomiales

A(z−1)= diag

[1−2z−1 + z−2] (28)

B(z−1)= diag

[−0.00003098z−1 −0.00003098z−2] (29)

donde A(z−1

)y B

(z−1

)son matrices polinomiales de orden 3×

3.

Ejemplo numericoDebido a la naturaleza numerica del metodo GPC se pre-

senta un ejemplo de calculo con un horizonte de prediccion deNp = 3, con horizonte de control Nu = 2 y peso para el errorde R = diag(0.1,0.1,0.1) y Q = diag(1,1,1) y para el controlde donde se obtiene la expresion analıtica para el caso no re-stringido.


Al resolver de manera iterativa la ecuacion (4) se obtienenlas matrices E j y Fi de orden 3×3 con j = 1, ...,3 dadas por

E1 = diag(1)

E2 = diag(1+3z−1)

E3 = diag(1+3z−1 +6z−2)

F1 = diag(1−3z−1 +3z−2)

F2 = diag(3−8z−1 +6z−2)

F3 = diag(6−15z−1 +10z−2)

Al utilizar E j y Fj en la ecuacion (3) se llega a la forma de laecuacion (5) de donde se observa que

G =

G1 0G2 G1G3 G2

; G jp =

G1pG2pG3p

G1 = diag(−0.0289×10−3)

G2 = diag(−0.1156×10−3)

G3 = diag(−0.2602×10−3)

G1p = diag(−0.0289×10−3)

G2p = diag(−0.0867×10−3)

G3p = diag(−0.1735×10−3)

G1...Np y G1p...Np p son matrices cuadradas de orden 3×3.Finalmente al utilizar la expresion (8) se obtiene la secuen-

cia de control u =[

∆u(t) ∆u(t +1) ∆u(t +2)]

de donde solose utiliza la primera componente. En el caso en que apare-cen restricciones lineales entonces la prediccion de las senalesde posicion se utiliza para calcular la secuencia de controloptimo utilizando un algoritmo de programacion cuadratica conla funcion de costo (7) sujeta a las restricciones (9).

RESULTADOS EN SIMULACIONEl controlador propuesto se evalua utilizando simulaciones

numericas en el software Matlab. Para el desempeno mostradoen las figuras de esta seccion se utilizaron parametros de hor-izonte de prediccion Np = 60, horizonte de control Nu = 8,Rp = diag(19,19,1) y Q = diag(1.2,1.2,1). la trayectoria dereferencia esta dada como xre f = cos(ωt) m, yre f = sin(ωt) m yz=−0.5 m. Por cuestion de espacio no se presenta el controladorTECS pero puede revisarse junto con los parametros en [10].

En la figura 2 se presenta el error de seguimiento de latrayectoria en los ejes x, y y z donde se observa la convergencia

del error a cero utilizando el controlador predictivo y el contro-lador de energıa total TECS.

0 5 10 15 20 25

-1

-0.5

0

0.5

1x-x

dGPC

x-xdTECS

0 5 10 15 20 25

Error

en pos

ición[m

]

-0.5

0

0.5

y-ydGPC

y-ydTECS

Tiempo[s]0 5 10 15 20 25

-0.5

0

0.5

1

z-zdGPC

z-zdTECS

FIGURE 2. ERROR DE DESPLAZAMIENTO

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2-0.5

0

0.5

1

eRψ GPC

eRψ TECS

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2Error

de or

ientac

ión [ra

d]

-0.5

0

0.5 eRθ GPC

eRθ TECS

Tiempo[s]0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

-2

-1

0

1

eRφ GPC

eRφ TECS

FIGURE 3. ERROR DE ORIENTACION

La figura 3 muestra el error de seguimiento de los angulosde orientacion del cuatrirotor alrededor de los ejes xb, yb y zb.En las figuras 4 y 5 se presentan las senales de control del cu-atrirotor. En las figuras 2-4 se muestran pruebas sin agregarrestricciones. En la figura 6 se presenta el seguimiento de latrayectoria imponiendo restricciones en el desplazamiento en lasdirecciones xy. En este experimento se ajustaron las restriccionesde desplazamiento como −0.9 < x < 0.9 y −0.9 < y < 0.9. Losparametros de simulacion que se usaron en esta prueba fueronNp = 90, Nu = 3, Rp = diag(0.1,0.1,0.1) y Q = diag(1,1,1).Se observa que el seguimiento de la referencia en lınea punteadase realiza respetando las restricciones impuestas y vuelve a latrayectoria de refencia. Las restricciones en control predictivose han utilizado para modelar obstaculos y realizar evacion deobstaculos por lo que es importante que un dispositivo pueda


0 0.2 0.4 0.6 0.8 1 1.2

-1

0

1 LGPC

LTECS

0 0.2 0.4 0.6 0.8 1 1.2

Mome

ntos [N

m]

-2

-1

0

1

2M

GPCM

TECS

Tiempo[s]0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

-0.5

0

0.5 NGPC

NTECS

FIGURE 4. MOMENTOS DE LOS MOTORES

Tiempo[s]0 1 2 3 4 5 6 7 8 9 10

Empu

je tot

al[N]

0

5

10

15

20

25

30

35

Empuje total GPCEmpuje total TECS

FIGURE 5. EMPUJE TOTAL DE LOS MOTORES

operar en presencia de restricciones como lo muestra este ex-perimento. Aunque en el caso restringido se requiere cierto es-fuerzo computacional es posible acelerar el proceso de calculoutilizando lenguajes de programacion mas eficientes y utilizandoesquemas de control predictivo con tiempos de muestreo masgrandes como se usa en otros enfoques de control predictivo.

Desplazamiento en x [m]-1 -0.5 0 0.5 1

Des

plaz

amie

nto

en y

[m]

-1

-0.5

0

0.5

1Desplazamiento xy

FIGURE 6. MOVIMIENTO RESTRINGIDO

CONCLUSIONEn este trabajo se ha propuesto un controlador para el

seguimiento de trayectorias utilizando la combinacion de los en-foques de control geometrico y control predictivo generalizado.El comportamiento presentado en las simulaciones numericasmuestra que el seguimiento de las trayectorias de referencia serealiza con tiempos de asentamiento mas cortos respecto al en-foque TECS y tiene la capacidad de incluir restricciones sin in-estabilizar el cuatrirotor. Ademas el controlador propuesto es

susceptible de ser implementado en tiempo real lo cual quedacomo trabajo futuro.

REFERENCES[1] A, Zulu and S. John 2014 A Review of Control Algorithms

for Autonomous Quadrotors. Open Journal of Applied Sci-ences, Vol 4, pp. 547-556

[2] M. Neunert and C. de Crousaz and F. Furrer and M. Kameland F. Farshidian and R. Siegwart and J. Buchli 2016 FastNonlinear Model Predictive Control for Unified TrajectoryOptimization and Tracking. IEEE/RSJ International Con-ference on Robotics and Automation (ICRA), May. Stock-holm, Sweden, pp. 1-7

[3] A. Bemporad and C. A. Pascucci and C. Rocchi 2009 Hi-erarchical and Hybrid Model Predictive Control of Quad-copter Air Vehicles. 3rd IFAC Conference on Analysis andDesign of Hybrid Systems, September 16-18 Zaragoza,Spain, pp. 14-19

[4] A. Bemporad and C. Rocchi 2011 Decentralized LinearTime-Varying Model Predictive Control of a Formation ofUnmanned Aerial Vehicles. 50th IEEE Conference on Deci-sion and Control and European Control Conference (CDC-ECC), December 12-15 Orlando, FL, USA, pp. 7488-7493

[5] Y. Rochefort and S. Bertrand H. and Piet-Lahanier andD. Beauvois and D. Dumur 2012 Cooperative NonlinearModel Predictive Control for Flocks of Vehicles. EmbeddedGuidance, Navigation and Control in Aerospace (EGNCA),February 13-15. Bangalore, India, pp. 1-6

[6] M. W. Mueller and R. D’Andrea 2013 A model predictivecontroller for quadrocopter state interception. EuropeanControl Conference (ECC), July 17-19. Zurich, Switzer-land, pp. 1383 - 1389

[7] Z. Weihua and H. Go and Tiauw 2013 Quadcopter forma-tion flight control combining MPC and robust feedback lin-earization. Journal of the Franklin Institute, Vol. 351, pp.1335–1355

[8] M. Mejari and A. Guptaand N.M.Singh and F. Kazi 201510TH International Conference on Soft Computing Modelsin Industrial and Enviromental Applications. Springer In-ternational Publishing, Switzerland.

[9] L. Taeyoung and M. Leok and N. H. McClamroch 2010Geometric Tracking Control of a Quadrotor UAV on SE(3).49th IEEE Conference on Decision and Control, December.Atlanta, GA USA, pp. 5420-5425

[10] M. A. Vasquez Beltran 2015 Tesis: Seguimiento de unareferencia visual en un plano con un cuatrirotor. Cen-tro de Investigacion y de Estudios Avanzados del InstitutoPolitecnico Nacional, D.F., Mexico.

[11] E. F. Camacho and C. Bordons. 2004 Model PredictiveControl. Springer-Verlag, Great Britain.


Documents

CONTROL PREDICTIVO GENERALIZADO APLICADO A UN