Несмотря на прогресс в технологиях и развитие микроэлектроники, задача поиска оптимального пути по-прежнему является весьма тяжёлой для современных вычислителей — будь то CPU или GPU. Горизонт планирования у многих локальных алгоритмов (например, DWA, TEB, MPPI на CPU) как правило не превышает нескольких метров, а иногда и дециметров. Однако планирование на большой временной интервал и дистанцию позволяет алгоритмам лучше находить пути в насыщенных препятствиями средах и является важным элементом системы уклонения от движущихся, динамических препятствий. Для решения задачи создания модуля поиска пути с дальним горизонтом планирования в этой статье будет рассмотрен пакет локального планировщика MPPI-Generic, работающий на GPU. Он может работать в связке с планировщиком State Lattice Planner из ROS NAV2, но в этой демонстрации будет использоваться отдельно от него — как «универсальный планер». Тесты работы обоих пакетов планирования будут проводиться в самодельном Qt-OpenGL симуляторе автомобиля (пикапа), выполненном на C++ для более плотной интеграции с симулятором и удобного обращения к CUDA-ядрам MPPI-Generic.

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

Связка «глобальный + локальный планер» — вынужденная мера из-за ограничения вычислительной мощности современных компьютеров и сложности создания массово-параллельного глобального планера. У слова «глобальный» тоже есть оговорки. Настоящий глобальный планер предполагает планирование из вашего текущего местоположения до пункта назначения. В терминах робототехники и ROS глобальный планер — это планер, способный проложить путь от одной точки до другой на складе, в офисе или у вас дома, то есть с горизонтом планирования в десятки метров. Иначе время планирования начинает выходить за рамки realtime и достигать десятков секунд или даже минут.

Однако в outdoor-робототехнике планирования на несколько метров часто бывает недостаточно из-за большой скорости движения транспорта на улице. Чтобы вовремя распознать опасное столкновение, нужно предсказывать траекторию всех видимых ровером машин хотя бы линейно (по текущему вектору скорости), а также знать свою траекторию на несколько десятков секунд вперёд. При этом успевать её пересчитывать на частоте поступления данных с главного источника информации о препятствиях — лидара, а это, как правило, 10–20 Гц, чтобы успеть отреагировать.

К сожалению, многие алгоритмы в открытом доступе не вписываются в такие жёсткие рамки. Например, время работы алгоритмов гибридного A* из случайных репозиториев GitHub на десктопе:

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

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

На время поиска пути влияет всё: длина маршрута, количество препятствий, целевая ориентация (носом или кормой относительно стартовой), дистанция от целевой позиции до препятствий, форма препятствий и т.д. Это увеличивает время планирования и делает его порой непредсказуемым.

Однако именно глобальный планер может найти на карте кратчайший и кинематически выполнимый путь гарантированно (если он существует для вашего робота), перебрав все варианты траекторий. В ROS NAV2 самым быстрым глобальным планером является State Lattice (решётка состояний). Это алгоритм семейства A*, который перебирает готовый набор примитивов из заранее сгенерированных скриптом. Примитивы это дуги, повороты, которые как-бы накладываются на карту проходимости, идущие из клетки с текущим положением ровера в соседние клетки а иногда и через 1-2 клетки впереди:

Комбинируя их с проверкой на кинематическую выполнимость хода (чтобы меньше поворачивать кузов), глобальный планер строит путь по карте проходимости (costmap). Глобальный планер по итогу работы возвращает кратчайший путь — траекторию, двигаться по которой должен контроллер. Задача контроллера преобразовать путь в понятные команды управления для моторов или другому более низкоуровневому контроллеру, таким образом контроллер может быть любым, например по типу простого, геометрического вычисления направления на точку (расчёта азимута) расчитвающего курс для рулевой сервомашинки, более сложных алгоритмов следования пути LOS/PLOS (LOS с марковокой, аналог Pure Pursuit и Vector Pursuit в ROS NAV2 ) или его роль может играть локальный планер.

Локальный планер, в отличие от глобального, работает очень быстро:

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

локальный планер не может найти путь к целевой позиции (обозначеной таким-же пикапом) через запретную (пурпурную) зону на карте проходимости и стоит в ловушке.

В особенности MPPI-Generic благодаря массовому параллелизму на GPU в отличие от CPU реализации MPPI ROS NAV2 позволяет быстро работать даже с тяжёлыми нейросетевыми моделями движения машины такими как autorally, балансирующим на пределе возможности сцепления шины с грунтом:

Конкретно для такой быстрой и агрессивной езды требуется дообучение на треке и составление карт сцепления шин с дорогой, что требует многократного его прохождения. Строить такие карты отдельная сложная задача, для их построения на лету нужно обучать нейросетевую модель либо составлять её по мере движения и многократного прохождения одного маршрута сохраняя и накапливая текущие состания из модели. Для outdoor такое сложное решение сделать быстро и надёжно будет очень трудно. Поэтому я решил сделать контроллер, который стабилизирует угловую скорость по гироскопу. Он не сможет разогнать ровер до экстремальных скоростей и балансировать на пределе сцепления. Но за счёт быстрой реакции гироскопа в экстренной ситуации поможет поймать машину и позволит ехать достаточно быстро даже с невысокой частотой обновления уставки.

Также я решил испытать MPPI-Generic не объединяя оба подхода к планированию (глобальный + локальный планер) как это обычно принято. Т.к. у такого решения есть и минусы оно увеличивает время планирования создовая задержку в управлении и нагружает CPU у которого и так много задач. Чтобы глобальный планер (например State Lattice) быстрее нашёл путь, он сначала строит простой грубый путь с помощью A* 2D, затем, используя этот путь как эвристику расстояния (чтобы перебирать меньше примитивов и делать меньше шагов по карте), а потом уже строит кинематически выполнимый путь. Локальный планер будет уже третьим по счёту перепланированием одного и того же маршрута, всё это время суммируется в общую задержку:

Как пример — время планирования случайного маршрута в State Lattice Planner:

На этом снимке зелёным цветом обозначен найденный кратчайший путь к цели (красная точка) через препятствия (чёрным и безопасная зона/инфляционный слой вокруг них голубым), фиолетовым экспансии т.е. проверенные алгоритмом ячейки и варианты пути

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

Что-бы проверить MPPI-Generic в роли универсального планера я решил провести тесты в самодельном симуляторе, создающем среду, похожую на outdoor с большими открытыми пространствами:

Для начала — о том, как работает планирование в симуляторе. Здесь есть два планера: State Lattice Planner и MPPI-Generic. В данный момент они работают параллельно, то есть оба получают координаты цели и ищут к ней путь.

Машина управляется через низкоуровневый (реактивный) контроллер, принимающий уставки угловой и линейной скорости (w_cmd и v_cmd). Такое управление вполне можно реализовать на реальном ровере, имея на борту микроконтроллер, гироскоп и энкодеры приводных колёс.

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

Это необходимое ограничение: если его не ввести и ничего не делать всё будет наоборот приоритет линейной скорости над угловой и тогда машина либо не сможет вписаться в траекторию (и врежется, но зато на полном ходу!), либо просто перевернётся от резкого поворота рулевых колёс на скорости. Подобный контроллер реализован в ROS NAV2 Regulated Pure Pursuit (RPP) — но ему нужна одометрия и траектория от глобального планировщика, а также в Ardupilot (acro mode / turn rate mode для роверов) — там это обычно работает через GPS.

В моей реализации контроллера управление строится через энкодеры приводных колёс. Это позволяет в прошивке МК обойтись без сложного фильтра EKF — достаточно простого фильтра Махони для получения данных об угловой скорости корпуса (рысканье) с гироскопа и компенсации наклона, а также данных о скорости приводных колёс от энкодеров. Даже если колёса сорвутся в пробуксовку, это не приведёт к расходимости алгоритма. Наоборот, из-за завышенной скорости угол поворота рулевых колёс будет ограничен сильнее. При резком возвращении сцепления это позволит не перевернуться, в остальных случаях пробуксовку компенсирует инерция корпуса.

При этом, кроме ограничения по срыву колёс (центробежной силой), контроллер больше ничего не ограничивает. Если локальный планер будет подавать команды (w_cmd и v_cmd) в МК согласованно этому простому правилу, ограничения вообще никогда не применятся и не будут мешать управлять ровером, разве что в случае ошибки.

Такая схема управления немного напоминает ESP/ESC (Electronic Stability Program) но управляющего не тормозами а рулём. Локальный планер/контроллер работающий на Jetson/miniPC рулит скоростью поворота корпуса машины через уставку w_cmd и v_cmd, что как-бы скрывет шасси под слоем абстракции, все несовершенства геометрии корпуса и дороги контроллер берёт на себя, а также отрабатывает (одноразово, в отличие от контроллера по угловому положению) резкие броски от ударов колёс при наезде на небольшие препятствия — сразу на МК.

Контроллер может работать на высокой частоте (100 - 400 Гц) с минимально возможными задержками (по прерываниям IMU и DMA), не требует построения сложных карт сцепления колёс с грунтом на ходу, позволяет локальному и глобальному планеру работать на низкой частоте лидара (10-20 Гц). Что также даёт бóльшие интервалы времени для долгосрочного планирования. Для большей реалистичности в симуляции измерения контроллера текущая угловая скорость и скорость от энкодера зашумлены (случайное блуждание + белый шум + дрейф, энкодеры белый шум), что больше соответствует требованиям outdoor.

Глобальный планер State Lattice в симуляторе в данный момент работает в связке с State Lattice + Regulated Pure Pursuit Controller это простой геометрический контроллер, что позволяет State Lattice следовать по маршруту бегая за марковкой расположенной на пути вперёд для выравнивания вдоль линии маршрута. Напоминает PLOS в частности как мне показалось унаследовал его собенность чуствительность к шуму и даже не большим перестройкам маршрута от State Lattice, но свою работу выполняет.

траектория глобального планера обозначена зелёным и синими линиями обозначены проверенные ячейки (экспансии) их направление соответствует направлению примитивов

Локальный планер в симуляторе также выводит траекторию, а точнее целый набор траекторий: синим обозначена оптимальная траектория с нулевым шумом, красными — рандомная выборка из 1024 возмущений вокруг текущего оптимального управления (синяя траектория). MPPI генерирует шум с нулевым средним относительно предыдущего решения, поэтому все траектории “крутятся” вокруг синей. Это свойство локального оптимизатора: планер предполагает, что предыдущее решение было хорошим, и ищет улучшения поблизости. В этом есть и минус, если робот застрял, красные траектории не выведут его из ловушки — нужен глобальный планер или дополнительная эвристика:

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

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

В 1м видео “эталонное” прохождение трассы в ручном режиме через задавание уставки низкоуровневому контроллеру с клавиатуры, это также демострирует возможности управления машиной через низкоуровневый контроллер на МК:

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

В 2м видео маршрут проходит связка, State Lattice + Regulated Pure Pursuit Controller с вдвое меньшей скоростью для большей управляемости как я уже писал цель убедится, что контроллеры работают стабильно:

Из-за вдвое меньшей скорости в уставке контроллер не смог подняться в крутую горку. Однако в остальном маршрут пройден хорошо. Из недочётов стоит отметить, что маршрут State Lattice не всегда оптимален для машины. Иногда планер идёт своим путём, пока не упирается в препятствие, а потом резко изгибается. При заходе на цель он не строит маршрут заранее, а в последний момент делает резкие изгибы. Это провоцирует RPP на рывки и создаёт колебания, что заставляет State Lattice перестраивать маршрут — возникает своеобразный резонанс. В моменты когда маршрут идёт ровно без лишних изгибов RPP минимально корректирует уставку.

В этом видео MPPI-Generic проходит маршрут с простой моделью (не на нейросетях как у autorally а скорее аналог MPPI ROS NAV2 с не большими доработками) и функции стоимости ведущей его к целевой позиции (не по маршруту глобального планера как MPPI из ROS NAV2) т.е. локальный планировщик используется как универсальный планер, видео ускорено примерно в 2 раза:

Скорость MPPI тут меньше, потому что, в отличие от RPP, он настраивает её сам. Добиться баланса, чтобы MPPI везде ехал быстро, довольно сложно, и с первого знакомства с этой библиотекой у меня этого не получилось. Вообще “настраивать” MPPI-Generic если этот процесс можно так назвать (приходится постоянно что-то изобредать в модели, программировать а не просто крутить ручки параметров) сложно и алгоритм очень капризный, но порой в различных комбинация (в том числе и не правильных) он творит с моделью машины довольно интересные вещи.

В одном из тестов я оставил симуляцию на 2 часа включённой и ушёл на обед. За это время неисправная модель (со смещениями системы координат) с помощью множества микроповоротов на месте идеально совпала со своей целью. Казалось, что машина адаптируется к своим ошибкам — в отличие от простых контроллеров, MPPI справляется с некоторыми проблемами и тем самым маскирует их. В отличие от простых контроллеров, что своим видом сразу показываю, если что-то идёт не так. В этом тесте MPPI пару раз задел, несмотря на инфляционный слой, препятствия и в конце плохо припарковал машину почему-то вообще не используя задний ход предпочитая ездить кругами, хотя он это умеет, от запуска к запуску симуляции MPPI ведёт себя немного по-разному). Тем не менее он прошёл весь маршрут сам без глобального планера в равных с ним условиях и справился почти со всем, кроме случая заезда на горку где и связка State Lattice + Regulated Pure Pursuit Controller не справилась хотя ехала быстрее, что на мой взгляд делает MPPI-Generic ближайшим кандидатом на роль универсально массово параллельного планера.

В дальнейшем я не планирую улучшать MPPI-Generic в сторону универсального планера — настраивать его очень сложно. Я хочу просто добавить ему 10 критиков от mppi_nav2 и сделать связку State Lattice + MPPI-Generic, возможно сравнить их с State Lattice + mppi_nav2. Посмотрим, что в итоге будет работать лучше. Однако уже сейчас MPPI-Generic быстро работает с большим горизонтом планирования и 1024 сэмплами (хотя это далеко не предел) на GPU разгружая тем самым CPU для более приоритетных задач т.к. почти весь ROS/ROS2 работает на CPU нодах и ROS NAV2 состоит из кода на CPU. Код контроллеров из симулятора в дальнейшем планирую перенести на самодельный ровер сделанный из квадроцикла ATW-800:

UPD: Пока статья проходила модерацию, проблему с парковкой MPPI (ошибку в модели продольной динамики) удалось устранить:

Итоги тестов показывают, что использование низкоуровневого контроллера с MPPI — это тупиковая ветвь развития. Низкоуровневый контроллер конфликтует с моделью динамики (например, если MPPI закладывает занос в поворот, контроллер пытается с ним бороться). Кроме того, с более сложными моделями движения MPPI становится трудно предсказывать поведение машины, когда контроллер вносит свою задержку и исправляет траекторию по своему усмотрению. Моделировать его ограничения в модели MPPI, имхо не имеет смысла.

Поэтому с переходом на более сложные модели (чем простая кинематика Аккермана из mppi_nav2) от контроллера придётся отказаться, чтобы MPPI управлял тормозами, рулевой сервой и мотором напрямую на высокой частоте (≈50 Гц).