2426

3.8. Методика планирования траектории на основе модифицированного алгоритма вероятностной дорожной карты

Алгоритм вероятностной дорожной карты PRM (Probabilistic Road Map) относится к современным подходам в области планирования траекторий. Этот подход считается одним из ведущих методов планирования движения, в первую очередь для механических систем со многими степенями свободы в среде с препятствиями. Вероятностный метод PRM считается высокоэффективным, простым в реализации и применимым для различных видов задач, связанных с планиро-

ванием движения [212, 221, 228, 229, 230, 234].

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

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

ства конфигураций [141, 212, 221, 234].

Формируется граф Gr=(Sr, Er), где Sr={s1, s2,…, sng} – множество

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

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

ких вершин графа дорожной карты Gr: S*={sp} snp=1. Каждой вершине

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

120

Структура графа дорожной карты определяется квадратной мат-

рицей весов дуг N=[Li1,j1]ing1, j1=1. Значения весов Li1,j1 определяются по

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

Li1,j1=∞. (3.97)

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

Традиционный подход, успешно применяемый для соединения точек дугами в 2- и 3-мерном евклидовом пространстве, заключается в использовании быстрого и простого алгоритма соединении дугами каждой точки с ближайшими соседями, например при помощи метода триангуляции [221, 230, 234].

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

Для решения поставленной в разделе 3.1 задачи данный подход является неполным, т.к. в n-мерном случае, т.е. при рассмотрении не только трех линейных, но и нескольких угловых координат, проверка на пересечение с препятствиями промежуточных положений объекта между двумя точками становится необходимой, без нее возможно соединение вершин с ближайшими соседями только с учетом линейных координат, в результате чего могут быть потеряны глобально оптимальные пути перемещения.

В настоящей монографии предложен и реализован другой подход, обладающий полнотой [63, 100]. Полученные случайным образом вершины графа соединяются между собой дугами с учетом видимости, т.е. выполнением условия непересечения с препятствиями при движении из вершины в вершину по прямой в пространстве конфигураций. Выполняется проверка видимости между текущей вершиной si1 {Sr} и каждой из подмножества вершин sj1 {Sx} с большими или равными значениями линейной координаты груза x. Подмножество {Sx} формируется из множества {Sr}по условию

121

где {Sx} {Sr}.

После того, как сформирована матрица весов графа [N], осуществляется поиск кратчайшего пути между двумя вершинами графа (sнач и sкон) при помощи традиционных алгоритмов поиска на графе [216].

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

ке S={sp}ipmax=1 .

Блок-схема обобщенного алгоритма ВДК представлена на рис. 3.22.

(Rg)ig

Рис.3.22. Обобщенная блок-схема алгоритма ВДК

Описание алгоритма ВДК перемещения груза, положение которого определяют 3 линейные и 2 угловые обобщенные координаты

[63, 100]. Предложенный модифицированный алгоритм ВДК заключается в следующей последовательности шагов:

122

1. Задание численных значений исходных данных: sнач=(xн0, yн0,

zн0, γн0, ωн0); sкон=(xк0, yк0, zк0, γк0, ωк0); { Rig }; imax; jmax; kmax; lmax; mmax;

[YПР]; ng; cγω; u; δopt; lзап_г; lзап_в.

Параметры соответствуют описанным при постановке задачи в разделе 3.1, за исключением следующего собственного параметра алгоритма ВДК, задаваемого эмпирически: ng – количество вершин графа дорожной карты.

2. По методике, изложенной в разделе 3.4, формируется массив [Ymin] гиперповерхности минимальных значений вертикальных координат условного центра груза с учетом его угловых координат:

Ymin(i,k,l,m), i [1; imax]; k [1; kmax]; l [1; lmax]; m [1; mmax].

3. Генерируется случайным образом множество вершин Sr={s2,…, sng–1} графа дорожной карты, представляющих собой точки в пространстве конфигураций, т.е. возможные положения груза в пределах заданной сетки координат, в которых он не пересекается с препятствиями.

Для создания дорожной карты при помощи генератора случайных чисел создается ng точек в пространстве конфигураций с координатами

где

xp=ip ∙Δl;yp=jp∙Δl; zp=kp∙Δl;γp=(lp–0,5∙lmax)∙Δu;ωp=(mp–0,5∙mmax)∙Δu; (3.100) ip= Rand∙imax ; jp= Rand∙jmax ; kp= Rand∙kmax ;

Значения xp, yp, zp, γp, ωp, полученные для каждого значения p, должны удовлетворять условию непересечения груза с эквидистантной (полидистантной) поверхностью [YЭ], что через массив гиперповерхности Ymin описывается следующим условием:

yp≥Ymin(ip,kp,lp,mp). (3.102)

При выполнении этого условия значение p увеличивается на 1, в противном случае генерация отдельной точки по (3.100), (3.101) повторяется.

Первая (p=1) и последняя (p=ng) точки траектории будут совпадать с начальной и конечной заданными точками:

123

4. Формируется матрица весов дуг N=[Li1,j1]ing1, j1=1. Выполняется

проверка видимости между текущей точкой si1 {Sr} и каждой из подмножества точек sj1 {Sx} с большими или равными значениями линейной координаты груза x. Подмножество {Sx} формируется из множества {Sr} с выполнением условия (3.98). Для этого используются два вложенных цикла: внешний i1 [1; ng] и внутренний j1 [1; ng]. Для каждого сочетания значений i1 и j1 осуществляется проверка выполнения условия (3.98). При невыполнении данного условия вес дуги (si1,sj1) принимается равным бесконечно большому значению по

(3.97).

Если условие выполняется, то проводится дополнительная проверка выполнения условия непересечения с препятствиями всех точек, лежащих на прямой в пространстве конфигураций между точками si1 и sj1. Находят применение два способа осуществления подобной проверки [221, 234, 244]: 1) при помощи инкрементного алгоритма (алгоритма последовательных малых приращений); 2) при помощи рекурсивного алгоритма деления отрезка пополам (используя метод дихотомии).

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

Проведенные рядом зарубежных авторов исследования показали, что рекурсивный алгоритм деления отрезка работает в общем случае быстрее инкрементного [221, 244]. Причина в том, что в среднем положении любого отрезка груз имеет самую высокую вероятность пересечения с препятствием, если таковое имеет место.

124

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

4.1. Переменная break, определяющая наличие (break=1) либо отсутствие (break=0) пересечения груза с препятствиями при движении по прямой в пространстве конфигураций между двумя точками si1 и sj1, принимается равной нулю:

break=0. (3.104)

4.2. В цикле с идентификатором i2 происходит изменение значения индекса i2 от 1 до некоторого максимального значения i2max, определяемого условием

i2=4; j2 [1;8]

…………………………………………

Рис. 3.23. Графическая интерпретация модификации рекурсивного алгоритма деления отрезка пополам (пример на плоскости, дерево рекурсии)

Изменение индекса i2 на каждой итерации цикла i2 происходит по закону геометрической прогрессии (увеличение в 2 раза):

125

Значение i2 определяет количество равноотстоящих друг от друга точек на прямой между точками si1 и sj1, в которых на текущей итерации цикла i2 происходит проверка пересечения груза с препятствиями

(рис. 3.23).

4.3. Кроме того, на каждой итерации цикла i2 во вложенном цикле j2 с интервалом в 1 варьируется вспомогательный индекс j2:

Максимальное значение j2=i2∙2 определяет суммарное количество точек, проверенных на пересечение с препятствиями с начала цикла i2. Причем на каждой итерации цикла i2 во вложенном цикле j2 проверяются новые точки, не совпадающие с проверенными на предыдущей итерации i2 (см. рис. 3.23). Для этого на каждой итерации алгоритма i2 определяются собственные значения координат точки, с которой начинается проверка:

x2нач=xi1+(xi1–xj1)/(i2∙2); y2нач=yi1+(yi1–yj1)/(i2∙2); z2нач=zi1+(zi1–zj1)/(i2∙2);

γ2нач=γi1+(γi1–γj1)/(i2∙2); ω2нач=ωi1+(ωi1–ωj1)/(i2∙2). (3.110)

Затем во вложенном цикле j2 вычисляются координаты текущей проверяемой точки:

4.4. Далее во вложенном цикле j2 для текущей проверяемой точки с индексами координат (3.112) выполняется собственно проверка условия непересечения груза с препятствиями (3.102). В случае, если данное условие не выполняется, т.е. имеет место пересечение груза с препятствиями, переменная break принимается равной 1 и одновременно циклы j2 и i2 прерываются:

break=1. (3.113)

126

4.5. Данный пункт выполняется после завершения либо прерывания вложенных циклов j2 и i2. Выполняется проверка условия (3.104). Если данное условие не выполняется, т.е. имеет место пересечение груза с препятствиями при движении из точки si1 в sj1, вес соответствующей дуги (ребра) графа принимается равным бесконечно большому значению по (3.97). В случае выполнения условия (3.104) вес дуги рассчитывается как значение целевой функции СВД по (3.18).

5. Осуществляется поиск кратчайшего пути между двумя вершинами графа (sнач и sкон) при помощи алгоритма Дейкстры [216]. Результатом поиска является оптимальная траектория S* с минимальным значением целевой функции L*, представляющая собой последовательность из нескольких вершин графа дорожной карты Gr:

S*={sp}sn= .

p 1

6. Осуществляется линейная интерполяция найденной траектории на равномерной сетке обобщенной координаты xi (i [1; imax]) для вычисления обобщенных координат yi, zi, γi, ωi. Под интерполяцией подразумевается вычисление значений каждой из обобщенных координат груза в промежутках между узловыми точками найденной траектории, которое выполняется по известному алгоритму [65, 191].

Построение массива гиперповерхности [Ymin] по методике раздела 3.4

4

s1=sнач=(xн0, yн0, zн0, γн0, ωн0)

p=2 5 6

ip=Rand∙imax; jp=Rand∙jmax; kp=Rand∙kmax; lp=Rand∙lmax; mp=Rand∙mmax;

xp=ip∙Δl; yp= jp∙Δl; zp=kp∙Δl; γp=(lp–0,5∙lmax)∙Δu; ωp=(mp–0,5∙mmax)∙Δu

7 yp≥Ymin(ip,kp,lp,mp) Нет

Да 8 p=p+1

Рис. 3.24. Блок-схема модифицированного алгоритма ВДК для пяти учитываемых обобщенных координат груза (начало)

127

Рис. 3.24. Блок-схема модифицированного алгоритма ВДК для пяти учитываемых обобщенных координат груза (окончание)

128

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

равномерной решетке S*={sp}ipmax=1 .

7.Выполняется локальная оптимизация интерполированной траектории S* по методике, изложенной в разделе 3.5.

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

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

Блок-схема модифицированного алгоритма ВДК перемещения груза, положение которого определяют 3 линейные и 2 угловые обобщенные координаты, приведена на рис. 3.24. Вычислительные реализации модифицированного алгоритма и описанной методики на его основе в средах Microsoft Visual C++ и MATLAB показали работоспособность и эффективность алгоритма для решения поставленной задачи.

3.9. Методика планирования траектории на основе алгоритма декомпозиции линейных и угловых координат

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

Предлагается разделить на две отдельные подзадачи оптимизацию линейных координат груза и его угловых координат. Все преобразования в трехмерном пространстве могут быть сведены к композиции двух преобразований: вращения и переноса вдоль координатных осей. Это позволяет разделить и выполнять по отдельности: 1) нахождение траектории определенной точки груза в трехмерном пространстве с препятствиями; 2) оптимизацию траекторий трех угловых обобщенных координат груза. Необходимо разработать методику планирования траектории на основе алгоритма декомпозиции линейных и угловых координат.

129