Skip to main content
STEM:Hub

📐 Кинематика — FK и IK

Кинематика отвечает на главные вопросы робототехники:

  • Где окажется рука робота, если я поверну моторы на определённые углы?
  • На какие углы повернуть моторы, чтобы рука попала в нужную точку?

1. Прямая кинематика (Forward Kinematics — FK)

“От моторов к координатам”

[Углы моторов] → [Формулы] → [Координаты X, Y, Z]

Пример: 2D-манипулятор

         (x, y) — конец руки
          ╱
         ╱ L2 (локоть → кисть)
        ╱
       ● θ2 (угол в локте)
      ╱
     ╱ L1 (плечо → локоть)
    ╱
   ● θ1 (угол в плече)
   ║
   ╨ основание

Формулы:

x = L1 × cos(θ1) + L2 × cos(θ1 + θ2)
y = L1 × sin(θ1) + L2 × sin(θ1 + θ2)

Код на Python

import math

# Длины звеньев (см)
L1, L2 = 15, 10

# Углы (градусы → радианы)
theta1 = math.radians(30)  # Плечо
theta2 = math.radians(45)  # Локоть

# Прямая кинематика
x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)

print(f"Кисть в точке: ({x:.1f}, {y:.1f}) см")
# Результат: Кисть в точке: (20.1, 14.7) см

Особенность FK: У задачи всегда один правильный ответ.

2. Обратная кинематика (Inverse Kinematics — IK)

“От координат к моторам”

[Координаты X, Y, Z] → [Сложные формулы] → [Углы моторов]

Почему IK сложнее?

ПроблемаОписание
Несколько решенийЛокоть можно согнуть вверх или вниз
Нет решенияТочка слишком далеко — робот не дотянется
СингулярностьРука “заклинивает” в некоторых позах

Пример: два решения IK

Решение 1: "локоть вверх"     Решение 2: "локоть вниз"

       ●                              ●
      ╱                                ╲
     ╱                                  ╲
    ●          цель ✕                    ●
     ╲        ╱                         ╱
      ╲      ╱                         ╱
       ●────●                   ●────●

Оба положения достигают одной точки!

Код на Python (для 2D)

import math

def inverse_kinematics(x, y, L1, L2):
    """Обратная кинематика для 2D-манипулятора"""
    
    # Проверяем достижимость
    dist = math.sqrt(x**2 + y**2)
    if dist > L1 + L2:
        return None  # Слишком далеко!
    if dist < abs(L1 - L2):
        return None  # Слишком близко!
    
    # Угол локтя (теорема косинусов)
    cos_theta2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    theta2 = math.acos(cos_theta2)  # "Локоть вниз"
    # theta2 = -math.acos(cos_theta2)  # "Локоть вверх"
    
    # Угол плеча
    k1 = L1 + L2 * math.cos(theta2)
    k2 = L2 * math.sin(theta2)
    theta1 = math.atan2(y, x) - math.atan2(k2, k1)
    
    return math.degrees(theta1), math.degrees(theta2)

# Пример
result = inverse_kinematics(x=20, y=10, L1=15, L2=10)
if result:
    print(f"Углы: плечо = {result[0]:.1f}°, локоть = {result[1]:.1f}°")
else:
    print("Точка недостижима!")

3. Сравнение FK и IK

ХарактеристикаFK (прямая)IK (обратная)
ВходУглы моторовКоординаты точки
ВыходКоординатыУглы моторов
СложностьПростаяСложная
РешенийВсегда одно0, 1 или несколько
ПрименениеСимуляцияУправление роботом

4. Когда что использовать?

FK — прямая кинематика

  • Симуляция: “Если поверну мотор на 10°, куда попадёт рука?”
  • Визуализация: Показать текущее положение робота
  • Проверка: Убедиться, что IK рассчитан правильно

IK — обратная кинематика

  • Управление: “Робот, возьми предмет в точке (X, Y)”
  • Траектории: Рассчитать путь руки по линии
  • Слежение: Робот следит камерой за объектом

5. Практика для школьных проектов

Уровень 1: Картонный манипулятор

Материалы: Картон, кнопки (шарниры), транспортир

Задание:

  1. Вырежьте 2 звена: L1 = 15 см, L2 = 10 см
  2. Соедините кнопками
  3. Установите θ1 = 30°, θ2 = 45°
  4. Измерьте, где оказалась “кисть”
  5. Сравните с расчётом по формуле

Уровень 2: Arduino-манипулятор

#include <Servo.h>
#include <math.h>

Servo shoulder, elbow;
const float L1 = 15.0, L2 = 10.0;

void setup() {
  shoulder.attach(9);
  elbow.attach(10);
}

// Обратная кинематика
bool moveToXY(float x, float y) {
  float dist = sqrt(x*x + y*y);
  if (dist > L1 + L2) return false;  // Недостижимо
  
  float cos_theta2 = (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2);
  if (abs(cos_theta2) > 1) return false;
  
  float theta2 = acos(cos_theta2);
  float k1 = L1 + L2 * cos(theta2);
  float k2 = L2 * sin(theta2);
  float theta1 = atan2(y, x) - atan2(k2, k1);
  
  // Преобразуем радианы в градусы для сервоприводов
  int angle1 = (int)(theta1 * 180 / PI) + 90;  // Сдвиг на 90° для серво
  int angle2 = (int)(theta2 * 180 / PI);
  
  shoulder.write(constrain(angle1, 0, 180));
  elbow.write(constrain(angle2, 0, 180));
  
  return true;
}

void loop() {
  moveToXY(20, 10);  // Переместить в точку (20, 10)
  delay(2000);
  
  moveToXY(15, 15);  // Переместить в точку (15, 15)
  delay(2000);
}

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

Ошибка 1: Забыли про радианы

// НЕПРАВИЛЬНО:
float theta = 45;  // Это градусы!
float x = L * cos(theta);  // cos ожидает радианы!

// ПРАВИЛЬНО:
float theta = 45 * PI / 180;  // Перевели в радианы
float x = L * cos(theta);

Ошибка 2: Не проверили достижимость

// НЕПРАВИЛЬНО:
float cos_theta2 = ...;
float theta2 = acos(cos_theta2);  // Может быть NaN!

// ПРАВИЛЬНО:
if (abs(cos_theta2) > 1) {
  Serial.println("Точка недостижима!");
  return false;
}

Ошибка 3: Попали в сингулярность

Проблема: Рука полностью вытянута (θ2 = 0°)
Симптом: IK не может решить задачу или даёт странные углы
Решение: Добавить проверку и ограничить углы

7. Для продвинутых: 3D-кинематика

В 3D добавляются:

  • Третья координата Z
  • Больше углов (обычно 6 для полной свободы)
  • Матрицы преобразования (Denavit-Hartenberg)

Для школьных проектов используйте библиотеки:

  • Python: ikpy, roboticstoolbox
  • ROS 2: MoveIt
  • Симуляторы: CoppeliaSim, Gazebo

Проверь себя

✅ Смогу ли я:

  1. Рассчитать FK для 2D-манипулятора?
  2. Объяснить, почему IK может иметь несколько решений?
  3. Написать код для простой IK на Arduino?
  4. Привести пример, когда у IK нет решения?

Что дальше?