Ingeniería de Telecomunicación Trabajo Fin de Gradobibing.us.es/proyectos/abreproy/91499/fichero/memoriafinaltfg.pdfTodo esto será a modo de simulación, para lo cual usaremos la

Tesis Doctoral

Ingeniería de Telecomunicación

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

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla

2013

Control predictivo para un vehículo tipo Segway 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

Dep. Ingeniería de Sistemas y Automática



Sevilla, 2017

iii

Trabajo Fin de Grado

Grado en Ingeniería de Tecnologías Industriales


Autor:

Victor Manuel Villalar Lara

Tutor:

Daniel Limón Marruedo

Profesor titular

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



Sevilla, 2017

v

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

vii

ix

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

xi

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.

xiii

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.

Í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

xv

Q y R 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

Í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 R0/ Q0 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

≥

𝐶𝑇

ℤ≥0

Mayor o igual

Transpuesta de 𝐶

Conjunto de números enteros no negativos

xxi

1

1 INTRODUCCIÓN

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.

E

“Para el logro del triunfo siempre ha sido indispensable pasar

por la senda de los sacrificios”.

- Simón Bolivar -

3

2. DESCRIPCIÓN Y MODELADO DEL SEGWAY

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].

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

-Pablo Picasso-

4


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

2𝑚𝑟(��𝑟

2 + ��𝑟2) · 2 +

1

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

2𝑀) · 𝑅2��2 +

1

2𝑀𝐿2��2 +𝑀𝑅𝐿��cos( 𝜙 + 𝜙0) (2.11)

6


Energía cinética de rotación:

𝑇𝑟𝑜𝑡 =1

2𝑚𝑟𝑅

2��2 +1

2𝑀𝐿2��2 (2.12)

La energía potencial es:

𝑉 = 𝑀𝑔𝐿cos( 𝜙 + 𝜙0) (2.13)

Quedando como resultado:

𝐿 = (𝑚𝑟 +1

2𝑀)𝑅2��2 +

1

2𝑚𝑟𝑅

2��2+1

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

] [

𝜙

𝜙

��

] + [

0−𝑒

𝑓⁄

1

] �� + [

0−𝑑

𝑓⁄

0

]𝜙0 (2.22)

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

8


𝑡 = 𝑘 ∗ 𝑇𝑚

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):

Sistema continuo

{

𝐴 = (

0 1 039.9 0 00 0 0

)

𝐵 = (0

−0.361

)

𝐵𝑑 = (039.90)

⟶ Sistema discreto

{

𝐴 = (

1.008 0.0201 00.7997 1.008 00 0 1

)

𝐵 = (−7.2𝑒 − 05−0.00720.02

)

𝐵𝑑 = (0.0080.79970

)

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 = 𝑦𝑘 + ℎ (

1

6𝑘1 +

2

6𝑘2 +

2

6𝑘3 +

1

6𝑘4)

𝑘1 = 𝑓𝑘 = 𝑓(𝑡𝑘 , 𝑦𝑘)

𝑘2 = 𝑓 (𝑡𝑘 +ℎ

2, 𝑦𝑘 +

ℎ

2𝑘1)

𝑘3 = 𝑓 (𝑡𝑘 +ℎ

2, 𝑦𝑘 +

ℎ

2𝑘2)

𝑘4 = 𝑓 (𝑡𝑘 +ℎ

2, 𝑦𝑘 +

ℎ

2𝑘3)

(2.32)

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.

10


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 ·

𝜋

180𝑟𝑎𝑑

−4 𝑟𝑎𝑑/𝑠−12 𝑟𝑎𝑑/𝑠

] ≤ [

𝜙

𝜙

��

] ≤ [30 ·

𝜋

180𝑟𝑎𝑑

4 𝑟𝑎𝑑/𝑠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

11

3. CONTROL LINEAL CUADRÁTICO (LQR)

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

E

12


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].

14


𝑒𝑘+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 𝑘 = 𝑛:

𝑢𝑛 = 𝑢0 + 𝐾𝑥(𝑥𝑛 − 𝑥0) + 𝐾𝑒∑𝑒𝑘

𝑛−1

𝑘=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.

17

4. CONTROL PREDICTIVO BASADO EN

MODELO

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

E

18


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

Control Predictivo Basado en Modelo 19

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

] [𝑥∞𝑢∞] = [

−𝐵𝑑��∞𝑦∞ − 𝐶𝑑��∞

]

20


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

min𝑢∑(𝑥(𝑘) − 𝑥𝑟)

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

𝑁−1

𝑘=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] 𝑢𝑘 + [

𝐿𝑥𝐿𝑑] (−𝑦𝑘 + 𝐶𝑥𝑘 )

22


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®:

24


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:

[𝐴 − 𝐼 𝐵𝐻𝐶 0

] [𝑥𝑟𝑢𝑟] = [

−𝐵𝑑𝑑𝑟

] (4.39)

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

[𝐿𝑥𝐿𝑢] = [

𝐴 − 𝐼 𝐵𝐻𝐶 0

] \ [−𝐵𝑑𝑑𝑟

] (4.41)

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)

26

5. RESOLUCIÓN DEL PROBLEMA DE CONTROL

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:

min𝑧

1

2𝑧𝑇𝐻𝑧 + 𝑓𝑇𝑧 (3.1)

𝑠. 𝑎. 𝐴𝑧 ≤ 𝑏

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.

En primer lugar, debemos saber que una función cuadrática de la forma 1

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.

E

Resolución del problema de control

27

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

min𝑧

1

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:

y=1

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.

28


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

Desarrollando la ecuación (3.3):

𝑦 =1

2𝑑𝑧2 − 𝑑𝑧0𝑧 +

1

2𝑑𝑧0

2 (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

2𝑑𝑧2 − 𝑑𝑧0𝑧 =

1

2𝑑𝑧2 + 𝑓𝑧 (3.6)

𝑧∗ = 𝑧0 = −𝑓

𝑑 (3.7)

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

min𝑧

1

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𝑧12𝑑11 + 𝑓𝑖𝑧1 +⋯+

1

2𝑧𝑛2𝑑𝑛𝑛 + 𝑓𝑛𝑧𝑛 (3.8)

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𝑖 = −

𝑓𝑖

dii 𝑖 = 1,… , 𝑛 (3.9)


29


min𝑧

1


𝑠. 𝑎. |𝑧𝑖| ≤ 𝑎𝑖 𝑖 = 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

2Δ𝑧𝑇𝐻Δ𝑧 + 𝑧𝑘

𝑇𝐻Δ𝑧+1

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

minΔ𝑦

1

2𝐿Δ𝑦𝑇Δ𝑦+(𝐻𝑦𝑘 + 𝑓)

𝑇 Δ𝑦

𝑠. 𝑎. 𝑎𝑖 ≤ 𝑦𝑘𝑖+Δ𝑦𝑖 ≤ 𝑏𝑖 𝑖 = 1,… , 𝑛

30


b. 𝑧𝑘 ← 𝑦𝑘 + Δ𝑦

c. 𝑡𝑘+1 ←1

2(1 + √1 + 4𝑡𝑘

2)

d. 𝑦𝑘+1 ← 𝑧𝑘 +𝑡𝑘−1

𝑡𝑘+1(𝑧𝑘 − 𝑧𝑘−1)

e. 𝑘 ← 𝑘 + 1

f. Si |Δ𝑧𝑖| ≤ 𝑇𝑂𝐿, entonces FIN ← SI

3. 𝑧∗ ← 𝑧𝑘


min𝑧

1


𝑠. 𝑎. 𝐿𝐵 ≤ 𝑧 ≤ 𝑈𝐵

𝐴𝑧 = 𝑏

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:

J(z) = 1

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:

𝑥∗ = argmax 𝑥 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

𝑧(𝑥) = arg min 𝐿𝐵≤𝑧≤𝑈𝐵

1

2𝑧𝑇𝐻𝑧 + (𝑓 − 𝐴𝑇𝑥)𝑇𝑧 (3.24)


31

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

arg min 𝐿𝐵≤𝑧≤𝑈𝐵

1

2𝑧𝑇𝐻𝑧 + (𝑓 − 𝐴𝑇𝑥)𝑇𝑧

b. Si |𝐴z(𝑦𝑘) − b| ≤ 𝑇𝑂𝐿, entonces FIN ← SI

c. 𝑆𝑖 𝑛𝑜

i. Δ𝑦 ← −𝑄(𝐴𝑧(𝑦𝑘) − 𝑏)

ii. 𝑥𝑘 ← 𝑦𝑘 + Δ𝑦

iii. 𝑡𝑘+1 ←1

2(1 + √1 + 4𝑡𝑘

2)

iv. 𝑦𝑘+1 ← 𝑥𝑘 +𝑡𝑘−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.

32


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:

min𝑧 𝑧𝑇𝐻𝑧 + 𝑓𝑇𝑧

𝑠. 𝑎. 𝐿𝐵𝑧 ≤ 𝑧 ≤ 𝑈𝐵𝑧

𝐸𝑧 = 𝑏

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)⋮

𝑥(𝑁 − 1)𝑢(𝑁 − 1)]

(3.34)

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 𝑓 ∈ ℝ𝑁(𝑛+𝑚).


33

Definidos z y H ya tendríamos la función de coste. Ahora abordaremos las restricciones:

𝐿𝐵𝑧 ≤ 𝑧 ≤ 𝑈𝐵𝑧

[ 𝐿𝐵𝑥𝐿𝐵𝑢𝐿𝐵𝑥𝐿𝐵𝑢⋮𝐿𝐵𝑥𝐿𝐵𝑢]

≤

[ 𝑥(0)

𝑢(0)

𝑥(1)

𝑢(1)⋮

𝑥(𝑁 − 1)𝑢(𝑁 − 1)]

≤

[ 𝑈𝐵𝑥𝑈𝐵𝑢𝑈𝐵𝑥𝑈𝐵𝑢⋮

𝑈𝐵𝑥𝑈𝐵𝑢]

(3.37)

𝐿𝐵𝑥, 𝑈𝐵𝑥, 𝐿𝐵𝑢, 𝑈𝐵𝑢 ∈ ℝ𝑁(𝑛+𝑚).

Por último, nos faltaría la restricción 𝐸𝑧 = 𝑏.

Identifiquemos el primer termino 𝑥(0) = 𝑥0 :

[1 ⋱ 1

] [𝑥1(0)⋮

𝑥𝑛(0)] = [

𝑥1⋮𝑥𝑛] → 𝐼 𝑥(0) = 𝑥0 (3.38)

Procedamos ahora a identificar los siguientes términos k>0

[𝐴 𝐵 −𝐼] [

𝑥(𝑘)𝑢(𝑘)

𝑥(𝑘 + 1)] = [

−𝐵𝑑𝑑−𝐵𝑑𝑑−𝐵𝑑𝑑

] → 𝐴𝑥(𝑘) + 𝐵𝑢(𝑘) − 𝑥(𝑘 + 1) = −𝐵𝑑𝑑 (3.39)

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) + 𝐵𝑢(𝑁 − 1) + 𝐵𝑑𝑑 = 𝑥𝑟 (3.40)

Así que ya podemos identificar la restricción 𝐸𝑧 = 𝑏 con:

[ 𝐼 𝐴 𝐵 −𝐼 𝐴 𝐵 −𝐼 ⋱ 𝐴 𝐵]

[ 𝑥(0)

𝑢(0)

𝑥(1)

𝑢(1)⋮

𝑥(𝑁 − 1)𝑢(𝑁 − 1)]

=

[

𝑥0−𝐵𝑑𝑑−𝐵𝑑𝑑⋮

𝑥𝑟−𝐵𝑑𝑑]

(3.41)

Siendo 𝐸 ∈ ℝ𝑛(𝑁+1)×𝑁(𝑛+𝑚), 𝑏 ∈ ℝ𝑛(𝑁+1).

35

6. DISEÑO DEL MPC

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.

Q y R

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:

𝑥0 = [

𝜙

𝜙

��

] = [5 · 𝜋/180

00

]

C

36


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.

38


Diseño del MPC

39

40


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.

42


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

44


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

47

7. APLICACIÓN SOBRE SIMULADOR NO LINEAL

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]

E

48


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.

50



51

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

52


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


53

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 ·

𝜋

180𝑟𝑎𝑑

−4 𝑟𝑎𝑑/𝑠−12 𝑟𝑎𝑑/𝑠

] ≤ [

𝜙

𝜙

��

] ≤ [30 ·

𝜋

180𝑟𝑎𝑑

4 𝑟𝑎𝑑/𝑠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

54


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


55

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.

56


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.


57

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.

58


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.

59

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.

61

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

63

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

64


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);

66


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

68


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

71

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

73

75

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

Documents

Ingeniería de Telecomunicación Trabajo Fin de Gradobibing.us.es/proyectos/abreproy/91499/fichero/memoriafinaltfg.pdfTodo esto será a modo de simulación, para lo cual usaremos la