Симулятор манипулятора: кинематика и планирование ROS

Управление

Симулятор предоставляет ползунки суставов, перетаскиваемую целевую точку инструмента, отображение систем координат и элементы управления размерами звеньев; основание расположено по центру, поэтому виден весь достижимый рабочий объём на 360°. Прямой режим вычисляет позу инструмента по заданным углам суставов. Обратный режим отслеживает перетаскиваемую целевую точку, находя углы суставов в реальном времени и сообщая о недостижимых или неоднозначных целях вместо самопроизвольной подстановки результата. Третий режим захвата и перемещения (pick-and-place) позволяет задать точку захвата и место назначения, а затем проводит манипулятор через всю задачу: он подходит к объекту, захватывает его, переносит и отпускает в зоне размещения — типовая задача, для которой создан каждый настоящий манипулятор.

Теория

Каждый сустав добавляет преобразование между системами координат. Перемножение этих преобразований от основания к инструменту даёт позу прямой кинематики. Обратная кинематика работает назад и может не иметь решения, иметь одно или несколько решений.

Близко к сингулярности небольшое движение инструмента может потребовать очень большого движения суставов. Планировщик также должен соблюдать ограничения на положение, скорость, ускорение, момент и столкновения. Одной геометрической достижимости недостаточно для безопасной траектории.

Алгоритм

1. Определите длины звеньев, оси суставов, начальные преобразования и пределы суставов.
2. Вычислите прямую кинематику для текущего состояния суставов.
3. Сравните текущую позу инструмента с запрошенной целью.
4. Используйте аналитический или численный решатель, чтобы уменьшить ошибку позы в пределах ограничений.
5. Отбрасывайте кандидатов со столкновениями или сингулярностями и выбирайте непрерывное решение.
6. Параметризуйте траекторию по времени и отправляйте синхронизированные уставки регуляторам суставов.

Исходный код

Плоская рука с двумя звеньями имеет компактную модель прямой кинематики:

from math import cos, sin

def forward_kinematics(theta_1, theta_2, link_1, link_2):
    elbow = (link_1 * cos(theta_1), link_1 * sin(theta_1))
    tool = (
        elbow[0] + link_2 * cos(theta_1 + theta_2),
        elbow[1] + link_2 * sin(theta_1 + theta_2),
    )
    return elbow, tool

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

Допущения и отличия от реального оборудования

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

Схема соединений

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

Управляемые эксперименты

Перетаскивание цели — самый быстрый способ развить интуицию о вещах, которые утомительно доказывать на бумаге и дорого обнаруживать на реальном оборудовании.

1. Определите форму рабочего объёма

Медленно перетаскивайте цель наружу, пока решатель не сообщит, что цель недостижима, затем обойдите всю границу. Теперь перетащите цель внутрь к основанию.

При разных длинах звеньев обнаружатся две границы: внешняя окружность на L1 + L2 и внутренняя на |L1 − L2|. Рабочий объём — кольцо, а не диск, и в его середине есть отверстие, которого рука физически не может достичь.

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

2. Посмотрите, как два решения сливаются

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

Теперь перетащите цель наружу к границе. Позы сближаются, а при полном вытяжении становятся одинаковыми. Это сингулярность, место, где рука теряет способность перемещать инструмент в одном направлении независимо от действий суставов.

3. Почувствуйте цену сингулярности

Медленно и равномерно перетаскивайте цель по прямой, проходящей рядом с полным вытяжением. Следите за углами суставов, а не за рукой.

Инструмент движется с постоянной скоростью, а суставы — нет. Близко к границе небольшое движение инструмента требует огромного движения суставов, поэтому гладкая декартова траектория превращается в резкое движение суставов. На реальном оборудовании это сервопривод, ускоряющийся до предела.

Держите запланированные траектории примерно в пределах 95% максимального вылета, и симптом перестанет быть заметным.

4. Пересеките границу ветви и посмотрите на переключение

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

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

Что следует наблюдать

Переход к реальному оборудованию

Геометрия переносится точно. Больше ничего не переносится, и именно в этом промежутке проходит время первой сборки руки:

Два числа показывают суть. Четыре градуса хода в плече при вылете 190 mm дают более 13 mm люфта на захвате — больше любой ошибки, вносимой математикой. А длина звена, измеренная на 10 mm больше для звена 95 mm, даёт систематическую ошибку 10%, которую не устранит никакая настройка.

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