Problema de singularidad en solucionador cinemático inverso

Estoy luchando con este problema por días. Realmente espero que alguien pueda darme una pista de cuál es el problema.

El robot consta de 5 ejes. El primer eje gira alrededor del eje z y otros 4 ejes giran alrededor del eje y. Y el solucionador básicamente funciona.

Esto es lo que he hecho hasta ahora:

1. Calculo el factor de manipulabilidad con mi matriz jacobiana (solo parte traslacional, ya que solo se rastrea la posición aquí. En realidad, también probé con una matriz jacobiana combinada, así que no solo la parte traslacional sino también la parte rotacional. Pero el movimiento brusco estaba allí de todas formas):

   

2. Entonces el factor de amortiguación es:

   

3. El factor de amortiguamiento se integra luego en el cálculo pseudo inverso:

Como puede ver, este es solo un solucionador cinemático pseudo inverso clásico con un método de mínimo cuadrado amortiguado. El factor de manipulabilidad según el segundo movimiento (problema) es: La manipulabilidad cae al comienzo del video. ¿Pero por qué? Hasta donde sé, este factor de manipulabilidad indica la dependencia lineal de los ejes. Para mí, los ejes no parecen ser linealmente dependientes en la parte inicial.

Este movimiento brusco me vuelve loco. Como puede ver en la primera animación, el solucionador parece funcionar correctamente. ¿Que me estoy perdiendo aqui?

Como otros ya han señalado, debe haber un problema con su implementación del algoritmo IK ya que no habrá ningún comportamiento singular en las descripciones que proporcionó.

Ahora tiene dos alternativas: comenzar a depurar el código o puede aprovechar el hecho de que el problema se puede dividir fácilmente en dos subproblemas para los que puede emplear fácilmente la mayor parte del código escrito hasta ahora.

Dado el objetivo 3D deseado $\left( x_d,y_d,z_d \right)$, es sencillo observar que el valor deseado de la primera articulación es: $\theta_{1d}=\arctan\left( \frac{y_d}{x_d}\right)$.

La ley de control para conducir la primera articulación del manipulador a $\theta_{1d}$ puede ser tan simple como:

$\dot{\theta_1}=K_1 \cdot \left( \theta_{1d}-\theta_{1} \right).$

Entonces, deja $R \in SO(3)$ ser la matriz que explica la rotación de $\theta_{1d}$ alrededor de $z$ eje:

$R = \left( \begin{array}{cccc} \cos{\theta_{1d}} & -\sin{\theta_{1d}} & 0 \\ \sin{\theta_{1d}} & \cos{\theta_{1d}} & 0 \\ 0 & 0 & 1 \\ \end{array} \right).$

Mediante $R$, obtendrás el nuevo objetivo $\left( x_d,0,z_d \right)_1=R^T \cdot \left( x_d,y_d,z_d \right)^T$ eso establecerá un nuevo problema plano IK 2D en el $xz$ avión.

En este punto, puedes resolver $\left( x_d,z_d \right)_1$ usando el jacobiano del manipulador 4-DOF restante.

Creo que ha introducido una singularidad algorítmica en el primer eje de la muñeca. Me parece que, cuando alcanza los 90 grados "hacia abajo", que en lugar de llegar a 91, intenta pasar de cero a -269 grados.

Por supuesto, esto es especulativo sin ver el código.