INACAP Sede Calama

Ingeniería en automatización y control Industrial

Guía N°3 Cinemática Inversa aplicada a robótica

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Cinemática Inversa

El objetivo del problema cinemático inverso consiste en encontrar los valores que deben adoptar las
coordenadas articulares del robot q = [q1 q2…..qn], para que su extremo se posicione y oriente
según una determinada localización espacial. Así cómo es posible abordar el problema cinemático
directo de una manera sistemática a partir de la utilización de matrices de transformación
homogéneas, e independientemente de la configuración del robot, no ocurre lo mismo con el
problema cinemático inverso, siendo el procedimiento de obtención de las ecuaciones fuertemente
dependiente de la configuración del robot. Se han desarrollado algunos procedimientos genéricos
susceptibles de ser programados, de modo que un computador pueda, a partir del conocimiento de la
cinemática del robot (con sus parámetros de Denavit-Hartenberg, por ejemplo) obtener los valores
articulares que posicionan y orientan su extremo. El inconveniente de estos procedimientos es que
se trata de métodos numéricos iterativos, cuya velocidad de convergencia e incluso su convergencia
en sí no está siempre garantizada.

A la hora de resolver el problema cinemático inverso es mucho más adecuado encontrar una
solución cerrada. Esto es, encontrar una relación matemática explícita de la forma:

𝑞𝑘 = 𝑓𝑘(𝑥, 𝑦, 𝑧, ∅, 𝜃, 𝛾)

𝑘 = 1 … . 𝑛 (𝐺𝐷𝐿)

Este tipo de solución presenta, entre otras, las siguientes ventajas:

1. En muchas aplicaciones, el problema cinemático inverso ha de resolverse en tiempo real

(por ejemplo, en el seguimiento de una determinada trayectoria). Una solución de tipo
iterativo no garantiza tener la solución en el momento adecuado.
2. Al contrario de lo que ocurría en el problema cinemático directo, con cierta frecuencia la
solución del problema cinemático inverso no es única; existiendo diferentes juntas [q1, ...,
qn] T que posicionan y orientan el extremo del robot del mismo modo. En estos casos una
solución cerrada permite incluir determinadas reglas o restricciones que aseguren que la
solución obtenida sea la más adecuada de entre las posibles (por ejemplo, límites en los
recorridos articulares).

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial
No obstante, a pesar de las dificultades comentadas, la mayor parte de los robots poseen cinemáticas
relativamente simples que facilitan en cierta medida la resolución de su problema cinemático
inverso. Por ejemplo, si se consideran sólo los tres primeros grados de libertad de muchos robots,
éstos tienen una estructura planar, esto es, los tres primeros elementos quedan contenidos en un
plano. Esta circunstancia facilita la resolución del problema. Así mismo, en muchos robots se da la
circunstancia de que los tres grados de libertad últimos, dedicados fundamentalmente a orientar el
extremo del robot, corresponden a giros sobre ejes que se cortan en un punto. De nuevo esta
situación facilita el cálculo de la junta [q1, ..., qn] T correspondiente a la posición y orientación
deseadas. Por tanto, para los casos citados y otros, es posible establecer ciertas pautas generales que
permitan plantear y resolver el problema cinemático inverso de una manera sistemática. Los
métodos geométricos permiten, normalmente, obtener los valores de las primeras variables
articulares, que son las que consiguen posicionar el robot (prescindiendo de la orientación de su
extremo). Para ello utilizan relaciones trigonométricas y geométricas sobre los elementos del robot.
Se suele recurrir a la resolución de triángulos formados por los elementos y articulaciones del robot.

Como alternativa para resolver el mismo problema se puede recurrir a manipular directamente las
ecuaciones correspondientes al problema cinemático directo. Es decir, puesto que éste establece la
relación:

𝑛 𝑜 𝑎 𝑝
[ ] = [𝑡 ]
0 0 0 1 𝑖𝑗

donde los elementos 𝑡𝑖𝑗 son función de las coordenadas articulares [q1, ..., qn] T , es posible pensar
que mediante ciertas combinaciones de las ecuaciones, se puedan despejar las n variables articulares
qi en función de las componentes de los vectores n, o, a y p. Debe considerarse en este caso estas
responden a ecuaciones trigonométricas acopladas cuya resolución no es trivial. Para facilitar esta
solución se verá que se puede proceder de manera ordenada, despejando sucesivamente los grados
de libertad.

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Ejemplo 1 problema cinemático inversa robot Planar RR, utilizando métodos

trigonométricos

Imagen 1: Robot planar RR

Utilizar el teorema del coseno

2
𝐶2 𝐶 = 2𝑙1+2 𝑟𝑦
= 𝑟𝑥 2 2 − 2 ∗ 𝑙1 ∗ 𝑙2 ∗
+ 𝑙2 Aplicando Pitágoras
𝑐𝑜𝑠𝐶
𝐶 + 𝑞2 = 180°

𝑐𝑜𝑠𝐶 = cos(180 − 𝑞2)

Reemplazando la ecuación se obtiene lo siguiente:

Cos(180 − 𝑞2) = cos(180) ∗ cos(𝑞2) + 𝑠𝑒𝑛(180) ∗ 𝑠𝑒𝑛(𝑞2)

.

cos(180 − 𝑞2) = cos(180) ∗ cos(𝑞2)

cos 𝑐 = cos(180 − 𝑞2) = − cos(𝑞2)

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

𝐶2 = 𝑙12 + 𝑙22 − 2 ∗ 𝑙1 ∗ 𝑙2 ∗ 𝑐𝑜𝑠𝐶

𝐶2 = 𝑟𝑥2 + 𝑟𝑦2

cos 𝑐 = cos(180 − 𝑞2) = − cos(𝑞2)

𝑟𝑥2 + 𝑟𝑦2 = 𝑙12 + 𝑙22 − 2 ∗ 𝑙1 ∗ 𝑙2 ∗ (− cos(𝑞2))

𝑟𝑥2 + 𝑟𝑦2 = 𝑙12 + 𝑙22 + 2 ∗ 𝑙1 ∗ 𝑙2 ∗ cos(𝑞2)

𝑟𝑥2+𝑟𝑦2−𝑙12−𝑙22
cos(𝑞2) = 2∗𝑙1∗𝑙2 =M

𝑠𝑒𝑛(𝑞2) = ±√1 − 𝑀2
𝑠𝑒𝑛(𝑞2)
tan(𝑞2) =
cos(𝑞2)
±√1−𝑀2
tan(𝑞2) =
𝑀

±√1 − 𝑀2
𝑞2 = atan( 𝑀 )

Al encontrar q2 el signo de la operación del arco tangente se obtiene dos soluciones un codo
arriba y otro codo abajo.

Imagen N°2: Soluciones cinemática inversa codo abajo (a) y codo arriba (b)

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Se debe despejar q1 en función de la q2 ya encontrada:

𝛼 = 𝑞1 + 𝛽
𝑟𝑦
𝑡𝑎𝑛(𝛼) =
𝑟𝑥
𝑟𝑦
𝛼 = 𝑎𝑡𝑎𝑛
𝑟𝑥
𝑦
tan(𝛽) =
𝑙1 + 𝑥
𝑙2 ∗ 𝑠𝑒𝑛(𝑞2)
tan(𝛽) =
𝑙1 + 𝑙2 ∗ cos(𝑞2)

𝑙2∗𝑠𝑒𝑛(𝑞2)
𝛽 = atan( )
𝑙1+𝑙2∗cos(𝑞2)

𝛼 = 𝑞1 + 𝛽
𝑞1 = 𝛼 − 𝛽

𝑞1 = 𝑎𝑡𝑎𝑛 𝑟𝑦 − atan( 𝑙2∗𝑠𝑒𝑛(𝑞2) )

𝑟𝑥 𝑙1+𝑙2∗cos(𝑞2)

Utilizando Matlab se realiza un script de cinemática inversa planar como se muestra en la siguiente
imagen:

Imagen N°3: Scripts cinemática inversa planar

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Ejemplo 1 problema cinemático inversa robot Planar RR, utilizando métodos algebraicos con
algoritmo de búsqueda de ángulos

Imagen 4: Robot planar RR

Para determinar la cinemática inversa se debe determinar la cinemática directa

simbólicamente, se utilizará el método Denavit Hartenberg:

Link 𝜽 d a 𝑎
l1 q1 0 l1 0
l1 q1 0 l2 0

Se debe obtener la matriz de transformación de coordenadas homogéneas de manera simbólica:

𝑇 = 𝑅(𝑧, 𝑞1) ∗ 𝑡(𝑥, 𝑙1) ∗ 𝑅(𝑧, 𝑞2) ∗ 𝑡(𝑥, 𝑙2)

cos 𝑞1 − sin 𝑞1 0 0 1 0 0 𝑙1 cos 𝑞2 − sin 𝑞2 0 0 1 0 0 𝑙2

sin 𝑞1 cos 𝑞1 0 0 0 1 0 0 sin 𝑞2 cos 𝑞2 0 0 0 1 0 0
𝑇=[ ]∗[ ]∗[ ]∗ ]
0 0 1 0 [
0 0 1 0 0 0 1 0 0 1 0
0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Simbólicamente al realizar el producto de matrices se obtiene lo siguiente:

𝑇
cos 𝑞1 ∗ cos 𝑞2 − sin 𝑞1 ∗ sin − cos 𝑞1 ∗ sin 𝑞2 − cos 𝑞2 ∗ sin 0 𝑙2 ∗ (cos 𝑞2 ∗ cos 𝑞2 − sin 𝑞1 ∗ sin 𝑞2) + 𝑙1 ∗ cos
= 𝑞2 𝑞2 𝑞1 ]
[cos 𝑞1 ∗ sin 𝑞2 + cos 𝑞2 ∗ sin 𝑞1 cos 𝑞1 ∗ cos 𝑞2 − sin 𝑞1 ∗ sin 𝑞2 0 𝑙2 ∗ (cos 𝑞1 ∗ sin 𝑞2 + cos 𝑞2 ∗ sin 𝑞1) + 𝑙1 ∗ sin 𝑞1
0 0 1 0

De la matriz anterior se obtiene la siguiente relación con la posición:

𝑙2 ∗ (cos 𝑞2 ∗ cos 𝑞2 − sin 𝑞1 ∗ sin 𝑞2) + 𝑙1 ∗ cos 𝑞1 = 𝑥

𝑙2 ∗ (cos 𝑞1 ∗ sin 𝑞2 + cos 𝑞2 ∗ sin 𝑞1) + 𝑙1 ∗ sin 𝑞1 = 𝑦

Utilizando la propiedad trigonométrica de sumas de cosenos y senos se obtiene:

𝑙2 ∗ (cos(𝑞1 + 𝑞2)) + 𝑙1 ∗ cos 𝑞1 = 𝑥

𝑙2 ∗ (sin(𝑞1 + 𝑞2)) + 𝑙1 ∗ sin 𝑞1 = 𝑦

Ya obtenida estas ecuaciones se realizará un sistema de ecuaciones sumándolas:

𝑙2 ∗ (cos(𝑞1 + 𝑞2)) + 𝑙1 ∗ cos 𝑞1 + 𝑙2 ∗ (sin(𝑞1 + 𝑞2)) + 𝑙1 ∗ sin 𝑞1 = 𝑥 + 𝑦

Para reducir esta ecuación se elevará al cuadrado

𝑙12 ∗ 𝑐𝑜𝑠2 𝑞12 + 𝑙22 ∗ 𝑐𝑜𝑠2(𝑞1 + 𝑞2)2 + 2 ∗ 𝑙2 ∗ (cos(𝑞1 + 𝑞2)) ∗ 𝑙1 ∗ cos 𝑞1 ∗ 𝑙1 ∗ sin 𝑞1 + 𝑙12 ∗ 𝑠𝑖𝑛2 𝑞12 + 𝑙22
∗ 𝑠𝑖𝑛2(𝑞1 + 𝑞2)2 + 2 ∗ 𝑙2 ∗ (sin(𝑞1 + 𝑞2)) ∗ 𝑙1 ∗ sin 𝑞1 = 𝑥2 + 𝑦2

Factorizamos por las constantes l1 y l2:

𝑙12(𝑐𝑜𝑠2 𝑞12 + 𝑠𝑖𝑛2 𝑞12) + 𝑙22(𝑐𝑜𝑠2(𝑞1 + 𝑞2)2 + 𝑠𝑖𝑛2(𝑞1 + 𝑞2)2) + 2 ∗ 𝑙1
∗ 𝑙2(cos 𝑞1 ∗ cos(𝑞1 + 𝑞2) + sin 𝑞1 ∗ sin(𝑞1 + 𝑞2))

Utilizando la sumatoria cosenos y senos cuadrados que es igual a uno se reduce la ecuación
a lo siguiente:

𝑙12 + 𝑙22 + 2 ∗ 𝑙1 ∗ 𝑙2(cos 𝑞1 ∗ cos(𝑞1 + 𝑞2) + sin 𝑞1 ∗ sin(𝑞1 + 𝑞2)) = 𝑥2 + 𝑦2

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Determinamos lo siguiente:
cos 𝑞1 ∗ cos(𝑞1 + 𝑞2) + sin 𝑞1 ∗ sin(𝑞1 + 𝑞2)) = cos(𝑞1 − (𝑞1 + 𝑞2)) = cos(𝑞2)

Despejamos la siguiente ecuación:

𝑙12 + 𝑙22 + 2 ∗ 𝑙1 ∗ 𝑙2 ∗ cos 𝑞2 = 𝑥2 + 𝑦2

𝑥2 + 𝑦2 − 𝑙12 − 𝑙22
cos 𝑞2 =
2 ∗ 𝑙1 ∗ 𝑙2

𝑥2 + 𝑦2 − 𝑙12 − 𝑙22
𝑞2 = 𝑐𝑜𝑠−1( 2 ∗ 𝑙1 ∗ 𝑙2 )

Al encontrar la ecuación del ángulo q2, se utiliza un algoritmo para encontrar un ángulo q1 del valor
que recorra la escala de 0 hasta 180°, con un incremento de 0.001 Rad

For q1=0 : 0.001 : pi

IF ((𝑙2 ∗ (𝑐𝑜𝑠 𝑞2 ∗ 𝑐𝑜𝑠 𝑞2 − 𝑠𝑖𝑛 𝑞1 ∗ 𝑠𝑖𝑛 𝑞2) + 𝑙1 ∗ 𝑐𝑜𝑠 𝑞1) –x) <= 0.01)
Break

Utilizando Matlab y calculando previamente el Angulo q2 se obtendrá q1 con un algoritmo for.

Imagen 5: Scripts de Matlab para encontrar el ángulo q1

Robótica Aplicada a procesos Docente: Wilson Hernán Robles

INACAP Sede Calama
Ingeniería en automatización y control Industrial

Robótica Aplicada a procesos Docente: Wilson Hernán Robles