Cinématica directa 4GDL

Para definir la cinématica directa e inversa de un robot de 4 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=400$	$d_1=600$	$q_1$
2	$\pi$	$a_2=400$	$d_2=200$	$q_2$
3	$0$	$0$	$q_3$	$0$
4	$0$	$0$	$d_4=400$	$q_4$

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 & d_{1}\\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_{1} \cos{\left(q_{2} \right)}\\\sin{\left(q_{2} \right)} & - \cos{\left(q_{2} \right)} & 0 & a_{1} \sin{\left(q_{2} \right)}\\0 & 0 & -1 & d_{2}\\0 & 0 & 0 & 1\end{matrix}\right]$$ $$^{2}A_3 = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & q_{3}\\0 & 0 & 0 & 1\end{matrix}\right]$$ $$^{3}A_4 = \left[\begin{matrix}\cos{\left(q_{4} \right)} & - \sin{\left(q_{4} \right)} & 0 & 0\\\sin{\left(q_{4} \right)} & \cos{\left(q_{4} \right)} & 0 & 0\\0 & 0 & 1 & d_{4}\\0 & 0 & 0 & 1\end{matrix}\right]$$

Luego,

$$^{0}A_4 = \left[\begin{matrix}\left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{4} \right)} + \left(\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \sin{\left(q_{4} \right)} & - \left(- \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{4} \right)} + \left(\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \cos{\left(q_{4} \right)} & 0 & - a_{1} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + a_{1} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{1} \cos{\left(q_{1} \right)}\\\left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} - \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \sin{\left(q_{4} \right)} + \left(\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \cos{\left(q_{4} \right)} & \left(\sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} - \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)}\right) \cos{\left(q_{4} \right)} - \left(\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\right) \sin{\left(q_{4} \right)} & 0 & a_{1} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{1} \sin{\left(q_{1} \right)} + a_{1} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\\0 & 0 & -1 & d_{1} + d_{2} - d_{4} - q_{3}\\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)$, $(2,4)$ y $(3,4)$ de la matriz dan lugar a las posiciones $(x,y,z)$ del efector final del robot:

$$\begin{array}{cl} x &= - a_{1} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + a_{1} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{1} \cos{\left(q_{1} \right)} \\ y &= a_{1} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{1} \sin{\left(q_{1} \right)} + a_{1} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \\ z &= d_{1} + d_{2} - d_{4} - q_{3} \end{array}$$

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