2. Разделение плоскости на свободные области

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

Информацию о существующих свободных областях будем хранить в нескольких табличных структурах данных, которую обозначим TFR (Table of free region) – таблица свободных областей. Каждая строка таблицы свободных областей соответствует ячейке, т.е. TFR имеет I строк. Определим количество столбцов, пусть i-ая ячейка имеет максимальное количество свободных областей среди ячеек, т.е. A_i = max, тогда таблица свободных областей будет иметь A_i столбцов.

Рассмотрим шаги данного этапа алгоритма. Но сначала отметим, что при вызове алгоритма планирования траектории ему передаются: текущая конфигурация; целевая конфигурация; данные о препятствиях обнаруженных около текущей конфигурации, в виде массива запрещённых точек, заданных номерами узлов сетки дискретизации. Итак:

1) Если алгоритм планирования траектории вызывается впервые, т.е. перед началом движения, тогда создаём TFR_k для каждой плоскости k, в которую записываем, что все ячейки свободны, как показано в Таблице 1. Напомню, что ячейка свободна, если она имеет одну свободную область с границами – крайняя нижняя точка ячейки: крайняя верхняя точка ячейки. Возьмем TFR₁ - таблицу свободных областей для первой плоскости и перейдем к шагу 2.

Таблица 1 – TFR, отражающая ситуацию – все ячейки свободны

Ячейка	Свободные области
1	1:J
…	1:J
i	1:J
…	1:J
I	1:J

2) Шаг 2 – уточнение границ свободных областей в ячейках данной плоскости происходит рекуррентно для каждой вновь обнаруженной запрещённой точки, которая считывается из массива запрещённых точек, переданного алгоритму. Второй шаг состоит из следующих подпунктов:

2.1)Взять из массива запрещённых точек очередной номер узла;

2.2)Преобразовать его в координаты узла, получить номер ячейки, в которой лежит данная запрещённая точка, пусть это будет ячейка i;

2.3)Для каждой свободной области ячейки i выполняем следующие операции:

2.3.1)Получить границы текущей свободной области;

2.3.2)Сравнить ординату запрещённой точки, пусть это будет j, с границами свободной области, возможны следующие результаты сравнения:

• j равна одной из границ свободной области, тогда мы сужаем данную свободную область на одну точку;

• если j равна обеим границам свободной области, т.е. свободная область состоит из одной точки, то свободная область «исчезает», в этом случае в TFR записываем границы несуществующей свободной области, например 0:0 (нулевая горизонтальная координата является границей пространства обобщённых координат, в него не включается и является запрещённой);

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

2.3.4)Если свободные области текущей ячейки закончились перейти к шагу 2.4, иначе – к 2.3.1.

2.4)Если все запрещённые точки извлечены из массива, то переходим к шагу 2.5. Иначе вернуться к шагу 2.1.

2.5)Если TFR уже построена для все плоскостей, то завершаем этот этап и переходим к следующему основному этапу алгоритма разделения ячеек, иначе строим TFR для следующей плоскости, т.е. переходим к шагу 2.

Например, получен массив запрещённых точек, в котором две точки: (1,1) и (i, j) (рисунок 7).

x₁

0

1

…

i

…

I

…

…

j

1

J

Рисунок 7 – Пример расположения запрещённых конфигураций

Тогда Таблица 1 после уточнения примет вид Таблицы 2.

Таблица 2 – Уточнение TFR.

Ячейка	Свободные области
1	2:J
…	1:J
i	1:j-1	j+1:J
…	1:J
I	1:J

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

Рассмотрим пример матриц TFR для трехмерного случая, который представлен на рисунке 4. Для каждой плоскости будет своя матрица TFR (Таблица 3). Свободные области на каждой вертикальной ячейке представлены на рис. 8

Таблица 3 – Матрицы TFR для трех плоскостей

TFR1	TFR2	TFR3
1 2 1 1-3 2 1-1 3-3 3 1-1 3-3	1 2 1 1-3 2 1-1 3 1-1 3-3	1 2 1 1-3 2 1-1 3-3 3 1-3

Рисунок 8 – Разделение плоскостей на ячейки, выделение свободных областей