Библиографический список

1. Батищев, Д. И. Генетические алгоритмы решения экстремальных задач / Под ред. Львовича Я.Е.: Учеб. пособие. Воронеж, 1995.

2. Лопатин П.К., Бинчуров И.М. Егоров А.С., Применение в точном алгоритме управления манипуляционными роботами в неизвестной среде алгоритмов фронта распространения волны и полиномиальной аппроксимации// Молодежь и наука – третье тысячелетие: Сб.материалов Всероссийской научной конференции студентов, аспирантов и молодых ученых/ Сост. В.В.Сувейзда.; КРО НС «Интеграция», - Красноярск, 2007. – (557с.) с.399-404.

3.6.6. Диаграммы вороного

Подход на основе диаграмм Вороного [1, 3] предполагает обход запрещённых точек на некотором удалении от них, что может повысить вероятность решения задачи планирования пути в среде с известными запрещёнными состояниями.

Пусть в пространстве конфигураций имеются запрещённые ячейки p₁, p₂, …, p_M, которые соответствуют препятствиям в рабочей зоне манипулятора. Необходимо найти путь в пространстве обобщённых координат, который бы соединял заданные начальную точку q₀ и целевую точку q_T. Путь должен представлять собой последовательность ячеек, соседние элементы которой отличаются не более чем на один шаг по каждой координате. Ни одна из ячеек пути не должна быть запрещённой и выходить за границы, заданные векторами q_L (нижние ограничения) и q_H (верхние ограничения).

Рисунок 3.15. Пространство конфигураций (двумерный случай).

Построение диаграммы Вороного

Диаграмма Вороного конечного множества точек P = {p₁, p₂, …, p_M} представляет собой такое разбиение пространства, при котором каждая область этого разбиения образует множество точек, более близких к одному из элементов множества P, чем к любому другому элементу множества (рис. 3.16).

Рассмотрим область с центром в p_i (i = 1, 2, …, M). Множество точек q* области описываются системой неравенств

D(q*, p_i) ≤ D(q*, p_j)  j = 1, 2, …, M, j ≠ i,

где D(q₁, q₂) = – квадрат расстояния между точками q₁ и q₂.

После преобразования неравенства системы примут вид

. (3.124)

Добавим в систему ограничения на q* в виде неравенств

­ (3.125)

Рисунок 3.16. Диаграмма Вороного

Таким образом, каждая область диаграммы Вороного описывается системой линейных неравенств, задающих полупространства, и представляет собой многогранник, образованный пересечением этих полупространств (многогранник решений системы). Заметим, что точки, в которых одно или несколько неравенств системы обращаются в равенства, находятся на одинаковом удалении как от точки p_i, так и от всех точек p_j, соответствующих этим неравенствам, и лежат на пересечении соответствующих гиперплоскостей (или на гиперплоскости, если такое неравенство одно). Если какая-то точка обращает в равенство n линейно независимых неравенств системы, она является вершиной многогранника решений. Вершины многогранника совпадают с фундаментальными решениями системы неравенств. Множество точек, обращающих в равенство n-1 каких-то линейно независимых неравенств системы, являются ребром многогранника решений (ребром диаграммы Вороного). Для отыскания многогранника решений (области диаграммы Вороного) будем использовать алгоритм, основанный на методе Черникова определения фундаментальной системы решений для системы однородных линейных неравенств [2].

Многогранник будем задавать множеством вершин V и множеством рёбер E. Кроме того, с каждой вершиной v свяжем множество N(v), содержащее номера тех неравенств, которые обращаются в равенство в точке v (т.е. номера тех гиперплоскостей, которым принадлежит v).

Сначала рассмотрим многогранник, удовлетворяющий неравенствам (2). Очевидно, он представляет собой прямоугольный гиперпараллелепипед. Множество V содержит всевозможные точки, каждая из координат которых есть либо 0, либо q^H_k: (0, 0, …, 0), (q^H₁, 0, …, 0), (0, q^H₂, 0, …, 0), (q^H₁, q^H₂, 0, …, 0), …, (q^H₁, q^H₂, …, q^H_n). Каждая вершина лежит на пересечении n гиперплоскостей, задаваемых уравнением q_k = 0 или q_k = q^H_k (в зависимости от координат вершины). Множество E содержит всевозможные отрезки, соединяющие две вершины, отличающиеся только одной координатой.

Теперь будем добавлять к системе (3.125) по одному неравенству из (3.124). Каждое неравенство f_j(q) ≤ 0 представляет собой полупространство, поэтому необходимо найти пересечение текущего многогранника решений и этого полупространства. Все вершины v  V можно разделить на три множества по отношению к полупространству:

V^– – вершины, лежащие внутри полупространства (f_j(v) < 0);

V⁺ – вершины, лежащие вне полупространства (f_j(v) > 0);

V⁰ – вершины, лежащие на границе полупространства – гиперплоскости (f_j(v) = 0).

Если множество V⁺ пустое, неравенство f_j(q) ≤ 0 можно безболезненно удалить из системы, иначе необходимо выполнить следующие шаги:

1. V* = .

2. Для каждого ребра v₁v₂, где v₁  V^–, v₂  V⁺, добавить в V* вершину v₁ вместе с новой вершиной v = k∙v₁ + (1 – k)∙v₂, k = , лежащей на пересечении ребра v₁v₂ и гиперплоскости f_j(q) = 0. N(v) = N(v₁) ∩ N(v₂). Ребро v₁v₂ удалить из E, заменив его ребром v₁v.

3. V = V^–  V⁰  V*.

4. Пусть v₁, v₂  V*, v₁ ≠ v₂, Z = N(v₁) ∩ N(v₂), |Z| > n – 3, тогда если V не содержит вершины v, отличной от v₁ и v₂, такой что Z  N(v), то v₁ и v₂ соединены ребром. Рассмотрим всевозможные пары вершин из V* и найденные рёбра добавим в E.

5.  v  V: N(v) = N(v)  j.

После выполнения данного алгоритма последовательно для всех неравенств системы (1) мы получим вершины и рёбра многогранника, представляющего собой область диаграммы Вороного с центром в p_i.

Сформируем множество V вершин и множество E рёбер графа, задающего диаграмму Вороного, объединив многогранники, соответствующие всем её областям:

1. V = , E = , i = 1.

2. Пусть V_i и E_i – соответственно вершины и рёбра i-ой области диаграммы, V*_i – те вершины из V_i, которых нет в V. Добавим вершины из V*_i в V.

3. Рассмотрим все рёбра v₁v₂ из E_i. Если v₁v₂  E, добавим v₁v₂ в E (проверку можно не производить, если v₁  V*_i или v₂  V*_i).

4. Если i < M, то i = i + 1, перейти на шаг 2.

На шаге 2 приведённого алгоритма требуется определить, какие вершины из V*_i присутствуют в V. Чтобы эта операция выполнялась по возможности быстро, следует хранить V в виде дерева. При этом необходимо определить отношение порядка на множестве вершин.

Пусть v₁(v₁₁, v₁₂, …, v_1n) и v₂(v₂₁, v₂₂, …, v_2n) – вершины. Если < , где  – заданная малая константа, то v₁ = v₂. Иначе положим, что вершины v₁ и v₂ лежат внутри области q_1k ≤ q_k ≤ q_2k, k = 1, 2, …, n. Сравним v₁ и v₂ по следующему алгоритму:

1. k = 1.

2. .

3. Если v_1k < q* и v_2k ≥ q*, то v₁ < v₂, алгоритм завершён.

4. Если v_1k ≥ q* и v_2k < q*, то v₁ > v₂, алгоритм завершён.

5. Если v_1k < q* и v_2k < q*, то q_2k = q*.

6. Если v_1k ≥ q* и v_2k ≥ q*, то q_1k = q*.

7. Если k = n, перейти на шаг 1, иначе k = k + 1, перейти на шаг 2.

Планирование пути по диаграмме Вороного

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

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

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

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

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

Уравнение прямой, проходящей через точки q₁ и q₂, в параметрическом виде выглядит следующим образом:

q = q₁ + t∙r, где r = q₂ – q₁.

Точка пересечения прямой и гиперплоскости, заданной уравнением (a, q) + a₀ = 0, определяется по формуле:

.

Вычислим t для всех граней области (взяв в качестве q₁ центр области, а в качестве q₂ стартовую точку) и выберем из них наименьшее положительное – оно и будет соответствовать грани, которую пересекает луч. Вершины грани содержат во множестве N номер этой грани.

Описанные действия надо повторить для целевой точки.

Этап 3. Удаление из графа рёбер, которые налегают на препятствия. Пусть ребро соединяет точки q₁ и q₂, а препятствие представляет собой гиперкуб с центром в точке p и стороной 2a = 1.01∙Δq, где Δq – шаг дискретизации. Воспользуемся следующим алгоритмом:

1. l = 0, r = 1, k = 1.

2. d = q_2k – q_1k.

3. Если d <> 0, перейти на шаг 5.

4. Если p_k – a < q_1k < p_k + a, перейти на шаг 9, иначе отрезок не пересекает гиперкуб, алгоритм завершён.

5. Если d > 0, , , иначе , .

6. Если l′ > l, то l = l′. Если r′ < r, то r = r′.

7. Если l ≥ r, то отрезок не пересекает гиперкуб, алгоритм завершён.

8. Если k = n, то отрезок пересекает гиперкуб, алгоритм завершён.

9. k = k + 1, перейти на шаг 2.

Рисунок 3.17. Планирование пути по диаграмме Вороного

Этап 4. Выбор оптимального пути на графе с помощью модифицированного алгоритма Дейкстры. Отличие от оригинального алгоритма [2] заключается в том, что длиной ребра считается оценка вероятности пройти по ребру, не натолкнувшись на неизвестное препятствие, длиной пути считается оценка вероятности пройти путь, не натолкнувшись на неизвестное препятствие, а более коротким считается тот путь, у которого эта вероятность выше.

С каждой вершиной v графа, пройденной в процессе работы алгоритма, свяжем вершину P(v), из которой мы пришли в v, и длину L(v) пути от стартовой вершины до v. С помощью L(v₁, v₂) обозначим длину ребра, соединяющего вершины v₁ и v₂. Вершины, которые необходимо просмотреть, будем хранить в списке A, упорядоченном по убыванию P(v). По окончанию работы алгоритма список R будет содержать вершины, из которых состоит оптимальный путь, начиная со стартовой вершины и заканчивая целевой вершиной.

Длину ребра будем вычислять по формуле:

,

где d – расстояние от ребра v₁v₂ до центра области, k₁ = 2π, k₂ = 3π, k₃ = 2π.

Использование такой функции способствует тому, чтобы путь проходил по рёбрам, которые находятся не очень близко к запрещённым точкам, но и не слишком далеко. График зависимости L(v₁, v₂) от d приведён на рис. 4.

Рисунок 3.18. Зависимость «длины» ребра от расстояния до центра области.

Алгоритм имеет следующие шаги:

1. Пусть v₀ – стартовая вершина. Поместить v₀ в A, установить P(v₀) = v₀, L(v₀) = 1.

2. Если список A пуст, целевая вершина не достижима, алгоритм завершён.

3. Извлечь из A первый элемент v.

4. Если v – целевая вершина, путь найден, перейти на шаг 7.

5. Для каждой вершины v′, соседней по отношению к v, выполнить: L′ = L(v)∙L(v, v′); если P(v′) =  или L′ > L(v′), то поместить v′ в A, P(v′) = v, L(v′) = L′.

6. Перейти на шаг 2.

7. Поместить v в начало R.

8. Если v – стартовая вершина, алгоритм завершён.

9. v = P(v), перейти на шаг 7.

Этап 5. Дискретизация пути. Полученный на предыдущем этапе путь представляет собой ломаную линию. Необходимо определить последовательность дискретных точек, через которые проходит эта линия. Пусть q₁ и q₂ – концы отрезка, d_k (k = 1, 2, …, n) – шаг дискретизации по каждому измерению. Каждой ячейке пространства поставим в соответствие вектор x(x₁, x₂, …, x_n), x_k  Z, k = 1, 2, …, n, такой, что множество точек q(q₁, q₂, …, q_n) ячейки удовлетворяет неравенствам , k = 1, 2, …, n. Сформируем список X ячеек, которые пересекает отрезок q₁q₂, по следующему алгоритму (рис. 3.19):

1. , x_k = [t], mod_k = {t}∙d_k, , k = 1, 2, …, n.

2. Поместить x в X.

3. Если x = x*, алгоритм завершён.

4. Найдём ведущую координату i, такую что |x_i – x*_i| ≥ |x_k – x*_k|, k = 1, 2, …, n.

5. m = mod_i. Для k = 1, 2, …, n выполнить шаг 6.

6. , mod_k = mod_k – r∙m. Если x_i < x*_i, то d′_k = d_i∙r, mod_k = mod_k + d′_k, иначе d′_k = –d_i∙r.

7. Если |x_i – x*_i| = 1, перейти на шаг 12, иначе для k = 1, 2, …, n выполнить шаги 8-10.

8. s = 2mod_k + d′_k, mod_k = mod_k + d′_k.

9. Если s < 0, то mod_k = mod_k + d_k, x_k = x_k – 1.

10. Если s ≥ 2d_k, то mod_k = mod_k – d_k, x_k = x_k + 1.

11. Поместить x в X, перейти на шаг 7.

12. Поместить x* в X.

Рисунок 3.19. Дискретизация отрезка