Алгоритмы построения пути для беспилотного автомобиля. Лекция Яндекса

Яндекс уже некоторое время ведет разработку беспилотного автомобиля. Перед вами одна из первых технических лекций на эту тему. В направлении беспилотных автомобилей работают сотрудники Яндекса в разных городах, включая и Минск. Автор лекции Роман Удовиченко как раз из Минска — он руководит группой обработки дорожной ситуации. На сентябрьском Я.Субботнике Роман рассказал об одной из больших задач, стоящих перед его группой.

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

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

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

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

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

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

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

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

Первое, с чего начнем, это алгоритмы на графах. Первый вопрос, на который нам необходимо ответить, чтобы понять, какие алгоритмы можно применить, это как вообще построить граф? Вот стоит машина, мы можем понять, где она стоит, но в реальности графа никакого нет, вершин ребер на дороге не нарисовано. Нам этот граф нужно как-то придумать самим. Первое, что можем сделать: разобьем все пространство на клетки, рассмотрим маленькие клеточки и скажем, что есть вся наша поверхность земли, разбитая на клетки 25 на 25 или 50 на 50 см. Потом соединим соседние клетки ребрами и будем искать на них путь. Это будет довольно далеко от того пути, который машина может проехать, но какое-то приближение это даст. И у нас будут такие вершины в двумерном пространстве.

Мы можем усложнять наше пространство, добавляя туда текущий угол поворота машины. У нас уже будут клетки не просто x и y, но и текущая ориентация машины в направлениях север, юг, запад и восток, тоже как-то дискретизированных. Кроме направления мы можем много всего учитывать: текущую скорость машины, текущее ускорение, текущее тангенциальное ускорение, нормальное. Все это важно учитывать. Но чем больше мы усложняем наше пространство, тем более сложным становится наш граф.

Помимо разбиения пространства на клеточки, мы можем сказать, что способны двигаться небольшими шагами в виде примитивов. Например — проехать 50 см вперед или проехать 50 см вперед и повернуть налево или проехать 50 см вперед и повернуть направо. И такими примитивами мы можем замостить все наше пространство. В явном виде клеточек у нас нет, но если эти мелкие шаги достаточно хорошо согласованы друг с другом, то у нас получается регулярная сетка, которая хорошо и аккуратненько покрывает пространство.

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

Можем объединить эти два подхода и сказать, что мы рассматриваем примитивы движения под любыми углами, проехать вперед, проехать вперед и повернуть на 10 градусов, 15 градусов, что угодно. При этом все равно разобьем пространство на клеточки и скажем, что в одной клетке не может быть больше 1, 2, …, k вершин.

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

У нас есть граф, мы хотим на нем применить какой-то алгоритм. Есть алгоритм А*.

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

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

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

Самое простое, что мы можем сделать, это рассмотреть расстояние до цели напрямик. Очевидно, мы не можем приехать к конечной точке быстрее, чем если мы просто как танк поедем к ней напрямик.

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

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

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

Что мы получаем? Алгоритм А* позволяет найти заведомо оптимальный путь, который ведет нас к цели, огибая препятствия. Но если у нас пространство малой размерности, мы учитываем только x, y, ориентацию машины и то, что она не может мгновенно разогнаться, — значит, все эти ограничения будут учтены. Если мы будем в наш граф добавлять большое количество параметров, то он будет находиться в пространстве очень высокой размерности. У нас будет 7-, 8-, 10-мерное пространство, мы будем успевать рассматривать небольшие кусочки этого пространства и не сможем построить достаточно далекий маршрут из-за очень высокой вариативность параметров. В каких-то ситуациях А* сложно применять, а где-то он достаточно неплохо себя показывает.

Второй вариант — оптимизационные методы, основанные не на дискретном, а на более непрерывном подходе.

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

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

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

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

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

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

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

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

Получается, мы каждый раз едем от нашего построенного дерева в случайную сторону и можем вершины в этом дереве делать в пространстве любой размерности, то есть в каждой вершине учитывать текущее направление машины, текущую скорость, ускорение, все углы, которые нам важны. А потом, когда мы возьмем новую точку, то возьмем и ближайшую к этой точке вершину. Проедем с помощью какой-то симуляции, например, 5 метров в сторону этой точки. Затем возьмем другую точку и проедем в ее сторону.

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

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

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

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

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

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

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

Из нее он, следуя правилам, не сможем выбраться. Это шутка о том, что в жизни случается много ситуаций, в которых человек понимает, что делать. Здесь сплошная полоса, а впереди ремонт дороги. Рабочие не поставили все нужные знаки про объезд. Они просто поставили свой цементовоз, ломают асфальт. Человек поймет, что пересечения сплошной линии ничего страшного не случится — его, наверное, даже не оштрафуют. А вот с беспилотными автомобилями все сложнее.

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