Cinématica robot 2GDL

Para definir la cinématica directa e inversa de un robot de 2 grados de libertad (GDL) necesitamos conocer inicialmente los parámetros del robot, es decir, los valores de Denavit-Hartenberg.

$i$	$\alpha_i$ (rad)	$a_i$ (mm)	$d_i$ (mm)	$\theta_i$ (rad)
1	$0$	$a_1 = 40$	$0$	$q_1$
2	$0$	$a_2 = 40$	$0$	$q_2$

Cinemática directa

Con los valores de la tabla anterior podemos contruir las matrices de transformación siguiendo la estructura:

$$^{i-1}A_i = T_z(\theta_i)\cdot T(0,0,d_i) \cdot T(a_i,0,0) \cdot T_x(\alpha_i)$$

De esto tenemos

$$^{0}A_1 = \left[\begin{matrix}\cos{\left(q_{1} \right)} & - \sin{\left(q_{1} \right)} & 0 & a_{1} \cos{\left(q_{1} \right)}\\\sin{\left(q_{1} \right)} & \cos{\left(q_{1} \right)} & 0 & a_{1} \sin{\left(q_{1} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$ $$^{1}A_2 = \left[\begin{matrix}\cos{\left(q_{2} \right)} & - \sin{\left(q_{2} \right)} & 0 & a_{2} \cos{\left(q_{2} \right)}\\\sin{\left(q_{2} \right)} & \cos{\left(q_{2} \right)} & 0 & a_{2} \sin{\left(q_{2} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$

Luego,

$$^{0}A_2 = ^{0}A_1\cdot^{1}A_2 = \left[\begin{matrix}- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} & - \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} - \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} & 0 & a_{1} \cos{\left(q_{1} \right)} - a_{2} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + a_{2} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\\\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} & - \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} & 0 & a_{1} \sin{\left(q_{1} \right)} + a_{2} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$

En esta matriz tenemos la transformación que sufre un objeto que se encuentra amarrado en el efector final del robot. Los terminos en las posiciones $(1,4)$ y $(2,4)$ de la matriz dan lugar a las posiciones $(x,y)$ del efector final del robot:

$$\begin{array}{cll} x &= a_{1} \cos{\left(q_{1} \right)} - a_{2} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + a_{2} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} &\qquad\qquad (1) \\ y &= a_{1} \sin{\left(q_{1} \right)} + a_{2} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{2} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} &\qquad\qquad (2) \end{array}$$

Asi hemos completado la cinemática directa de este robot.

Cinemática inversa

La cinemática inversa la encontraremos usando las ecuaciones anteriores. Empezamos elevando las ecuaciones $(1)$ y $(2)$ al cuadrado y las sumamos.

$$x^{2} + y^{2} = a_{1}^{2} + 2 a_{1} a_{2} \cos{\left(q_{2} \right)} + a_{2}^{2} \qquad\qquad (3)$$

Si despejamos $q_2$ obtendremos:

$$q_{2} = \left\{2 \pi - \operatorname{acos}{\left(- \frac{a_{1}^{2} + a_{2}^{2} - x^{2} - y^{2}}{2 a_{1} a_{2}} \right)}, \ \operatorname{acos}{\left(\frac{- a_{1}^{2} - a_{2}^{2} + x^{2} + y^{2}}{2 a_{1} a_{2}} \right)}\right\}$$

como vemos hay dos posibles soluciones. Ahora encontremos $q_1$, para esto vamos a remplazar en $(1)$ y $(2)$ los siguientes terminos:

$$\begin{array}{cl} A &= a_{1} + a_{2} \cos{\left(q_{2} \right)} \\ B &= a_{2} \sin{\left(q_{2} \right)} \end{array}$$

quedando de la siguiente forma:

$$\begin{array}{cll} x &= A \cos{\left(q_{1} \right)} - B \sin{\left(q_{1} \right)} &\qquad\qquad (4) \\ y &= A \sin{\left(q_{1} \right)} + B \cos{\left(q_{1} \right)} &\qquad\qquad (5) \end{array}$$

se multiplicara la ecuación $(4)$ por $A$ y $(5)$ por $B$:

$$\begin{array}{cll} A x &= A \left(A \cos{\left(q_{1} \right)} - B \sin{\left(q_{1} \right)}\right) &\qquad\qquad (6) \\ B y &= B \left(A \sin{\left(q_{1} \right)} + B \cos{\left(q_{1} \right)}\right) &\qquad\qquad (7) \end{array}$$

Sumando las ecuaciones $(6)$ y $(7)$:

$$A x + B y = A^{2} \cos{\left(q_{1} \right)} + B^{2} \cos{\left(q_{1} \right)}\qquad\qquad (8)$$

Despejando $q_1$:

$$q_1 = \left\{2 \pi - \operatorname{acos}{\left(\frac{A x + B y}{A^{2} + B^{2}} \right)}, \operatorname{acos}{\left(\frac{A x + B y}{A^{2} + B^{2}} \right)}\right\}$$

Con esto tenemos entonces la cinemática inversa para el robot de 2GDL. Con la solución para $q_1$ y $q_2$, podemos calcular los angulos de las articulaciones a partir de la posición deseada del efector final.

Importante

La solucion para la cinemática inversa arroja 4 soluciones, de estas se deberá verificar con la cinemática directa cuales de la soluciones son adecuadas para el sistema.

Por ejemplo, el efector final puede alcanzar la posición $(40,40)$ con las posiciones articulares:

$$\left(0,\frac{\pi}{2}\right) \qquad \text{o} \qquad \left(\frac{\pi}{2},-\frac{\pi}{2}\right)$$