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

1. Введение

В робототехнике описание положения и ориентации (позы) объекта в пространстве — основа всех последующих расчётов. В этом модуле подробно рассказано, как описывать движение твёрдого тела с помощью матрицы вращения и вектора переноса, а также как объединить их в однородную матрицу преобразования (Homogeneous Transformation Matrix).

2. Основная теория

2.1 Матрица вращения (Rotation Matrix)

Вращение в двумерном или трёхмерном пространстве описывается ортогональной матрицей. Например, поворот на угол $θ$ вокруг оси $Z$ задаётся матрицей $R_{z} (θ)$ :

$R_{z} (θ) = cos θ sin θ 0 - sin θ cos θ 0 001$

2.2 Однородное преобразование (Homogeneous Transformation)

Чтобы объединить вращение ( $R$ , $3 \times 3$ ) и перенос ( $P$ , $3 \times 1$ ) в одну линейную операцию, вводится однородная матрица преобразования $T$ размером $4 \times 4$ :

$T = [R 0 P 1] = r_{11} r_{21} r_{31} 0 r_{12} r_{22} r_{32} 0 r_{13} r_{23} r_{33} 0 p_{x} p_{y} p_{z} 1$

3. Схема процесса (Mermaid)

graph LR
    A["Базовая система координат Frame {A}"] -->|матрица преобразования T_AB| B["Целевая система координат Frame {B}"]
    B -->|координаты точки P в B: P_B| C[Вычисление]
    C -->|P_A = T_AB * P_B| D["Координаты точки P в A: P_A"]
    style C fill:#f9f,stroke:#333,stroke-width:2px

4. Проверка на Python

Код показывает, как с помощью numpy построить однородную матрицу преобразования и перевести точку из локальной системы координат в глобальную.

import numpy as np

def get_rotation_z(theta_deg):
    """Создаёт матрицу поворота вокруг оси Z"""
    theta = np.radians(theta_deg)
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s, 0],
        [s,  c, 0],
        [0,  0, 1]
    ])

def get_homogeneous_matrix(R, P):
    """Объединяет поворот и перенос в однородную матрицу 4x4"""
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = P
    return T

# --- Моделирование сцены ---
# Система координат {B} концевого звена робота относительно базовой системы {A}:
# 1. Поворот вокруг оси Z на 90 градусов
# 2. Перенос на 2 м по оси X и на 1 м по оси Y

# 1. Задаём поворот и перенос
R_AB = get_rotation_z(90)
P_AB = np.array([2, 1, 0])

# 2. Строим матрицу преобразования T_AB
T_AB = get_homogeneous_matrix(R_AB, P_AB)

# 3. Задаём точку P на концевом звене (в системе {B} её координаты [1, 0, 0])
P_B = np.array([1, 0, 0, 1])  # однородные координаты

# 4. Вычисляем положение этой точки в базовой системе {A}
P_A = T_AB @ P_B

print("Матрица преобразования T_AB:\n", T_AB)
print("-" * 30)
print(f"Точка в локальной системе B: {P_B[:3]}")
print(f"Точка в глобальной системе A: {P_A[:3]}")

# Ожидаемый результат:
# После поворота на 90 градусов ось X системы B указывает вдоль оси Y системы A.
# Начало системы B находится в точке (2, 1) системы A.
# Точка (1, 0) в системе B — это шаг на 1 м вдоль оси X системы B,
# что соответствует шагу на 1 м вдоль оси Y системы A.
# Итоговое положение: (2, 1 + 1) = (2, 2).

Keyboard shortcuts