4 Cinemática inversa

El problema de la cinemática inversa consiste en determinar las coordenadas articulares de un manipulador a partir de una posición u orientación deseada para su elemento terminal. A diferencia de la cinemática directa, en la que se conocen las coordenadas articulares y se calcula la pose resultante, aquí se parte de una tarea especificada en el espacio operacional para buscar una o más configuraciones que la satisfagan.

La cinemática inversa depende de la geometría del manipulador y de las restricciones impuestas a la tarea. Una misma pose puede alcanzarse con configuraciones distintas, no ser alcanzable o, en ciertas circunstancias, admitir infinitas configuraciones. Por ello, resolver el problema no consiste solamente en despejar incógnitas: también interesa determinar qué soluciones existen y cuáles son admisibles.

4.0.1Formulación del problema y tipos de solución¶

Para un manipulador serial de n articulaciones, representamos las coordenadas articulares mediante $\vec{q}=[q_1,q_2,\ldots,q_n]^T$. Si se especifica una pose completa deseada, el problema se expresa como

donde $T_n^0(\vec{q})$ es la transformación obtenida por cinemática directa y $H$ contiene la posición y orientación deseadas respecto del sistema base. Buscamos valores de $\vec{q}$ dentro del conjunto de configuraciones admisibles que satisfagan esta igualdad.

No todas las tareas exigen una pose completa. Si únicamente se prescribe la posición del extremo, basta imponer

En otras aplicaciones se especifican la posición y una componente de orientación, como ocurre con ciertos manipuladores planos o con el robot SCARA. Las ecuaciones deben corresponder a las variables operacionales que realmente se desean controlar.

Aunque la igualdad entre dos transformaciones homogéneas puede escribirse comparando sus entradas, la pose espacial contiene seis parámetros independientes: tres de posición y tres de orientación. Las entradas de la matriz de rotación están relacionadas por las condiciones de ortogonalidad y determinante unitario; por tanto, no constituyen nueve restricciones independientes.

Según el objetivo y la geometría del robot, pueden presentarse distintas situaciones:

• Sin solución: el objetivo es inalcanzable, ya sea por la geometría del mecanismo o por sus límites articulares.

• Con una o varias soluciones discretas: distintas configuraciones articulares pueden producir la misma posición o pose, como sucede con las configuraciones de codo arriba y codo abajo.

• Con infinitas soluciones: algunas tareas dejan grados de libertad sin determinar, como ocurre en ciertos manipuladores redundantes o en configuraciones geométricas especiales.

Las soluciones deben interpretarse teniendo en cuenta los límites articulares y, cuando corresponda, las singularidades. El hecho de encontrar una solución matemática no garantiza que sea una configuración físicamente admisible.

En estos apuntes distinguiremos dos familias de procedimientos:

1. Métodos de solución cerrada: emplean relaciones algebraicas, trigonométricas o construcciones geométricas para obtener expresiones de las coordenadas articulares. Estudiaremos por separado los enfoques algebraico y geométrico por su utilidad didáctica.

2. Métodos numéricos: buscan soluciones aproximadas mediante procedimientos iterativos. Su desarrollo se relaciona estrechamente con la cinemática diferencial, que estudiaremos posteriormente.

No existe un procedimiento de solución cerrada aplicable de manera general a cualquier manipulador. La elección del método depende de la estructura cinemática y de las características de la tarea.

4.0.2Método algebraico¶

En el enfoque algebraico partimos del modelo de cinemática directa y formulamos ecuaciones escalares a partir de las componentes de posición y, cuando sea necesario, de orientación. Por ejemplo, en una tarea de pose completa buscamos las coordenadas articulares que verifiquen la ecuación (1).

Las ecuaciones resultantes suelen ser no lineales y contener funciones trigonométricas. Para resolverlas podemos emplear identidades trigonométricas, eliminación de variables y funciones trigonométricas inversas. Es importante conservar los cuadrantes y considerar las distintas ramas de solución; por esa razón utilizaremos \atantwo{y}{x} cuando debamos reconstruir un ángulo a partir de sus componentes seno y coseno.

En manipuladores de pocos grados de libertad suele ser posible obtener fórmulas compactas. Los ejemplos siguientes ilustran primero una tarea de posición en un robot RR plano y después una tarea de posición y orientación en un robot RRR plano.

Ejemplo. Calcule la cinemática inversa del manipulador RR mostrado en la Figura Figure 1. Considere que se requiere que el extremo del manipulador se posicione en las coordenadas $(P_x, P_y)$.

Solución:

Naturalmente, las longitudes de los eslabones ($l_1$ y $l_2$) del robot son valores constantes y por lo tanto se asumen conocidos. Para resolver la cinemática inversa por el método analítico debemos primero calcular la matriz que describe la cinemática directa. En este caso $T_2^0$ está dada por:

$$T_2^0 = \begin{bmatrix} c_{12} & -s_{12} & 0 & l_1 c_1 + l_2 c_{12} \\ s_{12} & c_{12} & 0 & l_1 s_1 + l_2 s_{12} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

(3)

Conocida $T_2^0$, igualamos sus componentes de posición con las coordenadas deseadas $(P_x,P_y)$. Obtenemos las ecuaciones:

$$l_1 c_1 + l_2 c_{12} = P_x$$

(4)

$$l_1 s_1 + l_2 s_{12} = P_y$$

(5)

Al elevar al cuadrado ambas ecuaciones y sumar, resulta

$$P_x^2+P_y^2 =l_1^2+l_2^2 +2l_1l_2\left(c_1c_{12}+s_1s_{12}\right).$$

(6)

Aplicando la identidad $\cos(A-B)=\cos A\cos B+\sin A\sin B$:

$$P_x^2+P_y^2=l_1^2+l_2^2+2l_1l_2\cos q_2.$$

(7)

Definimos

$$K=\frac{P_x^2+P_y^2-l_1^2-l_2^2}{2l_1l_2}=\cos q_2.$$

(8)

Como $\sin q_2=\pm\sqrt{1-K^2}$, obtenemos las dos ramas de solución:

$$\boxed{q_2=\atantwo{\pm\sqrt{1-K^2}}{K}}.$$

(9)

Para que exista una solución real es necesario que $|K|\leq 1$, suponiendo $l_1,l_2>0$. Esta condición es equivalente a que la distancia $r=\sqrt{P_x^2+P_y^2}$ satisfaga

$$|l_1-l_2|\leq r\leq l_1+l_2.$$

(10)

Para calcular $q_1$, retomamos las ecuaciones (4) y (5). Al multiplicar en cruz las expresiones de posición obtenemos

$$P_y\left(l_1 c_1+l_2 c_{12}\right) = P_x\left(l_1 s_1+l_2 s_{12}\right).$$

(11)

Aplicando las identidades trigonométricas de suma de ángulos:

$$P_y\left(l_1 c_1+l_2 c_1 c_2-l_2 s_1 s_2\right) = P_x\left(l_1 s_1+l_2 s_1 c_2+l_2 c_1 s_2\right).$$

(12)

Agrupando los términos que dependen de $\cos q_1$ y $\sin q_1$:

$$c_1\left(P_y l_1+P_y l_2 c_2-P_x l_2 s_2\right) = s_1\left(P_x l_1+P_x l_2 c_2+P_y l_2 s_2\right).$$

(13)

Para recuperar el ángulo sin perder información del cuadrante, utilizamos las dos cantidades

$$\begin{aligned} N &= P_y(l_1+l_2 c_2)-P_x l_2 s_2,\\ D &= P_x(l_1+l_2 c_2)+P_y l_2 s_2. \end{aligned}$$

(14)

De las ecuaciones originales de posición se comprueba que

$$N=r^2\sin q_1,\qquad D=r^2\cos q_1, \qquad r^2=P_x^2+P_y^2.$$

(15)

Por lo tanto, si $r\neq 0$, obtenemos la solución algebraica:

$$\boxed{ q_1=\atantwo{ P_y l_1+P_y l_2 c_2-P_x l_2 s_2 }{ P_x l_1+P_x l_2 c_2+P_y l_2 s_2 }. }$$

(16)

Una forma alternativa de interpretar el resultado. Si reescribimos las ecuaciones de posición como

$$\begin{bmatrix}P_x\\P_y\end{bmatrix} = \begin{bmatrix} c_1&-s_1\\s_1&c_1 \end{bmatrix} \begin{bmatrix} l_1+l_2c_2\\l_2s_2 \end{bmatrix},$$

(17)

observamos que el vector de posición resulta de rotar un ángulo $q_1$ el vector de componentes $(l_1+l_2c_2,\;l_2s_2)$. Esta interpretación conduce a la expresión equivalente

$$q_1=\atantwo{P_y}{P_x} -\atantwo{l_2\sin q_2}{l_1+l_2\cos q_2}.$$

(18)

Para cada valor de $q_2$ debe utilizarse el valor correspondiente de $q_1$. En general, las dos ramas habituales coinciden geométricamente cuando $|K|=1$. Existe un caso especial: si $l_1=l_2$ y $P_x=P_y=0$, entonces $q_2=\pi$, pero $q_1$ puede tomar cualquier valor; en ese caso no debemos evaluar ninguna de las expresiones anteriores con argumentos $(0,0)$ en la función arctan2.

Podemos comprobar cada solución sustituyéndola en las ecuaciones (4) y (5).

4.0.3Cinemática inversa de posición: método geométrico¶

En el método geométrico aprovechamos la disposición de los ejes articulares y las dimensiones conocidas de los eslabones para relacionar las variables articulares con la posición deseada. Es frecuente utilizar triángulos, proyecciones sobre planos y relaciones trigonométricas, como la ley de cosenos.

La elección del plano de proyección no sigue una regla universal basada únicamente en el tipo de articulación. Buscamos el plano en el que se aprecien mejor los movimientos y las longitudes relevantes. Por ejemplo, en un manipulador plano RR todas las articulaciones revolutas giran alrededor de ejes paralelos y podemos trabajar directamente en el plano de movimiento; en ciertos manipuladores espaciales conviene utilizar varias proyecciones.

Es recomendable construir diagramas en configuraciones suficientemente generales para distinguir longitudes y ángulos, pero no es incorrecto representar una configuración con ángulos múltiplos de $90^\circ$ o desplazamientos nulos. Lo importante es no confundir una posición elegida para dibujar el mecanismo con una restricción de su movimiento.

El enfoque geométrico permite obtener expresiones compactas cuando la arquitectura del robot favorece estas construcciones. Aunque aquí lo utilizaremos principalmente para cinemática inversa de posición, también puede ayudar a resolver aspectos de orientación y forma parte de los métodos de solución cerrada.

4.0.4Desacoplo cinemático¶

La mayoría de manipuladores industriales de 6 grados de libertad, como el que se muestra en la Figura Figure 10, están conformados por una morfología base (RRR), a la cual se le adiciona un mecanismo de muñeca esférica (RRR). Para este tipo de manipuladores es posible aplicar una técnica conocida como desacoplo cinemático, la cual consiste en dividir el problema de la cinemática inversa en dos subproblemas: cinemática inversa de posición y cinemática inversa de orientación. La cinemática inversa de posición se suele abordar utilizando un enfoque geométrico y la cinemática inversa de orientación mediante un procedimiento analítico.

A continuación, vamos a describir una serie de pasos necesarios para realizar cinemática inversa de un manipulador de 6 grados de libertad utilizando la técnica de desacoplo cinemático. Para lo subsiguiente asumimos que se conoce de entrada la matriz de transformación homogénea $H$ que describe la posición y orientación deseada del extremo del manipulador.

Lo primero que hacemos es construir el diagrama cinemático del manipulador, basándonos en los manuales y esquemáticos del fabricante. Es importante identificar correctamente las distancias entre los ejes articulares. Con el diagrama ya realizado, procedemos a establecer los sistemas de referencia siguiendo la metodología de Denavit-Hartenberg. Obtenemos la tabla de parámetros y calculamos las matrices $R_3^0$ y $R_6^3$. Es importante comprender que $R_3^0$ será una matriz que depende de los primeras tres variables articulares ( $R_3^0 (q_1,q_2,q_3)$ ), y que $R_6^3$ depende de las últimas tres $R_6^3 (q_4,q_5,q_6)$.

Una vez hecho lo anterior, comenzamos a calcular las coordenadas del centro de la muñeca esférica, utilizando la siguiente ecuación:

Donde $\vec{r}_e$ son las coordenadas de posición que deberá alcanzar el extremo del manipulador, $d_6$ la distancia entre los ejes $x_5$ y $x_6$, medida en la dirección del eje $z_5$, y $\vec{z}_6^0$ la orientación del eje $z$ del sistema de referencia del elemento terminal con respecto al de la base. Los términos $\vec{r}_e$ y $\vec{z}_6^0$ se obtienen de la matriz $H$, de manera específica:

Ahora procedemos a resolver la cinemática inversa de posición utilizando el método geométrico y referenciando todo el procedimiento a las coordenadas del centro de la muñeca esférica. Con esto calcularemos las tres primeras variables articulares ($q_1$, $q_2$ y $q_3$).

Con los valores de $q_1$, $q_2$ y $q_3$ ya calculados, procedemos a determinar de forma numérica la matriz de rotación $R_3^0$, sustituyendo los valores numéricos calculados previamente.

De la cinemática directa sabemos que:

Lo cual implica que:

Donde $R_6^0$ es la submatriz de rotación de $H$, $R_3^0$ la matriz numérica calculada previamente y $R_6^3$ la matriz de rotación en forma simbólica calculada al inicio del procedimiento. La expresión anterior define una ecuación matricial que deberemos resolver de manera analítica, y con lo cual calcularemos los valores para las tres variables articulares restantes ($q_4$, $q_5$ y $q_6$). Esta parte es lo que corresponde a la cinemática inversa de orientación. Usualmente deberemos trabajar con las ecuaciones que resulten de igualar los elementos correspondientes a la tercera fila y a la tercera columna: $r_{31}$, $r_{32}$, $r_{33}$, $r_{23}$, $r_{13}$; dado que suelen quedar expresiones más sencillas de manipular.

Para comprobar los resultados podemos sustituir los valores calculados en la matriz $T_6^0$ de la cinemática directa y verificar que sea igual a la matriz $H$.

4.0.5Resolviendo la cinemática inversa de forma numérica¶

4.0.5.1Método de Newton¶

4.0.5.2Descenso del gradiente¶

4.1Problemas¶

Calcule la cinemática inversa del manipulador RRP mostrado en la Figura Figure 14, considere que $d_1$ es una longitud conocida. El elemento terminal deberá alcanzar la posición dada por:

Calcule la cinemática inversa del manipulador RRP mostrado en la Figura Figure 14, considere que $d_1 = 300$mm. El elemento terminal deberá alcanzar la posición dada por:

Calcule la cinemática inversa del manipulador RRP mostrado en la Figura Figure 14, considere que $d_1 = 280$mm. El elemento terminal deberá alcanzar la posición dada por:

Calcula la cinemática inversa del manipulador RRR mostrado en la Figura Figure 2, considerando que el elemento terminal deberá alcanzar la posición dada por:

Y orientado de tal manera que el eje $x_3$ se encuentre alineado con $x_0$. Se sabe que $l_1=l_2=200 \text{ mm}$ y $l_3 = 150 \text{ mm}$.

En la Figura Figure 15 se muestra un manipulador PRR que deberá alcanzar dos objetos A y B, tal que sus posiciones y orientaciones están definidas mediante los sistemas de referencia $\{A\}$ y $\{B\}$ adheridos a estos. Se sabe que:

Calcule los valores articulares requeridos para alcanzar cada uno de los objetos. Considere que $l_2 = 250$ mm y $l_3 = 200$ mm.

En la figura se muestra el esquema de un manipulador planar PPR que deberá realizar operaciones de soldadura a lo largo de la trayectoria compuesta, esquematizada mediante la línea discontinua. Los puntos A y B denotan el inicio y fin de la trayectoria, y el elemento terminal del manipulador deberá alcanzar estos puntos con la orientación dada por los sistemas $\{A\}$ y $\{B\}$. La longitud $l_3$ es de 100 mm. Se sabe que:

Calcule los valores articulares requeridos para alcanzar los puntos $A$ y $B$.

Calcula la cinemática inversa del manipulador RRP mostrado en la Figura Figure 17, considerando que el elemento terminal deberá alcanzar el punto $(200, 250, 350)$.

Para el manipulador industrial mostrado en la Figura Figure 18, calcule la cinemática inversa de posición y orientación si el extremo del manipulador debe estar orientado y posicionado de acuerdo con:

Para el manipulador industrial mostrado en la Figura Figure 18, calcule la cinemática inversa de posición y orientación si el extremo del manipulador debe estar orientado y posicionado de acuerdo con:

Utiliza la técnica del desacoplo cinemático para calcular la cinemática inversa del robot FANUC LR Mate 200iC/5L.

4.1.1Problemas para resolver utilizando la computadora¶

Desarrolla una función denominada rr_planar_robot_ik que deberá calcular las dos soluciones (codo arriba y codo abajo) de la cinemática inversa del manipulador planar RR mostrado en la figura. La función deberá recibir como argumentos de entrada las coordenadas ($P_x$, $P_y$) del punto que deberá alcanzar el manipulador, las longitudes de los eslabones: $l_1$ y $l_2$, y un argumento denominado sol en el cual deberá indicarse la solución a calcular, se usará 'up' para la solución de codo arriba y 'down' para la solución de codo abajo. Enseguida se muestra una sugerencia de plantilla para la definición de la función, designación de los parámetros y de los valores a devolver.

def rr_planar_robot_ik(px, py, l1, l2, sol):
	# hacer los cálculos aquí
	return q1, q2

Desarrolla una función denominada rrr_planar_robot_ik que calcule la cinemática inversa de un manipulador planar RRR mostrado en la Figura. La función debe recibir como argumentos de entrada las coordenadas $\left(P_x, P_y\right)$ del punto que deberá alcanzar el manipulador, el ángulo $\phi$ que describe la orientación, las longitudes de los eslabones: $l_1$, $l_2$ y $l_3$, y un argumento denominado sol en el cual deberá indicarse la solución a calcular, se usará 'up' para la solución de codo arriba y 'down' para la solución de codo abajo.

def rrr_planar_robot_ik(px, py, phi, l1, l2, l3, sol):
	# hacer los cálculos aquí
	return q1, q2, q3