Modelamiento, Añalisis , Diseño, Simulación CAD de Brazo manipulador ROBOT de 6 DoF en Lenguaje R www.fernandojimenezmotte.com

UNIVERSIDAD SAN IGNACIO DE LOYOLA FACULTAD DE INGENIERÍA

Proyecto Integrador: Brazo manipulador de 6 DoF Curso:

Robótica

Bloque:

FC-SMVINF08E1T

Instructor:

Fernando Mauricio Jiménez Motte

Equipo:

Código:

Palli Laura, Iossif Moisés

1211424

Pardavé Andía, Christian

1320451

Ramos Mera, Brayams Joel

1312576

Rodríguez Maraví, Franz Anthony

1311695

Lima – Perú 2016

ÍNDICE ÍNDICE ....................................................................................................... 2 INTRODUCCIÓN ....................................................................................... 3 CONTENIDO ............................................................................................. 4 Modelamiento ............................................................................................ 4 Análisis ...................................................................................................... 8 Diseño .................................................................................................... 100 Simulación ............................................................................................. 101 Rapid Prototyping .................................................................................. 109 RosR ...................................................................................................... 109 Cuaterniones.......................................................................................... 111 CONCLUSIONES .................................................................................. 112 BIBLIOGRAFÍA ...................................................................................... 113

2

INTRODUCCIÓN En este segundo proyecto de robótica, el enfoque se halla en un reto planteado que consiste en mover objetos haciendo uso del gripper de un brazo robótico con 6 grados de libertad (6 DoF). En este proyecto, al igual que con anteriores, haremos uso del enorme poder computacional que nos brinda el lenguaje R, el cual, si bien se encuentra orientado al manejo de data para fines estadísticos, nos permite extrapolar esta ventaja a la gestión de cálculos avanzados y llevándonos a un paso del procesamiento de Big Data; además de la multitud de librerías que posee, permitiendo a este lenguaje realizar gran cantidad de tareas. Tomando como punto de partida la premisa “If you can do it, you can program it”, R brinda todas las facilidades del caso para programar lo que se desee, haciendo al lenguaje, a la percepción del equipo, un lenguaje sumamente completo a nivel de procesamiento y funcionalidad. Acerca del proyecto, se estará trabajando con un brazo robótico a 6 grados de libertad, mas a diferencia de proyectos anteriores, este bien llamado Proyecto Integrador, no exigirá únicamente la precisión a nivel de software, sino que busca integrar hardware y software, mostrando no solo el funcionamiento y el código que llevó a cabo dicha tarea, sino que se busca también expresar todo el trabajo realizado a través del lenguaje universal de las matemáticas.

3

CONTENIDO Modelamiento 1. Para la estructura mecánica del manipulador de robot, determine los marcos de referencia o coordinate frames. Para el modelamiento del gripper, tanto a nivel de parámetros DenavitHartemberg, como a nivel de codificación, se decidió separarlos y considerar dos frames distintos para las dos terminaciones del gripper; por lo que también esto significa que se conseguirán dos matrices finales, una por cada parte del gripper.

4

2. Los marcos de referencia o coordinate frames, deberán ser fijados utilizando la convención de Denavit Hartenberg – DH. Como se mencionó previamente, se consideró una división final de frames para el gripper, por lo que se obtendrán dos matrices DenavitHartemberg finales, en base a la estructura previamente mostrada, la siguiente es la matriz modelada para desde la base hasta el fin de la extensión del brazo, donde iniciaría el gripper: Denavit-Hartemberg Matrix – Shoulder to Wrist: Frame

(Link Twist)

(Link Lenght)

(Link Offset)

(Joint Angle)

1 2 3 4 5 6

Denavit-Hartemberg Matrix – Grippers: Frame

(Link Twist)

(Link Lenght)

(Link Offset)

(Joint Angle)

(Link Twist)

(Link Lenght)

(Link Offset)

(Joint Angle)

7 8 9

Frame 10 11 12

5

3. Derive los parámetros de Denavit Hartenberg – DH, y las transformaciones homogéneas de los links adyacentes para i = 1, 2, 3, 4, 5 y 6 , como una función de los ángulos de juntura o revolución (joint revolution angles).

En base a los resultados obtenidos en las matrices DenavitHartemberg, se obtienen las siguientes transformaciones homogéneas para los links adyacentes. Shoulder to wrist:

6

Primer Gripper:

Segundo Gripper:

7

Análisis 1. Derive la cinemática directa o forward kinematics

A partir de las transformaciones homogéneas halladas previamente, es posible obtener las transformaciones que reflejan la cinemática directa del brazo robótica. De igual forma que se hizo al obtener las matrices Denavit-Hartemberg, se separó los dos gripper. Shoulder to Wrist:

8

Primer Gripper:

9

Segundo Gripper:

10

Rotation Matrix  Quaternion:

11

2. Calcule los resultados para diferentes combinaciones de valores de los ángulos de juntura Se procede a dar un valor a los Joint Angles, mas se asignarán valores a las variables correspondientes al Link twist, Link Lenght y Link Offset. Para el caso, se hallaron 5 ángulos variables, por lo que a los siguientes se les darán los siguientes valores: Frame

(Joint Angle)

1 2 3 4 5 6

De esta forma, las matrices Denavit-Hartemberg para el siguiente gráfico serán las siguientes: Denavit-Hartemberg Matrix – Shoulder to Wrist: Frame

(Link Twist)

(Link Lenght)

1 2 3 4 5 6

12

(Link Offset)

(Joint Angle)

Denavit-Hartemberg Matrix – Grippers: Frame

(Link Twist)

(Link Lenght)

(Link Offset)

(Joint Angle)

(Link Twist)

(Link Lenght)

(Link Offset)

(Joint Angle)

7 8 9

Frame 10 11 12

De igual forma, se considerará como posición inicial ( ) de los links la siguiente:

13

De esta forma, en base a los nuevos resultados obtenidos en las matrices Denavit-Hartemberg, se obtienen las siguientes transformaciones homogéneas para los links adyacentes. Shoulder to wrist:

14

Rotation Matrix  Quaternion:

15

Primer Gripper:

Segundo Gripper:

Rotation Matrix  Quaternion (Para Primer y Segundo Gripper):

16

3. En esta sección deberá resolver el problema de la Cinemática Inversa del manipulador de robot, es decir: Dado encuentre los ángulos de juntura que engendraron la posición cartesiana x, y, z del efector final. Note que este problema de la ingeniería es altamente no lineal. Con el fin de operar con mayor facilidad la cinemática inversa del manipulador, el equipo optó por parametrizar las ecuaciones a coordenadas polares, convirtiendo así una operación en 3 dimensiones, a una en dos dimensiones.

17

(Pitch Angle – Orientation)

Se presenta una vista superior (Top View) del brazo robótico, con el fin de mostrar más a detalle sobre lo que se está operando:

18

De igual forma, se muestra un enfoque en la perspectiva del gripper (Gripper View) con el fin de mostrar de forma más objetiva las operaciones realizadas en el mismo, así como las ecuaciones que se han ido obteniendo.

Con el fin de hacer uso de las identidades trigonométricas, se elevan al cuadrado y suman y .

19

Elbow Up Elbow Down

20

21

4. Determine matemáticamente las velocidades lineales y angulares del manipulador de robot. Qué significado tienen los Jacobianos de la estructura. Para conocer los Jacobianos, los cuales son aquellas expresiones matriciales encargadas de relacionar las velocidades de las junturas (joint velocities) del brazo robótico y las velocidades cartesianas (cartesian velocities) es necesario iniciar conociendo las velocidades de las junturas, las cuales son las velocidades angulares con las que estas ejercen movimiento. Dichas velocidades se representan con la matriz derivada de los joint angles obtenidos junto con la matriz Denavit-Hartemberg, los cuales al ser multiplicados por los jacobianos, dan como resultado la velocidad cartesiana del efector final, donde es ejercido el torque respecto a la herramienta u objeto que pueda estar siendo afectado por la misma. La forma de operación de los jacobianos se puede explicar a través del siguiente diagrama:

22

(J non-singular)

Control 1

Joint 1

Control 2

Joint 2

Control 3

Joint 3

Control 4

Joint 4

Control 5

Joint 5

Control 6

Joint 6 6

Forward Kinematics

23

24

25

26

27

28

29

30

31

32

33

34

35

Posteriormente, se calculan las fuerzas y torques para los dos gripper, iniciando tanto del frame 9 (Gripper 1):

36

37

38

39

40

Donde para

:

41

Para

Y para

:

:

42

43

Luego, se calculan las fuerzas y torques para el segundo gripper, iniciando por el frame 12:

44

45

46

47

48

49

Finalmente, se muestra el jacobiano de fuerzas para el primer gripper a través de la trasposición de la matriz :

De igual forma, se obtiene el jacobiano de fuerzas para el segundo gripper a través de la trasposición de la matriz :

50

5. Investigue la dinámica del manipulador de robot. El comportamiento dinámico se describe en términos de la tasa de tiempo de cambio de la configuración de robot en relación con los torques ejercidos por los actuadores. Esta relación puede ser expresada por un conjunto de ecuaciones diferenciales, llamadas ecuaciones de movimiento, que rigen la respuesta dinámica del robot vinculación con los torques de entrada. Investigue que son los lagrangianos del manipulador de robot.

51

52

53

54

55

56

57

58

59

60

61

62

63

Donde para

[1, 1]:

64

Donde para

[2, 1]:

Donde para

[3, 1]:

65

Donde para

[1, 1]:

66

Donde para

[2, 1]:

67

Donde para

[3, 1]:

68

69

Donde para

1, 1]:

Donde para

[2, 1]:

70

Donde para

[3, 1]:

71

72

Donde para

[1, 1]:

Donde para

2, 1]:

73

Donde para

[3, 1]:

74

75

Donde para

[1, 1]:

Donde para

[2, 1]:

76

Donde para

[3, 1]:

Para calcular los tensores de inercia, es preciso definir previamente que se considerará la masa como un vector de masas:

77

78

79

Donde para

[1, 1]:

80

Donde para

[2, 1]:

81

Donde para

[3, 1]:

82

83

84

85

86

87

88

89

90

Para finalizar la dinámica, se calcula el torque ejercido por el manipulador, habiéndolo propagado desde la muñeca (wrist) hasta el hombro o base (shoulder):

Donde para :

91

Ecuaciones dinámicas del manipulador: Matriz de masa del manipulador:

Vector de fuerza centrífuga y Coriolis, conteniendo aquí aquellos términos que poseen dependencia de la velocidad de los joint:

Los términos que poseen al cuadrado son causados por la fuerza centrífuga. Por otro lado, aquellos que poseen la multiplicación de 2

92

joint velocities distintas, son causados por la fuerza Coriolis, la cual implica la gravedad ejercida por la tierra sobre el manipulador, puesto que esta fuerza, también afecta el torque ejercido por el manipulador. La gravedad contiene los términos gravitacionales g:

Ecuación Espacio-Estado Newton-Euler, la cual es una ecuación diferencial, vectorial y matricial:

Lagrangiano para calcular la energía cinética:

93

Energía potencial:

El lagrangiano define la diferencia entre la energía cinética y potencial de un sistema mecánico:

94

6. Plantee un Diagrama de Simulación para el Control por Computadora del Manipulador de Robot. Se trabajó el modelo de simulación en lenguaje R, tanto para el cálculo matemático, como para el modelado 3D, haciendo uso de la interfaz Twiddler de R. En la presente, modelamos tanto una opción con y sin frames explícitos. Frames Visibles:

95

Frames no Visibles:

De igual forma, se hizo uso de Twiddler, una interfaz con deslizadores que permite cambiar a distintos valores de ángulos para poder así movilizar los links del brazo robótico acorde a los Joint Angles.

96

Interfaz de Twiddler:

97

Para fines demostrativos, se hizo uso también de Shiny, otra interfaz que permite llevar a un modelo gráfico lo que Twiddler también ofrece, mas en esta ocasión, esto se realiza a través de una plataforma web, lo cual brinda la posibilidad que el público pueda utilizar sus propios dispositivos (Bring Your Own Device – BYOD). De esta forma, es posible observar el movimiento del simulador de brazo robótico.

De igual forma, es posible observar las matrices que representan la posición asignada al brazo robótico.

98

99

Diseño 1. Plantee un Diagrama de Simulación para el Control por Computadora del Manipulador de Robot. Implemente el robot utilizando diseño asistido por computadora CAD con los valores DH obtenidos y compare sus resultados utilizando la cinemática directa con las transformaciones homogéneas según la convención DH. Se hizo uso de la cinemática directa, tomando como referencia los cálculos realizados en el segundo apartado de la sección de análisis. Para este caso, R mostraba en consola las matrices de transformación desde cada gripper hasta el base frame por cada cambio realizado a través de la interfaz Twiddler, obteniendo, así como resultado, dos matrices finales.

Con el fin de corroborar estos resultados, se ingresaron los valores utilizados en el segundo apartado de la sección de análisis en MATLAB, de tal modo que se operó la cinemática directa, y tal como se muestra en dicho apartado, las matrices finales obtenidas son las siguientes para el primer y segundo gripper respectivamente:

100

Simulación 1. Generación de Secuencia: Escriba un “script” siguiendo el ejemplo del Anexo B, para ordenarle al manipulador de robot que tome las botellas de color siguiendo la siguiente secuencia en un cierto tiempo t: a) Verde V, Azul A, Rojo R b) Azul A, Verde V, Rojo R c) Rojo R, Verde V, Azul A Para el presente caso, se pueden tomar dos posibles escenarios, uno en el que se posean sensores como cámaras IP para poder detectar los colores de las botellas, o un sensor de colores RGB, así como un sensor de ultrasonido para poder calcular las distancias, y otro escenario en el que se ingresa la data manualmente en cuanto a ubicación, con el fin que el la siga a través de cinemática inversa. int [3]esquinaIni = [10, 10, 3] int [3]esquinaIni = [20, 20, 3] Si (sensorLuz.detectado() && sensorUS.detectado()) int alturaSensor = 1 ListaEnlazada objetosDetectados = recorrerBase(alturaSensor, 0, 359) Si (objetosDetectados.notEmpty()) Para i = 1 en objetosDetectados[i] objetosDetectados:tamaño()

hasta

//Se utiliza primera lectura de sensor de luz: rotar(objetosDetectados[i].getRotacion()) //Se utiliza la lectura del sensor de ultrasonido, //la cual es condicionada acorde la lectura del //sensor de luz: int [3] posicion = objetosDetectados[i].getPosicion() moverXY(posicion[1], posicion[2]) //Se aplica cinemática inversa: ang = calcularAngulos(posición[3]) //Se aplica cinemática directa: tomarObjeto(ang)

101

//Se mueve el objeto a la superficie deseada, considerándola //rectangular y plana por defecto, utilizando un contador para //separar los objetos por un espacio prudente, tomando en //consideración los límites de la superficie. moverObjeto(esquinaIni, esquinaFin, i) FinPara Sino Si (sensorLuz.detectado()) int alturaSensor = 1 ListaEnlazada objetosDetectados = recorrerBase(alturaSensor, 0, 359) Si (objetosDetectados.notEmpty()) Para i = 1 en objetosDetectados[i] objetosDetectados:tamaño()

hasta

rotar(objetosDetectados[i].getRotacion()) //Se pide al usuario ingresar posiciones de objetos int [3] posicion = ingresarPosiciones() moverXY(posicion[1], posicion[2]) ang = calcularAngulos(posición[3]) tomarObjeto(ang) moverObjeto(esquinaIni, esquinaFin, i) FinPara Sino Si (sensorUS.detectado()) int alturaSensor = 1 ListaEnlazada objetosDetectados = recorrerBase(alturaSensor, 0, 359) Si (objetosDetectados.notEmpty()) Para i = 1 en objetosDetectados[i] objetosDetectados:tamaño() //Se pide a usuario orden de recolección: int [] orden = ingresarOrden() rotar(orden)

102

hasta

int [3] posicion = objetosDetectados[i].getPosicion() moverXY(posicion[1], posicion[2]) //Se aplica cinemática inversa: ang = calcularAngulos(posición[3]) //Se aplica cinemática directa: tomarObjeto(ang) moverObjeto(esquinaIni, esquinaFin, i) FinPara Sino ListaEnlazada objetosDetectados = recorrerBase(alturaSensor, 0, 359) Si (objetosDetectados.notEmpty()) Para i = 1 en objetosDetectados[i] objetosDetectados:tamaño() int [] orden = ingresarOrden() rotar(orden) int [3] posicion = ingresarPosiciones() moverXY(posicion[1], posicion[2]) ang = calcularAngulos(posición[3]) tomarObjeto(ang) moverObjeto(esquinaIni, esquinaFin, i) FinPara FinSi

103

hasta

Finalmente, a nivel de robot real, se utilizó el siguiente algoritmo para lograr conectar R con el manipulador: observeEvent(input$move, { pitch = input$pitch x=input$x y=input$y z=input$z if(input$elbow){ up=-1; }else{ up=1; } theta1 = atan2(y,x) R = sqrt(x^2+y^2) Rw = R - (L3+b2)*cos(pitch) + d3*sin(pitch) Zw = z - (L3+b2)*sin(pitch) - d3*cos(pitch) c3 = (Rw^2+(Zw-L0)^2-L1^2-L2^2)/(2*L1*L2) s3 = up*sqrt(1-c3^2) #Elbow up or down theta3 = atan2(s3,c3) k1 = L1+L2*c3 k2 = L2*s3 gamma = atan2(k2,k1) theta2 = atan2(Zw-L0,Rw) - gamma theta4 = pitch - (theta2 + theta3) theta1 theta2 theta3 theta4

= = = =

theta1*180/pi theta2*180/pi theta3*180/pi theta4*180/pi

if(!is.nan(theta1) && !is.nan(theta2) && !is.nan(theta3) && !is.nan(theta4)){ t4 = theta4%%360 if (t4>180) t4=t4-360 plotArm(L0,L1,L2,L3,d3,b1,b2,theta1,theta2,theta3,t4,0,0, input$range,input$phi,input$rot,input$frame,FALSE) } con