Пряма та обернена кінематика для роботизованих маніпуляторів

1. Пряма та обернена кінематика: що і навіщо

Пряму кінематику використовують для симуляції та відтворення руху. Обернену кінематику використовують для інтерактивного керування: «перемістити руку робота до цього болта» або «змусити персонажа дотягнутися до дверної ручки». Більшість систем реального часу поєднують обидві — пряму для тіла, обернену для рук і ніг.

2. Пряма кінематика у 2D

2D-маніпулятор із n обертовими суглобами: кожен суглоб i має кут θᵢ та довжину ланки Lᵢ. Положення суглоба k — це накопичена сума векторів ланок:

Це пряме обчислення завжди однозначне й завжди швидке. Для дволанкового маніпулятора: x = L₁·cos θ₁ + L₂·cos(θ₁+θ₂), y = L₁·sin θ₁ + L₂·sin(θ₁+θ₂).

3. Параметри Денавіта–Хартенберга (3D)

У 3D кожен суглоб описується чотирма параметрами DH: довжина ланки a, кут скруту ланки α, зсув ланки d та кут суглоба θ. Перетворення від системи координат i−1 до системи координат i таке:

Останній стовпець T₀ₙ дає положення вихідної ланки; верхня ліва підматриця 3×3 задає її орієнтацію. Для промислового маніпулятора з 6 ступенями свободи (наприклад, KUKA, UR5) це добуток шести матриць 4×4.

4. Аналітична обернена кінематика

Для дволанкового 2D-маніпулятора обернена кінематика має розв’язок у замкненій формі за теоремою косинусів:

Промислові роботи типу 6R (6 обертових суглобів зі специфічною геометрією) також допускають аналітичні розв’язки — до 16 можливих конфігурацій. Ці розв’язки в замкненій формі найшвидші та найнадійніші, коли вони доступні, але існують лише для певних кінематичних архітектур.

5. Обернена кінематика на основі якобіана

Для загальних ланцюгів без аналітичного розв’язку якобіан задає лінійний зв’язок між малими змінами кутів суглобів і малими переміщеннями вихідної ланки:

Це ітерується до збіжності. Методи обчислення J⁺:

• Псевдообернена Мура–Пенроуза: J⁺ = Jᵀ(JJᵀ)⁻¹ — точна, але затратна та нестійка поблизу сингулярностей
• Демпфований метод найменших квадратів (DLS): J⁺ = Jᵀ(JJᵀ + λ²I)⁻¹ — додає коефіцієнт демпфування λ, щоб запобігти «вибуховим» розв’язкам у сингулярностях
• Метод транспонування: δθ = Jᵀ · δx — інверсія не потрібна; повільніша збіжність, але робастний. Підходить для застосунків реального часу.

6. CCD та FABRIK

Циклічний покоординатний спуск (CCD)

Обробляйте кожен суглоб по черзі, від вихідної ланки до основи. Для кожного суглоба повертайте його так, щоб вихідна ланка максимально наближалася до цілі. Повторюйте кілька циклів. Просто, швидко, але може збігатися до неприродних поз.

FABRIK (обернена кінематика з прямим і зворотним проходами)

Евристика, що працює безпосередньо з положеннями суглобів, а не з кутами:

1. Прямий прохід: перемістіть вихідну ланку до цілі. Потім перерозташуйте кожен суглоб так, щоб зберегти довжини ланок, рухаючись назад до основи.
2. Зворотний прохід: зафіксуйте основу в її початковому положенні. Перерозташуйте кожен суглоб уперед, зберігаючи довжини ланок.
3. Повторюйте до збіжності (зазвичай 3–10 ітерацій, часто <1 мс).

function fabrik(joints, target, tolerance) {
  const n = joints.length;
  const linkLengths = [];
  for (let i = 0; i < n - 1; i++)
    linkLengths[i] = distance(joints[i], joints[i + 1]);

  while (distance(joints[n - 1], target) > tolerance) {
    // Прямий прохід: від вихідної ланки до основи
    joints[n - 1] = target;
    for (let i = n - 2; i >= 0; i--) {
      const dir = normalize(sub(joints[i], joints[i + 1]));
      joints[i] = add(joints[i + 1], scale(dir, linkLengths[i]));
    }
    // Зворотний прохід: від основи до вихідної ланки
    joints[0] = rootPos; // фіксуємо основу
    for (let i = 1; i < n; i++) {
      const dir = normalize(sub(joints[i], joints[i - 1]));
      joints[i] = add(joints[i - 1], scale(dir, linkLengths[i - 1]));
    }
  }
}

FABRIK — найпопулярніший метод оберненої кінематики в іграх і процедурній анімації завдяки своїй швидкості, простоті та природному вигляду результатів.

7. Застосування та обмеження

• Промислові роботи: маніпулятори з 6 ступенями свободи (KUKA, ABB, Fanuc) використовують аналітичну обернену кінематику для планування траєкторій у реальному часі в контурах керування на 1 кГц
• Хірургічні роботи: da Vinci використовує обернену кінематику з обмеженнями суглобів, щоб відображати рухи рук хірурга в дрібномасштабні рухи вихідної ланки
• Ігрові персонажі: розміщення стоп на нерівному рельєфі за допомогою оберненої кінематики, тягнення руки до об’єктів, відстеження голови/очей
• VR-аватари: повнотіла обернена кінематика від трекерів голови та рук (3 входи → увесь скелет через обернену кінематику з обмеженнями)

Обернена кінематика в реальному світі завжди включає обмеження суглобів: кутові межі (людський лікоть згинається на 0°–145°, а не на 360°), уникнення зіткнень та обробку сингулярностей. Виробничі розв’язувачі оберненої кінематики (Animation Rigging від Unity, IK Rig від Unreal) поєднують FABRIK із проєкцією обмежень і шарами змішування.