Resolver Robots Articulados Con Arduino

RESOLVER ROBOTS ARTICULADOS CON ARDUINO
En esta entrada vamos a ver las ecuaciones necesarias para calcular un robot
articulado en un procesador como Arduino. Y al final ¡veremos una librería para que no
tengáis que hacerlo nunca más!
La resolución de la posición y ángulos de polígonos articulados, en especial

triángulos y cuadriláteros, resulta importante porque aparece frecuentemente al trabajar
con robots articulados, como en el caso de brazos robóticos, cuadrípedos, hexápodos,
bípedos, etc.
En general, en los robots articulados sabemos la distancia de cada uno de los lados
del polígono articulado y actuamos sobre los motores para variar el ángulo que forman
entre ellas.
También conocemos las coordenadas del punto inicial del polígono (o bien no nos
importan, si vamos a usar coordenadas relativas al mismo). Por su parte, cada nodo de
la articulación tiene sus propias coordenadas, teniendo especialmente interés en el
«punto final» porque normalmente es donde tendremos el efector (la pinza del brazo, la
pata del robot, etc.).
Con esto, tenemos dos tipos de cálculos que aparecen en la resolución de robots
articulados:
 Directo, donde sabemos los ángulos de las articulaciones, y queremos saber la posición del
efector.
 Inverso, donde sabemos la posición del efector, y queremos calcular los ángulos necesarios para
conseguirlo.
Siendo más frecuente en robots articulados el último de estos dos. Por ejemplo,
sabemos dónde está la pelota que queremos coger o donde queremos poner una pata, y
necesitamos saber qué ángulos fijar en los motores para conseguirlo.
El cálculo no es especialmente complicado, pero requiere de una pequeña dosis de

trigonometría. Por otro lado, destacar que no todas las posiciones van a tener
resolución, mientras que por el contrario algunas van a tener más de una (en general,
infinitas) soluciones posibles.
Sin más, vamos a ver cómo resolver este problema para el caso de triángulos y
cuadriláteros articulados en 2D, y 3D, los casos más frecuentes que tendremos al
trabajar con robots articulados.
TRIÁNGULO ARTICULADO 2D
DIRECTO
El cálculo directo de la posición del efectos (P2), en función de los ángulos

absolutos de los segmentos se obtiene simplemente proyectando en X e Y, de la
siguiente forma.
Donde la relación entre los ángulos relativos entre los segmentos (tanto interior,
como exterior) y los ángulos absolutos de los mismos se calculan mediante las
siguientes expresiones
INVERSO
El cálculo inverso, es igualmente sencillo, considerando que los dos segmentos

forman un triángulo junto con la línea imaginaría que unen P0 y P2.
Podemos calcular distancia D desde la base P0 a P2, aplicando el teorema de

pitágoras.
Con esta distancia y las longitudes de los segmentos podemos calcular todos los
ángulos interiores del triángulo aplicando el teorema del coseno generalizado.
Por otro lado, podemos calcular el ángulo que forma el triángulo respecto al eje X
con la siguiente expresión.
Finalmente, calculamos el ángulo absoluto del primer segmento haciendo.

Conocido el ángulo absoluto del primer segmento y el ángulo relativo entre los
mismos, podemos calcular el resto de ángulos relativos y absolutos usando las
expresiones que hemos visto con anterioridad.
Lógicamente, no todas las posiciones admiten una solución, como por ejemplo las
que excedan el alcance de los brazos.
CÓDIGO
Así sería un posible ejemplo de código para resolver un triángulo articulado en 2D
en un procesador como Arduino.
1 float L1, L2, D;

2 float AbsoluteAngle1, AbsoluteAngle2;
3 float RelativeAngle12;
4 float TargetX, TargetY;
5
6 float Pythagoras(const float& x, const float& y)
7 {
8 const float d = sqrt(x * x + y * y);
9 return d;
10 }
11
12 float SolveInverseCosine(const float& l1, const float& l2, const float& l3)
13 {
14 const float angle3 = acos((l1 * l1 + l2 * l2 - l3 * l3) / (2 * l1 * l2));
15
16 return angle3;
17 }
18
19 float AbsoluteToRelative(float absolute1, float absolute2)
20 {
21 return absolute2 - absolute1 + PI;
22 }
23
24 float RelativeToAbsolute(float relative12, float absolute1)
25 {
26 return absolute1 + relative12 - PI;
27 }
28
29 void SolveTriangle(float& x, float& y, float& l1, float& l2, float& d, float& absoluteAngle1, float&
30 absoluteAngle2, float& relativeAngle12)
31 {
32 const float angle_0 = atan2(y, x);
33 d = Pythagoras(x, y);
34
35 const float angle_l1 = SolveInverseCosine(d, l2, l1);
37 const float angle_D = PI - angle_l1 - angle_l2;
38
39 absoluteAngle1 = angle_0 + angle_l2;
40 relativeAngle12 = angle_D;
41 absoluteAngle2 = RelativeToAbsolute(relativeAngle12, absoluteAngle1);
42 }
43
44 void SolveDirect(float angle1, float angle2)
45 {
46 AbsoluteAngle1 = angle1;
48
49 SolveDirect();
50 }
51
52 void SolveDirect()
53 {
54 RelativeAngle12 = AbsoluteToRelative(AbsoluteAngle1, AbsoluteAngle2);
55
56 TargetX = L1 * cos(AbsoluteAngle1) + L2 * cos(AbsoluteAngle2);
57 TargetY = L1 * sin(AbsoluteAngle1) + L2 * sin(AbsoluteAngle2);
58 D = Pythagoras(TargetX, TargetY);
59 }
60
61 void SolveReverse()
62 {
64
65 float P2x = TargetX;
66 float P2y = TargetY;
67
68 float D0;
69 SolveTriangle(P2x, P2y, L1, L2, D0, AbsoluteAngle1, AbsoluteAngle2, RelativeAngle12);
70 }
71
72 void Debug()
73 {
74 Serial.print(F("Target X:\t"));
75 Serial.println(TargetX, 4);
76 Serial.print(F("Target Y:\t"));
77 Serial.println(TargetY, 4);
78
79 Serial.print(F("Abs. Angle 1:\t"));
80 Serial.println(degrees(AbsoluteAngle1), 4);
83
84 Serial.print(F("Rel. Angle 12:\t"));
85 Serial.println(degrees(RelativeAngle12), 4);
86 }
87
88
89 void setup()
90 {
91 Serial.begin(115200);
92 while (!Serial) { ; }
93
94 L1 = 100; L2 = 50;
95
96 Serial.println(F("Solve Direct Relative"));
97 SolveDirect(radians(55), radians(-20));
98 Debug();
99
100 Serial.println(F("Solve Inverse"));
101 TargetX = 104.3423;
102 TargetY = 64.8141;
103 SolveReverse();
104 Debug();
105 }
106
107
108 void loop()
109 {
110 // Do nothing
111 delay(10000);
}
CUADRILÁTERO ARTICULADO
2D
DIRECTO
La resolución de un cuadrilátero articulado es similar al triángulo articulado,
simplemente proyectamos el nuevo segmento en los ejes X e Y.
INVERSO
La resolución de un cuadrilátero articulado no es más compleja que la del triángulo
en 2D. El nuevo segmento añade un grado de libertad adicional por lo que, en general,
el problema admite múltiples soluciones (infinitas).
Para poder resolver el sistema debemos imponer una condición (o relación entre
condiciones). Lo habitual es proporcionar el ángulo absoluto del ultimo segmento, que
corresponde con el angulo de ataque del efector.
Con las coordenadas del punto P2, resolveríamos como en el caso anterior. Igual
que en el caso del triángulo, no todas las posiciones admiten solución. Por otro lado, es
posible que exista solución solo para un cierto rango de valores de ángulo del efector.
Con frecuencia, se establece una relación entre la posición del efector y su ángulo
alpha_3, basado en que la variación del ángulo durante el recorrido del robot sea
«suave».
CÓDIGO
Así sería un posible ejemplo de código para resolver un cuadrilátero articulado en
2D en un procesador como Arduino.
1 float L1, L2, L3, D;

2 float AbsoluteAngle1, AbsoluteAngle2, AbsoluteAngle3;
3 float RelativeAngle12, RelativeAngle23;
4 float TargetX, TargetY;
5
7 {
9 return d;
10 }
11
13 {
15
16 return angle3;
17 }
18
20 {
22 }
23
25 {
27 }
28
31 {
34
38
42 }
43
44 void SolveDirect(float angle1, float angle2, float angle3)
45 {
49
50 SolveDirect();
51 }
52
54 {
57
58 TargetX = L1 * cos(AbsoluteAngle1) + L2 * cos(AbsoluteAngle2) + L3 * cos(AbsoluteAngle3);
59 TargetY = L1 * sin(AbsoluteAngle1) + L2 * sin(AbsoluteAngle2) + L3 * sin(AbsoluteAngle3);
61 }
62
63 void SolveReverse(float absoluteAngle3)
64 {
65 AbsoluteAngle3 = absoluteAngle3;
67
68 float P2x = TargetX - L3 * cos(AbsoluteAngle3);
69 float P2y = TargetY - L3 * sin(AbsoluteAngle3);
70
71 float D0;
73
75 }
76
77 void Debug()
78 {
83
90
95 }
96
97
98 void setup()
99 {
102
103 L1 = 100; L2 = 50; L3 = 30;
104
106 SolveDirect(radians(55), radians(-20), radians(-70));
107 Debug();
108
110 TargetX = 114.60289;
111 TargetY = 36.62342;
112 SolveReverse(radians(-70));
113 Debug();
114
115 }
116
117
118 void loop()
119 {
120 // Do nothing
121 delay(10000);
}
TRIÁNGULO ARTICULADO 3D
DIRECTO
El caso de un polígono articulado en 3D puede resolverse con las mismas
herramientas que en el caso 2D, simplemente considerando que ocurren en un plano
girado un ángulo alpha_0 respecto al eje Z.
Para convertir las coordenadas de cualquier punto calculado en el plano (Pn’) a sus
equivalentes en 3D (Pn) usamos las siguientes relaciones.
Siendo sigma el ángulo de rotación entre el plano y el eje X.

INVERSO
De forma similar, podremos convertir el cálculo inverso de un triángulo en 3D a un
caso en 2D proyectando en el plano equivalente.
Para ello, llevaremos las coordenadas 3D del efector (P2) a su equivalente en 2D

(P2′) mediante la siguientes relaciones.
Finalmente, resolveríamos como en el caso 2D. Los ángulos entre segmentos

calculados son los mismos que en el caso 3D. Si quisiéramos calcular coordenadas de
puntos, usaríamos las relaciones anterior para pasar de 2D a 3D.
CÓDIGO
Así sería un posible ejemplo de código para resolver un triángulo articulado en 3D
en un procesador como Arduino.
1 float L1, L2, D;

2 float AbsoluteAngle0, AbsoluteAngle1, AbsoluteAngle2;
3 float RelativeAngle12;
4 float TargetX, TargetY, TargetZ;
5
7 {
9 return d;
10 }
11
12 float Pythagoras(const float& x, const float& y, const float& z)
13 {
14 const float d = sqrt(x * x + y * y + z * z);
15 return d;
16 }
17
19 {
21
22 return angle3;
23 }
24
26 {
28 }
29
31 {
33 }
34
37 {
40
44
48 }
49
50 void SolveDirect(float angle0, float angle1, float angle2)
51 {
55
56 SolveDirect();
57 }
58
60 {
62
63 float L = L1 * cos(AbsoluteAngle1) + L2 * cos(AbsoluteAngle2);
64 TargetX = L * cos(AbsoluteAngle0);
65 TargetY = L * sin(AbsoluteAngle0);
66 TargetZ = L1 * sin(AbsoluteAngle1) + L2 * sin(AbsoluteAngle2);
67 D = Pythagoras(TargetX, TargetY, TargetZ);
68 }
69
70 void SolveReverse()
71 {
72 AbsoluteAngle0 = atan2(TargetY, TargetX);
74
75 float Lx = Pythagoras(TargetX, TargetY);
76 float P2x = Lx;
77 float P2y = TargetZ;
78
79 float D0;
81 }
82
83 void Debug()
84 {
89 Serial.print(F("Target Z:\t"));
90 Serial.println(TargetZ, 4);
91
98
101 }
102
103
104 void setup()
105 {
108
109 L1 = 100; L2 = 50;
110
112 SolveDirect(radians(30), radians(55), radians(-20));
113 Debug();
114
116 TargetX = 90.3631;
117 TargetY = 52.1711;
118 TargetZ = 64.8142;
119 SolveReverse();
120 Debug();
121 }
122
123
124 void loop()
125 {
126 // Do nothing
127 delay(10000);
}
CUADRILÁTERO ARTICULADO
3D
DIRECTO
Análogamente, para el calculo directo de un cuadrilátero articulado en 3D
proyectamos igualmente en un plano 2D usando las expresiones del apartado anterior,
únicamente considerando que el ángulo alpha_0 esta vez es.
INVERSO
Igualmente, un cuadrilátero articulado puede resolverse con las mismas
herramientas que su equivalente en 2D. La única diferencia es que, en esta ocasión, las
coordenadas del efectos en el caso equivalente en 2D (P3′) se calculan a partir del punto
3D (P3) según la siguiente relación.
CÓDIGO
Así sería un posible ejemplo de código para resolver un cuadrilátero articulado en
3D en un procesador como Arduino.
1 float L1, L2, L3, D;

2 float AbsoluteAngle0, AbsoluteAngle1, AbsoluteAngle2, AbsoluteAngle3;
3 float RelativeAngle12, RelativeAngle23;
4 float TargetX, TargetY, TargetZ;
5
7 {
9 return d;
10 }
11
12 float Pythagoras(const float& x, const float& y, const float& z)
13 {
14 const float d = sqrt(x * x + y * y + z * z);
15 return d;
16 }
17
19 {
21
22 return angle3;
23 }
24
26 {
28 }
29
31 {
33 }
34
37 {
40
44
48 }
49
50 void SolveDirect(float angle0, float angle1, float angle2, float angle3)
51 {
56
57 SolveDirect();
58 }
59
61 {
64
65 float L = L1 * cos(AbsoluteAngle1) + L2 * cos(AbsoluteAngle2) + L3 * cos(AbsoluteAngle3);
66 TargetX = L * cos(AbsoluteAngle0);
67 TargetY = L * sin(AbsoluteAngle0);
68 TargetZ = L1 * sin(AbsoluteAngle1) + L2 * sin(AbsoluteAngle2) + L3 * sin(AbsoluteAngle3);
70 }
71
72 void SolveReverse(float absoluteAngle3)
73 {
74 AbsoluteAngle3 = absoluteAngle3;
75 AbsoluteAngle0 = atan2(TargetY, TargetX);
77
78 float Lx = Pythagoras(TargetX, TargetY);
79 float P2x = Lx - L3 * cos(AbsoluteAngle3);
80 float P2y = TargetZ - L3 * sin(AbsoluteAngle3);
81
82 float D0;
84
86 }
87
88 void Debug()
89 {
94 Serial.print(F("Target Z:\t"));
95 Serial.println(TargetZ, 4);
96
105
110 }
111
112
113 void setup()
114 {
117
118 L1 = 100; L2 = 50; L3 = 30;
119
121 SolveDirect(radians(30), radians(55), radians(-20), radians(-70));
122 Debug();
123
125 TargetX = 99.2490;
126 TargetY = 57.3014;
127 TargetZ = 36.62342;
128 SolveReverse(radians(-70));
129 Debug();
130
131 }
132
133
134 void loop()
135 {
136 // Do nothing
137 delay(10000);
}
LIBRERÍA DE ARDUINO
ARTICULATED
¿Y si lo metemos en una librería para que sea más cómodo de usar? Por supuesto
que sí, aquí una librería de Articulated para Arduino, que realiza todos los cálculos
anteriores de forma cómoda y sencilla. A disfrutarlo!

Resolver Robots Articulados Con Arduino

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Resolver Robots Articulados Con Arduino

Cargado por

Copyright:

Formatos disponibles

RESOLVER ROBOTS ARTICULADOS CON ARDUINO

La resolución de la posición y ángulos de polígonos articulados, en especial

El cálculo no es especialmente complicado, pero requiere de una pequeña dosis de

El cálculo directo de la posición del efectos (P2), en función de los ángulos

El cálculo inverso, es igualmente sencillo, considerando que los dos segmentos

Podemos calcular distancia D desde la base P0 a P2, aplicando el teorema de

Finalmente, calculamos el ángulo absoluto del primer segmento haciendo.

1 float L1, L2, D;

1 float L1, L2, L3, D;

Siendo sigma el ángulo de rotación entre el plano y el eje X.

Para ello, llevaremos las coordenadas 3D del efector (P2) a su equivalente en 2D

Finalmente, resolveríamos como en el caso 2D. Los ángulos entre segmentos

1 float L1, L2, D;

1 float L1, L2, L3, D;

También podría gustarte