Робототехника: кинематика, управление, ROS, обратная связь

Серверный код живёт в мире, где состояние меняется только тогда, когда вы его меняете. Робот живёт в мире, где состояние меняется само: колесо продолжает катиться, звено продолжает падать под собственным весом, батарея продолжает садиться. Программа здесь не «обрабатывает запросы», а непрерывно уменьшает расхождение между тем, что должно быть, и тем, что датчики показывают на самом деле. Отсюда три сдвига мышления, каждый из которых больно бьёт по привычкам разработчика «больших» машин. Первый: время — часть контракта. Регулятор с периодом 1 мс, который иногда отрабатывает за 1,4 мс, не «немного медленнее» — он считает производную по неправильному интервалу и выдаёт неправильный ток. Второй: ошибка интегрируется. Промах в одну сотую радиана на измерении угла превращается в полметра промаха на десяти метрах пути, потому что одометрия — это интеграл, а интеграл не забывает. Третий: отказ материален. Исключение в веб-сервисе даёт пятисотку, необработанное переполнение счётчика в контуре тока даёт запах горелой изоляции. Дальше — весь путь: как описывать положение в пространстве, как считать углы суставов и позу платформы, как замыкать обратную связь так, чтобы это работало на реальном железе с люфтом и шумом, как всё это раскладывается по узлам ROS 2 и по платам, и — отдельно — как это отлаживать, когда printf в контуре управления сам является причиной сбоя.

Предполагается, что вы уже знакомы с прерываниями и детерминизмом, RTOS и датчиками и приводами — здесь всё это собирается в машину.

Робот как иерархия замкнутых контуров

Самая полезная модель робота — не «мозг и тело», а набор вложенных контуров обратной связи с разными частотами и разной ценой опоздания. Внутренний контур быстрый, тупой и жёсткий по времени; внешний — медленный, умный и терпимый к задержкам. Каждый уровень выдаёт уставку следующему вниз и получает от него измерения.

Правило, которое стоит вбить намертво: каждый следующий внутрь контур должен быть в 5–10 раз быстрее внешнего. Иначе внешний контур видит внутренний не как исполнителя, а как ещё одну динамическую систему с фазовым запаздыванием — и вместо управления получается генератор колебаний. Отсюда типовая раскладка частот.

Граница между «жёстким» и «мягким» проходит ровно там, где проходит граница между микроконтроллером и Linux. Это не вопрос вкуса: планировщик Linux, сборщик мусора Python, DDS и сетевой стек не имеют доказуемой верхней границы задержки, а контур тока её требует.

Системы координат: без них не работает вообще ничего

Робот — это набор твёрдых тел, и каждая величина в нём осмысленна только вместе с ответом на вопрос «относительно чего»: точка «в 30 см впереди» — впереди камеры, центра масс или схвата? Девяносто процентов ошибок новичка в робототехнике — это перепутанные системы координат, и их особенность в том, что код при этом работает, просто робот едет не туда. Поза твёрдого тела в 3D — это шесть чисел: три на положение, три на ориентацию. Представление, которое композируется без раздумий, — однородная матрица преобразования 4×4:

$$ {}^{A}T_{B} = \begin{pmatrix} {}^{A}R_{B} & {}^{A}p_{B} \ 0 & 1 \end{pmatrix} $$

где ${}^{A}R_{B}$ — матрица поворота 3×3, ${}^{A}p_{B}$ — вектор сдвига. Читается справа налево: «положение системы B, выраженное в системе A». Ценность в том, что цепочки складываются умножением, а типы «сходятся» как в алгебре: ${}^{A}T_{C} = {}^{A}T_{B} \cdot {}^{B}T_{C}$, и одинаковые индексы посередине сокращаются. Если индексы не сокращаются — вы умножаете не то, и это единственная проверка, которая ловит ошибку до выезда робота в стену. Обращение бесплатно и без общей формулы обращения матриц:

$$ {}^{B}T_{A} = ({}^{A}T_{B})^{-1} = \begin{pmatrix} {}^{A}R_{B}^{\mathsf{T}} & -{}^{A}R_{B}^{\mathsf{T}} {}^{A}p_{B} \ 0 & 1 \end{pmatrix} $$

Матрица поворота ортогональна, поэтому обратная равна транспонированной — это O(1) перестановка индексов вместо O(n³) метода Гаусса. Подробности про ортогональные матрицы и их свойства — в статье Матрицы и линейные отображения.

Углы Эйлера против кватернионов. Углы (roll, pitch, yaw) удобны человеку и катастрофичны машине: у них есть шарнирный замок — при pitch = ±90° две оси совпадают, и одна степень свободы теряется, а численно это выглядит как взрыв значений в окрестности. Плюс существует двенадцать разных соглашений о порядке вращений, и половина ошибок интеграции — это ZYX против XYZ. Кватернион — четыре числа с единичной нормой, никакого замка, дешёвая композиция (16 умножений против 27), корректная интерполяция (slerp). Две ловушки: кватернион надо нормировать после каждого накопления (иначе численный дрейф превращает поворот в поворот с масштабом), и $q$ и $-q$ описывают один и тот же поворот — сравнивать их надо по модулю скалярного произведения, а не покомпонентно. В прошивке храните ориентацию в кватернионах, показывайте оператору в градусах Эйлера.

TF-дерево. В ROS 2 все системы координат образуют не граф, а дерево: у каждой ровно один родитель, циклов нет. Каноническая цепочка по REP-105: map → odom → base_link → сенсоры. Смысл разделения: odom → base_link меняется непрерывно и гладко (это одометрия, она врёт, но никогда не прыгает), а map → odom прыгает при каждой коррекции локализации (она не врёт в среднем, но скачет). Регулятор, которому важна гладкость, слушает odom; планировщик, которому важна абсолютная правда, слушает map.

import numpy as np

def T_inv(M: np.ndarray) -> np.ndarray:
    """Обращение однородной матрицы за O(1): транспонирование вместо решения СЛАУ."""
    R, p, out = M[:3, :3], M[:3, 3], np.eye(4)
    out[:3, :3], out[:3, 3] = R.T, -R.T @ p
    return out

# Камера видит объект. Где он относительно схвата? Индексы обязаны сократиться:
# gripper_T_obj = gripper_T_base · base_T_cam · cam_T_obj
gripper_T_obj = T_inv(base_T_gripper) @ base_T_cam @ cam_T_obj

Отдельная, очень дорогая деталь: у каждого преобразования есть метка времени. Поза схвата «сейчас» и облако точек, снятое 80 мс назад, относятся к разным моментам — если наложить их напрямую, объект окажется смещён на скорость робота, умноженную на задержку. Именно поэтому tf2 хранит буфер преобразований и умеет интерполировать на запрошенный момент, а часы всех узлов обязаны быть синхронизированы; про природу этой проблемы — Время и часы в распределённых системах.

Кинематика манипулятора: прямая, обратная, сингулярности

Прямая задача (FK): даны углы суставов — найти позу схвата. Это просто произведение преобразований вдоль цепочки, по одному на сустав. Классическая параметризация — Денавит—Хартенберг: четыре числа на звено ($a$, $\alpha$, $d$, $\theta$) вместо шести, потому что оси координат выбираются по правилам, а не произвольно. Сложность — O(n) по числу суставов, десятки микросекунд даже на Cortex-M4, задача решается всегда и однозначно. Обратная задача (IK): дана поза схвата — найти углы. Здесь всё ломается. Решений может быть несколько (типовой шестиосевой манипулятор даёт до восьми конфигураций для одной точки), ни одного (точка вне рабочей зоны), или бесконечно много (избыточный робот с семью суставами). Плюс на границе рабочей зоны решение существует, но становится численно неустойчивым.

Для простых механизмов IK решается аналитически — быстро (единицы микросекунд), детерминированно, без итераций, что критично для контура с жёстким дедлайном. Для промышленных шестиосевых роботов со «сферическим запястьем» (три последние оси пересекаются в одной точке) работает разложение Пайпера: задача распадается на позиционную и ориентационную части, каждая решается замкнутой формулой.

import numpy as np

L = np.array([0.14, 0.11])          # длины звеньев, м

def fk(q: np.ndarray) -> np.ndarray:
    """Прямая кинематика плоской цепи. O(n) по времени, O(1) по памяти."""
    a = np.cumsum(q)                # абсолютные углы звеньев в мировой системе
    return np.array([np.sum(L * np.cos(a)), np.sum(L * np.sin(a))])

def ik_2r(x: float, y: float, branch: int = +1) -> np.ndarray:
    """Аналитическая IK для 2R. O(1), две ветви: локоть вверх и локоть вниз."""
    c2 = (x*x + y*y - L[0]**2 - L[1]**2) / (2.0 * L[0] * L[1])
    if abs(c2) > 1.0 + 1e-9:
        raise ValueError("точка вне рабочей зоны")
    c2 = float(np.clip(c2, -1.0, 1.0))          # спасаемся от acos(1.0000000002)
    q2 = branch * np.arccos(c2)
    q1 = np.arctan2(y, x) - np.arctan2(L[1]*np.sin(q2), L[0] + L[1]*np.cos(q2))
    return np.array([q1, q2])

Когда замкнутой формулы нет (избыточные руки, произвольные конфигурации, дополнительные ограничения), IK решается численно — как задача минимизации ошибки схвата методом Ньютона. Ключевой объект здесь — якобиан $J(q)$: матрица частных производных позы схвата по углам суставов, связывающая скорости $\dot{x} = J(q)\,\dot{q}$. Наивное $\dot{q} = J^{-1}\dot{x}$ взрывается около сингулярностей, где $\det J \to 0$: чтобы вести схват с конечной скоростью, суставу нужна бесконечная. Практический ответ — демпфированный метод наименьших квадратов (Levenberg—Marquardt), он же DLS:

$$ \Delta q = J^{\mathsf{T}} \left( J J^{\mathsf{T}} + \lambda^{2} I \right)^{-1} e $$

Параметр $\lambda$ — плата за устойчивость: вдали от сингулярности он почти не мешает, рядом с ней гасит взрыв, превращая «дёрнуться и сломать редуктор» в «немного не дотянуться».

def jacobian(q: np.ndarray) -> np.ndarray:
    """Якобиан плоской цепи: столбец j — вклад сустава j в скорость схвата."""
    a = np.cumsum(q)
    J = np.empty((2, len(q)))
    for j in range(len(q)):
        J[0, j] = -np.sum(L[j:] * np.sin(a[j:]))
        J[1, j] =  np.sum(L[j:] * np.cos(a[j:]))
    return J

def ik_dls(target, q0, lam=0.05, iters=60, tol=1e-6):
    """Численная IK. O(k·(n·m + m³)); для m=2..6 и k≈10 это десятки микросекунд.
    Стартовать ВСЕГДА от текущей позы: тогда решение не перепрыгнет в другую ветвь."""
    q = q0.astype(float).copy()
    for k in range(iters):
        e = target - fk(q)
        if np.linalg.norm(e) < tol:
            return q, k
        J = jacobian(q)
        q += J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(len(e)), e)
    return q, iters                  # не сошлись — это штатный исход, а не исключение

Три производственных правила. Стартовое приближение — текущая поза робота, иначе численный метод найдёт математически верное, но физически недопустимое решение, и рука пойдёт в него через стол. Проверка пределов суставов и самопересечений — после IK, а не вместо неё: решение может быть корректным и при этом требовать вывернуть локоть на 200°. Несходимость — нормальное событие, обрабатывайте как «цель недостижима, остаёмся на месте», а не как ошибку, роняющую узел.

Мобильная платформа: кинематика и одометрия

Колёсный робот описывается ещё проще, и именно с него разумно начинать. Модель дифференциального привода: два независимо приводимых колеса на одной оси, поза — три числа $(x, y, \theta)$, управление — две скорости колёс.

Существенно, что это неголономная система: у неё три степени свободы в конфигурации и только два управления. Робот не умеет ехать боком — чтобы сместиться на 10 см вправо, ему нужен манёвр, а не команда. Это ограничение на скорости, а не на достижимые позы: приехать можно куда угодно, но не по любой траектории. Планировщик обязан об этом знать, иначе он выдаст путь, который платформа физически не отследит. Всенаправленные (омни- и меканум-колёса) платформы голономны, но платят проскальзыванием, шумной одометрией и нагрузкой на подшипники. Одометрия — интегрирование скоростей колёс в позу; код живёт на микроконтроллере, вызывается из задачи с фиксированным периодом и обязан быть аккуратным до мелочей.

#define TICKS_PER_REV   4096.0f     /* 1024 CPR × 4 фронта квадратуры, после редуктора */
#define WHEEL_R         0.0325f     /* радиус колеса, м */
#define WHEEL_BASE      0.1853f     /* колея, м — ИЗМЕРЕННАЯ, а не из чертежа */

typedef struct { float x, y, th; int32_t prev_l, prev_r; } odom_t;

static float wrap_pi(float a)
{
    while (a >  (float)M_PI) a -= 2.0f * (float)M_PI;
    while (a < -(float)M_PI) a += 2.0f * (float)M_PI;
    return a;
}

/* Вызывается строго раз в dt из задачи с xTaskDelayUntil, не из main-цикла. */
void odom_step(odom_t *o, int32_t enc_l, int32_t enc_r, float dt)
{
    /* Разность через беззнаковую арифметику: корректна при переполнении
       16- или 32-битного счётчика таймера в режиме энкодера. */
    int32_t dl = (int32_t)((uint32_t)enc_l - (uint32_t)o->prev_l);
    int32_t dr = (int32_t)((uint32_t)enc_r - (uint32_t)o->prev_r);
    o->prev_l = enc_l;
    o->prev_r = enc_r;

    const float k = 2.0f * (float)M_PI * WHEEL_R / TICKS_PER_REV;
    float sl = k * (float)dl;                 /* путь левого колеса за шаг, м */
    float sr = k * (float)dr;

    float ds  = 0.5f * (sl + sr);             /* линейное перемещение центра */
    float dth = (sr - sl) / WHEEL_BASE;       /* поворот за шаг, рад */

    /* Интегрирование по СРЕДНЕЙ точке дуги: локальная ошибка O(dt³) вместо O(dt²)
       у наивного варианта с cosf(o->th). На дуговых участках разница видна глазом. */
    float th_mid = o->th + 0.5f * dth;
    o->x  += ds * cosf(th_mid);
    o->y  += ds * sinf(th_mid);
    o->th  = wrap_pi(o->th + dth);
}

Одометрия всегда врёт, вопрос только в скорости накопления. Источники ошибки делятся на два класса, и лечатся они по-разному. Систематические — неверный радиус колёс (реальный меняется от износа и накачки), неравные диаметры левого и правого колеса, неверная колея. Они калибруются: классическая процедура UMBmark (Borenstein и Feng, 1996 — https://ieeexplore.ieee.org/document/544770) заставляет робота проехать квадрат 4×4 м по часовой и против часовой стрелки, и из двух облаков конечных точек вычисляются ровно два поправочных коэффициента: отношение диаметров и эффективная колея. Час работы, и точность улучшается в разы. Случайные — проскальзывание, неровности, наезд на кабель — не калибруются никогда и требуют внешней коррекции: IMU, лидар, метки, GNSS. И важная асимметрия: ошибка угла дороже ошибки расстояния. Промах в 1° после десяти метров пути даёт 17 см поперечного смещения, и дальше он растёт линейно с дистанцией. Поэтому первое, что добавляют к энкодерам, — гироскоп: его дрейф по углу (единицы градусов в час у дешёвого MEMS) на порядок меньше ошибки от проскальзывания колёс.

Обратная связь: ПИД, который выживает на реальном моторе

Разомкнутое управление («подать 60% ШИМ — колесо поедет со скоростью 1 м/с») не работает никогда: скорость зависит от напряжения батареи, температуры обмотки, нагрузки, трения в редукторе и уклона пола. Обратная связь превращает «подай напряжение» в «держи скорость». ПИД в непрерывном виде знают все, но на микроконтроллере важен дискретный вид, и в нём прячется вся практика. Ниже — реализация, которую можно ставить в прод: шаг фиксирован, производная берётся по измерению, D фильтруется, интегратор защищён от насыщения, есть канал прямой связи.

typedef struct {
    float kp, ki, kd;
    float dt;            /* фиксированный шаг, с. НЕ измеренный: джиттер испортит D */
    float tau_d;         /* постоянная времени фильтра производной, с */
    float tt;            /* постоянная антивиндапа back-calculation, обычно ≈ ki/kp */
    float i_acc;         /* накопитель интегратора, уже в единицах выхода */
    float meas_prev, d_state;
    float out_min, out_max;
} pid_t;

static inline float clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }

float pid_step(pid_t *p, float setpoint, float meas, float feedforward)
{
    float err  = setpoint - meas;
    float prop = p->kp * err;

    /* D по ИЗМЕРЕНИЮ, а не по ошибке: скачок уставки иначе даёт «удар производной» —
       мгновенный выброс выхода в насыщение и щелчок в механике. */
    float dmeas = (meas - p->meas_prev) / p->dt;
    float alpha = p->dt / (p->tau_d + p->dt);       /* однополюсный фильтр */
    p->d_state += alpha * (dmeas - p->d_state);
    float der   = -p->kd * p->d_state;

    float unsat = feedforward + prop + p->i_acc + der;
    float out   = clampf(unsat, p->out_min, p->out_max);

    /* Антивиндап back-calculation: в насыщении интегратор не растёт, а подтягивается
       назад. Без этого после длинного упора робот несколько секунд едет «по инерции
       интегратора» и не слушается уставки. */
    p->i_acc += (p->ki * err + (out - unsat) / p->tt) * p->dt;
    p->meas_prev = meas;
    return out;
}

Что здесь важнее коэффициентов. Шаг dt — константа из периода таймера, а не разность меток времени: дрожание периода на 10% даёт дрожание D-члена на 10%, а D — самый шумный член. Если период всё-таки плавает, чините планирование, а не формулу. Производная от шумного датчика бесполезна без фильтра: энкодер с разрешением 1024 на оборот на низкой скорости выдаёт 0 или 1 тик за период, и «скорость» скачет вдвое; в таких режимах переходят на измерение периода между фронтами вместо счёта фронтов за интервал. Прямая связь (feedforward) снимает половину работы с ПИД: для мотора хорошая модель — $u_{ff} = k_s \operatorname{sign}(\omega) + k_v \omega + k_a \dot{\omega}$, где $k_s$ компенсирует сухое трение, $k_v$ — противо-ЭДС, $k_a$ — инерцию. Регулятору остаётся возмущение, а не вся динамика; качество слежения улучшается в разы без роста коэффициентов.

Настройка на практике. Забудьте формулы Циглера—Николса как окончательный ответ: они дают агрессивную настройку с перерегулированием около 25%, что для механики с люфтом означает стук. Рабочий рецепт: обнулить I и D, поднимать P до начала автоколебаний, взять 40–60% от этого значения; добавить I ровно столько, чтобы статическая ошибка уходила за требуемое время (I — единственное лекарство от постоянного возмущения вроде уклона); D добавлять последним и только если помогает — на шумных сигналах он чаще вреден, чем полезен. И проверяйте не только ступеньку: важнее реакция на возмущение (толкнуть робота рукой) и на насыщение (упереть в стену на десять секунд и отпустить).

Профиль движения. Подавать регулятору ступенчатую уставку — значит требовать бесконечного ускорения и гарантированно уходить в насыщение. Правильный вход — траектория, ограниченная по скорости и ускорению: трапеция (разгон — плато — торможение) или, если механика жалуется на рывок, S-образный профиль с ограничением производной ускорения. Это дешёвый генератор на MCU, и он снимает большую часть проблем со слежением до того, как они дошли до ПИД.

Состояние робота: слияние датчиков

Ни один датчик не даёт того, что нужно регулятору. Акселерометр знает направление силы тяжести, но не отличает наклон от ускорения и шумит. Гироскоп даёт чистую угловую скорость, но её интеграл уплывает. Энкодер точен, пока колесо не пробуксовало. Лидар даёт абсолютную привязку, но раз в 100 мс и с задержкой. Задача оценки состояния — из нескольких лживых по-разному источников собрать одну оценку, которая лучше каждого.

Комплементарный фильтр — минимальный работающий вариант, десять строк и никакой матричной алгебры. Идея: у гироскопа хороши высокие частоты, у акселерометра — низкие; складываем через фильтры, дополняющие друг друга до единицы:

$$ \theta_k = \alpha \left( \theta_{k-1} + \omega_k \Delta t \right) + (1 - \alpha)\, \theta^{acc}_k, \qquad \alpha = \frac{\tau}{\tau + \Delta t} $$

Здесь $\tau$ — постоянная времени раздела: при $\tau = 0{,}5$ с и $\Delta t = 1$ мс получаем $\alpha \approx 0{,}998$. Это в чистом виде фильтр Калмана с зафиксированными коэффициентами, и на MCU без FPU он часто и есть правильный ответ.

Фильтр Калмана делает то же самое, но подбирает вес динамически, исходя из заявленных ковариаций шума. Два шага на каждой итерации: предсказание по модели движения и коррекция по измерению.

class Kalman1D:
    """Оценка угла и дрейфа гироскопа. Состояние: [угол, смещение нуля гироскопа]."""
    def __init__(self, q_angle=1e-3, q_bias=3e-3, r_meas=3e-2):
        self.x = np.zeros(2)                 # [угол, bias]
        self.P = np.eye(2) * 0.1
        self.Q = np.diag([q_angle, q_bias])  # доверие модели
        self.R = r_meas                      # доверие измерению

    def step(self, gyro_rate, angle_meas, dt):
        # ПРЕДСКАЗАНИЕ: интегрируем гироскоп, вычитая оценку его дрейфа
        F = np.array([[1.0, -dt], [0.0, 1.0]])
        self.x = F @ self.x + np.array([gyro_rate * dt, 0.0])
        self.P = F @ self.P @ F.T + self.Q * dt
        # КОРРЕКЦИЯ по акселерометру: H выбирает только угол
        H = np.array([1.0, 0.0])
        y = angle_meas - H @ self.x                 # невязка
        S = H @ self.P @ H + self.R                 # ковариация невязки
        K = self.P @ H / S                          # усиление Калмана
        self.x = self.x + K * y
        self.P = (np.eye(2) - np.outer(K, H)) @ self.P
        return self.x[0]

Сложность — O(n³) на обращение матрицы невязки, но для $n = 2..6$ это десятки арифметических операций, и фильтр спокойно живёт в задаче на 1 кГц на Cortex-M4 с FPU. Расширенный фильтр (EKF) добавляет линеаризацию нелинейной модели — именно он стоит в robot_localization, стандартном узле ROS для слияния колёсной одометрии, IMU и GNSS. Ключевая мысль про настройку: $Q$ и $R$ — это не «шум датчика из даташита», а ваша ставка на то, кому верить. Завысили $R$ — фильтр игнорирует измерения и уплывает вместе с моделью; занизили — пропускает шум насквозь. Теория за этим — в статьях Вероятность и статистика и Численные методы.

И отдельно про цену фильтрации: любой фильтр вносит фазовое запаздывание, а запаздывание в контуре обратной связи съедает запас устойчивости. Однополюсный фильтр с $\tau = 20$ мс в контуре, работающем на частоте 5 Гц, — это почти 30° потерянного фазового запаса, то есть прямая дорога к автоколебаниям. Практическое правило: полоса фильтра должна быть в 5–10 раз выше полосы контура, а если шум не даёт этого добиться — проблема в измерении (экранирование, дифференциальные линии, разделение земель), и чинить надо там.

ROS 2: где он помогает и где мешает

ROS — это не операционная система, а набор соглашений и библиотек: транспорт сообщений, система типов, сборка, инструменты визуализации и записи. Главная ценность — чужой код, который уже работает: драйверы лидаров, SLAM, планировщики Nav2, планировщик движений MoveIt 2, симулятор. Написать своё быстрее, чем разобраться в ROS, можно ровно один раз — до момента, когда понадобится второй датчик. Различия ROS 2 против ROS 1 принципиальны для встраиваемого мира: исчез центральный master (единая точка отказа), транспорт заменён на DDS с настраиваемым QoS, появились компонуемые узлы в одном процессе (обмен без сериализации), lifecycle-узлы с явными состояниями и поддержка микроконтроллеров.

QoS — то, на чём спотыкаются все. В отличие от ROS 1, подписчик и издатель должны быть совместимы по политике, иначе соединение молча не установится, и вы будете час смотреть на пустой ros2 topic echo. Разумные значения: телеметрия и cmd_vel — BEST_EFFORT с глубиной 1 (свежие данные важнее полноты); карты и параметры — RELIABLE + TRANSIENT_LOCAL (поздно подключившийся узел получит последнее сообщение); для контроля живости — DEADLINE и LIVELINESS, чтобы падение узла обнаруживалось средствами транспорта, а не эмпирически.

from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

class BaseController(Node):
    def __init__(self):
        super().__init__("base_controller")
        # Свежесть важнее доставки: устаревшая команда скорости вреднее потерянной
        qos = QoSProfile(depth=1, history=HistoryPolicy.KEEP_LAST,
                         reliability=ReliabilityPolicy.BEST_EFFORT)
        self.create_subscription(Twist, "cmd_vel", self.on_cmd, qos)
        self.odom_pub = self.create_publisher(Odometry, "odom", 10)
        self.create_timer(0.02, self.tick)          # 50 Гц телеметрии
        self.last_cmd = self.get_clock().now()

    def on_cmd(self, msg: Twist) -> None:
        self.last_cmd = self.get_clock().now()
        # В целых единицах: мм/с и мрад/с — на MCU нет причин принимать float
        self.link.send_setpoint(int(msg.linear.x * 1000), int(msg.angular.z * 1000))

    def tick(self) -> None:
        # Deadman на стороне Linux — вежливость. Настоящий deadman живёт на MCU,
        # потому что этот код может не выполниться вовсе: своп, OOM-killer, паника ядра.
        age_ns = (self.get_clock().now() - self.last_cmd).nanoseconds
        if age_ns > 300_000_000:
            self.link.send_setpoint(0, 0)
        state = self.link.read_state()
        if state is not None:
            self.odom_pub.publish(self.to_odometry(state))

Для железа существует ros2_control — прослойка, снимающая необходимость писать свой контроллер: вы реализуете только доступ к своему железу, а готовые контроллеры (дифференциальный привод, слежение за траекторией суставов) подключаются конфигурацией.

Чего ROS 2 не даёт — реального времени. Ни DDS, ни исполнители, ни Python не гарантируют верхней границы задержки. Мягкое реальное время достижимо: ядро с PREEMPT_RT, политика SCHED_FIFO, блокировка страниц через mlockall, отказ от аллокаций в цикле, C++ вместо Python, привязка к изолированным ядрам через isolcpus. Но это даёт единицы миллисекунд в худшем случае — прекрасно для слежения за траекторией и категорически недостаточно для коммутации BLDC. Отсюда каноническая архитектура: всё, где просрочка ломает железо, — на MCU; всё остальное — в ROS 2. Промежуточный вариант — micro-ROS (https://micro.ros.org/): ROS 2 поверх FreeRTOS или Zephyr прямо на микроконтроллере, через агента-посредника. MCU становится настоящим узлом графа: публикует топики, принимает параметры, участвует в системе типов — при этом его контуры продолжают работать в ISR, ничего не зная про DDS.

Архитектурная сторона вопроса — как резать функциональность робота по узлам и почему это по сути распределённая система со всеми её проблемами — разобрана в научных работах портала: Определение роботизированных систем и их классификация, Теоретические обоснования микросервисной архитектуры для роботизированных систем и Реализация сервисов. Хорошее продолжение темы: то, что внутри контроллера решается приоритетами задач, на уровне робота целиком решается разделением на узлы и контрактами между ними.

Железо: что ставить в робота

Выбор платы определяется не «мощностью», а сочетанием детерминизма и вычислений, и эти две оси почти ортогональны: микроконтроллер за 300 рублей даёт джиттер в единицы микросекунд и не потянет SLAM, а Jetson за сто тысяч перемолотит нейросеть и не сможет надёжно дёрнуть ногой раз в миллисекунду. Поэтому в любом нетривиальном роботе плат минимум две.

Связка уровней — отдельное инженерное решение. UART прост, но не имеет ни адресации, ни арбитража; годится для «один Linux — один MCU» на коротком проводе. CAN и CAN FD — то, что стоит в промышленных и автомобильных роботах: дифференциальная пара переживает помехи от силовых ключей, арбитраж по приоритету встроен в физику шины, а обрыв провода детектируется. Именно так общаются суставы серьёзных манипуляторов. SPI быстр, но требует короткого шлейфа и мастера на одной плате. Ethernet и EtherCAT — когда узлов много и нужна синхронизация с точностью до микросекунд. Детальный разбор — в статье про протоколы.

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

printf в контуре управления — это не «медленный вывод», а изменение изучаемой системы. Блокирующая отправка 60 байт по UART на 115200 бод занимает 5,2 мс: контур на 1 кГц пропустит пять периодов, интегратор накопит мусор, и вы будете отлаживать не исходный баг, а последствия своего инструмента. Классический «гейзенбаг»: с отладочным выводом робот едет ровно, без него — колеблется.

Осциллограф отвечает на вопросы, которые изнутри программы не видны в принципе. Как выглядит ШИМ на самом деле — скважность, мёртвое время между верхним и нижним ключом, звон на фронтах от паразитной индуктивности; отсутствие мёртвого времени означает сквозной ток и мгновенно горячий драйвер. Где стоит момент выборки АЦП относительно ШИМ — ток надо мерить в середине включённого состояния, а не на фронте, иначе вы измеряете помеху коммутации; два канала, ШИМ и триггер АЦП, показывают это за секунду. Сколько на самом деле длится ISR и как дрожит период — поднять GPIO в начале обработчика, опустить в конце, и режим послесвечения покажет распределение времени выполнения, а не среднее. Ловля редких срывов — триггер по длительности импульса («покажи период длиннее 1,2 мс») ловит одно опоздание за час, то самое, из-за которого робот раз в день дёргается.

/* Измерение времени и джиттера контура без единого printf.
   Стоит 2 такта на переключение и не искажает тайминги. */
#define DBG_HI()  (GPIOB->BSRR = (1u << 5))
#define DBG_LO()  (GPIOB->BSRR = (1u << (5 + 16)))

void TIM3_IRQHandler(void)          /* контур скорости, 1 кГц */
{
    DBG_HI();                       /* фронт = вход в обработчик */
    TIM3->SR = ~TIM_SR_UIF;
    float meas = encoder_speed();
    float u = pid_step(&pid_vel, sp_vel, meas, ff_vel(sp_vel));
    pwm_set(u);
    DBG_LO();                       /* ширина импульса = время выполнения */
}

Логический анализатор дополняет осциллограф там, где важна не форма, а логика и время между событиями. Квадратура энкодера: A, B, Z на трёх каналах с декодером — сразу видно и пропуски фронтов на высоких оборотах (счётчик отстаёт от реальности, одометрия молча врёт), и дребезг, и неверный порядок фаз, дающий инверсию направления. Шины: аппаратные декодеры I2C, SPI, CAN и UART показывают адреса, подтверждения и содержимое кадров на два порядка быстрее, чем гадание по коду. Сквозная задержка: канал 1 — приём байта команды по UART, канал 2 — изменение ШИМ; расстояние между ними и есть реальная латентность «команда → действие», включая всё, что вы не учли в расчётах.

SWD/JTAG в робототехнике требует осторожности. Точка останова замораживает процессор, но не мотор: колёса продолжают крутиться, рука продолжает падать, а сторожевой таймер молча перезагрузит контроллер посреди сессии (лечится битами DBGMCU для заморозки периферии в отладке — и обязательным возвратом их обратно в релизной сборке). Поэтому основной инструмент здесь — неинвазивные методы. SEGGER RTT — обмен через кольцевой буфер в ОЗУ, который отладчик читает по SWD параллельно с работой: вывод строки стоит сотни наносекунд вместо миллисекунд UART. ITM/SWO — аппаратная трассировка Cortex-M, запись в порт стоит одну инструкцию, а поток уходит наружу отдельным пином. Живой просмотр переменных (STM32CubeMonitor, SEGGER Ozone, Percepio): отладчик читает ОЗУ по SWD, пока программа выполняется, и рисует уставку, измерение и выход регулятора как осциллограмму — это главный инструмент настройки контура, перерегулирование и колебания видны глазами. Точки наблюдения за данными (watchpoints) — останов не по адресу кода, а по записи в переменную: единственный практичный способ поймать «кто испортил эту переменную».

Когда провод к роботу не подключить, работает кольцевой буфер в ОЗУ: контур пишет в него пачки по 8–16 байт (метка времени, уставка, измерение, выход, флаги), а низкоприоритетная задача сбрасывает во flash или отправляет по радио. На стороне Linux всё это соединяется с записью rosbag2 и разбирается в PlotJuggler (https://plotjuggler.io/) — инструменте, ради которого стоит терпеть ROS: он рисует любые топики на общей оси времени, и рассинхронизация уставки и отклика видна сразу. Единственное требование — общая шкала времени: MCU должен периодически получать отметку от хоста, иначе два графика не наложатся.

Безопасность: то, что нельзя чинить обновлением

У робота есть класс отказов, который не лечится кодом, потому что код к этому моменту уже не выполняется. Разделение обязательное: программные защиты предотвращают, аппаратные спасают.

Минимальный обязательный набор. Аварийный останов рвёт силовую цепь физически — контактором или входом STO (Safe Torque Off) драйвера, а не строчкой pwm_set(0): зависший MCU эту строчку не выполнит. Токовая защита сделана компаратором, чей выход заведён на вход аварийного отключения таймера (у STM32 это BKIN): ШИМ снимается аппаратурой за наносекунды, программа узнаёт постфактум из прерывания. Сторожевой таймер независимый (IWDG на собственном RC-генераторе), и «гладить» его должна задача, проверяющая живость всех остальных, а не таймерное прерывание — иначе он подтверждает лишь то, что таймеры живы, при намертво зависшей логике. Deadman на стороне MCU: не пришла уставка за 200 мс — плавное торможение до нуля; проверка на стороне Linux дополняет, но не заменяет её. Ограничения проверяются перед исполнением: пределы суставов, максимальная скорость, максимальный ток, ошибка слежения — превышение любого переводит в Fault с обесточиванием, а не в «попробуем дотянуть». Нормативная сторона тоже существует и она серьёзная: ISO 10218 для промышленных роботов, ISO/TS 15066 для коллаборативных (там нормированы допустимые силы и давления при контакте с человеком), IEC 61508 для функциональной безопасности в целом. Даже если сертификация не планируется, стоит прочитать их логику: категории останова 0/1/2 и понятие уровня полноты безопасности хорошо вправляют инженерную интуицию.

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

1. Путать системы координат. Классика: применить преобразование из системы камеры к точке в системе базы. Код работает, робот едет мимо. Лечение — дисциплина именования a_T_b и проверка сокращения индексов при каждом умножении.
2. Считать одометрию источником истины. Она интегратор ошибки: без внешней коррекции расхождение растёт неограниченно, и на длинных дистанциях доминирует именно угловая составляющая.
3. Брать dt как разность меток времени в регуляторе. Джиттер периода напрямую попадает в D-член и в интегратор. Период задаёт таймер, а не программа.
4. ПИД без антивиндапа. Первый же упор в препятствие — и робот несколько секунд после освобождения едет «сам по себе», выкатывая накопленный интеграл.
5. Дифференцировать шумный сигнал без фильтра, а потом бороться с автоколебаниями снижением P — проблема не в P. Обратная крайность не лучше: каждый фильтр «на всякий случай» — это фазовое запаздывание, то есть потерянный запас устойчивости.
6. Игнорировать сингулярности и пределы суставов. Численная IK честно вернёт решение, требующее скорости сустава в 40 рад/с; редуктор об этом узнает первым.
7. Ставить контур тока туда, где нет детерминизма — в Linux-процесс, в Python-узел, в цикл рядом со стеком Wi-Fi.
8. Отлаживать регулятор через printf. Инструмент меняет систему сильнее изучаемого эффекта; RTT, ITM, кольцевой буфер, GPIO и осциллограф — всегда.
9. Считать аварийный останов программной функцией. Кнопка обязана обесточивать привод независимо от состояния процессора, а сторожевой таймер — работать от собственного генератора.
10. Пропустить QoS в ROS 2. Несовместимые политики издателя и подписчика дают молчаливое отсутствие соединения без единого сообщения об ошибке.
11. Разрабатывать без симулятора. Gazebo или Isaac позволяют ловить ошибки логики без сломанного редуктора; но не верьте симулятору в вопросах трения, люфта и задержек — именно там он врёт сильнее всего.

Мини-итог

• Робот — иерархия вложенных контуров: чем ниже, тем быстрее и жёстче дедлайн. Каждый следующий внутрь контур в 5–10 раз быстрее внешнего, иначе система колеблется.
• Всё, что связано с положением в пространстве, живёт в системах координат: однородные матрицы композируются умножением, обращаются транспонированием, а в ROS 2 образуют дерево TF с метками времени.
• Прямая кинематика тривиальна и однозначна за O(n); обратная имеет несколько решений, ни одного или бесконечно много, а у границы рабочей зоны вырождается. Аналитика — если можно, демпфированный МНК — если нельзя.
• Одометрия дифференциального привода интегрируется по средней точке дуги; систематические ошибки калибруются процедурой UMBmark, случайные лечатся только внешней коррекцией.
• Работающий ПИД — это фиксированный шаг, производная по измерению с фильтром, антивиндап и прямая связь; профиль движения по скорости и ускорению снимает половину проблем до регулятора.
• Слияние датчиков начинается с комплементарного фильтра и дорастает до EKF; главная настройка — не шум из даташита, а решение, кому вы верите больше.
• ROS 2 даёт экосистему, инструменты и чужой отлаженный код, но не реальное время: жёсткие контуры остаются на микроконтроллере, а micro-ROS делает его полноценным узлом графа.
• Отладка робота — это осциллограф, логический анализатор, SWD с неинвазивным чтением ОЗУ и кольцевой буфер, но не printf в контуре. Безопасность — аппаратная: STO, компаратор на BKIN, независимый сторожевой таймер, deadman на MCU.

Источники

• Craig J. J. «Introduction to Robotics: Mechanics and Control» — стандартный учебник по кинематике, якобианам и управлению манипуляторами; Siciliano B., Khatib O. (ред.) «Springer Handbook of Robotics» — https://link.springer.com/referencework/10.1007/978-3-319-32552-1
• Thrun S., Burgard W., Fox D. «Probabilistic Robotics» — байесовская локализация, фильтры, SLAM: https://mitpress.mit.edu/9780262201629/probabilistic-robotics/
• Lynch K., Park F. «Modern Robotics» — учебник и открытый курс с кодом: https://modernrobotics.northwestern.edu/
• Borenstein J., Feng L. «Measurement and Correction of Systematic Odometry Errors in Mobile Robots» (UMBmark), IEEE T-RA, 1996 — https://ieeexplore.ieee.org/document/544770
• Åström K., Hägglund T. «Advanced PID Control» — антивиндап, настройка, практика: https://www.isa.org/products/advanced-pid-control
• ROS 2, документация: концепции, QoS, tf2 — https://docs.ros.org/en/rolling/ ; REP-103 (единицы и оси) — https://www.ros.org/reps/rep-0103.html ; REP-105 (системы координат) — https://www.ros.org/reps/rep-0105.html
• ros2_control — https://control.ros.org/ ; Nav2 — https://docs.nav2.org/ ; MoveIt 2 — https://moveit.picknik.ai/ ; micro-ROS — https://micro.ros.org/
• robot_localization (EKF/UKF для слияния одометрии, IMU и GNSS) — https://docs.ros.org/en/melodic/api/robot_localization/html/ ; PlotJuggler — https://plotjuggler.io/ ; SEGGER RTT — https://www.segger.com/products/debug-probes/j-link/technology/about-real-time-transfer/
• ST AN4013 и AN5325: таймеры STM32 для управления моторами, вход BKIN, синхронизация АЦП с ШИМ — https://www.st.com/resource/en/application_note/an4013-stm32-crossseries-timer-overview-stmicroelectronics.pdf

Что дальше

Отладка и производство: SWD, логический анализатор, прошивки, OTA — как устроены SWD и ITM изнутри, чем ловить редкие сбои в поле, как собирать и подписывать прошивки, разложить flash под безопасное обновление и выкатывать OTA на парк устройств, не превращая роботов в кирпичи.