5. Estartegias Basicas de Control

Dr. Martn Velasco Villa

Seccion de Mecatronica

Abril de 2015

Control Proporcional Derivativo (PD) y control PD aumentado(PD+)

I Una ley de control Proporcional-Derivativo PD puede obtenerse en el caso de unmanipulador al linealizar su dinamica alrededor de un punto de equilibrio y deesta forma, mediante el uso de la teora de control para sistemas lineales, obteneruna retroalimentacion PD que resuelva el problema de regulacion o seguimientode trayectorias.

I En este caso es facil probar la estabilidad local del sistema al analizar la ubicacionde los polos de lazo cerrado resultante.

I En el caso no lineal, como resulta el modelo de un manipulador, el analisis deestabilidad en lazo cerrado puede llevarse a cabo considerando un enfoque basadoen tecnicas de Lyapunov.

Control PD

Considere un manipulador en el cual los terminos de amortiguamiento y de gravedadson nulos, esto es,

M (q) q + C (q, q) q = u. (1)

En su forma mas simple, una ley de control PD esta dada por

u = kv q kp q (2)donde q = q qd con qd representando una posicion deseada de las coordenadasgeneralizadas.

I La retroalimentacion (2) no contiene un termino integral clasico (PID) ya queesto puede complicar la determinacion de la estabilidad del sistema.

El problema de regulacion para el manipulador (1) se resuelve a partir del siguienteresultado en el cual se prueba que la posicion de las coordenadas generalizadas q(t)converge a su posicion deseada qd (t).

Teorema 1Si qd = 0 y Kv , Kp > 0 (Matrices definidas positivas) entonces el sistema en lazocerrado (1)-(2) tiene un punto de equilibrio q = qd globalmente asintoticamenteestable.

Demostracion. Considerando qd = 0, el sistema en lazo cerrado es,

M (q) q + C (q, q) q +Kv q + kp (q qd ) = 0. (3)Notese que el punto de equilibrio del sistema anterior resulta

q0 = qd

entonces, sin perdida de generalidad, se puede considerar qd = 0.

Recuerdese que si qd 6= 0, existe un cambio de coordenadas p = q q0, p = q quetransforma el sistema (3) en un nuevo sistema en las coordenadas p = q q0, p = qel cual tiene presenta un equilibrio en el origen.

I Por lo tanto puede considerarse qd = 0.

Considere la funcion candidata de Lyapunov,

V (q, q) =1

2qT M (q) q +

1

2qT Kpq. (4)

Entonces, V es radialmente no acotada y definida positiva.

Derivando V a lo largo de las trayectorias del sistema,

V (q, q) = qT M (q) q + 12 qT M (q) q + qT Kpq

= qT[C (q, q) q Kv q

kpq

]+ 12 q

T M (q) q +

qT Kpq

= qT Kv q + 12 qT M (q) q qT C (q, q) q= qT Kv q + 12 qT

[M (q) 2C (q, q)] q.

(5)

Como M (q) 2C (q, q) es antisimetrica,

V (q, q) = qT Kv q (6)donde V resulta semidefinida negativa por lo que solo se obtiene estabilidad del puntode equilibrio.

Considerando el Teorema de LaSalle, el conjunto S para el cual V = 0 resulta,

S ={

q, q| V = 0} = {q = 0} . (7)Para encontrar el maximo conjunto invariante contenido en S , se sustituye q = 0 en laecuacion de lazo cerrado (3), donde se considera,

q = 0 q = 0y por lo tanto se obtiene,

Kpq = 0. (8)

Como Kp es definida positiva, Kp es de rango pleno y por lo tanto q = 0, de donde seprueba que el equilibrio del sistema es asintoticamente estable.

Control PD aumentado (PD+)

Considere ahora el modelo general,

M (q) q + C (q, q) q +B (q) q + g (q) = u (9)

y el problema de control en el cual qd 6= 0.En este caso se pretende lograr el seguimiento de una trayectoria de referencia y nosolo estabilizar el equilibrio del sistema.En este caso, la retroalimentacion (2) puede modificarse como

u = M (q) qd + C (q, q) qd +Bqd + g (q)Kv q Kp q (10)

Teorema 2Bajo el conocimiento de q, q, la ley de control (10) aplicada al sistema (9), resulta enun seguimiento asintotico de la trayectoria deseada si Kv , Kp > 0.

Demostracion. El sistema en lazo cerrado (9)-(10) toma la forma,

M (q) q + C (q, q) q +B q +Kv q +Kp q = 0

que puede reescribirse en la forma,

M (q) q + C (q, q) q +KBv q +Kp q = 0, KBv = B +Kv . (11)

Notese que el punto de equilibrio del sistema (11) resulta q = 0, q = 0.La ecuacion (11) representa la dinamica del error de seguimiento q del sistema en lazocerrado (9)-(10).Utilizando de nueva cuenta la funcion candidata de Lyapunov (4),

V (q, q) =1

2qT M (q) q +

1

2qT Kp q (12)

Se obtiene,

V = qT M (q) q + 12 qT M (q) q + qT Kp q

= qT[C (q, q) q KBv q

kp q

]+ 12 q

T M (q) q + qT Kp q

= qT[C (q, q) + 12 M (q)] q qT KBv q

= qT KBv q

(13)

lo cual prueba la estabilidad del error.La utilizacion del Teorema de LaSalle prueba la estabilidad asintotica del sistema enlazo cerrado. En este caso el conjunto S para el cual V = 0 resulta,

S = { q = 0} . (14)Y el maximo conjunto invariante contenido en S resulta,

q = q = 0

de donde se obtiene el resultado buscado.

Control por Par Calculado

Considere el modelo general de un robot manipulador,

M (q) q + C (q, q) q +B (q) q + g (q) = u. (15)

Se desea encontrar una estrategia de control que sea robusta con respecto al error deseguimiento ocasionado por las condiciones iniciales, al ruido de los sensores y aincertidumbres parametricas.Despreciando la dinamica de los actuadores y conservando solamente terminos deamortiguamiento en el mecanismo, el modelo (15) puede escribirse como,

D (q) q + C (q, q) q +B (q) q + g (q) = . (16)

Una estrategia simple consiste en cancelar las no linealidades del sistema e imponer eltorque necesario que venza las inercias del manipulador.

Considere entonces la retroalimentacion,

= D (q) qd + C (q, q) q +B (q) + g (q) (17)

donde qd es la senal de referencia deseada.Sustituyendo (17) en (16) se obtiene en lazo cerrado,

D (q) q = D (q) qdD (q) q = 0 (18)

donde q = q qd .Como D (q) es definida positiva y por lo tanto invertible, la ecuacion (18) esequivalente a

q = 0 = q = qd . (19)

I Por lo tanto, si la posicion y la velocidad inicial del manipulador son igual a cero(q = q = 0), el robot seguira la trayectoria deseada.

I Si el error inicial no es cero, la ley de control (17) no puede corregirlo.

Suponga que (19) se satisface y que q (t0) = a y q (t0) = b, entonces tt0

q () d = q (t) q (t0) = q (t) b

entonces tt0

tt0

q () d = t

t0( q () b) d = q (t) q (t0) b (t t0)

= q (t) a b (t t0) = 0.Por lo tanto, es claro que aunque q (t) = 0, la ley (19) no garantiza la convergenciade q, q a cero ya que conforme t se obtiene,

q b, q a + b (t t0) . (20)

Para mejorar el desempeno de la ley de control anterior, considere ahora laretroalimentacion,

= D (q) (qd Kv q Kp q) + C (q, q) q +Bq + g (q) (21)donde, como se ha definido anteriormente,

q = q qdcon qd la trayectoria deseada y kv , kp matrices constantes definidas positivas.

I La ecuacion (21) describe el denominado Par calculado.

Sustituyendo (21) en (15) se obtiene,

D (q) q = D (q) (qd Kv q Kp q)esto es,

D (q) ( q +Kv q +Kp q) = 0

y como D (q) es invertible se obtiene,

q +Kv q +Kp q = 0 (22)

que representa un sistema lineal facilmente estabilizable mediante la eleccionapropiada de las matrices Kv , Kp .

En el caso mas simple, Kv , Kp se eligen como matrices diagonales, produciendo lossistemas desacoplados de primer orden

qi +Kvii qi +Kpii qi = 0 (23)

donde los polos deben de colocarse en el lado izquierdo del plano complejo.La ley de control por Par Calculado consiste de dos partes. La ecuacion (21) se puedeescribir como,

= D (q) qd + C (q, q) q +B (q) + g (q) ff

+D (q) (Kv q Kp q) fb

. (24)

I Al termino ff se le conoce como la componente predictiva y provee la cantidadde momento necesaria para llevar al sistema a lo largo de la trayectoria nominal.

I El termino fb es la componente de retroalimentacion que reduce los errores deseguimiento de trayectoria.

Teorema 3Considere el manipulador (15) en lazo cerrado con la retroalimentacion (21). Si Kv ,Kp > 0 son matrices constantes (definidas positivas) entonces la retroalimentacion,

= D (q) (qd Kv q Kp q) + C (q, q) q +B (q) + g (q)aplicada al sistema (15), resulta en un seguimiento exponencial.

Demostracion. La dinamica (22) se puede escribir como,

d

dt

[qq

]=

[0 IKp Kv

] [qq

](25)

que bajo la consideracion de,

q1 = q, q2 = q, q = [q1, q2]T

se obtiene, [q1q2

]=

[0 IKp Kv

] [q1q2

]q = Aq.

Para probar la convergencia exponencial del error basta con probar que los valorespropios de A tienen parte real negativa.

Sea C un valor propio de A correspondiente al vector propio v = [ v1 v2 ]T ,con v 6= 0 y v C2n. Entonces [v = Av , (I A) v = 0],

[v1v2

]=

[0 IKp Kv

] [v1v2

]=

[v2

Kpv1 Kv v2]

(26)

Notese que:

I Si = 0 entonces v = 0 y por lo tanto = 0 no es un valor propio de A.

I Si 6= 0,{

v2 = 0 implica que v1 = 0v1 = 0 implica que v2 = 0

}Por lo tanto v1, v2 6= 0.

Recuerdese que en el caso complejo:

v = (v v ) 12donde (v v ) es el producto interior complejo definido como,

(u w ) = u1w1 + ...+ unwn = w ucon wi representando el complejo conjugado de wi y w

= wT la transpuestahermitiana de w .Supongase sin perdida de generalidad que v1 = 1, entonces,

v 1 2v1 = 2v 1 v1 = 2 v12 = 2

Entonces,2 = v 1 2v1 = v 1 (v1) = v 1 v2

= v 1 (Kpv1 Kv v2)= v 1 Kpv1 v 1 Kv v1

(27)

Como v 1 Kpv1 > 0 y v 1 Kv v1 > 0 ya que son formas cuadraticas se tiene entonces,

2 + + = 0 (28)

con , > 0 lo que permite concluir que la parte real de es negativa.

El teorema anterior puede demostrarse alternativamente al considerar la funcion deLyapunov

V =1

2qT q +

1

2qT Kp q. (29)

Notese que el equilibrio de (22) es el origen y que V es definida positiva cuandoKp > 0. Entonces

V = qT q + qT Kp q = qT (Kv q Kp q) + qT Kp q = qT Kv q. (30)Kv se puede descomponer en la forma,

Kv =1

2

(Kv +K

Tv

)+

1

2

(Kv K Tv

)= A+B

donde A > 0 es simetrica y B > 0 antisimetrica. Por lo tanto,

V = qT A q.Dado que

min (A) q2 qT A q max (A) q2

se tiene entonces,V min (A) q2 .

Como V es semidefinida negativa, se ha probado hasta este momento que el sistemaes solamente estable.Considerando de nuevo el Teorema de LaSalle,

S ={

q, q| V = 0} = { q = 0} .El maximo conjunto invariante contenido en S se obtiene al sustituir q = 0 en (22)obteniendo,

Kp q = 0

dado que q = 0. Entonces ( q,q) = (0, 0) es el maximo conjunto invariante contenidoen S y por lo tanto se prueba la estabilidad asintotica.La estabilidad exponencial se obtiene del hecho que (22) es un sistema lineal.

Control basado en la estructura pasiva de robots rgidos

Considere las siguientes variables,

q = q qdqr = qd q

s = q qr = q +q(44)

La siguiente retroalimentacion garantiza un seguimiento asintotico sin utilizar elTeorema de LaSalle.

Teorema 4Considere el robot manipulador,

D (q) q + C (q, q) q +Bq + g (q) = . (45)

Sea qd una senal continua, acotada y con primera y segunda derivadas continuas yacotadas; si > 0 en (44), entonces la ley de control,

= D (q) qr + C (q, q) qr +Bqr + g (q)Kv s (46)con Kv > 0, en lazo cerrado con el sistema (45) produce un seguimiento asintotico delas trayectorias deseadas qd , qd .

Demostracion. El sistema en lazo cerrado toma la forma,

D (q) q + C (q, q) q +Bq + g (q) = D (q) qr + C (q, q) qr +Bqr + g (q)Kv sesto es,

D (q) s + C (q, q) s +Bs +Kv s = 0D (q) s + C (q, q) s +KBv s = 0

(47)

donde KBv = B +Kv .Considere ahora la funcion,

V (t) =1

2sT D (q) s.

Su derivada temporal a lo largo de (47) produce,

V (t) = sT D (q) s +1

2sT D (q) s

= sT [C (q, q) s KBv s ] + 12

sT D (q) s

= sT[

1

2D (q) C (q, q)

]s sT KBv s

= sT KBv s.Por lo tanto,

lmt s = 0.

Demostracion alterna

Notese que la ecuacion (47) no solo depende de s sino tambien de la variable q. De laecuacion (44) es posible ver que estas dos variables se relacionan dinamicamente apartir de

q = q + s.Por lo tanto, el sistema en lazo cerrado (45)-(46) puede describirse formalmente apartir del sistema aumentado,

D (q) s + C (q, q) s +KBv s = 0q = q + s. (48)

La prueba anterior se realiza a partir de la convergencia de una funcion de Lyapunovque depende solo de s y por lo tanto no esta completa formalmente ya que no se tomaen cuenta la dinamica de q. La ecuacion (48) debe reescribirse en la forma,

D (q + qd ) s + C (q + qd , q + qd ) s +KBv s = 0q = q + s (49)

donde se ha considerado que q = q + qd .

El sistema (49) tiene un equilibrio en el origen, por lo tanto, considerando la funcionde Lyapunov,

V =1

2sT Ds +

1

2qT q.

Se obtiene entonces,

V = sT Ds +1

2sT Ds + qT q

= sT [Cs KBv s ] + 12

sT Ds + qT [q + s ]

= sT[

1

2D C

]s sT KBv s qTq + qT s

= sT KBv s qTq + qT s.Por lo tanto,

V min (KBv ) s2 min () q2 + q s

[ s q ] [ min (KBv ) 12 12 min ()] [ sq

]

Entonces, el sistema resulta asintoticamente estable si

min (KBv ) min () 14> 0, min (KBv ) min () >

1

4(50)

lo cual siempre puede satisfacerse ya que KBv y son matrices de diseno.De lo anterior se obtiene la convergencia de s y q lo cual implica

q 0 y s 0y entonces

q qd y s q 0 q qd .

Diseno en el espacio de trabajo

Basado en un cambio de coordenadas al espacio cartesiano.

I Se desea que el efector final siga una trayectoria predeterminada.

I Conociendo la trayectoria cartesiana del efector final, el uso de la cinematicainversa permite obtener una trayectoria articular deseada equivalente.

I Alternativamente, es posible realizar la tarea de control en el espacio Cartesiano,lo que se conoce como Control en el espacio de trabajo.

Sea f : Q Rp un mapeo suave e invertible entre las variables articulares q Q y lasvariables del espacio de trabajo x Rp .

I El caso optimo es aquel en el cual n = p, esto es, el numero de grados de libertaddel robot es igual al numero de variables del espacio de trabajo.

I Si p < 6, las variables del espacio de trabajo solo proporcionan unaparametrizacion parcial de SE (3) = R3 SO(3).

La dinamica de un manipulador esta dada en general por:

D (q) q + C (q, q) q +Bq + g (q) = (51)

La relacion de velocidad entre la posicion Cartesiana x Rp del efector final y lasvariables articulares se obtiene al considerar el Jacobiano del sistema,

x = J (q) q, J (q) =f

q. (52)

Considerando que f es un mapeo suave e invertible, se puede escribir,

q = J1x q = J1x + ddt

(J1

)x . (53)

Definiendo la notacion,

JT =(J1

)Tla ecuacion (51) produce,

D (q)

[J1x + d

dt

(J1

)x

]+ C (q, q) J1x +BJ1x + g (q) =

D (q) J1x +[

C (q, q) J1 +D (q)d

dt

(J1

)]x +BJ1x + g (q) =

Equivalentemente, como J (q) es invertible,

JT D (q) J1x +[JT C (q, q) J1 + JT D (q) ddt

(J1

)]x

+JT BJ1x + JT g (q) = JT (54)

Lo que produce la nueva representacion,

D (q) x + C (q, q) x + Bx + g (q) = F (55)

donde,

D (q) = JT D (q) J1, C (q, q) = JT[C (q, q) J1 +D (q) ddt

(J1

)]B = JT BJ1, g (q) = JT g (q) , F = JT .

(56)

Notese que (55) se encuentra en terminos de q y q. Es posible considerar que,

x = f (q) , q = f 1 (x) = q = J1 (q)q=f 1(x) x (57)y de esta forma realizar completamente el cambio de coordenadas y evitar el uso de q,q. Para propositos de control esto es innecesario por lo que se acostumbra utilizar larepresentacion (55).

La ecuacion (55) tiene la misma estructura que la ecuacion (51).Esto nos permite demostrar las siguientes propiedades:

Lema 5La ecuacion (55) satisface las siguientes propiedades

1. D (q) es simetrica, definida positiva.

2. D (q) 2C (q, q) Rnn es una matriz antisimetrica.

Demostracion: La propiedad (1) es trivial. Para demostrar (2) notese que,

D (q) 2C (q, q) = JT D (q) J1 + ddt

(JT

)D (q) J1 + JT D (q)

d

dt

(J1

) 2JT

[C (q, q) J1 +D (q)

d

dt

(J1

)].

Entonces,

D (q) 2C (q, q) = JT [D (q) 2C (q, q)] J1 + 2 ddt

(JT

)D (q) J1

2JT D (q) ddt

(J1

)= 2

d

dt

(JT

)D (q) J1 2JT D (q) d

dt

(J1

)= T .

Por lo tanto (recuerdese que para cualquier matriz antisimetrica A, se satisfaceA+AT = 0),(

D (q) 2C (q, q))+(

D (q) 2C (q, q))T

= T +( T

)T= 0 (58)

I Las dos propiedades anteriores permiten el empleo inmediato de las leyes decontrol desarrolladas en el espacio de coordenadas articulares.

Control por par calculado en el espacio de trabajo

Sistema:D (q) x + C (q, q) x + Bx + g (q) = F (59)

Retroalimentacion:

F = D (q) (xd Kv x Kp x) + C (q, q) x + Bx + g (q) (60)de donde

= JT F . (61)

I xd representa la trayectoria deseada en el espacio de trabajo.

I x representa el error de seguimiento en el espacio de trabajo, x = x xd .El sistema en lazo cerrado produce,

D (q) x = D (q) (xd Kv x Kp x) . (62)

Con lo cual, como D (q) es invertible, se obtiene la dinamica,

x + kv x + kp x = 0 (63)

donde kv y kp se eligen tales que (63) resulta una ecuacion asintoticamente(exponencialmente) estable.

I Recuerdese que una condicion necesaria y suficiente para estabilidades que kv ykp son matrices definidas positivas. En particular, eligiendo kv y kp diagonales seresuelve facilmente el problema.

I La prueba final sigue los pasos del problema resuelto anteriormente en el espacioarticular.