Memoria Final

Tesis Doctoral
Ingeniería de Telecomunicación
Trabajo
Control Fin de
predictivo Grado
para un vehículo tipo Segway
Grado en Ingeniería de Tecnologías Industriales
Control predictivo para un vehículo tipo Segway
Dep. Teoría de la Señal y Comunicaciones

Escuela Técnica Superior de Ingeniería
Autor:deVictor
Universidad Sevilla Manuel Villalar Lara
Tutor: Daniel Limón Marruedo
2013
Dep. Ingeniería de Sistemas y Automática

Universidad de Sevilla
Sevilla, 2017
Trabajo Fin de Grado
Grado en Ingeniería de Tecnologías Industriales
Control predictivo para un vehículo tipo Segway
Autor:
Victor Manuel Villalar Lara
Tutor:
Daniel Limón Marruedo
Profesor titular
Dep. de Ingeniería de Sistemas y Automática

Universidad de Sevilla
Sevilla, 2017
iii
Trabajo Fin de Grado: Control predictivo para un vehículo tipo Segway
Autor: Victor Manuel Villalar Lara
Tutor: Daniel Limón Marruedo
El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:
Presidente:
Vocales:
Secretario:
Acuerdan otorgarle la calificación de:
Sevilla, 2017
El Secretario del Tribunal
v
vii
Agradecimientos
Este espacio se lo quiero dedicar a todas aquellas personas que me han acompañado durante estos cuatro años
de carrera. En especial a mi familia, que me han soportado cuando he estado tanto de risas como de agobios, y
han sido los que me han dado esta oportunidad de futuro. También a mis compañeros que me han hecho las
clases mas amenas y han arrimado el hombro cuando lo he necesitado. Y por último y no por ello menos
importante, a mi novia Patri que ha estado a mi lado en todo momento durante la realización de este proyecto.
Además, agradezco a mi tutor, Dani, todo su esfuerzo y dedicación. Sin él este trabajo no hubiese sido ni de
lejos posible.
Mis más sinceros agradecimientos.
Victor Manuel Villalar Lara

Sevilla, 2017
ix
Resumen
El objetivo de este proyecto es el control de un vehículo tipo Segway, usando un control predictivo basado en
modelo (MPC). El cual se implementará y simulará usando la herramienta de cálculo MATLAB®.
Se describirá en profundidad el controlador y los diferentes elementos que se usarán. Entre ellos método
numérico conocido como Runge-Kutta, para el simulador no lineal del sistema, el algoritmo FISTA (Fast
Iterative Shrinkage-Thresholding Algorithm) para la resolución del problema de optimización, el filtro de
Kalman como observador de estados y el SSTO (Steady-State Target Optimization) para obtener los
correspondientes estados de equilibro de referencia en cada iteración.
Por últimos, una serie de simulaciones para validar el controlador y observar su respuesta en diferentes
situaciones.
xi
Abstract
The objective of this thesis is to be able to manage a vehicle of the characteristics of a Segway. In order to
undertake this target we will be using a “model predictive control” (MPC). This controller will be done by using
the mathematical tool MATLAB.
In this essay, the controller will be effusively described as well as the diverse elements used in the controller.
One of the mathematical theories that my essay is going to research is the german method of Runge-Kutta for
the non-lineal system simulator. Also, the algorithm FISTA which stands for Fast iterative Shrinkage-
thresholding algorithm for finding out the optimization problem regarding the Kalman filter as states observer.
Finally, the last element named as SSTO (Steady Target Optimization) to acquire the corresponding points of
equilibrium in each iteration.
In conclusion, there will be a series of simulations to validate the controller and to observe different answers in
several situations.
xiii
Índice
Agradecimientos ix
Resumen xi
Abstract xiii
Índice xiv
Índice de Tablas xvi
Índice de Figuras xviii
Notación xx
1 Introducción 1
2. Descripción y Modelado del Segway 3
Descripción del Sistema: Segway 3
Descrpción física y modelo de primeros principios 4
Modelo lineal 6
Modelo lineal discretizado 7
Simulador no lineal 8
Perturbaciones 9
Límites fisicos del sistema 9
3. Control Lineal Cuadrático (LQR) 11
Introducción al control óptimo 11
Ley de control óptima 12
Control óptimo estocástico 12
LQR con efecto integral 13
4. Control Predictivo Basado en Modelo 17
Control Predictivo Basado en Modelo 17
Modelo Discreto 17
Restricciones 18
Regulador MPC 18
Cancelación de offset 19
Esquema del bucle 20
Componentes del Controlador 21
Observador de estados 21
SSTO 24
Filtro de referencias 25
5. Resolución del problema de control 26
Minimización de la función objetivo 26
Problema de optimización cuadrática 26
Algoritmo FISTA 29
Desarrollo del problema de optimización asociado al control predictivo 31
6. Diseño del MPC 35
Ajuste de parámetros 35
QyR 35
Horizonte de predicción: N 37
Ajuste del filtro de referencias: 𝝆 41
Diseño del observador de estados 42
Ceros de fase no minima 44
7. Aplicación sobre simulador no lineal 47
Validación del control LQR 47
Validación y ajuste del observador 47
Regulación 49
Seguimiento de referencia 51
Rechazo de perturbaciones. 52
Validación del control predictivo 53
Validación y ajuste del observador 53
Regulación en MPC 55
Seguimiento de referencias 56
Rechazo de perturbaciones en MPC 57
8. Conclusiones y Líneas futuras 59
9. índice de Códigos 61
10. Códigos MATLAB 63
11. Referencias 71
12. Índice de Conceptos 75
xv
ÍNDICE DE TABLAS
Tabla 2–1. Parámetros físicos del robot 4

xvii
ÍNDICE DE FIGURAS
Figura 2-1. Segway para transporte humano 3

Figura 2-2. Segway autónomo 4
Figura 2-3. Esquema del sistema con las variables 5
Figura 3-1. LQR con offset 13
Figura 3-2. LQR con efecto integral sin offset 15
Figura 4-1. Esquema general del bucle de control 20
Figura 4-2. Esquema en profundidad del bucle de control 21
Figura 4-3. Rango de la matriz de observabilidad 23
Figura 4-4. Filtrado de escalón a exponencial 25
Figura 5-1. Parábola convexa 27
Figura 5-2. Solución del Ejemplo 5-1 28
Figura 6-1. Respuestas ante cambios en el parámetro 𝑅/𝑄 37
Figura 6-2. Respuestas ante cambios en el parámetro N 41
Figura 6-3. Respuestas ante cambios en el parámetro ρ 42
Figura 6-4. Estimación de estados para diferentes parámetros R 0 / Q 0 44
Figura 6-5. Amortuguación de ceros de fase no mínima 45
Figura 7-1. Ajuste del observador del LQR 49
Figura 7-2. Verificación del observador del LQR 49
Figura 7-3. Respuesta en regulación LQR 51
Figura 7-4. Actuación LQR en regulación 51
Figura 7-5. Seguimiento de referencia con LQR 52
Figura 7-6. Rechazo a perturbación en el centro de gravedad LQR 52
Figura 7-7. Rechazo perturbación fuerza externa LQR 53
Figura 7-8. Ajuste del observador del MPC 54
Figura 7-9. Verificación del observador MPC 54
Figura 7-10. Regulación en MPC 56
Figura 7-11. Seguimiento de referencia MPC 56
Figura 7-12. Rechazo a perturbación en el centro de gravedad MPC 57
Figura 7-13. rechazo a perturbación fuerza externa MPC 58
xix
Notación
∈ Conjugado
∀ Para todo
𝑛
ℝ Valor Real 𝑛-vector
𝑛×𝑚
ℝ Valor Real 𝑛 × 𝑚 matriz
𝑥̇ Derivada temporal de 𝑥
𝑥𝑘 Valor de 𝑥 en el instante de tiempo 𝑘
+
𝑥 Valor de 𝑥 en el siguiente instante de tiempo
s.a. Sujeto a
min Mínimo de
∗
𝑧 Óptimo de 𝑧
𝜆 autovalor
𝑥̂ Valor estimado de 𝑥
: Tal que
< Menor
> Mayor
≤ Menor o igual
≥ Mayor o igual
𝐶𝑇 Transpuesta de 𝐶
ℤ≥0 Conjunto de números enteros no negativos
xxi
1 INTRODUCCIÓN
“Para el logro del triunfo siempre ha sido indispensable pasar

por la senda de los sacrificios”.
- Simón Bolivar -
E
l control predictivo basado en modelo (MPC) se desarrolló a finales de los años setenta, y se ha ido
desarrollando desde entonces. El control predictivo no es una estrategia de control específica, sino que se
trata más bien de un campo muy amplio de métodos de control desarrollados en torno a ciertas ideas
comunes. Estos métodos de diseño conducen a controladores lineales que poseen prácticamente la misma
estructura y presentan suficientes grados de libertad. Las ideas que aparecen en mayor o menor medida en toda
la familia de controladores predictivos son básicamente:
-Uso explícito de un modelo para predecir la salida del proceso en futuros instantes de tiempo (horizonte).
-Cálculo de las señales de control minimizando una cierta función objetivo.
-Estrategia deslizante, de forma que en cada instante el horizonte se va desplazando hacia el futuro, lo que
implica aplicar la primera señal de control en cada instante y desechar el resto, repitiendo el cálculo en cada
instante de muestreo.
-En el caso concreto del MPC, la consideración de restricciones en el diseño, evitando posibles situaciones
indeseadas y resultando un control más preciso en torno al punto de operación.
Esta estrategia de control se usará para el caso concreto del segway, basado en el extendido modelo de péndulo
invertido, característico por ser no lineal, multivariable y con un cero de fase no mínima.
El sistema de péndulo tiene dos puntos de equilibrio, uno estable y otro inestable. En este caso buscaremos el
equilibrio en torno al punto inestable, actuando sobre la aceleración de las ruedas del segway.
Una vez consiga equilibrarse, pondremos a prueba que sea capaz de seguir una referencia en velocidad y que
sea capaz mantener el equilibrio si hacemos una variación en el centro de gravedad del robot.
Todo esto será a modo de simulación, para lo cual usaremos la herramienta MATLAB®. Una potente
herramienta de cálculo útil en muchos aspectos del control automático, tanto en el diseño como en la simulación
de sistemas.
1
2. DESCRIPCIÓN Y MODELADO DEL SEGWAY
“Los ordenadores son inútiles. Sólo pueden darte respuestas”.

-Pablo Picasso-
Descripción del Sistema: Segway

Un segway es un vehículo de transporte ligero, que hace uso del giróscopo para mantenerse erguido, eléctrico
de dos ruedas. Fue inventado en 2001 por Dean Kamen, y fue el primer vehículo autoequilibrado. El controlador
y los motores de las ruedas mantienen el equilibrio del robot.
Figura 2-1. Segway para transporte humano

En el caso que aquí nos atañe, diseñaremos en base a un segway autónomo. Nos basaremos en el segway
diseñado en esta misma escuela, y que ha sido el centro de estudio de varios proyectos anteriores [1].
3
4 Control predictivo para un vehículo tipo Segway
Figura 2-2. Segway autónomo

El sistema del segway, conocido como péndulo invertido, es un sistema de control que típico por ser muy
completo en el sentido de que es no lineal, multivariable, tiene un cero de fase no mínima y además en el caso
que aquí vamos a estudiar, una perturbación en la entrada, que como se mostrará posteriormente, será la variación
del centro de gravedad del robot. Tal y como se ha mencionado, el punto de equilibrio buscado es inestable y se
dará cuando el balance de momentos sobre el segway sea nulo.
Descripción física y modelo de primeros principios
Para obtener el modelo no lineal vamos a usar las ecuaciones de Lagrange. Tendremos las siguientes
consideraciones:
- No existe deslizamiento en las ruedas.
- Se desprecia los efectos de la fricción.
- Los diferentes elementos del robot se consideran masas puntuales.
Los parámetros físicos son:
mr, masa de las ruedas;
R, radio de las ruedas;
M, masa del robot sin ruedas;
L, distancia entre el cetro de la rueda y el centro de gravedad del robot;
g, aceleración de la gravedad.
𝜙0 , ángulo entre el centro de gravedad y el centro geométrico. (desconocido)
Particularmente, los parámetros usados en este documento serán:
Parámetro Valor
𝑚𝑟 0.140 kg
𝑀 0.82 kg
R 0.05 m
Descripción y Modelado del Segway 5
L 0.098 m
g 9.81 m/𝑠 2
Tabla 2–1. Parámetros físicos del robot
Figura 2-3. Esquema del sistema con las variables

Las variables generalizadas de las ecuaciones de Lagrange son 𝜃, ángulo entre el giro de la rueda y la vertical, y
𝜙, ángulo de inclinación del robot.
Ecuación de Lagrange:
𝐿 = 𝑇 − 𝑉 = 𝑇𝑡𝑟𝑎𝑛𝑠 + 𝑇𝑟𝑜𝑡 − 𝑉 (2.1)
Energía cinética de translación:
1 1
𝑇𝑡𝑟𝑎𝑛𝑠 = 2 𝑚𝑟 (𝑥̇ 𝑟2 + 𝑦̇𝑟2 ) · 2 + 2
2
𝑀(𝑥̇ 𝑚 2)
+ 𝑦̇𝑚 (2.2)
La velocidad lineal de la rueda y del centro de gravedad del robot son:
𝑥𝑟 = R · 𝜃 (2.3)
𝑥̇ 𝑟 = R·𝜃̇ (2.4)
𝑦𝑟 = R (2.5)
𝑦̇𝑟 = 0 (2.6)
𝑥𝑚 = 𝑅 · 𝜃 + 𝐿 · sin( 𝜙 + 𝜙0 ) (2.7)
𝑥̇ 𝑚 = 𝑅 · 𝜃̇ + 𝐿 · 𝜙̇ · cos(( 𝜙 + 𝜙0 ) (2.8)
𝑦𝑚 = 𝑅 + 𝐿 · cos( 𝜙 + 𝜙0 ) (2.9)
𝑦̇𝑚 = −𝐿 · 𝜙̇ · sin( 𝜙 + 𝜙0 ) (2.10)
Entonces:
1 1
𝑇𝑡𝑟𝑎𝑛𝑠 = (𝑚𝑟 + 𝑀) · 𝑅 2 𝜃̇ 2 + 𝑀𝐿2 𝜙̇ 2 + 𝑀𝑅𝐿𝜃̇ 𝜙̇cos( 𝜙 + 𝜙0 ) (2.11)
2 2
Energía cinética de rotación:

1 1
𝑇𝑟𝑜𝑡 = 2 𝑚𝑟 𝑅 2 𝜃̇ 2 + 2 𝑀𝐿2 𝜙̇ 2 (2.12)
La energía potencial es:
𝑉 = 𝑀𝑔𝐿cos( 𝜙 + 𝜙0 ) (2.13)
Quedando como resultado:
1 1 1
𝐿 = (𝑚𝑟 + 2 𝑀) 𝑅 2 𝜃̇ 2 + 2 𝑚𝑟 𝑅 2 𝜃̇ 2+2 𝑀𝐿2 𝜙̇ 2 + 𝑀𝑅𝐿𝜃̇ 𝜙̇cos( 𝜙 + 𝜙0 ) − 𝑀𝑔𝐿cos( 𝜙 + 𝜙0 ) (2.14)
Muestra la ecuación de Lagrange particularizada en las coordenadas generalizadas basadas en el par motor
externo:
𝑑 𝜕𝐿 𝜕𝐿
− =𝑇
𝑑𝑡 𝜕𝜃̇ 𝜕𝜃
(2.15)
𝑑 𝜕𝐿 𝜕𝐿
− =𝑇
̇
𝑑𝑡 𝜕𝜙 𝜕𝜙
(2.16)
De (2.27), (2.28) y (2.29) se obtiene la siguiente ecuación:
(2𝑎 + 𝑐 cos 𝜙)𝜃̈ + (𝑐 cos𝜙 + 2𝑏)𝜙̈ − 𝑐𝜙̇ sin(𝜙 + 𝜙0 ) − 𝑑 sin(𝜙 + 𝜙0 ) = 0 (2.17)
Donde:
a= (𝑚𝑟 + 𝑀)𝑅 2
b= 𝑀𝐿2
c= 𝑀𝑅𝐿
d=−𝑀𝑔𝐿
Modelo lineal
Para diseñar controladores se va a desarrollar un modelo lineal que describa la dinámica del péndulo en torno al
punto de equilibrio en el que se desea estabilizar.
El objetivo del controlador será mantener el péndulo en una posición de equilibrio, que usando la ecuación
(2.17), igualando todas las derivadas igual a 0, puesto a que en el punto de equilibrio el estado del sistema
permanece constante, obtenemos:
sin(𝜙 + 𝜙0 ) = 0
Por lo que
𝜙𝑒𝑞 = 0 − 𝜙0
ó
𝜙𝑒𝑞 = 𝜋 − 𝜙0
Buscamos el primer caso, que es la posición vertical del péndulo.
Considerando pequeños ángulos, 𝜙 << 1 entonces
sin(𝜙 + 𝜙0 ) ≃ (𝜙 + 𝜙0 ); cos( 𝜙 + 𝜙0 ) ≃ 1
La nueva ecuación simplificada es:
(2𝑎 + 𝑐)𝜃̈ + (𝑐 + 2𝑏)𝜙̈ − 𝑐( 𝜙 + 𝜙0 )𝜙̇ − 𝑑( 𝜙 + 𝜙0 ) = 0 (2.19)
En el punto de equilibrio:
𝜙̇𝑒𝑞 = 0; (2.20)
La ecuación linealizada:
(2𝑎 + 𝑐)𝜃̈ + (𝑐 + 2𝑏)𝜙̈ − 𝑑( 𝜙 + 𝜙0 ) = 0 (2.21)
Asumiendo que
e = 2a + c
f = c + 2b
En consecuencia, el sistema descrito en espacio de estados será:
𝜙̇ 0 1 0 𝜙 0 0
−𝑑 ̇ −𝑒⁄ ̈ −𝑑
̈
[𝜙 ] = [ ⁄𝑓 0 0 ] [𝜙 ] + [ 𝑓 ] 𝜃 + [ ⁄𝑓] 𝜙0 (2.22)
𝜃̈ 0 0 0 𝜃̇ 1 0
Como aclaración, identificar las anteriores matrices:
𝑥̇ = 𝐴𝑥 + 𝐵𝑢 + 𝐵𝑑 𝑑 (2.23)
𝑛 𝑚 𝑛𝑑
Identificamos 𝑥 ∈ ℝ con n=3, 𝑢 ∈ ℝ con m=1, 𝑑 ∈ ℝ con 𝑛𝑑 = 1. Tomamos 𝜙0 como perturbación a
la entrada puesto a que genera un par en el segway al igual que 𝜃̈.
La salida la dividiremos en dos, por un lado, la salida medible
𝑦𝑚 = 𝐶𝑥𝑘 (2.24)
Con C=I. Y por otro lado tendremos la salida controlable
𝑦𝑐 = 𝐻𝑥𝑘 (2.25)
Con 𝐻 = [0 0 1]. Es decir, como controlable solo tendremos la última componente. Si observamos las ecuaciones,
y pensamos en el sistema que tenemos delante, es la única componente libre, a la cual podemos pasar una
referencia, las demás quedan fijadas por el punto de equilibrio, 𝑥̇ = 0; . Observemos el sistema de ecuaciones
(2.34):
𝜙̇ = 0
𝜙̈ = 0 = −𝑑⁄𝑓 ( 𝜙 + 𝜙0 ) − 𝑒⁄𝑓 𝜃̈ (2.26)
𝜃̈ = 0
Por lo que observamos, en el punto de equilibrio todas las componentes del estado quedan determinadas, excepto
𝜃̇. Tenemos, por tanto:
−𝜙𝑑
𝑥𝑜 = [ 0 ] ; 𝑢𝑜 = 0 ; 𝑑 = 𝜙𝑑 (2.27)
𝜃̇𝑟
Siendo 𝑥𝑜 el estado en el punto de equilibrio, 𝑢𝑜 la entrada en el punto de equilibrio, 𝜙𝑑 será el ángulo del
segway en el punto de equilibrio, que debe ser igual y de signo contrario al ángulo del centro de gravedad, y 𝜃̇𝑟
la velocidad de la rueda en el punto de equilibrio que queda libre, pudiendo tomar un valor indeterminado,
conocido como offset, que debemos evitar y llevar dicha componente a la referencia, ya veremos cómo.
Modelo lineal discretizado
El Sistema de control se va a implementar en un microcontrolador por lo que resulta necesario obtener un modelo
de control en tiempo discreto para poder diseñar un controlador adecuado.
El cambio consiste en pasar
𝑥̇ (𝑡) = 𝐴𝑥(𝑡) + 𝐵𝑢(𝑡) → 𝑥𝑘+1 = 𝐴𝑥𝑘 + 𝐵𝑢𝑘 +𝐵𝑑 𝑑𝑘
Por lo que el tiempo quedará definido de esta manera
𝑡 = 𝑘 ∗ 𝑇𝑚
Donde k ∈ ℤ≥0 el número muestra, y 𝑇𝑚 tiempo de muestreo.
Para este paso usaremos los siguientes comandos de MATLAB®:
sys=ss(A,B,C,D);
sysd=c2d(sys,Tm);
sys sería el sistema continuo y sysd el sistema discreto. La función que realiza el cambio es c2d(sistema continuo,
tiempo de muestreo). Usa el método Zero-order hold (por defecto), que supone que las entradas son constantes
durante el tiempo de muestreo Tm.
Ahora expondremos lo que obtendríamos en nuestro caso particular, con 𝑇𝑚 = 0.02 (20 ms):
0 1 0 1.008 0.0201 0
𝐴 = (39.9 0 0) 𝐴 = (0.7997 1.008 0)
0 0 0 0 0 1
0 −7.2𝑒 − 05
Sistema continuo 𝐵 = (−0.36) ⟶ Sistema discreto 𝐵 = ( −0.0072 )
1 0.02
0 0.008
𝐵𝑑 = (39.9) 𝐵𝑑 = (0.7997)
{ 0 { 0
El tiempo de muestreo ha sido seleccionado en base a un microcontrolador en el cual se ejecuta el bucle de
control de forma cíclica con un periodo de 50 Hz.
C y D no tienen ninguna variación.
Simulador no lineal
Para que el comportamiento simulado sea lo más fiel posible con el sistema real, debemos usar el modelo no
lineal.
Para este simulador hemos usado el método numérico conocido como Runge-Kutta clásico de orden 4. Un
procedimiento de integración numérica para resolver ecuaciones diferenciales a partir de un valor inicial dado,
y un [t0 ,tf]. El método parte de la igualdad
𝑡 𝑡
𝑦(𝑡𝑘+1 ) = 𝑦(𝑡𝑘 ) + ∫𝑡 𝑘+1 𝑦 ′ (𝑡)𝑑𝑡 = 𝑦(𝑡𝑘 ) + ∫𝑡 𝑘+1 𝑓(𝑡, 𝑦(𝑡))𝑑𝑡 (2.28)
𝑘 𝑘
Aproximan la última integral por alguna fórmula de cuadratura, cambian adecuadamente los valores 𝑦(𝑡𝑘 ) por
los 𝑦k, finalmente, generan la correspondiente recurrencia numérica para determinar dichos valores 𝑦k.
Por ejemplo, el método de Euler es un método particular de los denominados métodos Runge-Kutta explícitos
que se obtiene al estimar la integral mediante la conocida regla del rectángulo, que aproxima el integrando por
su valor en el extremo inferior del intervalo de integración.
𝑡 𝑡
∫𝑡 𝑘+1 𝑓(𝑡, 𝑦(𝑡))𝑑𝑡 ≈ ∫𝑡 𝑘+1 𝑓(𝑡𝑘 , 𝑦(𝑡𝑘 ))𝑑𝑡 = 𝑓(𝑡𝑘 , 𝑦(𝑡𝑘 ))(𝑡𝑘+1 − 𝑡𝑘 ) ≈ 𝑓(𝑡𝑘 , 𝑦𝑘 )ℎ𝑘 (2.29)
𝑘 𝑘
Siendo ℎ𝑘 el paso de integración, si todos los ℎ𝑘 son iguales se dice que el mallado es de paso fijo, como lo será
en nuestro caso (𝑛 es el número de particiones del tiempo de muestreo):
𝑡𝑓 − 𝑡0
ℎ= (2.30)
𝑛
Por tanto, denotando 𝑓𝑘 = 𝑓(𝑡𝑘 , 𝑦𝑘 ), el método de Euler se define como
𝑦𝑘+1 = 𝑦𝑘 + ℎ𝑓𝑘 , 𝑘 = 0,1 … 𝑛 − 1. (2.31)
Una vez textualizado el método, presentaremos el que usaremos que será de cuatro etapas:
1 2 2 1
𝑦𝑘+1 = 𝑦𝑘 + ℎ ( 𝑘1 + 𝑘2 + 𝑘3 + 𝑘4 )
6 6 6 6
𝑘1 = 𝑓𝑘 = 𝑓(𝑡𝑘 , 𝑦𝑘 )
ℎ ℎ (2.32)
𝑘2 = 𝑓 (𝑡𝑘 + 2 , 𝑦𝑘 + 2 𝑘1 )
ℎ ℎ
𝑘3 = 𝑓 (𝑡𝑘 + , 𝑦𝑘 + 𝑘2 )
2 2
ℎ ℎ
{ 𝑘4 = 𝑓 (𝑡𝑘 + ,𝑦 + 𝑘3 )
2 𝑘 2
La diferencia entre el método de Euler y el Runge-Kutta de cuatro etapas está en el error de integración, que
tiene la siguiente forma:
E= 𝐶ℎ k (2.33)
Siendo 𝐶 una constante real positiva, k es lo que se conoce como grado del método y ℎ el paso de integración.
El método de Euler es de grado 0, por lo que el error es de la forma e≤ 𝐶ℎ. Por otro lado, el método que usaremos
es de grado 4, por lo que el error sería, e≤ 𝐶ℎ4. Y por poner un orden de magnitud, usaremos un paso de
integración ℎ = 0.005.
Perturbaciones
En el simulador no lineal vamos a incluir dos tipos de perturbaciones. En primer lugar, un tipo de perturbación
que vendría dada por un cambio en el centro de gravedad del robot, que podría venir dado por colocar algún
objeto sobre él, lo que provocaría un par constante. O también podría venir dado por una mala medida de los
parámetros físicos del robot.
Luego, existiría otro tipo de perturbación que sería equivalente a una fuerza externa aplicada sobre el péndulo,
lo que se traduciría como un par momentáneo.
Para modelar estos efectos debemos añadir algunos términos al simulador no lineal. Para el primer tipo lo
haremos de dos formas, una variando el ángulo del centro de gravedad que anteriormente hemos denotado como
𝜙0 . Este tipo de perturbación, la tenemos que considerar una perturbación a la entrada, puesto a que provocaría
un par en el segway, es decir, una aceleración (como nuestra entrada del sistema). Este tipo de perturbación tiene
una forma particular de ser solventada, como ya veremos, ya que de no ser tomada de esta forma provocaría un
offset en la velocidad de las ruedas.
Para el caso de un mal modelado, añadiremos un cierto 𝜀 a alguno de los parámetros conocidos, por ejemplo, la
longitud del segway.
Para el segundo tipo de perturbación, el caso en el que algo o alguien aplique una fuerza sobre el segway. Esto
lo veremos como un término añadido en 𝜃̈ durante un instante de tiempo.
Si el controlador completo, incluyendo el observador, está bien diseñado compensará las perturbaciones y
además lo hará sin dejar ningún tipo de offset.
Límites fisicos del sistema

En este apartado describiremos cuales son los límites físicos y operativos del sistema, que posteriormente en el
MPC usaremos como restricciones.
En primer lugar, tenemos los límites del estado, que serán unos límites operativos más que físicos.
Tomaremos el límite de ángulo de inclinación en 30°, dado a que incluso en ese ángulo ya estamos muy lejos
del punto de linealización, y el modelo linealizado se va dispersando más del modelo real. Realmente el límite
físico sería el suelo, es decir, 90 °.
Para el máximo de variación de inclinación tomaremos 4 rad/s que es una variación que necesita ya bastante
esfuerzo del motor para ser compensada.
Y por último va velocidad de giro máxima que podrán alcanzar las ruedas será 12 rad/s, puesto a que más toparía
con los límites del motor y con menos sería imposible de estabilizar el sistema en algunas ocasiones.
𝜋 𝜋
−30 · 𝑟𝑎𝑑 𝜙 30 · 𝑟𝑎𝑑
180 ̇ 180
[ −4 𝑟𝑎𝑑/𝑠 ] ≤ [𝜙] ≤ [ 4 𝑟𝑎𝑑/𝑠 ]
−12 𝑟𝑎𝑑/𝑠 𝜃̇ 12 𝑟𝑎𝑑/𝑠
En segundo lugar, tenemos el límite de par que puede dar el motor, que sí que sería un límite físico del sistema.
Para ello nos hemos fijado en el modelo usado en un proyecto con Segway de esta escuela (Figura 2-2), del
cual, hemos extraído un valor experimental de la aceleración máxima que es capaz de desarrollar los motores de
las ruedas de aproximadamente 150 𝑟𝑎𝑑/𝑠 2 . Por lo que ese será el valor que usemos para la restricción de
actuación.
−150 ≤ 𝑢 ≤ 150 𝑟𝑎𝑑/𝑠 2
3. CONTROL LINEAL CUADRÁTICO (LQR)
E
n primera instancia vamos a controlar el sistema objetivo con un controlador óptimo, del cual
comentaremos sus principios teóricos en los que nos basaremos para su diseño. Y posteriormente lo
compararemos con nuestro controlador predictivo.
Introducción al control óptimo

El fin del control óptimo es el diseño de una ley de control que logre el mejor desempeño posible del sistema
𝑢(𝑘) = ℎ(𝑥(𝑘))
Una forma de medir la bondad de un controlador es definiendo una función de coste o índice de desempeño para
un ensayo determinado. Sea 𝑥(𝑘) la trayectoria del Sistema en bucle cerrado
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘)
Partiendo de 𝑥(0) = 𝑥0 y con 𝑢(𝑘) = ℎ(𝑥(𝑘)).
A cada pareja (𝑥(𝑘), 𝑢(𝑘)) se le asocial una función de coste 𝐿(𝑥, 𝑢) > 0 que mide la distancia al punto de
equilibrio (0 ,0).
El índice de desempeño será el coste de la trayectoria, que mide la bondad de la trayectoria, a menor coste mejor.
∞
𝐽(𝑥0 ) = ∑ 𝐿(𝑥(𝑘), 𝑢 (𝑘))

𝑘=0
Este índice nos permite definir una ley de control óptima:

∞
min 𝐽( 𝑥0 , ℎ) = ∑ 𝐿 (𝑥(𝑘), ℎ(𝑥(𝑘)))

𝑘=0
Basándonos en el principio de optimalidad de Bellman: la trayectoria óptima y el coste óptimo cumplen que:
𝐽∗ (𝑥(𝑘)) = 𝐿(𝑥(𝑘), 𝑢 (𝑘)) + 𝐽∗ (𝑥(𝑘 + 1))
La estabilidad del sistema se asegura si existe la ley óptima y 𝐽∗ (𝑥0 ) < ∞, por lo que
lim 𝐿(𝑥(𝑘), 𝑢 (𝑘)) = 0 ⇒ lim 𝑥(𝑘) = 0
𝑘→∞ 𝑘→∞
11
Ley de control óptima

El control óptimo para un sistema lineal con índice de desempeño cuadrático
𝐿(𝑥, 𝑢) = 𝑥 𝑇 𝑄𝑥 + 𝑢𝑇 𝑅𝑢
Siendo 𝑄 y 𝑅 matrices simétricas definidas positivas.
Se puede demostrar que la ley de control es lineal
𝑢 =𝐾·𝑥
Y el coste óptimo es
𝐽∗ (𝑥) = 𝑥 𝑇 𝑃𝑥
Haciendo uso del principio de optimalidad de Bellman antes descrito:
𝐽∗ (𝑥) = 𝑥 𝑇 𝑃𝑥 = 𝑥 𝑇 𝑄𝑥 + 𝑢𝑇 𝑅𝑢 + (𝐴𝑥 + 𝐵𝑢)𝑇 𝑃(𝐴𝑥 + 𝐵𝑢)
= 𝑥 𝑇 𝑄𝑥 + 𝑢𝑇 𝑅𝑢 + 𝑥 𝑇 𝐴𝑇 𝑃𝐴𝑥 + 2𝑥 𝑇 𝐴𝑇 𝑃𝐵𝑢 + 𝑢𝑇 𝐵𝑇 𝑃𝐵𝑢
= 𝑥 𝑇 (𝑄 + 𝐴𝑇 𝑃𝐴)𝑥 + 𝑢𝑇 (𝑅+𝐵𝑇 𝑃𝐵)𝑢 + 2𝑥 𝑇 𝐴𝑇 𝑃𝐵𝑢
La ley óptima será
∇𝑢 𝐽∗ (𝑥) = 0
∇𝑢 𝐽∗ (𝑥) = 2(𝑅+𝐵𝑇 𝑃𝐵)𝑢 + 2𝐵𝑇 𝑃𝐴𝑥 = 0 ⇒ 𝑢 = −(𝑅+𝐵𝑇 𝑃𝐵)−1 𝐵𝑇 𝑃𝐴 · 𝑥 = 𝐾 · 𝑥
Sustituyendo 𝑢 = −(𝑅+𝐵𝑇 𝑃𝐵)−1 𝐵𝑇 𝑃𝐴𝑥 en la ecuación de optimalidad de Bellman se llega a la
ecuación de Riccati, que permite calcula la P óptima y a partir de ella, K.
𝑃 = 𝑄 + 𝐴𝑇 𝑃𝐴 − 𝐴𝑇 𝑃𝐵(𝑅+𝐵𝑇 𝑃𝐵)−1 𝐵𝑇 𝑃𝐴
Control óptimo estocástico

Siendo un Sistema lineal que presenta perturbaciones y ruidos estocásticos
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) + 𝑤(𝑘)
𝑦(𝑘) = 𝐶𝑥(𝑘) + 𝐷𝑢(𝑘) + 𝑣(𝑘)
Donde 𝑤(𝑘) y 𝑣(𝑘) son ruidos blancos de media nula y covarianza 𝐻𝑤 y 𝐻𝑣 respectivamente.
Debemos encontrar un estimador del estado, 𝑥̂(𝑘), que a partir de medidas 𝑦(𝑘), minimice la covarianza del
error de estimación.
𝑇
𝑃 = 𝐸 [(𝑥(𝑡) − 𝑥̂(𝑡)) (𝑥(𝑡) − 𝑥̂(𝑡))]
Para la estimación del estado usaremos un filtro de Kalman en régimen permanente. Los principios teóricos del
filtro de Kalman se encuentran en el apartado 5.2.1.
En el cuál se calcula la ganancia de Kalman 𝐿 fuera de línea, y lo haremos usando la función de MATLAB®
L=dlqr(A',C',Q,R)
Y en línea se estima el estado

𝑥̂𝑘+1 = 𝐴𝑥̂𝑘 + 𝐵𝑢𝑘 + 𝐿 (𝑦𝑘 − 𝐶𝑥̂𝑘 )
Si existe ahora una perturbación tal que el sistema pasa a ser:
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘)+𝐵𝑑 𝑑(𝑘) + 𝑤(𝑘)
Estimaremos la perturbación de la siguiente manera:
Control Lineal Cuadrático (LQR) 13
𝑑̂𝑘+1 = 𝑑̂𝑘 + [−1 0 0] (𝑦𝑘 − 𝐶𝑥̂𝑘 )

Dado a que es de la misma dimensión física que la primera componente del estado, ambos son ángulos.
Haremos algunas pruebas con el modelo lineal discretizado de nuestro sistema para comprobar el control, y la
correcta estimación cuando le añadimos una perturbación.
Usaremos parámetros estándar como 𝑄 = 1 y 𝑅 = 1.
Figura 3-1. LQR con offset

A modo de aclaración, el tiempo se mide en muestras, es decir, una iteración corresponde a 20 ms reales.
Como podemos apreciar al añadir una perturbación constante, del tipo variar el ángulo del centro de gravedad,
el sistema presenta un offset en la velocidad de la rueda, aunque las otras componentes del estado están
estabilizadas. Para solventar este problema añadiremos un integrador al controlador.
LQR con efecto integral

A pesar de que el sistema presenta un integrador de por sí, el control LQR presenta un offset antes perturbaciones
en la entrada, pues éstas también afectan de forma integrada sobre la inclinación del péndulo, añadiendo un cero
al sistema. Por ello es necesario añadir efecto integral.
Si nuestro objetivo es conseguir hacer la diferencia entre el estado y la referencia igual a cero, y cuando tenemos
perturbación en el ángulo del centro de gravedad, nos queda un término constante en una de las componentes
del estado, sería lógico pensar en ir aumentando la acción de control para que vaya disminuyendo esa diferencia,
este es el efecto que tiene añadir un integrador.
Nuestro sistema en términos incrementales será
∆𝑥𝑘+1 = 𝐴∆𝑥𝑘 + 𝐵∆𝑢𝑘 + 𝐵𝑑∆𝑑𝑘
∆𝑥𝑘+1 = 𝑥𝑘+1 − 𝑥𝑘
∆𝑥𝑘 = 𝑥𝑘 − 𝑥𝑘−1
∆𝑢𝑘 = 𝑢𝑘 − 𝑢𝑘−1
∆𝑑𝑘 = 𝑑𝑘 − 𝑑𝑘−1
Para que el error sea nulo la referencia y la salida deben ser iguales. Teniendo en cuenta que como salidas
controlables de nuestro sistema sólo tenemos la tercera componente del estado, 𝑦𝑐 = 𝐻𝑥 , con H= [0 0 1].
𝑒𝑘+1 = 𝑟𝑘+1 − 𝑦𝑘+1

Siendo 𝑦𝑘+1 la salida controlable. Por tanto 𝑒𝑘 ∈ ℝ.
Si se considera que la referencia no cambia, usando los términos incrementales, tenemos que la variación de la
referencia es nula.
∆𝑟𝑘+1 = 𝑟𝑘+1 − 𝑟𝑘 = 0
De las dos ecuaciones anteriores obtenemos
∆𝑒𝑘+1 = ∆𝑟𝑘+1 − ∆𝑦𝑘+1 = −∆𝑦𝑘+1
Si ∆𝑒𝑘+1 = 𝑒𝑘+1 − 𝑒𝑘 , nos queda por tanto:
𝑒𝑘+1 − 𝑒𝑘 = −∆𝑦𝑘+1
Ya que buscamos la fórmula del error, tomamos
𝑒𝑘 = 𝑒𝑘−1 − ∆𝑦𝑘
Quedando como nuevo sistema completo
∆𝑥𝑘+1 𝐴 0 ∆𝑥𝑘 𝐵
[ ]=[ ][ ] + [ ] ∆𝑢𝑘
𝑒𝑘 −𝐻 1 𝑒𝑘−1 0
Una vez obtenido el nuevo sistema, diseñaremos el controlador, cuya fórmula es:
∆𝑥
∆𝑢𝑘 = 𝐾 [ 𝑘 ] = 𝐾𝑥 ∆𝑥𝑘 + 𝐾𝑒 𝑒𝑘−1
𝑒𝑘−1
Donde 𝐾𝑥 es la contante del controlador LQR anterior, y 𝐾𝑒 la constante que multiplica al error.
Para hallar la nueva ley de control sustituiremos 𝑢𝑘 hasta hayar el patron que nos dicha ley.
Sustituiremos en la formula con 𝑘 = 1.
𝑢1 − 𝑢0 = 𝐾𝑥 (𝑥1 − 𝑥0 ) + 𝐾𝑒 𝑒0
𝑢1 = 𝑢0 + 𝐾𝑥 (𝑥1 − 𝑥0 ) + 𝐾𝑒 𝑒0
De nuevo con 𝑘 = 2.
𝑢2 = 𝑢1 + 𝐾𝑥 (𝑥2 − 𝑥1 ) + 𝐾𝑒 𝑒1
Sustituyendo 𝑢1 por la ecuación anterior
𝑢2 = 𝑢0 + 𝐾𝑥 (𝑥1 − 𝑥0 ) + 𝐾𝑒 𝑒0 + 𝐾𝑥 (𝑥2 − 𝑥1 ) + 𝐾𝑒 𝑒1
Ordenando un poco y reagrupando términos obtenemos
𝑢2 = 𝑢0 + 𝐾𝑥 (𝑥2 − 𝑥0 ) + 𝐾𝑒 (𝑒0 + 𝑒1 )
Llegado a este punto podemos deducir la ley de control, cuando 𝑘 = 𝑛:
𝑛−1
𝑢𝑛 = 𝑢0 + 𝐾𝑥 (𝑥𝑛 − 𝑥0 ) + 𝐾𝑒 ∑ 𝑒𝑘
𝑘=0
Para calcular 𝐾 usaremos la misma función de MATLAB® que en el apartado anterior, pero con el nuevo
sistema ampliado con el error, así obtendremos 𝐾𝑥 que valdrá lo mismo de antes si usamos la misma 𝑄 y 𝑅, y
obtendremos 𝐾𝑒 .
Ahora comprobemos si realmente funciona para eliminar el offset.
Control Lineal Cuadrático (LQR) 15
Figura 3-2. LQR con efecto integral sin offset

Como podemos ver funciona para eliminar el offset, en este caso se ha usado una 𝐾𝑒 con una 𝑄 = 10000 y
𝑅 = 1, debemos ser consecuentes en su diseño puesto que podría volver inestable el sistema, 𝐾𝑥 tiene el mismo
valor que en el apartado anterior.
4. CONTROL PREDICTIVO BASADO EN
MODELO
E
n este apartado se hará una introducción a los principios teóricos y matemáticos del control predictivo
basado en modelo.
Control Predictivo Basado en Modelo

A raíz de lo visto anteriormente respecto al MPC. Lo más importante es el modelo del sistema a controlar, que
se usará para predecir su comportamiento y optimizar dicha predicción para tomar la mejor decisiónϵ control a
tiempo. Como toda acción de control óptimo depende del estado inicial del sistema. Además, el MPC tiene en
cuenta las acciones y estados pasados para una mejor predicción y optimización de la señal de control.
Ahora presentaremos el modelo dinámico en forma de ecuaciones de estados:
𝑑𝑥
= 𝑓(𝑥, 𝑢, 𝑡)
𝑑𝑡
𝑦 = ℎ(𝑥, 𝑢, 𝑡) (4.1)
𝑥(t0) = 𝑥 0
Donde 𝑥 ∈ ℝ𝑛 es el estado, 𝑢 ∈ ℝ𝑚 es la entrada, 𝑦 ∈ ℝ𝑝 es la salida y 𝑡 ∈ ℝ es el tiempo. 𝑥 0 será la
condición inicial, es decir, el estado en el instante inicial t0 , que por lo general tomaremos a 0, y con la que
buscaremos solución a la ecuación diferencial.
El modelo lineal expresado en ecuaciones de estado:
𝑑𝑥
= 𝐴𝑥 + 𝐵𝑢
𝑑𝑡
𝑦 = 𝐶𝑥 + 𝐷𝑢 (4.2)
𝑥(0) = 𝑥 0
Donde A ∈ ℝ𝑛×𝑛 es la matriz de transición de estado, B ∈ ℝ𝑛×𝑚 es la matriz de entrada, C ∈ ℝ𝑝×𝑛 es la
matriz de salida y D ∈ ℝ𝑝×𝑚 es la matriz que relaciona u y y. En muchos casos, como el nuestro, D = 0. Además,
destacar que A, B, C, D son constantes en el tiempo.
Modelo Discreto
De cara a un sistema real a controlar mediante MPC, se hará de forma discreta. Si el sistema discreto es diseñado
17
correctamente para el tiempo de muestreo elegido, nos aseguramos de que no se pierde información entre
muestras.
El modelo en espacio de estados discreto será lineal, dimensionalmente finito e invariante en el tiempo:
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘)
𝑦(𝑘) = 𝐶𝑥(𝑘) + 𝐷𝑢(𝑘) (4.3)
𝑥(0) = 𝑥 0
Donde k ∈ ℤ≥0 el número muestra, que se relaciona con el tiempo de forma t = k · Tm con Tm como tiempo de
muestreo. ℤ≥0 denota el conjunto de números enteros no negativos.
Normalmente se denota el número de muestra como subíndice:
𝑥𝑘+1 = 𝐴𝑥𝑘 + 𝐵𝑢𝑘
𝑦𝑘 = 𝐶𝑥𝑘 + 𝐷𝑢𝑘 (4.4)
𝑥0
Para reducir la complejidad de la notación reescribiremos la ecuación (4.4) de la siguiente forma:
𝑥 + = 𝐴𝑥 + 𝐵𝑢
𝑦 = 𝐶𝑥 + 𝐷𝑢 (4.5)
𝑥0
De esta forma denotamos la siguiente muestra con el superíndice +, para hacer más simple la explicación
matemática del MPC.
Restricciones
Las entradas manipulables de casi todos los sistemas físicos están acotadas. De esta forma presentamos los
límites de dichas entradas:
𝑈𝐵𝑢 ≤ 𝑢𝑘 ≤ 𝐿𝐵𝑢 𝑘 ∈ ℤ≥0 (4.6)
De la misma manera, en muchas ocasiones, por razones internas o externas al sistema, es necesario imponer
límites en estados o salidas:
𝑈𝐵𝑥 ≤ 𝑥𝑘 ≤ 𝐿𝐵𝑥 𝑘 ∈ ℤ≥0 (4.7)
De este modo
𝐸𝑢𝑘 ≤ 𝑒 𝑘 ∈ ℤ≥0 (4.8)
Donde
𝐼 𝑈𝐵𝑢
𝐸=[ ] 𝑒= [ ] (4.9)
−𝐼 −𝐿𝐵𝑢
Debemos tener en cuenta una diferencia entre las restricciones a la entrada y restricciones en los estados. Las
restricciones en la entrada representan límites físicos, que si no se cumplen en el controlador el sistema físico se
encarga de hacerlas cumplir. Por otra parte, las restricciones en estados son deseables y su cumplimiento a veces
no es posible. Es el MPC el encargado de calcular en tiempo real si las restricciones son alcanzables o no, y
optimizar si no queda más remedio.
Regulador MPC
Vamos a diseñar en principio un controlador para llevar un sistema lineal y determinista al origen. El modelo
del sistema sería:
𝑥 + = 𝐴𝑥 + 𝐵𝑢
𝑦 = 𝐶𝑥 (4.10)
Asumimos que en principio el estado es medido, lo que quiere decir, 𝐶 = 𝐼. Usaremos este modelo para predecir
los N estados futuros dado un conjunto de entradas u

𝐮 = {𝑢(0), 𝑢(1), … , 𝑢(𝑁 − 1)} (4.11)
Ahora definiremos una función objetivo V (·) para medir la deviación de la trayectoria de x(k), u(k) al cero
mediante la suma cuadrática con diferentes pesos:
𝑉(𝑥(0), 𝐮) = ∑𝑁−1 ′ ′
𝑘=0 [𝑥(𝑘) 𝑄𝑥(𝑘) + 𝑢(𝑘)𝑅𝑢(𝑘)] + 𝑥(𝑁) 𝑃𝑓 𝑥(𝑁) (4.12)
Sujeto a
𝑥 + = 𝐴𝑥 + 𝐵𝑢
La función objetivo depende de la secuencia de entradas y del vector de estados. El estado inicial está disponible
por medición. El resto de estados, x(k), k=1, …, N, está determinado por el modelo y la secuencia de entradas 𝐮.
Los parámetros de ajuste del controlador son las matrices Q y R. En el caso de la diferencia del estado final y el
origen tiene una matriz de peso diferente, 𝑃𝑓 , que en nuestro caso será nula.
Elegir los valores de Q y R no es trivial, y es donde reside la dificultad para los técnicos de la industria del MPC.
Un valor de Q grande en comparación con R significa un intento de llevar el estado al origen rápidamente
expensas de grandes acciones de control. De forma contraria, si R es mayor que Q se reduce la acción de control
y el estado se acercará al origen más lentamente.
Se formula así el problema de control óptimo
min V(x(0),u) (4.13)
Las matrices Q y R serán diagonales y definidas positivas para garantizar que la solución al problema de control
óptimo existe y es única.
Cancelación de offset
Como se ha apreciado en la sección anterior, con la prueba del LQR, vemos que la perturbación en el ángulo del
centro de gravedad nos produce un offset en la salida. En el caso del MPC no es tan sencillo de solventar como
añadir un integrador en el LQR.
La cancelación del offset en el sistema de control predictivo ha sido una de las principales contribuciones de este
estudio.
Para ello se ha utilizado la técnica del modelo de perturbaciones, pero no en su forma estándar, pues no cancela
su contribución al sistema en forma de offset. Se ha estudiado la naturaleza de éstas y se ha extendido el modelo
de perturbaciones para lograr dicha cancelación
El espacio de estados tiene una forma tal que así
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘)+𝐵𝑑 𝑑(𝑘)
𝑦(𝑘) = 𝐶𝑥(𝑘)+𝐶𝑑 𝑑(𝑘) (4.14)
𝑛𝑑 𝑛×𝑛𝑑 𝑝×𝑛𝑑
Donde 𝑑 ∈ ℝ , 𝐵𝑑 ∈ ℝ y 𝐶𝑑 ∈ ℝ en nuestro caso será nula, pero la tendremos en cuenta en los
siguientes cálculos para hacerlo en un caso más general.
El estimador debe ser diseñado basado en el un estado extendido de la siguiente forma
𝑥̂(𝑘 + 1) 𝐴 𝐵𝑑 𝑥̂(𝑘) 𝐵 𝐿
[̂ ]=[ ] [ ̂ ] + [ ] 𝑢(𝑘) + [ 𝑥 ] (−𝑦(𝑘) + 𝐶𝑥̂(𝑘)+𝐶𝑑 𝑑̂(𝑘))
𝑑(𝑘 + 1) 0 𝐼 𝑑(𝑘) 0 𝐿 𝑑
Donde 𝐿𝑥 y 𝐿𝑑 son elegidos de manera que el estimador sea estable.

Por estabilidad, el observador no tiene polos en (1, 0) y por tanto
𝐴 − 𝐼 + 𝐿𝑥 𝐶 𝐵𝑑 + 𝐿𝑥 𝐶𝑑
det ([ ]) ≠ 0
𝐿𝑑 𝐶 𝐿𝑑 𝐶𝑑
El espacio de estados del observador en un instante en el infinito sería tal que así
𝐴−𝐼 𝐵 𝑥̂∞ −𝐵𝑑 𝑑̂∞
[ ][ ] = [ ]
𝐶 0 𝑢∞ 𝑦∞ − 𝐶𝑑 𝑑̂∞
La estimación converge al real si y solo si 𝐿𝑑 (−𝑦∞ + 𝐶𝑑 𝑑̂∞ + 𝐶𝑥̂∞ ) = 0 y dado que 𝐿𝑑 es de rango completo
por filas, implica que
−𝑦∞ + 𝐶𝑑 𝑑̂∞ + 𝐶𝑥̂∞ = 0
En cuanto al diseño del MPC en sí tendremos que tener en cuenta la perturbación tanto en la identificación del
problema a resolver
𝑁−1
min ∑(𝑥(𝑘) − 𝑥𝑟 )𝑇 𝑄(𝑥(𝑘) − 𝑥𝑟 ) + (𝑢(𝑘)−𝑢𝑟 )𝑇 𝑅(𝑢(𝑘) − 𝑢𝑟 )

𝑢
𝑘=0
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) +𝐵𝑑 𝑑(𝑘)

𝑑(𝑘 + 1) = 𝑑(𝑘)
𝑥(0) = 𝑥0
𝐿𝐵𝑥 ≤ 𝑥 ≤ 𝑈𝐵𝑥
𝐿𝐵𝑢 ≤ 𝑢 ≤ 𝑈𝐵𝑢
Como a la hora de calcular los puntos de equilibrio (𝑥𝑟 , 𝑢𝑟 ) en el SSTO, resustantes de la ecuación
𝐴−𝐼 𝐵 𝑥𝑟 −𝐵𝑑 𝑑̂
[ ] [𝑢 ] = [ ]
𝐻𝐶 0 𝑟 𝑟 − 𝐻𝐶𝑑 𝑑̂
Esquema del bucle

En la Figura 4-1. Se muestra el esquema de lo que sería el controlador y la planta o sistema a controlar. Esta
imagen nos sirve para ver gráficamente los parámetros que se le pasan previamente al controlador.
Figura 4-1. Esquema general del bucle de control

En primer lugar, y como en cualquier controlador, hay que pasarle una referencia r. Además de los parámetros
Q, R y N, previamente definidos en la sección 4.2. 𝑅0 y 𝑄0 serán parámetros para el estimador de estados, donde
𝑅0 implica el peso del modelo y 𝑄0 el peso de las medidas tomadas por los sensores. Por último, 𝜌 (𝑟ℎ𝑜) que
sarán parámetros para el filtro de suavizado de referencia. A modo de aclaración, los parámetros modificables
una vez comience el bucle de control, son la referencia, 𝑅0 y 𝑄0 y el parámetro 𝜌 , los demás parámetros no son
modificables.
Componentes del Controlador
Figura 4-2. Esquema en profundidad del bucle de control

En la Figura 4-2, se puede apreciar el interior del controlador expuesto anteriormente, que corresponde al
recuadro en rojo.
•OBS: Este bloque corresponde al observador o estimador de estados. El cual tiene como entradas la
actuación u que proviene del bloque MPC, y la salida del sistema y. Las cuales usará para dar una estimación
del estado que llamaremos 𝑥̂ y de la perturbación 𝑑̂𝑟 .
•Filtro: La función de este bloque será suavizar el cambio de referencia para que no sea un cambio muy
brusco y evitar la posible pérdida de factibilidad del controlador predictivo.
•SSTO (Steady-State Target Optimization): dada una referencia este bloque calcula el punto de
equilibrio o en caso de ser una referencia no alcanzable, calcula el punto optimo 𝑥𝑟 y 𝑢𝑟 .
•MPC: teniendo como entradas el punto de equilibrio obtenido en el bloque SSTO y la estimación del
estado 𝑥̂ procedente del observador, este bloque resuelve el problema expuesto en el apartado 3 de optimización
para minimizar la función objetivo y devolver una actuación u.
Observador de estados
Es muy normal que en un sistema algunas de las variables de estado no estén accesibles para ser medidas y sea
necesaria una estimación, o incluso siendo posible la medida puede ser necesaria su estimación.
Para ello se ha utilizado el bien conocido, filtro de Kalman. Sus principales características son.
•Estimador óptimo del estado de un sistema. Proporciona la estimación del estado más probable.
•Filtra el ruido de las medidas. Mejora y suaviza las medidas con ruido sin introducir retraso.
•Es recursivo. Obtiene una nueva estimación usando sólo la última medida. Puede usarse para
aplicaciones en tiempo real.
Si obtenemos el estado y la salida de una ecuación de la siguiente forma:
𝑥𝑘+1 = 𝐴𝑥𝑘 + 𝐵𝑢𝑘 +𝐵𝑑 𝑑𝑘 (4.15)
𝑑𝑘+1 = 𝑑𝑘
𝑦𝑘 = 𝐶𝑥𝑘
El observador tendrá una forma parecida tal que así:
𝑥̂𝑘 ∆𝑥
𝑥̂𝑘+1 =⏞
𝐴𝑥̂𝑘 + 𝐵𝑢𝑘 +𝐵𝑑 𝑑̂𝑘 + ⏞
𝐿 (𝑦𝑘 − 𝐶𝑥̂𝑘 ) (4.16)
Extendiendo el estado más la perturbación para la estimación tendremos el sistema tal que así
𝑥̂𝑘+1 𝐴 𝐵𝑑 𝑥̂𝑘 𝐵 𝐿
[̂ ]=[ ] [ ] + [ ] 𝑢𝑘 + [ 𝑥 ] (−𝑦𝑘 + 𝐶𝑥̂𝑘 )
𝑑𝑘+1 0 𝐼 𝑑̂𝑘 0 𝐿𝑑
Donde 𝑥̂𝑘 es el estado estimado del instante actual, 𝑢𝑘 conocida por haber sido calculada previamente, y 𝐿 es
una matriz de ganancias del observador. El observador es estable siempre y cuando la matriz (𝐴 − 𝐿 𝐶) tenga
todos sus autovalores 𝜆𝑖 en el círculo unidad, es decir, se cumpla 𝐿 ∶ |𝜆𝑖 (𝐴 − 𝐿 C)| < 1.
Si ahora suponemos que también hay ruido en la salida:
𝑥𝑘+1 = 𝐴𝑥𝑘 + 𝐵𝑢𝑘 +𝐵𝑑 𝑑𝑘 + 𝑤 (4.17)
𝑦𝑘 = 𝐶𝑥𝑘 + 𝑣
Siendo 𝑤 errores en el modelado y 𝑣 ruido en los sensores. No se puede resolver de forma algebraica, hay que
resolver un problema de optimización, y esto es lo que hizo Kalman. Siendo la solución:
∆𝑥 = 𝑃𝐶 𝑇 (𝐶𝑃𝐶 𝑇 + 𝑅)−1 (𝑦𝑘 − 𝐶𝑥̂𝑘 ) (4.18)
Respecto al ruido, se supone que es ruido blanco Gaussiano, estacionario y con media cero.
𝐸[𝑣(𝑡)] = 𝐸[𝑤(𝑡)] = 0
𝐸[𝑣(𝑡)𝑣(𝜏)𝑇 ] = 𝑅 𝛿(𝑡 − 𝜏) (4.19)
𝐸[𝑤(𝑡)𝑤(𝜏)𝑇 ] = 𝑄 𝛿(𝑡 − 𝜏)
Donde 𝑅 es la matriz de covarianza de los sensores y 𝑄 matriz de covarianza del modelo.
El filtro de Kalman genera una estimación del estado que minimiza el error cuadrático medio:
𝑇
𝐸 [(𝑥(𝑡) − 𝑥̂(𝑡)) (𝑥(𝑡) − 𝑥̂(𝑡))] (4.20)
La estimación optima se obtiene igualando a cero la derivada de 𝑃. Quedando como condición necesaria:
𝐸[((𝑥(𝑡) − 𝑥̂(𝑡))𝑦(𝑡)𝑇 )] = 0 (4.21)
Con lo que se puede sacar la expresión del observador (𝐿), que en esta aplicación se suele denominar 𝐾, matriz
de ganancias de Kalman.
Las ecuaciones del filtro de Kalman:
Ganancia de Kalman:
𝐾(𝑡) = 𝑃(𝑡)𝐶 𝑇 𝑅 −1 (4.22)
Donde 𝑃(𝑡) es la mariz de covarianza del error
𝑇
𝑃 = 𝐸 [(𝑥(𝑡) − 𝑥̂(𝑡)) (𝑥(𝑡) − 𝑥̂(𝑡))] (4.23)
Que se obtiene de la Ecuación de Ricatti

𝑃̇(𝑡) = 𝑃(𝑡)𝐴𝑇 + 𝐴𝑃(𝑡) + 𝑄 − 𝑃(𝑡)𝐶 𝑇 𝑅 −1 𝐶𝑃(𝑡) (4.24)
De la que una solución sub-óptima, que tiende al óptimo, pero fácil de calcular:
0 = 𝑃(𝑡)𝐴𝑇 + 𝐴𝑃(𝑡) + 𝑄 − 𝑃(𝑡)𝐶 𝑇 𝑅 −1 𝐶𝑃(𝑡) (4.25)
Por tanto, tendríamos nuestro observador, tal que, partiendo de las salidas medibles y la actuación calcula una
estimación del estado de la siguiente forma:
Con una primera fase de predicción:
𝑥𝑘− = 𝐴𝑥̂𝑘−1 + 𝐵𝑢𝑘−1 +𝐵𝑑 𝑑̂𝑘−1 (4.26)
𝑃𝑘− 𝑇
= 𝐴𝑃𝑘−1 𝐴 + 𝑄 (4.27)
Y una segunda fase de estimación:
𝐾𝑘 = 𝑃𝑘− 𝐶 𝑇 (𝐶𝑃𝑘− 𝐶 𝑇 + 𝑅)−1 (4.28)
𝑥̂𝑘 = 𝑥𝑘− + 𝐾𝑘 (𝑦𝑘 − 𝐶𝑥𝑘− ) (4.29)
𝑃𝑘 = (𝐼 − 𝐾𝑘 𝐶)𝑃𝑘− (4.30)
Siendo necesario conocer el estado inicial 𝑥0 y su covarianza 𝑃0 .
Para el observador denotaremos 𝑅 y 𝑄, como 𝑅0 y 𝑄0 para no confundirnos con las del controlador.
Para la estimación de 𝑑̂𝑘 usaremos el filtro de Kalman en régimen permanente dado que podemos tomar
𝑑𝑘+1 = 𝑑𝑘
Así que directamente
𝑑̂𝑘+1 = 𝑑̂𝑘 + 𝐿𝑑 (𝑦𝑘 − 𝐶𝑥̂𝑘 )
Tomaremos 𝐿𝑑 = [−1 0 0 ] que comprobado experimentalmente toma una estimación bastante acertada, ya
que tanto 𝑑 como 𝑥 son de la misma dimensión física, y están relacionadas como hemos visto en la descripción
del sistema.
Observabilidad
La observabilidad es esencial en el control de sistemas con variables de estado inaccesibles.

Considerando un sistema:
𝑥((𝑘 + 1)𝑇) = 𝐺𝑥(𝑘𝑇)

𝑦(𝑘𝑇) = 𝐶𝑥(𝑘𝑇) (4.31)
Se sabe que la solución de la ecuación es:
𝑥(𝑘𝑇) = 𝐺 𝑘 𝑥(0) (4.32)
Y de ahí
𝑦(𝑘𝑇) = 𝐶𝐺 𝑘 𝑥(0) (4.33)
La observabilidad completa implica que usando:
𝑦(0), 𝑦(𝑇), 𝑦(2𝑇), … , 𝑦((𝑛 − 1)𝑇) (4.34)
Se pueden determinar
𝑥1 (0), 𝑥2 (0), … , 𝑥𝑛 (0) (4.35)
Donde 𝑥𝑖 (0) indica la componente iésima de 𝑥(0). Dicho de otra forma, el sistema es completamente
observable si
𝑦(0) = 𝐶𝑥(0)
𝑦(𝑇) = 𝐶𝐺 𝑥(0)
⋮
𝑦((𝑛 − 1)𝑇) = 𝐶𝐺 𝑛−1 𝑥(0) (4.36)
nos permiten determinar 𝑥1 (0), 𝑥2 (0), … , 𝑥𝑛 (0). Teniendo en cuenta que 𝑦(𝑘𝑇) es un vector de m
componentes, el sistema de ecuaciones anterior tiene dimensión 𝑛 × 𝑚, donde las incognitas son las 𝑛
componentes de 𝑥(0). Para que la solución del sistema sea única, debe haber 𝑛 ecuaciones linealmente
independientes, dándonos la siguiente condición de observabilidad:
𝑅𝑎𝑛𝑔𝑜 {[𝐶 ∗ ⋮ 𝐺 ∗ 𝐶 ∗ ⋮ ⋯ ⋮ (𝐺 ∗ )𝑛−1 𝐶 ∗ ]} = 𝑛 (4.37)
∗
Donde indica la conjugada traspuesta de una matriz y a la matriz que aparece en el a condición se le llama
matriz de observabilidad.
Comprobamos que nuestro sistema es completamente observable usando el siguiente comando de MATLAB®:
Figura 4-3. Rango de la matriz de observabilidad

Al ser el rango igual al número de variables del sistema, significa que es completamente observable.
SSTO
Partiendo de una referencia en salida, r, para las salidas controlables introducidas en el apartado (2.3.2), tal que;
𝑥 + = 𝐴𝑥𝑟 + 𝐵𝑢𝑟 +𝐵𝑑 𝑑

𝑟 = 𝐻𝐶𝑥 (4.38)
Obtendríamos el sistema:
𝐴 − 𝐼 𝐵 𝑥𝑟 −𝐵 𝑑
[ ][ ] = [ 𝑑 ] (4.39)
𝐻𝐶 0 𝑢𝑟 𝑟
Podría ser ahora que r no fuera una referencia alcanzable, por lo que el SSTO debería encontrar un punto de
equilibro lo más próximo posible al de referencia, usando la siguiente estrategia:
𝑥𝑟 𝐿
[𝑢 ] = [ 𝑥 ] 𝜆 (4.40)
𝑟 𝐿𝑢
Con 𝜆 ∈ (0,1) y siendo
𝐿𝑥 𝐴−𝐼 𝐵 −𝐵 𝑑
[ ]=[ ] \[ 𝑑 ] (4.41)
𝐿𝑢 𝐻𝐶 0 𝑟
Se debe cumplir que
𝐿𝐵𝑥 ≤ 𝜆𝐿𝑥 ≤ 𝑈𝐵𝑥
𝐿𝐵𝑢 ≤ 𝜆𝐿𝑢 ≤ 𝑈𝐵𝑢 (4.42)
Definiendo ahora
𝐿𝑥
𝐿
𝑐=[ 𝑢 ] (4.43)
−𝐿𝑥
−𝐿𝑢
𝑈𝐵𝑥
𝑑 = [ 𝑈𝐵𝑢 ] (4.44)
−𝐿𝐵𝑥
−𝐿𝐵𝑢
La desigualdad de restricción estará expresada tal que
𝜆𝑐 ≤ 𝑑 (4.45)
Y se debe cumplir para todas sus componentes:
𝜆𝑐𝑖 ≤ 𝑑𝑖 ∀𝑖 (4.46)
Cuando esta condición no se cumpla debemos calcular
𝑑𝑖
𝜆𝑖 ≤ (4.47)
𝑐𝑖
De manera que
𝑑𝑖
𝜆 = min (4.48)
𝑖 ∈ 𝑐𝑖 >𝑑𝑖 𝑐𝑖
Filtro de referencias
Si se conoce la referencia, se puede conseguir una sensible mejora en el control si modificamos un cambio brusco
en éstas por un cambio más suave y progresivo. Para ello usaremos un filtro de primer orden que un escalón de
referencia se transforme en una referencia filtrada exponencial. De la siguiente forma:
̂ 𝑘+1 = (1 − 𝛼)𝑤
𝑤 ̂ 𝑘 + 𝛼𝑤 (4.49)
Figura 4-4. Filtrado de escalón a exponencial

Siendo 𝛼 un parámetro entre 0 y 1, que influye en el filtrado como se muestra en la Figura 4-4.
Que pasado a nuestra nomenclatura quedará:
𝑟_𝑓𝑖𝑙𝑡𝑟𝑎𝑑𝑎 = (1 − 𝜌)𝑟 + 𝜌𝑟 (4.50)
5. RESOLUCIÓN DEL PROBLEMA DE CONTROL
E n este capítulo se mostrará con detalle el problema de minimización de la función objetivo mediante la
resolución de un problema de optimización cuadrática.
Minimización de la función objetivo

Con el objetivo de minimizar la función objetivo del MPC, vamos usar un algoritmo que lo haga de forma rápida
y eficiente. Pero antes, expondremos como se abordará el problema de la optimización cuadrática.
Problema de optimización cuadrática
Por optimización matemática entendemos la búsqueda de la mejor solución posible del problema. En ocasiones,
la mejor solución, es decir, la que minimice o maximice una cierta función puede no estar disponible, ya sea por
restricciones o por sobrepasar el tiempo de computo.
En nuestro caso nos enfrentamos al problema conocido como programación cuadrática. El cual se caracteriza
por tener una función objetivo cuadrática y restricciones lineales.
La formulación del problema quedaría como:
1
min 𝑧 𝑇 𝐻𝑧 + 𝑓 𝑇 𝑧 (3.1)
𝑧 2
𝑠. 𝑎. 𝐴𝑧 ≤ 𝑏
Donde 𝑓 ∈ ℝ𝑛 ; H ∈ ℝ𝑛𝑥𝑛 simétrica; A ∈ ℝ𝑚𝑥𝑛 y b ∈ ℝ𝑚 . Por lo tanto, el problema es de n variables y m
restricciones.
Con el objetivo de facilitar la comprensión del método que usaremos para resolver el problema de optimización
cuadrática, vamos a poner una serie de ejemplos y los principios matemáticos que vamos a usar.
1
En primer lugar, debemos saber que una función cuadrática de la forma 2 𝑑𝑥 2 + 𝑓𝑥 + 𝑐 será convexa si 𝑑 > 0.
Y una función convexa es aquella que su derivada segunda es mayor o igual a 0 en todos sus puntos.
26
Resolución del problema de control 27
Ejemplo 5–1. Sea z un número real, calcular la solución 𝑧 ∗
1
min 𝑑𝑧 2 + 𝑓𝑧
𝑧 2
𝑠. 𝑎. 𝑎 ≤ 𝑧 ≤ 𝑏
Siendo a < b y d > 0.
La función para la que buscamos una solución es una parábola cuya ecuación es de la forma:
1
y=2 𝑑(𝑧 − 𝑧0 )2 (3.2)
Figura 5-1. Parábola convexa

Se puede observar en la Figura 5-1, que la z que hace mínima la parábola será 𝑧0 si es que está comprendida
entre a y b. Si no es así, tomará el valor más cercano de la forma
𝑆𝑖 𝑎 ≤ 𝑧0 ≤ 𝑏 → 𝑧 ∗ = 𝑧0
𝑆𝑖 𝑧0 > 𝑏 → 𝑧 ∗ = 𝑏 (3.3)
𝑆𝑖 𝑧0 < 𝑎 → 𝑧 ∗ = 𝑎
Es decir, la solución tendría forma de rampa con saturación superior e inferior tal como muestra la Figura 5-2.
Figura 5-2. Solución del Ejemplo 5-1

Desarrollando la ecuación (3.3):
1 1
𝑦 = 2 𝑑𝑧 2 − 𝑑𝑧0 𝑧 + 2 𝑑𝑧02 (3.4)
Eliminando los términos que no dependen de z, ya que lo que nos interesa es la solución mínima, llegamos a:
1
𝑦 = 2 𝑑𝑧 2 − 𝑑𝑧0 𝑧 (3.5)
Por último, igualamos (3.5) a la ecuación de nuestro problema (3.2), y obtenemos el resultado:
1 1
𝑑𝑧 2 − 𝑑𝑧0 𝑧 = 𝑑𝑧 2 + 𝑓𝑧 (3.6)
2 2
𝑓
𝑧 ∗ = 𝑧0 = − (3.7)
𝑑
Ejemplo 5–2. Sea z ∈ ℝ𝑛 , calcular la solución 𝑧 ∗

1
min 𝑧 𝑇 𝐻𝑧 + 𝑓 𝑇 𝑧
𝑧 2
𝑠. 𝑎. 𝑎𝑖 ≤ 𝑧𝑖 ≤ 𝑏𝑖 𝑖 = 1, … , 𝑛
Siendo 𝑎𝑖 < 𝑏𝑖 y H ∈ ℝ𝑛 × 𝑛 diagonal con sus elementos 𝑑𝑖𝑖 > 0.
Este problema de nuevo tiene una solución similar al ejemplo anterior. Desarrollando (3.8):
1 2 1
𝑧 𝑑 + 𝑓𝑖 𝑧1 + ⋯ + 𝑧𝑛2 𝑑𝑛𝑛 + 𝑓𝑛 𝑧𝑛 (3.8)
2 1 11 2
De esta forma podemos separar este problema en n problemas distintos, cada uno similar que el Ejemplo 3-1.
Cuyas soluciones serán
𝑆𝑖 𝑎𝑖 ≤ 𝑧0𝑖 ≤ 𝑏𝑖 → 𝑧𝑖∗ = 𝑧0𝑖
𝑆𝑖 𝑧0𝑖 > 𝑏𝑖 → 𝑧𝑖∗ = 𝑏𝑖
𝑆𝑖 𝑧0𝑖 < 𝑎𝑖 → 𝑧𝑖∗ = 𝑎𝑖
Donde
𝑓𝑖
𝑧𝑖∗ = 𝑧0𝑖 = − 𝑖 = 1, … , 𝑛 (3.9)
dii

1
𝑧 2
𝑠. 𝑎. |𝑧𝑖 | ≤ 𝑎𝑖 𝑖 = 1, … , 𝑛
Siendo 𝑎𝑖 >0 y H ∈ ℝ𝑛 × 𝑛 definida positiva.
Para resolver este ejemplo vamos a introducir el algoritmo FISTA, el cual usaremos en nuestro programa.
Algoritmo FISTA
El algoritmo FISTA (Fast Iterative Shrinkage-Thresholding Algorithm), es un algoritmo de minimización
mediante umbral que se usa para resolver problemas de optimización convexa para funciones de la forma:
min 𝑔1 (𝑥) + 𝑔2 (𝑥) + ⋯ + 𝑔𝑛−1 (𝑥)+𝑔𝑛 (𝑥) (3.10)
𝑥∈ ℝ𝑛
Donde 𝑔1 , ..., 𝑔𝑛 son funciones convexas desde 𝑔 ∶ ℝ𝑛 → ℝ de las cuales una o más pueden ser no
diferenciables. Por lo que haremos unas modificaciones matemáticas al problema. Siendo la función de coste:
1
𝑔(𝑧) = 2 𝑧 𝑇 𝐻𝑧 + 𝑓 𝑇 𝑧 (3.11)
Y sea:
𝐿 = 𝜆𝑚𝑎𝑥 (𝐻) (3.12)
Cumple que:
𝑧 𝑇 𝐻𝑧 ≤ 𝐿𝑧 𝑇 𝑧 ∀𝑧 (3.13)
Derivando una aproximación local de 𝑔(𝑧) en torno a un punto 𝑧𝑘 obtenemos:
1
𝑔(𝑧𝑘 + Δ𝑧) = (𝑧𝑘 + Δ𝑧)𝑇 𝐻(𝑧𝑘 + Δ𝑧) + 𝑓 𝑇 (𝑧𝑘 + Δ𝑧)
2
1 1
= Δ𝑧 𝑇 𝐻Δ𝑧 + 𝑧𝑘𝑇 𝐻Δ𝑧+ 𝑧𝑘𝑇 𝐻𝑧𝑘 + 𝑓 𝑇 𝑧𝑘 + 𝑓 𝑇 Δ𝑧
2 2
1 𝑇
= Δ𝑧 𝐻Δ𝑧 + (𝐻𝑧𝑘 + 𝑓)𝑇 Δ𝑧 + 𝑔(𝑧𝑘 )
2
Usando la condición (3.13):
1
𝑔(𝑧𝑘 + Δ𝑧) ≤ 2 𝐿Δ𝑧 𝑇 Δ𝑧+(𝐻𝑧𝑘 + 𝑓)𝑇 Δ𝑧 + 𝑔(𝑧𝑘 ) (3.14)
El algoritmo FISTA denota el punto de linealización como 𝑦𝑘 , distinto de la solución. Expuesto en forma de
receta hará lo siguiente:
1.inicializamos
𝑘←1
𝑧0 ← 0
𝑦1 ← 0
𝑡1 ← 1
FIN ← NO
2. Hacer hasta FIN = SI
a. Calcular Δ𝑦 tal que
1
min 2
𝐿Δ𝑦 𝑇 Δ𝑦+(𝐻𝑦𝑘 + 𝑓)𝑇 Δ𝑦
Δ𝑦
𝑠. 𝑎. 𝑎𝑖 ≤ 𝑦𝑘𝑖 +Δ𝑦𝑖 ≤ 𝑏𝑖 𝑖 = 1, … , 𝑛
b. 𝑧𝑘 ← 𝑦𝑘 + Δ𝑦
1
c. 𝑡𝑘+1 ← (1 + √1 + 4𝑡𝑘2 )
2
𝑡
d. 𝑦𝑘+1 ← 𝑧𝑘 + 𝑡𝑘−1 (𝑧𝑘 − 𝑧𝑘−1 )
𝑘+1
e. 𝑘 ← 𝑘 + 1
f. Si |Δ𝑧𝑖 | ≤ 𝑇𝑂𝐿, entonces FIN ← SI
3. 𝑧 ∗ ← 𝑧𝑘

1
𝑧 2
𝑠. 𝑎. 𝐿𝐵 ≤ 𝑧 ≤ 𝑈𝐵
𝐴𝑧 = 𝑏
Siendo H diagonal con todos sus elementos positivos, A ∈ ℝ𝑚 × 𝑛 con 𝑚 < 𝑛 y [𝐴, 𝑏] 𝑑𝑒 𝑟𝑎𝑛𝑔𝑜 𝑚 (por lo
que las restricciones son linealmente independientes).
Este será el ejemplo que nos interese finalmente, puesto que es lo que tendremos que resolver para minimizar la
función objetivo del MPC.
Para resolver este ejemplo usaremos dualidad. Sea la función de coste:
1
J(z) = 2 𝑧 𝑇 𝐻𝑧 + 𝑓 𝑇 𝑧 (3.15)
Se define la función Lagrangiana del problema como:
L(z, x) = J(z)−𝑥 𝑇 (𝐴𝑧 − 𝑏) (3.16)
Donde x es el multiplicador de Lagrange.
Definiendo la función
𝑧 𝑜 (𝑥) = arg min 𝐿(𝑧, 𝑥) (3.17)
𝐿𝐵≤𝑧≤𝑈𝐵
La función dual será

g(x) = L(𝑧 𝑜 (𝑥), 𝑥) (3.18)
∗ ∗
Por tanto, si 𝑧 es la solución óptima y 𝐽 el coste óptimo,
𝐽∗ = 𝐿(𝑧 ∗ , 𝑥) ≥ 𝑔(𝑥) ∀𝑥 (3.19)
Como consecuencia de que
𝐴𝑧 ∗ − 𝑏 = 0 (3.20)
La función dual g(x) es una cota inferior al problema de optimización. Por lo tanto, debemos buscar la mejor
cota inferior, maximizando g(x), de modo que debemos resolver el problema:
𝑥 ∗ = arg max g(x) (3.21)
𝑥
Si
𝑔∗ = 𝑔(𝑥 ∗ ) (3.22)
Se obtiene que
𝐽∗ ≤ 𝑔∗ (3.23)
Por lo que se concluye que ahora la solución del problema se puede escribir como
1 𝑇
𝑧(𝑥) = arg min 𝑧 𝐻𝑧 + (𝑓 − 𝐴𝑇 𝑥)𝑇 𝑧 (3.24)
𝐿𝐵≤𝑧≤𝑈𝐵 2
Por lo que para resolver este problema de optimización vale con resolver el problema dual, usando el descrito
anteriormente algoritmo FISTA. Basado en la desigualdad:
1
𝑔(𝑥 + Δ𝑥) ≥ 𝑔(𝑥) − Δ𝑥 𝑇 (𝐴𝑧(𝑥) − 𝑏) − 2 Δ𝑥 𝑇 𝑊Δ𝑥 (3.25)
Donde
𝑊 = 𝐴𝐻 −1 𝐴𝑇 (3.26)
Puesto que W es invertible, por ser H definida positiva y A de rango completo, el óptimo del problema se alcanza
cuando 𝑔(𝑥 + Δ𝑥) = 𝑔(𝑥), es decir, se anula el gradiente.
(𝐴𝑧(𝑥) − 𝑏) + 𝑊Δ𝑥 = 0 (3.27)
Despejando
Δ𝑥 = −𝑊 −1 (𝐴𝑧(𝑥) − 𝑏) (3.28)
De nuevo, presentaremos el pseudocódigo del algoritmo FISTA introduciendo algunos cambios para resolver
este problema en particular:
1. inicializamos
𝑘←1
𝑥0 ← 0
𝑦1 ← 0
𝑡1 ← 1
Q ← 𝑊 −1
FIN ← NO
2. Hacer hasta FIN = SI
a. Calcular z(𝑦𝑘 ) tal que
1 𝑇
arg min 𝑧 𝐻𝑧 + (𝑓 − 𝐴𝑇 𝑥)𝑇 𝑧
𝐿𝐵≤𝑧≤𝑈𝐵 2
b. Si |𝐴z(𝑦𝑘 ) − b| ≤ 𝑇𝑂𝐿, entonces FIN ← SI

c. 𝑆𝑖 𝑛𝑜
i. Δ𝑦 ← −𝑄(𝐴𝑧(𝑦𝑘 ) − 𝑏)
ii. 𝑥𝑘 ← 𝑦𝑘 + Δ𝑦
1
iii. 𝑡𝑘+1 ← 2 (1 + √1 + 4𝑡𝑘2 )
𝑡𝑘−1
iv. 𝑦𝑘+1 ← 𝑥𝑘 + (𝑥𝑘 − 𝑥𝑘−1 )
𝑡𝑘+1
v. 𝑘 ← 𝑘 + 1
∗
3. 𝑧 ← z(𝑦𝑘 )
Desarrollo del problema de optimización asociado al control predictivo

En este apartado, vamos a identificar los términos de la función de nuestro problema de control, concretamente
la función objetivo del MPC, para que quede de la misma forma que en los ejemplos del apartado anterior y
podamos resolverlo de la misma manera.
Para empezar, definiremos algunos elementos:

L: Coste
𝐿(𝑥, 𝑢) = 𝑥 𝑇 𝑄𝑥 + 𝑢𝑇 𝑅𝑢 𝑄, 𝑅 > 0 (3.29)
El modelo lineal
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) +𝐵𝑑 𝑤 (3.30)
Restricciones
𝐿𝐵𝑥 ≤ 𝑥 ≤ 𝑈𝐵𝑥 (3.31)
𝐿𝐵𝑢 ≤ 𝑢 ≤ 𝑈𝐵𝑢 (3.32)
Nuestro problema tiene el siguiente planteamiento:
min ∑𝑁−1 𝑇 𝑇
𝑘=0 (𝑥(𝑘) − 𝑥𝑟 ) 𝑄(𝑥(𝑘) − 𝑥𝑟 ) + (𝑢(𝑘)−𝑢𝑟 ) 𝑅(𝑢(𝑘) − 𝑢𝑟 ) (3.33)
𝑢
𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) +𝐵𝑑 𝑤

𝑥(0) = 𝑥0
𝐿𝐵𝑥 ≤ 𝑥 ≤ 𝑈𝐵𝑥
𝐿𝐵𝑢 ≤ 𝑢 ≤ 𝑈𝐵𝑢
Y nuestro objetivo es dejarlo de la siguiente forma:
𝑧
𝑠. 𝑎. 𝐿𝐵𝑧 ≤ 𝑧 ≤ 𝑈𝐵𝑧
𝐸𝑧 = 𝑏
Empezaremos definiendo z que será el vector formado por el estado y las actuaciones para el horizonte de
predicción N.
𝑥(0)
𝑢(0)
𝑥(1)
𝑧= 𝑢(1) (3.34)
⋮
𝑥(𝑁 − 1)
[𝑢(𝑁 − 1)]
Siendo 𝑧 ∈ ℝ𝑁(𝑛+𝑚) .
x(k) tendrá dimensión n y u(k) dimensión m. En nuestro caso particular, y como ya hemos visto anteriormente
n=3 y m=1.
La matriz diagonal H será
𝑄
𝑅
𝐻= 𝑄 (3.35)
𝑅
[ ⋱]
Siendo Q y R también diagonales y positivas. H tendrá dimensión 𝑁(𝑛 + 𝑚).
−𝑄𝑥𝑟
−𝑅𝑢𝑟
−𝑄𝑥𝑟
𝑓 = −𝑅𝑢𝑟 (3.36)
⋮
−𝑄𝑥𝑟
[−𝑅𝑢𝑟 ]
Siendo 𝑓 ∈ ℝ𝑁(𝑛+𝑚) .
Definidos z y H ya tendríamos la función de coste. Ahora abordaremos las restricciones:

𝐿𝐵𝑧 ≤ 𝑧 ≤ 𝑈𝐵𝑧
𝐿𝐵𝑥 𝑥(0) 𝑈𝐵𝑥
𝐿𝐵𝑢 𝑢(0) 𝑈𝐵𝑢
𝐿𝐵𝑥 𝑥(1) 𝑈𝐵𝑥
𝐿𝐵𝑢 ≤ 𝑢(1) ≤ 𝑈𝐵𝑢 (3.37)
⋮ ⋮ ⋮
𝐿𝐵𝑥 𝑥(𝑁 − 1) 𝑈𝐵𝑥
[𝐿𝐵𝑢] [𝑢(𝑁 − 1)] [𝑈𝐵𝑢]
𝐿𝐵𝑥, 𝑈𝐵𝑥, 𝐿𝐵𝑢, 𝑈𝐵𝑢 ∈ ℝ𝑁(𝑛+𝑚) .
Por último, nos faltaría la restricción 𝐸𝑧 = 𝑏.
Identifiquemos el primer termino 𝑥(0) = 𝑥0 :
1 𝑥1 (0) 𝑥1
[ ⋱ ][ ⋮ ] = [ ⋮ ] → 𝐼 𝑥(0) = 𝑥0 (3.38)
1 𝑥𝑛 (0) 𝑥𝑛
Procedamos ahora a identificar los siguientes términos k>0
𝑥(𝑘) −𝐵𝑑 𝑑
[𝐴 𝐵 −𝐼 ] [ 𝑢(𝑘) ] = [−𝐵𝑑 𝑑] → 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) − 𝑥(𝑘 + 1) = −𝐵𝑑 𝑑 (3.39)
𝑥(𝑘 + 1) −𝐵𝑑 𝑑
En el último paso de predicción debemos variar la ecuación anterior puesto a que hemos decidido ver hasta un
cierto estado futuro, quedando:
𝑥(𝑁 − 1)
[𝐴 𝐵] [ ] = [𝑥𝑟 − 𝐵𝑑 𝑑] → 𝐴𝑥(𝑁 − 1) + 𝐵𝑢(𝑁 − 1) + 𝐵𝑑 𝑑 = 𝑥𝑟 (3.40)
𝑢(𝑁 − 1)
Así que ya podemos identificar la restricción 𝐸𝑧 = 𝑏 con:
𝑥(0)
𝐼 𝑢(0) 𝑥0
𝐴 𝐵 −𝐼 𝑥(1) −𝐵𝑑 𝑑
𝐴 𝐵 −𝐼 𝑢(1) = −𝐵𝑑 𝑑 (3.41)
⋱ ⋮ ⋮
[ 𝐴 ]
𝐵 𝑥(𝑁 − 1) [𝑥𝑟 −𝐵 𝑑 𝑑]
[𝑢(𝑁 − 1)]
Siendo 𝐸 ∈ ℝ𝑛(𝑁+1)×𝑁(𝑛+𝑚) , 𝑏 ∈ ℝ𝑛(𝑁+1).
6. DISEÑO DEL MPC
C
omo se dijo previamente, la simulación del sistema y su control se hará mediante la herramienta de
cálculo MATLAB®. En caso de que fuese posible la implementación real, esta herramienta nos sería
útil para ver cómo reacciona el sistema si hacemos cambios en el diseño del controlador, sin poner en
riesgo la integridad del robot. Esto mismo será lo que veamos en esta sección.
Ajuste de parámetros
En este apartado discutiremos sobre resultados la influencia de los parámetros de diseño, nombrados en el
apartado 4.1, sobre el modelo lineal del segway.
En las siguientes simulaciones no haremos uso de las restricciones definidas en la sección 2.4, puesto a que nos
impediría ver ciertos efectos de los parámetros.
QyR
En primer lugar, aclarar que en este apartado y en los sucesivos variaremos la relación 𝑅/𝑄, puesto a que tiene
el mismo efecto sobre el sistema poner 𝑅 = 0.1 y 𝑄 = 10, que poner 𝑅 = 0.1 y 𝑄 = 1.
En nuestro caso 𝑄 en una matriz de dimensión 3x3, que tomará el valor:
10 0 0
𝑄=[0 1 0]
0 0 10
Y para variar la relación 𝑅/𝑄 en las sucesivas pruebas, mantendremos 𝑄 y variaremos R.
El resto de parámetros tendrán valores intermedios, tales como 𝑁 = 20.En este apartado queremos observar la
influencia de 𝑅/𝑄 sobre el sistema por lo que aislaremos su efecto lo máximo posible de los demás parámetros
de diseño. Se usará un problema de regulación en el que se partirá de una posición inicial inestable y deberá
estabilizarse:
𝜙 5 · 𝜋/180
𝑥0 = [𝜙̇ ] = [ 0 ]
𝜃̇ 0
35
Diseño del MPC 37
Figura 6-1. Respuestas ante cambios en el parámetro 𝑅/𝑄

Observamos que para 𝑅 < 𝑄 el sistema es más rápido, pero precisa de un mayor esfuerzo de control, y el valor
máximo de la actuación es notablemente mayor, lo que podría hacer inviable su control en un sistema real. Con
𝑅 = 𝑄 se da una oscilación más pequeña que en el primer caso, pero tarda más en llegar a la referencia. Al
parecer, en este caso, no se obtiene gran diferencia cuando hacemos 𝑄 ≫ 𝑅, en comparación con el segundo
caso. Por último, se observa que este parámetro no es influyente en el tiempo de computo.
Horizonte de predicción: N
En esta sección veremos la influencia del parámetro N, manteniendo constante los demás. Las condiciones serán
las mismas que en el apartado anterior pero ahora 𝑅/𝑄 =1.
Diseño del MPC 39
Diseño del MPC 41
Figura 6-2. Respuestas ante cambios en el parámetro 𝑁

Analizando cada caso, para N=5, el sistema es incapaz de estabilizarse respetando las restricciones y se vuelve
inestable. En caso N=10, la actuación llega hasta el límite puntualmente y es capaz de estabilizarlo. En el caso
de N=20 y N=30 la actuación no llega al límite y es capaz de estabilizarse sin problemas.
En general, podemos deducir que a mayor horizonte de predicción mejor respuesta obtenemos, pero hace
notablemente mayor el tiempo de computo. Aunque como hemos visto anteriormente, cambiando R/Q podemos
modificar la respuesta, pero no conseguiremos bajar el tiempo de computo, algo que es crítico a la hora de pasar
al control real.
Cuando obtenemos por pantalla exceso de bucle se debe a que en la resolución del problema (con el algoritmo
FISTA) no se ha llegado a una cierta distantacia menor o igual a la tolerancia del estado de referencia, en el
número de iteraciones del algoritmo, que en este caso esta programado en 200 iteraciones. Por eso, en el caso de
N=5 es tan grande el tiempo de computo.
Ajuste del filtro de referencias: 𝝆
Las condiciones serán, parte de una posición de equilibrio y le se da un escalón unitario en la referencia, que
como se especificó en la sección 2.1.3, la referencia será en velocidad de las ruedas.
Los demás parámetros valdrán N=20 y 𝑅/𝑄 =1.
Figura 6-3. Respuestas ante cambios en el parámetro 𝜌

Se observa como al ser más brusco el cambio de referencia, se da una mayor sobreoscilación. Y esto ocurre para
el caso de un horizonte de predicción alto, en un horizonte de predicción menor el sistema podría incluso hacerse
inestable.
Diseño del observador de estados
El filtro de Kalman se puede determinar como el dieño de un LQR. Las ponderaciones del LQR considerado
son 𝑄0 y 𝑅0 , que son los parámetros de ajuste.
Para comprobar la bondad del observador debemos suponer un ruido en la lectura de la salida, de esta forma:
𝑦𝑘 = 𝐶𝑥𝑘 + 𝑣
Donde a 𝑣 se daremos un orden de magnitud de 0,2.
En las siguientes pruebas veremos la eficacia del filtro mostrando una trayectoria real de un sistema que nada
tiene que ver con el nuestro, para que sea independiente del resto de parámetros del controlador y podamos
observar con claridad el efecto de los parámetros del observador.
Diseño del MPC 43
Figura 6-4. Estimación de estados para diferentes parámetros 𝑅0 / 𝑄0

Como podemos observar, 𝑅0 y 𝑄0 están relacionadas con la influencia de las medidas o del modelo, a mayor
relación del 𝑅0 / 𝑄0 mayor es la influencia del modelo, y a menor 𝑅0 / 𝑄0 más influencia de los sensores.
Debemos comprobar como de bueno es nuestro modelo y como de buenos nuestros sensores para dar más peso
a uno u a otro. En este caso se ve claramente que los sensores son malos, y el modelo bueno, por lo que debemos
tomar un valor alto de 𝑅0 / 𝑄0 .
Ceros de fase no minima
El sistema del segway, o péndulo invertido, es un sistema de fase no mínima, es decir, tiene ceros en el semiplano
derecho. Presenta una respuesta inversa, por lo que evoluciona en sentido contrario al de la señal de control
durante un cierto tiempo, despues la respuesta recupera el sentido habitual.
Este es un problema difícil de solventar, en nuestro caso, con un MPC se puede mejorar aumentando el horizonte
de predicción, lo que nos llevaría a un mayor coste de ejecución.
Diseño del MPC 45
Figura 6-5. Amortiguación de ceros de fase no mínima

7. APLICACIÓN SOBRE SIMULADOR NO LINEAL
E
n esta última sección veremos todo el bucle de control del MPC funcionando en conjunto sobre el
simulador no lineal diseñado en la sección (2.2), y haremos una comparativa con el control mediante
LQR.
Validación del control LQR

En las siguientes simulaciones vamos a validar el diseño del controlador LQR en las distintas situaciones
posibles. Primero en regulación, que consiste en partir de una posición inestable y controlar el segway hasta la
posición de equilibrio. Posteriormente en seguimiento de referencia, en el cual desde una posición de equilibrio
deberá alcanzar una referencia dada. Y por último veremos el rechazo de perturbaciones, donde veremos como
reacciona el sistema cuando tiene un cambio en el ángulo del centro de gravedad con respecto al centro
geométrico, y si es capaz de eliminar el offset. Y otro tipo de perturbación que equivaldría a ejercer una fuerza
sobre el segway en un instante de tiempo.
Como el control LQR no tiene en cuenta restricciones, los elementos físicos como los motores tienen unos
valores de saturación, por lo que la actuación de debe estar obligatoriamente entre esos valores
−150 ≤ 𝑢 ≤ 150 𝑟𝑎𝑑/𝑠 2
Validación y ajuste del observador
Validaremos el observador del LQR comparando la diferencia entre el estado real y el estimado, en un momento
determinado se produce un cambio de referencia, el sistema se ve afectado por un ruido tal que:
𝑣 = [±0.0015 ± 0.0015 ± 0.0015]
47
Aplicación sobre simulador no lineal 49
Figura 7-1. Ajuste del observador del LQR.

Se observa que el mejor ajuste se da con Ro/Qo=10, ahora validaremos el observador con una prueba en la que
variaremos algunos parámetros físcos del sistema real como:
Radio de las ruedas R=0.05 →R=0.06
Masa del robot M=0.82 → M=0.81
Longitud del centro de gravedad al centro de las ruedas L=0.098 → L=0.097
Figura 7-2. Verificación del observador del LQR

Parece que se obtiene incluso una mejor estimación que en el caso que no variamos los parámetros. Esto puede
ser debido a que el modelo obtenido de la linealización no este del todo correcto. En cualquier caso, se verifica
una estimación saticfactoria por parte del observador utilizado.
Regulación
Ahora vamos a comprobar si el sistema es capaz de alcanzar el equilibrio partiendo de una posición inicial de
10º de inclinación del segway.
Figura 7-3. Respuesta en regulación LQR

La respuesta en regulación parece un poco lenta cuando R/Q =1 por lo que aumentaremos Q para que el sistema
responda más rápido. Como podemos observar con R/Q=100 el sistema es más rápido, pero no termina de
eliminar el error, por lo que pasaremos a ajustar la ganancia del término integral manualmente, llegando a
𝐾𝑒 = 0.7.
Una vez ha sido ajustado el controlador LQR, pasamos observar cuales han sido los valores de actuación y el
tiempo de computo.
Figura 7-4. Actuación LQR en regulación
Como podemos observar, la actuación ha alcanzado el valor límite ,150, donde ha saturado durante un instante.
Seguimiento de referencia
En este apartado veremos si el sistema es capaz de seguir una referencia establecida, en este caso partiendo de
un estado de equilibrio y usando los ajustes realizados en el apartado anterior. Pasaremos a una referencia de
2rad/s y usaremos un filtro de referencias de 𝜌 = 0.1
Figura 7-5. Seguimiento de referencia con LQR.
Podemos observar que el sistema presenta una gran sobreoscilación debido a la alta ganancia que le hemos
puesto. Respecto a los límites, esta vez no alcanza ninguno de ellos.
Rechazo de perturbaciones.
En esta ocasión veremos si es sistema es capaz de compensar el efecto tanto de una perturbación en el ángulo
del centro de gravedad, como una perturbación instantánea en la entrada producida por un par provocado con
una fuerza puntual sobre el segway.
En primer lugar, observaremos la respuesta antes un cambio en el ángulo del centro de gravedad de 15º:
Figura 7-6. Rechazo a perturbación en el centro de gravedad LQR

Como era de esperar, el ángulo final de inclinación del segway alcanza un valor igual y de signo contrario al del
centro de gravedad, quedando así compensado y el segway en equilibrio. Por otro lado, vemos como el segway
supera la velocidad límite establecida en 12 rad/s, ya que no podemos hacer uso de restricciones en este tipo de
control. En esta simulación podemos ver como cuando existe una perturbación de este tipo el observador filtra
mejor el ruido, debido a que el observador puede dar un pequeño valor residual de la perturbación cuando no
existe, pero cuando sí que existe da una estimación muy precisa.
Ahora veremos como responde el sistema ante una fuerza externa que provoca una aceleración de 20 rad/𝑠 2 .
Figura 7-7. Rechazo perturbación fuerza externa LQR
Vemos como el sistema es capaz de recuperarse sin mucha dificultad.
Validación del control predictivo

La principal característica que hace que el control MPC haya triunfado en la industria, es el uso de restricciones.
En las siguientes simulaciones tendremos en cuenta las restricciones definidas en la sección 2.4 tal que el estado
este restringido a:
𝜋 𝜋
−30 · 𝑟𝑎𝑑 𝜙 30 · 𝑟𝑎𝑑
180 ̇ 180
[ −4 𝑟𝑎𝑑/𝑠 ] ≤ [𝜙] ≤ [ 4 𝑟𝑎𝑑/𝑠 ]
−12 𝑟𝑎𝑑/𝑠 𝜃̇ 12 𝑟𝑎𝑑/𝑠
Y la actuación queda restringida a:
−150 ≤ 𝑢 ≤ 150 𝑟𝑎𝑑/𝑠 2
Validaremos el diseño de la sección anterior en las siguientes simulaciones con el sistema no lineal. Como en el
apartado anterior validaremos los resultados poniendo a prueba nuestro sistema en las diferentes situaciones
descritas, usando los valores de los parámetros que mejor resultado nos han ofrecido, teniendo en cuenta el
tiempo de computo.
𝑅/𝑄 =1; N=20; 𝜌 =0.1
Validación y ajuste del observador
En este caso el observador de estados elegido es un filtro de Kalman. El cual se diseña mediante ponderación de
Ro y Qo.
Usaremos un orden de magnitud de ruido de 0.0015 en las medidas de las salidas. Siendo el ruido
𝑣 = [±0.0015 ± 0.0015 ± 0.0015]
Comprobaremos la bondad del observador, comparando los estados reales con los estados estimados. En el
instante 100 daremos un cambio de referencia. Usaremos el mismo valor de los parámetros de ponderación que
en el observador del LQR, Ro/Qo=10.
Figura 7-8. Validación del observador del MPC

Como podemos observar, la diferencia entre el estado real y el estimado tiende a ciertos valores de una dimensión
despreciable, excepto en la primera componente, que tiende a 0.
Ahora validaremos el diseño del observador variando los parámetros reales del segway. Tal que:
Radio de las ruedas R=0.05 →R=0.06
Masa del robot M=0.82 → M=0.81
Longitud del centro de gravedad al centro de las ruedas L=0.098 → L=0.097
Figura 7-9. Verificación del observador MPC

En este caso, existe una diferencia apreciable cuando se da el cambio de referencia, pero consigue volver a hacer
pequeña la diferencia, alcanzando de nuevo un valor residual del orden de las milésimas.
Regulación en MPC
Usaremos las mismas condiciones que en apartado de regulación del LQR para que sean comparables, partiendo
igualmente desde un punto inicial de 10º de inclinación.
Vemos como este controlador consigue alcanzar el punto de equilibrio en menos iteraciones que el LQR y
además no alcanza el valor de saturación del motor, 150 rad/𝑠 2 .
Para que sea comparable el tiempo de cálculo del MPC y el LQR haremos la simulación de nuevo con el mismo
número de puntos usado en ese caso.
Figura 7-10. Regulación en MPC
El tiempo de computo es notablemente mayor, de 0.77 segundos en el LQR a 1.85 segundos en el MPC, más
del doble. Aunque es evidente la mejor respuesta obtenida en el MPC.
Seguimiento de referencias
Igualmente, en esta ocasión, haremos un cambio de referencia de 0 a 2rad/s, usando un filtro de referencias de
𝜌 = 0.1.
Figura 7-11. Seguimiento de referencia MPC

Como se puede observar, el seguimiento de la referencia sería perfecto si no fuese por el cero de fase no mínima,
pero el sistema no presenta sobreoscilación como en el caso del LQR.
Rechazo de perturbaciones en MPC
De nuevo pondremos a prueba el controlador MPC en las mismas condiciones que pusimos al LQR, para el caso
del rechazo a efectos de las perturbaciones.
Empezaremos con la perturbación en el ángulo del centro de gravedad, variándolo en 15º a partir de un instante.
Figura 7-12. Rechazo a perturbación en el centro de gravedad MPC
En este caso, el controlador necesita alcanzar la máxima actuación, pero no sobrepasa la restricción en estado
como ocurría en el LQR, ya que este no tenía restricción en estado.
Por último, probaremos que el sistema sea capaz de estabilizarse cuando sufre una fuerza externa en un
momento determinado. La fuerza implicará en el sistema una aceleración de 20rad/𝑠 2 .
Figura 7-13. Rechazo perturbación fuerza externa MPC

En este caso el sistema tiene más dificultad para recuperarse que en el caso del LQR, siendo las velocidades
alcanzadas de un orden muy superior.
8. CONCLUSIONES Y LÍNEAS FUTURAS
Conclusiones
Se ha diseñado un sistema de control para el segway que garantiza la estabilización del péndulo y seguimiento
de velocidad de avance del segway. El sistema de control está formado por un observador, un filtro de referencias
y un controlador óptimo con capacidad de rechazar las perturbaciones existentes.
Para ello primero se ha desarrollado un modelo realista del segway a partir de los primeros principios y se ha
obtenido un modelo de control para el diseño.
Como controlador se ha diseñado un control LQR y un control LQR con efecto integral para rechazo de
perturbaciones en el centro de gravedad.
También se ha diseñado un control predictivo que incorpora un estimador de perturbaciones para lograr el
rechazo a perturbaciones.
Estos controladores se han ajustado de forma adecuada y se han validado sobre el modelo realista en ensayos de
regulación, seguimiento de referencia y rechazo a perturbaciones.
Los controladores se han implementado en programas que permitirán su fácil implementación en el sistema de
control del segway.
Este trabajo pone de manifiesto la capacidad del sistema de control propuesto y su bondad para controlar el
segway en entornos realistas.
Líneas futuras
Implementación en el sistema de control. Era el objetivo inicial llegar a implementar el controlador en una placa
Arduino para luego ajustar y validar el controlador sobre un sistema real como el de la referencia [1].
Incorporación en una estructura para seguimiento de caminos como, por ejemplo, el método de persecución
pura. En el cual el robot mediante una diferencia de velocidad entre las dos ruedas es capaz de girar para seguir
la trayectoria previamente monitorizada.
59
9. ÍNDICE DE CÓDIGOS
Código MATLAB®: Parametros del modelo del segway..................................................................... 63

Código MATLAB®: Simulador no lineal Runge-Kutta ......................................................................... 63
Código MATLAB®: Modelo no lineal Segway...................................................................................... 64
Código MATLAB®: Controlador LQR. ................................................................................................... 66
Código MATLAB®: Datos Segway para MPC. ..................................................................................... 65
Variables para MPC .............................................................................................................................. 66
Código MATLAB®: Bucle de control MPC. ........................................................................................... 67
Código MATLAB®: SSTO ....................................................................................................................... 68
Código MATLAB®: Actualización de los valores del MPC ................................................................... 68
Código MATLAB®: Algoritmo FISTA ..................................................................................................... 69
Código MATLAB®: saturación .............................................................................................................. 69
61
10. CÓDIGOS MATLAB
Código MATLAB®: Parámetros del modelo del segway.
g=9.81;
mr=0.140; % Masa de cada rueda
R=0.05; % Radiod e la rueda
M=0.82; % Masa del cuerpo del pendulo
L=0.098; % Distancia del centro de gravedad al eje de las ruedas
% Momentos de inercia.
Jr=0.5*mr*R^2;
J=M*L^2;
% Constantes
a=(mr+M/2)*R^2+Jr;
b=M*L^2/2+J/2;
c=M*R*L;
d=-M*g*L;
e=2*a+c;
f=2*b+c;
% Condiciones iniciales:
phi0=5*pi/180; % angulo inical de inclinación del pendulo
dphi0=0; % velodidad inicial de giro del pendulo
vr0=0; % velocidad de giro de las ruedas.
% Matrices del sistema linealizado:
A=[0 1 0; -d/f 0 0; 0 0 0];
B=[0; -e/f; 1];
C=eye(3);
D=[0;0;0];
Ep=[0; -d/f; 0];
%vamos a pasar a un modelo discreto
Tm=0.02; % Tiempo de muestreo
sys=ss(A,B,C,D);
sysd=c2d(sys,Tm);
[Ad,Bd,Cd,Dd]=ssdata(sysd);
Código MATLAB®: Simulador no lineal Runge-Kutta
function [T,Y]=Rungekutta(f,intervalo,y0,h)
t0=intervalo(1);
tf=intervalo(2);
y0=y0(:);
T=(t0:h:tf)';
N=length(T)-1;
Y=ones(N+1,1)*y0';
for k=1:N
k1=f(T(k),y0);
k2=f(T(k)+0.5*h,y0+0.5*h*k1);
k3=f(T(k)+0.5*h,y0+0.5*h*k2);
k4=f(T(k)+h,y0+h*k3);
y1=y0+(h/6)*(k1+2*k2+2*k3+k4);
Y(k+1,:)=y1';
y0=y1;
end
63
Código MATLAB®: Modelo no lineal Segway
function [ dx ] = Mdlsegwconpert(x,u,w,Ep)
g=9.81;
mr=0.140; % Masa de cada rueda
R=0.05; % Radiod e la rueda
M=0.82; % Masa del cuerpo del pendulo
L=0.098; % Distancia del centro de gravedad al eje de las ruedas
Jr=0.5*mr*R^2;
J=M*L^2;
a=(mr+M/2)*R^2+Jr;
b=M*L^2/2+J/2;
c=M*R*L;
d=-M*g*L;
dx=zeros(3,1);
dx(1)=x(2);
dx(2)=(((c*x(2)*sin(x(1)+w))-d*sin(x(1)+w)-u*(2*a+c*cos(x(1))))/(2*b+c*cos(x(1))));
dx(3)=u+Ep;
Código MATLAB®: Controlador LQR.
inic_segway
Q=[1000 0 0;0 1 0;0 0 1000];
R=1;
[Kd,Sd,Ed] = dlqr(Ad,Bd,Q,R);
Ae=[A zeros(3);-eye(3) eye(3)];
Be=[B;zeros(3,1)];
Qe=[10 0 0 0 0 0;0 1 0 0 0 0 ;0 0 10 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 10];
Re=R;
[Ke,Se,Ee] = dlqr(Ae,Be,Qe,Re);
Kee=Ke(4:6);
A=Ad;
B=Bd;
Bd=[0.0080;0.7997;0];
%inicialización
xk=[10*pi/180 0 0]';
xref=[0 0 0]';
u=0;
xt=[];
xt_gorro=[];
xtref=[];
ut=[];
posicion=0;
e=xk;
Kee=[0.0000 0.0000 0.7];
Tsim=10;
umax=0;
usat=150;
x_gorro=xk;
dr_gorro=0;
dr=0;
alpha=0.2;
r_rampa=0;
rho=0.2;
n=3;
p=1;
m=1;
Códigos MATLAB 65
Qo=[10 0 0;0 1 0;0 0 10];

rr=1;
Ro=rr*eye(n);
Lo=dlqr(A',C',Qo,Ro)';
w=0*pi/180;
Ep=0;
tic
for k=1:(Tsim/Tm)
xt=[xt,xk];
ut=[ut,u];
r=xref(3);
u=-(Kd*(x_gorro-xref))-Kee*e;
if u>=usat
u=usat;
end
if u<=-usat
u=-usat;
end
if u>=umax
umax=u;
end
h=0.005;
ruido=[0.0015*rand-0.0015*rand 0.0015*rand-0.0015*rand 0.0015*rand-0.0015*rand]';
[t,y] = Rungekutta(@(t,y) Mdlsegwconpert(y,u,w,Ep),[(k-1)*Tm k*Tm],xk,h);
xk=y(end,:)';
xtref=[xtref,xref];
inxk=xt(end)-xt(end-1);
inref=xtref(end)-xtref(end-1);
e=e+(xref-xk);
xk';
% OBSERVADOR
x_gorroant=x_gorro;
x_gorro=A*x_gorro+B*u+Bd*dr_gorro+Lo(1:n,:)*(C*xk+ruido-C*x_gorro);
dr_gorro=dr_gorro+[-1 0 0]*(C*xk+ruido-C*x_gorro);
xt_gorro=[xt_gorro,x_gorro];
end
toc
Código MATLAB®: Datos Segway para MPC.
global A B Bd C Cd D G n m p LBx UBx LBu UBu Q R b xk E H

% Parametros del modelo del segway.
N=20;
% Condiciones iniciales:
phi0=0*pi/180; % angulo inical de inclinación del pendulo
dphi0=0; % velodidad inicial de giro del pendulo
vr0=0; % velocidad de giro de las ruedas.
%declaracion variables
np=150;
Tm=0.02;
A=[1.00798621711988 0.0200532131547173 0;0.799683012389336 1.00798621711988 0;0 0 1];
B=[-7.21091143109317e-05;-0.00722049411971725;0.0200000000000000];
Bd=[0.0080;0.7997;0];
C=[1 0 0;0 1 0;0 0 1];
G=[0 0 1];
Cd=zeros(n,p);
D=[0;0;0];
LBx=-[30/pi*180 ; 4 ; 12]; % limite superior e inferior de X
UBx=[30/pi*180 ; 4 ; 12];
LBu=-150; % limite superior e inferior de U
UBu=150;
[fil,n]=size(A);
[filb,m]=size(B);
p=n;
xf=zeros(n,np); %estado
y=zeros(p,np); %salida
u=zeros(m,np); %acc. cntrol
xf(1,1)=phi0;
xf(2,1)=dphi0;
xf(3,1)=vr0;
Q=[10 0 0;0 1 0;0 0 10];
R=1;
xk=xf(:,1);
lim=zeros((n+m)*N,1);
for i=1:n+m:(n+m)*N
lim(i:i+n-1)=UBx;
end
for i=n+1:n+m:(n+m)*N
lim(i:i+m-1)=UBu;
end
% %% OBSERVADOR
x_gorro=xk;
dr_gorro=0;
dr=0;
r_rampa=0;
Variables para MPC
H=zeros(N*(n+m));
E=zeros((N+1)*n,(n+m)*N);
I=eye(n);cont=0;
for i=1:2*N
if flag==1
for aux=1:m
H(i+cont+aux-1,i+cont+aux-1)=R(aux,aux);flag=0;
end
cont=cont+m-1;
else
for aux=1:n
H(i+cont+aux-1,i+cont+aux-1)=Q(aux,aux);flag=1;
end
cont=cont+n-1;
end
end
contIn=0;contIm=0;
for i=1:N
if i==1
E(1:n,1:n)=I;
else
E(1+contIn:n+contIn,1+contIn+contIm:n+contIn+contIm)=-I;
end
contIn=contIn+n;
contIm=contIm+m;
Códigos MATLAB 67
end
contAn=0;contAm=0;
for i=1:N
E(n+1+contAn:2*n+contAn,1+contAn+contAm:n+contAn+contAm)=A;
contAn=contAn+n;
contAm=contAm+m;
end
contBn=0;contBm=0;
for i=1:N
E(n+1+contBn:2*n+contBn,n+1+contBn+contBm:n+m+contBn+contBm)=B;
contBn=contBn+n;
contBm=contBm+m;
end
b=zeros((N+1)*n,1);
Código MATLAB®: Bucle de control MPC.
datosSegway;
histy=[xk];u=[0];histu=[u ];histPpred=[ ];histPest=[ ];
histdr=[ ];histref=[ ];histrr=[ ];histgorro=[x_gorro ];
dr_gorro=0;
dr=0;
alpha=1;
r_rampa=0;
rho=.1;
np=500;
Cm=[1 0 0];
r=0;
Ep=0;
Pest = zeros(3,3); % matriz de covarianza estimada
Ppred = zeros(3,3); % matriz de covarianza predicha
Ko = zeros(3,3); % matriz de ganancias de Kalman
tsim=14;
ruido=[0.0015*rand-0.0015*rand 0.0015*rand-0.0015*rand 0.0015*rand-0.0015*rand]';
tic
for k=1:np
h=0.005;
w=dr;
[t,y] = Rungekutta(@(t,y) Mdlsegwconpert(y,u,w,Ep),[(k-1)*Tm k*Tm],xk,h);
xk=y(end,:)';
yk=C*xk+ruido;
% OBSERVADOR: Filtro de Kalman
Ro = I;
Qo = diag(1);
xpred=A*x_gorro+B*u+Bd*dr_gorro;
Ppred=A*Pest*A'+Qo;
Ko=Ppred * C' / (C * Ppred * C' + Ro);
x_gorro=xpred+Ko*(yk-C*xpred);
Pest=(I-Ko*C)*Ppred;
dr_gorro=dr_gorro+[-1 0 0]*(C*xk-C*x_gorro);
r_rampa=(1-rho)*r_rampa+rho*r';
[xr,ur]=SSTO(r_rampa,dr_gorro);
MPC;
end
toc
Código MATLAB®: SSTO
function [xr,ur]=SSTO(r,dr_gorro)
global A B Bd C Cd D G n m LBx UBx LBu UBu
I=eye(n);
EE=[(A-I) B
G*C G*Cd];
L=EE\[-Bd*dr_gorro;r];
c=[L;-L];
d=[UBx;UBu;-LBx;-LBu];
e=c-d;
indices=[];bandera=0;cont=0;lambda=1e6;
for i=1:(n+m)*2
if e(i)>0
indices=[indices i];
bandera=1;
cont=cont+1;
end
end
if bandera==0
REF=L;
xr=REF(1:n,:);
ur=REF(n+1:m+n,:);
else
for i=1:cont
aux=d(indices(i))/(c(indices(i),:));
if aux<lambda
lambda=aux;
end
end
REF=lambda*(L);
xr=REF(1:n,:);
ur=REF(n+1:m+n,:);
end
Código MATLAB®: Actualización de los valores del MPC
Qxr=Q*xr;
Rur=R*ur;
nn=0;mm=0;
for i=1:N
f(1+nn+mm:n+nn+mm,1)=-Qxr;
f(n+1+nn+mm:n+m+nn+mm,1)=-Rur;
nn=nn+n;mm=mm+m;
end
for i=1:n:(N+1)*n
b(i:i+n-1)=-Bd*dr_gorro;
end
b((N+1)*n-n+1:(N+1)*n)=xr-Bd*dr_gorro;
b(1:n,1)=x_gorro;
calc
u=z(n+1:n+m,1);
Códigos MATLAB 69
Código MATLAB®: Algoritmo FISTA
x=zeros(1,(N+1)*n)';y=zeros(1,(N+1)*n)';t=1;FIN=0;x_ant=zeros(1,(N+1)*n)';
Qfis=inv(E/H*E');
kk=1;
while ~FIN
z=sat2(H,f-E'*y,lim);
if norm(E*z-b)<=1e-6
FIN=1;
else
incr_y=-Qfis*(E*z-b);
x=y+incr_y;
t_sig=0.5*(1+sqrt(1+4*(t^2)));
y=x+(((t-1)/t_sig)*(x-x_ant));
kk=kk+1;
x_ant=x;
t=t_sig;
end
if kk>2000
FIN=1;
disp('exceso de bucle')
end
end
Código MATLAB®: saturación
function y=sat2(d,f,a)
n=size(d,1);
y=zeros(1,n)';
for i=1:n
z=-f(i,1)/d(i,i);
y(i)=min(abs(z),a(i,1))*sign(z);
end
11. REFERENCIAS
[1] C. Gonzalez, I. Alvarado, D. Muñoz La Peña, Low cost two-wheels self-balancing robot for control
education, Dpto. de Ingeniería de Sistemas y Automática, Universidad de Sevilla.
James B. Rawling y David Q. Mayne , Model Predictive Control: Theory and Desing. Wisconsin. Nob
Hill Publishing LLC., 2009
Beck, A, y M Teboulle. A fast iterative shrinkahe-thresholding algorithm for linear inverse problems.
SIAM J. Imaging Science., 2009.
Daniel Rodríguez Ramírez y Carlos Bordóns Alba. Apuntes de ingeniería de control. Dpto. de Ingeniería
de Sistemas y Automática, Universidad de Sevilla. 2007.
Diaz Madrigal, Santiago y Pizarro Solano, Luis. Métodos Matemáticos de Teleco (3º). Universidad de
Sevilla. 2014
Alfonso García Navarro, Implementación de controladores predictivos en Arduino, Trabajo Fin de Grado,
Universidad de Sevilla. 2016
José Ramón Romero Gómez, Implementación de controladores predictivos en LabVIEW, Trabajo Fin de
Grado, Universidad de Sevilla. 2016
71
73
12. ÍNDICE DE CONCEPTOS
FISTA: Fast Iterative Shrinkage-Thresholding Algorithm

LQR: Regulador Lineal Cuadrático
MPC: Control predictivo basado en modelo (Model Predictive Control)
MATLAB: MATrix LABoratory
SSTO: Steady-State Target Optimization
OBS: Observador
75

Memoria Final

Cargado por

Información del documento

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Memoria Final

Cargado por

Copyright:

Formatos disponibles

Tesis Doctoral

Grado en Ingeniería de Tecnologías Industriales

Control predictivo para un vehículo tipo Segway

Dep. Teoría de la Señal y Comunicaciones

Dep. Ingeniería de Sistemas y Automática

Control predictivo para un vehículo tipo Segway

Dep. de Ingeniería de Sistemas y Automática

Autor: Victor Manuel Villalar Lara

Tutor: Daniel Limón Marruedo

Acuerdan otorgarle la calificación de:

El Secretario del Tribunal

Victor Manuel Villalar Lara

Tabla 2–1. Parámetros físicos del robot 4

Figura 2-1. Segway para transporte humano 3

“Para el logro del triunfo siempre ha sido indispensable pasar

“Los ordenadores son inútiles. Sólo pueden darte respuestas”.

Descripción del Sistema: Segway

Figura 2-1. Segway para transporte humano

Figura 2-2. Segway autónomo

Descripción física y modelo de primeros principios

Tabla 2–1. Parámetros físicos del robot

Figura 2-3. Esquema del sistema con las variables

Energía cinética de rotación:

Modelo lineal discretizado

Límites fisicos del sistema

Introducción al control óptimo

𝐽(𝑥0 ) = ∑ 𝐿(𝑥(𝑘), 𝑢 (𝑘))

Este índice nos permite definir una ley de control óptima:

min 𝐽( 𝑥0 , ℎ) = ∑ 𝐿 (𝑥(𝑘), ℎ(𝑥(𝑘)))

Ley de control óptima

Control óptimo estocástico

Y en línea se estima el estado

𝑑̂𝑘+1 = 𝑑̂𝑘 + [−1 0 0] (𝑦𝑘 − 𝐶𝑥̂𝑘 )

Figura 3-1. LQR con offset

LQR con efecto integral

𝑒𝑘+1 = 𝑟𝑘+1 − 𝑦𝑘+1

Figura 3-2. LQR con efecto integral sin offset

Control Predictivo Basado en Modelo

los N estados futuros dado un conjunto de entradas u

Donde 𝐿𝑥 y 𝐿𝑑 son elegidos de manera que el estimador sea estable.

min ∑(𝑥(𝑘) − 𝑥𝑟 )𝑇 𝑄(𝑥(𝑘) − 𝑥𝑟 ) + (𝑢(𝑘)−𝑢𝑟 )𝑇 𝑅(𝑢(𝑘) − 𝑢𝑟 )

𝑥(𝑘 + 1) = 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) +𝐵𝑑 𝑑(𝑘)

Esquema del bucle

Figura 4-1. Esquema general del bucle de control

Componentes del Controlador

Figura 4-2. Esquema en profundidad del bucle de control

Que se obtiene de la Ecuación de Ricatti

La observabilidad es esencial en el control de sistemas con variables de estado inaccesibles.

𝑥((𝑘 + 1)𝑇) = 𝐺𝑥(𝑘𝑇)

Figura 4-3. Rango de la matriz de observabilidad

𝑥 + = 𝐴𝑥𝑟 + 𝐵𝑢𝑟 +𝐵𝑑 𝑑

Figura 4-4. Filtrado de escalón a exponencial

Minimización de la función objetivo

Problema de optimización cuadrática

Ejemplo 5–1. Sea z un número real, calcular la solución 𝑧 ∗

Siendo a < b y d > 0.

Figura 5-1. Parábola convexa

Figura 5-2. Solución del Ejemplo 5-1

Ejemplo 5–2. Sea z ∈ ℝ𝑛 , calcular la solución 𝑧 ∗

Siendo 𝑎𝑖 < 𝑏𝑖 y H ∈ ℝ𝑛 × 𝑛 diagonal con sus elementos 𝑑𝑖𝑖 > 0.

Ejemplo 5–3. Sea z ∈ ℝ𝑛 , calcular la solución 𝑧 ∗

Siendo 𝑎𝑖 >0 y H ∈ ℝ𝑛 × 𝑛 definida positiva.

Ejemplo 5–4. Sea z ∈ ℝ𝑛 , calcular la solución 𝑧 ∗

La función dual será