Análisis cinemático de un brazo robótico de 4GDL con lenguaje Python

Análisis
cinemático de un
brazo robótico de
4 GDL
Lenguaje Python
Roberto Sanz Benito
24/09/2018

Análisis cinemático de un brazo robótico de 4 GDL
Roberto Sanz Benito
Resumen
El objetivo de este proyecto es estudiar la cinemática directa e inversa de un brazo
robótico.
Para ello se emplea el lenguaje de programación Python. Este lenguaje incorpora
módulos que permiten realizar cálculos matemáticos complejos y la representación en
tres dimensiones de la posición y orientación del brazo robótico. Además se indican
por pantalla las coordenadas cartesianas que ocupa el extremo final del brazo robot en
función de los ángulos de giro de sus articulaciones.
El modelado del brazo robot se realiza a través del software de diseño 3D paramétrico
FreeCAD y su consola Python dando lugar a un mecanismo articulado de tres
eslabones y cuatro grados de libertad.
Se ha realizado el modelo cinemático completo del brazo robótico. Por un lado, el
problema de la cinemática directa ha sido solucionado mediante el método Denavit-
Hartenberg. Por otro lado, se ha resuelto la cinemática inversa a través de métodos
geométricos.
El propósito de este trabajo es analizar y modelar la cinemática de un mecanismo
articulado con métodos de cálculo y simulación de bajo coste y fácil manejo. Los
programas informáticos empleados en su estudio son de código abierto. Esto permite
que el proyecto pueda modificarse, ampliarse o mejorarse por parte de la comunidad.

Roberto Sanz Benito
Contenido
Introducción..................................................................................................................3
Problema cinemático directo.........................................................................................3
Algoritmo de Denavit-Hartenberg ..............................................................................3
Problema cinemático inverso........................................................................................5
Modelado del brazo robot articulado de 4 grados de libertad en FreeCAD y Pyooml....5
Análisis cinemático directo............................................................................................8
Parámetros de Denavit-Hartenberg...........................................................................8
Algoritmo de Denavit-Hartenberg ............................................................................12
Matrices de transformación .....................................................................................13
Parámetros cinemáticos..........................................................................................14
Problema cinemático directo de posición resuelto en Python ..................................14
Resultados obtenidos ..........................................................................................17
Comprobación de los resultados .............................................................................17
Análisis cinemático inverso.........................................................................................18
Problema cinemático inverso de posición resuelto en Python .................................20
Resultados obtenidos ..........................................................................................22
Comprobación de los resultados .............................................................................22
Conclusiones ..............................................................................................................25
Referencias ................................................................................................................26

Roberto Sanz Benito
Introducción
La cinemática estudia el movimiento del robot respecto a un sistema de referencia
relacionando las trayectorias articulares (q1, q2, q3, q4) con el espacio cartesiano del
extremo del robot (x, y, z). La cinemática es la base de todo estudio dinámico.
El problema de la cinemática del robot puede abordarse de dos formas:
 Cinemática directa. Consiste en determinar la posición y orientación del
extremo final del robot a partir de un sistema de coordenadas origen, los
valores de las articulaciones y los parámetros que definen los elementos
geométricos del robot.
 Cinemática inversa. Consiste en determinar la posición y orientación de las
articulaciones a partir de la posición y orientación del extremo final del robot y
sus parámetros geométricos.
Problema cinemático directo
El problema cinemático consiste en encontrar una matriz homogénea de
transformación T que relacione la posición y orientación del extremo del robot respecto
de un sistema de referencia fijo localizado en su base.
Para un robot de n grados de libertad, formado por n eslabones unidos por n
articulaciones, se pueden obtener las n matrices de transformación homogéneas que
representan la orientación y traslación relativas entre eslabones consecutivos del
robot. La matriz i-1Ai describe la posición y orientación del eslabón i respecto del
eslabón i-1.
Para considerar todos los grados de libertad, se utiliza la matriz T, que vendrá dada
por el producto de cada una de las matrices de transformación homogéneas relativas a
cada par de eslabones.
Algoritmo de Denavit-Hartenberg
Se trata de un método sistemático, propuesto en 1955 por Denavit y Hartenberg que
permite establecer un sistema de coordenadas ligado a cada eslabón para poder
determinar después las ecuaciones cinemáticas de la cadena completa.
Este sistema se basa en la utilización de cuatro transformaciones básicas que
consisten en una sucesión de rotaciones y traslaciones que permiten relacionar el
sistema de referencia del elemento i con el sistema del elemento i-1. Estas son las
cuatro transformaciones básicas:
 Ángulo θi: Es el ángulo desde Xi−1 hasta Xi girando alrededor de Zi−1.

Roberto Sanz Benito
 Distancia di: Es la distancia desde el sistema Oi−1 hasta la intersección de las
normales común entre Zi−1 y Zi, a lo largo de Zi−1.
 Distancia ai: Es la longitud de la normal común, es decir, es la distancia de Zi−1
a Zi medida a lo largo de Xi.
 Ángulo αi: Es el ángulo que hay que rotar Zi−1 para llegar a Zi, rotando
alrededor de Xi.
Los pasos del algoritmo son los siguientes:
1. Se numeran los eslabones, empezando desde la base fija, a la que se le asocia el
valor 0, y terminando por el extremo.
2. Se numeran las articulaciones comenzando por el primer grado de libertad y
terminando por el último.
3. Se localiza el eje de cada articulación. Para pares de revolución, es el eje de giro; y
para pares prismáticos, el eje a lo largo se mueve el eslabón.
4. Se colocan los ejes Z: se sitúa el eje Zi-1 en los ejes de las articulaciones i.
5. El origen de coordenadas O0 se coloca en cualquier punto de Z0.
6. El resto de orígenes Oi se colocan estudiando la relación entre dos ejes Z
consecutivos, atendiendo a las siguientes consideraciones:
a. Ejes paralelos: el origen Oi se ubica en la intersección entre la normal común de
los ejes Zi-1 y Zi y el eje Zi.
b. Ejes secantes: el origen Oi se ubica en el punto de intersección.
c. Ejes cruzados: el origen Oi se encuentra en la intersección entre la normal común
de los ejes Zi-1 y Zi y el eje Zi.
d. Ejes coincidentes: se puede posicionar el origen Oi a lo largo del eje Zi,
escogiendo la normal común en la misma dirección que en el caso anterior Ni-1.
7. El sistema On se coloca en el extremo del robot, con su eje Zn paralelo a Zn-1.
8. La orientación de X0 e Y0 es arbitraria, siempre siguiendo un sistema dextrógiro.
9. Cada eje Xi va en dirección a la normal común a Zi-1 y Zi. Su sentido es indistinto.
10. Los ejes Yi se colocan siguiendo un sistema dextrógiro.
Después de haber seguido todos estos pasos ya se puede obtener la matriz A de
transformación entre cada par de eslabones consecutivos.

Roberto Sanz Benito
Problema cinemático inverso
El problema cinemático inverso persigue obtener los valores que deben adoptar las
coordenadas articulares del robot para que su extremo alcance una posición y
orientación predeterminadas. Su cálculo supone la resolución de una serie de
ecuaciones cuya solución no tiene por qué ser única.
Los valores articulares que posicionan el extremo final del robot pueden obtenerse
mediante métodos geométricos. Para ello se utilizan relaciones trigonométricas y
geométricas sobre las diferentes articulaciones del robot y el plano en el que se
encuentra.
Modelado del brazo robot articulado de 4 grados
de libertad en FreeCAD y Pyooml
El robot de este caso de estudio es un brazo robótico de configuración angular,
también llamado antropomórfico, con 3 eslabones y 4 grados de libertad.

Roberto Sanz Benito
En este apartado se dibuja el mecanismo utilizando el programa de diseño 3D FreeCAD y
la biblioteca Pyooml. Las transformaciones se calculan utilizando matrices de
transformación homogéneas.
Una vez importada la biblioteca Pyooml en la consola de Python, se definen en primer
lugar los parámetros del robot: los ángulos de los eslabones y la longitud de los mismos.
A continuación se calcularán los vectores correspondientes a los eslabones y se crearán
los elementos gráficos de los sistemas de referencia que son 5 sistemas.
Con esto ya tenemos resuelta la problemática de la cinemática directa que nos permite
calcular en todo momento cuales son las coordenadas del punto final respecto al origen de
coordenadas (vector naranja).
El código de programación empleado en la modelización del mecanismo se escribe en la
consola Python del programa FreeCAD y es el que viene a continuación:
from FreeCAD import Vector
from pyooml import*
f0 = frame()
f1 = frame()
f2 = frame()
f3 = frame()
f4 = frame()
a1=0
a2=-45
a3=70
a4=20
L1=20
L2=40
L3=40
L4=10
v1 = Vector(0, 0, L1)
v2 = Vector(L2, 0, 0)
v3 = Vector(L3, 0, 0)
v4 = Vector(L4, 0, 0)
sv1 = svector(v1).color("yellow")
sv2=svector(v2).color("yellow")
import HMatrix
Ma = HMatrix.Rotz(a1)
Mb = HMatrix.Translation(v1)

Roberto Sanz Benito
sv1.T = Ma
f1.T = Ma * Mb
Mc = HMatrix.Roty(a2)
Md = HMatrix.Translation(v2)
sv2.T = Ma * Mb * Mc
f2.T = Ma * Mb * Mc * Md
Me = HMatrix.Roty(a3)
Mf=HMatrix.Translation(v3)
sv3.T =Ma*Mb*Mc*Md*Me
f3.T=Ma*Mb*Mc*Md*Me*Mf
Mg = HMatrix.Roty(a4)
Mh=HMatrix.Translation(v4)
sv4.T =Ma*Mb*Mc*Md*Me*Mf*Mg
f4.T=Ma*Mb*Mc*Md*Me*Mf*Mg*Mh
vr = f4.T.multiply(Vector(0,0,0))
svr = svector(vr).color("orange")
l1 = link(l = L2, D = 10, w = 5).ice(80)
l2 = link(l = L3, D = 10, w = 3).ice(80)
l3 = link(l = L4, D = 10, w = 3).ice(80)
l3.T = Ma * Mb * Mc*Md*Me*Mf*Mg
l2.T = Ma * Mb * Mc*Md*Me
l1.T = Ma*Mb*Mc
bancada=cylinder(5,20).ice(80)
base = sphere(r = 5, angle1 = 0).translate(0, 0, 20).ice(80)
soporte_base=cylinder(10,1).ice(80)
La representación gráfica del mecanismo son 3 eslabones unidos por 3 articulaciones
en el que la base gira sobre un eje vertical Z. Por lo tanto tenemos el siguiente sistema
mecánico de 4 grados de libertad:

Roberto Sanz Benito
Análisis cinemático directo
Como ya se ha mencionado, el robot de este caso de estudio es un brazo robótico de
configuración angular, también llamado antropomórfico, con 4 grados de libertad.
Conociendo el modelo cinemático del brazo robot se podrá describir de manera fiel en
todo momento los movimientos y el posicionamiento espacial del extremo final del
brazo robot.
Parámetros de Denavit-Hartenberg
La convención Denavit-Hartenberg es una metodología ampliamente utilizada en el
ámbito académico y de investigación en robótica que permite establecer sistemas de
coordenadas para cada eslabón del brazo robot. Mediante transformaciones de
rotación y translación se pueden conocer las coordenadas cartesianas (X0, Y0, Z0) del
extremo final, referido a un sistema de coordenadas fijo con base en los ángulos de
todos los elementos del brazo robot.
El método comienza situando los ejes de giro de cada articulación y trazando la recta
perpendicular a ambos. La figura nos muestra esquemáticamente la posición de los
cuatro ejes de giro del robot:

Roberto Sanz Benito
El siguiente paso es ubicar las bases vectoriales en cada tramo. Existen varias
convenciones, y el objetivo es seleccionarlas de manera que queden definidas por el
menor número de parámetros posibles. No obstante, se puede usar cualquier
convención siempre que las matrices de transformación sean coherentes con la
selección de las bases.
La notación más generalizada es la de Denavitt-Hartenberg, en la que se sitúa el eje Z
de cada tramo es coincidente con el eje de la articulación y los orígenes de las bases
se sitúan en el punto de intersección entre dos ejes, si se cortan, o en los puntos de
intersección entre los ejes y la recta perpendicular que los une, si se cruzan.
Una vez situados los orígenes y el eje de giro Z, el criterio para la selección de los ejes
X e Y varía según el autor. Pero de lo que se trata es que los vectores de traslación de
una base a la siguiente tenga el menor número de parámetros posible y que las
matrices de transformación sean también lo más simples posible.

Roberto Sanz Benito
Entonces, para el brazo robot antropomórfico de cuatro gados de libertad presentado
en este estudio, el problema cinemático directo se reduce a encontrar la matriz de
transformación homogénea 0
T4
𝑇 = 𝑇1
0
∗ 𝑇2
1
∗ 𝑇3
2
∗ 𝑇4
3
4
0
En la siguiente figura se presenta un esquema que muestra la vista superior del brazo
robot, en la que se indican los ejes de giro de las articulaciones y el ángulo de giro q1
de la base respecto al eje vertical Z. El círculo en color amarillo representa la posición
espacial del extremo final en coordenadas cartesianas (X0, Y0, Z0), con respecto del
origen de coordenadas.

Roberto Sanz Benito
A continuación se presenta otro esquema que muestra la vista lateral del brazo
robot, donde se indican los parámetros d1, l2, l3, l4, q2, q3 y q4, necesarios para el
análisis cinemático directo. La distancia vertical desde la base hasta el centro de la
articulación del brazo es d1, l2 es la longitud efectiva del brazo, l3 es la longitud
efectiva del antebrazo, l4 es la longitud efectiva de la muñeca o herramienta, q2 es el
ángulo de rotación del brazo con respecto del eje horizontal, q3 es el ángulo de
rotación del antebrazo con respecto del eje longitudinal del brazo y q4 es el ángulo de
rotación de la muñeca con respecto del eje longitudinal del antebrazo.

Roberto Sanz Benito
Algoritmo de Denavit-Hartenberg
Anteriormente se explicaron los pasos a seguir en el método Denavit-Hartenberg para
obtener la matriz A de transformación entre cada par de eslabones consecutivos.

Roberto Sanz Benito
Al aplicar el algoritmo de Denavit-Hartenberg con los parámetros y configuración del
brazo robot (θi, di, li, αi) se obtienen las matrices de transformación homogéneas para
cada uno de los elementos, las cuales quedan como se muestra a continuación.
Matrices de transformación
La correcta selección de las bases y sus correspondientes matrices de rotación son la
base de todo el estudio tanto cinemático como dinámico. En el siguiente apartado se
muestran una a una las bases vectoriales escogidas y las correspondientes matrices
de transformación.
Matriz de transformación de la Base:
𝑇1
0
=
[
cos⁡( 𝑞1) 0 sen (
π
2
) ∗ sen(𝑞1) 0
sen⁡( 𝑞1) 0 −sen (
π
2
) ∗ cos⁡( 𝑞1) 0
0 1 0 𝑑1
0 0 0 1 ]
Matriz de transformación del Brazo:
𝑇2
1
= [
cos⁡( 𝑞2) −sen(𝑞2) 0 𝑙2 ∗ cos⁡( 𝑞2)
sen⁡( 𝑞2) cos⁡( 𝑞2) 0 𝑙2 ∗ sen(𝑞2)
0 0 1 𝑙2
0 0 0 1
]
Matriz de transformación del Antebrazo:
𝑇3
2
= [
cos⁡( 𝑞3) −sen(𝑞3) 0 𝑙3 ∗ cos⁡( 𝑞3)
sen⁡( 𝑞3) cos⁡( 𝑞3) 0 𝑙3 ∗ sen(𝑞3)
0 0 1 𝑙3
0 0 0 1
]

Roberto Sanz Benito
Matriz de transformación de la Muñeca o herramienta:
𝑇4
3
= [
cos⁡( 𝑞4) −sen(𝑞4) 0 𝑙4 ∗ cos⁡( 𝑞4)
sen⁡( 𝑞4) cos⁡( 𝑞4) 0 𝑙4 ∗ sen(𝑞4)
0 0 1 𝑙4
0 0 0 1
]
Al realizar la multiplicación de estas matrices y simplificando términos se obtiene la
matriz de transformación homogénea:
𝑇 = 𝑇1
0
∗ 𝑇2
1
∗ 𝑇3
2
∗ 𝑇4
3
4
0
𝑇4
0
= [
𝑇11 𝑇12 𝑇13 𝑇14
𝑇21 𝑇22 𝑇23 𝑇24
𝑇31 𝑇32 𝑇33 𝑇34
𝑇41 𝑇42 𝑇43 𝑇44
]
En dicha matriz, los elementos (T14, T24, T34) describen las coordenadas cartesianas
(X0, Y0, Z0) de la localización espacial del extremo final del brazo robot,
respectivamente.
Parámetros cinemáticos
La tabla siguiente muestra los parámetros cinemáticos del robot:
Parámetros de Denavit - Hartenberg
Eslabón θi di li αi
1 q1 d1 0 π/2
2 q2 0 l2 0
3 q3 0 l3 0
4 q4 0 l4 0
Problema cinemático directo de posición resuelto en Python
El problema cinemático directo de la posición del robot se resuelve calculando las
operaciones matemáticas descritas. Estas operaciones se realizarán empleando el
lenguaje de programación Python y la extensión Numpy que le agrega mayor soporte
para vectores y matrices.

Roberto Sanz Benito
Python es un lenguaje de programación fácil de entender capaz de realizar cálculos de
gran complejidad.
El código escrito para calcular las matrices de transformación se presenta a
continuación:
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.pyplot import title, figure, show, plot, legend, xlabel, ylabel
d1=5 #Altura de la base d1 en cm
l2=10 #Longitud del eslabon l2 en cm
Q1= float(input("Dame el ángulo Q1 en grados:"))
A1=0 #Ángulo Alfa 1 en grados
Q1=(Q1/180.0)*np.pi #Theta 1 en radianes
A1=(A1/180.0)*np.pi #Alpha 1 en radianes
# Matriz de transformación de la base
T0_1=[[np.cos(Q1),0,np.sin(A1)*np.sin(Q1),0],
[np.sin(Q1),0,-np.sin(A1)*np.cos(Q1),0],
[0,1,0,d1],
[0,0,0,1]]
# Matriz de transformación del brazo
T1_2=[[np.cos(Q2),-np.sin(Q2),0,l2*np.cos(Q2)],
[np.sin(Q2),np.cos(Q2),0,l2*np.sin(Q2)],
[0,0,1,l2],
[0,0,0,1]]
# Matriz de transformación del antebrazo
[0,0,1,l3],
[0,0,0,1]]
# Matriz de transformación de la muñeca
[0,0,1,l4],

Roberto Sanz Benito
[0,0,0,1]]
print ("T0_1=")
print (np.matrix(T0_1))
print("n")
print ("T1_2=")
print("n")
print ("T2_3=")
print("n")
print ("T3_4=")
print("n")
T0_2=np.dot(T0_1,T1_2)
T0_3=np.dot(T0_2,T2_3)
T0_4=np.dot(T0_3,T3_4)
print ("T0_2=")
print("n")
print ("T0_3=")
print("n")
print ("T0_4=")
print("n")
print ("Los elementos (T14, T24, T34) de la matriz T0_4 describen las coordenadas cartesianas
(X0, Y0, Z0)")
T14=T0_4[0][3]
T24=T0_4[1][3]
T34=T0_4[2][3]
X0=T14
Y0=T24
Z0=T34
#Redondeamos los valores a dos decimales
X0=round(X0,2)
Y0=round(Y0,2)
Z0=round(Z0,2)
print ("X0=",X0)
print ("Y0=",Y0)
print ("Z0=",Z0)

Roberto Sanz Benito
Resultados obtenidos
Los resultados de la posición final del brazo robot para los valores angulares q1=0,
q2=45, q3=-45 y q4=-45 son las coordenadas cartesianas X0, Y0 y Z0: (20,6, 0, 8,53)
Comprobación de los resultados
En este apartado se comprobará que tanto los parámetros de Denavit-Hartenberg
como los cálculos cinemáticos realizados mediante el lenguaje de programación
Python son correctos. Para ello se representa gráficamente la posición espacial del
robot con la biblioteca de graficación matplotlib y la herramienta mplot3d.
El código escrito para representar la gráfica 3D de la posición se presenta a
continuación:
#Representación gráfica de X0, Y0 y Z0
##from mpl_toolkits.mplot3d import Axes3D
##from matplotlib.pyplot import title, figure, show, plot, legend
a=figure()
a.gca(projection='3d')
T13=T0_3[0][3]
T23=T0_3[1][3]
T33=T0_3[2][3]
T12=T0_2[0][3]
T22=T0_2[1][3]
T32=T0_2[2][3]
T13=round(T13,2)
T23=round(T23,2)
T33=round(T33,2)
T12=round(T12,2)
T22=round(T22,2)
T32=round(T32,2)
x=[0,0,T12,T13,X0]
y=[0,0,T22,T23,Y0]
z=[0,d1,T32,T33,Z0]
plot(x,y,z, label='Cinemática robot')
xlabel("Eje X", fontsize = 10)
ylabel("Eje Y", fontsize = 10)
legend()
show()

Roberto Sanz Benito
Análisis cinemático inverso
La cinemática inversa permite obtener los ángulos de rotación de los elementos del
brazo robot a partir de las coordenadas cartesianas (X0, Y0, Z0) del extremo final del
brazo robot. Para obtener las ecuaciones que permitan calcular los ángulos requeridos
se emplea el método geométrico.
En la siguiente figura se presenta el esquema de la vista superior del brazo robot con
el ángulo de giro de la base del brazo robot q1 y la distancia horizontal desde el origen
de coordenadas hasta el extremo final del brazo, la cual será llamada “Módulo” y es la
suma de l2, l3 y l4.
Partiendo de un brazo robot de cuatro grados de libertad, del cual conocemos la
longitud de su brazo, antebrazo, codo y muñeca, podemos calcular el ángulo de los
mismos para situarlo en una posición X e Y.
El ángulo de la base q1 se calcula como:
𝑞1 = 𝑡𝑎𝑛−1
(
𝑌0
𝑋0
)

Roberto Sanz Benito
A partir de la observación de la figura se obtiene el cálculo del “Módulo”:
𝑀ó𝑑𝑢𝑙𝑜 = √𝑋0
2
+ 𝑌0
2
En la siguiente figura se muestra otra vista lateral del brazo robot, donde se indican
otros parámetros necesarios que permitirán establecer las ecuaciones para el cálculo
de los ángulos q2, q3 y q4. En esta figura se observa el ángulo q, llamado “ángulo de
cabeceo” o “pitch”. Dicho ángulo permanece constante a la vista del observador aún
cuando el brazo se haya movido a otra posición; el ángulo q se mide con respecto al
eje horizontal. Así, para aplicar la cinemática inversa del brazo robot, tanto el ángulo q
como las coordenadas (X0, Y0, Z0) deberán ser dados por el usuario.
𝑙 𝑥 = 𝑙4 ∗ cos⁡( 𝑞)
𝑙 𝑧 = 𝑙4 ∗ sen⁡( 𝑞)
𝑋1 = 𝑀ó𝑑𝑢𝑙𝑜 − 𝑙 𝑥
𝑍1 = 𝑍0 + 𝑙 𝑧 − 𝑑1
ℎ = √𝑋1
2
+ 𝑍1
2

Roberto Sanz Benito
𝑎1 = 𝑡𝑎𝑛−1
(
𝑍1
𝑋1
)
𝑎2 = 𝑐𝑜𝑠−1
(
𝑙2
2−𝑙3
2+ℎ2
2𝑙2ℎ
) Teormea del coseno
𝑞2 = 𝑎1 + 𝑎2
𝑎3 = 𝑐𝑜𝑠−1
(
𝑙2
2
+𝑙3
2
−ℎ2
2𝑙2 𝑙3
) Teorema del coseno
𝑞3 = 𝑎3 − 180
𝑞4 = 𝑞 − 𝑞2 − 𝑞3
Problema cinemático inverso de posición resuelto en Python
Al igual que se ha hecho en el problema cinemático directo de la posición del robot, las
operaciones matemáticas de la cinemática inversa se realizarán mediante el lenguaje
de programación Python y la extensión Numpy, pero en este caso añadimos el módulo
matemático Math.
El código escrito para calcular las ecuaciones trigonométricas se presenta a
continuación:

Roberto Sanz Benito
import numpy as np
import math
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.pyplot import title, figure, show, plot, legend, xlabel, ylabel
d1=5 #Altura de la base d1 en cm
X0= float(input("Dame X0:"))
Y0= float(input("Dame Y0:"))
Z0= float(input("Dame Z0:"))
Q= float(input("Dame el ángulo de cabeceo Q en grados:"))
#Cálculo del Módulo
Modulo=math.sqrt(X0**2+Y0**2)
print('Módulo: ',Modulo)
#Cálculo de X1, Z1 y h
q=math.radians(Q) #El ángulo de cabeceo hay que pasarlo a radianes
lx=l4*math.cos(q)
lz=l4*math.sin(q)
print('lx: ',lx)
print('lz: ',lz)
X1=Modulo-lx
print('X1: ',X1)
Z1=Z0+lz-d1
print('Z1: ',Z1)
h=math.sqrt(X1**2+Z1**2)
print('h: ',h)
#Cálculo de a1, a2 y a3
a1=math.atan(Z1/X1)* (180.0 / math.pi)
print('a1: ',a1)
a2=math.acos((l2**2-l3**2+h**2)/(2*l2*h))* (180.0 / math.pi)
print('a2: ',a2)
a3=math.acos((l2**2+l3**2-h**2)/(2*l2*l3))* (180.0 / math.pi)
print('a3: ',a3)
#Cálculo de los ángulos Q1, Q2, Q3 y Q4
Q1 = math.atan(Y0/X0) * (180.0 / math.pi)
Q1=round(Q1)
print('Ángulo Q1 en grados: ' ,Q1)
Q2=a1+a2
Q2=round(Q2)
Q3=-(180-a3)
Q3=round(Q3)

Roberto Sanz Benito
Q4=-(Q-Q2-Q3)
Q4=round(Q4)
Resultados obtenidos
Los resultados obtenidos para las coordenadas cartesianas X0, Y0 y Z0 (20,6, 0, 8,53)
establecidas para el extremo final del robot son los valores angulares: q1=0, q2=45,
q3=-45 y q4=-45
Comprobación de los resultados
En este apartado se comprobará que tanto los parámetros de Denavit-Hartenberg
como los cálculos cinemáticos realizados mediante el lenguaje de programación
Python son correctos utilizando para ello la biblioteca de graficación matplotlib y la
herramienta mplot3d.
El código escrito para representar la gráfica 3D de la posición se presenta a
continuación:
#Representación gráfica
A1=0 #Ángulo Alfa 1 en grados
A1=(A1/180.0)*np.pi #Alpha 1 en radianes
# Matriz de transformación de la base
T0_1=[[np.cos(Q1),0,np.sin(A1)*np.sin(Q1),0],
[np.sin(Q1),0,-np.sin(A1)*np.cos(Q1),0],
[0,1,0,d1],
[0,0,0,1]]
# Matriz de transformación del brazo
[0,0,1,l2],
[0,0,0,1]]
# Matriz de transformación del antebrazo
[0,0,1,l3],
[0,0,0,1]]

Roberto Sanz Benito
# Matriz de transformación de la muñeca
[0,0,1,l4],
[0,0,0,1]]
print ("T0_1=")
print("n")
print ("T1_2=")
print("n")
print ("T2_3=")
print("n")
print ("T3_4=")
print("n")
T0_2=np.dot(T0_1,T1_2)
T0_3=np.dot(T0_2,T2_3)
T0_4=np.dot(T0_3,T3_4)
print ("T0_2=")
print("n")
print ("T0_3=")
print("n")
print ("T0_4=")
print("n")
print ("Los elementos (T14, T24, T34) de la matriz T0_4 describen las coordenadas cartesianas
(X0, Y0, Z0)")
T14=T0_4[0][3]
T24=T0_4[1][3]
T34=T0_4[2][3]
X0=T14
Y0=T24
Z0=T34
X0=round(X0,2)
Y0=round(Y0,2)

Roberto Sanz Benito
Z0=round(Z0,2)
print ("X0=",X0)
print ("Y0=",Y0)
print ("Z0=",Z0)
#Representación gráfica de X0, Y0 y Z0
##from mpl_toolkits.mplot3d import Axes3D
##from matplotlib.pyplot import title, figure, show, plot, legend
a=figure()
a.gca(projection='3d')
T13=T0_3[0][3]
T23=T0_3[1][3]
T33=T0_3[2][3]
T12=T0_2[0][3]
T22=T0_2[1][3]
T32=T0_2[2][3]
T13=round(T13,2)
T23=round(T23,2)
T33=round(T33,2)
T12=round(T12,2)
T22=round(T22,2)
T32=round(T32,2)
x=[0,0,T12,T13,X0]
y=[0,0,T22,T23,Y0]
z=[0,d1,T32,T33,Z0]
plot(x,y,z, label='Cinemática inversa robot')
xlabel("Eje X", fontsize = 10)
ylabel("Eje Y", fontsize = 10)
legend()
show()

Roberto Sanz Benito
Conclusiones
Este trabajo presenta el estudio cinemático de un robot articulado de 4 grados de
libertad empleando como herramienta de cálculo y análisis el lenguaje de
programación Python.
En primer lugar se ha realizado un modelado completo del robot con el programa
FreeCAD y la biblioteca Pyooml.
En segundo lugar se ha realizado el análisis cinemático directo en el que se han
explicado los fundamentos matemáticos que lo guían y el método Denavitt-Hartenberg
(D-H) que se ha empleado en su resolución. Se muestra el código de programación
Python para realizar los cálculos y la representación gráfica de los resultados.
En tercer lugar se ha realizado el análisis cinemático inverso en el que se han
explicado las relaciones trigonométricas y geométricas para su resolución. Se muestra
el código de programación Python para realizar los cálculos y la representación gráfica
de los resultados.

Roberto Sanz Benito
Referencias
Sodemann, A. (2017. Junio 11). Robotics 1 U1 (Kinematics) S3 (Rotation Matrices) P2
(Rotation Matrices in Python). Recuperado de https://youtu.be/tSy8QbcOSxc
Sodemann, A. (2017. Junio 12) Robotics 1 U1 (Kinematics) S4 (Displacement Vectors)
P3 (Displacement Vectors in Python). Recuperado de https://youtu.be/Xq8fOGywS7U
Sodemann, A. (2017. Junio 12) Robotics 1 U1 (Kinematics) S5 (Homogeneous
Transformation Matrix) P1 (HTM from Rotation Matrix). Recuperado de
https://youtu.be/fXewWpehAWw
Sodemann, A, (2017. Septiembre 2) Robotics 1 U1 (Kinematics) S5 (HTM) P4 (Denavit
Hartenberg in Python). Recuperado de https://youtu.be/V6TKocmc9k8
González, J. (2015, Abril 15). Cinemática directa de un brazo robot de 2 GDL en
Freecad y Pyooml. Recuperado de https://youtu.be/2kPvyDbLMts
Campos, J.M. Modelado y simulación dinámica del robot industrial Fanuc M-
410IB/700. Valencia: Universitat Politècnica de València.
Mazo, F. (2011). Modelado Dinámico y Simulación del robot industrial Stäubli TX90.
Escola Tècnica Superior d’Enginyeria Industrial de Barcelona.
Garcia, F. (2013). Control automático de un brazo robot de 5 grados de libertada con
arduino. Universidad de Valladolid.
Medina, J., Villafuerte, R., Mejía, (2007) E. Simulador 3D para brazo robot de 4 grados
de libertad. Revista Iberoamericana para la Investigación y el Desarrollo Educativo.
Metodología de Denavit-Hartenberg. (s.f.). En http://wiki.robotica.webs.upv.es/.
Recuperado el 15 de septiembre de de 2018

Análisis cinemático de un brazo robótico de 4GDL con lenguaje Python

Recomendados

Recomendados

Más contenido relacionado

La actualidad más candente

La actualidad más candente (20)

Similar a Análisis cinemático de un brazo robótico de 4GDL con lenguaje Python

Similar a Análisis cinemático de un brazo robótico de 4GDL con lenguaje Python (20)

Más de Roberto Sanz Benito

Más de Roberto Sanz Benito (7)

Último

Último (20)

Análisis cinemático de un brazo robótico de 4GDL con lenguaje Python