CONTROL DE LOS MOVIMIENTOS DE UN ROBOT 1. INTRODUCCIÓN. CONTROL DEL PUMA-560. 2. CONTROL PD DE UNA ARTICULACIÓN INDEPENDIENTE. a) Aproximación mecánica de una articulación independiente. b) Control PD. c) Efecto de las perturbaciones. 3. TÉCNICAS DEL PAR CALCULADO. a) Par calculado para articulaciones independientes. b) Par calculado para articulaciones acopladas. 4. IMPLEMENTACIÓN DIGITAL. 1. INTRODUCCIÓN. CONTROL DEL PUMA-560 Problema de control: conseguir que la trayectoria real coincida con la deseada. Posicion Deseada PLANIFICADOR DE TRAYECTORIAS Error de Posicion + - Tension CC al motor ALGORITMO DE CONTROL Posicion Real MOTOR CC + RESPUESTA DINAMICA SENSOR POSICION Dificultad: complejidad de la respuesta dinámica del robot (acoplamientos inerciales, fuerzas centrı́petas y de Coriolis, y carga gravitatoria). Solución sencilla: considerar cada articulación como un servomecanismo independiente. Problemas: respuesta lenta, poco precisa, vibraciones innecesarias. Ejemplo: PUMA-560. 2. CONTROL PD DE UNA ARTICULACIÓN INDEPENDIENTE 2.1. Aproximación mecánica de una articulación independiente Cáculo del par en el motor: θ(t) ENGRANAJE n JL θ(t) = nθm (t) τ (t) = τm (t)/n CARGA Jm θm(t) MOTOR CC Par motor: τm (t) ∗ (t) + τ ∗ (t) = τm L ∗ (t) = J τ̈ : par debido exclusivamente al motor. τm m m τL∗ (t) : par debido a la carga visto en el motor. τL∗ = nτL τL = JL θ̈ = nJL θ̈m Par motor: τm τL∗ = n2 JL θ̈m = (Jm + n2 JL )θ̈m = Jef θ̈m Jef = Jm + n2 JL → Momento de Inercia Eficaz (momento de inercia referido al eje del motor) Comportamiento dinámico de la articulación: se ha simplificado como, τm = Jef θ̈m No se han tenido en cuenta: efectos inerciales (acoplamiento entre distintas articulaciones), centrı́fugos y gravitatorios. Función de tranferencia del motor (sin autoinducciones ni rozamientos): Gm (s) = Θm (s) Ka = Vin (s) s (Ra Jef s + Ka Kb ) 2.2. Control PD Control PD: 1. Parte P: una ganancia kp grande permite aumentar la rapidez de la respuesta, reducir errores en régimen permanente y mejorar el rechazo a perturbaciones. 2. Parte D: proporciona una respuesta más estable (rápida y sin sobreoscilaciones). Diagrama de bloques: θd (t) + e(t) k p +sk d - 1 n v (t) in Ka + - τm (t) Ra . 1 θm (t) 1 sJ ef s θm (t) θ (t) n Kb MOTOR CC Acción de control: se introduce un bloque 1/n para contrarrestar el efecto del engranaje. kp + skd kp E(s) + kd Ė(s) E(s) = n n kp e(t) + kd ė(t) kp (θd (t) − θ(t)) + kd (θ̇d (t) − θ̇(t)) vin (t) = = n n Vin (s) = Funciones de transferencia: FTBA FTBC Ka (kp + skd ) s(sRa Jef + Ka Kb ) G(s) Ka (kp + skd ) = 2 M (s) = 1 + G(s) s Ra Jef + s(Ka Kb + Ka kd ) + Ka kp G(s) = Gc (s)Gp (s) = Requerimientos de diseño: respuesta rápida y sin error (error de velocidad pequeño) ⇒ amortiguamiento crı́tico o un ligero sobreamortiguamiento. Ka Kb + Ka kd Ka kp s+ = s2 + 2δωn s + ωn2 Ra Jef Ra Jef K a kp 2 ωn = Ka Kb + Ka kd Ra Jef p ≥1 δ = Ka Kb + Ka kd 2 K k J R a p ef a 2δωn = Ra Jef s2 + p 2 Ka kp Jef Ra − Ka Kb kd ≥ Ka Requerimiento adicional: que la frecuencia natural ωn del sistema no alcance la frecuencia de resonancia estructural ωr : 0 < ωn ≤ 0,5ωr Ka kp 0< ≤ (0,5ωr )2 Ra Jef Se puede comprobar: Ã ωr = ω0 J0 Jef !1/2 Despejando kp : ωr2 Jef Ra ω02 J0 Ra = 0 < kp ≤ 4Ka 4Ka Tomando kp máximo: p Ra ω0 J0 Jef − Ka Kb kd ≥ ka 2.3. Efecto de las perturbaciones Perturbaciones: además de la tensión aplicada vin (t), los efectos centrı́fugos y gravitatorios también contribuyen al par motor. Consideramos estos efectos mediante un par perturbador τper (t). + - s .e(t) TACOMETRO τper (t) kd . s θd (t) 1 n + θd (t) v (t) in Ka + - Ra τin (t) - + τm (t) . 1 θm (t) n sJ ef s θ (t) Kb kp e(t) PLANIFICADOR + - Respuesta del sistema: Tm (s) = Tin (s) − Tper (s) · ¸ Ka Θ(s) = Vin (s) − Kb s − Tper (s) Ra n · ¸ Ka kp + skd Θ(s) = (Θd (s) − Θ(s)) − Kb s − Tper (s) Ra n n n Θ(s) = Tm (s) Jef s2 kp + skd Ra Θd (s) − Tper (s) n Ka Θ(s) = nKa Ra Jef s2 + (Ka Kb + Ka kd )s + Ka kp Error en régimen permanente: Tper (s) Ra Jef s2 + (Ka Kb + Ka kd )s + Ka kp nRa θ(∞) = lı́m sΘ(s) = − lı́m [sTper (s)] s→0 Ka kp s→0 Θ(s) = −nRa TÉCNICAS DEL PAR CALCULADO 3. 3.1. Par calculado para articulaciones independientes Componentes de perturbación: pares centrı́fugos y gravitatorios, mas otros pares desconocidos: τper (t) = n (τC (t) + τG (t)) + τe (t) Técnica del par calculado: compensar los pares perturbadores conocidos calculándolos de forma anticipada. + - s .e(t) TACOMETRO COMPENSACION F.C.E.M. kd . θd (t) θd (t) CALCULO PAR DE COMPENSACION τcomp CONVERSION PAR/TENSION v comp Ra τper (t) Kb 1 n + v (t) Ka in Ka + - τin (t) Ra - + τm (t) . 1 θm (t) n sJ ef s Kb kp e(t) + - Respuesta del sistema: (kp + skd )Θd (s) + Vcomp (s) Ra − Tper (s) n Ka Θ(s) = nKa Ra Jef s2 + (Ka Kb + Ka kd )s + Ka kp Ka kp + skd Tcomp (s) Θd (s) + − Tper (s) Ra n n = nRa Ra Jef s2 + (Ka Kb + Ka kd )s + Ka kp Cálculo del par de compensación: τcomp (s) = n2 (τC (t) + τG (t)) Error en régimen permanente: θ(t = ∞) = − nRa lı́m [sTe (s)] θ (t) Mejoras a la técnica del par calculado: 1. Compensación de la fuerza contraelectromotriz (en lı́nea de puntos en gráfico anterior): Ka kp + skd Tcomp (s) Θd (s) + − Tper (s) Ra n n Θ(s) = nRa Ra Jef s2 + Ka kd s + Ka kp 2. Precálculo del par motor: Tcomp (s) = Jef Θ̈d (s) + n2 (τC (t) + τG (t)) µ ¶ J ef = n2 + τC (t) + τG (t) n2 Respuesta del sistema: ¡ ¢ Ka kp + sKa kd + s2 Ra Jef Θd (s) − Te (s) Θ(s) = Ra Jef s2 + Ka kd s + Ka kp Caso de Te (s) = 0: θ(t) = θd (t) ∀t Fuentes de Error: 1. Te (s) 6= 0 en general (p.ej., pares τG y τC no exactamente compensados). 2. La caracterı́stica par/tensión no es ideal. 3. No se han considerado los acoplos inerciales entre articulaciones. Soluciones: 1. Caracterı́stica par/tensión: uso de tablas obtenidas experimentalmente. 2. Acoplos inerciales: uso del modelo dinámico. 3.2. Par calculado para articulaciones acopladas Par calculado anteriormente: 0 τcomp (t) = n2 τcomp (t) Jef θ̈d (t) + τC (t) + τG (t) n2 0 θ̈d (t) + τC (t) + τG (t) = Jef 0 τcomp (t) = 0 = Jef Jef n2 Compensación de acoplos debidos a las aceleraciones: uso del modelo dinámico completo. 0 τcomp (t) = D(q)q̈d + h(q, q̇) + c(q) Problema: gran cantidad de cáculo. Soluciones: 1. Simplificar el cáculo de τcomp . Inconvenientes: mal comportamiento (vibraciones) a altas velocidades. a) Calcular sólo los elementos diagonales de D(q) b) No calcular los términos centrı́fugos. 2. Realizar el cálculo de la secuencia de pares de compensación con antelación al movimiento. 4. IMPLEMENTACIÓN DIGITAL Esquema general de un control con par calculado: + A/D COMPENSACION ERROR VELOCIDAD . qd (t) PLANIFICADOR qd (t) CALCULO PAR DE COMPENSACION τcomp CONVERSION TACOMETRO COMPENSACION F.C.E.M. q (t) v (t) + D/A in MOTOR CC ENGRANAJE PAR/TENSION COMPENSACION ERROR POSICION + A/D COMPUTADOR TARJETA INTERFASE ROBOT Simplificación de la interfase: uso de codificadores y de entrada de pulsos al motor. v (t) in V max v̄in = t Frecuencia de muestreo: Fs = 1/T • Regla práctica: Fs ≥ 20fn • Valores tı́picos de resonancias: 5-10 Hz fn ≤ 0,5fr = 2,5-5 Hz ⇒ Fs ≥ 50 Hz. • Valor tı́pico de Fs : 60 Hz (T = 16 mseg). t1 Vmax t1 + t2