Планировщики

Перебор

Для полного перебора всех соседей ноды написан класс project.core.find_path.find_path_core.AllDirectionsPathFinder

Для перебора всех возможных соседей написан рекуррентный метод, который порождает все возможные смещения на 1:

/**
 * Запустить перебор комбинаций
 *
 * @param n        общее число элементов
 * @param k        размер комбинации
 * @param consumer обработчик для каждой найденной комбинации (const int *c)->{}
 */
template<typename F>
void loop(std::vector<int> data, int pos, const F &consumer) {
    if (pos >= data.size()) {
       consumer(data);
    } else
       for (int i = -1; i <= 1; i++) {
          data[pos] = i;
          loop(data, pos + 1, consumer);
       }
}

Для каждого смещения запускается лямбда-выражение consumer

Многомерные пространства

Если такой алгоритм применить для сцены с несколькими роботами, то размерность пространства возрастает пропорционально. Это означает, что количество соседей $c_n$ растёт как степенная функция:

c_n = 3^n - 1,

где $n$ - суммарное количество звеньев

При $n=6$ размер списка будет равен $728$ , а при $n=24$ (сцена из четырёх роботов) уже $282429536480\approx 3\times 10^{11}$ .

Такая количество проверок на каждом этапе приводит к необходимости планировать пути на вычислительных кластерах.

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

Единичные смещения

Самым простым решением является обработка только тех соседей, у которых изменяется ровно одна координата. Такие смещения будем называть единичными. Тогда размер списка смещений с ростом размерности будет расти пропорционально $n$ , а не экспоненциально и равен $2n$ . Этот подход пропускает случаи, когда только изменение двух или более координат сразу даст возможность построить маршрут.

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

Планировщик с единичными смещениями реализован в классе project.core.find_path.find_path_core.OneDirectionPathFinder

Упорядоченные смещения

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

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

Если бы открытое множество могло иметь бесконечный размер, то такой проблемы бы не было, но у нас оно ограничено.

Т. к. алгоритм добавляет новых соседей в открытое множество и удаляет старых, то порядок смещений становится важным.

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

Данная логика реализована в классе project.core.find_path.find_path_core.OneDirectionOrderedPathFinder

Многопоточность

Для многопоточности написан планировщик project.core.find_path.find_path_core.OneDirectionSyncPathFinder

На каждом такте планирования он запускает заданное количество потоков для перебора части соседей. Результаты сводятся с помощью std::promise<> и std::future<>.

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

Каждый поток должен обращаться к потокобезопасным открытому и закрытому множествам.

MultiRobot

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

Данная логика реализована в классе project.core.find_path.smart_path_finding.MultiRobotPathFinder

Такую логику можно сравнить с оптимизаторами: локальные и глобальные. Локальные планировщики решают конкретную задачу планирования, а глобальные являются своего рода надпрограммами для локальных. Они используют их внутри себя и с их помощью обеспечивают те или иные требования к проложенному пути.

Непрерывный планировщик

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

Чтобы решить эту проблему, был написан непрерывный планировщик project.core.find_path.smart_path_finding.ContinuousPathFinder.

Алгоритм его работы похож на multirobot, но при этом основным планировщиком является multirobot. При проверке определяются коллизии, но с промежуточными проверками каждого этапа пути. Количество проверок задаётся в параметре конструктора checkCnt.

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

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

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

Моделирование

Для сравнивания планировщиков был написан модуль project.core.find_path.find_path_generator

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