Геометрия пространства

Геометрия в робототехнике

Математическое описание положения и ориентации объектов в пространстве — фундамент для кинематики, навигации и управления.

Системы координат

Декартова система (Cartesian)

Положение точки задаётся тремя координатами $(x, y, z)$ относительно трёх взаимно перпендикулярных осей.

Правая система координат (ROS 2, стандарт):

X — вперёд (red)
Y — влево (green)
Z — вверх (blue)

Конвенция REP-103 (ROS):

Система	X	Y	Z
base_link	вперёд	влево	вверх
camera_link	вправо	вниз	вперёд
map/odom	восток	север	вверх

Цилиндрическая система

Положение задаётся $(r, \theta, z)$:

$r$ — расстояние от оси Z
$\theta$ — угол от оси X
$z$ — высота

$$x = r \cos\theta, \quad y = r \sin\theta, \quad z = z$$

Применение: Вращающиеся платформы, цилиндрические манипуляторы (SCARA).

Сферическая система

Положение задаётся $(\rho, \theta, \phi)$:

$\rho$ — расстояние от начала координат
$\theta$ — азимут (угол от оси X в плоскости XY)
$\phi$ — зенит (угол от оси Z)

Применение: Сферические манипуляторы, радары, лидары.

Представление ориентации

Углы Эйлера (Roll-Pitch-Yaw)

Последовательность трёх поворотов вокруг осей:

Угол	Ось	Описание
Roll ($\phi$)	X	Крен
Pitch ($\theta$)	Y	Тангаж
Yaw ($\psi$)	Z	Рыскание

$$R = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi)$$

Gimbal Lock: При $\theta = \pm 90°$ оси Roll и Yaw совпадают, теряется одна степень свободы.

Матрица поворота

Ортогональная матрица $R \in SO(3)$, где $R^T R = I$, $\det(R) = 1$.

Элементарные матрицы:

$$R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}$$$$R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}$$$$R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

Кватернионы

Представление поворота четырьмя числами $q = (w, x, y, z)$, где $\|q\| = 1$.

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

Нет gimbal lock
Компактнее матрицы (4 числа vs 9)
Эффективная интерполяция (SLERP)
Численно устойчивы

$$v' = q \otimes v \otimes q^*$$$$q_{total} = q_2 \otimes q_1$$

Преобразование из углов Эйлера:

import numpy as np
from scipy.spatial.transform import Rotation

r = Rotation.from_euler('xyz', [roll, pitch, yaw])
q = r.as_quat()  # [x, y, z, w] — конвенция SciPy

Однородные преобразования

Матрица 4×4

Комбинирует поворот и перенос в одной матрице:

$$T = \begin{bmatrix} R_{3\times3} & t_{3\times1} \\ 0_{1\times3} & 1 \end{bmatrix}$$$$\begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = T \cdot \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$$

Композиция преобразований

$$T_{A \to C} = T_{B \to C} \cdot T_{A \to B}$$

Внимание: Порядок умножения матриц — справа налево!

Обратное преобразование

$$T^{-1} = \begin{bmatrix} R^T & -R^T t \\ 0 & 1 \end{bmatrix}$$

TF-дерево (ROS 2)

Структура

Направленный ациклический граф (DAG) фреймов:

map
 └── odom
      └── base_link
           ├── base_laser
           ├── base_camera
           └── arm_base
                └── arm_link_1
                     └── arm_link_2
                          └── gripper

Публикация трансформации

import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped

class TFPublisher(Node):
    def __init__(self):
        super().__init__('tf_publisher')
        self.br = TransformBroadcaster(self)
        self.timer = self.create_timer(0.1, self.publish_tf)
    
    def publish_tf(self):
        t = TransformStamped()
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'base_link'
        t.child_frame_id = 'sensor'
        t.transform.translation.x = 0.1
        t.transform.translation.y = 0.0
        t.transform.translation.z = 0.2
        t.transform.rotation.w = 1.0  # Без поворота
        self.br.sendTransform(t)

Получение трансформации

from tf2_ros import Buffer, TransformListener

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, node)

try:
    t = tf_buffer.lookup_transform(
        'map', 'gripper',
        rclpy.time.Time()
    )
except TransformException as ex:
    node.get_logger().warn(f'Could not get transform: {ex}')

Static vs Dynamic TF

Тип	Частота	Пример
Static	Один раз	Датчик на роботе
Dynamic	10-100 Hz	Одометрия, суставы

# Static TF (командная строка)
ros2 run tf2_ros static_transform_publisher \
    0.1 0 0.2 0 0 0 base_link sensor

Практические формулы

Расстояние между точками

$$d = \sqrt{(x_2-x_1)^2 + (y_2-y_1)^2 + (z_2-z_1)^2}$$

Угол между векторами

$$\cos\alpha = \frac{\vec{a} \cdot \vec{b}}{|\vec{a}| \cdot |\vec{b}|}$$

Нормализация кватерниона

$$q_{norm} = \frac{q}{\|q\|} = \frac{(w, x, y, z)}{\sqrt{w^2 + x^2 + y^2 + z^2}}$$

Типичные ошибки

Ошибка	Симптом	Решение
Левая/правая система	Отзеркаливание	Проверить конвенцию
Порядок углов Эйлера	Неправильная ориентация	Уточнить ZYX vs XYZ
Порядок кватерниона	NaN, неверный поворот	Проверить wxyz vs xyzw
Ненормализованный кватернион	Искажения	Нормализовать
Gimbal lock	Скачки ориентации	Использовать кватернионы

Инструменты

Python

from scipy.spatial.transform import Rotation
import numpy as np

# Создание из разных представлений
r = Rotation.from_euler('xyz', [0.1, 0.2, 0.3])
r = Rotation.from_quat([x, y, z, w])
r = Rotation.from_matrix(R)

# Конвертация
euler = r.as_euler('xyz')
quat = r.as_quat()
matrix = r.as_matrix()

ROS 2

# Визуализация TF-дерева
ros2 run tf2_tools view_frames

# Echo трансформации
ros2 run tf2_ros tf2_echo base_link gripper