Хабрахабр

[Перевод] Инверсная кинематика в двухмерном пространстве

Часть 1. Математика

Введение

Мы так привыкли к взаимодействию с окружающим нас миром, что не задумываемся о том, насколько сложно двигаются наши руки и ноги. В академической литературе задача управления манипулятором робота называется инверсной кинематикой. Кинематика обозначает "движения", а понятие "инверсная" связано с тем, что обычно мы не управляем самой рукой. Мы управляем «двигателями», поворачивающими каждую отдельную часть. Инверсная кинематика — это задача определения того, как перемещать эти двигатели, чтобы сдвинуть руку в конкретную точку. И в своём общем виде эта задача чрезвычайно сложна. Чтобы вы понимали, насколько она сложна, то можете вспомнить о таких играх, как QWOP, GIRP или даже Lunar Lander, в которой вы выбираете не куда двигаться, а какие мускулы (или ускорители) приводить в действие.

Вас не должно удивлять то, что на протяжении веков математики и инженеры смогли разработать множество решений. Задача управления подвижными приводами распостраняется даже на область робототехники. Для различных схем (манипуляторов роботов, хвостов, щупалец, крыльев и т.д.) встроенных решений обычно не существует.
В большинстве 3D-редакторов и игровых движков (в том числе и в Unity) есть наборы инструментов, позволяющих выполнять риггинг человекоподобных и звероподобных существ.

Именно поэтому в предыдущей серии статей по процедурным анимациям и инверсной кинематике я предложил очень общее и эффективное решение, способное работать при любой схеме. Но такая мощь имеет свой компромисс: эффективность. Одной из важнейших причин критики моей статьи было то, что способ оказался слишком долгим и затратным при одновременном использовании для сотен персонажей. Поэтому я решил написать новую статью, посвящённую инверсной кинематике манипулятора в двух степенях свободы. Описанная в этом туториале техника чрезвычайно эффективна и может использоваться для десятков (если не сотен!) персонажей одновременно.

Инверсная кинематика

Давайте представим манипулятор робота с двумя сегментами и двумя шарнирами, похожий на показанную ниже схему. На конце манипулятора есть конечное звено, которым мы хотим управлять. Мы не имеем непосредственного контроля над позицией конечного звена и можем только поворачивать шарниры. Задача инверсной кинематики заключается в нахождении наилучшего способа поворота соединений для перемещения конечного звена в нужную позицию.

Решение, предлагаемое в этом туториале, будет работать только для манипуляторов с двумя шарнирами. В академической литературе часто пишут, что такие манипуляторы обладают двумя степенями свободы. И причина этого совершенно понятна из показанной ниже схемы. Манипулятор робота с двумя степенями свободы можно смоделировать в виде треугольника, который является одной из самых хорошо изученных фигур в геометрии.

Давайте начнём с того, что немного формализируем задачу. Два шарнира, $A$ и $B$ (оба выделены чёрным цветом) могут поворачиваться на углы $A$ (синий) и $B$ (зелёный). Это заставит конечное звено переместиться в позицию $C$.

Внутренние углы

Мы можем использовать три точки $A$, $B$ и $C$ для построения треугольника с внутренними углами $\alpha$, $\beta$ и $\gamma$, как в примере ниже.

Хотя все три угла нам неизвестны, мы знаем длину всех рёбер.
Знания трёх сторон треугольника достаточно для нахождения всех трёх углов. Это возможно благодаря теореме косинусов, которая является обобщением теоремы Пифагора для треугольников, которые необязательно являются правильными.

Давайте начнём с $\alpha$, который можно вычислить с помощью теоремы косинусов: Двумя углами, необходимыми для управления манипулятором, являются $\alpha$ и $\beta$.

$$display$$\begin a^2 = b^2 + c^2 - 2 bc \cos{\alpha} \end{equation*}$$display$$

Мы можем преобразовать уравнение, чтобы перенести $\cos\alpha$:

$\begin{split} \cos{\alpha} & =\frac{a^2-b^2-c^2}{-2bc} = \\ & =\boxed{\frac{b^2+c^2-a^2}{2bc}} \end{split}$

Теперь нам нужно применить обратную косинусу функцию $\cos^{-1}$ (также известную как арккосинус), чтобы найти $\alpha$:

$$display$$\begin{equation*} \alpha = \cos^{-1}{\left(\boxed{\frac{b^2+c^2-a^2}{2bc}}\right)} \end{equation*}$$display$$

С помощью такой же процедуры мы можем снова применить теорему косинусов, чтобы найти $\beta$:

$b^2 = a^2 + c^2 - 2ac \cos{\beta}$

$\cos{\beta}=\frac{a^2 + c^2 -b^2}{2ac}$

$$display$$\begin{equation*} \beta=\cos^{-1}{\left(\frac{a^2 + c^2 -b^2}{2ac}\right)} \end{equation*}$$display$$

Вывод теоремы косинусов

Углы шарниров

С помощью теоремы косинусов мы вычислили значения $\alpha$ и $beta$, которые являются внутренними углами треугольника, образованного манипулятором. Однако на самом деле нам требуются углы $A$ (синий) и $B$ (зелёный).

Давайте начнём с вычисления $B$, что будет проще. Из показанного выше рисунка очевидно, что $\beta$ и $B$ в сумме дают $180^{\circ}$ (что равно $ \pi$ радианам). Это значит, что:

$$display$$\begin{equation*} \begin{split} \beta + B = \pi \\ B = \pi - \beta \end{split} \end{equation*}$$display$$

Вычисление $A$ не сильно отличается. Единственное различие здесь в том, что нам нужно учитывать $A'$ (фиолетовый), который является углом отрезка $\overline{AC}$. Его можно вычислить с помощью функции арккотангенса $\tan^{-1}$:

$$display$$\begin{equation*} A' = \tan^{-1}{\left(\frac{C_Y-A_Y}{C_X-A_X}\right)} \end{equation*}$$display$$

Что даёт нам:

$$display$$\begin{equation*} A = \alpha + A' \end{equation*}$$display$$

Знак углов $A$, $A'$ и $B$ в основном произволен и зависит от способа движения каждого шарнира.

Множественные решения?

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

Хотя углы $A$ и $B$ на самом деле различны, вывод по сути остаётся тем же, за небольшими исключениями.

Часть 2. Код

Введение

В предыдущей части мы рассмотрели задачу инверсной кинематики для манипулятора робота с двумя степенями свободы.

Если точка, которой мы хотим достичь — это $C$, то конфигурация становится треугольником, все стороны которого известны. В таком случае обычно известна длина манипуляторов $cc$ и $a$.

Затем мы вывели уравнения для углов $A$ и $B$, управляющих поворотом шарниров манипуляторов:

$$display$$\begin{equation*} A = \underset{\alpha}{\underbrace{\cos^{-1}{\left(\frac{b^2+c^2-a^2}{2bc}\right)}}} + \underset{A'}{\underbrace{\tan^{-1}{\left(\frac{C_Y-A_Y}{C_X-A_X}\right)}}} \end{equation*}$$display$$

$$display$$\begin{equation*} B = \pi - \underset{\beta}{\underbrace{\cos^{-1}{\left(\frac{a^2 + c^2 -b^2}{2ac}\right)}}} \end{equation*}$$display$$

С первого взгляда они могут выглядеть довольно пугающими; с другой стороны, их геометрическая интерпретация достаточно интуитивно понятна из показанного выше рисунка.

Создание манипулятора робота

Первым шагом по реализации этого решения является создание манипулятора робота. Концепция «шарниров» неизвестна движку Unity. Однако имеющуюся в движке систему родительских элементов можно использовать для создания иерархии компонентов, которые будут вести себя в точности как манипулятор робота.

Сделав второй шарнир дочерним элементом первого шарнира, мы заставим их поворачиваться как на первом рисунке. Идея заключается в использовании для каждого шарнира GameObject, чтобы поворот его transform заставлял поворачиваться и прикреплённый к нему манипулятор.

В результате мы получим такую иерархию:

  • Корень
    • Шарнир A
      • Кость A
      • Шарнир B
        • Кость B
        • Рука

Затем мы можем добавить корневому объекту скрипт с названием SimpleIK, который будет выполнять повороты шарниров для достижения нужной точки.

using System.Collections;
using UnityEngine; namespace AlanZucconi.IK
{ public class SimpleIK : MonoBehaviour { [Header("Joints")] public Transform Joint0; public Transform Joint1; public Transform Hand; [Header("Target")] public Transform Target; ... }
}

Выведенные в предыдущей части туториала уравнения требуют знания длины первых двух костей (называемых соответственно $c$ и $a$). Так как длина костей не должна изменяться, их можно вычислить в функции Start. Однако это требует, чтобы манипулятор находился при запуске игры в хорошей конфигурации.

private length0;
private length1; void Start ()
{ length0 = Vector2.Distance(Joint0.position, Joint1.position); length1 = Vector2.Distance(Joint1.position, Hand.position );
}

Повороты шарниров

Прежде чем показывать готовую версию кода, давайте начнём с упрощённой. Если мы перенесём уравнения (1) и (2) непосредственно в код, то в результате получим что-то подобное:

void Update ()
{ // Расстояние от Joint0 до Target float length2 = Vector2.Distance(Joint0.position, Target.position); // Внутренний угол альфа float cosAngle0 = ((length2 * length2) + (length0 * length0) - (length1 * length1)) / (2 * length2 * length0); float angle0 = Mathf.Acos(cosAngle0) * Mathf.Rad2Deg; // Внутренний угол бета float cosAngle1 = ((length1 * length1) + (length0 * length0) - (length2 * length2)) / (2 * length1 * length0); float angle1 = Mathf.Acos(cosAngle1) * Mathf.Rad2Deg; // Угол между Joint0 и Target Vector2 diff = Target.position - Joint0.position; float atan = Mathf.Atan2(diff.y, diff.x) * Mathf.Rad2Deg; // Вот как они получаются в системе отсчёта Unity float jointAngle0 = atan - angle0; // Угол A float jointAngle1 = 180f - angle1; // Угол B ...
}

Математические функции $cos^{-1}$ и $tan^{-1}$ называются в Unity Mathf.Acos и Mathf.Atan2. Кроме того, окончательные углы преобразуются в градусы с помощью Mathf.Rad2Deg, потому что компонент Transform должен получать углы, а не радианы.

Нацеливаемся на недостижимые цели

Хотя показанный выше код кажется рабочим, существует условие, при котором он терпит неудачу. Что произойдёт, если цель недостижима? В текущей реализации это не учитывается, что приводит к нежелательным поведениям.

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

Показанный ниже код распознаёт недостижимые цели, проверяя больше ли расстояние от корня до неё общей длины манипулятора.

void Update ()
{ float jointAngle0; float jointAngle1; float length2 = Vector2.Distance(Joint0.position, Target.position); // Угол между Joint0 и Target Vector2 diff = Target.position - Joint0.position; float atan = Mathf.Atan2(diff.y, diff.x) * Mathf.Rad2Deg; // Достижима ли цель? // Если нет, то мы растягиваемся как можно дальше if (length0 + length1 < length2) { jointAngle0 = atan; jointAngle1 = 0f; } else { float cosAngle0 = ((length2 * length2) + (length0 * length0) - (length1 * length1)) / (2 * length2 * length0); float angle0 = Mathf.Acos(cosAngle0) * Mathf.Rad2Deg; float cosAngle1 = ((length1 * length1) + (length0 * length0) - (length2 * length2)) / (2 * length1 * length0); float angle1 = Mathf.Acos(cosAngle1) * Mathf.Rad2Deg; // Вот как определяются углы в системе отсчёта Unity jointAngle0 = atan - angle0; jointAngle1 = 180f - angle1; } ...
}

Повороты шарниров

Теперь нам осталось научиться поворачивать шарниры. Это выполняется при помощи доступа к свойству localEulerAngles компонента Transform шарниров. К сожалению, мы не можем изменять угол z напрямую, поэтому вектор необходимо копировать, изменять и вставлять значение.

Vector3 Euler0 = Joint0.transform.localEulerAngles;
Euler0.z = jointAngle0;
Joint0.transform.localEulerAngles = Euler0; Vector3 Euler1 = Joint1.transform.localEulerAngles;
Euler1.z = jointAngle1;
Joint1.transform.localEulerAngles = Euler1;

На этом туториал по инверсной кинематике для двухмерных манипуляторов завершается.

Теги
Показать больше

Похожие статьи

Добавить комментарий

Ваш e-mail не будет опубликован. Обязательные поля помечены *

Кнопка «Наверх»
Закрыть