0.3 Часть 0 · Введение · основы

Кинематика и управление

Политика выдаёт цель: «захват — сюда» или «суставы — в эти углы». Мотор же понимает только момент: с какой силой крутить сустав. Между целью и моментом стоит слой классических методов, и в уроке мы пройдём его по шагам: кинематика переводит позу захвата в углы суставов, регулятор — углы в моменты, импеданс задаёт, как сильно давить при контакте, а планирование на горизонт помогает не заехать в тупик. Без этого слоя непонятно, что выдают нейросетевые политики и кто исполняет их команды. Если тема знакома, урок можно пропустить.

≈ 50 минут3 лаборатории и 6 интерактивов22 миссии с цельюформулы — в блоках «Под капотом»

КИНЕМАТИКАКакие углы ставят захват в цель: подбери их вручную и найди цель, до которой рука не дотянется

СЕТЬМожно ли выучить кинематику: обучи две сети и найди цель, где одна из них промахнётся на 40 см

РЕГУЛЯТОРКакой момент дать мотору: подними чашку к полке без раскачки и провиса

ИМПЕДАНСКак сильно давить: подбери жёсткость руки, чтобы протереть стол, когда камера ошиблась с его высотой

ГОРИЗОНТКак не заехать в тупик: найди, на сколько секунд вперёд должен смотреть робот

Начать урок ↓

1 Кинематика

Углы суставов и поза захвата

Ада тянется к чашке рукой из двух звеньев. Политика говорит: «захват — к чашке». А моторы стоят в суставах и поворачивают звенья. Значит, нужно найти углы суставов, при которых захват окажется у чашки.

Ада — домашний гуманоидный робот, на котором построены лабораторные работы курса. Подробнее о ней — в уроке 0.1.

Почему это трудно? Каждый сустав ведёт захват по своей дуге, и положение захвата зависит от всех углов сразу. Зато по известным углам положение захвата считается легко: векторы звеньев складываются. Это прямая кинематика. На левой сцене попробуй попасть захватом в цель, двигая только углы.

Удобнее решать обратную задачу: по нужной позе захвата сразу находить углы. Это обратная кинематика, и для руки из двух звеньев её решает формула. На правой сцене перетаскивай цель, а рука покажет все решения. Проверь, всегда ли решение одно.

Углы → поза

Плечо θ₁30° Локоть θ₂75°

Поза → углы

Под капотом: формулы двухзвенной руки

Прямая кинематика:

\begin{aligned} x &= L_1\cos\theta_1 + L_2\cos(\theta_1 + \theta_2),\\ y &= L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2). \end{aligned}

Обратная кинематика:

\begin{aligned} \cos\theta_2 &= \frac{x^2 + y^2 - L_1^2 - L_2^2}{2L_1L_2},\\ \theta_2 &= \pm\arccos(\ldots),\\ \theta_1 &= \operatorname{atan2}(y, x)\\ &\quad - \operatorname{atan2}(L_2\sin\theta_2,\ L_1 + L_2\cos\theta_2). \end{aligned}

Знак $\pm$ даёт два решения. Если правая часть первой строки больше единицы по модулю, цель вне досягаемости. На внешней и внутренней границе рабочей зоны решения сливаются в одно. В уроке $L_1 = 0{,}5$ м, $L_2 = 0{,}4$ м. Чтобы границу было легче поймать, цель на правой сцене прилипает к ней в пределах 2 см.

У настоящих рук решений больше: у PUMA со сферическим запястьем их 8, у шестизвенной руки общего вида — до 16, у семизвенной — бесконечно много. Подробно — главы 4–6 книги Lynch и Park «Modern Robotics».

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

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

2 Якобиан

Куда рука двигается легко

Чтобы вести захват по пути, нужно знать, куда он сдвинется от малого поворота каждого сустава. Ответ зависит от позы руки.

Эту связь описывает якобиан: он переводит скорости суставов в скорость захвата. Повернём оба сустава на малый угол во всех сочетаниях и отметим, куда сдвинется захват. Получится эллипс: вдоль длинной оси рука двигается легко, вдоль короткой — с трудом. Если эллипс сплющился в отрезок, в одну сторону захват не сдвинуть никаким поворотом суставов. Это сингулярность.

Перетаскивай захват. Каждое положение ложится точкой на график: насколько круглый эллипс на этом расстоянии от плеча. Найди, где рука двигается легче всего и где она теряет направление.

По горизонтали — расстояние от плеча до захвата, м. По вертикали — округлость эллипса: короткая ось, делённая на длинную.

Под капотом: якобиан, скорости и силы

\begin{gathered} \dot x = J(\theta)\,\dot\theta, \qquad \tau = J^{\mathsf T}(\theta)\, f,\\ \det J = L_1 L_2 \sin\theta_2. \end{gathered}

Для двухзвенной руки якобиан — матрица $2 \times 2$ . Его определитель обнуляется при вытянутой ( $\theta_2 = 0^\circ$ ) и сложенной ( $\theta_2 = 180^\circ$ ) руке. Эллипс на сцене — образ единичной окружности скоростей суставов, его полуоси — сингулярные числа $J$ . Округлость — отношение меньшего из них к большему. Она зависит только от $\theta_2$ , а значит, и от расстояния между плечом и захватом, поэтому точки ложатся на одну кривую. У руки из урока округлость не больше 0,84: максимум при $\theta_2 \approx 135^\circ$ , в 0,36 м от плеча. Совсем круглым эллипс бывает, если второе звено короче первого в $\sqrt{2}$ раз.

Тот же якобиан работает и для сил: чтобы прижать захват к столу с силой $f$ , суставам нужны моменты $J^{\mathsf T} f$ . Он пригодится в лаборатории про импеданс. Сингулярности бывают не только на краю досягаемости: у шестизвенной руки сингулярность запястья лежит внутри рабочей зоны.

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

Для руки из двух звеньев кинематика и якобиан считаются по формулам в несколько строк. У шестизвенной руки решений обратной задачи до 16, у семизвенной — бесконечно много. Можно ли обойтись без формул и выучить кинематику по примерам?

🔬 Лаборатория 1

Нейросеть учит кинематику

Для сложной руки формулы обратной кинематики бывает трудно вывести, а для каждой новой руки их приходится выводить заново.

Тогда можно обучить сеть: на вход — поза захвата, на выход — углы суставов. Примеры для обучения получить легко. Берём случайные углы, по прямой кинематике считаем позу захвата и записываем пару «поза → углы». Таких пар можно набрать сколько угодно.

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

Сеть Авсе позы руки: у цели два решения

Сеть Блокоть в одну сторону: решение одно

карта промахов

Под капотом: данные, сеть и откуда среднее

Данные. 6000 пар. Углы $\theta_1$ берутся равномерно от −180° до 180°, $\theta_2$ — от −175° до 175°. У сети Б остаются только позы с $\theta_2 > 0$ .
Вход и выход. Вход — координаты захвата, делённые на 0,9 м. Выход — $\cos\theta_1, \sin\theta_1, \cos\theta_2, \sin\theta_2$ : так угол не прыгает на границе ±180°.
Сеть. 2 → 48 → 48 → 4 с Leaky ReLU, Adam, 900 шагов по 64 примера, шаг обучения 0,005 и 0,0015 в конце. Обе сети начинают с одинаковых весов.

Почему среднее. У двух решений одной позы $\cos\theta_2$ одинаковы, а $\sin\theta_2$ равны по модулю и противоположны по знаку. Минимум среднеквадратичной ошибки — среднее ответов, то есть $\sin\theta_2 = 0$ . Значит, сеть выдаёт $\theta_2 = 0^\circ$ или $180^\circ$ : рука вытянута или сложена и направлена на цель. Промах — разница между расстоянием до цели и длиной такой руки. Поэтому ошибка обучения сети А не падает ниже 0,8, а у сети Б уходит к 0,01.

Данных у сетей поровну, а промахи разные: в среднем 23,5 см у сети А и 2 см у сети Б. У сети А на одну позу захвата приходится два правильных ответа, и она выдаёт их среднее. Регрессия даёт неверный ответ, когда у одного входа несколько правильных ответов. Это мультимодальность, ей посвящён урок 1.3.

Углы суставов теперь можно получить формулой или сетью. Но как повернуть сустав в нужный угол, если мотор создаёт только момент?

🔬 Лаборатория 2

Регулятор: пружина и демпфер

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

Почему это трудно? Разогнавшаяся рука по инерции проскакивает цель. А рука с чашкой ещё и всё время тянется вниз под весом.

ПД-регулятор решает это так: считает момент для каждого сустава из двух частей. Пропорциональная тянет к цели, как пружина: чем дальше от цели, тем сильнее. Дифференциальная гасит скорость, как демпфер.

Рука висит в вертикальной плоскости и держит чашку, её нужно поднять к полке. Ручки регулятора появляются по одной, от миссии к миссии.

Kp60 Kd0 компенсация гравитации

Под капотом: ПД с компенсацией гравитации

\tau = K_p\,(q^* - q) - K_d\,\dot q + g(q)

$q^*$ — целевые углы, $q$ и $\dot q$ — текущие углы и скорости, $g(q)$ — моменты, которые по модели уравновешивают вес руки. $K_p$ — жёсткость виртуальной пружины в Н·м/рад, $K_d$ — демпфер. С точной компенсацией гравитации ПД-регулятор приходит в цель из любого начального положения (Takegaki и Arimoto, 1981).

Без компенсации в равновесии $K_p\,(q^* - q) = g(q)$ : пружина растянута ровно настолько, чтобы держать вес. Поэтому провис обратно пропорционален $K_p$ : примерно $12{,}6 \cdot 60 / K_p$ см. Интегральная часть ПИД-регулятора убирает провис без модели веса: она копит ошибку и наращивает момент.

Как считаются критерии. Каждый запуск длится 4 с. Провис — наибольшая ошибка захвата за последние 0,5 с, размах — разница наибольшей и наименьшей ошибки за последнюю секунду, время успокоения — момент, после которого ошибка отходит от итоговой меньше чем на 1 см.

В лаборатории звенья 0,5 и 0,4 м, массы 1,2 и 0,9 кг сосредоточены на концах звеньев, шаг симуляции 1 мс. Для сравнения, в настоящих системах: в legged_gym для робота A1 $K_p = 20$ Н·м/рад и $K_d = 0{,}5$ , у ANYmal $K_p = 50$ и $K_d = 0{,}1$ .

Пружина без демпфера раскачивается, демпфер гасит раскачку. Провис под весом жёсткость только уменьшает: 12,6 см при Kp = 60, 3,8 см при Kp = 190. Убирает его компенсация гравитации, и тогда хватает умеренного Kp.

Такой регулятор точно приводит захват в цель, и в свободном движении это то, что нужно. Но цель для захвата часто вычисляют по камере, а камера ошибается. Что будет, если из-за ошибки цель окажется внутри стола?

🔬 Лаборатория 3

Импеданс: как сильно давить

Ада протирает стол. Высоту стола она берёт с камеры, а камера ошиблась на 4 см: цель для захвата оказалась под поверхностью.

В лаборатории 2 мы добивались, чтобы регулятор точно держал цель. Жёсткий позиционный регулятор любой ценой тянет захват в заданную точку. Здесь точка под столом: стол не пускает к ней захват, а регулятор продолжает тянуть.

Выход — импедансный регулятор. Он ведёт себя как пружина заданной жёсткости в пространстве захвата: сила растёт с расстоянием до цели, и только. Жёсткость мы выбираем сами, поэтому заранее знаем, какую силу даст ошибка.

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

Жёсткость400 Н/м Ошибка камеры4 см

Под капотом: импедансный регулятор

F = K\,(x^* - x) - D\,\dot x, \qquad \tau = J^{\mathsf T} F + g(q)

Регулятор изображает пружину жёсткостью $K$ и демпфер $D$ в пространстве захвата, а моменты суставов получает через транспонированный якобиан. В лаборатории $K$ задаёт ползунок (сначала 400 Н/м), $D = 2\sqrt{1{,}5\,K}$ , стол — жёсткая пружина 6000 Н/м с демпфером. Позиционный регулятор — ПД по углам с $K_p = 300$ и компенсацией гравитации. Цель лежит на 2 см ниже стола, чтобы тряпка прижималась, плюс ошибка камеры. Сила прижима в покое — средняя сила за последние 0,5 с трёхсекундного опыта.

Идею сформулировал Невилл Хоган в 1985 году: «Control of position or force alone is inadequate; control of dynamic behavior is also required». Обучаемые политики тоже учатся задавать податливость: VICES (2019) выдаёт параметры регулятора на 20 Гц, а ACP (2025) меняет жёсткость в пространстве и во времени.

При ошибке камеры в 4 см позиционная рука бьёт стол с силой 299 Н, импедансная — 76 Н. Импеданс превращает ошибку восприятия в предсказуемую силу, примерно $K\,\Delta x$ . Жёсткость подбирают под задачу: чем она больше, тем ближе импеданс к позиционному регулятору.

Регуляторы урока тянут захват прямо к цели. Что делать, если прямой путь к цели ведёт в тупик?

3 Планирование наперёд

Скользящий горизонт

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

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

Возьмём пример попроще: два робота едут по полу к цели, а на пути стоит ловушка в форме буквы П. Планировщики у них одинаковые, горизонты разные. Насколько далеко нужно смотреть, чтобы выбраться?

Горизонт А0,6 с Высота ловушки0,64 м Глубина ловушки0,51 м

Под капотом: как устроен этот планировщик

Точка едет со скоростью 0,5 м/с и на каждом шаге (0,1 с) выбирает курс. Курс на горизонте задают 5 опорных значений, между ними — линейная интерполяция. Лучший план ищет метод перекрёстной энтропии: 4 итерации по 64 кандидата, из них 20 — случайные курсы по всему кругу, а 8 лучших задают распределение для следующей итерации. Стоимость плана — расстояние до цели в его конце, плюс среднее расстояние по пути с весом 0,3, плюс штраф 2 за столкновение. Тёплый старт — план прошлого шага, сдвинутый на шаг вперёд. Случайность перебора задаёт генератор с зерном 11, поэтому каждый запуск повторяется в точности.

Когда ловушку меняешь, проверка «путь есть» ищет проход от старта к цели по сетке с шагом 2 см. В Cheetah 3 вместо перебора решают выпуклую задачу квадратичного программирования: модель упрощена так, что оптимум находится за доли миллисекунды.

С горизонтом 0,6 с робот застревает в ловушке, с горизонтом 3 с находит объезд. Самый короткий горизонт, с которым он выбирается, — 2,1 с. Длинный горизонт стоит вычислений, и для любого горизонта найдётся ловушка длиннее него.

В шагающих роботах MPC работает на 20–100 Гц. У MIT Cheetah 3 горизонт — до 0,5 с, а задача решается быстрее чем за миллисекунду. Идея «спланировать пачку действий, исполнить начало и спланировать заново» вернётся в уроке 1.7: так устроен action chunking.

Теперь у нас есть весь классический слой: кинематика, регулятор, импеданс и MPC. Как он стыкуется с нейросетевыми политиками в настоящих роботах?

4 Классика внутри нейросетевых систем

Кто исполняет команды сети

В настоящих роботах сеть и классика обычно стыкуются одинаково. Нейросетевая политика почти никогда не выдаёт ток моторов. Она выдаёт цели — углы суставов или позы захвата, — а исполняет их слой из этого урока: регуляторы, кинематика, MPC. Выбери систему и посмотри, где кончается сеть и начинается классика.

🧩 Задача

Разложи по частоте

Слои различаются и частотой. Разложи карточки по корзинам: перетащи карточку или нажми её, а потом корзину.

5 Проверь себя

Восемь вопросов о типичных заблуждениях

6 Итог

Итоги урока

Прямая кинематика — геометрия. У обратной бывает два решения, одно или ни одного.
Якобиан связывает скорости суставов и захвата, а через $J^{\mathsf T}$ — силы и моменты. В сингулярности рука теряет направление движения.
Сеть, обученная кинематике, ошибается, когда у одного входа несколько правильных ответов: регрессия выдаёт их среднее.
ПД-регулятор — пружина и демпфер. Провис под весом убирает компенсация гравитации, а не бесконечный рост Kp.
Импеданс задаёт, как сильно давить: ошибка восприятия превращается в умеренную силу, а не в удар.
MPC планирует на горизонт и пересчитывает план на каждом шаге.
Нейросетевые политики обычно выдают цели, а исполняет их этот слой. Чем ближе слой к мотору, тем чаще он работает.

Открытые вопросы

Почему при вставке штекера в розетку лучше мягкий импеданс, а при удержании тяжёлой кастрюли — жёсткий?

Зазор между штекером и розеткой — доли миллиметра, а камера ошибается сильнее. Мягкий захват уступит и сам скользнёт в отверстие, жёсткий упрётся и создаст большую силу. Кастрюля тяжёлая: под её весом мягкая пружина заметно провиснет, поэтому нужна большая жёсткость или компенсация веса.

Сеть выдаёт целевые углы 50 раз в секунду, регулятор работает на 1 кГц. Что будет, если убрать регулятор и подавать моменты прямо из сети?

Между вызовами сети проходит 20 мс, и всё это время момент не меняется: рука не успевает гасить толчки и раскачивается. Сеть пришлось бы запускать гораздо чаще и учить саму стабилизировать суставы. Так делают редко: например, в работе Chen и соавторов 2022 года политика для робота A1 выдавала моменты напрямую на 200 Гц.

Гид по статье

N. Hogan, «Impedance Control: An Approach to Manipulation: Part I — Theory», ASME JDSMC, 1985.

Читать внимательноПочему управлять только позицией или только силой недостаточно и что значит «манипулятор должен вести себя как импеданс».
Можно пропуститьПодробные выкладки и доказательства.
Вопрос по ходуОкружение принимает силу и отвечает движением. Почему из этого следует, что робот должен отвечать силой на движение?

Материалы

Modern Robotics: Mechanics, Planning, and ControlK. Lynch, F. Park, 2017 — бесплатная книга и видео; кинематика — главы 4–6, управление — глава 11
Robotic ManipulationКурс Р. Тедрейка в MIT — главы 3 и 8
legged_gymКод Rudin et al.: политика ходьбы выдаёт целевые углы, моменты считает ПД

➡️

Дальше: где робот и куда ехать. Во всех лабораториях урока регулятор знал точное положение руки, а планировщик — точную карту стен. У настоящего робота датчики шумят, а карты незнакомого дома нет. Прежде чем вести робота к цели, нужно оценить, где он, и найти путь. Об этом — урок 0.4 «Оценка состояния и планирование».

Урок пригодился? Скажи спасибо — так автор узнает, что курс читают.