Cinématica robot 3GDL

Para definir la cinématica directa e inversa de un robot de 3 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	$\pi/2$	$0$	$d_1=100$	$q_1$
2	$-\pi/2$	$0$	$0$	$q_2$
3	$0$	$0$	$q_3$	$0$

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

Luego,

$$^{0}A_3 = \left[\begin{matrix}\cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} & - \sin{\left(q_{1} \right)} & - \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} & - q_{3} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)}\\\sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} & \cos{\left(q_{1} \right)} & - \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} & - q_{3} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)}\\\sin{\left(q_{2} \right)} & 0 & \cos{\left(q_{2} \right)} & d_{1} + q_{3} \cos{\left(q_{2} \right)}\\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 &= - q_{3} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} \\ y &= - q_{3} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} \\ z &= d_{1} + q_{3} \cos{\left(q_{2} \right)} \end{array}$$

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

Cinemática inversa

Para encontrar la cinemática inversa de este robot usaremos la resolución mediante las matrices de transformación homogeneas. Para esto usaremos la relación:

$$T = ^{0}A_1 \cdot ^{1}A_2 \cdot ^{2}A_3 \qquad\qquad (1)$$

donde

$$T= \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{y} & o_{y} & a_{y} & p_{y}\\n_{z} & o_{z} & a_{z} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$$

Para este método iremos pasando cada matriz de transformación $^{i-1}A_i$ al otro lado de la igualdad e iremos encontrando ecuaciones que nos permitan relacionar la variable articulares $q_i$ con las posiciones del efector final $(p_x,p_y,p_z)$.

$$^{0}A_1^{-1} \cdot T = ^{1}A_2 \cdot ^{2}A_3 \qquad\qquad (2)$$

luego,

$$\left[\begin{matrix}n_{x} \cos{\left(q_{1} \right)} + n_{y} \sin{\left(q_{1} \right)} & o_{x} \cos{\left(q_{1} \right)} + o_{y} \sin{\left(q_{1} \right)} & a_{x} \cos{\left(q_{1} \right)} + a_{y} \sin{\left(q_{1} \right)} & p_{x} \cos{\left(q_{1} \right)} + p_{y} \sin{\left(q_{1} \right)}\\n_{z} & o_{z} & a_{z} & - d_{1} + p_{z}\\n_{x} \sin{\left(q_{1} \right)} - n_{y} \cos{\left(q_{1} \right)} & o_{x} \sin{\left(q_{1} \right)} - o_{y} \cos{\left(q_{1} \right)} & a_{x} \sin{\left(q_{1} \right)} - a_{y} \cos{\left(q_{1} \right)} & p_{x} \sin{\left(q_{1} \right)} - p_{y} \cos{\left(q_{1} \right)}\\0 & 0 & 0 & 1\end{matrix}\right] = \left[\begin{matrix}\cos{\left(q_{2} \right)} & 0 & - \sin{\left(q_{2} \right)} & - q_{3} \sin{\left(q_{2} \right)}\\\sin{\left(q_{2} \right)} & 0 & \cos{\left(q_{2} \right)} & q_{3} \cos{\left(q_{2} \right)}\\0 & -1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$$

extrayendo el termino $(3,4)$:

$$p_{x} \sin{\left(q_{1} \right)} - p_{y} \cos{\left(q_{1} \right)} = 0$$

despejando $q_1$, obtenemos:

$$q_{1} = \operatorname{atan}{\left(\frac{p_{y}}{p_{x}} \right)} \qquad\qquad (Q1)$$

repetimos el proceso de pasar una matriz en la ecuación $(2)$:

$$^{1}A_2^{-1} \cdot ^{0}A_1^{-1} \cdot T = ^{2}A_3 \qquad\qquad (3)$$

quedando:

$$\left[\begin{matrix}n_{x} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + n_{y} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + n_{z} \sin{\left(q_{2} \right)} & o_{x} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + o_{y} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + o_{z} \sin{\left(q_{2} \right)} & a_{x} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{y} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + a_{z} \sin{\left(q_{2} \right)} & - d_{1} \sin{\left(q_{2} \right)} + p_{x} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + p_{y} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + p_{z} \sin{\left(q_{2} \right)}\\- n_{x} \sin{\left(q_{1} \right)} + n_{y} \cos{\left(q_{1} \right)} & - o_{x} \sin{\left(q_{1} \right)} + o_{y} \cos{\left(q_{1} \right)} & - a_{x} \sin{\left(q_{1} \right)} + a_{y} \cos{\left(q_{1} \right)} & - p_{x} \sin{\left(q_{1} \right)} + p_{y} \cos{\left(q_{1} \right)}\\- n_{x} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} - n_{y} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + n_{z} \cos{\left(q_{2} \right)} & - o_{x} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} - o_{y} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + o_{z} \cos{\left(q_{2} \right)} & - a_{x} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} - a_{y} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + a_{z} \cos{\left(q_{2} \right)} & - d_{1} \cos{\left(q_{2} \right)} - p_{x} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} - p_{y} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + p_{z} \cos{\left(q_{2} \right)}\\0 & 0 & 0 & 1\end{matrix}\right] = \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & q_{3}\\0 & 0 & 0 & 1\end{matrix}\right]$$

extrayendo el termino $(1,4)$:

$$- d_{1} \sin{\left(q_{2} \right)} + p_{x} \cos{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + p_{y} \sin{\left(q_{1} \right)} \cos{\left(q_{2} \right)} + p_{z} \sin{\left(q_{2} \right)} = 0$$

despejando $q_2$, obtenemos:

$$q_{2} = \operatorname{atan}{\left(\frac{\sqrt{p_{x}^2 + p_{y}^{2}}}{d_{1} - p_{z}} \right)} \qquad\qquad (Q2)$$

teniendo presente que:

$$p_{x} \cos{\left(q_{1} \right)} + p_{y} \sin{\left(q_{1} \right)} = \sqrt{p_{x}^2 + p_{y}^{2}}$$

Para $q_3$ podemos extraer el termino $(3,4)$ de la ecuación $(2)$:

$$- d_{1} \cos{\left(q_{2} \right)} - p_{x} \sin{\left(q_{2} \right)} \cos{\left(q_{1} \right)} - p_{y} \sin{\left(q_{1} \right)} \sin{\left(q_{2} \right)} + p_{z} \cos{\left(q_{2} \right)} = q_{3}$$

reorganizando los terminos:

$$q_{3} = \left( p_{z} - d_{1}\right) \cos{\left(q_{2} \right)} - \left(\sqrt{p_{x}^2 + p_{y}^{2}}\right) \sin{\left(q_{2} \right)} \qquad\qquad (Q3)$$

con las ecuaciones $(Q1)$, $(Q2)$ y $(Q3)$ completamos la cinemática inversa de este robot.