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&
30 absoluteAngle1, float& 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,
70 RelativeAngle12);
71 }
72
73 void Debug()
74 {
75 Serial.print(F("Target X:\t"));
76 Serial.println(TargetX, 4);
77 Serial.print(F("Target Y:\t"));
78 Serial.println(TargetY, 4);
79
80 Serial.print(F("Abs. Angle 1:\t"));
81
82
83
84
85
86 Serial.println(degrees(AbsoluteAngle1), 4);
89
90 Serial.print(F("Rel. Angle 12:\t"));
91 Serial.println(degrees(RelativeAngle12), 4);
92 }
93
94
95 void setup()
96 {
97 Serial.begin(115200);
98 while (!Serial) { ; }
99
10 L1 = 100; L2 = 50;
0
10 Serial.println(F("Solve Direct Relative"));
1 SolveDirect(radians(55), radians(-20));
10 Debug();
2
10 Serial.println(F("Solve Inverse"));
3 TargetX = 104.3423;
10 TargetY = 64.8141;
4 SolveReverse();
10 Debug();
5 }
10
6
10 void loop()
7 {
10 // Do nothing
8 delay(10000);
10 }
9
11
0
11
1
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
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 *
59 cos(AbsoluteAngle3);
60 TargetY = L1 * sin(AbsoluteAngle1) + L2 * sin(AbsoluteAngle2) + L3 *
61 sin(AbsoluteAngle3);
63 }
64
65 void SolveReverse(float absoluteAngle3)
66 {
67 AbsoluteAngle3 = absoluteAngle3;
69
70 float P2x = TargetX - L3 * cos(AbsoluteAngle3);
71 float P2y = TargetY - L3 * sin(AbsoluteAngle3);
72
73 float D0;
76
78 }
79
80 void Debug()
81 {
86
93
98 }
99
10
0 void setup()
10 {
2
10 L1 = 100; L2 = 50; L3 = 30;
3
4 SolveDirect(radians(55), radians(-20), radians(-70));
10
5
10
6
10
7
10
8
10
9 Debug();
11
11 TargetX = 114.60289;
1 TargetY = 36.62342;
11 SolveReverse(radians(-70));
2 Debug();
11
3 }
11
4
11 void loop()
5 {
11 // Do nothing
6 delay(10000);
11 }
7
11
8
11
9
12
0
12
1
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
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;
82 }
83
84 void Debug()
85 {
90 Serial.print(F("Target Z:\t"));
91 Serial.println(TargetZ, 4);
92
99
10 }
1
10
2 void setup()
10 {
4
10 L1 = 100; L2 = 50;
5
6 SolveDirect(radians(30), radians(55), radians(-20));
10 Debug();
7
8 TargetX = 90.3631;
10 TargetY = 52.1711;
9 TargetZ = 64.8142;
11 SolveReverse();
0 Debug();
11 }
1
11
2
11
3
11
4
11
5
11
6
11
7
11
8
11 void loop()
9 {
12 // Do nothing
0 delay(10000);
12 }
1
12
2
12
3
12
4
12
5
12
6
12
7
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
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 *
66 cos(AbsoluteAngle3);
67 TargetX = L * cos(AbsoluteAngle0);
68 TargetY = L * sin(AbsoluteAngle0);
69 TargetZ = L1 * sin(AbsoluteAngle1) + L2 * sin(AbsoluteAngle2) + L3 *
70 sin(AbsoluteAngle3);
72 }
73
74 void SolveReverse(float absoluteAngle3)
75 {
76 AbsoluteAngle3 = absoluteAngle3;
77 AbsoluteAngle0 = atan2(TargetY, TargetX);
79
80 float Lx = Pythagoras(TargetX, TargetY);
81 float P2x = Lx - L3 * cos(AbsoluteAngle3);
82 float P2y = TargetZ - L3 * sin(AbsoluteAngle3);
83
84 float D0;
87
89 }
90
91 void Debug()
92 {
97 Serial.print(F("Target Z:\t"));
98 Serial.println(TargetZ, 4);
99
10
6 }
10
7
10 void setup()
8 {
11
0 L1 = 100; L2 = 50; L3 = 30;
11
11 SolveDirect(radians(30), radians(55), radians(-20), radians(-70));
2 Debug();
11
11 TargetX = 99.2490;
4 TargetY = 57.3014;
11 TargetZ = 36.62342;
5 SolveReverse(radians(-70));
11 Debug();
6
11 }
7
11
8 void loop()
11 {
9
12
0
12
1
12
2
12
3
12
4
12
5
12
6
12
7
12 // Do nothing
8 delay(10000);
12 }
9
13
0
13
1
13
2
13
3
13
4
13
5
13
6
13
7
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

Загружено:

Сведения о документе

Авторское право

Доступные форматы

Поделиться этим документом

Поделиться или встроить документ

Параметры публикации

Этот документ был вам полезен?

Это неприемлемый материал?

Авторское право:

Доступные форматы

Resolver Robots Articulados Con Arduino

Загружено:

Авторское право:

Доступные форматы

RESOLVER ROBOTS ARTICULADOS CON ARDUINO

En esta entrada vamos a ver las ecuaciones necesarias para calcular un

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

En general, en los robots articulados sabemos la distancia de cada uno

También conocemos las coordenadas del punto inicial del polígono (o

Con esto, tenemos dos tipos de cálculos que aparecen en la resolución

 Directo, donde sabemos los ángulos de las articulaciones, y queremos saber la

Siendo más frecuente en robots articulados el último de estos dos. Por

El cálculo no es especialmente complicado, pero requiere de una

Donde la relación entre los ángulos relativos entre los segmentos

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

Con esta distancia y las longitudes de los segmentos podemos calcular

Por otro lado, podemos calcular el ángulo que forma el triángulo

Conocido el ángulo absoluto del primer segmento y el ángulo relativo

Lógicamente, no todas las posiciones admiten una solución, como por

1 float L1, L2, D;

Para poder resolver el sistema debemos imponer una condición (o

Con las coordenadas del punto P2, resolveríamos como en el caso

Con frecuencia, se establece una relación entre la posición del efector y

1 float L1, L2, L3, D;

Para convertir las coordenadas de cualquier punto calculado en el

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

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

1 float L1, L2, D;

1 float L1, L2, L3, D;

Вам также может понравиться