Механика

Механика робототехнических систем

Кинематика и динамика — математическое описание движения роботов и сил, необходимых для этого движения.

Степени свободы (DoF)

Определение

Степень свободы — независимое движение, которое может совершать механизм. Для твёрдого тела в 3D-пространстве максимум 6 DoF:

3 поступательных (x, y, z)
3 вращательных (roll, pitch, yaw)

Формула Грюблера-Кутцбаха

$$M = 3(n-1) - 2j_1 - j_2$$$$M = 6(n-1) - 5j_1 - 4j_2 - 3j_3 - 2j_4 - j_5$$

Где:

$n$ — число звеньев (включая основание)
$j_k$ — число шарниров с $k$ ограничениями

Классификация манипуляторов

Тип	DoF	Характеристика
Недоприводной	< 6	Ограниченная рабочая зона
Полноприводной	= 6	Полная манипуляция в 3D
Избыточный	> 6	Обход препятствий, оптимизация

Типовые конфигурации

Робот	DoF	Структура
SCARA	4	RRP + R
Puma-like	6	RRR RRR
Universal Robot	6	RRR RRR
Kuka iiwa	7	RRR RRR R
Diff-drive	2	Управление, 3 DoF движения

Кинематика

Прямая задача (FK)

Дано: Углы в сочленениях $\mathbf{q} = (q_1, q_2, ..., q_n)$
Найти: Положение и ориентацию рабочего органа $\mathbf{x} = (x, y, z, \phi, \theta, \psi)$

$$\mathbf{x} = f(\mathbf{q})$$

Параметры Денавита-Хартенберга (DH)

Стандартизированный метод описания кинематической цепи.

Параметры:

Параметр	Описание
$a_i$	Длина звена (вдоль $x_i$)
$\alpha_i$	Угол закрутки (вокруг $x_i$)
$d_i$	Смещение (вдоль $z_{i-1}$)
$\theta_i$	Угол поворота (вокруг $z_{i-1}$)

$$T_i = R_z(\theta_i) \cdot T_z(d_i) \cdot T_x(a_i) \cdot R_x(\alpha_i)$$$$T_i = \begin{bmatrix} \cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\ \sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$$$$T_0^n = T_1 \cdot T_2 \cdot ... \cdot T_n$$

Обратная задача (IK)

Дано: Целевое положение $\mathbf{x}_{target}$
Найти: Углы сочленений $\mathbf{q}$

$$\mathbf{q} = f^{-1}(\mathbf{x})$$

Особенности IK:

Может не иметь решений (точка вне рабочей зоны)
Может иметь несколько решений (конфигурации elbow-up/down)
Может иметь бесконечно много решений (избыточные манипуляторы)

Методы решения IK

Метод	Применение
Аналитический	6-DoF с известной структурой
Геометрический	Простые манипуляторы
Численный (Jacobian)	Универсальный
Оптимизационный	Избыточные системы

Якобиан

Связывает скорости в пространстве сочленений и декартовом:

$$\dot{\mathbf{x}} = J(\mathbf{q}) \cdot \dot{\mathbf{q}}$$$$J = \begin{bmatrix} J_v \\ J_\omega \end{bmatrix}$$$$J_v^i = z_{i-1} \times (p_n - p_{i-1}), \quad J_\omega^i = z_{i-1}$$

Сингулярности

Конфигурации, где $\det(J) = 0$:

Потеря манипулируемости в некотором направлении
Требуются бесконечные скорости сочленений

Типы:

Граничные: На границе рабочей зоны
Внутренние: Выравнивание осей (например, вытянутая рука)

$$w = \sqrt{\det(J J^T)}$$

Динамика

Уравнения движения

$$M(\mathbf{q})\ddot{\mathbf{q}} + C(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + G(\mathbf{q}) = \boldsymbol{\tau}$$

Где:

$M(\mathbf{q})$ — матрица инерции
$C(\mathbf{q}, \dot{\mathbf{q}})$ — матрица Кориолиса и центробежных сил
$G(\mathbf{q})$ — вектор гравитационных моментов
$\boldsymbol{\tau}$ — вектор моментов в сочленениях

Прямая динамика

Дано: Моменты $\boldsymbol{\tau}$
Найти: Ускорения $\ddot{\mathbf{q}}$

$$\ddot{\mathbf{q}} = M^{-1}(\boldsymbol{\tau} - C\dot{\mathbf{q}} - G)$$

Применение: Симуляция движения.

Обратная динамика

Дано: Траектория $\mathbf{q}(t), \dot{\mathbf{q}}(t), \ddot{\mathbf{q}}(t)$
Найти: Требуемые моменты $\boldsymbol{\tau}$

$$\boldsymbol{\tau} = M\ddot{\mathbf{q}} + C\dot{\mathbf{q}} + G$$

Применение: Управление по траектории, выбор моторов.

Алгоритм Ньютона-Эйлера

Эффективный рекурсивный алгоритм для обратной динамики:

Прямой проход: Вычисление скоростей и ускорений от базы к концу
Обратный проход: Вычисление сил и моментов от конца к базе

Сложность: $O(n)$ vs $O(n^3)$ для прямого вычисления.

Рабочее пространство

Типы

Тип	Описание
Достижимое	Все точки, до которых может дотянуться
Ловкое (dexterous)	Точки с полной ориентацией
Первичное	Без учёта самопересечений

Визуализация

# Python + matplotlib
import numpy as np
import matplotlib.pyplot as plt

def workspace_2dof(l1, l2, n=100):
    theta1 = np.linspace(0, 2*np.pi, n)
    theta2 = np.linspace(0, 2*np.pi, n)
    T1, T2 = np.meshgrid(theta1, theta2)
    
    X = l1*np.cos(T1) + l2*np.cos(T1+T2)
    Y = l1*np.sin(T1) + l2*np.sin(T1+T2)
    
    plt.scatter(X.flatten(), Y.flatten(), s=0.1)
    plt.axis('equal')
    plt.title('Workspace')

Инструменты

URDF/XACRO

Описание кинематики для ROS 2:

<robot name="arm">
  <link name="base_link"/>
  
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1"/>
  </joint>
  
  <link name="link1">
    <visual>
      <geometry><cylinder radius="0.05" length="0.3"/></geometry>
    </visual>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.01" iyy="0.01" izz="0.001"/>
    </inertial>
  </link>
</robot>

MoveIt 2

from moveit.planning import MoveItPy

moveit = MoveItPy(node_name="moveit_py")
arm = moveit.get_planning_component("arm")

# FK
robot_state = arm.get_start_state()
pose = robot_state.get_pose("gripper")

# IK
arm.set_goal_state(pose_goal=target_pose, pose_link="gripper")
plan = arm.plan()

PyBullet / MuJoCo

Динамическая симуляция:

import pybullet as p

# Загрузка робота
robot_id = p.loadURDF("robot.urdf")

# Обратная динамика
torques = p.calculateInverseDynamics(
    robot_id,
    joint_positions,
    joint_velocities,
    joint_accelerations
)