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

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

Однородное преобразование — матрица, объединяющая поворот и перенос, действующая на расширенные координаты точки $(x, y, 1)$.

Перенос — это сложение, поворот — умножение; смешивать их неудобно. Трюк однородных координат: добавляем точке третью координату-единицу и записываем оба действия одной матрицей $3\times3$ (на плоскости):

$$ T = \begin{bmatrix} \cos\theta & -\sin\theta & t_x \\\\ \sin\theta & \cos\theta & t_y \\\\ 0 & 0 & 1 \end{bmatrix} $$

Тогда точка преобразуется как $\begin{bmatrix} x' \\\\ y' \\\\ 1 \end{bmatrix} = T \begin{bmatrix} x \\\\ y \\\\ 1 \end{bmatrix}$, а цепочка преобразований — просто произведение матриц $T_1 T_2 T_3$.

Зачем это

У манипулятора каждое звено добавляет «поворот в суставе + сдвиг вдоль звена». Перемножив матрицы всех звеньев, мы сразу получаем позу схвата в базовой системе. Это и есть прямая кинематика, записанная одним произведением.

Считаем цепочку

Двухзвенная рука: сустав 1 поворот $30^\circ$, звено $L_1=1.0$; сустав 2 поворот $45^\circ$, звено $L_2=0.7$. Найдём позицию схвата.

import math

def mat_mult(A, B):
    return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)]

def T(theta, dx, dy):
    c, s = math.cos(theta), math.sin(theta)
    return [[c, -s, dx], [s, c, dy], [0, 0, 1]]

L1, L2 = 1.0, 0.7
t1, t2 = math.radians(30), math.radians(45)

# поворот в суставе 1, сдвиг по звену 1, поворот в суставе 2, сдвиг по звену 2
M = T(t1, 0, 0)
M = mat_mult(M, T(0, L1, 0))
M = mat_mult(M, T(t2, 0, 0))
M = mat_mult(M, T(0, L2, 0))

print("схват: x =", round(M[0][2], 4), "y =", round(M[1][2], 4))

Вывод:

схват: x = 1.0472 y = 1.1761

Как работает под капотом

В правом верхнем углу однородной матрицы накапливается итоговый перенос (позиция схвата), а левый верхний блок $2\times2$ — итоговый поворот (ориентация схвата). Перемножение матриц автоматически «прокручивает» поворот каждого следующего сустава через все предыдущие — поэтому второй сдвиг идёт уже под суммарным углом $30^\circ + 45^\circ$.

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

• Перепутать порядок множителей: $T_1 T_2 \neq T_2 T_1$. Цепь читается от базы к схвату слева направо.
• Забыть строку $(0, 0, 1)$ — без неё перенос не «приклеится» к повороту.
• Применять матрицу к точке без единицы в третьей координате.

Итог

• Однородная матрица объединяет поворот и перенос в одну операцию.
• Композиция преобразований цепи — произведение матриц от базы к схвату.
• Правый столбец накапливает позицию, левый блок — ориентацию.
• Это компактная запись прямой кинематики.