CONTROL PID DE UN MANIPULADOR ROBOTICO DE 5 GRADOS DE LIBERTAD

(1)



Abstract

This research deals with the PID control for tracking the trajectory of a robotic manipulator of 5 degrees of freedom, from Lynx6 robot model. We performed a manual tuning of the gains of our controller, and a design which takes into account the electric dynamics of motors of direct current (DC) in order to use parameters and constraints that could appear in the implementation of our robot.

Index Terms: PID control, electric dynamics, PID tuning.

Resumen

El presente trabajo de investigación trata sobre el control PID para el seguimiento de trayectoria de un manipulador robótico de 5 grados de libertad, a partir del modelo del robot Lynx6. Se realiza una sintonización manual de las ganancias de nuestro controlador, y un diseño en cual se toma en consideración la dinámica eléctrica de los motores de corriente directa (DC) con la finalidad de usar parámetros y restricciones que se podrían presentar en la implementación de nuestro robot.

Index Terms: control PID, dinámica eléctrica, sintonización PID.

I. INTRODUCCION

n las últimas décadas, el problema de controlar manipuladores robóticos ha atraído y desafiado a muchos investigadores. Generalmente, la dinámica de un robot manipulador es caracterizada por la no linealidad y variación de parámetros debido a las variaciones en las cargas, fricciones y ruidos externos, etc. Los controladores convencionales para este tipo de sistemas podrían no arrojar resultados esperados. Por otro lado, los controladores convencionales como los PID, son ampliamente usados en la industria debido a su confiabilidad y facilidad de implementación.

Para esta primera entrega se realizó un control PID, sintonizándolo manualmente, considerando valores admisibles de torques y voltajes de los actuadores.

Este trabajo se realiza el proceso de modelado del robot manipulador. Primero se desarrolla la cinemática directa con el uso del algoritmo de Denavit-Hartenberg. La cinemática inversa, la cual incluye la dirección del efector, se calcula

directamente. La dinámica se determina usando Solidworks y el Simechanics de SIMULINK, y como comprobación también se calcula las matrices de dinámica usando el MATLAB con las ecuaciones de Lagrange. Luego, se realiza la compensación de gravedad con el objetivo de comprobar nuestras matrices de dinámica con la dinámica obtenida de Solidworks-Simulink. Finalmente se presenta el control PID para una trayectoria considerando la dinámica eléctrica de los motores y sus restricciones.

II. MODELAMIENTO DEL ROBOT Nuestro robot de 5 grados de libertad está basado en el manipulador Lynx6 de Lynxmotion, modelo del cual obtendremos la cinemática directa e inversa, jacobianos y la dinámica.

Figura 1. Robot manipulador Lynx6.

PID CONTROL OF A ROBOTIC MANIPULATOR OF 5

DEGREES OF FREEDOM

CONTROL PID DE UN MANIPULADOR ROBOTICO DE 5

GRADOS DE LIBERTAD

MSc. David R. Achanccaray D., Víctor Caballero L., Génesis Díaz M., Miguel Fernández Z., Franz Huanay M.

(2)

Representación del robot

Figura 2. Equivalente en D-H.

Cinemática Directa

Procedemos a calcular los parámetros D-H del manipulador para utilizar las matrices de transformación y encontrar la posición del efector final.

Estas ecuaciones son la base para desarrollar las simulaciones y observar la trayectoria que sigue el manipulador al utilizar diversos algoritmos en su control. Tabla 1. Parámetros DH del manipulador Lynx6.

4 3

_𝐴

5 4

𝑇

₀5

𝑧

𝑜

𝑧

0

0 𝑎

0

𝑧

𝛼

y

𝛽

, ya que se controlarán dos parámetros de orientación además de los que corresponden a la trayectoria.

Cinemática Inversa

Para hallar la cinemática inversa existen diversos métodos, de los cuales se eligió el método geométrico, por la facilidad del cálculo usando los parámetros de orientación elegidos y la posición final del efector.

Y

X (xe,ye)

X'

_

Figura 3. Giro del primer grado de libertad.

 ₂ ₁ _ _  3 (xw,yw) (xe,ye) l3 l4 r l2 l1 Z X'

Figura 4. Giro de los 4 GDL restantes.

𝒒

𝟏

= 𝒂𝒕𝒂𝒏𝟐(𝒑

𝒚

, 𝒑

𝒙

)

𝜑

₁

= 𝛼

𝑥

𝑒

= 𝑝

𝑥2

+ 𝑝

𝑦2

𝑦

𝑒

= 𝑝

𝑧

− 𝑙

1

𝑥

_𝑤

= 𝑥

_𝑒

− 𝑙

₄

𝑐𝑜𝑠𝜑

₁

𝑦

𝑤

= 𝑦

𝑒

− 𝑙

4

𝑠𝑒𝑛𝜑

1

(3)

𝜑

2

= atan⁡(

𝑦

_𝑤

𝑥

_𝑤

)

𝜑

₃

= acos⁡(

𝑙

2 2

_{+ 𝑙}

32

− 𝑥

𝑤2

− 𝑦

+ 𝑙

22

− 𝑙

32

2𝑙

₂

𝑥

_𝑤2

_{+ 𝑦}

𝑤2

)

𝒒

𝟐

= 𝝋

𝟐

− 𝜸

𝒒

_𝟒

= 𝝋

_𝟏

− 𝒒

_𝟐

− 𝒒

_𝟑

𝒒

𝟓

= 𝜷

OBS:

𝛼

y

𝛽

son datos de entrada.

Jacobianos

Partiendo del producto de matrices (T), hallamos el jacobiano.

𝑱 =

𝜕𝑻

𝜕𝑞

_𝑖

𝑖 = 1, 2, 3, 4, 5

Al jacobiano lineal de nuestro robot manipulador le hemos agregado el jacobiano angular con respecto a los parámetros de orientación

𝛼

y

𝛽

, con el objetivo de poder controlarlos también.

𝑱 =

𝜕𝛼

𝜕𝑞

₁

𝜕𝛽

₂

𝜕𝛽

₃

𝜕𝛽

₄

𝜕𝛽

₅

𝜕𝛽

𝜕𝑞

₅

Primero calculamos el jacobiano lineal.

𝐽

₁₁

2

)

𝐽

21

2

)

𝐽

31

=

𝜕𝑝

_𝑧

𝜕𝑞

1

= 0

𝐽

₁₂

2

)

𝐽

22

2

)

𝐽

32

₁₃

=

𝜕𝑝

𝑥

𝜕𝑞

₃

= −𝐶

1

(𝑙

4

𝑆

234

+ 𝑙

3

𝑆

23

)

𝐽

23

=

𝜕𝑝

𝑦

𝜕𝑞

₃

= −𝑆

1

(𝑙

4

𝑆

234

+ 𝑙

3

𝑆

23

)

𝐽

33

=

𝜕𝑝

=

𝜕𝑝

_𝑧

𝜕𝑞

₄

= 𝑙

4

𝐶

234

𝐽

₁₅

=

𝜕𝑝

𝑥

𝜕𝑞

₅

= 0

𝐽

₂₅

=

𝜕𝑝

𝑦

𝜕𝑞

₅

25

𝐽

₃₅

0

1

Ahora tenemos un jacobiano de 5x5, en el cual ya no se tendrán inconvenientes para calcular el jacobiano inverso, tan sólo se tendrán en cuenta los puntos de singularidad, en los cuales se tomará al jacobiano como una matriz identidad.

El jacobiano inverso se calcula de la siguiente manera:

𝐽

−1

₌

𝐴𝑑𝑗(𝐽)

det⁡(𝐽)

Existen puntos donde no está definido el jacobiano inverso, estos puntos son llamados de singularidad.

Singularidad ↔ det 𝐽 = 0

Para evitar este problema en las simulaciones, se asumió un

(4)

Dinámica

En esta parte procedemos a calcular el modelo dinámico de nuestro manipulador.

𝝉 = 𝑯𝒒 + 𝑪(𝑞, 𝑞 ) + 𝑮(𝑞)

𝝉

: Fuerzas y torques en los actuadores.

𝑯

: Matriz cuadrada de inercias.

𝑪

: Matriz columna de fuerzas de coriolis.

𝑮

: Matriz columna de gravedad.

Usando las siguientes ecuaciones se calculan las matrices ya mencionadas.

Primero se calcula el tensor de inercia.

𝑯 = (𝑱

_𝐿𝑖 𝑇

_𝑚

𝑖

𝑱

𝐿𝑖

+ 𝑱

𝑨𝒊 𝑇

𝑰

𝒊

𝑱

𝐴𝑖

)

𝑛

𝑖=1

𝑱

_𝐿𝑖_{: Jacobianos individuales respecto del centro de masa de} cada eslabón.

𝑚

𝑖: Masa de cada eslabón.

𝑰

_𝒊: Momento de inercia de cada eslabón.

Luego se calculan los coeficientes de Christoffel a partir del tensor de inercia.

ℎ

_𝑖𝑗𝑘

=

𝜕𝐻

𝑖𝑗

𝜕𝑞

𝑘

−

1

2 𝜕𝐻

_𝑗𝑘

𝜕𝑞

𝑖

Ahora calculamos la matriz columna de gravedad

𝐺

𝑖

= 𝑚

𝑗

𝒈

𝑇

𝑱

𝑳𝒊

𝒋 𝑛

𝑗 =1

𝒈

: Vector de gravedad respecto del sistema base.

Por último se forma la ecuación de la dinámica como se muestra a continuación.

𝜏

𝑖

= 𝐻

𝑖𝑗

𝑞

𝑗 5 𝑗 =1

+ ℎ

𝑖𝑗𝑘

𝑞

𝑗

𝑞

𝑘 5 𝑘=1 5 𝑗 =1

+ 𝐺

𝑖

La dinámica se obtuvo usando el MATLAB siguiendo este procedimiento, así como también usando el Simechanics de SIMULINK, el cual calcula la dinámica con algoritmos propios del software.

Compensación de la gravedad

Como paso previo al desarrollo del control se usó la compensación de gravedad para comprobar que nuestras ecuaciones de dinámica calculadas fuesen admisibles.

Figura 5. Compensación de la gravedad usando la dinámica de MATLAB.

Figura 6. Compensación de la gravedad usando el bloque del robot de simechanics.

III. LEY DE CONTROL DEL ROBOT

Principio de control PID

Para el estudio se realiza el control individual de cada elemento.

𝜏𝑖 = 𝐾𝑃𝑖 𝑞𝑑𝑒𝑠− 𝑞 + 𝐾𝐷𝑖 𝑞 𝑑𝑒𝑠− 𝑞 + 𝐾𝐼𝑖 𝑞𝑑𝑒𝑠− 𝑞 𝑑𝑇

𝑢𝑖 = 𝜏𝑖

(5)

Para nuestro modelo consideramos las siguientes ganancias del controlador: 𝐾𝑃 = 𝑑𝑖𝑎𝑔([100,100,70,45,10]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([20,30,20,10,10]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([150,140,100,85,45]) + -q control PID Motor ROBOT Motor DC e R

Figura 8. Esquema del Control PID. Primero se realizó el control PID directamente, sin saturaciones en las entradas, ni restricciones.

Figura 9. Bloque de Control PID.

Figura 10. Torques obtenidos con el PID inicial.

Función de Transferencia de Motores DC

Ahora modificamos el PID inicial, para poder utilizar las características de los motores DC. Se crea el diagrama de bloques dentro de nuestro lazo de control, el cual tendrá como entrada Voltaje y Torque como salida, teniendo en cuenta los valores límites de la entrada Voltaje.

Figura 11. Bloque del Motor DC.

Como se muestran en las figuras 11 y 12, se pueden modificar los parámetros de los motores de acuerdo al los valores reales que disponemos. Se construyó el modelo de los motores despreciando los efectos de viscosidad e inercia del motor por ser despreciables respecto a las inercias del robot.

Figura 12. Bloque de los actuadores de manipulador.

control PID Motor ROBOT R + -q Motor DC e

Figura 13. Esquema del control por PID usando la dinámica eléctrica de los actuadores.

Figura 14. Bloque de control PID usando la dinámica eléctrica de los actuadores.

En la figura 14 se observa que en el nuevo bloque de control PID, se ha tomado a los voltajes como variables de control, para luego calcular los torques correspondientes.

(6)

IV. SIMULACION Y RESULTADOS Control PID

A continuación se muestran los resultados obtenidos para una trayectoria helicoidal durante 30s de simulación.

Figura 15. Trayectoria helicoidal deseada (izquierda) y seguida (derecha). La trayectoria deseada es la siguiente.

𝑥 = 0.6 + 0.1cos⁡(𝑡) 𝑦 = 0.1sen⁡(𝑡) 𝑧 = 0.01𝑡 + 0.2 𝛼 = 0

𝛽 = 0

Figura 16. Trayectorias helicoidales superpuestas.

Figura 17. Variación de las orientaciones 𝛼 (arriba) y 𝛽 (abajo) durante

los 30s de simulación.

Figura 18. Voltajes durante el control.

Figura 19. Torques durante el control.

Como se puede observar, se ha saturado la alimentación de voltaje a los actuadores para simular condiciones reales en una posible implementación de nuestro robot.

Figura 20. Inicio de la simulación del control PID

Como se podrá observar, efectivamente el penúltimo eslabón del robot permanece casi horizontal (𝛼=0) durante toda la simulación, tal como se había previsto, de igual forma el efector final casi no rota en su eje ( 𝛽=0).

(7)

Figura 21. Fin de la simulación del control PID

Adicionalmente se realiza el control P, PD y PI del robot, de los cuales sólo se mostrarán los resultados del control PD, puesto que no se pudo sintonizar las ganancias para los otros dos, ya que el robot se movía descontroladamente.

Control PD

Con este control se obtuvieron resultados favorables. Se usaron los siguientes valores en las ganancias del controlador.

𝐾𝑃 = 𝑑𝑖𝑎𝑔([500,400,300,300,200]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([55,40,30,35,25]) 𝐾𝐼= 𝑧𝑒𝑟𝑜𝑠(5,5)

La trayectoria seguida es la misma que la anterior.

Figura 22. Torques al inicio del control PD.

Figura 23. Torques del control PD durante los 30s.

Figura 24. Errores de posición durante los 30s.

Figura 25. Trayectorias deseada y seguida por el robot.

Como se observa, los resultados son similares a los obtenidos en el control PID.

Control PID con desacople de la matriz H, C y G

(8)

Figura 27. Torques al inicio del control PID-HCG.

Figura 28. Torques durante los 30s.

Figura 29. Trayectorias deseada y seguida por el robot. Como se observan en los resultados, los torques obtenidos son mayores al inicio con respecto al control PID.

La trayectoria al inicio también cambió.

V. PRUEBAS ADICIONALES Control de Velocidad

En el control cinematico, además de la trayectoria espacial que debe ser realizada por el robot es necesario especificar la velocidad media del recorrido o la precisión con que se deba alcanzar el punto de destino. Esto implica que las constantes del controlador a usar dependen de la velocidad usada en la trayectoria. Haremos las pruebas para 4 trayectorias.

a. Trayectoria Helicoidal

Se incrementó la velocidad del recorrido sin modificar los parámetros del PID que usamos, el resultado fue el siguiente:

𝐾𝑃 = 𝑑𝑖𝑎𝑔([392,392,392,392,392]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([30,30,30,30,30]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([28,28,28,28,28])

Figura 30. Comparación de trayectorias incrementando la velocidad de

recorrido

La velocidad del recorrido para las trayectorias presentadas fueron: 4,15 y 30 cm/s respectivamente.

Figura 31.Diferencias entre las trayectorias para una velocidad de

recorrido de 30 cm/s.

Al incrementar la velocidad del recorrido se reducen los tiempos de asentamiento modificando así las especificaciones de diseño.

(9)

Se incrementó entonces las constantes Kd y Kp del PID para velocidades mayores y se obtuvo el siguiente resultado. Para v = 0.15 m/s 𝐾𝑃 = 𝑑𝑖𝑎𝑔([2450,2450,2450,2450,2450]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([30,30,30,30,30]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([70,70,70,70,70]) Para v = 0.30m/s 𝐾𝑃 = 𝑑𝑖𝑎𝑔([22050,22050,22050,22050,22050]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([30,30,30,30,30]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([210,210,210,210,210])

Figura 32. Comparación de trayectorias con Kd y Kp modificados

Figura 33. Incremento de torques para las tres velocidades.

b. Trayectoria rectilínea

Primero modificamos las velocidades del robot para ver su efecto en la trayectoria considerando las ganancias iniciales, que fueron sintonizadas para la frecuencia inicial

𝐾𝑃 = 𝑑𝑖𝑎𝑔([200,200,200,200,200]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([30,30,30,30,30]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([20,20,20,20,10])

Figura 34. Comparación de trayectorias rectilíneas incrementando la

velocidad de recorrido

La Figura 34 es el resultado del control PID sin modificar sus ganancias para velocidades 0.032, 0.096 y 0.192 m/s

Como se puede observar hay un error casi constante que se genera debido a este cambio de velocidad.

Para ello sintonizamos el PID para estas nuevas condiciones. Para v = 0.096 m/s 𝐾𝑃 = 𝑑𝑖𝑎𝑔([700,500,400,350,400]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([500,500,500,500,500]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([50,60,45,50,50]) Para v = 0.192m/s 𝐾𝑃 = 𝑑𝑖𝑎𝑔([800,800,600,500,400]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([1200,1200,1200,1200,1000]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([40,55,55,40,30])

(10)

Figura 36. Torques de entrada al robot para las 3 velocidades analizadas.

c. Trayectoria - sinusoide inscrito en un cilindro Considerando las ganancias iniciales, que fueron sintonizadas para la frecuencia inicial, tenemos las gráficas:

𝐾𝑃 = 𝑑𝑖𝑎𝑔([105.27,105.27,105.27,38.28,38.28]) 𝐾𝐷= 𝑑𝑖𝑎𝑔([30,30,30,40,30])

𝐾𝐼= 𝑑𝑖𝑎𝑔([2.45,2.45,2.45,1.75,1.75])

Figura 37. Comparación de trayectorias rectilíneas incrementando la

velocidad de recorrido

La Figura 37 es el resultado del control PID sin modificar sus ganancias para diferentes velocidades.

Como se puede observar hay un error que se incrementa debido a este cambio de velocidad.

Para ello sintonizamos el PID para estas nuevas condiciones. Para: 𝐾𝑃 = 𝑑𝑖𝑎𝑔([421,421,421,153,153]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([30,30,30,40,30]) 𝐾𝐼= 𝑑𝑖𝑎𝑔([4.9,4.9,4.9,3.5,3.5]) Para: 𝐾𝑃 = 𝑑𝑖𝑎𝑔([1364.3,1364.3,1364.3,496,496]) 𝐾𝐷 = 𝑑𝑖𝑎𝑔([30,30,30,40,30]) 𝐾𝐼 = 𝑑𝑖𝑎𝑔([8.82,8.82,8.82,6.3,6.3])

Figura 38. Trayectorias para la nueva sintonización de las ganancias.

(11)

VI. CONCLUSIONES

-El software SimMechanics de Matlab es una potente herramienta en la simulación de sistemas robóticos, ya que nos permite importar nuestro diseño realizado en algún programa CAD (en este caso SolidWorks) incluyendo las características inerciales del diseño, además permite realizar análisis dinámicos, cinemáticos, de dinámica inversa y también estática. La ventaja de utilizar este software de simulación es que permite realizar el análisis matemático y, en la mayoría de los casos, poder obtener importantes deducciones de una forma eficaz y rápida. Otra ventaja de este programa de simulación es que su complejidad no aumenta a medida que se añaden más grados de libertad, cosa que no sucede con el análisis matemático.

-Se realizó un control con regular precisión, los valores del error angular alcanzan unos cuantos ángulos para razonables magnitudes de torque, por lo cual el control realizado en el presente trabajo tiene un desempeño aceptable.

-El sobreimpulso de la respuesta transitorio está condicionado a partir de las condiciones iniciales del robot; es decir, mientras la distancia entre una posición cualquiera del robot y un punto inicial de la trayectoria sea mayor, la respuesta transitoria será más pronunciada.

-El incremento de la velocidad de recorrido requiere una reducción de los tiempos de asentamiento, sin embargo, al no modificar los parámetros del PID principalmente los Kd y Kp que están relacionados con los tiempos de corrección del controlador, la trayectoria real mostrará diferencias con respecto a la deseada incrementándose éstas a medida que la velocidad aumente (fig.30 y fig.31). Luego de modificar los Kd y Kp, se observó que, el Kp depende directamente con el cuadrado de la velocidad de recorrido y el Kd de manera proporcional. Los resultados de modificar los parámetros de acuerdo a esta ley se observan en la fig. 32.

-Si bien el sistema controlado con desacople de la matriz H, C y G queda aliviado de toda la dinámica acoplada de los eslabones y sus principales propiedades físicas normales, el torque de control queda directamente proporcional a los valores de aceleraciones angulares en cada junta, las cuales se incrementarán al incrementarse la velocidad de recorrido especificada por el usuario. Esto se puede observar en la fig. 33 donde los torques de control se hacen mayores a medida que la velocidad crece.

REFERENCES

[1] Ing. César Anchayhua Aréstegui.

Apuntes de clases: Análisis y control de robots Universidad Nacional de Ingeniería,

Facultad de Ingeniería Mecánica,

Escuela profesional de Ingeniería Mecatrónica. Lima - Perú 2010

[2] A. Barrientos, L. F. Peñin, C. Balaguer, R. Aracil Fundamentos de robótica, Mc Graw Hill, 2004 [3] John J. Craig

Introduction to robotics mechanics and control Addison-Wesley Publishing Company, 1989 [4] Schilling, J. Robert. , Fundamentals of Robotics.

Estados Unidos: Prentice-Hall, Inc. (1990). [5] “Introduction to Robotics” (2006), H. Harry Asada.

Massachussets Institute of Technology.

[6] Katsuhiko Ogata, “Ingeniería de Control Moderna”, Pearson, 2003, 4ta edición

[7] Ahmed A. Shabana, “Computational Dynamics”, Editorial John Wiley& Sons.