Problema de singularidade no solucionador cinemático inverso

Estou lutando com esse problema há dias. Eu realmente espero que alguém possa me dar uma dica de qual é o problema.

O robô é composto por 5 eixos. O primeiro eixo gira em torno do eixo z e outros 4 eixos giram em torno do eixo y. E o solucionador basicamente funciona.

Aqui está o que eu fiz até agora:

1. Eu calculo o fator de manipulação com minha matriz jacobiana (apenas parte translacional, já que apenas a posição é rastreada aqui. Na verdade, eu também tentei com uma matriz jacobiana combinada, portanto, não apenas a parte translacional, mas também a parte rotacional. Mas o movimento brusco estava lá de qualquer forma):

   

2. Então o fator de amortecimento é:

   

3. O fator de amortecimento é então integrado ao cálculo pseudo inverso:

Como você pode ver, este é apenas um solucionador cinemático pseudo-inverso clássico com o método de mínimos quadrados amortecidos. O fator de manipulação de acordo com o segundo movimento (problema) é: A capacidade de manipulação cai no início do vídeo. Mas por que? Até onde eu sei, esse fator de manipulação indica a dependência linear dos eixos. Para mim, os eixos não parecem ser linearmente dependentes na parte inicial.

Esse movimento brusco me deixa louco. Como você pode ver na primeira animação, o solucionador parece funcionar corretamente. O que estou perdendo aqui?

Como outros já apontaram, deve haver um problema com a implementação do algoritmo IK, pois não deve haver nenhum comportamento singular nas descrições que você forneceu.

Agora você tem duas alternativas: ou você começa a depurar o código ou pode querer explorar o fato de que o problema pode ser facilmente dividido em dois subproblemas para os quais você pode empregar prontamente a maior parte do código escrito até agora.

Dado o destino 3D desejado , é fácil observar que o valor desejado da primeira junta é: .$\left( x_d,y_d,z_d \right)$$\theta_{1d}=\arctan\left( \frac{y_d}{x_d}\right)$

A lei de controle para conduzir a primeira articulação do manipulador para pode ser tão simples quanto:$\theta_{1d}$

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

Então, seja a matriz responsável pela rotação de torno do eixo :$R \in SO(3)$$\theta_{1d}$$z$

$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).$

Através de , você obterá o novo destino que estabelecerá um novo problema planar IK 2D no plano .$R$$\left( x_d,0,z_d \right)_1=R^T \cdot \left( x_d,y_d,z_d \right)^T$$xz$

Nesse ponto, você pode resolver usando o jacobiano do manipulador de 4-DOF restante.$\left( x_d,z_d \right)_1$

Eu acho que você introduziu uma singularidade algorítmica no primeiro eixo do pulso. Parece-me que, quando atinge 90 graus "para baixo", em vez de ir para 91, tenta voltar de zero a -269 graus.

Claro que isso é especulativo sem ver o código.