Материал: 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