Оптимизация траектории перемещения объекта в пространстве с препятствиями на основе модифицированного направленного волнового алгоритма

УДК 621.87:621.865.8:51.74

В.С. Щербаков, М.С. Корытов

ОПТИМИЗАЦИЯ ТРАЕКТОРИИ ПЕРЕМЕЩЕНИЯ ОБЪЕКТА В ПРОСТРАНСТВЕ С ПРЕПЯТСТВИЯМИ НА ОСНОВЕ МОДИФИЦИРОВАННОГО НАПРАВЛЕННОГО

ВОЛНОВОГО АЛГОРИТМА

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

Направленный волновой алгоритм, планирование оптимальной траектории, угловые координаты

V.S. Shcherbakov, M.S. Korytov

OPTIMIZING THE TRAJECTORY OF OBJECTS ADVANCING WITHIN THE OBSTRUCTIONS IN SPACE USING THE MODIFIED DIRECTED WAVE ALGORITHM

The article presents a new pattern of the directed wave algorithm which allows a research into an optimum trajectory of objects within the three-dimensional space having obstacles set in a discrete form, and taking into account the angular orientation.

Directional wave algorithm, planning the optimal trajectory, angular coordinates

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

хорошо зарекомендовали себя универсальные дискретные методы и алгоритмы, которые позволяют эффективно решать ряд оптимизационных задач [1-3].

Волновой алгоритм, основанный на алгоритме поиска на графе в ширину, характеризуется максимальной по сравнению с другими алгоритмами поиска простотой реализации и надежностью [1-3]. Это детерминированный алгоритм со строго заданной последовательностью действий, который гарантированно находит оптимальное решение за определенное конечное число шагов, меньшее или равное некоторому предельному значению. Это также можно считать достоинством алгоритма. Однако ему свойственны некоторые ограничения и недостатки: прежде всего это экспоненциальная временная и пространственная сложность данного алгоритма 0(Ьй)(Ь - число узлов графа, й - глубина решения), не позволяющая практически использовать его при больших размерах графа задачи. Понятие больших размеров определяется быстродействием вычислительной техники на момент проведения исследований.

Алгоритм в общем случае осуществляет поиск во все стороны от начального узла графа, при этом обрабатывается очень много лишних узлов, заведомо не содержащих оптимальный путь. Поиск в ширину является оптимальным, только если веса всех ребер графа равны, т.е. необходимо рассматривать граф с единичными стоимостями всех дуг [1, 2]. Если веса различных ребер не равны, может быть использован поиск по критерию стоимости, являющийся модификацией поиска в ширину. Однако данная модификация, которая требует вместо развертывания самого поверхностного узла развертывания узла с наименьшей стоимостью пути, в худшем случае имеет временную и пространственную сложность намного большую, чем поиск в ширину [1, 3]: 0(Ь 1+£) (с - стоимость оптимального решения, £ - некоторое положительное значение). Это объясняется, во-первых, вычислительными издержками на поддержание приоритетной очереди узлов, во-вторых тем, что долго может выполняться проверка больших деревьев, состоящих из множества дуг малой стоимости, но не содержащих оптимальных путей [1, 3].

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

Постановка задачи. Заданы линейные и угловые координаты перемещаемого объекта в начальной янач и конечной $кон точках траектории объекта в пространстве:

^нач = (хн 0’ Ун 0’ %н 0,Ун0,Он0 ) ; ^ кон = [Хк 0’ У к 0’ ^к0,Ук0,Ок0 ) ,

где хн0, ун0, 0 - линейные координаты точки начала локальной прямоугольной системы коорди-

нат объекта 0gХв неподвижной прямоугольной системе координат 00Х0¥0Z0, связанной с рабочей областью перемещений, соответствующие начальному положению объекта; хк0, ук 0, гк 0 -линейные координаты, соответствующие конечному положению объекта; ^н0, н 0- координаты углов поворота объекта вокруг осей Х , ¥8 в начальной точке; ук0, (Ок0 - угловые координаты, соответствующие конечному положению объекта в пространстве.

Ось X0 неподвижной системы координат направлена параллельно линии, соединяющей две

точки в пространстве: начальную $нач и конечную $кон точки траектории. Это позволяет упростить расчет и уменьшить объем вычислений.

Линейные и угловые координаты объекта заданы на равномерной сетке: г е [1; гтах ];

3 е [1; 3тах ] ; к е[1; ктах ] ; 1 е [1; 1тах ] ; т е [1; ттах ]. Индексы I, ], к соответствуют линейным перемещениям точки начала локальной системы координат объекта соответственно вдоль осей X0, ¥0, Z0, а индексы I, т - двум углам поворота объекта у0, С00 вокруг собственных осей соответственно. Задан Аытар - шаг дискретности угловых координат у и О .

В локальной системе координат объекта 08Хg¥gZg заданы координаты габаритных точек

(Ь \ , 18 е[1; С г ] на поверхности объемного тела объекта, определяющие его форму. Координаты

точек заданы векторами вида (^8)« =[ х8 У 8 ^ 1]Г , где хг, уг, - координаты точки г в ло-

кальной системе координат объекта.

128

В неподвижной системе координат О0 Х0У0 Z 0 задана дискретная матрица высот препятствий УПР(г,к), где г, к- индексы координат х0, у0 соответственно: I 6 [1; 1тах]; к 6 [1;ктах].

Необходимо найти оптимальную траекторию 8 с минимальным значением целевой функции

Т* ~

Ь произвольного вида из начальной вершины $нач в конечную вершину $КОН, представляющую собой последовательность из нескольких вершин: 8* = {^р} ™=1- Каждой вершине графа соответствует

определенное пространственное линейно-угловое положение объекта в свободном пространстве.

Описание модификации направленного волнового алгоритма перемещения объекта, положение которого определяют 3 линейные и 2 угловые обобщенные координаты.

1. Задание численных значений исходных данных: 8нач =(хн0, ун0,1н0,Ун0,Юн0);

^кон (XкС, У ^, ^к0,Ук0,^к0 ) ; { Rig }; гтах ; ]тах ; ктах ; 1тах ; ттах ; [^ЯР ] ; Су ; Аи ; ор,тах ; 1зап _г ; 1зап _в .

Параметры соответствуют описанным при постановке задачи.

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

поверхностей препятствий, заданных дискретно в трехмерном пространстве [4], формируется массив [Утгп ] гиперповерхности минимальных значений вертикальных координат условного центра объекта

с учетом его угловых координат: Утг„ (i, к, I т), г 6 [1; гтах ] ; к 6 [1; ктах ] ; 1 6 11тах ] ; т 6 1 ттах ]. Ги-

перповерхность представляет собой функцию минимально возможных (при выполнении условия не-пересечения с препятствиями) значений вертикальной координаты у точки начала локальной системы координат объекта ОвХgУgZg , принятой за условный центр объекта, в неподвижной системе координат О0Х0У0Z0.

3. Используя вложенные циклы по г, ],к,I,т, определяющие х, у, г, у,О соответственно, рассматриваются всевозможные сочетания координат объекта х, у, г, У и О на дискретной равномерной сетке, и для каждого сочетания указанных координат в 5-мерном массиве У/гоШ сохраняется информация о наличии/отсутствии препятствия в текущем узле:

У/гоп,

1 при у < Утп(1Мт),

шт '

где у = ] 'А1 ; г 6 [1; гтах ]; ] 6 [1; ]тах ] ; к 6 1 ктах ] ; 1 6 [1; 1тах ] ; т 6 [1; ттах ]. Значение (-1) элемента

массива У/гоШ соответствует наличию препятствия в узле, 0 - отсутствию препятствия.

Одновременно происходит инициализация двух других 5-мерных массивов (определения уз-ла-родителя Угой и значений целевой функции перемещения от начальной точки УйИп):

Угой

Элементы массива У/гоШ могут принимать следующие значения: (-1) - узел занят препятствием (непроходим); 0 - узел свободен, не пройден и ни разу не просматривался; 1 - узел просматривался, но еще не пройден; 2 - узел пройден (окончательно закрыт).

4. Начальная точка положения объекта на равномерной решетке в массиве У/гоШ, помечается как пройденная:

У/гоШ

где

]нач = [ ун0/А1 ] ; кнач = ^0 / А1 ] ; 1нач = \_

5. Запускается распространение волны из начальной точки. Используются вложенные циклы

по г\ j, к, 1, 1П, где г 6 [1; (*'_ - 1) ] ; ] 6 [1; ]тах ] ; к 6 1 ктах ] ; 1 6 11тах ] ; т 6 1 ттах ] .

5.1. Для каждого сочетания индексов ]', к, 1, т на текущей итерации г проверяется выполне-

ние условия

У/гоШ

При выполнении данного равенства выполняется п. 5.2, в противном случае - рассматривается следующее сочетание индексов (]', к, 1, т) , и вновь выполняется п. 5.1. Текущее сочетание всех индексов (г, ]', к, 1, т) для краткости названо текущей вершиной 5г1. После завершения вложенных

129

циклов по (j, к, l, m) , т.е. перед началом следующей итерации цикла по i, выполняется п. 5.5 алгоритма.

Условие (5) позволяет алгоритму пропускать непроверяемые вершины при развертывании дерева поиска (рис. 1). Непроверяемые вершины возникают вследствие ограниченного количества вер-шин-преемников у каждой вершины и направлений преемственности. На каждой итерации цикла i происходит увеличение диапазонных значений индексов всех прочих координат (j, к, l, m) , максимально на 2 единицы, до тех пор, пока диапазонные значения не достигнут пределов исходной сетки

j e [1; jmax ] ; к e [1; kmax ] ; l e [1; lmax ] ; m e [1; mmax ] .

Обозначим диапазонные значения индексов jdmax , jdmin , kdmax , Kmin , ldmax , ldmin , mdmax , mdmin .

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

jdmin (i) i (imax jmax ); jdmax (i) (imax + jmax ) i ;

кЛ • (i) = i - (i - к ); кЛ (i) = (i + к ) - i;

omin \ f \ max max / ’ omax \ / V max max / ’

lo

omin max max omax max max

m (i ) = i - (i - m ); mA (i ) = ( i + m ) - i;

omin max max omax max max

Происходит коррекция пределов варьирования индексов j, к, l, m :

j e [max(1; J^ );min( Jmx ; jmax )]; к G [max(1; ^ ^^Kmcx ; ^max )] ;

l e [max

5.2. Для каждой текущей вершины si1 = (i, j, к, l, m) рассматривается ограниченная область-гиперкуб с центром в точке (j, к, l, m) и постоянным значением индекса (i +1) . Для этого индексы j 2, к 2,12, m2 варьируются в следующих диапазонах (область гиперкуба):

j2 e [

То есть просматриваются все состояния-преемники для текущего состояния объекта в многомерном линейно-угловом пространстве.

Значения индексов j2, к2,12, m2 , варьируемые по (8), проверяются на выполнение условия невыхода за границы диапазонов исходной сетки (7) и при необходимости корректируются.

Для каждого сочетания индексов i +1, j2, к 2,12, m2 проверяется выполнение условия

Vfront

При выполнении данного условия, т.е. если вершина-преемник, находящаяся впереди текущей, уже просматривалась, выполняется п. 5.3, в противном случае - п. 5.4.

5.3. Рассчитывается стоимость перемещения от начальной вершины до вершины-преемника Sj1 = (i +1, j2, к2,12, m2) через текущую вершину si1 = (i, j, к, l, m):

= Vdlin

где Li1 j - значение целевой функции между двумя вершинами sn и Sj1 .

Если стоимость перемещения от начальной вершины до вершины-преемника Sj1 = (i +1, j2, к2,12, m2) через текущую вершину s;1 = (i, j, к, l, m) меньше, чем

Vdlin(i +1, j 2, к 2,12, m2) - рассчитанная ранее стоимость перемещения в вершину s;1 через некоторую другую вершину, то вновь рассчитанная стоимость заносится в элемент массива Vdlin(i +1, j2, к2,12, m2), заменяя бывшее до этого значение данного элемента:

т/л/- (-^1 -ою п о) IL”“ при L-k<Vdlin(i +1J2,к2>l2,m2);

Vdlin

[Vdlin(i + 1,j2,к 2,l 2,m2) при LmeK > Vdlin(i + 1,j2,к 2,l 2,m2).

Одновременно с заменой значения элемента массива Vdlin при выполнении условия

LmeK < Vdlin

рассчитывается Rodтек - код текущей вершины-родителя si1 для вершины преемника Sj1 .

Кодирование вершины-родителя осуществляется при помощи 4-разрядного целого числа в десятичной системе счисления:

RodmeK = 1000 •

Если условие (12) выполняется, то происходит замена вершины-родителя для вершины пре-

емника Sj1:

V Л(^1'0Ю70 ^ \RodmeK при LmeK < Vdlin(i + 1,j2,k 2>l 2,m2) „ „

Vrod

[Vrod(i + 1,j2,k2,l2,m2) при LmeK > Vdlin(i + 1,j2,k2,l2,m2).

5.4. Данный пункт алгоритма выполняется, если вершина-преемник Sj1 еще ни разу не просматривалась, что формализуется условием

Vfront

По (13) рассчитывается RodmeK - код текущей вершины-родителя sn для вершины преемника Sj1, который заносится в элемент массива Vrod :

Vrod

По (10) рассчитывается стоимость перемещения LmeK от начальной вершины до вершины-

преемника Sj1 = (i +1, j2, k2,12, m2) через текущую вершину sn = (i, j, k, l, m), которая заносится в

элемент массива Vdlin :

Vdlin

В элементе массива Vfront с индексами (i +1, j2, k2,12, m2) сохраняется информация о том, что вершина-преемник Sj1 уже просматривалась, т.е. выполняется присваивание данному элементу

единичного значения по (9).

5.5. Данный пункт выполняется после того, как завершились вложенные циклы по (j, k, l, m), на текущей итерации цикла по i, т.е. после того как все возможные вершины-преемники для всех вершин со значением инде^а i были посещены. Перед увеличением значения i на единицу выполняется следующая последовательность действий. Используя вложенные циклы по j, k, l, m , где

j G 1 jmax ] ; k ek kmax ] ; l G I1; lmax ] ; m G I1; mmax L для кажЛого сочетания индексов i + 1, j, k,l, m

проверяется выполнение условия

Vfront

При выполнении данного условия вершины следующего за текущим ряда с одинаковыми значениями координаты x (с индексом i +1) закрываются, т.е. соответствующая вершина помечается как пройденная и закрытая:

Vfront

Таким образом помечаются не все вершины ряда с индексом i + 1, а только свободные и при этом просмотренные на текущей итерации цикла i . Вершины, занятые препятствиями, а также не вошедшие в множество проверяемых вершин в массиве Vfront , остаются непомеченными как закрытые (сохраняются старые значения соответствующих элементов). Это позволяет исключить их из рассмотрения как вершины-родители на следующей итерации цикла i .

Множество проверяемых вершин на каждой итерации i формируется в пространстве состояний объекта, представляющем собой равномерную решетку линейных (x, y, z) и угловых (у,&>) координат, по принципу развертывания-свертывания дерева поиска. Дерево поиска развертывается из единственной начальной вершины SHa4 по принципу рассмотрения всех возможных вершин-преемников для множества вершин, рассмотренных на предыдущей итерации i (координаты x , по условиям (5), (18), (19)), и свертывается в единственную конечную вершину SKOH по граничным условиям (7). Двухмерная иллюстрация развертывания-свертывания дерева поиска (движения фронта волны) приведена на рис. 1.

°  - свободные не проверяемые вершины ;

• - свободные проверяемые вершины;

- занятые препятствиями не проверяемые вершины;

- направления открытия вершин алгоритмом

Рис. 1. Иллюстрация развертывания-свертывания дерева поиска модифицированного волнового алгоритма

б. После завершения всех вложенных циклов по i, j, k, l, m (прохождения фронта волны), происходит восстановление найденной оптимальной по значению целевой функции траектории по кодам вершин-родителей.

6.1. В качестве первой вершины-преемника назначается SKOH, для которой из массива Vrod восстанавливается код вершины-родителя:

Rodтек = Vrod

где

Ікон = L Ук0/Al J; kKOH = L z,0/Al J;l кон = L^-7mJ/Au J;= L

6.2. Восстанавливаются индексы вершины-родителя si1 = (i, j, k, l, m) по коду Rodтек и ин-

дексам вершины-преемника sj1 = (i2, j2,k2,l2, m2) . При рассмотрении конечной вершины, с которой начинается восстановление траектории, i2 = / ; І2 = Ікон; k2 = k„nu; 12 = / ; m2 = mrn„. В

max кон кон кон кон

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

dm = Rod„єк % 10; Rod„єк = Rod„єк - dm ; dl = (RodmeK % 100) / 10; Rod„кк = Rod„кк - dl •10;

dk =

dj = dj - 2; dk = dk - 2; dl = dl - 2; dm = dm - 2 ; i = i 2-1; j = j 2 + dj; k = k 2 + dk; l = 12 + dl; m = m2 + dm.

С использованием традиционной математической формы записи выражения (22) будут иметь следующий вид:

dm=(Rod „єк - LRod „єк /10 J •10);Rod „єк = Rod „єк - dm; dl =(Rod „єк - LRod „єк /100 J •100) /10;Rod „єк = Rod „єк - dl •10;

dk = (RodmeK - L RodmeK /1000J • 1000) /100; RodmeK = RodmeK - dk • 100;

dj = [ Rod„кк /1000J

После восстановления индексов i, j, k, l, m пересчитывается код Rod „кк по (20) с подстановкой значений индексов

i 2 = i; j 2 = j; k 2 = k; 12 = l; m2 = m.

Последовательность действий пункта 6.2 алгоритма выполняется в цикле imax раз. В результате формируется траектория S из точек S* = {sp}

7. Выполняется локальная оптимизация траектории S*.

8. Определяется значение целевой функции L оптимизированной траектории S .

9. Вывод результатов: L , S . Окончание работы алгоритма.

Вычислительные реализации модифицированного волнового алгоритма и описанной методики на его основе в средах Microsoft Visual C++ и MATLAB показали работоспособность и эффективность алгоритма для решения поставленной задачи.

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

Перечисленные отличительные особенности обеспечивают методике на основе модифицированного направленного волнового алгоритма определенные преимущества перед альтернативными методиками планирования траектории в многомерном пространстве.

ЛИТЕРАТУРА

1. Кормен Т.Х. Алгоритмы: построение и анализ: пер. с англ. / Томас X. Кормен, Чарльз И. Лейзерсон, Рональд Л. Ривест, Клиффорд Штайн. М.: Изд. дом «Вильямс», 2005. 1296 с.

2. Рассел С. Искусственный интеллект: современный подход: пер. с англ. / Стюарт Рассел, Питер Норвиг. М.: Изд. дом «Вильямс», 2006. 1408 с.

3. Horowitz Е. Fundamentals of computer algorithms / Е. Horowitz, S. Sahni, S. Rajasekaran. Ori-entLongman, 2008. 808 p.

4. Корытов М.С. Использование полидистантных поверхностей в задаче поиска пути переме-

щения груза в среде с препятствиями / М.С. Корытов // Материалы 64-й научно-технической конференции ГОУ «СибАДИ». Омск: СибАДИ, 2010. Кн. 1. С. 302-306.

Щербаков Виталий Сергеевич - Vitaliy S. Shcherbakov -

доктор технических наук, профессор, Dr. Sc., Professor,

декан факультета «Нефтегазовая и строительная Dean: Faculty of Oil-and-Gas and Construction Engineering, техника» Сибирской государственной Siberian State Automobile and Highways Academy,

автомобильно-дорожной академии, г. Омск Omsk

Корытов Михаил Сергеевич - Mikhail S. Korytov -

кандидат технических наук, доцент Ph. D., Associate Professor,

кафедры «Конструкционные материалы Department of Construction Materials

и специальные технологии» and Special Technologies,

Сибирской государственной Siberian State Automobile and Highways Academy,

автомобильно-дорожной академии, г. Омск Omsk

Статья поступила в редакцию 12.01.12, принята к опубликованию 02.03.12