Якобиан и скорости схвата

Как скорость вращения моторов превращается в скорость движения схвата в пространстве.

Якобиан $J$ — матрица частных производных положения схвата по суставным координатам; связывает скорости суставов $\dot q$ и скорость схвата $\dot{\mathbf{p}}$.

Прямая кинематика даёт позицию. Если продифференцировать её по времени, получим связь скоростей:

$$ \dot{\mathbf{p}} = J(q) \, \dot q $$

Якобиан зависит от текущей позы $q$ — в разных конфигурациях одна и та же скорость моторов даёт разную скорость схвата. Для двухзвенника якобиан выводится дифференцированием формул FK по $\theta_1, \theta_2$:

$$ J = \begin{bmatrix} -L_1 s_1 - L_2 s_{12} & -L_2 s_{12} \\\\ L_1 c_1 + L_2 c_{12} & L_2 c_{12} \end{bmatrix} $$

Считаем скорость схвата

Поза $\theta = (30^\circ, 45^\circ)$, скорости суставов $\dot\theta_1 = 0.5$, $\dot\theta_2 = -0.3$ рад/с.

import math

L1, L2 = 1.0, 0.7
a, b = math.radians(30), math.radians(45)
dq1, dq2 = 0.5, -0.3  # рад/с

J = [[-L1*math.sin(a) - L2*math.sin(a+b), -L2*math.sin(a+b)],
     [ L1*math.cos(a) + L2*math.cos(a+b),  L2*math.cos(a+b)]]

vx = J[0][0]*dq1 + J[0][1]*dq2
vy = J[1][0]*dq1 + J[1][1]*dq2
speed = math.hypot(vx, vy)
print("vx =", round(vx, 4), "м/с")
print("vy =", round(vy, 4), "м/с")
print("модуль скорости =", round(speed, 4), "м/с")

Вывод:

vx = -0.3852 м/с
vy = 0.4692 м/с
модуль скорости = 0.6071 м/с

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

Столбцы якобиана — это вклад каждого сустава в скорость схвата. Первый столбец показывает, куда поедет схват при вращении только первого сустава, второй — только второго. Полная скорость — линейная комбинация с весами $\dot q$. Поэтому управление скоростью схвата (например, «веди инструмент по прямой со скоростью 0.1 м/с») сводится к решению $\dot q = J^{-1} \dot{\mathbf{p}}^*$.

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

• Думать, что якобиан постоянный: он пересчитывается в каждой позе.
• Смешивать линейную и угловую части якобиана (у пространственного робота он $6\times n$).
• Обращать якобиан без проверки на вырожденность (см. следующий урок про сингулярности).

Итог

• Якобиан связывает скорости суставов и скорость схвата: $\dot{\mathbf{p}} = J \dot q$.
• $J$ зависит от текущей позы и пересчитывается на каждом шаге.
• Столбцы якобиана — вклад отдельных суставов в скорость схвата.
• Обратный якобиан даёт скорости суставов под заданную скорость схвата.