3. Соединение свободных областей
Перейдём к следующему этапу – соединению свободных областей. В любом случае, при вызове алгоритма планирования траектории перед началом движения или во время движения, при столкновении с препятствием, необходимо соединить те свободные области в смежных ячейках, которые пересекаются, переход в таких областях из одной в другую осуществляется беспрепятственно. Свободные области в смежных ячейках пересекаются, если их точки, имеющие одинаковое значение ординаты, не запрещены. Соединение может проходить не единственным образом, т.е. из свободной области одной ячейки возможен переход в несколько областей другой ячейки. В результате получается множество списков соединенных областей для каждой плоскости.
Все возможные последовательности соединённых областей плоскости будем хранить в матричной структуре данных – LCRk (List of connected regions), где k - номер плоскости. Т.е. LCRk – это матрицы размером m на I. Каждый i-ый (i= 1…I) столбец матрицы соединенных областей соответствует i-ой ячейке. Количество строк m – переменная величина, зависит от количества последовательностей соединённых областей. Элементом матрицы LCRk является номер свободной области в ячейке, который соответствует номеру столбца таблицы TFRk, содержащего эту область. Необходимая для построения LCRk информация берётся из TFRk.
Рассмотрим шаги этого этапа.
1)Определяем первоначальное количество последовательностей соединённых областей. Возьмём первую ячейку на плоскости:
1.1)Если она содержит только одну свободную область, начнём строить одну последовательность соединённых областей, т.е. в матрице LCR создадим первую строку и в первую позицию, которая соответствует первой ячейке, запишем номер области. Если, кроме того, что ячейка имеет одну область, эта область занимает всю ячейку, то запишем в LCR вместо номера ячейки ноль (номера ячеек начинаются с единицы). Договоримся, что ноль в последовательности соединённых областей соответствует случаю, когда ячейка свободна.
1.2)Если же в первой ячейке несколько свободных областей, создадим в LCR количество строк равное количеству свободных областей в первой ячейки, и первые позиции каждой строки инициализируем номерами свободных областей.
2)В цикле перебираются все ячейки, определяется какие области текущей ячейки соединимы со свободными областями предыдущей ячейки, и в столбец матрицы LCR, соответствующий текущей ячейке записываются номера областей, ноль или минус один.
2.1)Взять из TFR границы первой области текущей ячейки.
2.2)Если её границы совпадают с границами ячейки, т.е. ячейка пустая, то записываем ноль во все строки LCR в столбце данной ячейки. Из предыдущей ячейки в эту ячейку возможен переход из любой свободной области предыдущей ячейки.
2.3)Если текущая ячейка не пустая.
2.3.1)А предыдущая ячейка пустая. Записываем всевозможные сочетания свободной области предыдущей ячейки с областями текущей ячейки, если количество последовательностей в LCR равно m, а количество свободных областей текущей ячейки равно n, то в результате мы получим m*n строк в LCR.
2.3.2)И предыдущая ячейка тоже не пустая. Сравниваем все свободные области предыдущей ячейки со всеми областями текущей (во вложенном цикле), ищем среди них пары пересекающихся областей. Обозначим: Нп, Вп – соответственно нижняя и верхняя границы свободной области предыдущей ячейки; Нт, Вт – так же для текущей ячейки. Возможны три вида пересечения:
Нп ≤ Вт и Вп ≥ Нт, рисунок 9;
n
m
n
m
Рисунок 9
Нп ≥ Нт и Вп ≤ Вт, рисунок 10;
Рисунок 10
Нп ≤ Нт и Вп ≥ Вт, рисунок 11;
m
n
Рисунок 11
2.3.2.1) Если границы областей имеют один из видов пересечения, добавляем в LCR в строку с предыдущей пересекаемой областью номер новой области. Если одна предыдущая область пересекает несколько областей текущей ячейки, то для каждой новой области добавляем ещё одну строку в LCR, копируем в неё всю данную последовательность до текущей ячейки и в конец добавляем номер новой области.
2.3.2.2) Если границы областей не имеют одного из видов пересечений, но существует (начало) изолированной свободной области, которая не пересекается с предыдущей областью (рис. 12), то для каждой подобной области добавляем ещё одну строку в LCR, записываем в неё все позиции до текущей -1, означающие несуществующий ранее маршрут и в конец добавляем номер свободной области.
Рисунок 12 – Начало и конец изолированной свободной области
2.3.2.3) Если границы областей не имеют одного из видов пересечений, но существует (конец) «изолированной» свободной область (рис. 12), когда существует несколько областей предыдущей ячейки и одно или несколько из них не пересекается с текущей областью из-за наличия запрещенных точек, которые образуют «тупик», то для каждой подобной области добавляем ещё одну строку в LCR, записываем в во все позиции после текущей -1, означающие отсутствие продолжение маршрута и в текущую добавляем номер свободной области.
2.4) Если не все ячейки просмотрены вернуться к шагу 2.1, иначе к шагу 2.5.
2.5) Если матрица LCR построена для всех плоскостей, то конец этого этапа алгоритма, иначе переходим к шагу 1 для построения LCR для следующей плоскости.
В результате получаем матрицу LCR, заполненную нулями, отрицательными значениями и номерами свободных областей. Каждая строка матрицы определяет так называемый «пустой коридор» в пространстве обобщённых координат, двигаясь внутри которого манипулятор не встретит известные препятствия. Несколько нулей идущих подряд указывают на присутствие пустого пространства.
Например, для примера матрицы TFR (Таблица 2) матрица LCR будет иметь вид:
.
Числа 1 и 2 стоят в i-ом столбце. Во всех остальных столбцах, на месте многоточий, нули. При построении этой матрицы сначала была инициализирован первый элемент одной первой строки, т.к. первая ячейка имеет одну свободную область. Затем, при достижении места раздвоения соединения областей, была добавлена ещё одна строка, в которую до места раздвоения были скопированы все элементы из первой строки.
Рассмотрим пример матриц LCR для трехмерного случая, который представлен на рисунке 4. Матрица TFR для этого случая приведена в Таблице 3. Соединение свободных областей на каждой плоскости представлены на рисунке 13.
Построим матрицы LCR для каждой плоскости:
|
|
|
Рисунок 13 – Соединение свободных областей