Всем привет! Меня зовут Алексей Староверов, я научный сотрудник группы «Embodied agents» в AIRI. К числу моих научных интересов в основном относятся алгоритмы обучения с подкреплением (RL) и их применение для робототехнических систем. В этом году в рамках конференции ICRA 2024 мы с коллегами из МФТИ представили статью на тему автономной навигации мобильных роботов, о которой я бы и хотел вам рассказать.

Введение

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

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

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

Такая концепция оптимизации в режиме скользящего окна в англоязычной литературе называется Model predictive control. На русский этот термин переводят по‑разному — «модельно‑предиктивное управление», «модельное прогнозирующее управление», а то и «управление с прогнозирующими моделями». Наша же работа посвящена конкретному вопросу: как при оптимизации участка траектории учесть опасность столкновения с препятствиями?

Например, в функцию стоимости задачи оптимизации можно добавить штрафное слагаемое, отвечающее за избегание столкновений с препятствиями. Если рассматривать это слагаемое как отдельную математическую функцию (ее называют репульсивным или отталкивающим потенциалом), то можно сказать, что она формирует искусственное потенциальное поле (Artificial Potential Field, APF).

Градиент потенциального поля показывает, в какую сторону нужно сдвинуть траекторию, чтобы сделать ее более безопасной. Это позволяет контроллеру найти правильный баланс между безопасностью траектории и ее сходством с эталонным путем. Чтобы можно было посчитать градиент, функция репульсивного потенциала должна быть дифференцируемой.

Сами значения APF посчитать легко: для любой точки на карте они зависят от того, на каком расстоянии от робота находится граница ближайшего препятствия. А вот с градиентами проблема: расстояние до ближайшей точки препятствия — это не дифференцируемая функция, оно высчитывается алгоритмически. Ее можно задать в виде такой функции для некоторых простых случаев (например, если робот круглый, а препятствий немного, и они тоже круглые), но если у нас робот сложной формы едет по произвольной карте, найти аналитическое приближение APF становится непросто.

Нейронное потенциальное поле

Чтобы справиться с этой трудностью, мы предложили модель нейронного потенциального поля (Neural Potential Field, NPField) — нейронную сеть для вычисления искусственного потенциала. Наш подход концептуально вдохновлен моделью NeRF (Neural Radiance Field) из области машинного зрения. NeRF принимает на вход положение и ориентацию виртуальной камеры и возвращает интенсивность изображения, которое такая камера могла бы снимать (что позволяет генерировать вид объекта с нового ракурса). Наша же модель принимает на вход положение и ориентацию робота вместе с картой препятствий и возвращает значение отталкивающего потенциала. При этом на практике для нас важно не получить эти значения сами по себе, а использовать их градиент для оптимизации.

Общая схема нашего подхода показана на рисунке ниже:

В верхней части показана схема нейросети, в нижней — схема контроллера, который используется для планирования траектории. Нейронная сеть состоит из двух основных частей: блока энкодеров и нейронной функции потенциала (NPFunction) — подсети, которая вычисляет потенциал для заданного положения робота. В свою очередь, контроллер состоит из двух высокоуровневых блоков. Первый блок (Parameter definition) на основе актуальных данных определяет набор параметров для задачи MPC. Это делается один раз на каждую итерацию цикла управления. Второй блок (Numerical MPC Solver) выполняет численное решение задачи оптимизации с учетом данного набора параметров.

NPField обучается как единая нейросеть, а затем делится на две части. Энкодеры изображений включаются в блок определения параметров, а NPFunction интегрируется в численный решатель MPC. На каждой итерации контроллера энкодеры вызываются один раз, а NPFunction вызывается и дифференцируется несколько раз в процессе итеративной оптимизации.

Архитектура нейронной сети подробнее показана на следующем рисунке:

Данные для обучения

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

• Карта препятствий преобразуется в карту стоимости:
  
  ◦ Алгоритмически вычисляется функция расстояния со знаком (SDF) для каждой ячейки на карте. SDF равна расстоянию от текущей ячейки до ближайшей границы препятствия. Она положительна для ячеек свободного пространства и отрицательна для ячеек с препятствиями.
  
  ◦ Для каждой ячейки вычисляется отталкивающий потенциал: . Это сигмоидная функция, которая имеет низкие значения вдали от препятствий, асимптотически стремится к ​ внутри препятствий и имеет максимальную производную на границе препятствия.

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

Реализация и эксперименты

Модуль численного решения MPC реализован с использованием фреймворка Acados, который опирается на более низкоуровневый фреймворк CasADi. Эти фреймворки позволяют высокоэффективно решать задачи нелинейной оптимизации, при этом задача разработчика сводится к тому, чтобы корректно закодировать математическую постановку задачи. Для интеграции в солвер нейросетевой модели NPFunction мы использовали библиотеку L4CasADi.

Наш локальный планировщик работает вместе с алгоритмом планирования пути Theta*, который генерирует геометрический путь между точкой старта и точкой финиша в виде ломаной. Стоит обратить внимание, что Theta* использует упрощенную модель формы робота (круг с диаметром, равным ширине робота). Эта упрощенная модель не гарантирует безопасность пути, которая обеспечивается нашим локальным планировщиком.

Мы считаем, что карты препятствий имеют разрешение 256×256, где каждый пиксель соответствует 2×2 сантиметрам реальной среды (т. е. размер карты составляет 5.12×5.12 метра). Для обучения мы собрали набор данных на основе городских карт MovingAI. Он включает 4,000,000 образцов, взятых с 200 карт для роботов двух разных форм. Оба робота соответствуют реальному мобильному манипулятору Husky UGV с робототехнической рукой UR5. В первом случае робот перемещается в пространстве со сложенным манипулятором, во втором — с вытянутым. Для каждой карты с каждым роботом было сгенерировано 10,000 случайных позиций. Генерация набора данных заняла 40 часов на процессоре Intel Core i5–10 400F.

Одна процедура оптимизации в нашей реализации занимает 60–70 мс, из которых на кодирование параметров приходится около 10 мс, а работа солвера Acados занимает оставшиеся 50–60 мс. Стоит обратить внимание, что солвер Acados нуждается в «прогреве» перед использованием в реальном времени: первое выполнение процедуры оптимизации может занять около одной секунды; после этого решатель работает быстрее.

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

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

Мы сравнили траектории NPField с бейзлайновым алгоритмом оптимизации траекторий CIAO, который основан на выпуклом приближении свободного пространства вокруг робота. Траектория, сгенерированная CIAO, показана на правой верхней части рисунка. Видно, что робот почти касается краев препятствий. При тестировании на более разнообразных сценариях CIAO не смог найти допустимый путь в почти половине случаев. Это может быть связано с тем, что CIAO реализует избегание столкновений как набор неравенств, которые не дифференцируются во время оптимизации. Поэтому он только проверяет факт наличия столкновения, а не пытается найти баланс между безопасностью и отклонением пути в функции стоимости.

Мы сравнили наш алгоритм с бейзлайнами на 20 сценариях, используя фреймворк BenchMR. Задачи включают прохождение через узкие проходы, аналогичные показанным на рисунке выше. Мы сравнили стандартные метрики фреймворка: время планирования, длину пути, гладкость пути (Smoothness) и Angle‑over‑length. Все метрики трактуются по принципу «меньше — лучше».

Результаты приведены в таблице ниже:

Мы сравниваем наш стек (Theta* + NPField) с популярными алгоритмами планирования: RRT, RRT*, Informed RRT, SBL и RRT + GRIPS. Кроме того, в таблице отсутствуют результаты для PRM, Theta* + CIAO и Theta* + CHOMP, так как эти алгоритмы смогли найти путь менее, чем для половины задач. Этот результат особенно важен для Theta* + CIAO и Theta* + CHOMP, так как они являются планировщиками на основе оптимизации, аналогичными нашему подходу, и используют те же геометрические пути. Однако они не смогли справиться с рассматриваемыми сценариями из‑за столкновений (CHOMP) или невозможности найти результат (CIAO).

Результаты в таблице показывают, что наш стек в целом сопоставим с другими планировщиками. Он обеспечивает почти самую короткую длину пути, лучшую плавность, хороший AOL, хорошее время вычислений и хорошее безопасное расстояние. Мы не можем указать подход, который определенно лучше нашего (RRT с GRIPS быстрый и безопасный, но обеспечивает менее плавные траектории).

Мы развернули наш подход на реальном мобильном манипуляторе Husky UGV в виде модуля ROS для локального планирования и управления MPC, который работает с глобальным планировщиком Theta*. Сценарий тестирования включает проезд через извилистый коридор. Манипулятор держит объект в разложенном состоянии, поэтому его форма является более вытянутой, чем обычно. Во время нашего эксперимента робот успешно преодолел путь длиной 15 метров, который включал три поворота и один узкий проход.

А ниже можно посмотреть, как он едет по маршруту

Заключение

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

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

Надеюсь, наш опыт был вам полезен и интересен!