Монотонные траектории

Когда построен путь, необходимо составить зависимость состояния робота от времени. Такую зависимость называют траекториями.

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

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

При планировании обычными сплайнами возникает проблемы вылетов (overshooting). В таких случаях робот попадает в опорные точки траектории в заданное время, но в промежуточных положениях углы поворота могут выйти за границы допустимых значений. Это приведёт к аварийной остановке робота.

Чтобы гарантировать отсутствие вылетов, необходимо строить монотонные траектории. Самый популярный алгоритм построения монотонных траекторий разработан Фритчем и Карлсоном. Подробнее о нём можно прочитать здесь.

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

Планировщик траекторий project.find_trajectory.MonotoneTrajectoryFinder построен по следующему принципу. Для каждой координаты создаётся свой монотонный интерполятор, а после в любой момент времени по временной метке из интерполятора получаем соответствующее состояние.

Для подготовки траектории нужно вызвать метод

/**
 * Подготовка траектории
 * @param startState стартовое положение
 * @param endState конечное
 * @param errorCode код ошибки
 */
virtual void prepareTrajectory(
    const std::vector<double> &startState, const std::vector<double> &endState, int &errorCode
);

В качестве аргументов он принимает стартовое и конечное состояния. Путь при этом прокладывается внутри метода prepareTrajectory(). Спланированный путь сохраняется в переменную _lastPath.

Все параметры планировщика задаются с помощью метода инициализации

/**
 * инициализация
 * @param scene сцена
 * @param intervalDuration интервал между временными метками
 * @param showTrace флаг, выводить ли лог
 * @param maxOpenSetSize максимальный размер открытого множества
 * @param gridSize размер сетки
 * @param maxNodeCnt максимальное кол-во нод в открытом множестве
 * @param checkCnt количество проверок каждого этапа пути
 * @param threadCnt количество потоков планировщика
 */
virtual void init(const std::shared_ptr<bmpf::Scene> &scene, double intervalDuration,
                 bool showTrace,
                 unsigned int maxOpenSetSize, int gridSize, unsigned int maxNodeCnt, int checkCnt,
                 int threadCnt = 1);

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

Для визуализации работы монотонных траекторий написано два приложения в пакете project.find_trajectory.demo (сцены с одним и четырьмя роботами).

Чтобы написать свой планировщик траекторий, достаточно переопределить в потомке метод init()