Это первый проект третьего семестра Нанодегрита Udacity Self-Driving Car Engineer. Вы можете найти весь код, связанный с этим проектом, на github. Вы также можете прочитать мои сообщения о предыдущих проектах:

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

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

  • Никаких столкновений с другими транспортными средствами
  • Максимальная скорость 50 миль / ч (~ 80 км / ч)
  • Максимальное ускорение 10 м / с²
  • Максимальный рывок 10 м / с³
  • Автомобиль не может находиться между полосами движения более 3 секунд
  • Автомобиль не может выезжать за пределы трех полос шоссе
  • Автомобиль не может двигаться по неправильной стороне шоссе

У нас было много проблем с завершением этого проекта, и на гифке ниже показаны некоторые из наших ранних нарушений ограничений.

Для начала давайте подробнее рассмотрим различные уровни, участвующие в планировании пути.

Функциональные слои в самоуправляемом автомобиле

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

  • Контроль движения: отвечает за движение автомобиля и максимально точное следование эталонной траектории. Этот слой работает с самой быстрой шкалой времени.
  • Sensor Fusion: отвечает за объединение выходных сигналов датчиков (например, RADAR + LIDAR).
  • Локализация: отвечает за понимание с максимально возможной точностью, где находится наш автомобиль на карте и где находятся другие объекты (например, другие автомобили) по отношению к нашему автомобилю.
  • Прогнозирование: отвечает за идентификацию сущностей, обнаруженных датчиками (также известную как восприятие), а также за прогнозирование ближайших будущих изменений в сцене на основе автомобиля. текущая траектория, траектории других транспортных средств и различные элементы на сцене (например, светофоры). Одна из важных задач этого слоя - также предвидеть столкновения.
  • Поведение: координирующий уровень, который принимает всю информацию с нижних уровней и определяет будущее состояние, к которому нужно перейти, а также траекторию, которую следует принять.
  • Траектория: отвечает за вычисление траекторий с учетом набора ограничений (например, скорости, расстояния, полосы движения, рывка и т. д.)

Существует много подходов к генерации траекторий, и в этом проекте мы выбрали расчет траекторий в системе координат Frenet.

Анализ данных Sensor Fusion

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

В нашем случае симулятор предоставляет следующую информацию от своего сенсорного модуля:

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

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

Генерация траектории

Система координат Френета

Мы часто используем традиционную декартову систему координат для представления заданной точки (x, y) на плоскости, и это фактически система по умолчанию, используемая в симуляторе для идентификации нашей машины на дороге. Однако в реальном мире дороги не всегда прямые, и поэтому «простые» операции, выполняемые людьми, такие как определение полосы движения, на которой находится машина, гораздо труднее воспроизвести с помощью декартовой системы для компьютера. На изображении ниже показана проблема, с которой мы столкнулись в традиционной системе координат (X, Y):

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

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

Минимизация рывков

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

Оказывается, относительно легко вычислить минимальную траекторию рывка в одном измерении, расширив кинематическое уравнение для вычисления траектории с учетом нашего текущего положения s_0, текущей скорости s_0_v, и текущее ускорение s_0_a. Для заданного периода времени T (например, 1 секунда) , мы можем выразить:

  • желаемое конечное положение (в момент времени t) s_f
  • желаемая конечная скорость s_f_v
  • желаемое конечное ускорение s_f_a

с использованием пятерых многочленов (т.е. степени 5), которые восходят ко второй производной от jerk. Уравнения относительно начальных значений выглядят следующим образом:

Значения в левой части знака равно - это прогнозируемые значения положения, скорости и ускорения в момент времени t ≤ T. В нашем случае для t задана частота обновления контроллера, которая составляет 20 миллисекунд (0,02 секунды). Мы можем включить все это в решатель полиномов, задав соответствующие граничные условия. В нашем случае мы установили желаемое ускорение равным 0, так как мы хотели бы уменьшить рывок. К сожалению, это устанавливает только окончательное ускорение для t = T, и мы не можем контролировать ускорение транспортного средства, когда t ≠ T. Таким образом, нам необходимо поэкспериментировать с различными значениями T, чтобы определить, какой временной горизонт выбрать для создания траекторий с минимальным рывком. В нашем случае мы выбрали T = 1,7 секунды. Другая проблема заключается в том, что у нас есть идеальный контроллер, который будет перемещать транспортное средство в любую следующую точку траектории, независимо от законов физики (например, может переместиться на 1 км дальше за 20 миллисекунд), поэтому мы необходимо быть очень бдительными в отношении предлагаемых нами траекторий.

Поскольку мы используем координаты Френе, мы можем генерировать одномерные минимальные траектории рывков в измерениях s и d отдельно. Эта статья Верлинга и Каммеля представляет собой хорошее прочтение для более глубокого ознакомления с этой темой.

Поскольку симулятор не принимает траектории, выраженные в координатах Френе, мы преобразуем из координат Френе обратно в координаты реального мира, чтобы вычислить точку (x, y), которая соответствует заданному (s, d ) Точка Френе.

Улучшение локализации

Создание более плавных траекторий

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

К сожалению, путевые точки на карте, которые нам дают, довольно редки и могут привести к очень «угловым» траекториям, генерируемым, когда мы пытаемся преобразовать из Френета обратно в координаты реального мира. Это, в свою очередь, вызывает резкие скачки ускорения и рывки. Поскольку функция toRealWorld (s, d) - ›(x, y) использует базовую интерполяцию между двумя путевыми точками, чтобы найти наилучшие приблизительные значения для x и y, мы всегда рискуем сгенерировать не- плавная траектория.

Что мы можем сделать, чтобы улучшить это? Из некоторых проектов в предыдущих терминах мы видели, что линии, полученные из полинома, имеют тенденцию создавать очень гладкие траектории. Поэтому мы должны использовать этот метод вместо базовой интерполяции, которая используется в настоящее время. Мы прибегаем к использованию сплайнов, созданных путем взятия позиции s в координатах Френе, чтобы получить реальные координаты x, y и смещения dx и dy. Затем мы подставляем эту формулу, чтобы получить ближайшие реальные координаты

x = spline_s_x(s) + d * spline_s_dx(s)
y = spline_s_y(s) + d * spline_s_dy(s)

Теперь мы видим, насколько удивительно стала наша траектория.

Двумерный конечный автомат

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

В нашем случае наш конечный автомат довольно прост и проиллюстрирован ниже:

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

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

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

Функции затрат

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

Все наши функции затрат соответствуют следующему интерфейсу, который мы определили в файле cost_functions.h в репозитории проекта:

typedef function<double (const Vehicle&, const vector<Vehicle>&,  const Trajectory&, const State&, const double&)> CostFunction;

Это делает добавление функции стоимости очень тривиальным (вес является последним параметром). Мы определили следующие функции стоимости, вес которых полностью регулируется:

  • speedCostFunction: функция, которая наказывает наш автомобиль, если он едет медленно.
  • centerOfLaneDistCostFunction: функция, которая наказывает наше транспортное средство, если его траектория заканчивается за пределами центра целевой полосы.
  • laneChangeCostFunction: функция, которая всегда наказывает смену полосы движения, поскольку это обычно более опасно, чем движение по той же полосе.
  • distanceToClosestCarAheadCostFunction: функция, которая наказывает нашу машину как можно ближе к машине впереди.
  • distanceToClosestCarAheadFutureLaneCostFunction: функция, которая штрафует транспортное средство, если оно слишком близко к транспортным средствам впереди или сзади при смене полосы движения.
  • averageLaneSpeedDiffCostFunction: функция, которая штрафует полосу, по которой автомобиль хочет ехать, на основе средней скорости транспортных средств впереди по этой полосе.
  • speedDifferenceWithClosestCarAheadCostFunction: функция стоимости, которая наказывает наше транспортное средство, если оно движется медленнее, чем впереди идущий.
  • collisionTimeCostFunction: функция, которая штрафует нашу траекторию, если это приводит к потенциальному столкновению с другим транспортным средством.
  • futureDistanceToGoalCostFunction: функция, которая штрафует нашу траекторию по мере того, как она продвигается к цели (т. е. конец трека на s = 6945,554).

Мы пробовали много различных конфигураций веса, но в конечном итоге решили, что, поскольку мы не хотели идти на компромисс в отношении безопасности, мы приписываем collisionTimeCostFunction самый высокий вес с 10000. Веса других функций стоимости сильно различаются, но мы дали, например, очень мало weight в speedCostFunction, поскольку высокая скорость - это хорошо, но далеко не так критично, как отсутствие столкновений.

Окончательные результаты

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

Вы также можете посмотреть видео полного круга, загруженное на YouTube:

Улучшения

Текущий планировщик пути относительно консервативен и не оптимизирован для максимальной скорости. Это означает, что, хотя скорость иногда может достигать 48 миль в час (~ 77,2 км / ч), она обычно остается ниже этой скорости и почти никогда не достигает допустимого ограничения скорости в 50 миль в час (~ 80,4 км / ч). На данный момент мы с радостью согласимся на такой компромисс, но необходимо провести работу, чтобы автомобиль мог двигаться со скоростью, близкой к 50 миль в час.

Более того, планировщик учитывает только прилегающую полосу движения транспортного средства и, следовательно, никогда не «видит», когда несмежная полоса была бы лучшим выбором (например, автомобиль находится на полосе 1, а планировщик оценивает только полосы 1 и 2, в то время как дорожка 3 находится на полосе движения). крайний правый может быть свободен и, следовательно, является хорошим кандидатом для движения). Это потребует более сложного метода оценки пути, при котором планировщик оценивает все полосы и в конечном итоге решает, на какую соседнюю полосу двигаться, исходя из того факта, что полоса рядом с нашей соседней полосой станет жизнеспособным кандидатом для перехода, как только транспортное средство достигнет нужной полосы. соседний переулок. Другая проблема, которую мы испытываем в связи с этим подходом, связана с безопасностью, поскольку выполнить такой смелый шаг опаснее и сложнее из-за бокового расстояния, которое необходимо преодолеть, а также сложности прогнозирования поведения других транспортных средств на дороге. Дорога.

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

Наконец, наш текущий планировщик генерирует только одну траекторию для данного возможного следующего состояния, что означает, что мы могли бы игнорировать лучшие траектории для того же будущего состояния (например, дальше / позади или больше слева или справа от данной полосы) . У нас есть реализация для такой схемы нескольких траекторий для каждого состояния, которая предполагает, что все конечные позиции Френе s и d (полученные путем вычисления минимальной траектории рывка) следуют гауссовскому распределению с данные средние и стандартное отклонение (mean_s, std_s) и (mean_d, std_d) для распределений G (s) и G (d) соответственно. Однако мы должны выбрать подходящие значения для стандартных отклонений, в то время как средние останутся фиксированными в исходных желаемых конечных положениях end_s и end_d.

Благодарности

До сих пор это был, без сомнения, самый сложный проект, который я предпринял в рамках этой наностепени Self-Driving Car за все три семестра. Я не был уверен, что смогу когда-нибудь его завершить, и даже не знал, с чего именно начать!

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

Более того, мне особенно понравились и вдохновили статьи Митхи об этом проекте, а также сообщения других студентов. Наконец, я благодарен всей команде Udacity и их партнерам из Daimler за то, что они создали такой отличный контент и сложный проект!