Sistemas Mecanicos SubActuados Ing. Robert Gerardo Castro Salguero Universidad Nacional de Ingenieria Facultad de Ingenieria Electrica y Electronica Seccion de Postgrado Asesor PhD Arturo Rojas Marzo del 1 1
1 Control de Sistemas Mecanicos Subactuados Figure 1: Fotografia del Acrobot En esta sección presentamos algunas ideas sobre el control de sistemas mecánicos Subactuados usando switching y saturación. Enfocamos el problema de control swingup para una clase de robot gimnasta. La metodología esta basada en linearizacion parcial de la realimentacion en una primera etapa para linealizar los grados de libertad actuados seguido del control de transferencia de la energía del grado de libertad actuado al no actuado en una segunda etapa. En un tipico control swingup el equilibrio es inestable en el sistema de lazo cerrado como consecuencia del comportamiento de fase no minima del sistema. Por esta razon es necesario para el controlador switch en el tiempo apropiado para un controlador hacer el equilibrio estable. La exitosa implementacion del control switching ha demostrado ser no trivial, tanto en la simulación como en la experimentacion. Discutiremos ambos metodos de diseno global y local y presentaremos algunos resultados de la simulacion. 1.1 Introduccion Sistemas mecanicos subactuados son aquellos con menos actuadores que grados de libertad y se eleva en varios caminos, de diseno intencional como el robot brachiation de Fokuda o el acrobot, en sistemas de robot movil cuando un brazo manipulador es adherido a una plataforma movil, un espacio de plataforma, o un vehiculo sumergido, o porque el modelo matematico usado por el diseno de control como cuando una junta flexible es incluida en modelo. En tal sentido, entonces, todos los sistemas mecanicos son subactuados si uno desea modos de control flexible que no son directamente actuados (el problema de no colocacion), o nivelar para incluir alguna cosa como actuador dinamico en la descripcion del modelo. Consideraremos un sistema
de n grados de libertad con coordenadas generalizadas q 1,..., q n,ym n actuadores, cada uno de los cuales actua directamente en un simple grado de libertad. Particionamos el vector q R n de coordenadas generalizadas como q 1 R l y q R m, donde q 1 R l representa las juntas activas (actuadas). La ecuacion de Euler-Lagrange del movimiento de tales sistemas son dadas por: M 11 q 1 + M 1 q + h 1 + φ 1 = (1) M 1 q 1 + M q + h + φ = τ () Donde: M11 M M(q) = 1 (3) M 1 M Esta matriz de inercia es simetrica y definida positiva, las funciones vectoriales h 1 (q, q) R l y h (q, q) R m contienen terminos de colioris y centrifugos, la funcion vectorial φ 1 (q) R l y φ (q) R m contiene terminos gravitacionales, y τ R m, representa la fuerza de entrada generalizada producida por m actuadores en las juntas activas. Por simplicidad notacional de aqui en adelante no escribiremos la explicita dependencia de q de aquellos coeficientes. 1. Linearizacion Parcial de la Realimentacion A diferencia de sistemas completamente actuados, los cuales son siempre linealizables por realimentacion, el sistema (1)-(), es no linealizable en las coordenadas q, aunque en algunos casos, el sistema es linealizable despues de transformacion de coordenadas no lineales. Sin embargo, podemos linealizar una porcion del sistema en la original coordenada q. Para ver esto consideremos la ecuacion (1): M 11 q 1 + M 1 q + h 1 + φ 1 = (4) El termino M 11 es una matriz invertible lxl como una consecuencia de que la matriz de inercia del robot M es definida positiva en (3). Por lo tanto podemos resolver q 1 en la ecuacion (4) como: q 1 = M 1 11 (M 1 q + h 1 + φ 1 ) (5) Y sustituir la expresion resultante (5) en () para obtener: Donde los terminos M 1, h y φ esta dado por: M q + h + φ = τ (6) M = M M 1 M 11 1 M 1 h = h M 1 M 11 1 h 1 3
φ = φ M 1 M 11 1 φ 1 Se puede demostrar que la matriz M es tambien simetrica y definida positiva. Un controlador de linearizacion parcial de la realimentacion puede por lo tanto ser definido por la ecuacion (6) de acuerdo a: τ = M u + h + φ (7) Donde u R m es una entrada de control adicional. El sistema completo hasta este punto puede ser escrito como: Haciendo: Y definiendo variables de estado: M 11 q 1 + h 1 + φ 1 = M 1 u (8) q = u (9) u = k 1 q k q + u (1) z 1 = q z = q η 1 = q 1 η = q (11) Podemos escribir en sistema en espacios de estado como: z = Az + Bu (1) η = ω(z,η,u) (13) Donde z T =(z 1 T,z T ), η T =(η 1 T,η T ), y A es la matriz de Hurwitz. Vemos de (1) y (13) que z =yu = define una invariante multiple es espacio de estado. Desde que A es una matriz de Hurwitz para valores positivos de ganancia en las matrices k p y k d esto resulta atractivo. La dinamica de la manipulacion esta dada por: η = ω(,η) (14) Tomaremos como punto inicial del problema el diseno de la entrada de control u para estabilizar el sistema (1)-(13). Esta clase de sistemas falla en algunas clases de sistemas realimentados. 1.3 El Acrobot El Acrobot es un robot en el plano con dos eslabones con un simple actuador en el codo. Las ecuaciones de movimiento estan dados por (4): m 11 q 1 + m 1 q + h 1 + φ 1 = (15) m 1 q 1 + m q + h + φ = τ (16) 4
Figure : Esquema del Acrobot Donde: m 11 = m 1 l c1 + m (l 1 + l c +l 1 l c cos(q )) + I 1 + I m = m l c + I m 1 = m1 = m (l c + l 1 l c cos(q )) + I h 1 = m l 1 l c sin(q ) q m l 1 l c sin(q ) q q 1 h =m l 1 l c sin(q ) q 1 φ 1 =(m 1 l c1 + m l 1 )g cos(q 1 )+m l c g cos(q 1 + q ) φ = m l c g cos(q 1 + q ) Los parametros m i, l i, l ci y I i son masas, longitudes de eslabones, centros de masa y momentos de inercia respectivamente. La configuracin cero, q i =, en este modelo corresponde al brazo extendido horizontalmente. Por lo tanto, la tarea del control es mover el robot de la configuracion vertical inferior, q 1 = π/,q =, a la configuracin invertida: q 1 =+π/,q =. 1.3.1 Control usando switching y saturacion La estrategia es la siguiente: Primero aplicamos el control de la linearizacion parcial de la realimentacion (7) con el termino de lazo mas externo dado por (1). El sistema resultante puede ser escrito como: m 11 q 1 + h 1 + φ 1 = m 1 (u k q k 1 q ) (17) q + k q + k 1 q = u (18) Elegimos el control adicional u para balancear el segundo eslabon en fase con el movimiento del primer eslabon de tal forma que la amplitud del balanceo 5
del primer enlace incrementa en cada oscilacion. Esto puede ser alcanzado con control switching o control de saturacion. Se comprueba que la simple eleccion dada por: u = k 3 sat( q 1 ) (19) Donde sat() es la funcion de saturacion, incrementa la energia y levanta el acrobot. En el sistema de lazo cerrado, la configuracion de equilibrio estable en lazo abierto: q 1 = π/,q =, se hace inestable y la trayectoria es conducida hacia la configuracion invertida. El paso final es para cambiar a un controlador de balance cuando el controlador u alcanza el estado en la zona de atraccion del controlador de balance. Se han investigado varios metodos para diseno de controladores de balance, principalmente pseudolinealizacion y metodo cuadratico lineal. La parte dificil de esta estrategia es disenar la ganancia k i tal que el estado entero entra a la zona de atraccion del controlador de balance. Esto involucra busqueda para controladores robustos o controladores de balance con grandes zonas de atraccion y autosintonizacion de la ganancia, ambos de los cuales son problemas no triviales. 1.3. Control basado en Energia Aplicaremos el control de la linearizacion parcial de la realimentacion, donde el sistema resultante puede ser escrito como: m 11 q 1 + h 1 + φ 1 = m 1 (u k q k 1 q ) () q + k q + k 1 q = u (1) Entonces elegimos el control adicional u para balancear el segundo eslabon en fase con el movimiento del primero. Sea E la energia total del Acrobot: E = 1 m 11 q 1 + m 1 q + 1 m q + P (q 1,q ) () Donde P (q 1,q ) es la energia potencial. Sea E c la Energia del Acrobot en su posicion de balanceo, es decir la energia potencial en la configuracion q 1 = π/, q =. Haciendo Ẽ = E E c y elegimos u como: u = sat(ẽq 1) (3) P (q 1,q )=m 1 gl c1 sin q 1 + m g(l 1 sin q 1 + l c sin(q 1 + q 1 )) (4) E c = m 1 gl c1 + m g(l 1 + l c ) (5) Donde sat(), es la funcion de saturacion. La energia total E deberia converger a E c, mientras q y q converge a cero. La trayectoria de estado debera converger a un conjunto invariante consistente en la union de las dos configuraciones de equilibrio (q 1, q 1,q, q )=(±π/,,, ). Con el conjunto (E,Ė1,q, q )=(E c,,, ). El paso final es cambiar a un controlador de balance cuando se alcanza un estado cercano a la zona de atraccion. 6
1.3.3 Linealizacion de la union sin actuador Denominado el caso no colocado, (non-collocated): m 1 q + h 1 + φ 1 = m 11 v 1 (6) q 1 = v 1 (7) Si q d 1 (t) es una trayectoria de referencia dada. Para q 1 podemos elegir el termino de entrada v 1, como: v 1 = q d 1 + k d ( q d 1 q 1 )+k p (q d 1 q 1 ) (8) Donde k p y k d son ganancias positivas. Considerando: q d 1 = π/ yq d 1 = q d 1 = y haciendo: q 1 = z 1, q = z, se tiene: q 1 = z 1 q = z ż 1 = k p (π/ q 1 ) k d z 1 ż = d 11(k p (π/ q 1 ) k d z 1 ) h 1 φ 1 d 1 Se ha realizado una simulacion usando los siguientes parametros m 1 =1, m =1,l 1 =1,l =,l c1 =.5, l c =1,I 1 =.83, I =.33, g =9.8, k p =, k d = 8, ver la siguiente figura: Linearizacion Parcial de la Realimentacion del Acrobot q1 1 3 4 5 6 7 8 9 1 q(*pi) q 5 1 3 4 5 6 7 8 9 1 5 1 1 1 3 4 5 6 7 8 9 1 q1" 5 1 3 4 5 6 7 8 9 1 q" 4 1 3 4 5 6 7 8 9 1 Figure 3: Control mediante Linearizacion Parcial de la Realimentacion Se observa que el eslabon, rota 36 grados en estado estable. El problema de sintonizacion consiste en elegir un juego de ganancias para mover el 7
acrobot tan cerca posible del punto de equilibrio y entonces cambiar al controlador de balance. Se ilustra este proceso usando un Regulador Cuadratico Lineal, para balancear el Acrobot cerca a la vertical. La linearizacion de la dinamica del Acrobot cerca del equilibrio vertical, q 1 = π/, q = resulta en un sistema lineal controlable: ẍ = Ax + Bu Donde el vector de estado x =(q 1 π/,q, q 1, q ), la entrada u = τ, ylas matrices A y B, estan dadas por: A = 1 1 1.49 1.54 14.49 9.36 Usando LQR, con MatLab: A = B = 1 1 1 1.98 5.98 C = 1 1 y R = 1, con el controlador de realimentacion de estado u = kx, donde, k = 4.5 96.33 14.59 49.5, esta ley de control lineal es aplicada cuando el acrobot alcanza una posicion cercana a la vertical. La siguiente figura muestra un exitoso control usando linealizacion parcial de la realimentacion seguido de un Regulador Cuadratico Lineal. 6 Linearizacion Parcial de la Realimentacion + LQR 4 q1 q 4 6 8 1 3 4 5 6 7 Figure 4: control usando linealizacion parcial de la realimentacion seguido de un Regulador Cuadratico Lineal 8
1.4 Aplicacion del Control Predictivo al Acrobot 1.4.1 Ecuaciones dinámicas del sistema Vector de estados: x =q 1,q, q1, q1 El sistema se puede escribir como: M11 M 1 q1 + M 1 M q M 11 q 1 + M 1 q + h 1 + φ 1 = (9) M 1 q 1 + M q + h + φ = τ (3) h1 Φ1 + = h Φ τ q1 q M11 M = 1 M 1 M 1 ( τ h1 h Φ1 Φ ) Ademas, considerando: q 1 = z 1,y q = z, considerando un entrada u = τ y las salidas y 1 = q 1 y y = q, el sistema tendra la forma: ẋ = f(x) = M11 M 1 M 1 M 1 ( τ z 1 z h1 h Φ1 Φ ) (31) 1.4. Linealizacion del sistema Hacemos uso de la matriz Jacobiana que está dada por : q 1 q z 1 z = f 1 f 1 f 1 f 1 q 1 q z 1 z f ) f f f q 1 q z 1 z f 3 f 3 f 3 f 3 q 1 q z 1 z f 4 f 4 f 4 f 4 q 1 q z 1 z y = 1 1 q 1 q + z 1 z q 1 q z 1 z f 1 u f u f 3 u f 4 u u Usando el Toolbox de matemática símbolica para el modelo no lineal y cálculo de los Jacobianos en el punto de operación: 9
% Codificacion en Matlab: m1=1; m=1; l1=1; l=1; lc1=.5; lc=1; I1=.83; I=.33;g=9.8; syms m11 m m1 m1 h1 h q1 q z1 z M phi1 phi phi h tau u m11=m1*lc1*lc1+m*(l1*l+lc*lv+*l1*lc*cos(q))+i1+i; m=m*lc*lc+i; m1=m*(lc*lc+l1*lc*cos(q))+i; m1=m1; h1=-m*l1*lc*sin(q)*z*z-*m*l1*lc*sin(q)*z*z1; h=m*l1*lc*sin(q)*z1*z1; phi1=(m1*lc1+m*l1)*g*cos(q1)+m*lc*g*cos(q1+q); phi=m*lc*g*cos(q1+q); M=m11 m1; m1 m; h=h1;h; phi=phi1;phi; tau=;u; QQ=inv(M)*(tau-h-phi); f=z1; z; QQ(1,1); QQ(,1); v=q1, q, z1, z; U=u; q1=-pi/; q=; z1=; z=; u=; A=subs(jacobian(f,v)) B=subs(jacobian(f,U)) C=1 ; 1 D=; Luego de reemplazar valores tendremos: A = 1 1 1.49 1.54 14.49 9.36 Representación del sistema linealizado B =.98 5.98 C = 1 1 ẋ = Ax + Bu y = Cx 1.4.3 Resultados de la simulación del sistema Se ha implementado un programa para control predictivo, tanto control egoista como solidario y no se ha logrado controlar el sistema, al parecer debido a que la respuesta al escalon crece rapidamente y el sistema es inestable. 1
1 Input 1 Output 1 Amplitude 1 3..4.6.8 1 1. 1.4 Time (secs) 4 Input 1 Output 3 Amplitude 1..4.6.8 1 1. 1.4 Time (secs) Figure 5: Respuesta al escalon x 151 u y y1 4 6..4.6.8 1 1. 1.4 1.6 1.8 Tiempo en segundos x 151 u y salida y 4 6..4.6.8 1 1. 1.4 1.6 1.8 Tiempo en segundos Figure 6: Simulacion del Control Predictivo 11
1.5 Control en modo deslizante del Acrobot Se utiliza una estructura de manipulador dinamico y segundo metodo de Lyapunov a fin de estabilizar la superficie de deslizamiento de la interseccion de la superficie switching de una manera directa: 1.5.1 Esquema de Control en modo deslizante La ecuacion generica de un robot rigido de dos eslabones. u = M(q) q + B(q, q) q + G(q) (3) Donde u =τ 1 τ T, para el Acrobot consideraremos τ 1 =, se ha tomado el modelo del paper Simple sliding mode control scheme appied to robot manipulator de Bailey y Arapostathis (1987) y se ha adaptado al presente trabajo y los resultados se veran en la siguiente seccion: 1.5. Simulacion del Control en modo deslizante del Acrobot Existe un unico actuador en el codo y solo se logra controlar el eslabon, q, mientras que el eslabon 1, oscila ligeramente alrededor de su posicion inicial. 4 Un actuador en el codo (Acrobot): s1 6 8 4 6 8 1 1 1 s 1 4 6 8 1 1 q_d1 vs q1 q_d vs q 4 6 8 1 1 5 5 4 6 8 1 1 t seg Figure 7: Control deslizante del Acrobot 1
1.5.3 Simulacion del Control en modo deslizante sin actuador en el codo Existe un unico actuador y se logra controlar el eslabon 1, q 1, que va de π/ a +π/, mientras que el eslabon, esta girando. 1 Sin Actuador en el codo s1 1 4 6 8 1 1 s 1 4 6 8 1 1 q_d1 vs q1 q_d vs q 4 6 8 1 1 5 4 6 8 1 1 t seg Figure 8: Sin actuador en el codo 1.5.4 Simulacion del Control en modo deslizante con dos actuadores Aqui si se consigue controlar los dos angulos q1 y q de ( π/, )a(π/, ): Al parecer el control deslizante no permite controlar el acrobot, talvez habria que combinarlo con alguna otra tecnica para obtener mejores resultados. 13
1 Actuadores u1 y u s1 1 4 6 8 1 1 1 s q_d vs q q_d1 vs q1 1 4 6 8 1 1 4 6 8 1 1.5.5 4 6 8 1 1 t(seg) Figure 9: Dos actuadores 14