Геометрическая обратная кинематика

Обратная задача: дана точка на столе — какие углы выставить в суставах, чтобы схват туда попал.

Обратная кинематика (IK) — нахождение суставных координат $q$ по заданному положению (и ориентации) схвата.

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

Вывод формул

Расстояние до цели $r^2 = x^2 + y^2$. По теореме косинусов для второго сустава:

$$ \cos\theta_2 = \frac{x^2 + y^2 - L_1^2 - L_2^2}{2 L_1 L_2} $$

$$ \theta_2 = \operatorname{atan2}\left(\pm\sqrt{1 - \cos^2\theta_2}, \; \cos\theta_2\right) $$

Знак $\pm$ даёт два решения — «локоть вверх» и «локоть вниз». Первый угол:

$$ \theta_1 = \operatorname{atan2}(y, x) - \operatorname{atan2}\big(L_2 \sin\theta_2, \; L_1 + L_2 \cos\theta_2\big) $$

Почему atan2

Функция $\operatorname{atan2}(y, x)$ возвращает угол с учётом четверти (от $-\pi$ до $\pi$), в отличие от обычного $\arctan$, который путает противоположные направления. В IK это критично.

Считаем

Цель $(1.2, 0.5)$, $L_1 = 1.0$, $L_2 = 0.7$. Решим и проверим прямой кинематикой.

import math

def ik(x, y, L1=1.0, L2=0.7, elbow_up=True):
    cos2 = (x*x + y*y - L1*L1 - L2*L2) / (2 * L1 * L2)
    cos2 = max(-1.0, min(1.0, cos2))
    sin2 = math.sqrt(1 - cos2*cos2)
    if not elbow_up:
        sin2 = -sin2
    theta2 = math.atan2(sin2, cos2)
    theta1 = math.atan2(y, x) - math.atan2(L2*math.sin(theta2), L1 + L2*math.cos(theta2))
    return theta1, theta2

t1, t2 = ik(1.2, 0.5)
print("theta1, град =", round(math.degrees(t1), 2))
print("theta2, град =", round(math.degrees(t2), 2))

# проверка прямой кинематикой
xc = math.cos(t1) + 0.7*math.cos(t1+t2)
yc = math.sin(t1) + 0.7*math.sin(t1+t2)
print("проверка FK: x =", round(xc, 4), "y =", round(yc, 4))

Вывод:

theta1, град = -9.58
theta2, град = 81.79
проверка FK: x = 1.2 y = 0.5

Как работает под капотом

Геометрия даёт замкнутую форму: ответ считается напрямую, без итераций — быстро и точно. Но она существует не для всех роботов: для 6R в общем виде вывести формулы вручную почти невозможно, и тогда переходят к численным методам (следующий урок).

Частые ошибки

• Использовать atan вместо atan2 — теряется информация о четверти, углы получаются неверными.
• Не зажимать $\cos\theta_2$ в диапазон $[-1, 1]$: для недостижимой цели аргумент выходит за границы и sqrt падает.
• Забывать про два решения: алгоритм управления должен выбрать локоть вверх/вниз осознанно.

Итог

• IK ищет углы суставов по заданному положению схвата.
• Для двухзвенника есть точное геометрическое решение через теорему косинусов.
• Знак корня даёт два решения: локоть вверх и локоть вниз.
• $\operatorname{atan2}$ обязателен для правильного определения четверти угла.