Обратная кинематика манипулятора с двумя звеньями

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

Достижимое рабочее пространство

При длинах звеньев L₁ и L₂ рабочий орган может достигать только точек внутри кольца. Внешний радиус равен L₁ + L₂ (манипулятор полностью выпрямлен), а внутренний — |L₁ − L₂| (манипулятор полностью сложен). Всё за кольцом слишком далеко; всё внутри отверстия слишком близко. Никакие углы суставов не достигнут этих точек, поэтому правильный ответ просто «недостижимо».

Два решения: локоть вверх и локоть вниз

Для большинства целей внутри кольца локоть можно согнуть двумя способами, и рабочий орган всё равно окажется в той же точке. Это конфигурации локоть вверх и локоть вниз. Они возникают из-за знака ± в выражении для угла второго сустава по теореме косинусов:

cos(θ₂) = (x² + y² − L₁² − L₂²) / (2·L₁·L₂)
θ₂ = ± acos(cos(θ₂))        # + is one elbow, − is the other
θ₁ = atan2(y, x) − atan2(L₂·sin θ₂, L₁ + L₂·cos θ₂)

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

Сингулярности

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

Когда решения нет?

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

Аналитические и численные решатели

Для двух или трёх звеньев геометрия имеет решение в замкнутой форме (аналитическое), поэтому углы суставов можно вычислить напрямую, точно и быстро. Это делает аналитическую обратную кинематику идеальной для обучения и проверки. Манипуляторы с избыточными степенями свободы или пространственные манипуляторы обычно не имеют замкнутой формы и требуют численных решателей, которые итеративно приближаются к решению: они универсальнее, но медленнее и труднее для анализа, поскольку могут пропускать решения или останавливаться вблизи сингулярностей.

Решатель, написанный безопасно

Приведённые выше уравнения верны и на реальном оборудовании всё равно могут выдавать NaN. Вот та же математика с явной обработкой каждого отказа:

enum IkStatus { IK_OK, IK_TOO_FAR, IK_TOO_CLOSE, IK_JOINT_LIMIT, IK_SINGULAR };

struct IkResult {
  IkStatus status;
  float theta1, theta2;      // radians
};

IkResult solveIk(float x, float y, float L1, float L2, bool elbowUp) {
  IkResult r{};

  const float r2 = x * x + y * y;
  const float reach = sqrtf(r2);

  // 1. Reachability FIRST, before any trigonometry.
  //    A small epsilon keeps a target exactly on the boundary from
  //    producing an acos argument of 1.0000001 through rounding.
  const float eps = 1e-4f;
  if (reach > L1 + L2 - eps)          { r.status = IK_TOO_FAR;   return r; }
  if (reach < fabsf(L1 - L2) + eps)   { r.status = IK_TOO_CLOSE; return r; }

  // 2. Now acos is guaranteed to be in range.
  float c2 = (r2 - L1 * L1 - L2 * L2) / (2.0f * L1 * L2);
  c2 = constrain(c2, -1.0f, 1.0f);              // belt and braces
  const float s2 = sqrtf(1.0f - c2 * c2);

  r.theta2 = elbowUp ? atan2f(s2, c2) : atan2f(-s2, c2);

  // 3. atan2, never atan — this is what gets all four quadrants right.
  r.theta1 = atan2f(y, x) - atan2f(L2 * sinf(r.theta2), L1 + L2 * cosf(r.theta2));

  // 4. Warn near the boundary, where the arm is ill-conditioned.
  if (reach > (L1 + L2) * 0.95f) { r.status = IK_SINGULAR; return r; }

  // 5. Mechanical limits are part of the answer, not an afterthought.
  if (r.theta1 < T1_MIN || r.theta1 > T1_MAX ||
      r.theta2 < T2_MIN || r.theta2 > T2_MAX) { r.status = IK_JOINT_LIMIT; return r; }

  r.status = IK_OK;
  return r;
}

Пять деталей отличают этот вариант от учебного, и каждая соответствует реальному отказу:

Проверка достижимости до тригонометрии. Если цель находится за L1 + L2, правило косинусов даёт аргумент вне диапазона от −1 до 1, и acos возвращает NaN. Затем это NaN попадает в команду сервопривода как бессмысленный угол — поэтому недостижимая цель часто вызывает резкое движение, а не отказ двигаться.

Эпсилон. Цель, вычисленная точно на границе, после округления, обусловленного конечной точностью чисел с плавающей запятой, может дать c2 = 1.0000001. Проверка достижимости с небольшим запасом перехватывает это до acos.

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

Предупреждение о сингулярности вместо отказа. Близко к полному выпрямлению решение всё ещё допустимо, но плохо обусловлено — малое движение рабочего органа требует огромного движения суставов. Флаг позволяет вызывающему коду замедлиться или обойти эту область.

Пределы суставов как статус. У манипулятора, плечо которого не может опуститься ниже −10°, есть цели, геометрически достижимые, но механически недостижимые. Молчаливое ограничение создаёт манипулятор, который уходит куда-то рядом с целью и сообщает об успехе.

Выбор между двумя решениями

Математика даёт обе ветви и не сообщает, какую использовать. Это решение в робототехнике, и у него есть четыре распространённых правила:

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

// Pick the branch closest to where the arm already is.
IkResult up   = solveIk(x, y, L1, L2, true);
IkResult down = solveIk(x, y, L1, L2, false);

if (up.status == IK_OK && down.status == IK_OK) {
  const float costUp   = fabsf(up.theta1   - cur1) + fabsf(up.theta2   - cur2);
  const float costDown = fabsf(down.theta1 - cur1) + fabsf(down.theta2 - cur2);
  return (costUp <= costDown) ? up : down;
}
return (up.status == IK_OK) ? up : down;

Для перемещения по прямой нужна обратная кинематика (IK) на каждом шаге

Интерполяция между двумя позами в пространстве суставов — линейное перемещение каждого сустава от начального угла к конечному — проста и заставляет рабочий орган двигаться по кривой. Для многих задач это нормально. Для всего, что должно перемещаться по прямой, интерполируйте в декартовом пространстве и решайте обратную кинематику на каждом шаге:

void moveStraight(float x0, float y0, float x1, float y1, int steps) {
  for (int i = 1; i <= steps; i++) {
    const float t = (float)i / steps;
    const float x = x0 + (x1 - x0) * t;
    const float y = y0 + (y1 - y0) * t;

    IkResult r = solveClosest(x, y);
    if (r.status != IK_OK) { abortMove(r.status); return; }   // do not skip the point
    commandJoints(r.theta1, r.theta2);
  }
}

При первом выполнении этого кода становятся очевидны две вещи. Угловая скорость суставов не постоянна, даже если скорость рабочего органа постоянна: у края рабочего пространства суставам приходится двигаться гораздо быстрее для того же движения рабочего органа. И прямая может проходить через область, которой манипулятор не может достичь, даже если обе конечные точки допустимы, поэтому проверка статуса внутри цикла важнее проверки только на концах.

От углов суставов к значениям команды сервопривода

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

struct Joint {
  uint8_t channel;
  float   offsetDeg;   // servo angle when the joint is at mathematical zero
  int8_t  sign;        // +1 or -1
  float   minDeg, maxDeg;
};

int servoAngle(const Joint &j, float jointRad) {
  const float deg = jointRad * 57.2957795f;
  const float clamped = constrain(deg, j.minDeg, j.maxDeg);
  return (int)(j.offsetDeg + j.sign * clamped);
}

Эта структура — весь мост между кинематикой и оборудованием, и его явное описание позволяет отличить ошибку решателя от ошибки калибровки. Манипулятор, который движется плавно, но оказывается далеко от цели, почти всегда демонстрирует именно эту проблему, а не математическую ошибку.

Диагностика

Попробуйте сами

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