ROS2 Nav2 и системы реального времени для автономной навигации мобильных роботов

Настройка системы реального времени (PREEMPT_RT) и среды разработки для робототехники

Почему робот, который отлично работает в симуляторе, начинает пропускать шаги, терять ориентацию и врезаться в стены при переносе на реальное железо? Ответ почти всегда один: стандартное ядро Linux не гарантирует, что планировщик задач выполнит ваш код точно вовремя. Когда навигационный стек Nav2 отправляет команду скорости, а ядро в этот момент решает заняться дефрагментацией памяти или обслуживанием сетевого стека, робот получает команду с задержкой — и вот уже траектория сбита, costmap устарел, а контроллер пытается компенсировать ошибку, которой можно было избежать. Именно поэтому первый шаг к надёжной автономной навигации — это не установка ROS, а настройка системы реального времени.

Зачем роботу реальное время

Обычный Linux использует планировщик CFS (Completely Fair Scheduler), который оптимизирует пропускную способность: все процессы получают справедливую долю процессорного времени. Но «справедливость» здесь означает «в среднем хорошо», а не «гарантированно в срок». Для веб-сервера это нормально — задержка в 50 миллисекунд никого не убьёт. Для робота, который едет со скоростью 0.5 м/с и должен реагировать на препятствие за 100 мс, задержка в 50 мс — это 2.5 сантиметра неконтролируемого движения. А если рядом стена или человек — это уже проблема безопасности.

PREEMPT_RT — это патч к ядру Linux, который превращает его в мягкую систему реального времени (soft real-time). Он не делает Linux жёстким RTOS вроде VxWorks, но снижает максимальную задержку (worst-case latency) с десятков миллисекунд до единиц — с 30–50 мс до 1–5 мс на типичном оборудовании. Этого достаточно для управления мобильным роботом с циклом управления 20–50 Гц.

Установка PREEMPT_RT на Ubuntu

На Ubuntu 22.04 (Jammy) и 24.04 (Noble) патч PREEMPT_RT доступен через стандартный репозиторий. Установка занимает три команды:

После перезагрузки убедитесь, что загрузилось именно RT-ядро:

Если в репозитории пакета нет (например, на ARM для Raspberry Pi), придётся собирать ядро вручную: скачать исходники ядра, применить патч с kernel.org и сконфигурировать с опцией CONFIG_PREEMPT_RT=y. Это занимает 1–3 часа на современном ПК, но даёт полный контроль над конфигурацией.

Настройка приоритетов и изоляции CPU

Установка RT-ядра — это только половина дела. Без настройки приоритетов процессов и изоляции ядер вы не получите реального выигрыша. Представьте себе скорую помощь с сиреной, которая застряла в общем потоке машин — приоритет есть, но инфраструктура не позволяет его реализовать.

Назначение RT-приоритетов

В Linux RT-процессы получают приоритеты от 1 до 99, где 99 — наивысший. Навигационные ноды Nav2 нужно запускать с повышенным приоритетом:

Флаг -f указывает планировщик SCHED_FIFO — задача выполняется до завершения или пока не будет вытеснена задачей с более высоким приоритетом. Альтернатива SCHED_RR (round-robin) добавляет кванты времени, что полезно, когда несколько задач имеют одинаковый приоритет.

Изоляция ядер CPU

Для критических задач можно выделить отдельные ядра, которые не будут обрабатывать обычные процессы. В файле /etc/default/grub добавьте параметр:

После обновления GRUB (sudo update-grub) ядра 2 и 3 будут доступны только для процессов, явно привязанных к ним:

Это особенно важно на встраиваемых платформах вроде Jetson или Raspberry Pi, где каждый процент CPU на счету.

Настройка limits и групп управления

Даже с RT-ядром и изоляцией ядер, обычный пользователь не может создавать RT-процессы — система это блокирует из соображений безопасности. Нужно явно разрешить это через PAM limits:

Затем добавьте вашего пользователя в группу realtime:

Добавьте sourcing в .bashrc:

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

colcon — это система сборки для ROS 2, аналог catkin из ROS 1. Флаг --symlink-install позволяет редактировать Python-скрипты и XML-файлы без пересборки — критически удобно при настройке behavior tree и параметров.

Проверка окружения

Верификация RT-настроек

Чтобы убедиться, что система реального времени работает корректно, используйте утилиту cyclictest из пакета rt-tests:

Ключевые параметры: приоритет 80, 5 потоков, интервал 200 мкс, 100 000 итераций. Обратите внимание на значение Max — это максимальная задержка за весь прогон. На RT-ядре с правильной настройкой оно должно быть , на стандартном ядре может достигать .

| Параметр | Стандартное ядро | PREEMPT_RT | |---|---|---| | Средняя задержка | 15–30 мкс | 5–10 мкс | | Максимальная задержка | 10–50 мс | 50–200 мкс | | Стд. отклонение | Высокое | Низкое |

Если Max превышает 500 мкс, проверьте: не запущены ли фоновые задачи с высоким приоритетом, не работает ли irqbalance (его лучше отключить: sudo systemctl stop irqbalance), и правильно ли изолированы ядра.

Теперь у вас есть полностью подготовленная среда: RT-ядро гарантирует предсказуемое планирование, изолированные ядра дают dedicated-ресурсы для критических нод, а ROS 2 с Nav2 и Gazebo позволяют разрабатывать и тестировать навигацию в симуляции перед переносом на реального робота.

Архитектура Nav2: компоненты, плагины и подготовка окружения с TurtleBot3 в Gazebo

Когда вы впервые открываете конфигурационный файл Nav2 и видите 300+ параметров, разбросанных по десятку секций, возникает ощущение, что кто-то специально сделал систему сложнее, чем она должна быть. Но если понять архитектуру — увидеть, как компоненты связаны между собой, за что отвечает каждый плагин и как данные текут от сенсоров к командам управления — хаос превращается в логичную структуру. Именно этому посвящена эта статья: разбор архитектуры Nav2 и практическая настройка окружения с TurtleBot3 в Gazebo.

Обзор архитектуры Nav2

Nav2 — это не одна программа, а набор взаимодействующих серверов (servers), каждый из которых выполняет конкретную задачу. Все они общаются через lifecycle manager — менеджер жизненного цикла, который управляет запуском, остановкой и переходом состояний каждого сервера. Это ключевое отличие от ROS 1 Navigation: каждый компонент может быть независимо перезапущен, диагностирован и заменён без остановки всей системы.

Основные серверы Nav2:

Каждый сервер реализован как lifecycle node — это означает, что он проходит чётко определённые состояния: Unconfigured → Inactive → Active → Finalized. Lifecycle manager переводит все серверы в состояние Active последовательно и может перезапустить любой из них при сбое.

Плагинная архитектура: почему всё настраивается

Главная сила Nav2 — плагинная архитектура на основе библиотеки pluginlib. Каждый планировщик, контроллер и даже behavior tree node — это плагин, который загружается динамически по имени из конфигурационного файла. Это значит, что вы можете заменить алгоритм планирования, не перекомпилируя Nav2, и не трогая код других компонентов.

| Роль | Стандартные плагины | Что делает | |---|---|---| | Глобальный планировщик | NavfnPlanner, SmacPlanner2D, ThetaStarPlanner | Строит путь по глобальной costmap | | Локальный контроллер | RegulatedPurePursuit, MPPI, DWB | Следует локальному пути, генерируя cmdizar | | Behavior Tree nodes | NavigateToPose, RateController, RecoveryNode | Управляют логикой навигации | | Costmap layers | StaticLayer, ObstacleLayer, InflationLayer | Формируют слои карты препятствий | | Recovery behaviors | Spin, BackUp, Wait | Обрабатывают ситуации, когда робот застрял |

Каждый плагин настраивается через YAML-файлы. Например, чтобы заменить контроллер с RegulatedPurePursuit на MPPI, достаточно изменить одну строку в конфигурации:

TurtleBot3 в Gazebo: быстрый старт

TurtleBot3 — это стандартная платформа для обучения и прототипирования в ROS. Она существует в трёх моделях: Burger, Waffle и Waffle Pi. Для навигации лучше всего подходит Waffle — у него есть LiDAR и камера.

Запуск мира

Этот команд запускает Gazebo с предустановленным миром, в котором есть коридоры, стены и препятствия. Робот появляется с LiDAR (публикует в /scan), одометрией (/odom) и IMU.

Запуск Nav2

Параметр use_sim_time:=True критически важен: он говорит ROS 2 использовать симуляционное время Gazebo, а не системные часы. Без него все таймеры Nav2 будут работать некорректно — costmap не будет обновляться, а планировщик будет получать устаревшие данные.

Запуск SLAM для построения карты

После этого в RViz (который запускается автоматически) вы увидите, как робот исследует пространство и строит карту. Для управления роботом вручную используйте:

Разбор nav2_params.yaml

Конфигурационный файл Nav2 — это единый YAML, в котором собраны параметры всех серверов. Вот ключевые секции и их назначение:

amcl — адаптивная локализация Монте-Карло. Определяет положение робота на известной карте. Параметр max_particles (по умолчанию 2000) контролирует точность vs производительность: больше частиц — точнее локализация, но выше нагрузка на CPU.

controller_server — здесь выбирается контроллер и его параметры. controller_frequency задаёт частоту генерации команд скорости (по умолчанию 20 Гц). Для систем с PREEMPT_RT можно увеличить до 50 Гц, но только если ядро действительно даёт гарантии по времени.

planner_server — глобальный планировщик. expected_planner_frequency задаёт, как часто пересчитывать путь. По умолчанию 1.0 Гц — этого достаточно, так как глобальная карта меняется медленно.

global_costmap и local_costmap — две карты препятствий. Глобальная покрывает всю карту и обновляется редко (1 Гц), локальная — только область вокруг робота и обновляется часто (5–20 Гц). Каждая costmap состоит из слоёв (layers): static_layer (известная карта), obstacle_layer (динамические препятствия из сенсоров), inflation_layer (расширение препятствий для безопасного проезда).

bt_navigator — указывает путь к XML-файлу behavior tree. Стандартный файл navigate_w_replanning_and_recovery.xml реализует логику: планирование → следование → перепланирование при отклонении → восстановление при застревании.

Проверка системы

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

Если какой-то сервер остался в состоянии inactive, проверьте логи: ros2 lifecycle log /имя_сервера. Типичная причина — неправильный путь к YAML-файлу параметров или отсутствие необходимого плагина.

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

Автономная навигация: SLAM, планировщики, costmaps и управление в Nav2

Робот стоит в начале коридора, цель — комната в конце здания. Между ними — двери, повороты, узкие проходы и, возможно, движущиеся люди. Как робот решает, куда ехать? Как понимает, что стена слишком близко? Что делать, если путь перегородил человек? Каждый из этих вопросов对应ует конкретному компоненту Nav2: SLAM строит карту, планировщик ищет путь, costmap оценивает опасность, а контроллер генерирует команды управления. Разберём, как они работают вместе.

SLAM: одновременная локализация и построение карты

SLAM (Simultaneous Localization and Mapping) — это задача, при которой робот одновременно строит карту окружающего пространства и определяет своё положение на ней. Это классическая «курица и яйцо»: чтобы знать, где ты есть, нужна карта; чтобы построить карту, нужно знать, где ты находишься.

В Nav2 для SLAM используются два основных подхода:

Cartographer (от Google) — использует подход на основе подграфов оптимизации. Робот непрерывно сканирует окружение LiDAR'ом, сопоставляет новые сканы с предыдущими и корректирует свою позицию. Cartographer особенно хорош в больших помещениях и при длительных сессиях: он умеет обнаруживать замыкания контура (loop closure) — когда робот возвращается в уже посещённое место и корректирует всю карту разом.

SLAM Toolbox — более современная альтернатива, оптимизированная для ROS 2. Поддерживает режим lifelong mapping — обновление существующей карты без полной перестройки. Это критически важно для эксплуатации: если в помещении появился новый стол, робот может добавить его на карту, не забывая всё остальное.

Ключевой параметр — resolution (разрешение карты в метрах на пиксель). Стандартное значение 0.05 (5 см) — хороший баланс между детализацией и производительностью. Для узких коридоров можно уменьшить до 0.025, но размер карты вырастет в 4 раза.

Глобальный планировщик: поиск пути

Когда карта построена, planner server ищет от неё оптимальный путь от текущей позиции робота до цели. Nav2 предлагает несколько плагинов планировщиков, каждый со своими особенностями.

NavfnPlanner — классический алгоритм Дейкстры (или A* с эвристикой). Работает на сетке costmap, где каждая ячейка имеет стоимость от 0 (свободно) до 255 (занято). Алгоритм находит путь с минимальной суммарной стоимостью. Надёжный, предсказуемый, но не оптимален для роботов с ограничениями на радиус поворота.

SmacPlanner2D — более продвинутый вариант, использующий гибридный A* с учётом кинематических ограничений робота. Генерирует более гладкие пути, которые проще отслеживать контроллеру. Особенно полезен для роботов с дифференциальным приводом или Ackermann-рулевым управлением.

ThetaStarPlanner — модификация A*, которая позволяет «видеть» прямые линии между точками, не ограничиваясь ортогональными направлениями. Результат — более короткие и естественные пути.

Costmaps: как робот «видит» опасность

Costmap — это двумерная сетка, в которой каждая ячейка содержит числовое значение «стоимости» проезда через эту точку. Это не просто «стена / не стена» — costmap кодирует градиент опасности.

Nav2 использует две costmap, каждая со своим назначением:

Global costmap — покрывает всю известную карту. Используется планировщиком для поиска пути. Обновляется редко (1–2 Гц), так как статические препятствия не меняются быстро. Состоит из слоёв:

Local costmap — небольшое окно вокруг робота (по умолчанию 5×5 м). Используется контроллером для принятия решений в реальном времени. Обновляется часто (5–20 Гц). Имеет те же слои, но работает с «rolling window» — окно движется вместе с роботом.

Параметр inflation_radius в inflation_layer — один из самых важных для безопасности. Он задаёт расстояние (в метрах), на котором препятствие начинает «отталкивать» робот. Значение должно быть не меньше половины ширины робота плюс запас на погрешность локализации. Для TurtleBot3 Waffle (ширина 0.287 м) разумное значение — 0.35–0.5 м.

Контроллер: следование пути

Controller server — это компонент, который берёт глобальный путь и преобразует его в последовательность команд скорости (cmd_vel). Это самый «горячий» компонент Nav2 — он работает на максимальной частоте и напрямую влияет на плавность и безопасность движения.

Regulated Pure Pursuit — контроллер, который следует за точкой на пути, находящейся на определённом расстоянии впереди робота (lookahead distance). Он «регулирует» скорость в зависимости от кривизны пути: на прямых участках разгоняется, на поворотах замедляется. Простой, надёжный, хорошо подходит для большинства задач.

MPPI (Model Predictive Path Integral) — контроллер на основе метода Монте-Карло: генерирует сотни случайных траекторий, оценивает каждую по функции стоимости и выбирает лучшую. Способен учитывать сложные ограничения (кинематические, динамические, комфорта пассажиров). Требует больше вычислительных ресурсов, но даёт более гладкое поведение.

Recovery behaviors: что делать, когда всё пошло не так

Робот застрял, путь перегорожен, цель недостижима — recovery behaviors определяют, как система реагирует на нештатные ситуации. Они управляются через behavior tree и выполняются последовательно:

Если все восстановительные действия исчерпаны, BT Navigator возвращает статус FAILURE, и система уведомляет оператора.

Теперь вы понимаете, как SLAM строит карту, планировщик ищет путь, costmap оценивает опасность, контроллер генерирует управление, а recovery behaviors обрабатывают сбои. В следующей статье мы перейдём к разработке: как создавать собственные ноды, behavior tree nodes и интегрировать дополнительные сенсоры.

Разработка программного обеспечения навигационного стека: ноды, поведения и интеграция сенсоров

Стандартная конфигурация Nav2 — это отправная точка, а не финальный продукт. Реальные роботы работают в уникальных условиях: склад с движущимися погрузчиками, больница с узкими коридорами, офис с динамической мебелью. Каждый сценарий требует адаптации — собственных behavior tree nodes, интеграции дополнительных сенсоров, модификации логики восстановления. В этой статье мы разберём, как расширять Nav2 программно: создавать ноды, писать BT-плагины и подключать новые сенсоры.

Создание ROS 2 пакета для навигации

Любая кастомизация Nav2 начинается с пакета. Создадим его:

Структура пакета:

Написание custom behavior tree node

Behavior tree в Nav2 — это XML-файл, описывающий логику навигации как дерево задач. Каждый узел дерева — это C++ класс, зарегистрированный как плагин. Рассмотрим создание узла, который проверяет, свободен ли путь от динамических препятствий, прежде чем робот начнёт движение.

Регистрация в CMakeLists.txt:

Отладка behavior tree

Для визуальной отладки behavior tree используйте Groot2 — GUI-инструмент, который показывает дерево в реальном времени, подсвечивая активные узлы и их статусы. Groot2 подключается к Nav2 через ZeroMQ и отображает:

Это особенно полезно при отладке кастомных BT nodes: вы сразу видите, на каком узле дерево «застряло» или почему recovery behavior сработал неожиданно.

Теперь у вас есть инструменты для создания собственных компонентов Nav2: BT nodes для кастомной логики, контроллеры для специфических требований, интеграция дополнительных сенсоров. В финальной статье мы займёмся диагностикой и оптимизацией — как найти и устранить узкие места в навигационном стеке.

Диагностика, оптимизация производительности и решение проблем навигации в реальном времени

Робот едет по коридору, и вдруг — рывок, остановка, повторная попытка планирования, снова рывок. В симуляторе всё было плавно. Что изменилось? Ответ обычно кроется не в одном месте, а в комбинации факторов: перегруженный CPU, неоптимальные параметры costmap, устаревшие TF-трансформации, утечки памяти в долгих сессиях. Эта статья — практическое руководство по диагностике, профилированию и оптимизации навигационного стека Nav2 на реальном оборудовании.

Диагностика: где искать проблему

Первый шаг — определить, какой именно компонент Nav2 является узким местом. Навигационный стек работает как конвейер: LiDAR → costmap → планировщик → контроллер → cmd_vel. Задержка на любом этапе каскадно влияет на все последующие.

Мониторинг частоты топиков

Самый быстрый способ найти проблему — проверить, с какой частотой публикуются ключевые топики:

Если /cmd_vel публикуется реже, чем задано в controller_frequency, значит контроллер не успевает — CPU перегружен или costmap обновляется слишком медленно.

Мониторинг задержек

Задержка (latency) — это время между моментом, когда LiDAR зафиксировал препятствие, и моментом, когда робот отреагировал изменением скорости. Измерить её можно через ros2 topic delay:

Допустимая задержка для навигации: при скорости робота — это неконтролируемого движения. При задержка в даёт — уже опасно в узком коридоре.

Профилирование с Valgrind и callgrind

Для глубокого анализа производительности используйте callgrind — инструмент из набора Valgrind. Он записывает каждый вызов функции и подсчитывает, сколько инструкций было выполнено. Это позволяет найти точные «горячие» функции, которые потребляют больше всего CPU.

Подготовка

Для профилирования Nav2 нужно скомпилировать пакеты с отладочной информацией:

Установите инструменты:

Профилирование controller server

Контроллер — самый «горячий» компонент, поэтому начните с него. Важно изолировать его от остальных нод Nav2: закомментируйте controller_server в navigation_launch.py и запустите его отдельно:

Дайте системе поработать 2–5 минут в типичном сценарии навигации, затем остановите (Ctrl+C). Результат сохранится в файл callgrind.out.<PID>.

Анализ результатов

Откройте результат в kcachegrind:

Интерфейс kcachegrind показывает:

Типичные находки при профилировании Nav2:

| Функция | Доля CPU | Что означает | |---|---|---| | Costmap2DROS::updateMap() | 30–40% | Обновление costmap — главный потребитель | | SmacPlanner::createPlan() | 15–25% | Планирование пути | | RegulatedPurePursuit::computeVelocityCommands() | 10–15% | Генерация команд управления | | TF-трансформации | 5–10% | Преобразование систем координат |

Если updateMap() занимает более 40% — нужно оптимизировать costmap. Если планировщик — уменьшить max_iterations или увеличить tolerance.

Оптимизация costmap

Costmap — самый ресурсоёмкий компонент. Каждое обновление требует пересчёта всех ячеек, raycasting для obstacle layer и вычисления расстояний для inflation layer.

Снижение частоты обновлений

Уменьшение размера локальной costmap

Уменьшение width и height с 5 до 2 метров снижает количество ячеек в 6.25 раз — это прямая экономия CPU.

Оптимизация inflation layer

Параметр cost_scaling_factor определяет, как быстро стоимость убывает от края препятствия. Больше значение — более резкий переход — меньше ячеек с промежуточными значениями — быстрее вычисления.

Оптимизация планировщика

Глобальный планировщик обычно вызывается редко (1 Гц), но каждый вызов может быть дорогим:

Downsampling — одна из самых эффективных оптимизаций: costmap с разрешением 0.05 м преобразуется в карту с разрешением 0.1 м, что ускоряет поиск пути в 4 раза (в 2D сетке).

Управление TF-задержками

TF (transform framework) — система преобразования координат между системами отсчёта (base_link, odom, map). Nav2 критически зависит от своевременности TF: если трансформация map → base_link устарела более чем на 100 мс, планировщик и контроллер работают с неверными координатами.

Проверка актуальности TF:

Если TF устаревает, проверьте:

Мониторинг в реальном времени

Для непрерывного мониторинга используйте связку ros2 topic + htop + rqt:

Топик /diagnostics публикует данные от каждого сервера Nav2: частота обновлений, время обработки, статусы. rqt_robot_monitor визуализирует эти данные в реальном времени, подсвечивая компоненты с проблемами жёлтым (предупреждение) и красным (ошибка).

Типичные проблемы и решения

Робот «дрожит» на месте — контроллер генерирует осциллирующие команды. Решение: увеличить min_x_velocity_threshold и min_theta_velocity_threshold (отсечь мелкие команды), уменьшить controller_frequency или увеличить lookahead_dist.

Робот не может пройти узкий проход — inflation layer слишком велик. Решение: уменьшить inflation_radius до значения, близкого к robot_radius, или уменьшить cost_scaling_factor.

Планировщик не находит путь — allow_unknown: false и часть карты не исследована. Решение: установить allow_unknown: true или использовать SLAM для полного исследования.

Утечка памяти при длительной работе — costmap накапливает данные. Решение: проверить, что rolling_window: true для local_costmap, и перезапускать amcl периодически через lifecycle manager.

Высокая задержка на встраиваемом оборудовании — Jetson/Raspberry Pi не справляются. Решение: использовать composition (объединение нод в один процесс для исключения сериализации), уменьшить разрешение costmap, перейти с SmacPlanner2D на NavfnPlanner.

Связка PREEMPT_RT и Nav2

Вернёмся к теме первой статьи. Все оптимизации параметров бессмысленны, если ядро не даёт гарантий по времени. На практике связка PREEMPT_RT + правильные приоритеты даёт наибольший эффект именно для controller server:

Контроллер с приоритетом 90 будет вытеснять все остальные процессы, включая обновление costmap и планирование. Это гарантирует, что cmd_vel будет публиковаться точно с заданной частотой, даже если остальные компоненты отстают.

Проверка на практике: запустите cyclictest параллельно с Nav2 и убедитесь, что максимальная задержка остаётся в пределах :

Если Max превышает 1 мс при работающем Nav2, значит какой-то компонент блокирует CPU — используйте isolcpus для выделения ядра контроллеру.