Сенсорные системы, 2021, T. 35, № 2, стр. 164-174

ВВЕДЕНИЕ

Задача планирования пути является актуальной, даже несмотря на большое количество проведенных исследований в данной области. Особую значимость она получила в связи со стремительным развитием высокоавтоматизированных транспортных средств (ВАТС). Условия эксплутации ВАТС накладывают высокие требования на свойства алгоритмов, основные из них – высокая скорость вычислений и безопасность вычисленных путей. Поскольку вычислению пути предшествует распознавание объектов и анализ дорожной ситуации (Кунина и др., 2018; Vishal et al., 2019; Лобанов, Шоломов, 2019), временные ограничения на работу алгоритмов планирования становятся еще серьезнее (Gonzalez et al., 2015). Помимо этого, к планируемому пути может предъявляться и ряд дополнительных требований, например, он должен быть наикратчайшим, а в случае перевозки людей будет цениться комфорт поездки, который достигается путем минимизации первой и второй производной скорости движения. Множественность выдвигаемых требований сводит задачу планирования пути к задаче поиска оптимального решения по множеству критериев.

Задачу планирования пути принято делить на две подзадачи – глобальное и локальное планирование. Глобальный путь строится на основе заранее известной карты, соединяя начальную точку с конечной. Локальный путь позволяет транспортному средству двигаться вдоль глобального пути, принимая во внимание поступающую в реальном времени информацию, полученную от сенсоров ВАТС. Необходимость разделения задачи продиктована большой вычислительной сложностью поиска глобального пути и, как следствиe, невозможностью работы лежащих в основе алгоритмов поиска в режиме реального времени. Для быстрого принятия решений о маневрировании с целью избегания столкновений с препятствиями применяется локальное планирование.

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

Сильный толчок для развития в области планирования пути для ВАТС дало проведение соревнований Darpa Grand Challenge. Во время проведения соревнований в 2005 г. перед началом испытаний участникам выдавался файл, в котором хранилась последовательность точек – глобальный путь (Thrun et al., 2006). Для победы в соревновании было необходимо проехать вдоль глобального пути быстрее остальных команд, не выехав за пределы трассы. Все препятствия, которые могли встретиться на маршруте, были статичными. В 2007 г. соревнования проходили в воссозданных городких условиях, поэтому приходилось справляться не только со статичными препятствиями, но и избегать столкновений с другими участниками дорожного движения. Для решения этой задачи участниками были придуманы различные подходы. Так, например, команда из Массачусетского технологического института (MIT) использовала модифицированный метод быстро растущих случайных деревьев (Kuwata et al., 2008). Главным недостатком предложенного подхода было введение “ленивой проверки”. На каждой итерации работы алгоритма осуществлялся рост дерева, однако он происходил не с нуля, а на основе дерева, посчитанного на предыдущей итерации. Решение, которое было лучшим в предыдущий момент времени, могло стать наихудшим на текущей итерации. “Ленивая проверка” лишь контролировала соответствие этого решения всем накладываемым ограничениям, но не позволяла для текущей итерации найти лучшее решение. Введение такой особенности было необходимо для освобождения вычислительных ресурсов под рост дерева – увеличивение пространства поиска.

Со времен проведения Darpa Grand Challenge фокус исследователей в области планирования пути ВАТС был нацелен на разработку алгоритмов, позволяющих находить оптимальный по множеству критериев путь в режиме реального времени для маневрирования в динамической среде (Gonzalez et al., 2015). Разработанные подходы на основе численной оптимизации (Ziegler et al., 2014; Rosmann et al., 2012) потенциально способны найти наилучшее решение и сойтись к глобальному экстремуму. Однако они вычислительно сложны, а в условиях ограниченного времени найденное решение может оказаться далеко от оптимального. Конкуренцию подходам на основе численной оптимизации составляют подходы, основанные на поиске пути в графе. Их основное преимущество – скорость вычислений. Такие методы способны найти оптимальное решение, но лишь в рамках предпосчитанного графа переходов в пространстве состояний ВАТС (так же известном как пространственная решетка). Переходы вычисляются на этапе инициализации алгоритма, затем сохраняются в память и используются на этапе планирования в режиме реального времени для сокращения затрат на формирование графа. Примеры таких методов подробно описаны в работах (Pivtoraiko, Kelly, 2005; Ziegler, Stiller, 2009; Pothan et al., 2017).

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

АЛГОРИТМ ЛОКАЛЬНОГО ПЛАНИРОВАНИЯ ПУТИ НА ОСНОВЕ МНОГОКРИТЕРИАЛЬНОЙ ОПТИМИЗАЦИИ ТРАЕКТОРИИ В ПРОСТРАНСТВЕННОЙ РЕШЕТКЕ СОСТОЯНИЙ

В этой главе приводится детальное описание предложенного алгоритма планирования пути. Отдельно рассматривается процесс построения графа переходов – пространственной решетки и многокритериальной оптимизации пути в полученном графе.

ПОСТРОЕНИЕ ПРОСТРАНСТВЕННОЙ РЕШЕТКИ

Предложенный алгоритм планирования пути взаимодействует с моделью окружающей среды, которая содержит статичное представление среды в каждый момент времени на рассматриваемом горизонте планирования. Одним из входов предложенного алгоритма является ближайший к ВАТС отрезок глобального пути – отрезок пути, который должен быть пройден на некотором горизонте планирования. Множество потенциальных локальных путей строится вдоль глобального пути. В момент инициализации алгоритма строится шаблон возможных маневров в форме направленного невзвешенного графа. Вершина такого графа представляет собой точку в криволинейной системе координат (Werling et al., 2010) и описывается двумя координатами – продольным смещением l вдоль глобального пути и поперечным смещением r относительно него. Значение продольной и поперечной координат дискретны с заданным по каждой из осей шагом (${{\Delta }}l,{{\Delta }}r$). Вершины с одинаковым значением продольного смещения будем называть слоем (рис. 1). Ребра в шаблоне соединяют вершины первого слоя со следующими слоями с заданными индексами. Ребра представляют из себя путь, по которому происходит движение ВАТС – маневр. Маневры задаются полиномами третьей степени, что позволяет найти кривизну ребра в любой его точке аналитически и исключить ребра, не удовлетворяющие кинематическим ограничениям ВАТС. На рис. 2, а представлен пример набора маневров, в котором вершины первого слоя соединяются только со следующим, на (б) приведен пример шаблона, в котором вершины первого слоя соединяются с вершинами из 2, 3, 5 слоев. Последовательность индексов слоев задается исходя из параметров ВАТС, доступных вычислительных мощностей и специфики решаемой задачи. Увеличение количества слоев в задаваемой последовательности приведет к возрастанию количества ребер и, соответственно, увеличит время вычислений, но позволит приблизиться к оптимальному пути (глобальному оптимуму). Помимо этого, более длинные ребра позволяют находить более гладкие маневры, позволяющие поддерживать высокую скорость при движении по ним, а короткие – выполнять резкие перестроения. Включение в шаблон ребер различной длины увеличивает пространство поиска и позволяет более оптимально выполнять объезды препятствий различных размеров. Локальный путь, представленный на рис. 3, а, построен на основе шаблона маневров, в котором вершины первого слоя соединяются с вершинами только второго слоя. Объезд препятствий осуществляется путем последовательного выполнения коротких маневров, что приводит к излишнему рулению (в). Локальный путь, представленный на (б), построен на основе шаблона, в котором вершины первого слоя соединялись с вершинами 2, 3, 5, 7 слоев, тем самым образуя набор из коротких, средних и длинных ребер. Путь более гладкий, а сумма модулей изменения угла ориентации ВАТС при движении по пути уменьшилась в 2 раза (г) по сравнению со случаем, когда переход был возможен только между соседними слоями.

Рис. 1.

Пример определения координат точки ${{P}_{1}}$ в криволинейной системе координат. Точки ${{P}_{2}}$ – ${{P}_{{11}}}$ образуют два слоя.

Рис. 2.

Шаблоны возможных маневров.

a – продольный шаг ${{\Delta }}l$ между слоями 2 м, поперечный шаг ${{\Delta }}r$ между вершинами в одном слое 0.5 м, вершины первого слоя соединяются с вершинами только второго слоя, минимальный радиус разворота ВАТС 1 м, максимальное поперечное смещение вершин 1 м; б – продольный шаг ${{\Delta }}l$ между слоями 2 м, поперечный шаг ${{\Delta }}r$ между вершинами в одном слое 0.5 м, вершины первого слоя соединяются с вершинами из 2, 3, 5 по счету слоев, минимальный радиус разворота ВАТС 1 м, максимальное поперечное смещение вершин 2 м.

Рис. 3.

Влияние длины маневров на локальный путь и его характеристики.

а – локальный путь (зеленый), построенный на основе шаблона, содержащего только короткие маневры (фиолетовый) для перехода между соседними слоями; б – путь, построенный на основе шаблона, содержащего как длинные, так и короткие маневры. На графиках (в и г) приведены характеристики каждого из путей. Для верхних графиков: зеленый и красный – расстояние от глобального и локального путей соответственно до ближайших препятствий, синий – расстояние между глобальным и локальным путями. Для нижних графиков: зеленый и красный – относительное изменение угла поворота ВАТС при движении по глобальному и локальному путям соответственно, синий и фиолетовый – сумма модулей изменения угла поворота ВАТС при движении по глобальному и локальному путям соответственно.

Построенный шаблон возможных маневров в криволинейной системе координат используется для получения множества потенциальных путей. Шаблон “дублируется” вдоль глобального пути в криволинейной системе координат с заданным шагом продольного смещения. Затем все вершины и ребра переводятся из криволинейной системы координат в декартову. После перевода в декартову систему координат кривизна маневров изменяется, поскольку сам глобальный путь обладает кривизной. Поэтому маневры в шаблоне должны обладать запасом по кривизне, размер которого зависит от максимальной кривизны глобального пути. Первой вершиной графа является текущее положение транспортного средства. Ребра, соединяющие первую вершину с первым слоем графа, строятся на основе полиномов пятой степени (Takahashi et al., 1989) и соединяются только с теми вершинами, переход в которые удовлетворяет кинематическим ограничениям и не приведет к столкновению. В последнем слое графа находится всего одна вершина, которая лежит строго на глобальном пути на расстоянии от автомобиля, равному горизонту планирования (рис. 4).

Рис. 4.

Граф возможных переходов в декартовой системе координат, составленный на основе шаблона маневров в криволинейной системе координат. Шаблон включает как длинные, так и короткие ребра.

ПОИСК ОПТИМАЛЬНОГО ПУТИ

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

Критерий удаленности ребра от ближайшего препятствия считается по следующей формуле: ${{w}_{{safe}}}\left( {{{E}_{i}}} \right) = \sum\nolimits_{n = 1}^N f \left( {{{e}_{n}}} \right)$. Для оценки ${{w}_{{safe}}}$ производится деление ребра на сегменты ${{e}_{n}}$. Количество сегментов определяется как продольная длина ребра $L\left( {{{E}_{i}}} \right)$, деленная на продольный шаг шаблона $N = \frac{{L\left( {{{E}_{i}}} \right)}}{{{{\Delta }}l}}$. Для каждого сегмента проводится поиск расстояния между ребром и ближайшим препятствием. На рис. 5 представлена функция безопасности $f\left( e \right)$, которая определяет зависимость веса от минимального расстояния между ВАТС и препятствиями. Функция является квадратичной, поскольку, чем ближе ВАТС проходит к препятствию, тем опаснее двигаться по этому сегменту пути. Если расстояние больше радиуса инфляции, вес критерия безопасности равен нулю – ребро графа не штрафуется.

Рис. 5.

Функция безопасности $f\left( e \right)$.

Критерий удаленности ребра от глобального пути: ${{w}_{{dist}}}\left( {{{E}_{i}}} \right) = L\left( {{{E}_{i}}} \right)dist\left( {{{E}_{i}}} \right)$. Введем следующее обозначение: ${{r}_{{from}}}$ и ${{r}_{{to}}}$ – поперечное смещение начала ребра и конца ребра соответственно, тогда $dist\left( E \right) = \left( {\left| {{{r}_{{from}}}\left| {\; + \;} \right|{{r}_{{to}}}} \right|} \right) \times 0.5$ Чем дальше от глобального пути находится ребро, тем больший штраф на него накладывается.

Критерий изменения поперечного положения ${{w}_{{man}}}\left( {{{E}_{i}}} \right) = \frac{{\left| {{{r}_{{from}}} - {{r}_{{to}}}} \right|}}{{L\left( {{{E}_{i}}} \right)}}$. Данный критерий накладывает штраф на ребра, меняющие поперечное положение транспортного средства, позволяя сократить излишнее маневрирование.

Таким образом, задача поиска оптимального локального пути сводится к поиску пути с минимальным весом во взвешенном направленном графе. Значение коэффициентов ${{k}_{{man}}}$, ${{k}_{{dist}}}$, ${{k}_{{safe}}}$ должно быть задано, исходя из предположений о том, какой вклад он должен вносить в общий вес ребра. Чем больше его значение, тем более значим соответствующий критерий.

ЭКСПЕРИМЕНТАЛЬНАЯ ОЦЕНКА АЛГОРИТМА

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

ПОДГОТОВКА ТЕСТОВЫХ СЦЕН

Эксперименты для проверки алгоритма заключаются в вычислении локального пути объезда статичных препятствий транспортным средством для каждой сцены из заданного набора тестовых сценариев. Сцена состоит из априорной локальной карты проходимости (Shvets et al., 2016) с масштабом 10 пикселей на метр, глобального пути, положения ВАТС в начальный момент времени. При создании сцен использовались реальные участки городских дорог с соблюдением пропорций и размеров с точностью до масштаба карты. Транспортное средство аппроксимируется многоугольником, размеры которого приближены к размерам среднего легкового автомобиля (ширина среднего легкового автомобиля 1.65 м, длина 4 м). На карты проходимости (рис. 6) были добавлены различные часто встречающиеся на дорогах объекты (легковые и грузовые авто, пешеходы, велосипедисты). Размеры препятствий соответствуют реальным. Расположение препятствий, глобальный путь и стартовое положение транспортного средства задавались вручную.

Рис. 6.

Дискретизированная и приведенная к черно-белому изображению проекция. а – силуэт идущего человека; б – легкового автомобиля; в – велосипедиста; г – автобетоносмесителя .

Всего было создано 11 сцен, приближенных к реальным дорожным сценам, представленных на рис. 7, а–е и рис. 8, а–д. В тестовый набор были включены как сложные сцены, предполагающие объезд одного или нескольких препятствий, так и сцены без препятствий. Такие сцены предназначены для проверки свойства алгоритма локального планирования генерировать путь, который в соответствии с введенным критерием оптимальности (1) не отклоняется от глобального пути при отсутствии препятствий.

Рис. 7.

Визуализация результатов экспериментов. На всех изображениях коричневая линия – глобальный путь, зеленая – локальный путь, желтый треугольник указывает на ориентацию транспортного средства, а его начальное положение совпадает с первой точкой глобального пути, ${{k}_{{safe}}} = 7$, ${{k}_{{dist}}} = 0.14$, ${{k}_{{man}}} = 2$.

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

Рис. 8.

Визуализация результатов экспериментов. Условные обозначения аналогичны рис. 7.

а и б – проезд перекрестков; в – объезд аварии с последующим движением по прямой; г – объезд аварии; д – тестовая сцена без препятствий.

АНАЛИЗ РЕЗУЛЬТАТОВ

Цель проведения экспериментов – показать, что описанный алгоритм находит локальный путь, который позволит избежать столкновений с препятствиями в статичной среде с незначительным увеличением проходимого расстояния по сравнению с движением по глобальному пути. В десяти из одиннадцати тестовых сцен движение по глобальному пути приводило к столкновению, а движение по локальному пути объезда позволило избежать их полностью. При этом локальный путь объезда стал длиннее в среднем всего на 1.3% по сравнению с глобальным путем. Найденные пути удовлетворяют кинематическим ограничениям, а за счет введения длинных ребер переходы между вершинами становятся более плавными, что делает возможность их прохождения на высокой скорости. Еще для одной сцены построить безопасный путь объезда не удалось из-за большой плотности препятствий (рис. 7, г). Локальный путь не может быть найден, если отсутствует хотя бы один путь в направленном графе между начальной вершиной, соответствующей положению ВАТС, и конечной – последней точкой глобального пути.

Также был проведен анализ влияния весовых коэффициентов (1) на генерируемый путь. Для проведения эксперимента был выбран тестовый сценарий объезда препятствий (рис. 8, г). В эксперименте каждый из весовых коэфициентов целевой функции поочередно приравнивался к нулю. Результаты представлены на рис. 9, а, б. Синим цветом обозначен локальный путь, для которого все значения коэффициентов были ненулевыми. Данный путь построен в качестве референсного. На основе полученных результатов могут быть сделаны следующие выводы:

Рис. 9.

Влияние весовых коэффициентов на форму локального пути. а – зависимость форм локальных путей от значений весовых коэффициентов; б – различные показатели качества локального и глобального путей. Для верхних рядов графиков: зеленый и красный – расстояние от глобального и локального путей соответственно до ближайших препятствий, синий – расстояние между глобальным и локальным путями. Для нижних рядов графиков: зеленый и красный – относительное изменение угла поворота ВАТС при движении по глобальному и локальному путям соответственно, синий и фиолетовый – сумма модулей изменения угла поворота ВАТС при движении по глобальному и локальному путям соответственно.

• Обнуление коэффициента ${{k}_{{man}}}$ привело к более резкому маневрированию (результаты обозначены зеленым цветом). Особенно хорошо это заметно на участках пути с 7.5 по 15 м (на референсном изображении маневры происходят с 5 по 15 м; указанные маневры выделены оранжевым цветом на обоих изображениях), при этом отклонение от глобального пути не изменилось по сравнению с референсным.

• Обнуление ${{k}_{{safe}}}$ (графики, обозначенные желтым) привело к строгому следованию глобального пути, поскольку он сам по себе удовлетворяет критерию безопасности. В общем случае нулевое или малое значение коэффициента безопасности может стать причиной построения локального пути вблизи препятствий.

• Обнуление ${{k}_{{dist}}}$ (графики обозначены голубым) привело к увеличению отдаленности локального пути от глобального. На приведенном графике наблюдается увеличение суммарного изменения ориентации ВАТС. На 30-метровом пути оно составило почти три радиана (для референсного пути это значение чуть больше двух радиан).

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

ЗАКЛЮЧЕНИЕ

В работе предложен алгоритм локального планирования на основе поиска оптимального пути в направленном взвешенном графе. Граф получается путем построения пространственной решетки – повторяющегося шаблона кинематически достижимых маневров, предпосчитанных на этапе инициализации алгоритма. Предложенный в работе способ соединения не только соседних, но и отстоящих друг от друга слоев графа, позволяет находить более сглаженные, кинематически достижимые пути, оставляя при этом возможность совершать резкие маневры для объезда динамических препятствий, возникающих на пути следования ВАТС. После построения графа предложенный алгоритм осуществляет поиск наилучшего пути. Наилучшим признается путь, ребра, входящие в состав которого, минимизируют заданный функционал качества. Многокритериальность применяемого функционала, с одной стороны, гарантирует удаленность пути от препятствий, а с другой – позволяет минимизировать излишнее маневрирование. Предложенный алгоритм был протестирован на сценариях, имитирующих реальные дорожные ситуации. Алгоритм локального планирования позволил избежать столкновения во всех представленных сценариях, в которых безопасный путь мог быть найден. Полученные локальные пути оказались в среднем всего на 1.3% длиннее априорно заданного глобального пути, не учитывающего наличие других участников дорожного движения, что подтверждает эффективность алгоритма. Дальнейшая работа по улучшению алгоритма может быть направлена на улучшение проверки кинематических ограничений после перевода ребер в декартову систему координат. Также работа может быть направлена на сравнение алгоритма с другими существующими подходами к локальному построению пути для выявления потенциальных слабых сторон алгоритма и поиска путей его дальнейшего усовершенствования.