Universidad Autónoma de Querétaro

Facultad de Ingeniería

Ingeniería en Automatización

Robótica

Práctica 3: Análisis Cinemático de Posición Inverso:

Implementación, P=Cte

Profesor:
Dr. Gerardo Israel Pérez Soto

Integrantes:

Yessica Vannesa Dominguez Ramirez

Joan Abed Cervantes Balbino
David Israel Bárcenas Ramos

26 de Febrero, 2025
 1

I. Implementación del Control de Movimiento

Para la implementación del control de movimiento, se utilizó una Raspberry Pi Pico

como unidad principal de procesamiento, junto con un servo Dynamixel para realizar los
movimientos. Se desarrolló un código en MicroPython que permite enviar comandos al
servo y leer su posición actual mediante la comunicación UART.
El control incluye funcionalidades para:

• Mover los servomotores a un ángulo específico cada uno con una velocidad ajustable.
• Leer la posición actual del servo.

El circuito se ensambló siguiendo un diseño sencillo, donde los pines GPIO de la

Raspberry Pi Pico se conectaron al sistema de comunicación UART del servo, junto
con un pin adicional para habilitar o deshabilitar la transmisión (control de triestado).

II. Verificación del espacio de trabajo

El objetivo de esta sección es analizar el espacio de trabajo de un robot planar de

dos grados de libertad, considerando las restricciones físicas y mecánicas. El robot está
montado sobre una base fija, lo que impide la rotación completa del primer eslabón.
Además, se deben tener en cuenta dos restricciones adicionales: una pared ubicada a 10
cm del origen y la posibilidad de autocolisión debido al ancho de los eslabones.

El robot se compone de dos eslabones con longitudes L1 y L2 , los cuales están conectados
por articulaciones rotacionales. La posición del efector final está dada por:

x = L1 cos(θ1 ) + L2 cos(θ1 + θ2 ) (1)

y = L1 sin(θ1 ) + L2 sin(θ1 + θ2 ) (2)

Donde:
• L1 = 10 cm (longitud del primer eslabón)
• L2 = 7 cm (longitud del segundo eslabón)
• Los ángulos θ1 ∈ [−20◦ , 20◦ ] y θ2 ∈ [−110◦ , 110◦ ]

Se consideraron los valores máximos y mínimos de θ1 y θ2 en función de las restricciones

previamente mencionadas. Con estos límites, el brazo puede moverse libremente, abar-
cando un amplio espacio de trabajo sin colisionar con las paredes ni entre sus propios
 2

eslabones.

Para visualizar el espacio de trabajo, se realizó una simulación en MATLAB en la que se

graficaron todas las posibles posiciones alcanzables por el efector final. Dentro del área
se pueden apreciar dos tonos de azul, el más fuerte quiere decir que se puede llegar a
ese punto por medio de dos posiciones (codo arriba y codo abajo), mientras que el azul
claro significa que solamente se puede llegar a ese punto por medio de una posición.

Fig. 1. Espacio de trabajo.

En la implementación física del control del robot en la Raspberry Pi Pico, se desarrolló

un sistema que recibe como entrada la posición deseada del efector final y calcula los
ángulos θ1 y θ2 mediante cinemática inversa. Además, el código ejecutado en la Raspberry
verifica si el punto solicitado se encuentra dentro del espacio de trabajo antes de realizar
el movimiento, considerando las ecuaciones previamente mencionadas y aplicando las
restricciones de la pared ubicada a 10 cm del origen, así como la autocolisión.

Para la implementación en el robot real, se siguieron los siguientes pasos:

• Ingreso de coordenadas: Se introdujo el punto al que se quería que el efector final
 3

llegara.
• Verificación del espacio de trabajo: Se comprobó si la posición deseada se
encontraba dentro del espacio de trabajo calculado.
• Cálculo de ángulos: La Raspberry Pi Pico resolvió la cinemática inversa para
determinar los ángulos articulares.
• Ejecución del movimiento: Se enviaron las señales de control a los motores para
mover el robot a la posición calculada.

III. Resultados de la Implementación

Se activaron las articulaciones del robot para posicionar el efector final según el punto
ingresado en el código. La posición final se verificó utilizando una regla y un transporta-
dor. En las siguientes imágenes se pueden observar distintas posiciones adoptadas por el
robot, junto con su comprobación utilizando las herramientas mencionadas.

Para verificar que los ángulos calculados por el código de Micropython coincidieran
con los ángulos físicos, se midieron mediante un transportador, permitiendo así obtener
el ángulo de orientación del efector final.

A continuación, se muestran imágenes del robot con el transportador y el efector final

apuntando al ángulo de orientación correspondiente.

Los resultados de los valores de θ1 y θ2 que nos arroja el programa son los siguientes:
• Para la posición (10,10): θ1 = 17.55069 y θ2 = 68.63637
• Para la posición (11,-3): θ1 = 16.5901 y θ2 = -94.9171
• Para la posición (13,-4): θ1 = 17.88877 y θ2 = -78.04503
• Para la posición (4,-9): θ1 = -24.7455 y θ2 = -111.8037
• Para la posición (2,8): θ1 = 32.1458 y θ2 = 125.3501

Se pudo observar que los valores teóricos y prácticos fueron muy similares, esto se
puede deber a errores a la hora de medir o por el error que puede tener el motor a la
hora de posicionarse.
 4

Fig. 2. Posición (10,10) Fig. 3. Posición (11,-4) Fig. 4. Posición (13,-3)

Fig. 5. Posición (4,-9) Fig. 6. Posición (2,8)

A. Cálculo del Error Cuadrático Medio

El error cuadrático medio (ECM) es una métrica utilizada para cuantificar la diferencia
entre los valores teóricos y los valores obtenidos experimentalmente. En esta práctica, se
desea calcular el ECM entre los puntos teóricos y los puntos prácticos del robot en las
coordenadas x, y.

Para ello, se definen dos conjuntos de datos: xteórico y yteórico , que representan las posi-
ciones calculadas teóricamente, y xpráctico y ypráctico , que corresponden a las posiciones
medidas en el experimento. A partir de estos valores, se calcula el error en cada coorde-
 5

Fig. 7. Verificación de án- Fig. 8. Verificación de án- Fig. 9. Verificación de án-

gulo 2 de la posición (10,10) gulo 2 de la posición (11,-4) gulo 2 de la posición (13,-3)

Fig. 10. Verificación de án- Fig. 11. Verificación de án-

gulo 2 de la posición (4,-9) gulo 2 de la posición (2,8)

nada restando los valores prácticos a los teóricos, es decir:

ex = xteórico − xpráctico

ey = yteórico − ypráctico

Posteriormente, se eleva al cuadrado cada error y se obtiene el promedio para calcular

el ECM de cada coordenada de la siguiente manera:

n
1X
ECMx = ex (i)2 (3)
n i=1
 6

n
1X
ECMy = ey (i)2 (4)
n i=1
Finalmente, el ECM total se obtiene promediando los ECM de ambas coordenadas:

ECMx + ECMy
ECMtotal = (5)
2
Este cálculo permite evaluar la precisión con la que el robot alcanza las posiciones de-
seadas en comparación con los valores teóricos, proporcionando una medida cuantitativa
del error en la implementación práctica.

B. Tabla Comparativa de Resultados

A continuación, se presenta una tabla comparativa entre los resultados teóricos y

prácticos obtenidos de tal forma que se pueda visualizar fácilmente el error cuadrático
medio

TABLE I
Comparativa de Resultados Teóricos y Prácticos y el Error Cuadrático Medio

Teóricos (X) Teóricos (Y) Prácticos (X) Prácticos (Y) Error X Error Y Error Total
10.0 10.0 9.6 9.8 0.40 0.20 0.447
11.0 -4.0 10.7 -4.1 0.30 0.10 0.316
13.0 -3.0 12.5 -3.0 0.50 0.00 0.500
4.0 -9.0 3.7 -8.8 0.30 0.20 0.361
2.0 8.0 1.8 8.0 0.20 0.00 0.200
Promedio NaN NaN NaN 0.34 0.10 0.365

C. Discusión de Resultados

Link de video demostrativo: [Link] T C3gRT EiQtv =

nK3Y qOef CU 4f eature = [Link]

D. Discusión de Resultados

Los resultados mostraron una buena concordancia entre los valores teóricos y prácticos,
con una diferencia general pequeña en las posiciones de los puntos medidos. Sin embargo,
se observaron ciertos errores que pueden explicarse por varias razones, tales como la
precisión limitada de los instrumentos de medición y las tolerancias en los componentes
mecánicos del sistema (como los motores y las articulaciones).
 7

Los errores en las posiciones X y Y fueron menores, con un error medio de 0.34 para el eje
X y 0.10 para el eje Y. Estos errores se distribuyen de manera uniforme entre los distintos
puntos, lo que sugiere que las fuentes de error son consistentes en todas las mediciones.
El error total fue de 0.365, lo que indica que, aunque existen pequeñas desviaciones entre
los resultados teóricos y prácticos, el comportamiento general del sistema sigue el modelo
esperado.

Es importante destacar que las diferencias observadas podrían ser el resultado de la

limitación de los resultados teóricos, que no consideran factores como el desgaste de
los componentes o la fricción en las articulaciones. Además, los sistemas físicos siempre
estarán sujetos a ciertas variaciones debido a las tolerancias de fabricación, lo cual es un
aspecto fundamental a considerar en la práctica.

IV. Conclusión

En conclusión, la comparación entre los resultados teóricos y prácticos mostró que el

sistema se comporta de manera bastante consistente con las predicciones. Aunque los
errores obtenidos fueron pequeños, es fundamental reconocer que siempre existirán por
diferentes tipos de cuestiones.

V. Anexo

from machine import Pin, UART

import time

# Configuracion del UART

uart0 = UART(0, baudrate=1000000, tx=Pin(0), rx=Pin(1))
en1 = Pin(15, [Link])

def readpos(posicion_servo_1, posicion_servo_2):

[Link](1) # Habilitar la comunicacion

# Parametros del mensaje

n = 6 # Numero de motores a controlar
l = 4 # Longitud de datos de posicion y velocidad
bid = 0xFE # Broadcast ID
 8

speed = 0x1E # Velocidad de movimiento

length = ((l + 1) * n) + 4 # Longitud del mensaje

# Posiciones de los motores (ejemplo)

pos = [0x00, posicion_servo_2, 0x00, posicion_servo_1, 0x00, 0x00]

# Construir el mensaje
data = [0xFF, 0xFF, bid, length, 0x83, 0x1E, l]

# Anadir datos de posicion y velocidad para cada motor

for j in range(n):
[Link](j + 1) # ID del motor (1 a 6)
[Link](pos[j] & 0xFF) # Byte bajo de la posicion
[Link]((pos[j] >> 8) & 0x03) # Byte alto de la posicion
[Link](speed & 0xFF) # Byte bajo de la velocidad
[Link]((speed >> 8) & 0x03) # Byte alto de la velocidad

# Calcular el checksum
checksum = 0
for j in range(2, len(data)):
checksum += data[j]
checksum = ~checksum & 0xFF # Aplicar mascara de 8 bits
[Link](checksum)

# Enviar el mensaje
[Link](bytearray(data))
print("Mensaje enviado:", data)
print("Mensaje enviado:", [hex(x) for x in data])

[Link](5)
[Link](0) # Deshabilitar la comunicacion
[Link](5)
 9

def read_current_position(servo_id):
[Link](1) # Habilitar la comunicacion

# Construir el mensaje para solicitar la posicion actual

data = [0xFF, 0xFF, servo_id, 0x04, 0x02, 0x24, 0x02]
checksum = 0
for j in range(2, len(data)):
checksum += data[j]
checksum = ~checksum & 0xFF # Aplicar mascara de 8 bits
[Link](checksum)

# Enviar el mensaje
[Link](bytearray(data))
print("Mensaje enviado:", data)
print("Mensaje enviado:", [hex(x) for x in data])
print("Solicitud de posicion enviada al servo ID:", servo_id)

# Esperar y leer la respuesta

[Link](0.1) # Esperar a que el servo responda
if [Link]():
response = [Link]() # Leer la respuesta
print("Respuesta recibida:", response)

# Decodificar la posicion (suponiendo que la respuesta es de 6 bytes)

if len(response) >= 6:
pos_low = response[5] # Byte bajo de la posicion
pos_high = response[6] # Byte alto de la posicion
posicion_actual = (pos_high << 8) | pos_low # Combinar bytes
print("Posicion actual del servo ID", servo_id, ":", posicion_actual)
else:
print("Respuesta invalida del servo ID", servo_id)
else:
print("No se recibio respuesta del servo ID", servo_id)
 10

[Link](0) # Deshabilitar la comunicacion

# Ejecutar la funcion

from math import atan2, sin, cos, sqrt, degrees

# Valores de entrada
Px = 10
Py = -10

# Calculo de u, v, w
u = (Px ** 2) + (Py ** 2) + 51
v = (-20) * Px
w = (-20) * Py

# Calculo de theta_1 (en radianes)

discriminante = (4 * (w ** 2)) - (4 * (u - v) * (u + v))
if discriminante < 0:
raise ValueError("No hay solucion real para theta_1 (discriminante negativo).")

# Seleccion de la formula para theta_1 segun el valor de Py

if Py <= 0:
theta_1 = 2 * atan2((-2 * w) + sqrt(discriminante), 2 * (u - v))
else:
theta_1 = 2 * atan2((-2 * w) - sqrt(discriminante), 2 * (u - v))

# Calculo de theta_1_mas_theta_2 (en radianes)

theta_1_mas_theta_2 = atan2(Py - (10 * sin(theta_1)), Px - (10 * cos(theta_1)))

# Calculo de theta_2 (en radianes)

theta_2 = theta_1_mas_theta_2 - theta_1
 11

# Convertir angulos a grados

angulo_servo1 = degrees(theta_1) # theta_1 ya esta en radianes
angulo_servo2 = degrees(theta_2) # theta_2 ya esta en radianes

# Convertir angulos a posiciones (asegurarse de que sean enteros)

posicion_servo_1 = int((angulo_servo1 + 150.2935) / (150 / 511))
posicion_servo_2 = int((angulo_servo2 + (14625 / 128)) / (150 / 511))

# Mostrar resultados
print("Angulo servo 1 (grados):", angulo_servo1)
print("Angulo servo 2 (grados):", angulo_servo2)

# Enviar posiciones a los servos

readpos(posicion_servo_1, posicion_servo_2)

# Leer la posicion actual de los servos

read_current_position(0x02) # Leer posicion del servo ID 1
read_current_position(0x04) # Leer posicion del servo ID 2