Проблема сингулярности в обратном кинематическом решателе

Я борюсь с этой проблемой в течение нескольких дней. Я действительно надеюсь, что кто-то может дать мне подсказку, в чем проблема.

Робот состоит из 5 осей. Первая ось вращается вокруг оси z, а остальные 4 оси вращаются вокруг оси y. И решатель в основном работает.

Вот что я сделал до сих пор:

1. Я вычисляю коэффициент управляемости с помощью своей матрицы Якоби (только трансляционная часть, поскольку здесь отслеживается только позиция. На самом деле, я также пытался с комбинированной матрицей Якоби, так что не только трансляционная часть, но и вращательная часть. Но резкое движение было там тем не мение):

   

2. Тогда коэффициент демпфирования составляет:

   

3. Коэффициент демпфирования затем интегрируется в псевдообратный расчет:

Как видите, это просто классический псевдообращенный кинематический решатель с методом демпфирования наименьших квадратов. Коэффициент управляемости в соответствии со вторым (проблемным) движением: Управляемость падает в начале видео. Но почему? Насколько я знаю, этот фактор управляемости указывает на линейную зависимость осей. Мне кажется, что оси не являются линейно зависимыми в начальной части.

Это резкое движение сводит меня с ума. Как видно из первой анимации, решатель, кажется, работает правильно. Что мне здесь не хватает?

Как уже отмечали другие, должна быть проблема с вашей реализацией алгоритма IK, так как в описаниях, которые вы дали, не должно быть никакого особого поведения.

Теперь у вас есть две альтернативы: либо вы начинаете отладку кода, либо можете использовать тот факт, что проблема может быть легко разбита на две подзадачи, для которых вы можете легко использовать большую часть написанного кода.

Учитывая желаемую 3D цель , легко заметить, что желаемое значение первого сустава: .$\left( x_d,y_d,z_d \right)$$\theta_{1d}=\arctan\left( \frac{y_d}{x_d}\right)$

Закон управления для приведения первого соединения манипулятора к может быть простым:$\theta_{1d}$

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

Тогда пусть будет матрицей, которая учитывает вращение вокруг оси :$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).$

Через вы получите новую цель которая создаст новую плоскую задачу IK 2D в плоскости .$R$$\left( x_d,0,z_d \right)_1=R^T \cdot \left( x_d,y_d,z_d \right)^T$$xz$

На этом этапе вы можете решить для , используя якобиан оставшегося 4-DOF манипулятора.$\left( x_d,z_d \right)_1$

Я думаю, что вы ввели алгоритмическую особенность на первой оси запястья. Мне кажется, что, когда он достигает 90 градусов «вниз», вместо того, чтобы идти к 91, он пытается отскочить назад от нуля до -269 градусов.

Конечно, это умозрительно, не видя код.