INGENIERÍA MECATRÓNICA
ROBOTICA INDUSTRIAL
NRC: 7759
INFORME
PROYECTO SEGUNDO PARCIAL
PROFESOR:
Ing. DAVID LOZA
INTEGRANTES
FABRICIO VEINTIMILLA
29 DE JULIO 2019
SANGOLQUÍ
Objetivos
•
Encontrar y programar la cinemática Inversa del robot de 2grados de libertad
desarrollado en el primer parcial.
Objetivos específicos
•
•
•
Formular la cinemática inversa del robot de 2 grados de libertad
Programar utilizando Python la cinemática inversa del robot
Diseñar una interfaz HMI utilizando Qt designer
Desarrollo
Cinemática directa
La cinemática directa de un robot de dos grados de libertad es sencilla. La forma más
intuitiva y que resulta muy visual, es el método geométrico. Para ello partimos de un
robot tipo SCARA que consta de 3 grados de libertad, sin embargo, si eliminamos la
junta prismática del final que tienen movimiento en el eje Z este se convierte en un
robot de 2 grados de libertad
Figura 1 Modelamiento de robot SCARA
Si vemos al robot desde una vista alzada tendríamos algo como esto
Figura 2 Cinemática directa por método geométrico
Donde 𝑙2 𝑦 𝑙3 son las longitudes de los brazos. 𝑞1 𝑦 𝑞2 los ángulos que recorre 𝑙2 con
respecto al eje x y el ángulo que recorre 𝑙3 con respecto al eje imaginario que genera 𝑙2
si extendemos una línea colineal a partir de ella, respectivamente.
Para encontrar la coordenada del un punto P a partir de los ángulos que gira cada
eslabón tenemos que
𝑝𝑥 = 𝑙2 cos(𝑞1 ) + 𝑙3 cos (𝑞1 + 𝑞2 )
𝑝𝑦 = 𝑙2 sin(𝑞1 ) + 𝑙3 sin(𝑞1 + 𝑞2 )
Las ecuaciones descritas anteriormente son la respuesta de la cinemática directa.
El modelo mecánico lo mostramos en la figura 3.
Figura 3 Modelo CAD del robot SCARA
Cinemática inversa
Para obtener la cinemática inversa del robot de dos grados de libertad, lo más sencillo, al
ser un robot muy simple es encontrar las ecuaciones mediante el método geométrico. En
este aparatado se busca que dados unos puntos x e y en el espacio de tarea obtener los
valores que deben tomar las articulaciones 𝑞1 y 𝑞2 .
Figura 4 Relaciones geométricas de un robot de 2GDL
En la imagen se observan relaciones geométricas importantes que se usaran para obtener
el modelo de cinemática inversa.
Para 𝒒𝟐
En la figura 4 podemos ver que existe un ángulo entre 𝑙1 y 𝑙2 al se le llamará 𝛼, entonces:
Realizando ley de cosenos tenemos:
Reemplazando por 𝛼:
Despejando:
𝛼 = 𝜋 − 𝑞2
𝑃𝑥𝑦 = 𝑙12 + 𝑙22 − 2𝑙1 𝑙2 cos(𝛼)
𝑃𝑥𝑦 = 𝑙12 + 𝑙22 + 2𝑙1 𝑙2 cos(𝑞2 )
cos(𝑞2 ) =
𝑥 2 + 𝑦 2 − 𝑙12 − 𝑙22
2𝑙1 𝑙2
Lo anterior podemos asignarlo a una variable de la siguiente manera:
𝑎 = cos(𝑞2 ) =
𝑥 2 + 𝑦 2 − 𝑙12 − 𝑙22
2𝑙1 𝑙2
De la identidad trigonométrica sen2(𝑥) + cos 2 (𝑥) = 1, despejando el seno tenemos:
sin(𝑞2 ) = ±√1 − cos2 (𝑞2 ) = ±√1 − 𝑎2
Por lo tanto:
±√1 − 𝑎2
)
𝑞2 = arctan (
𝑎
El signo dentro del arctan nos dará 2 respuestas validas que tienen el nombre de la
configuración de codo arriba o codo abajo.
Para 𝒒𝟏
En la figura 4 se observa que:
𝑦
𝛾 = arctan ( )
𝑥
también podemos definir el valor de 𝛽 (teniendo en cuenta el valor positivo de la raíz)
como:
𝑙2 √1 − 𝑎2
𝑙2 sin(𝑞2 )
) = arctan (
)
𝛽 = arctan (
𝑙1 + 𝑙2 cos(𝑞2 )
𝑙1 + 𝑙2 𝑎
Y finalmente sabemos que:
Resumiendo, tenemos que:
𝑞1 = 𝛾 − 𝛽
𝑞1 = 𝛾 − 𝛽
√1 − 𝑎2
𝑞2 = arctan (
)
𝑎
Donde:
𝑎=
𝑥 2 + 𝑦 2 − 𝑙12 − 𝑙22
2𝑙1 𝑙2
𝑦
𝛾 = arctan ( )
𝑥
𝛽 = arctan (
𝑙2 √1 − 𝑎2
)
𝑙1 + 𝑙2 𝑎
Programación
La utilización de roscore nos permite realizar espacios de trabajo, crear paquetes y
archivos.py que ayudan a ejecutar diferentes tareas al momento de diseñar un robot.
En este caso tenemos una tarjeta Arduino Uno que sería la encargada de gestionar los
movimientos de los servomotores. Para conectar el microcontrolador y la computadora,
se hace uso de las librería rosserial para Linux y ros-lib para Arduino. El uso de ambas
librerías nos permite una conexión serial entre dispositivos.
Con librería servo.h se puede calibrar el rango de movimiento del servomotor mediante
la duración de los pulsos como se ve en la figura #. Se calibró ambos motores haciendo
prueba y error hasta lograr un rango de movimiento satisfactorio. En los ángulos cercanos
a 0° existe un error debido al propio limite físico del robot, sin embargo para ángulos
cercanos a 180° se puede ver buena exactitud.
Figura 5 Modulación de ancho de pulso para servomotores
El código para el Arduino Uno es:
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <WProgram.h>
#endif
#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>
ros::NodeHandle
nh;
Servo servo1;
Servo servo2;
int pos1 = 0;
int pos = 0;
int mapSpeed = 0;
void servo_cb1( const std_msgs::UInt16& cmd_msg){
mapSpeed = map(30,0,30,30,0);
if(cmd_msg.data > pos){
for(pos = pos1; pos <= cmd_msg.data; pos +=1){
servo1.write(pos);
pos1 = pos;
delay(mapSpeed);
}
}
servo1.write(cmd_msg.data); //set servo angle, should be from 0-180
}
void servo_cb2( const std_msgs::UInt16& cmd_msg){
servo2.write(cmd_msg.data); //set servo angle, should be from 0-180
}
ros::Subscriber<std_msgs::UInt16> sub1("servo1", servo_cb1);
ros::Subscriber<std_msgs::UInt16> sub2("servo2", servo_cb2);
void setup(){
pinMode(13, OUTPUT);
nh.initNode();
nh.subscribe(sub1);
nh.subscribe(sub2);
servo1.attach(9,500, 2300); //attach it to pin 9
servo2.attach(10,500, 2300); //attach it to pin 10
}
void loop(){
nh.spinOnce();
delay(1);
}
La ventaja de ros-lib es que ya viene con un archivo de ejemplo para el control de
servomotores. Ese código se compila y se carga en el microcontrolador; esta librería
convierte a la tarjeta en un nodo. El la parte de ROS es importante identificar el puerto
serial al que se conectó la tarjeta Arduino. Una vez hecho esto, se crea un archivo.py
dentro de la carpeta src del paquete que se va a utilizar para poder crear los nodos para
así poder suscribirse y/o publicar en los tópicos que se quieran.
Una vez probado que la conexión entre Arduino y ROS es exitosa. Se procedió a la
programación de una HMI utilizando el software Qt designer. Esta es una herramienta
que puede ser utilizada con Python para crear HMI de manera rápida y con mejor aspecto
estético.
Seguidamente, el archivo .ui que nos entrega el Qt designer lo transformamos a un archivo
Python el cual desde otro archivo que sea principal se puede importar todas las cosas
utilizadas en el diseño que se hizo anteriormente
También, se realizó la programación en la cual un usuario nos da 2 valores de ángulo
(𝑞1 𝑦 𝑞2 ) : se calcula la cinemática directa, se envía los ángulos ingresados hacia la tarjeta
y se muestra en pantalla el punto correspondiente a esos dos ángulos.
Por último, el usuario puede poner los valores de los puntos a los que desea llegar y al
momento de dar calcular este resuelve la cinemática inversa y envía los valores calculados
de los ángulos hacia el controlador.
El código es el siguiente
#!/usr/bin/env python3
import sys
import numpy as np
from disenio_final5 import*
from PyQt5.QtWidgets import QApplication, QMainWindow
from PyQt5.QtCore import Qt, QPropertyAnimation
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as
FigureCanvas
import matplotlib.pyplot as plt
from sympy import atan2, cos, sin, pi
import rospy
from std_msgs.msg import UInt16
global theta
theta = 0
global fi
fi = 0
global px
global py
class MiApp(QMainWindow):
def _init_(self):
super()._init_()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.grafica = Canvas_grafica()
self.ui.grafica_uno.addWidget(self.grafica)
#eliminar barra y de titulo - opacidad
self.setWindowFlag(Qt.FramelessWindowHint)
self.setWindowOpacity(1)
#SizeGrip
self.gripSize = 10
self.grip = QtWidgets.QSizeGrip(self)
self.grip.resize(self.gripSize, self.gripSize)
# mover ventana
self.ui.frame_superior.mouseMoveEvent =
self.mover_ventana
#acceder a las paginas
self.ui.bt_inicio.clicked.connect(lambda:
self.ui.stackedWidget.setCurrentWidget(self.ui.page))
self.ui.bt_uno.clicked.connect(lambda:
self.ui.stackedWidget.setCurrentWidget(self.ui.page_uno))
self.ui.bt_dos.clicked.connect(lambda:
self.ui.stackedWidget.setCurrentWidget(self.ui.page_dos))
self.ui.bt_tres.clicked.connect(lambda:
self.ui.stackedWidget.setCurrentWidget(self.ui.page_tres))
self.ui.bt_cuatro.clicked.connect(lambda:
self.ui.stackedWidget.setCurrentWidget(self.ui.page_cuatro))
self.ui.bt_cinco.clicked.connect(lambda:
self.ui.stackedWidget.setCurrentWidget(self.ui.page_cinco))
#control barra de titulos
self.ui.bt_minimizar.clicked.connect(self.control_bt_minimizar)
self.ui.bt_restaurar.clicked.connect(self.control_bt_normal)
self.ui.bt_maximizar.clicked.connect(self.control_bt_maximizar)
self.ui.boton_calcular_directo.clicked.connect(self.calcular_pu
ntos)
self.ui.boton_calcular_inverso.clicked.connect(self.calcular_pu
ntos_inverso)
self.ui.bt_cerrar.clicked.connect(lambda: self.close())
self.ui.bt_restaurar.hide()
#menu lateral
self.ui.bt_menu.clicked.connect(self.mover_menu)
def control_bt_minimizar(self):
self.showMinimized()
def
control_bt_normal(self):
self.showNormal()
self.ui.bt_restaurar.hide()
self.ui.bt_maximizar.show()
def
control_bt_maximizar(self):
self.showMaximized()
self.ui.bt_maximizar.hide()
self.ui.bt_restaurar.show()
def mover_menu(self):
if True:
width = self.ui.frame_lateral.width()
normal = 0
if width==0:
extender = 200
else:
extender = normal
self.animacion =
QPropertyAnimation(self.ui.frame_lateral, b'minimumWidth')
self.animacion.setDuration(300)
self.animacion.setStartValue(width)
self.animacion.setEndValue(extender)
self.animacion.setEasingCurve(QtCore.QEasingCurve.InOutQuart)
self.animacion.start()
## SizeGrip
def resizeEvent(self, event):
rect = self.rect()
self.grip.move(rect.right() - self.gripSize,
rect.bottom() - self.gripSize)
## mover ventana
def mousePressEvent(self, event):
self.clickPosition = event.globalPos()
def mover_ventana(self, event):
if self.isMaximized() == False:
if event.buttons() == Qt.LeftButton:
self.move(self.pos() + event.globalPos()
- self.clickPosition)
self.clickPosition = event.globalPos()
event.accept()
if event.globalPos().y() <=20:
self.showMaximized()
else:
self.showNormal()
def calcular_puntos(self):
global theta
global fi
global px
global py
pub1 = rospy.Publisher('servo1', UInt16, queue_size =
1)
pub2 = rospy.Publisher('servo2', UInt16, queue_size =
1)
theta = int(self.ui.lineEdit_theta.text())
theta_1 = self.ui.lineEdit_theta.text()
fi = int(self.ui.lineEdit_fi.text())
fi_1 = self.ui.lineEdit_fi.text()
#self.ui.posx_label.setText(theta_1)
#self.ui.posy_label.setText(fi_1)
self.ui.grafica_uno.removeWidget(self.grafica)
if theta > 180 or theta < 0 or fi > 180 or fi< 0:
self.ui.posx_label.setText("PUNTO FUERA DE
RANGO 0<THETA<180 Y 0<FI<180")
self.ui.posy_label.setText("PUNTO FUERA DE
RANGO 0<THETA<180 Y 0<FI<180")
else:
self.grafica = Canvas_grafica()
self.ui.grafica_uno.addWidget(self.grafica)
self.ui.posx_label.setText(str(px))
self.ui.posy_label.setText(str(py))
pub1.publish(theta)
pub2.publish(fi)
#print("Hola desde calcular puntos")
def calcular_puntos_inverso(self):
global theta
global fi
global px
global py
pub1 = rospy.Publisher('servo1', UInt16, queue_size =
1)
pub2 = rospy.Publisher('servo2', UInt16, queue_size =
1)
p_x = float(self.ui.lineEdit_posx.text())
p_y = float(self.ui.lineEdit.text())
l_2 = 100
l_3 = 100
a = ((p_x*2)+(p_y2)-(l_22)-(l_3*2))/(2*l_2*l_3)
gamma = float(atan2(p_y,p_x))
beta = float(atan2((l_3*((1-a*2)*(1/2))),(l_2+l_3*a)))
q1 = gamma - beta
q2 = float(atan2((1-(a*2))*(1/2),(a)))
theta = int(q1*180/pi)
fi = int(q2*180/pi)
print(theta)
print(fi)
self.ui.grafica_uno.removeWidget(self.grafica)
if theta > 180 or theta < 0 or fi > 180 or fi< 0:
self.ui.theta_label.setText("PUNTO FUERA DE
RANGO")
self.ui.fi_label.setText("PUNTO FUERA DE
RANGO")
else:
self.grafica = Canvas_grafica()
self.ui.grafica_uno.addWidget(self.grafica)
self.ui.theta_label.setText(str(theta))
self.ui.fi_label.setText(str(fi))
pub1.publish(theta)
pub2.publish(fi)
class Canvas_grafica(FigureCanvas):
def _init_(self, parent=None):
self.fig , self.ax = plt.subplots(1, dpi=100,
figsize=(5, 5),
sharey=True, facecolor='white')
super()._init_(self.fig)
self.ax.grid()
self.ax.margins(x=0)
colors=['orange', 'purple', 'green','red']
l2 = 100
l3 = 100
global theta
q1 = float(theta*pi/180)
print(theta)
global fi
q2 = float(fi*pi/180)
print(fi)
global px
global py
px = float(l2*cos(q1) + l3*cos(q1 + q2))
py = float(l2*sin(q1) + l3*sin(q1 + q2))
x=[0,l2*cos(q1),px]
y=[0,l2*sin(q1),py]
self.fig.figure
self.ax.plot(x,y)
if _name_ == "_main_":
app = QApplication(sys.argv)
mi_app = MiApp()
mi_app.show()
rospy.init_node('MSI', anonymous = True)
sys.exit(app.exec_())
Análisis de resultados
Figura 6 HMI para controlar el robot
La HMI que se logró desarrollar es la que se muestra en la figura 6; se puede apreciar que
solo es necesario ingresar los dos ángulos y dar click en el botón calcular, para resolver
la cinemática directa, también se pueden ingresar los punto deseados en coordenadas
cartesianas y al hacer click en calcular el robot que se muestra en la figura 7 mueve los
brazos que tiene acoplados en cada servo motor.
Figura 7 Robot de dos grados de libertad construido
Por último, en la figura 5 se muestra la red que se ha creado, esta tiene 2 topicos y 2
nodos, el /serial_node corresponde a la tarjeta arduino y /MSI a la computadora.
Figura 8 Res de conexión del robot
Los dos tópicos /servo1 y /servo2 son por los cuales se envía la información que ingresa
el usuario y se utiliza para mover los servomotores.
Conclusiones
•
•
•
Una tarjeta Arduino puede ser convertida en un nodo y trabajar con ROS, eso
abarata mucho los costos y permite una mayos flexibilidad para el desarrollo de
robots
La cinemática inversa de un robot de dos grados de libertad es sencilla pues para
su calculo se puede utilizar conceptos matemáticos no muy avanzados que
facilitan la programación utilizando Python
La librería servo.h permite calibrar el ancho de pulso necesario para efectuar el
movimiento de los motores, esto es muy útil debido a que cada servomotor puede
tener una configuración diferente dependiendo del fabricante.