Задача планирования движения

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

Постановка задачи

Пусть задано непустое множество препятствий [math]O \subset W[/math] в области евклидова пространства [math]W \subset E^N,\ N \in \{2,3\}[/math]. Пусть также задано твердое тело [math]A \subset W[/math], либо кинематическая цепь [math]A \langle B, J \rangle[/math], где [math]B = \{B_1, B_2, \dots, B_n\} \subset W[/math] — множество твердотельных звеньев (элементов кинематической цепи), а [math]J = (J_1, J_2, \dots, J_k)[/math] — множество кинематических ограничений таких, что при корректной конфигурации цепи предикаты ограничений [math]J_1(c), J_2(c), \dots, J_k(c)[/math] принимают истинное значение. Под конфигурацией [math]c \in C_A[/math] здесь понимается набор значений параметров, однозначно определяющий положение точек объекта [math]A[/math] в пространстве сцены. Обычно используется минимальный набор параметров, соответствующий количеству степеней свободы объекта и определяющий пространство состояний или конфигурационное пространство объекта [math]C_A[/math].

Определение:

Пространством допустимых состояний [math]C_{free}[/math] назовем множество всех конфигураций объекта [math]c \in C_A[/math], удовлетворяющих кинематическим ограничениям и исключающих столкновения с препятствиями сцены. [math]C_{free} = \{c \in C_A | J_1(c) \wedge J_2(c) \wedge \dots \wedge J_k(c), B_1(c) \cap O = \varnothing,…, B_n(c) \cap O = \varnothing\}[/math] для кинетической цепи; [math]C_{free} = \{c \in C_A | A(c) \cap O = \varnothing\}[/math] для простого твердого тела.

Тогда постановка задачи поиска пути может быть сформулирована следующим образом. Для пары заданных бесконфликтных конфигураций [math]c_{init},\ c_{goal} \in C_{free}[/math] требуется найти непрерывный путь [math]p(\tau): [0,1] \rightarrow C_{free}[/math] такой, что [math]p(0) = c_{init}[/math] и [math]p(1) = c_{goal}[/math], где [math]\tau[/math] — момент времени.

Рисунок 1. Конфигурационные пространства двумерного твердого тела

Рисунок 2. Конфигурационные пространства двухзвенного манипуляционного робота. Углы поворота верхнего и нижнего звена соответствуют углу поворота образующей окружности тора и угловой координате точки на этой окружности соответственно

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

Этапы

Рисунок 3. Доля машинного обучения в используемых на каждом этапе методах

Восприятие/анализ обстановки (англ. Perception)

Рисунок 4. Визуализация восприятия автомобилем изображения с камеры

Анализ данных об окружении, выделение объектов и препятствий, определение их размеров, скоростей, и расстояний до них. Зачастую осуществляется путем применения алгоритмов машинного обучения для распознавания объектов на изображениях и прочих массивах данных (таких как данные с датчиков).

Предсказание движения объектов (англ. Prediction)

Рисунок 5. Предсказание движения окружающих объектов

Анализ собранных за время наблюдения данных об окружающих объектах для последующего построение модели их движения и предсказания их траекторий. Этот этап будет подробно рассмотрен далее.

Принятие решения/планирование траектории движения (англ. Planning)

Рисунок 6. Планирование траектории движения

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

Предсказание траекторий движения объектов

Поскольку в общем случае мы не можем определить, как будут двигаться объекты, для предсказания траекторий их движений необходимо строить модели на основе прошлых измерений. Эти модели могут представлять из себя как простые предсказания (например, “объект продолжит двигаться с неизменной скоростью/ускорением”), так и более сложные алгоритмы.

Стандартный подход

Рисунок 7. Точность IMM по сравнению с точностью одной модели (константной скорости) с высокой и низкой степенью доверия предсказанию

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

Также проблематичным является тот факт, что одной модели (особенно простой) как правило недостаточно для описания траектории движения объекта. В связи с этим существует алгоритм, использующий множество взаимодействующих моделей (англ. Interacting Multiple Model, IMM) — подход применения сразу нескольких моделей, для каждой из которых поддерживается актуальная (меняющаяся по мере прошествия времени и получения новых измерений) вероятность того, что объект двигается согласно этой модели. Таким образом, используя, например, модель для каждого возможного движения, такого как поворот или ускорение, мы можем делать более точные предположения о том, где объект будет находиться в будущем.

Рисунок 8. Диаграмма процесса работы IMM. На картинке KF означает фильтр Калмана (англ. Kalman filter) — один из возможных предсказательных алгоритмов

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

Применение машинного обучения

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

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

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

Решение задачи для беспилотных автомобилей (англ. Self-driving cars)

Для организации управления беспилотным автомобилем можно воспользоваться классическим подходом из робототехники. Задача самостоятельного передвижения разбивается на четыре модуля.

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

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

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

Алгоритмы на графах

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

• Разбиение пространства на клетки и построение графа на них.
• Построение графа из регулярных примитивов движения (например, дуг).
• Другие, более специализированные подходы, основанные на особенностях конкретной системы.

Самым популярным и зачастую самым оптимальным является алгоритм А*.

Преимущества:

• Гарантированно находит кратчайший (в дискретизированном пространстве) путь.

Недостатки:

• В пространствах малой размерности путь редко является кинематически выполнимым (зависит от метода построения графа).
• В пространствах большой размерности наблюдается заметное увеличение времени работы.

Оптимизационные алгоритмы

Идея оптимизационных алгоритмов заключается в следующем: рассмотрим траекторию нашего положения во времени, [math]x[/math] и [math]y[/math] — координаты, зависящие от времени [math]t[/math], то есть поймем, в какой точке мы хотим оказаться в момент времени [math]t[/math]. Можно сказать, что оптимальной в этом случае будет траектория, которая минимизирует функционал [math]J[/math], являющийся интегралом некоторой функции от траектории по времени.

[math]J[\text{x}(t)] = \int\limits_{t_0}^{t_0 + T} L(\text{x}, \text{x}', \text{x}'', \text{x}''') dt[/math], где [math]\text{x}(t) = (x(t), y(t))^T[/math] — траектория.

Функция от траектории [math]L[/math] здесь каким-либо образом нас штрафует за резкие повороты, резкие разгоны, нахождение близко к препятствиям. Тогда, если просуммировать вдоль траектории все необходимые штрафы и попытаться это минимизировать с помощью стандартного математического аппарата, никак не связанного с автомобилями в целом и беспилотными автомобилями в частности, это решит задачу в общем виде.

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

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

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

Рисунок 9. Для удобства вычисления расстояний представим автомобиль в виде набора окружностей

Рисунок 10. Для плавной функции расстояния до препятствий введем поле градиентов

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

Что нужно, чтобы плавно изменялось расстояние? Евклидово расстояние до невыпуклых многоугольников не обладает необходимыми свойствами и плохо дифференцируемо в местах, где наблюдается отсутствие выпуклости. Поэтому можно построить псевдорасстояние по градиентному полю до ломаной, которая обозначена на Рис. 10 красным и представляет собой препятствие. Введем поле расстояний от каждой точки до этой ломаной, которое направлено в сторону ломаной и обладает необходимыми свойствами дифференцируемости — пусть и не являясь строго кратчайшим. Это позволит построить гладкую и аккуратную траекторию.

Преимущества:

• Пространство управления непрерывно.

Недостатки:

• Сходится к локальным минимумам.
• Ограничения должны быть заданы дифференцируемыми функциями.

Стохастические алгоритмы

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

Рисунок 11.. Анимация 10000 итераций работы алгоритма RRT

Самым распространенным стохастическим алгоритмом является построение быстро исследующего случайного дерева (Rapidly-exploring Random Tree, RRT) или деревьев на его основе (RRT* и прочие).

Принцип заключается в итеративном построении дерева. На каждой итерации происходят следующие действия:

1. Выбор случайной точки пространства.
2. Нахождения ближайшего к этой точке узла уже построенного дерева.
3. Построение ребра в сторону новой точки с помощью симуляции проезда нескольких метров.

В результате достаточно быстро строится карта путей, которая некоторым (вероятнее всего, не оптимальным) образом покрывают пространство. Можно продолжать ее строить до тех пор, пока не найдется путь до искомой точки или пока не будет сочтено нужным остановиться.

Преимущества:

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

Недостатки:

• Отсутствие гарантий на оптимальность.
• Высокая вероятность того, что траектория движения будет сильно извилистой (зависит от выбора дерева; например, RRT* лишен этого недостатка).

Специализированные алгоритмы

Рисунок 12. Построение и выбор плавной траектории смещения

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

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

Области применения

• Беспилотные автомобили (англ. Self-driving cars).

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

• Компьютерная анимация — вид трехмерной анимации, создаваемый при помощи компьютерной графики. Процедурная анимация полностью или частично рассчитывается компьютером, например:
  • Симуляция физического взаимодействия твёрдых тел.
  • Имитация движения систем частиц, жидкостей и газов.
  • Имитация взаимодействия мягких тел (ткани, волос).
  • Расчёт движения иерархической структуры связей (скелета персонажа) под внешним воздействием.
  • Имитация автономного движения персонажа.

• Фолдинг белка — процесс спонтанного свертывания полипептидной цепи в уникальную пространственную структуру. Механизм сворачивания белков до конца не изучен, но аминокислотная последовательность белка обычно известна. Поэтому учёные пытаются использовать различные биофизические методы, чтобы предсказать пространственную структуру белка.

• Архитектурный дизайн.

• Автоматизация процессов.

См. также

• Сверточные нейронные сети
• Рекуррентные нейронные сети
• Алгоритм A*
• Задача нахождения объектов на изображении
• Анализ видео

Источники информации

• "Motion Planning", Wikipedia
• Казаков К.А. и Семенов В.А. (2016) "Обзор современных методов планирования движения"
• Клюев Л. (2017) "Алгоритмы построения пути для беспилотного автомобиля. Лекция Яндекса", Хабр
• Brian Douglas (2019) "Understanding Sensor Fusion and Tracking, Part 4: Tracking a Single Object With an IMM Filter", YouTube
• Brian Douglas (2020) "Autonomous Navigation, Part 4: Path Planning with A* and RRT", YouTube