Преобразования координат — как камера видит мир

У робота есть камера, лидар, ультразвуковой датчик… Каждый из них “видит” мир в своей системе координат. Чтобы робот понял общую картину, нужно уметь переводить координаты из одной системы в другую.

1. Зачем нужны преобразования?

Пример: робот с камерой

         Камера видит мяч в точке (0.5, 0, 0.3)
         в СВОЕЙ системе координат
              │
              ▼
┌──────────────────────┐
│      [Камера]        │  ← Камера на высоте 0.2 м
│         │            │     смотрит вперёд
│    ┌────┴────┐       │
│    │  Робот  │       │  ← Центр робота (0, 0, 0)
│    └─────────┘       │
│         ● мяч        │
└──────────────────────┘

Вопрос: Где мяч относительно ЦЕНТРА РОБОТА?

Решение:

Камера на 0.2 м выше центра робота
Камера видит мяч на 0.5 м вперёд и 0.3 м вверх от себя
Значит, мяч на 0.5 м вперёд и 0.3 + 0.2 = 0.5 м вверх от центра робота

Это и есть преобразование координат!

2. Простые преобразования

Сдвиг (Translation)

Когда системы координат просто смещены друг относительно друга:

Система A:          Система B (сдвинута на 2 вправо):
    Y                    Y
    │                    │
    │                    │
────┼──── X          ────┼──── X
    │                    │
    (0,0)_A              (0,0)_B = (2,0)_A

$$P_A = P_B + T$$

где $T$ — вектор сдвига

// Точка в системе B
float x_B = 1.0;
float y_B = 0.5;

// Сдвиг системы B относительно A
float shift_x = 2.0;
float shift_y = 0.0;

// Точка в системе A
float x_A = x_B + shift_x;  // 1.0 + 2.0 = 3.0
float y_A = y_B + shift_y;  // 0.5 + 0.0 = 0.5

Поворот (Rotation)

Когда системы координат повёрнуты друг относительно друга:

Система A:          Система B (повёрнута на 90°):
    Y                    X_B
    │                    │
    │                    │
────┼──── X          ────┼──── Y_B

$$x_A = x_B \cos(\theta) - y_B \sin(\theta)$$$$y_A = x_B \sin(\theta) + y_B \cos(\theta)$$

float rotate2D(float x_B, float y_B, float angle_rad, float* x_A, float* y_A) {
  *x_A = x_B * cos(angle_rad) - y_B * sin(angle_rad);
  *y_A = x_B * sin(angle_rad) + y_B * cos(angle_rad);
}

// Пример: точка (1, 0) в системе B, B повёрнута на 90° (π/2)
float x_A, y_A;
rotate2D(1.0, 0.0, PI/2, &x_A, &y_A);
// Результат: x_A ≈ 0, y_A ≈ 1

Комбинация: сдвиг + поворот

В реальности нужно и то, и другое:

void transform2D(float x_local, float y_local,    // Точка в локальной системе
                 float robot_x, float robot_y,     // Позиция робота
                 float robot_angle,                // Угол поворота робота
                 float* x_world, float* y_world) { // Результат в мировой системе
  
  // Сначала поворот
  float x_rot = x_local * cos(robot_angle) - y_local * sin(robot_angle);
  float y_rot = x_local * sin(robot_angle) + y_local * cos(robot_angle);
  
  // Потом сдвиг
  *x_world = x_rot + robot_x;
  *y_world = y_rot + robot_y;
}

3. Дерево преобразований (TF Tree)

В сложном роботе много систем координат. Они образуют дерево:

        world (мир)
           │
         odom (одометрия)
           │
       base_link (центр робота)
        /    \
  camera    lidar
     │
  object (то, что видит камера)

Правило: Чтобы перевести координаты из одной системы в другую, нужно пройти по дереву через общего “предка”.

Пример: где объект в мировой системе?

Камера видит объект: object в системе camera
Знаем, где камера на роботе: camera → base_link
Знаем, где робот по одометрии: base_link → odom
Знаем начальную позицию: odom → world

$$P_{world} = T_{odom→world} \cdot T_{base→odom} \cdot T_{camera→base} \cdot P_{camera}$$

4. Практика: робот с камерой

Настройка в коде

// Где камера относительно центра робота
const float CAMERA_X = 0.1;   // 10 см вперёд
const float CAMERA_Y = 0.0;   // По центру
const float CAMERA_Z = 0.15;  // 15 см вверх
const float CAMERA_PITCH = -0.2;  // Наклонена вниз на ~11°

// Робот
float robot_x = 2.0;
float robot_y = 1.0;
float robot_yaw = 0.5;  // ≈ 29°

// Камера видит объект
float obj_in_camera_x = 0.8;  // 80 см вперёд от камеры
float obj_in_camera_y = 0.1;  // 10 см влево
float obj_in_camera_z = 0.0;  // На той же высоте

// Преобразуем в мировые координаты
float obj_world_x, obj_world_y;
// ... применяем цепочку преобразований ...

Упрощённая версия для 2D-робота

struct Transform2D {
  float x, y;     // Сдвиг
  float angle;    // Поворот (радианы)
};

// Камера на роботе
Transform2D camera_on_robot = {0.1, 0.0, 0.0};

// Робот в мире
Transform2D robot_in_world = {2.0, 1.0, 0.5};

// Объект в системе камеры
float obj_cam_x = 0.8;
float obj_cam_y = 0.1;

// Шаг 1: Камера → Робот
float obj_robot_x = obj_cam_x + camera_on_robot.x;
float obj_robot_y = obj_cam_y + camera_on_robot.y;

// Шаг 2: Робот → Мир
float obj_world_x = obj_robot_x * cos(robot_in_world.angle) 
                  - obj_robot_y * sin(robot_in_world.angle)
                  + robot_in_world.x;
float obj_world_y = obj_robot_x * sin(robot_in_world.angle)
                  + obj_robot_y * cos(robot_in_world.angle)
                  + robot_in_world.y;

5. ROS 2 и TF2 (для продвинутых)

В ROS 2 есть готовая система TF2, которая автоматически пересчитывает координаты:

# Python пример (ROS 2)
import rclpy
from tf2_ros import TransformListener, Buffer

# Создаём буфер и слушатель
tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, node)

# Преобразуем точку из camera_link в base_link
point_in_camera = PointStamped()
point_in_camera.header.frame_id = "camera_link"
point_in_camera.point.x = 0.8
point_in_camera.point.y = 0.1

point_in_base = tf_buffer.transform(point_in_camera, "base_link")

Преимущества TF2:

Автоматически строит дерево
Учитывает время (для движущегося робота)
Готовые инструменты визуализации (RViz)

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

Ошибка 1: Неправильный порядок операций

Проблема: Сначала сдвинули, потом повернули (вместо наоборот)
Результат: Координаты совсем неправильные

Решение: Порядок зависит от того, ЧТО вы преобразуете
• Точку из локальной в мировую: сначала поворот, потом сдвиг
• Точку из мировой в локальную: сначала сдвиг (с минусом), потом обратный поворот

Ошибка 2: Перепутали знаки углов

Проблема: Угол должен быть положительным при повороте против часовой,
          но в вашей системе наоборот

Решение: Проверьте соглашение о знаках для вашего датчика/библиотеки

Ошибка 3: Забыли, что камера наклонена

Проблема: Камера смотрит не прямо, а под углом вниз
          Объект "впереди" на самом деле ниже

Решение: Учитывайте pitch камеры при преобразовании

7. Эксперимент: преобразования на бумаге

Что нужно: Два листа бумаги, карандаш

Что делать:

На первом листе нарисуйте систему координат “мира” (X вправо, Y вверх)
Положите второй лист — это “робот” со своей системой координат
Сдвиньте “робота” на (3, 2) и поверните на 45°
Отметьте точку (1, 0) в системе робота
Найдите эту точку в системе мира:
- $x_{мир} = 1 \cdot \cos(45°) - 0 \cdot \sin(45°) + 3 = 0.71 + 3 = 3.71$
- $y_{мир} = 1 \cdot \sin(45°) + 0 \cdot \cos(45°) + 2 = 0.71 + 2 = 2.71$
Проверьте линейкой — совпадает?

Проверь себя

✅ Смогу ли я:

Объяснить, зачем нужны преобразования координат?
Перевести точку из системы камеры в систему робота (с известным сдвигом)?
Нарисовать дерево TF для робота с двумя датчиками?
Найти ошибку, если объект “прыгает” при повороте робота?

Что дальше?

Кинематика — преобразования для манипуляторов
SLAM и навигация — как робот строит карту
Сенсорная фузия — как объединять данные от разных датчиков