Матриці обертання та кватерніони в роботиці

Група обертань SO(3)

Будь-яку жорстку орієнтацію у 3D-просторі можна представити матрицею обертання 3×3 R, що належить спеціальній ортогональній групі SO(3). Належність до неї визначають дві умови:

Композиція обертань — це просто множення матриць: R_total = R₂ · R₁ спочатку застосовує R₁, потім R₂. Обертання точки — добуток матриці на вектор: p' = R · p. Матриця обертання легко застосовується до векторів і легко компонується, але вона зберігає 9 чисел для 3 ступенів свободи — надлишково, і схильна поступово втрачати ортогональність після багаторазового множення з плаваючою комою (числова похибка накопичується, і R поступово перестає бути справжнім обертанням).

Кути Ейлера та gimbal lock

Кути Ейлера описують будь-яку орієнтацію як три послідовні обертання навколо координатних осей — наприклад крен (X), тангаж (Y), рискання (Z), або аерокосмічна конвенція Z-Y-X. Вони інтуїтивні для людини (три числа в градусах), але страждають на gimbal lock: коли середнє обертання досягає ±90°, перша і третя осі вирівнюються, і один ступінь обертальної свободи втрачається.

Для 6-DOF руки-маніпулятора це має конкретний наслідок: біля пози gimbal lock нескінченно мала зміна цільової орієнтації може вимагати величезного, розривного стрибка одного з кутів Ейлера — рука «перекидається» замість плавного руху. Тому кути Ейлера використовують для людиночитаного відображення в UI (повзунки, показники), але рідко — для внутрішнього стану орієнтації.

Вісь-кут і формула Родріга

Будь-яке 3D-обертання можна описати єдиною одиничною віссю n̂ та кутом θ обертання навколо неї (теорема обертання Ейлера). Формула обертання Родріга перетворює вісь-кут безпосередньо на матрицю обертання, зовсім оминаючи кути Ейлера:

Вісь-кут компактний (4 числа: 3 для осі + 1 для кута, або 3, якщо масштабувати вісь на θ), не має gimbal lock і саме це представлення внутрішньо використовують більшість інтеграторів кутової швидкості — кутова швидкість ω — це, по суті, швидкість вісь-кута (вісь = ω/|ω|, швидкість = |ω|).

Кватерніони

Одиничний кватерніон q = w + xi + yj + zk (з w² + x² + y² + z² = 1) кодує рівно ту саму інформацію, що й вісь-кут, але у формі, яка компонується та інтерполюється значно чистіше:

Кватерніони мають рівно 4 числа на 3 ступені свободи (одне обмеження: одиничну норму) — менш надлишкові за 9-числову матрицю, компактніші за окреме зберігання осі й кута, і головне: без gimbal lock, оскільки немає «середньої осі», яка може вирівнятися з іншими.

Добуток Гамільтона та обертання векторів

Композиція двох обертань — спочатку q₁, потім q₂ — це добуток Гамільтона q₂ * q₁ (множення кватерніонів некомутативне — порядок важливий, точно як і при множенні матриць):

Щоб обернути вектор p кватерніоном q, «затискаємо» його між q і спряженим q⁻¹ (для одиничного кватерніона спряжений q* = (w,−v) дорівнює оберненому):

На практиці цей «сендвіч» ніколи не реалізують буквально — його алгебраїчно розкривають один раз і отримують пряму формулу (див. код нижче) — саме її й виконує кожен ігровий рушій та робототехнічна бібліотека.

Інтерполяція: SLERP проти LERP

Інтерполяція орієнтації — наприклад, плавний рух кінцевого ефектора від орієнтації q_a до q_b по траєкторії — саме там, де кватерніони справді розкриваються. Наївна лінійна інтерполяція (LERP) чотирьох компонент з подальшою нормалізацією дає візуально нерівномірну швидкість; SLERP (сферична лінійна інтерполяція) рухається з постійною кутовою швидкістю вздовж найкоротшої дуги великого кола на 4D одиничній гіперсфері:

Коли Ω дуже мале (майже однакові орієнтації), SLERP чисельно нестабільний (ділення майже на нуль) — виробничий код у такому випадку переходить на нормалізований LERP, оскільки різниця на такому крихітному куті непомітна.

Порівняльна таблиця

Більшість робототехнічних стеків внутрішньо зберігають орієнтацію як кватерніон, конвертують у кути Ейлера лише для відображення, і конвертують у матрицю обертання безпосередньо перед пакетним перетворенням великих масивів точок (наприклад, хмари точок) — бо множення матриці на вектор краще векторизується на GPU/SIMD, ніж повторні добутки Гамільтона.

Реалізація на JavaScript

Клас Quat

class Quat {
  constructor(w = 1, x = 0, y = 0, z = 0) {
    this.w = w; this.x = x; this.y = y; this.z = z;
  }

  // Побудова з осі (одиничний вектор) + кута в радіанах
  static fromAxisAngle(axis, theta) {
    const h = theta * 0.5, s = Math.sin(h);
    return new Quat(Math.cos(h), axis.x * s, axis.y * s, axis.z * s);
  }

  // Добуток Гамільтона: this * other
  mul(o) {
    return new Quat(
      this.w*o.w - this.x*o.x - this.y*o.y - this.z*o.z,
      this.w*o.x + this.x*o.w + this.y*o.z - this.z*o.y,
      this.w*o.y - this.x*o.z + this.y*o.w + this.z*o.x,
      this.w*o.z + this.x*o.y - this.y*o.x + this.z*o.w
    );
  }

  conjugate() { return new Quat(this.w, -this.x, -this.y, -this.z); }

  // Обертання вектора {x,y,z} цим одиничним кватерніоном (розкритий сендвіч-добуток)
  rotateVec(v) {
    const qv = new Quat(0, v.x, v.y, v.z);
    const r = this.mul(qv).mul(this.conjugate());
    return { x: r.x, y: r.y, z: r.z };
  }

  normalize() {
    const n = Math.hypot(this.w, this.x, this.y, this.z);
    return new Quat(this.w/n, this.x/n, this.y/n, this.z/n);
  }
}

SLERP

function slerp(qa, qb, t) {
  let { w: w2, x: x2, y: y2, z: z2 } = qb;
  let cosOmega = qa.w*w2 + qa.x*x2 + qa.y*y2 + qa.z*z2;

  // Обираємо коротший шлях на гіперсфері (виправлення подвійного покриття)
  if (cosOmega < 0) { cosOmega = -cosOmega; w2 = -w2; x2 = -x2; y2 = -y2; z2 = -z2; }

  let s1, s2;
  if (cosOmega > 0.9995) {
    // майже однакові: переходимо на нормалізований LERP, щоб уникнути 0/0
    s1 = 1 - t; s2 = t;
  } else {
    const omega = Math.acos(cosOmega);
    const sinOmega = Math.sin(omega);
    s1 = Math.sin((1 - t) * omega) / sinOmega;
    s2 = Math.sin(t * omega) / sinOmega;
  }
  return new Quat(
    s1*qa.w + s2*w2, s1*qa.x + s2*x2,
    s1*qa.y + s2*y2, s1*qa.z + s2*z2
  ).normalize();
}

Орієнтація у роботизованій руці

У 6-DOF маніпуляторі кожне суглобове перетворення в ланцюзі прямої кінематики несе і зсув, і обертання. У Three.js кожен Object3D вже зберігає своє обертання внутрішньо як кватерніон (object3d.quaternion) — кути Ейлера (object3d.rotation) — це лише представлення-геттер/сеттер над ним, що конвертується туди й назад. Задавати кути суглобів, щоразу записуючи .rotation.x для обертального суглоба, — цілком нормально (обертання навколо однієї осі ніколи не потрапляє у gimbal lock), але світову орієнтацію кінцевого ефектора — накопичений добуток усіх шести обертань суглобів — варто зчитувати й інтерполювати як кватерніон, а не розкладати на кути Ейлера, коли потрібен плавний рух між цільовими позами.