UN NIVERSIDAD L LA SALL LE FACULTA AD DE INGENIERÍA CON RECONO OCIMIENTO DE VALIDEZ V OFICIA AL DE ESTUDIO OS No. 952015 DE FECHA 13 DE FEBRERO DE 1995

Estabbilización de uun Pénddulo Inverrtido M Móvil M Mediantte un Conntroladdor Ópttimo LQG dee Tiemppo Continnuo y H Horizonte Inffinito

TESIS PROFESIONAL QU UE PARA OB BTENER EL L TÍTULO DE LICENC TRICA CIADO EN INGENIER RÍA ELÉCT YE EN SISTEM MAS ELEC CTRÓNICOS PRESENTA ÚS RODRIG A CENICER GO AVILA JESÚ ROS

ASESOR DE TESIS M. en C HEZ C. LUIS FER RNANDO LUP PIÁN SÁNCH

MÉXICO O, D.F.

2013

“No hay plazo que no se cumpla ni deuda que no se pague”.

Jose Zorrilla, el Burlador de Sevilla.

UNIVERSIDAD LA SALLE

Resumen ESCUELA DE INGENIERÍA Ingeniero Eléctrico y en Sistemas Electrónicos Licenciatura en Ingeniería Eléctrica y Sistemas Electrónicos

por Avila Ceniceros Jesús Rodrigo

Esta tesis propone el uso de una estrategia de control denominada Linear Quadratic Gaussian (LQG) y demuestra que puede usarse una cantidad reducida de sensores para observar todas las variables de estado necesarias y estabilizar el Péndulo Invertido Móvil (PIM) a diferencia de otras soluciones encontradas en la literatura. Se utiliza un estimador de Kalman-Bucy para filtrar las variables medidas directamente y estimar las variables no medidas, después se aplica una estrategia de retroalimentación LQR para mantener el estado de equilibrio vertical. El conjunto de esto, resulta en un controlador LQG probado en una simulación y verificado en un ambiente virtual en Open GL.

Agradecimientos Primero que nada quiero agradecer a Dios porque es el centro de mi vida. A mis padres por apoyarme en la culminación de mis estudios. A mi asesor Luis. A Karolina.

V

Índice general Resumen

IV

Agradecimientos

V

Índice de Figuras

IX

Índice de Cuadros

XI

Abreviaciones

XIII

Símbolos

XV

1. Introducción 1.1. Modelo Dinámico del Robot . . . . . . . . . . . . . . . . . . . . . . . . . . .

1 4

2. Linealización 2.1. Estados de Equilibrio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2. Jacobiano . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.3. Linealización del modelo del PIM . . . . . . . . . . . . . . . . . . . . . . . .

9 11 12 15

3. Control Óptimo LQG 3.1. LQG . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2. Filtro de Kalman . . . . . . . . . . . . . . . . . . . . . . 3.3. Filtro de Kalman-Bucy . . . . . . . . . . . . . . . . . . . 3.4. Estimación de variables de estado para el PIM . . . . . 3.5. Estabilización del PIM por retroalimentación del estado .

17 17 17 18 21 23

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

4. Resultados de Simulación

27

5. Conclusiones

31

A. Modelo Dinámico

33

B. Artículo de Investigación

35

Bibliografía

43 VII

Índice de figuras 1.1. Péndulo Invertido Móvil (vista lateral) . . . . . . . . . . . . . . . . . . . . . 1.2. Péndulo Invertido Móvil (vista superior) . . . . . . . . . . . . . . . . . . . .

5 5

2.1. Péndulo simple . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

10

3.1. Diagrama de control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

25

4.1. Ángulos yaw y pitch medidos . . . . . . . . . . . . . . . . . . . . . . . . . . 4.2. Variables de estado observadas . . . . . . . . . . . . . . . . . . . . . . . .

28 29

IX

Índice de cuadros 4.1. Parámetros del modelo dinámico . . . . . . . . . . . . . . . . . . . . . . . .

XI

28

Abreviaciones PIM

Péndulo Invertido Móvil

LQR

Linear Quadratic Regulator

FPGA

Field Programable Gate Array

LQG

Linear Quadratic Gaussian

MMSE

Minimum Mean Square Eerror

VSS

Variable System Structure

RLS

Recursive Leastystem Square

XIII

Símbolos α

pitch angle, inclinación

θ

yaw angle, orientación

(x, y)

posición del robot con respecto a una referencia inercial

φl , φr

ángulo de rotación de las ruedas derecha e izquierda

α˙ θ˙

velocidades angulares

v

velocidad hacia el frente del robot

≡

introduce notación

XV

A Dios y a mis padres . . .

XVII

Capítulo 1

Introducción El péndulo invertido es un problema de control muy común en clases de ingeniería y libros de control, esto se debe principalmente, a su naturaleza inestable y a las alinealidades que presenta, lo que lo convierten en un problema interesante para su análisis. El problema mencionado anteriormente, es similar al de equilibrar el palo de una escoba en la palma de la mano en el cual, el que realiza el experimento debe mover su mano en la dirección en la que se cae la escoba para evitar que esta caiga; en el caso del Péndulo Invertido Móvil (PIM) las ruedas son las que realizan la función de la mano para así lograr el equilibrio. Por otro lado, la idea del Péndulo Invertido Móvil ha ido cobrando fuerza en nuestra sociedad debido a la aparición de vehículos comerciales; tal es el caso del Segway Personal Transporter [1],[2]. En la literatura se pueden encontrar varias publicaciones que involucran Péndulos Invertidos Móviles; uno de los primeros y más importantes trabajos sobre PIMs es el de JOE [3] en el que se presenta un sistema basado en dos controladores de espacio de estados desacoplados, uno controlando la estabilidad del eje lateral (yaw) y el segundo, actuando en la dinámica del eje vertical (pitch), donde la salida de cada controlador es transformada por la unidad de desacoplamiento, lo que se traduce en un par aplicado a los motores. Las variables que describen el comportamiento del vehículo se obtienen de un modelo de computadora y se someten a experimentación. Además de una simulación por computadora, también se realiza la implantación del robot utilizando como sensores, un giroscopio para obtener el pitch rate o velocidad angular y dos encoders conectados a un FPGA donde se decodifican sus señales para obtener información del ángulo y de la velocidad angular entre el rotor y el estator (ruedas y chasis),

1

Capítulo 1. Introducción

2

luego se obtiene el pitch por integración numérica. Después se utiliza localización de polos y se prueban controladores con diferentes ubicaciones. Otro caso es el de Salerno et al. [4],[5] que es un trabajo similar al presentado en esta tesis ya que también se realiza la simulación del comportamiento del PIM y se utiliza un control LQR. En su trabajo se realiza un análisis de accesibilidad local (local accessibility) y un análisis de controlabilidad local en un tiempo pequeño (small-time controllability analysis) para así probar que es suficiente el uso del torque desarrollado por dos motores, uno en cada rueda, para controlar un robot parecido al presentado en esta tesis con algunas diferencias importantes, por ejemplo, que el vehículo presentado por Salerno et al. localiza su centro de masa por debajo del eje de las ruedas. Una base importante para la realización de este trabajo son los artículos publicados por Pathak et al. [6],[7] donde desarrollan el modelo dinámico directamente en términos de las variables de interés con respecto al control de posición y orientación, el cual es utilizado en esta tesis. Utilizan una aproximación Lagrangiana para derivar las ecuaciones y se eliminan las fuerzas restrictivas no holonómicas, después simplifican y reducen las ecuaciones y revisan la condición de fuerte accesibilidad (strong accessibility condition); también prueban que el máximo grado relativo es 4. Asimismo diseñan dos controladores que hacen uso de la linealización parcial, el primer controlador mantiene el ángulo de inclinación dentro de los límites especificados y sigue los puntos de ajuste para la orientación y velocidad dada, mientras que el segundo controlador estabiliza el vehículo a un punto desde cualquier configuración inicial. Finalmente se realiza una simulación en la que se puede notar, entre otras cosas, que a mayor radio de las ruedas la amplitud de oscilación es mayor. La importancia del artículo de Pathak et al. [6] para esta tesis, radica en que de aquí se obtuvo el modelo dinámico que se utilizó como base para el análisis, con algunas adaptaciones que toman en cuenta los sensores propuestos más adelante, debido a que dentro de todos los modelos dinámicos publicados, éste se encontro como el más preciso y completo. Existen otros artículos como en el caso de Shimada et al. [8] que también toman en cuenta las mismas variables dentro de su modelo dinámico. En el artículo se proponen dos métodos de control, uno para cuando el robot está cerca del punto de equilibrio donde se usa una ley de control LQ para servo y otro para cuando se encuentra en movimiento o en modo de control inestable en el que utiliza retroalimentación por linealización parcial. En el artículo únicamente analizan las características del péndulo invertido y no toman en cuenta la curva de movimiento y giro K que es un problema de control no lineal típico para sistemas no holonómicos.

Capítulo 1. Introducción

3

Cabe destacar que a pesar de la similitud del uso de las variables, el modelo dinámico no se encuentra desarrollado, lo que impide utilizarlo como base para esta tesis. Por su parte, Hirata et al. [9] obtiene la ecuación dinámica y presenta el método de control ajustable para el péndulo invertido del tipo de rotación, es decir, no tiene ruedas y se desplaza sobre una articulación rotatoria. El control es a base de dos tipos de controladores adaptables y la estabilización se logra separando el control en dos etapas que son, la etapa desconocida transitoria y la estable conocida. El ángulo de rotación se estabiliza por medio de un sistema de estructura variable (VSS) para los parámetros desconocidos en la primera etapa y está diseñado para una aproximación lineal alrededor del origen. Todos los parámetros básicos son estimados simultáneamente mediante el uso de RLS (Recursive Least Square). Posteriormente, se valida la convergencia del parámetro estimado calculando los eigen valores y cuando convergen lo suficiente, el controlador cambia a control LQR utilizando el método basado en la matriz de Hamilton para resolver la ecuación de Riccati que también se utiliza en este trabajo, pero la diferencia del artículo de Hirata et al. radica en la propuesta de las variables medidas, que son: la corriente del motor, los ángulos de rotación y los demás datos como las velocidades y aceleraciones angulares que se obtienen usando un filtro paso bajas. Otro diferenciador importante, es que se calculan los filtros de forma discreta usando la transformada bilineal y en esta tesis solamente se trata el problema en tiempo continuo. Dentro de la literatura podemos encontrar que los métodos utilizados para equilibrar el PIM, en muchos casos son bastante aproximados y la principal diferencia entre autores radica en la forma de controlar la posición. Tal es el caso del trabajo realizado por Gans et al. [10] en el cual se analizan dos métodos para eliminar los efectos de movimientos sobre los que no se está actuando y se utiliza un controlador basado en linealización por retroalimentación parcial (partial feedback linearization). Para el equilibrio (pitch) se utilizan sensores de ángulo y para el control de posición y velocidad se utilizan datos de la cámara. También se analizan 3 métodos que se pueden usar en conjunto para mejorar el desempeño. Primero se diseña un mejor controlador para reducir el deslizamiento y movimientos indeseados para esto diseñan un controlador a través de linealización por retroalimentación parcial y se hace mención a Pathak [6] pero a diferencia de su trabajo utilizan una referencia espacial, además de que Gans et al. está más enfocado al uso de cámaras. También consideran una serie de características que no son afectados por movimientos en los grados de libertad sobre los que no se está actuando. Después se sustraen los efectos de movimiento indeseado con el uso de sensores con los cuales miden la inclinación de la cámara y corrigen esta característica como si no hubiera inclinación. Una diferencia importante con respecto al trabajo presentado en esta tesis es que al igual que muchos otros autores, Gans et al. asumen la completa disponibilidad de las variables

Capítulo 1. Introducción

4

de estado, es decir, se asume que todas las variables se pueden medir y están libres de ruido. Como se puede observar, existen muchos trabajos enfocados a los PIMs pero todos resuelven el problema del equilibrio de manera distinta; la diferencia más importante con respecto a los trabajos citados anteriormente, será demostrar que es posible equilibrar el PIM midiendo únicamente un grupo reducido de variables; se plantea utilizar la información de únicamente dos enconders localizados en los motores, una brújula digital para medir el giro hacia la derecha y la izquierda y dos sensores infrarrojos que apuntan diagonalmente al piso cuyas mediciones serán usadas para obtener una aproximación del ángulo de inclinación. La selección de estos sensores se debe a que se encuentran disponibles en el laboratorio de robótica y sistemas automatizados de la Universidad La Salle en caso de llevar a cabo en un futuro la implantación del robot. El resto de las variables son estimadas mediante el uso de un filtro de Kalman-Bucy y después se utiliza control LQR para estabilizar el péndulo. Esta combinación es llamada LQG o Linear Quadratic Gaussian.

1.1.

Modelo Dinámico del Robot

Para desarrollar un sistema de control eficiente, es necesario describir la dinámica del vehículo mediante un modelo matemático y existen varias propuestas de modelos dinámicos con respecto al PIM con diferentes grados de detalle. Algunos modelos utilizan EulerLagrange para producir el modelo dinámico, mientras que otros utilizan una aproximación Newtoniana [4],[11] como en el caso de Grasser et al. [3] cuyas ecuaciones se obtienen de las fuerzas que actúan en el vehículo, sobre las ruedas y sobre el chasis. Como ya se mencionó, para esta tesis se utiliza como base el modelo realizado por Pathak et al. [6], al cual se le hicieron algunas adaptaciones que toman en cuenta los sensores seleccionados para este trabajo. Pathak et al. utiliza Euler-Lagrange que es un método basado en la energía cinética para la obtención de las ecuaciones. De acuerdo con las variables que afectan el modelo dinámico, el vector de estados es:

 T z ≡ x y φl φr θ α α˙ v θ˙

(1.1)

donde (x, y) representa la posición del robot con respecto a una referencia inercial y no serán medidas, φl y φr son los ángulos de rotación de las ruedas los cuales serán medidos por los encoders localizados en cada una de las ruedas, θ (yaw angle) es el ángulo de orientación con respecto al plano horizontal medido desde el eje x, α (pitch angle) es el ángulo de inclinación desde el punto de equilibrio al suelo y α˙ y θ˙ sus velocidades angulares

Capítulo 1. Introducción

5

Figura 1.1: Péndulo Invertido Móvil (vista lateral)

correspondientes; finalmente, la variable v representa la velocidad hacia el frente del robot. (ver Fig. 1.1)

Figura 1.2: Péndulo Invertido Móvil (vista superior)

Pathak et al. [6] tuvieron buenas razones para despreciar las posiciones angulares φl y φr debido a que no hay necesidad de controlarlas, sin embargo, en este caso son necesarias para observar las variables de estado que no son medidas directamente por los sensores (α˙ , θ˙, v ). El vector de control para el modelo dinámico del robot es:

 T u ≡ τl τr

(1.2)

donde τl y τr son los torques del motor derecho e izquierdo respectivamente, los cuales son tomados como positivos en la dirección hacia adelante. Con respecto al vector de estado z y el vector de control u, la ecuación de estado puede escribirse como:

z˙ = f (z) + g(z) u

(1.3)

Capítulo 1. Introducción

6

donde z˙ denota la derivada ordinaria respecto al tiempo de z . Por simplicidad se definen dos particiones del vector de estado z. La primera adecuada para las derivaciones de la ecuación del controlador.

  zxy    z=  zφ  zc

(1.4)

donde zc representa las variables de estado controladas con

zxy ≡

x y

! , zφ ≡

φl φr

!

 T , zc ≡ θ α α˙ v θ˙

(1.5)

La segunda partición se usa para las derivaciones de la ecuación del observador.

    zxy α˙  T        z =  zm , zm ≡ φl φr θ α , ze ≡  v   ze θ˙

(1.6)

donde zm y ze representan las variables de estado medidas y observadas, respectivamente, con zxy igual que en (1.5). Con respecto a la partición para el vector de estado del controlador dada en (1.4), las funciones f (z) y g(z) pueden particionarse como

    fxy (z) gxy (z)        f (z) ≡   fφ (z) , g(z) ≡  gφ (z)  fc (z) gc (z)

(1.7)

con

fxy (z) ≡

v cos(θ) v sin(θ)

! , fφ (z) ≡

1 Rv 1 Rv

− α˙ − − α˙ +

Dr ˙ R θ Dr ˙ R θ

!

gxy (z) ≡ 02×2 , gφ (z) ≡ 02×2

(1.8) (1.9)

donde R es el radio de las ruedas, Dr la distancia de las ruedas al poste vertical y

 T fc (z) ≡ fθ (z) fα (z) fα˙ (z) fv (z) fθ˙ (z)

(1.10)

Capítulo 1. Introducción

7

gc (z) ≡

0 0 gα˙ (z) gv (z)

gθ˙ (z)

!T

0 0 gα˙ (z) gv (z) −gθ˙ (z)

(1.11)

Los elementos del vector fc (z) en (1.10) y los elementos restantes de gc (z) en (1.11) se encuentran definidos en el Apéndice. Es posible verificar que la dinámica del sistema en (1.3) tiene un punto de equilibrio inestable en:

n o zeq ≡ z α = 0, α˙ = 0, v = 0, θ˙ = 0

(1.12)

Es importante notar que el estado de equilibrio no depende de la posición del robot (x, y) ni del ángulo (yaw) θ ya que utilizaremos esta característica más adelante. Con lo mostrado hasta ahora, podemos decir que el problema de control que se resuelve en este trabajo está dividido en dos sub problemas:

1. Linealizar el modelo (1.3) alrededor del punto de equilibrio (1.12). 2. Construir un controlador óptimo de horizonte infinito LQG de manera que se pueda controlar la velocidad v , y los ángulos de inclinación (α, θ) junto con sus correspondientes velocidades angulares sin considerar las variables de posición del vehículo

(x, y) ni las posiciones angulares de las ruedas φl and φr .

Capítulo 2

Linealización Para analizar y entender mejor el uso de la linealización utilizada en el PIM, resulta prudente analizarlo primero como un péndulo ordinario en dos dimensiones. Todo objeto que oscila se llama péndulo físico. El péndulo simple es un caso especial de péndulo físico y consta de una varilla de longitud l con una masa m unida a su extremo inferior. Al describir el movimiento de un péndulo simple en el plano vertical, se establecen las hipótesis simplificatorias de que la masa de la varilla es insignificante y que no actúan fuerzas externas de amortiguamiento ni de impulso, el ángulo θ de desplazamiento del péndulo medido respecto a la vertical (ver Fig. 2.1), se considera positivo cuando está hacia la derecha de OP y negativo cuando está a la izquierda. Se sabe que el arco s, de un círculo de radio l, se relaciona con el ángulo central θ por la fórmula s = lθ; por tanto, la aceleración angular es:

a=

d2 θ d2 s = l dt2 dt2

De acuerdo con la segunda ley de Newton

F = ma = ml

d2 θ dt2

En la figura Fig. 2.1 se ve que la magnitud del componente tangencial de la fuerza, debido al peso W , es mgsenθ. La dirección de esta fuerza es −mgsenθ, porque apunta a la izquierda cuando θ>0 y a la derecha cuando θ 0 tal que si la posición inicial X0 satisface a |X0 − X1 | < r, entonces la solución X(t) correspondiente satisface a

|X(t) − X1| < ρ para todo t > 0. Si además limt→∞ X(t)=X1 siempre que |X0 − X1| < r, se dice que X1 es un punto crítico asintóticamente estable. Dado cualquier disco de radio

Capítulo 2. Linealización

12

ρ en torno al punto crítico X1 , una solución permanecerá dentro de este disco siempre que X(0) = X0 se seleccione suficientemente cercano a X1 . No es necesario que una solución tienda al punto crítico para que X1 sea estable. Para subrayar que X0 se debe seleccionar cercano a X1 , también se usa la terminología punto crítico localmente estable. [12] 2.- El objeto se aparta más de su posición, en cuyo caso se dice que está en equilibrio inestable. Un lápiz parado sobre su punta está en equilibrio inestable, si su centro de gravedad está directamente arriba de su punta, la fuerza y el momento netos sobre él serán cero pero si se desplaza aunque sea un poco, por alguna corriente de aire o vibración, habrá un momento sobre él y continuará cayendo en dirección del desplazamiento original. La definición de punto de equilibrio o punto crítico inestable es: Sea X1 un punto crítico de un sistema autónomo y X = X(t) la solución que satisface la condición inicial X(0) = X0 , donde X0 6= X1. Se dice que X1 es un punto crítico inestable si hay un disco de radio ρ > 0 con la propiedad que, para toda r > 0, hay al menos una posición inicial X0 que satisface a |X0 −X1 | < r, y sin embargo, la solución correspondiente

X(t) satisface a |X(t) − X1| >= ρ para al menos un t > 0. Si un punto crítico X1 es inestable, independientemente de lo pequeña que sea la vecindad de X1 , siempre se puede encontrar una posición inicial X0 que resulte ser una solución que salga del disco de radio ρ en algún tiempo t futuro. 3.- El objeto permanece en su nueva posición, en este caso el cuerpo se encuentra en equilibrio neutro. Una esfera que descansa sobre una mesa horizontal; si se desplaza ligeramente hacia un lado permanecerá en su posición nueva. Un objeto cuyo centro de gravedad está debajo de su punto de apoyo se encuentra en equilibrio estable, sin embargo, el caso del PIM cae en el tipo de equilibrio inestable ya que su centro de gravedad está localizado por arriba del eje de las ruedas. Un estado Xe es un punto de equilibrio del sistema x˙ = f (x, t) si satisface f (Xe , t) = 0 para todo valor de t. Cuando f (x, t) es una función lineal e invariante con respecto al tiempo hay dos opciones: (1) el único estado de equilibrio está en el origen, (2) el sistema tiene una cantidad infinita de puntos de equilibrio uno de los cuales es el origen.

2.2.

Jacobiano

Se llama Jacobiano al determinante de la matriz Jacobiana la cual es una matriz formada por las derivadas parciales de primer orden de una función. Una de las aplicaciones más

Capítulo 2. Linealización

13

importantes de esta matriz es la posibilidad de aproximar linealmente a la función en un punto. Rara vez es posible determinar la estabilidad de un punto crítico de un sistema no lineal determinando soluciones explícitas. En su lugar, se remplaza el término g(x) en el sistema original autónomo x˙ = g(x) por un término lineal A(x−x1 ), que se aproxime lo más posible a g(x) en una vecindad X1 , a este proceso de sustitución se le llama linealización. Una ecuación de la recta tangente a la curva y = g(x) en x = x1 es

y = g(x1 ) + g 0 (x1 )(x − x1 )

y si x1 es un punto crítico de x˙ = g(x) entonces

x˙ = g(x) ≈ g 0 (x1 )(x − x1 )

La solución general de la ecuación diferencial lineal

x˙ = g 0 (x1 )(x − x1 ) es x = x1 + ceλ1 t , donde λ1 = g 0 (x1 ). Así si g 0 (x1 ) < 0, entonces x(t) tiende a x1 . El criterio de estabilidad afirma que se tiene el mismo comportamiento en la ecuación original, siempre y cuando se seleccione x(0) = x0 suficientemente cercano a x1 . El criterio de estabilidad dice: sea x1 un punto crítico de la ecuación diferencial autónoma

x0 = g(x), siendo g diferenciable en x1 . Si g 0 (x1 ) < 0, entonces x1 es un punto crítico asintóticamente estable. Si g 0 (x1 ) > 0, entonces x1 es un punto crítico inestable. Se puede hacer un análisis parecido para un sistema considerando el modelo general de variables de estado no lineal.

x0 = P (x, u, t)

(2.3)

y = Q(x, u, t)

(2.4)

Suponiendo una solución nominal conocida xn (t), un (t) y yn (t). La diferencia entre estas funciones vectoriales nominales y algunas funciones perturbadas ligeramente x(t),u(t)y

y(t) pueden definirse mediante

Capítulo 2. Linealización

14

δx = x(t) − xn (t), δu = u(t) − un (t), δy = y(t) − yn (t); tomando en cuenta lo anterior y que

x0 (t) = x0n (t) + δx0 La ecuación (2.3) puede escribirse como:

x0n + δx0 = P (xn + δx, un + δu, t) ∂P ∂P = P (xn , un , t) + δx + δu + términos de orden superior ∂x n ∂u n yn + δy = Q(xn + δx, un + δu, t) ∂Q ∂Q δx + δu + términos de orden superior = Q(xn , un , t) + ∂x n ∂u n donde |n significa que las derivadas se evalúan en las soluciones nominales. Debido a que estas soluciones satisfacen la ecuación (2.3) los primeros términos de la serie se cancelan. Para las perturbaciones δx, δy, δu suficientemente pequeñas los términos de orden superior se eliminan dejando la ecuación.

δx0 =

∂P ∂x n δx



+

∂P 0 ∂u n δuδy



=



∂Q ∂x n δx

+



∂Q ∂u n δu

(2.5)

El sistema original x0 = P (x, u, t) se puede aproximar en una vecindad del punto crítico xn mediante el sistema lineal x0 = A(x − Xn ), donde

 A=



∂P ∂y n  ∂Q ∂y n

∂P ∂x n ∂Q ∂x n



(2.6)

Si xn (t) = xe = constante y si un (t) = 0 = δu(t), entonces la estabilidad del punto de equilibrio xe se rige por

δx0 =





∂f δx ∂x n

(2.7)

Capítulo 2. Linealización

15

Para este caso, la matriz Jacobiana δf /δx es constante y sus eigen valores o valores propios determinan la estabilidad del sistema en la vecindad de xe , en el siguiente sentido. Si toda λi tiene partes reales negativas el punto de equilibrio es asintóticamente estable para perturbaciones suficientemente pequeñas. Si uno o más eigen valores tienen partes reales positivas, el punto de equilibrio es inestable. Si uno o más de los eigen valores se encuentran en el eje jw y todos los demás en la mitad izquierda del plano, no se puede determinar ninguna conclusión sobre la estabilidad del modelo lineal. Si el comportamiento actual del sistema es divergente o convergente dependerá de los términos de orden superior despreciados. Así que, excepto el caso en la frontera jw la estabilidad de la ecuación (2.3) es la misma que en el modelo linealizado, al menos en una vecindad cercana del punto de equilibrio.

2.3.

Linealización del modelo del PIM

Tomando en cuenta la teoría antes mencionada y debido a que la mayor parte del tiempo el robot operará cerca del punto de equilibrio (1.12) resulta natural buscar una aproximación lineal de su modelo dinámico (1.3) alrededor del punto de equilibrio. Esta linealización se logra en dos pasos: Primero, el sistema (1.3) es linealizado con respecto a z por medio de la matriz Jacobiana A (2.6) evaluada en el punto de equilibrio.

          ∂f 09×5 = A≡ ∂z zeq         

0

0

cos(θ)

0

0

sin(θ)

0

−1

0

−1

1 R 1 R

0

0

0

0

1

0

0,5k9 k1 +k2 gk1 k1 +k2

0

0

0

0

0

0

0

0



 0   −Dr   R  Dr  R   1    0    0   0   0

(2.8)

Después, se linealiza con respecto al control u simplemente evaluando g(z) en el punto de equilibrio (1.12), lo cual da como resultado:

B ≡ g(z)|zeq =

02×6

k5 +k6 k1 +k2 k5 +k6 k1 +k2

k7 +k8 k1 +k2 k7 +k8 k1 +k2

Dr R k3 +k4 −Dr R k3 +k4

!T (2.9)

En la matriz (2.8) se puede ver claramente que las alinealidades no son removidas por completo del modelo mediante la técnica de linealización debido a que dos de los elementos

Capítulo 2. Linealización

16

de la matriz A dependen de θ, la cual es una de las variables de estado en el modelo. El modelo se vuelve lineal con respecto al control u y todas las variables de estado, excepto por x y y . Sin embargo, estas dos variables se encuentran desacopladas del resto del sistema debido a que ninguno de los elementos de f (z) depende de ellas. Esto se traduce en que la linealización puede completarse simplemente excluyendo x y y del modelo. Es importante mencionar que al hacer esto perdemos la posibilidad de controlar la posición

(x, y) del robot por retroalimentación de estados. A pesar de esta limitante, esto resulta aceptable ya que por el momento únicamente interesa estabilizar el PIM en su punto de equilibrio. (1.12) Si se hace la partición de las matrices A y B estas quedan como se muestra a continuación.

02×2 Axy (θ)

A=

07×2

Al

!

!

02×2

, B=

(2.10)

Bl

con

0 0 0 0 0 cos(θ) 0

Axy (θ) ≡ 

0 0 0 0 0 sin(θ) 0

,

− DRr



−1

1 R 1 R

Dr R

0

0

0

1

0 0

0

1

0

0

0 0

k9 2(k1 +k2 ) gk1 k1 +k2

0

0

0

0

0

0

0

0

0

0

             

0

−1

0 0

0

0 0

0 0 0

 0   0   Al ≡ 0  0   0 

!

0 0

0 0 0

(2.11)

(2.12)

y

Bl ≡

0 0 0 0 0 0 0 0

k7 +k8 k1 +k2 k7 +k8 k1 +k2

k5 +k6 k1 +k2 k5 +k6 k1 +k2

Dr R k3 +k4 −Dr R k3 +k4

!T (2.13)

de esta forma el modelo linealizado con respecto a las variables de estado en zφ y zc puede expresarse como

z˙ φ z˙ c

! = Al

zφ zc

! + Bl u.

(2.14)

Capítulo 3

Control Óptimo LQG 3.1.

LQG

El controlador LQG del Péndulo Invertido Móvil esta compuesto por dos partes. Primero, se construye el filtro de Kalman-Bucy [13],[14] para estimar las variables de estado que no se están midiendo v α˙ y θ˙, basado en la ruidosa medición proveniente de las variables de estado en Zm . Segundo, ya que todas las variables de interés han sido estimadas, se construye una matriz de retroalimentación óptima que por el principio de separación puede diseñarse independientemente del estimador de Kalman-Bucy.

3.2.

Filtro de Kalman

La base del controlador LQG (Linear Quadratic Gaussian) del PIM es el filtro de Kalman, el cual resuelve dos problemas principales. El primero se refiere a la estimación óptima de donde se deriva el error de estimación, la matriz de covarianza del error de estimación y las pérdidas cuadráticas. El segundo resuelve el problema del filtrado, que resulta ser el problema dual. El filtro de Kalman encuentra su aplicación en la predicción de señales aleatorias, separación de señales aleatorias de ruido aleatorio y detección de señales de forma conocida (pulsos, senoidales) en presencia de ruido. Su objetivo es dar una mejor solución al filtro de Wiener generalizándolo y extendiéndolo a problemas no estacionarios, puede pensarse como un estimador MMSE (Minimum Mean Square Error) secuencial de una señal con ruido donde la señal se caracteriza por un modelo dinámico o de estado. 17

Capítulo 3. Control Óptimo LQG

18

El filtro de Wiener tiene como objetivo minimizar el error cuadrático medio (MSE) entre la señal deseada y la salida . Existen tres problemas principales resueltos mediante al uso de este filtro. Filtrado: el problema es filtrar la señal de un ruido; es importante hacer notar que la señal de la muestra es estimada basándose en datos presentes y pasados únicamente. Suavizado: en contraste con el filtrado es permitido el uso de datos futuros. En el suavizado no se puede obtener una estimación hasta no haber recolectado todos los datos. Predicción: se estima un entero positivo basados en los datos presentes y pasados [15]. De la propuesta del filtro de Kalman se obtienen los siguientes resultados: - Se deriva una ecuación diferencial no lineal para la matriz de covarianza del error de estimación óptimo. De la solución de esta ecuación se obtienen los coeficientes de la ecuación diferencial del filtro lineal óptimo. - Se muestra que el problema del filtrado es un problema dual del problema del regulador libre de ruido.

3.3.

Filtro de Kalman-Bucy

El filtro de Kalman-Bucy es la versión en tiempo continuo del filtro de Kalman El Regulador Cuadrático Lineal (LQR) es una ley de control por retroalimentación de estados

u = −Kx para el sistema x˙ = Ax + Bu que minimiza una función de costo Z Jc (zc , u) ≡

∞


zc (t)T Πc zc (t) + u(t)T Ψc u(t) dt

(3.1)

0

A la ley de control se le llama ‘óptima’ con respecto a la función de costo J . Se podría preguntar si existe una técnica de diseño óptima para un estimador de estados; esto es, ¿existe alguna aproximación al diseño del observador que sea equivalente, en cierta forma, al regulador cuadrático lineal? (LQR). Dado el sistema observable.

x˙ = Axy = Cx se puede definir el sistema dual θ˙ = AT θ + CT Υ y diseñar un controlador LQR para minimizar la función de costo.

Capítulo 3. Control Óptimo LQG

19

Sin embargo, no es claro como se debe penalizar a Θ y Υ en la función de costo. Si ahora se considera el sistema de control lineal observable.

x˙ = Ax + Bu + wy = Cx + v en el cual las dinamicás están sujetos a disturbios aleatorios w y las mediciones están sujetas a ruido aleatorio v . En paralelo con el desarrollo del LQR, Kalman examinó el siguiente problema de estimación óptimo: se construye un observador completo que minimiza el efecto combinado de los disturbios y el ruido, proveyendo un estimado del estado del sistema. Resolver este problema, requiere cierta información acerca del proceso aleatorio; si el proceso es ruido blanco Gaussiano de media cero, el diseño del estimador óptimo se hace perfectamente análogo al problema del diseño del controlador LQR. Problema del observador óptimo: Se desea obtener una salida Ψ(y − yˆ, t) tal que la estimación del estado generada por el sistema ficticio

x ˆ˙ = Aˆ x + Bu + Ψ(y − yˆ, t) converge al valor del estado real x en el sentido que la siguiente medición del error de covarianza se minimiza

Z J=

∞

ETr (˜ xx ˜T )dt

(3.2)

0

en el cual x ˜ = x−x ˆ es el error estimado, y el operador Tr representa la suma de los elementos de la diagonal de una matriz, se asume que las dinámicas estimadas son inicializadas escogiendo x ˆ(0) = Ex(0) el valor medio del estado inicial se asume que es conocido. Resulta que la selección óptima de Ψ es lineal, invariante en el tiempo: Ψ(y − yˆ, t) =

G(y − yˆ) El error de las dinamicás del observador es x ˜ = x−x ˆ = (Ax + Bu + w) + (Aˆ x + Bu +

G(y − yˆ)) = (A − GC)˜ x + w − Gv . Nótese que el error de estimación x ˜ en cualquier instante se debe sólo al disturbio aleatorio

w(t) y el ruido aleatorio v(t). Si estas dos señales aleatorias desaparecen,y si (x ˆ(0) = x(0)) entonces el error permanecerá cero todo el tiempo. Debido a que el sistema es lineal, se puede descomponer el error de estimación en dos términos, uno proveniente del disturbio y otro proveniente del ruido. x ˜=x ˜w + x ˜v Con esto, la función de costo se convierte en

Capítulo 3. Control Óptimo LQG

Z

20

∞

ETr ((˜ xw + x ˜v )(˜ xw + x ˜v )T )dt

J=

(3.3)

0

Si el ruido y el disturbio del proceso no están correlacionados, entonces tampoco lo están los componentes resultantes de x ˜w y x ˜v de modo que

Z J=

∞

 Ek˜ xw k2 + Ek˜ xv k2 dt

(3.4)

0

Debido a que la dualidad y los disturbios y ruido del proceso se asumen como ruido blanco Gaussiano de media cero el problema de minimizar J mencionado anteriormente, es equivalente a ∞

Z

(ΘT W Θ + ΥT V Υ) dt

J=

(3.5)

0

sujeto a la dinámica

˙ = AT Θ + C T Υ Θ

(3.6)

Aquí la matriz W representa la matriz de covarianza de w(t) y la matriz V representa la matriz de covarianza de v(t). Por definición W y V son semidefinidas positivas. Más adelante se asume que V es definida positiva (por lo tanto invertible). Como ya se sabe de la teoría de LQR, la entrada a minimizar del sistema dual es:

Υ = −GT Θ

donde

GT = V −1 CP ∗

(3.7)

y P ∗ es la única solución semidefinida positiva a la ecuación algebráica de Riccati

AP + P AT − P C T V −1 CP + W = 0

(3.8)

La ganancia del observador minimiza la covarianza del error estimado. En un sentido cuantificable, la ganancia balancea el error estimado debido a las incertidumbre de la planta

Capítulo 3. Control Óptimo LQG

21

(representado por el disturbio desconocido w(t)) y la incertidumbre de medición (representado por el ruido desconocido v(t)). Si la covarianza del disturbio es grande con respecto a la covarianza del ruido, entonces la ganancia del observador resultante G será grande. Esto significa que la estimación de estados dependerá en mayor medida en el error de medición

y − yˆ. Por otro lado si la covarianza del disturbio es pequeña con respecto a la covarianza del ruido, entonces la ganancia del observador G será pequeña.

3.4.

Estimación de variables de estado para el PIM

De (1.4) y (1.6) se puede notar que

zφ

!

zc

zm

=

! (3.9)

ze

por lo que se puede decir que el modelo linealizado (2.14) es equivalente a

z˙ m z˙ e

!

zm

= Al

!

ze

+ Bl u

(3.10)

Si se define una ecuación de salida para el vector de variables de estado medidas zm como

y = Cl

zm

!

ze

(3.11)

con

  Cl ≡ I4×4 04×3

(3.12)

se puede verificar que el modelo linealizado dado por (3.10) y (3.11) es observable, lo que significa que el sistema original (1.3) es localmente observable alrededor del punto de equilibrio. Por lo tanto, es posible estimar las variables no medidas. Se tomaron en cuenta las incertidumbres del ruido de medición formulando un modelo estocástico.

z˙ m z˙ e

! = Al

y = Cl

zm

! + Bl u + w

ze zm ze

(3.13)

! +s

donde w es un vector de disturbio aleatorio, el cual toma en cuenta las incertidumbres y

s es el ruido de medición. El w y s se asumen que son ruido blanco Gaussiano de media cero, con matrices de covarianza W y S, respectivamente.

Capítulo 3. Control Óptimo LQG

22

Por el principio de dualidad se resuelve el problema del estimador óptimo de Kalman-Bucy encontrando la matriz Ke que minimiza el índice de desempeño de horizonte infinito. ∞

Z Je (ϕ, η) ≡ 0

 kΩϕ(t)k2Π + kΣη(t)k2Ψ dt

(3.14)

sujeto a las restricciones

ϕ˙ = ATl ϕ + ClT η

(3.15)

η = −KeT ϕ

(3.16)

donde Ω y Σ son “matrices triangulares superiores” tal que

W = ΩT Ω, S = ΣT Σ,

(3.17)

Π y Ψ son matrices de ponderación definidas positivas elegidas apropiadamente. La matriz Ke se obtiene resolviendo numéricamente la ecuación de Riccati

Al Pe + Pe ATl − Pe ClT S−1 Cl Pe + W = 0

(3.18)

para una matriz definida positiva simétrica Pe de la misma dimensión que Al y resolviendo

Ke = S−1 Cl Pe


T

.

(3.19)

El estimador de variables de estado completo de Kalman-Bucy es entonces

ˆ z˙ m ˆ z˙ e

! = Al

ˆ zm ˆ ze

! + Bl u + Ke

z m − Cl

ˆ zm ˆ ze

!! (3.20)

del cual el vector de estados de retroalimentación estimado ˆ zc puede ser formado como

  θˆ   ˆ zc ≡  ˆ α  ˆ ze

(3.21)

donde θˆ y α ˆ son las estimaciones óptimas de los ángulos de orientación e inclinación y

 T ˆ˙ vˆ θˆ˙ es el vector de las correspondientes velocidades angulares y frontales ˆ ze ≡ α estimadas.

Capítulo 3. Control Óptimo LQG

3.5.

23

Estabilización del PIM por retroalimentación del estado

Si bien las posiciones angulares de las ruedas φl y φr son necesarias para observar las variables no medidas v , α˙ y θ˙, no hay necesidad de controlarlas pero al hacer esto dejamos de lado la posibilidad de controlar las variables de estado en zc (1.5). Por lo tanto podemos dejar de lado las posiciones angulares, debido a que las dinámicas de las variables en zc son independientes de las posiciones angulares, lo cual puede verificarse al inspeccionar los contenidos de las matriz (2.13). Debido a esto es natural realizar una partición de matrices Al y Bl del modelo linealizado (2.14) de la siguiente manera

Al =

02×2 Aφ 05×2

!

Ac

02×2

, Bl =

! (3.22)

Bc

con

Aφ ≡

0 0 −1

 0  0   Ac ≡ 0  0  0

− DRr

1 R 1 R

0 0 −1

!

Dr R

, 

0

0 0 1

0

 1 0 0   0 0 0  0 0 0  0 0 0

k9 2(k1 +k2 ) gk1 k1 +k2

0

(3.23)

(3.24)

y

Bc ≡

0 0 0 0

k5 +k6 k1 +k2 k5 +k6 k1 +k2

k7 +k8 k1 +k2 k7 +k8 k1 +k2

Dr R k3 +k4 −Dr R k3 +k4

!T (3.25)

de donde las dinámicas de las variables de estado en zc se expresan como

z˙ c = Ac zc + Bc u.

(3.26)

Las variables de estado θ y θ˙ se encuentran desacopladas del resto de las variables en

zc , tal y como se encuentra desacoplada v . Sin embargo, no se realizará otra partición de la dinámica del sistema debido a que esto impediría combinar las variables de control y todas las variables de estado de interés en un índice de desempeño único, que tome en cuenta a todas las variables simultáneamente y así se perdería el sentido del problema de minimización del Regulador Cuadrático Lineal o LQR El problema de control óptimo LQR de horizonte infinito puede ahora formularse como un problema de minimización, en el cual se busca la matriz que minimiza el índice de desempeño Kc .

Capítulo 3. Control Óptimo LQG

24

Z Jc (zc , u) ≡

∞


zc (t)T Πc zc (t) + u(t)T Ψc u(t) dt

(3.27)

0

restringido por la dinámica del modelo lineal (3.26) y al control por retroalimentación de estados u = −Kc zc , donde Πc y Ψc son matrices diagonales definidas positivas. Los elementos de la diagonal de estas dos matrices son seleccionadas para expresar el costo relativo del error en las correspondientes variables de estados (para el caso de Πc ) y de las variables de control (para el caso de Ψc ). La matriz Kc se encuentra resolviendo la ecuacion´ de Riccati T ATc Pc + Pc Ac − Pc Bc Ψ−1 c Bc Pc + Π c = 0

(3.28)

para una matriz simétrica definida positiva Pc del mismo tamaño que Ac y resolviendo T Kc = Ψ−1 c Bc Pc .

(3.29)

Tomando en cuenta que el vector de estados zc no puede ser retroalimentado directamente debido a que las variables α˙ , θ˙ y v no son medidas sino estimadas, el control por retroalimentación para el robot tiene que definirse como u = −Kcˆ zc . Este control es suficiente para lograr el objetivo de mantener el PIM en su punto de equilibrio (1.12). Sin embargo, mediante un simple cambio en el ángulo (yaw) θ y en la velocidad hacia adelante v se le da al robot la capacidad de seguir un punto de ajuste para esas dos variables de estado, lo cual puede convertirse en la base para permitir al robot realizar tareas de seguimiento de trayectorias. El punto de ajuste puede ser especificado por un dispositivo externo manipulado por un ser humano (como un joystick o un controlador de radio frecuencia) o por un algoritmo de control de posición de alto nivel. Si estos puntos de ajuste son llamados θsp y

vsp respectivamente, el control por retroalimentación puede definirse como:  T ˆ˙ vˆ − vsp θˆ˙ . u = −Kc θˆ − θsp α ˆ α

(3.30)

En la Fig. 3.1 se muestra un diagrama de bloques que reúne todo el sistema de control propuesto para el Péndulo Invertido Móvil. Primero, se hace la medición de las variables φl , φr , θ, α ; para lo cual se propone el uso de los enconders en cada motor, la brújula digital y los sensores infrarojos; obteniendo así

Zm que son las variables medidas que alimentan el estimador de Kalman-Bucy el cual filtra estas variables y estima las faltantes para así obtener Zˆc compuesto por las variables ˆα ˆ˙ vˆ, θˆ˙ que son las que alimentan el controlador LQR. En este punto es controladas θ, ˆ , α, donde se podría hacer el cambio en las variables θˆ y vˆ ángulo y velocidad respectivamente y dar al robot la capacidad de seguir el punto de ajuste antes mencionado. Al resolver el

Capítulo 3. Control Óptimo LQG

25

Figura 3.1: Diagrama de control

problema de control Óptimo LQR y encontrar la matriz Kc obtenemos el τl y τr que debe aplicarse a los motores del PIM y la retroalimentación para el filtro de Kalman-Bucy.

Capítulo 4

Resultados de Simulación El desempeño del esquema de control propuesto es probado mediante una simulación. Para este experimento, todas las variables (incluyendo las del estimador de Kalman-Bucy) son inicializadas en cero con excepción del ángulo pitch α, el cual es inicializado en 25◦ . El punto de ajuste para el ángulo yaw y la velocidad hacia el frente es θsp = 30◦ y vsp =

30cm/s, respectivamente. Para fines de simulación, las matrices de covarianza para los disturbios w y el ruido de medicion s son tomadas como

W = 10−4 diag (1, 1, 100, 400, 400, 25, 400) , S = 10−4 I4×4 ,

(4.1)

Las matrices de peso Π y Ψ para el filtro de Kalman-Bucy se escogieron como Π =

I7×7 , Ψ = I4×4 , y las matrices del índice de desempeño del controlador LQR Πc y Ψc son

Πc = diag (0,01, 4, 1, 1, 0,01) , Ψc = I2×2 .

(4.2)

Los parámetros específicos para el modelo dinámico utilizados en la simulación, se muestran en la tabla 4.1. Las definiciones para cada uno de los parámetros se encuentran en el Apéndice. La Fig. 4.1 muestra como después de una pequeña oscilación transitoria en el ángulo pitch

α, este permanece dentro de una pequeña region alrededor de α = 0, el cual es esencial para que se mantenga en equilibrio. Dado que la matriz del índice de desempeño Πc expresa un mayor costo relativo para α y su primera derivada, se esperaba que el ángulo pitch

27

Capítulo 4. Resultados de Simulación

28

Cuadro 4.1: Parámetros del modelo dinámico

Mb Mw R cz Dr Ixx Iyy Izz Iwa Iwd g

0,97 27 5,05 16,44 11,85 3,57 × 10−2 3,50 × 10−2 2,24 × 10−3 9,71 × 10−4 4,89 × 10−4 9,81

kg g cm cm cm kg · m2 kg · m2 kg · m2 kg · m2 kg · m2 m/s2

α alcanzara una velocidad de respuesta mayor comparada a θ, la cual puede ser verificada en la Fig. 4.1. La respuesta del estimador de Kalman-Bucy se muestra en la Fig. 4.2. La estimación de la velocidad hacia adelante v es virtualmente perfecta, mientras que la estimación del las velocidades angulares α˙ y θ˙ convergen después de una transición corta de alrededor de

2 s para el pitch rate y 1,2 s para el yaw rate. La gráfica para la velocidad hacia adelante, muestra que se requiere una acción oscilatoria inicial para llevar al robot cerca del punto de equilibrio, después se aproxima gradualmente a la velocidad final deseada 30cm/s. Se desarrolló una simulación en OpenGL que da una mejor idea del experimento y se encuentra disponible en un CD en formato MPEG adjunto a esta tesis.

Figura 4.1: Ángulos yaw y pitch medidos

Capítulo 4. Resultados de Simulación

Figura 4.2: Variables de estado observadas

29

Capítulo 5

Conclusiones Utilizando la propiedad de observabilidad local del PIM se propone un estimador de KalmanBucy que dará estimaciones de las velocidades angulares y velocidad hacia adelante del sistema, junto con las estimaciones filtradas de las variables de estado medidas. Por medio del principio de dualidad se retroalimentan estas variables de estado estimadas mediante un controlador óptimo LQR. La estrategia de control propuesta en esta tesis fue probada exitosamente mediante una simulación utilizando señales de prueba con ruido, lo cual demostró que la cantidad reducida de sensores y actuadores utilizada es suficiente para observar y controlar el Péndulo Invertido Móvil. Una limitante en la forma como se soluciona el problema del equilibrio, es que la estimación del ángulo pitch se basa en que la superficie del suelo es horizontal y plana. sin embargo, esto puede solucionarse simplemente remplazando los dos sensores infrarrojos por un acelerómetro. Se tiene pensado realizar la implantación de la estrategia de control propuesta en un FPGA montado en el robot, esto implica que será necesario obtener una versión discreta del controlador LQG para ser implantado en un sistema digital. Por consiguiente, la propuesta para estudio futuro es obtener la versión discreta del controlador y llevar a cabo la implantación con el uso de un FPGA.

31

Apéndice A

Modelo Dinámico Este apéndice muestra los detalles del modelo dinámico utilizado para el desarrollo de este trabajo. Para la derivación completa de las ecuaciones, favor de dirigirse a [6].

fα˙ (z) ≡

fθ (z) ≡ θ˙

(A.1)

fα (z) ≡ α˙

(A.2)

H θ˙2 sin(2α) + k1 α˙ 2 sin(2α) + k9 sin(α) 2 (k1 cos2 (α) + k2 )

(A.3)

k10 θ˙2 sin(α) − k3 Mb cz θ˙2 sin(3α) 4 (k1 cos2 (α) + k2 ) k1 g sin(2α) + 2k6 k8 α˙ 2 sin(α) + 2 (k1 cos2 (α) + k2 )

(A.4)

k3 α˙ θ˙ sin(2α) + k7 v θ˙ sin(α) k3 cos2 (α) + k4

(A.5)

gα˙ (z) ≡

k5 + k6 cos(α) k1 cos2 (α) + k2

(A.6)

gv (z) ≡

k7 cos(α) + k8 k1 cos2 (α) + k2

(A.7)

gθ˙ (z) ≡

RDr k3 cos2 (α) + k4

(A.8)

fv (z) ≡

fθ˙ (z) ≡

I1 ≡Izz − Ixx − Mb c2z

(A.9)

k1 ≡k62 k2 ≡ − k5

(A.10)


Iyy + Mb c2z

k3 ≡R2 I1

(A.11) (A.12)

33

Apéndice A

34


k4 ≡R2 Izz + 2Iwd + 2Mw Dr2 + 2Dr2 Iwa − k3

(A.13)

k5 ≡M R2 + 2Iwa

(A.14)

k6 ≡Mb Rcz

(A.15)

k7 ≡ − Rk6

(A.16)


k8 ≡ − R Iyy + Mb c2z

(A.17)

k9 ≡ − 2gk5 Mb cz

(A.18)

k10 ≡4k6 k8 − k3 Mb cz H ≡k1 + k5 I1

(A.19) (A.20)

donde Mb es la masa del péndulo, Mw es la masa de las ruedas, M = Mb + 2Mw es la masa total del robot, R es el radio de las ruedas. Si se define un sistema de coordenadas ajustado al robot con el origen en el punto medio entre las ruedas, el eje X apuntando en dirección hacia adelante, el eje Y apuntando horizontalmente desde el origen a la rueda izquierda y el eje Z apuntando verticalmente y hacia arriba, entonces cz es la altura del centro de masa del péndulo medida desde el origen, Dr es la distancia desde cada rueda al origen. Ixx , Iyy e Izz son los momentos de inercia del péndulo alrededor de su centro de masa con respecto a los ejes correspondientes. Iwa e Iwd son los momentos de inercia de cada rueda con respecto a su eje de rotación y su diámetro respectivamente. Finalmente, g es la aceleración de la gravedad.

Apéndice B

Artículo de Investigación Debido a que este trabajo de tesis es una extensión de un artículo de investigación publicado en el Simposio Latinoamericano de Robótica, se presenta una copia del artículo original.

35

36

Apéndice B. Artículo de Investigación

37

Apéndice B. Artículo de Investigación

38

Apéndice B. Artículo de Investigación

39

Apéndice B. Artículo de Investigación

40

Apéndice B. Artículo de Investigación

41

Apéndice B. Artículo de Investigación

42

Bibliografía [1] Segway. Segway, 2008. URL http://www.segway.com/. [2] Dean L Kamen, Robert R Ambrogi, Robert J Duggan, Richard Kurt Heinzmann, Brian R Key, Andrzej Skoskiewicz, and Phyllis K Kristal. Transportation vehicles and methods, October 1999. [3] Felix Grasser, Aldo D’Arrigo, Silvio Colombi, and Alfred Rufer. JOE: A mobile, inverted pendulum. Industrial Electronics, IEEE Transactions on, 49(1):107 – 114, February 2002. [4] Alessio Salerno and Jorge Angeles. On the nonlinear controllability of a quasiholonomic mobile robot. In ICRA, volume 3, pages 3379–3384, September 2003. doi:

10.1109/ROBOT.2003.1242112. [5] A. Salerno and J. Angeles. A new family of two-wheeled mobile robots: Modeling and controllability. Robotics, IEEE Transactions on, 23(1):169–173, February 2007. ISSN 1552-3098. doi: 10.1109/TRO.2006.886277. [6] Kaustubh Pathak, Jaume Franch, and Sunil K Agrawal. Velocity control of a wheeled inverted pendulum by partial feedback linearization. Decision and Control, CDC 43rd IEEE Conference on, 4(1):3962–3967, December 2004. [7] Kaustubh Pathak, Jaume Franch, and Sunil K Agrawal. Velocity and position control of a wheeled inverted pendulum by partial feedback linearization. IEEE Transactions on Robotics, 21(3):505–513, June 2005. [8] Akira Shimada and Naoya Hatakeyama. High-speed motion control of inverted pendulum robots. Advanced Motion Control, 9th IEEE International Workshop on, pages 307 – 310, March 2006. [9] Hiroshi Hirata, Koji Haga, and Phornsuk Ratiroch-Anant. Self-tuning control for rotation type inverted pendulum using two kinds of adaptive controllers. Robotics, Automation and Mechatronics, 2006 IEEE Conference on, pages 1–6, 2006.

43