Как это работает: Математика Обратной Кинематики
В робототехнике существует два типа задач кинематики:
- Прямая кинематика (FK): когда мы знаем углы в суставах $\theta_1$ и $\theta_2$ и хотим узнать, где окажется захват $(x, y)$.
- Обратная кинематика (IK): когда мы знаем целевую координату цели $(x, y)$ и нам нужно рассчитать углы суставов, чтобы рука туда дотянулась.
Для решения этой задачи в 2D-пространстве наш симулятор использует тригонометрический метод, основанный на теореме косинусов. Угол второго шарнира $\theta_2$ вычисляется по формуле:
$$\cos(\theta_2) = \frac{x^2 + y^2 - L_1^2 - L_2^2}{2 L_1 L_2}$$
Где $L_1$ и $L_2$ — длины плеча и предплечья, а $x, y$ — координаты цели относительно плечевого сустава. Как только мы находим угол $\theta_2$, угол первого сустава $\theta_1$ вычисляется через разность полярного угла цели и внутреннего угла треугольника:
$$\theta_1 = \arctan2(y, x) - \arctan2(L_2 \sin(\theta_2), L_1 + L_2 \cos(\theta_2))$$
Если точка $(x, y)$ находится дальше, чем сумма длин звеньев ($L_1 + L_2$), то система не имеет решений — цель находится вне зоны досягаемости манипулятора. В этом случае алгоритм автоматически масштабирует вектор цели до максимального радиуса действия руки.