Cinématica inversa 3GDL

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.