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

Кинематика · 10 мин

Прямая кинематика отвечает на вопрос «где окажется схват при заданных углах». Обратная — наоборот: какие углы нужны, чтобы схват оказался в точке (x, y). Для плоской руки из двух звеньев длиной L1 и L2 задача решается аналитически.

Вывод

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

cos(θ2) = (x² + y² − L1² − L2²) / (2·L1·L2)

Если |cos θ2| > 1, точка недостижима. Первый угол:

θ1 = atan2(y, x) − atan2(L2·sin θ2, L1 + L2·cos θ2)

Знак sin θ2 выбирает конфигурацию: «локоть вверх» или «локоть вниз».

Код на Python

from math import atan2, acos, sin, cos, hypot

def ik_2link(x, y, l1, l2, elbow_up=True):
    c2 = (x*x + y*y - l1*l1 - l2*l2) / (2*l1*l2)
    if abs(c2) > 1:
        raise ValueError("точка вне рабочей зоны")
    t2 = acos(c2) * (-1 if elbow_up else 1)
    t1 = atan2(y, x) - atan2(l2*sin(t2), l1 + l2*cos(t2))
    return t1, t2

Проверка

Подставьте полученные углы в прямую кинематику x = L1·cos θ1 + L2·cos(θ1+θ2), y = L1·sin θ1 + L2·sin(θ1+θ2) — должна получиться исходная точка. Такой тест стоит держать в коде всегда: он ловит ошибки в знаках и единицах (градусы против радиан).