Control PID de un manipulador robótico de 5 GDL. Lima, Mayo 2010 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. E
11
Embed
CONTROL PID DE UN MANIPULADOR ROBOTICO DE 5 GRADOS DE LIBERTAD
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
Control PID de un manipulador robótico de 5 GDL. Lima, Mayo 2010 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.
E
Control PID de un manipulador robótico de 5 GDL. Lima, Mayo 2010 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.
Eslabón 𝜽𝒊 𝜶𝒊 𝒂𝒊 𝒅𝒊
1 𝜃1 90° 0 𝑙1
2 𝜃2 0° 𝑙2 0
3 𝜃3 0° 𝑙3 0
4 𝜃4 + 90° 90° 0 0
5 𝜃5 0° 0 𝑙4
Luego de hallar los parámetros DH, se calculan las matrices
homogéneas (A) de los eslabones.
La cinemática directa la obtenemos multiplicando las 5
matrices homogéneas (T), la cual se ha simplificado y
ordenado de la manera como se muestra a continuación.
𝑇50 = 𝐴1
0𝐴21𝐴3
2𝐴43𝐴5
4
𝑇05 =
𝑛𝑥 𝑜𝑥
𝑛𝑦 𝑜𝑦
𝑎𝑥 𝑝𝑥
𝑎𝑦 𝑝𝑦
𝑛𝑧 𝑜𝑧
0 0𝑎𝑧 𝑝𝑧
0 1
𝑛𝑥 = 𝑆5𝑆1 − 𝐶1𝐶5𝑆234
𝑛𝑦 = −𝑆5𝐶1 − 𝑆1𝐶5𝑆234
𝑛𝑧 = 𝐶5𝐶234
𝑜𝑥 = 𝐶5𝑆1 + 𝐶1𝑆5𝑆234
𝑜𝑦 = −𝐶5𝐶1 + 𝑆1𝑆5𝑆234
𝑜𝑧 = −𝑆5𝐶234
𝑎𝑥 = 𝐶1𝐶234
𝑎𝑦 = 𝑆1𝐶234
𝑎𝑧 = 𝑆234
𝑝𝑥 = 𝐶1(𝑙2𝐶2 + 𝑙3𝐶23 + 𝑙4𝐶234)
𝑝𝑦 = 𝑆1(𝑙2𝐶2 + 𝑙3𝐶23 + 𝑙4𝐶234 )
𝑝𝑧 = 𝑙1 + 𝑙2𝑆2 + 𝑙3𝑆23 + 𝑙4𝑆234
𝛼 = 𝑞2 + 𝑞3 + 𝑞4
𝛽 = 𝑞5
Como se observa en las ecuaciones anteriores, se han
incluido las variables 𝛼 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.
2
1
3
(xw,yw)
(xe,ye)
l3
l4
r
l2
l1
Z
X'
Figura 4. Giro de los 4 GDL restantes.
𝒒𝟏 = 𝒂𝒕𝒂𝒏𝟐(𝒑𝒚,𝒑𝒙)
𝜑1 = 𝛼
𝑥𝑒 = 𝑝𝑥2 + 𝑝𝑦
2
𝑦𝑒 = 𝑝𝑧 − 𝑙1
𝑥𝑤 = 𝑥𝑒 − 𝑙4𝑐𝑜𝑠𝜑1
𝑦𝑤 = 𝑦𝑒 − 𝑙4𝑠𝑒𝑛𝜑1
Control PID de un manipulador robótico de 5 GDL. Lima, Mayo 2010 3
𝜑2 = atan(𝑦𝑤𝑥𝑤
)
𝜑3 = acos(𝑙2
2 + 𝑙32 − 𝑥𝑤
2 − 𝑦𝑤2
2𝑙2𝑙3)
𝒒𝟑 = 𝝅 − 𝝋𝟑
𝛾 = acos(𝑥𝑤
2 + 𝑦𝑤2 + 𝑙2
2 − 𝑙32
2𝑙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.
𝑱 =
𝜕𝑝𝑥
𝜕𝑞1
𝜕𝑝𝑦
𝜕𝑞1
𝜕𝑝𝑧
𝜕𝑞1
𝜕𝛼
𝜕𝑞1
𝜕𝛽
𝜕𝑞1
𝜕𝑝𝑥
𝜕𝑞2
𝜕𝑝𝑦
𝜕𝑞2
𝜕𝑝𝑧
𝜕𝑞2
𝜕𝛼
𝜕𝑞2
𝜕𝛽
𝜕𝑞2
𝜕𝑝𝑥
𝜕𝑞3
𝜕𝑝𝑦
𝜕𝑞3
𝜕𝑝𝑧
𝜕𝑞3
𝜕𝛼
𝜕𝑞3
𝜕𝛽
𝜕𝑞3
𝜕𝑝𝑥
𝜕𝑞4
𝜕𝑝𝑦
𝜕𝑞4
𝜕𝑝𝑧
𝜕𝑞4
𝜕𝛼
𝜕𝑞4
𝜕𝛽
𝜕𝑞4
𝜕𝑝𝑥
𝜕𝑞5
𝜕𝑝𝑦
𝜕𝑞5
𝜕𝑝𝑧
𝜕𝑞5
𝜕𝛼
𝜕𝑞5
𝜕𝛽
𝜕𝑞5
Primero calculamos el jacobiano lineal.
𝐽11 =𝜕𝑝𝑥
𝜕𝑞1= −𝑆1(𝑙4𝐶234 + 𝑙3𝐶23 + 𝑙2𝐶2)
𝐽21 =𝜕𝑝𝑦
𝜕𝑞1= −𝑆1(𝑙4𝐶234 + 𝑙3𝐶23 + 𝑙2𝐶2)
𝐽31 =𝜕𝑝𝑧
𝜕𝑞1= 0
𝐽12 =𝜕𝑝𝑥
𝜕𝑞2= −𝐶1(𝑙4𝑆234 + 𝑙3𝑆23 + 𝑙2𝑆2)
𝐽22 =𝜕𝑝𝑦
𝜕𝑞2= −𝑆1(𝑙4𝑆234 + 𝑙3𝑆23 + 𝑙2𝑆2)
𝐽32 =𝜕𝑝𝑧
𝜕𝑞2= 𝑙4𝐶234 + 𝑙3𝐶23 + 𝑙2𝐶2
𝐽13 =𝜕𝑝𝑥
𝜕𝑞3= −𝐶1(𝑙4𝑆234 + 𝑙3𝑆23)
𝐽23 =𝜕𝑝𝑦
𝜕𝑞3= −𝑆1(𝑙4𝑆234 + 𝑙3𝑆23)
𝐽33 =𝜕𝑝𝑧
𝜕𝑞3= 𝑙4𝐶234 + 𝑙3𝐶23
𝐽14 =𝜕𝑝𝑥
𝜕𝑞4= −𝑙4𝐶1𝑆234
𝐽24 =𝜕𝑝𝑦
𝜕𝑞4= −𝑙4𝑆1𝑆234
𝐽34 =𝜕𝑝𝑧
𝜕𝑞4= 𝑙4𝐶234
𝐽15 =𝜕𝑝𝑥
𝜕𝑞5= 0
𝐽25 =𝜕𝑝𝑦
𝜕𝑞5= 0
𝐽35 =𝜕𝑝𝑧
𝜕𝑞5= 0
Ahora hallamos el jacobiano angular y lo unimos al
jacobiano escalar que se calculó previamente.
𝑱 =
𝐽11
𝐽21
𝐽31
00
𝐽12
𝐽22
𝐽32
10
𝐽13
𝐽23
𝐽33
10
𝐽14
𝐽24
𝐽34
10
𝐽15
𝐽25
𝐽35
01
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
𝐽 = [𝐼]5 para esos puntos de singularidad.
Control PID de un manipulador robótico de 5 GDL. Lima, Mayo 2010 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