Close

Cinemática Inversa

A project log for Proyecto Robótica I

Por Pablo Castillo y José Ramírez

ram17787ram17787 04/18/2021 at 09:580 Comments

Lo verdaderamente importante para controlar un robot es la cinemática inversa para saber, a partir de una pose deseada, la configuración necesaria para llegar a ella. Para esto, se parte del hecho que la cinemática diferencial es:

Por lo que con un simple despeje logramos tener una fórmula para encontrar la configuración siguiente a partir del error entre la posición actual y la deseada y la inversa del jacobiano:

Esto se convierte en un algoritmo iterativo en el cual se debe de recalcular el error y el jacobiano inverso hasta que se llegue a la posición deseada. Existen dos errores: el de posición y el de orientación. Para el primero únicamente se encuentra la cinemática directa tanto de la posición deseada como la posición actual y se restan los vectores de posición. Por el otro lado, para el error de orientación, se debe de utilizar cuaterniones. De las cinemáticas directas anteriores, también se extraen las matrices de rotación y se hace la conversión con:

A partir de estos cuaterniones, se utiliza el hecho que cuando un cuaternión se multiplica por su inverso, la componente epsilon es igual a 0. Para encontrar la inversa de un cuaternión, únicamente se multiplica la componente epsilon por -1. Para la multiplicación entre cuaterniones, se utiliza:

Para el jacobiano inverso, se necesita de una aproximación ya que la matriz no resulta ser cuadrada y, por ende, no tiene inversa directa. Luego de realizar pruebas con distintas configuraciones, se determinó que el más óptimo es el algoritmo de Levenberg-Marquardt. Este consiste en la ecuación:

Con esto, se tiene el algoritmo iterativo para encontrar la configuración deseada. Este algoritmo se repetirá hasta que el error esté por debajo de un valor. Usualmente se tiene un valor distinto para la posición y otro un poco mayor para el de orientación. En nuestro caso, se utilizaron 1e-6 y 1e-5 respectivamente. 

Aparte, en caso de intentar encontrar la cinemática inversa de una posición inalcanzable o una singularidad, se tiene un número máximo de iteraciones. Aquí se utilizó K = 100. En el código de Matlab, se tiene la opción de también regresar el historial si se desea.

Discussions