Алгоритм планирования пути на местности

В проекте участвуют 10 автомобилей: 6 Toyota Prius, 3 Lexus RX460h и 1 Audi TT, а также 12 водителей и 15 инженеров.

В 2012 году компания Google сообщила в своём блоге о том, что их автомобили проехали уже более 480 тысяч километров с минимальным участием человека. Это позволило компании снизить экипаж автомобилей до одного человека. Так же было заявлено, что начались тестирования системы на участках со сложным рельефом. Это значительно расширяет возможности использования системы.

Недостатки системы.

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

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

 

 

Алгоритм выбора оптимального маршрута по пересеченной местности.

1.1. Модель местности 

Карта местности представлена в виде матрицы Z = z(y, x), где каждый элемент определяет максимальные затраты на преодоление участка с координатами (y, x). Участок местности, соответствующий элементу матрицы, считается квадратом со стороной d , правильно ориентированным вдоль координатных осей. Под координатами участка понимаются матричные индексы: y – номер строки матрицы, x – номер столбца. Максимальные затраты соответствуют пересечению участка по диагонали. Полагается, что затраты при пересечении вдоль любой координатной оси в 2 раз меньше максимальных.

При переходе с данного участка на смежный участок будем считать, что переход осуществляется из центра одного участка в центр другого. Затраты на переход равны среднему значению затрат по двум смежным участкам: z12 = (z1 + z2 ) 2 . Затраты на маршруте определяются суммой затрат по всем переходам между точками маршрута:

где N – число точек маршрута. При построении оптимального маршрута на каждом шаге необходимо выбирать направление дальнейшего движения. Результат выбора называется шаговым управлением и обозначается ck . Управление всей операцией состоит из совокупности шаговых управлений: C = c1, c2,…cm−1 .

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

C = c(y, x) и Q = q(y, x), а шаговое перемещение реализуется в пределах маски размером

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

 

1.2. Фильтрующий алгоритм

 

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

В предлагаемом алгоритме матрица накопленных затрат Q и матрица направлений C интерпретируются как двумерные изображения. Значение каждого элемента матрицы соответствует уровню яркости изображения. Алгоритм построения веера оптимальных траекторий реализуется как многоступенчатый пространственный параметрический ранговый фильтр минимума с маской размером 3×3 (рис. 1).

Рисунок 1 – Многоступенчатый фильтр оптимальных затрат

 

Рисунок 2 – Маска пространственного фильтра

 

Фильтрация производится над матрицами накопленных затрат Q = q(y, x) и матрицей шаговых управлений C = c(y, x). Матрица Z = z(y, x) определяет параметр фильтра для каждой точки (элемента) матриц Q и C . Веер траекторий строится от множества стартовых точек и распространяется по всем направлениям карты. Эффект пространственной фильтрации матриц эквивалентен ослаблению дуг в алгоритме Форда-Беллмана. Размещение фильтрующей маски в точке с координатами (y, x) показано на рис. 2. Число ступеней фильтра заведомо не определено, каждая ступень улучшает предыдущее решение. Матрица затрат Z содержит относительные затраты, приведенные к диапазону [0,1] ( 0 – минимальные затраты, 1 – максимальные затраты в пределах зоны). Непреодолимые области имеют значение ∞. Матрица может иметь необрабатываемые области, которые помечены символом NaN. Значение элемента матрицы z(y, x) используется как параметр пространственного фильтра в точке (y, x). Матрица накопленных затратQ = q(y, x) имеет размер матрицы Z и содержит следующие области:

− необрабатываемая область – точки помечены символом NaN. При выполнении

пространственной фильтрации эти точки пропускаются и не участвуют в обработке;

− стартовые точки (в пределе это может быть одна точка) определяют границу, от

которой распространяется волновой процесс накопления затрат. В начальном сос-

тоянии стартовые точки заполняются соответствующими значениями из матрицы

затрат Z ;

− точки, до которых волновой фронт еще не дошел – имеют значение ∞ . В про-

цессе выполнения алгоритма значение этих точек изменяется и в конце первой

ступени алгоритма содержит оценку минимальных затрат на движение от множества

стартовых точек к данной точке.

Рисунок 3 – Возможные варианты шаговых управлений

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

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

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

(рис. 3).

 

1.3. Реализация алгоритма

Ступень 1. На первой ступени фильтрации алгоритм распространяется от стартовых точек в виде волны. Фронт волны строится следующим образом. Множество стартовых точек рассматривается как начальный фронт волны. Маска позиционируется в стартовую точку карты и в матрицах затрат Z , Q просматриваются все точки, покрываемые маской. Точки, для которых значения в матрице Z равны ∞, а значения в матрице Q конечны, переносятся в список граничных точек. Просматриваются все стартовые точки и строится общий список. Далее список редактируется, из него удалятся повторяющиеся точки (редактирование может производиться и при смене позиции маски). Логические условия построения нового фронта имеют вид:

(1)

где координаты y и x принадлежат полю движущейся маски, y* , x* – координаты точек текущего фронта волны. Фильтрация выполняется для всего списка граничных точек. С этой целью маска позиционируется центром в граничную точку (y, x) и выполняется вычисление нового значения накопленных затрат по следующему правилу:

(2)

где j, i = 0, ±1 – индексы маски с ненулевыми значениями элементов. Полагается, что

 z00 (y, x)=0 . В центральных областях карты матрица фильтрующей маски имеет вид:

Матрица маски наполняется нулями при пересечении границы карты. Значение соответствующего элемента матрицы шаговых управлений устанавливается по позиции точки минимума в выражении (2):

(3)

 

 

 

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

 

Ступень 2 и последующие. Последовательно просматриваются все точки карты, для которых q(y, x) ≠ NaN и выполняется коррекция значений матриц Q и C, следуя формулам (2) и (3). Фильтрация заканчивается, когда матрицы Q двух смежных ступеней совпадают. Дополнительный эффект ускорения вычислений достигается за счет рекурсивного принципа фильтрации.

 

Построение оптимального маршрута. При построении маршрута возможны следующие варианты:

1. Стартовых точек несколько. Необходимо  проложить маршрут от ближайшей

стартовой точки к заданной конечной точке с координатами (yk , xk ). В этом случае

точки маршрута восстанавливаются по цепочке шаговых управлений:

 

2. Конечная точка не задана, но  определено множество, которому она принадлежит. В этом случае на заданном множестве необходимо найти точку, в которой q(y, x)→min и далее повторить алгоритм пункта 1.

 

3. Если заданы начальная и  конечная точки, то алгоритм необходимо  выпол-

нить для начального фронта, состоящего из одной стартовой точки.

 

Построение фронта транспортной доступности. В матрице накопленных затрат ищутся точки, удовлетворяющие условию: q ( y, x) ≈ L, где L – уровень затрат на линии фронта. Степень приближения может быть задана, например, в процентах от уровня L .

 

 

 

2. Алгоритм оптимального  движения в транспортной сети

 

2.1. Модель транспортной  сети

 

Транспортная сеть (рис. 4) представляется топологической моделью в виде взвешенного графа G. Ребра графа соответствуют однородным сегментам транспортной сети, а узлы – точкам ветвления или иным точкам интереса (стартовым точкам, конечным точкам, точкам сети, ближайшим к заданному объекту и т.д.). Каждому ребру графа поставлено в соответствие значение временных затрат на преодоление сегмента. Сегментные затраты определяются геометрической длиной сегмента и средней скоростью движения объекта вдоль сегмента.

Постановка задачи по прокладке маршрута в транспортной сети не отличается от задачи маршрутизации по пересеченной местности.

 

2.2. Описание алгоритма

 

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

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

 

2.3. Реализация алгоритма

 

Взвешенный граф транспортной сети задается квадратной матрицей затрат Z = z(i, j). Размер матрицы определяется числом вершин в графе. Каждый элемент матрицы представляет собой затраты на перемещение между смежными вершинами. В общем случае матрица может быть не симметрична ( z(i, j) ≠ z( j,i)). Если вершины не являются смежными, то значение элемента матрицы равно ∞ . Кроме того элемент матрицы может иметь 0, если i = j или значение NaN – такой сегмент не участвует в построении оптимального маршрута. Накопленные затраты сохраняются в массиве Q. Каждый элемент массива соответствует вершине графа. Первоначально всем элементам массива Q присваивается значение ∞ . Элементу массива Q присваивается значение NaN, если все сегменты окружения для соответствующей вершины графа имеют значение NaN.

 

Ступень 1. На первой ступени алгоритма накопление затрат происходит в виде волны от стартовых точек. Фронт волны строится следующим образом. Множество стартовых точек рассматривается как начальный фронт волны. Алгоритм фокусируется в стартовой точке, в матрице весов Z и массиве накопленных затрат Q. Просматриваются все сегменты и узлы ближайшего окружения. Точки, для которых значения в массиве Q равны ∞ , и при этом соответствующие значения в матрице сегментных затрат Z конечны, переносятся в список граничных точек. Просматриваются все стартовые точки и строится общий список. Далее список редактируется, из него удалятся повторяющиеся точки. Логическая формула построения

нового фронта имеет вид:   i∈ Front если{q(i) = ∞ & z(i, j) ≠ ∞, NaN}. (4)

 

Фильтрация первой ступени. Алгоритм фокусируется в граничную точку i и выполняется вычисление нового значения накопленных затрат, следуя правилу:

  (5)