Я полагаю, что в процессе использования дронов серии P все знают функцию обхода препятствий дронов серии P. В предыдущем техническом сообщении: Prometheus (P450) - предотвращение препятствий внутри и снаружи, он подробно объяснил офицерам обход препятствий Алгоритм Принцип и фактическая демонстрационная статья о полете. Сегодня, чтобы быстрее провести вторичную разработку, мы сосредоточимся на реализации кода алгоритма A-star.
В проекте prometheus пакет функций алгоритма A_star хранится в
Путь Prometheus/Modules/planning/global_planning, как показано.
Войдя в папку src пакета функций global_planning, мы можем увидеть 5 файлов cpp, как показано на следующем рисунке,
A_star.cpp: основная часть реализации функций, запланированных A_star;
global_planner.cpp: в основном определяет получателя некоторых сообщений, таких как статус самолета, данные облака точек, статус целевой точки и другие сообщения, и подготавливает файл для алгоритма A_star;
global_planner_node.cpp: основная функция, создающая объект global_planner для вызова алгоритма a_star;
Occup_map.cpp: Он в основном отвечает за преобразование данных облака точек о препятствиях, полученных датчиком, в информацию о препятствиях на растровой карте для подготовки к алгоритму планирования A_star;
tools.cpp: в основном используется для публикации на терминале некоторых сообщений, которые интересуют пользователя, или сообщений процесса программы реакции;
Давайте посмотрим, как реализуется план A_star.
Во-первых, когда мы войдем в основную функцию, мы обнаружим, что в ней всего несколько коротких строк, как показано на следующем рисунке.
Самый важный код обведен красной рамкой: он объявляет объект класса Global_Planner с именем global_planner, а затем вызывает функцию инициализации объекта global_planner;
Далее мы входим в функцию инициализации, которая определена в файле global_planner.cpp, как показано на рисунке.
Сначала он считывает некоторые необходимые параметры, и значение каждого параметра дано на рисунке, затем он подписывается на точку цели, статус БПЛА, метод обновления карты и т. д., когда подписчик получит сообщение, он передаст полученное сообщение. в функцию обратного вызова для обработки данных;
Затем инициализируются некоторые издатели для публикации соответствующих сообщений.Функция, соответствующая каждой функции, дана на рисунке;
Далее нам нужно инициализировать планировщик A_star Код показан на рисунке Мы объявляем объект с именем Astar и указатель на него Astar_ptr, затем передаем хэндл в функцию инициализации объекта A_star;
Эта функция объявлена в файле A_star.cpp, как показано на рисунке.
Прежде всего, сначала прочитайте некоторые необходимые параметры, и информация о конкретных параметрах приведена на рисунке;
Объявлен tie_breaker_, этот параметр в основном предназначен для предотвращения появления симметричных путей, поэтому на основе расчетной стоимости определенного пути увеличивается очень небольшая сумма, чтобы уменьшить вероятность симметричных путей;
path_node_pool_ — это контейнер, используемый для хранения узлов, которые посетил планировщик.Каждый член контейнера представляет каждую точку на карте сетки, которая включает местоположение, статус и т. д. точки карты;
Затем используйте функцию инициализации объекта Occupy_map_ptr для инициализации карты;
Как показано на рисунке, это код для инициализации карты.Основной код отмечен красным прямоугольником, что указывает на то, что формула расчета трех измерений x, y и z представляет собой размер, деленный на его разрешение. Вышеприведенное изображение равно 0,2, что означает 1. Длина метров будет разложена на 5 сеток на карте сетки, а затем мы можем рассчитать размер всей карты сетки, вычислив размер определенного номера сетки каждого измерения;
Затем объявите контейнер occupancy_buffer_ как размер карты сетки.Это контейнер, используемый для хранения информации о препятствиях на карте.Все это состоит из 0 и 1. 0 означает, что точка сетки не занята, а 1 означает, что сетка точка, на ней есть препятствия, инициализируем ее всеми 0;
Затем вернитесь к A_star.cpp и прочитайте параметры инициализированной карты, как показано на следующем рисунке;
После выполнения функции инициализации объекта A_star вернитесь к точке вызова функции, то есть к функции Global_Planner::init, как показано на рисунке;
Затем выполните состояние и инструкции планировщика инициализации.Будь то симуляция или реальный полет, программа одна и та же, но есть некоторые различия в параметрах, которые мы вводим.После завершения ввода параметров программа входит в красное поле, и функция обратного вызова сообщения ros обрабатывается, некоторые функции обратного вызова, которые мы объявили ранее, как показано ниже,
Они не будут перезванивать сразу после получения сообщения, а будут продолжать получать сообщения в фоновом режиме, пока программа не запустится до ros::spinOnce(), и функция обратного вызова не будет выполнена единообразно один раз;
функция target_cb, основная функция которой состоит в том, чтобы получить целевое очко, заданное пользователем;
Функция drone_state_cb считывает состояние самолета, возвращенное контроллером полета;
функция обратного вызова основного цикла mainloop_cb, отвечающая за наиболее важные вопросы планирования;
Введите функцию mainloop_cb,
Приведенный выше код проверяет статус, чтобы убедиться, что текущий статус соответствует требованиям планирования;
Приведенный выше код, проверьте состояние exec_state, давайте сосредоточимся на чтении кода в красном поле, во-первых, это сброс планировщика A_star, код выглядит следующим образом:
Очистить расширенный набор узлов и набор узлов пути, объявить пустую последовательность приоритетов, заменить пустую последовательность приоритетов на open_set_set, то есть очистить открытый набор, удалить ранее обработанные узлы один за другим, восстановить состояние инициализации и, наконец, поместите use_node_num_ и iter_num_ в 0, вернитесь в состояние инициализации, и выполнение будет завершено.
Вернитесь к функции mainloop_cb и выполните следующее предложение, то есть введите функцию Astar::search следующим образом:
Сначала проверьте, есть ли препятствия в целевой точке, а затем продолжите выполнение следующих процедур, если препятствий нет;
Затем инициализируйте начальную точку, объявите объект структуры узла, который используется для хранения данных начальной точки.Конкретный код приведен выше;
Затем поместите объект начальной точки в открытый набор, готовый войти в основной цикл!
Код основного цикла такой же, как и выше, сначала проверьте, что open_set не пуст, затем удалите узел с наименьшим значением f в открытом наборе, а затем оцените, достиг ли извлеченный узел условия завершения.
Если условие завершения достигнуто, назначьте текущий узел для terminate_node, затем передайте его в функцию retrievePath, выполните поиск назад и сохраните путь в контейнере path_node_.Код выглядит следующим образом:
Если условие завершения не выполняется, извлеките текущий узел из открытого набора и поместите его в закрытый набор, а затем разверните текущий узел.Код выглядит следующим образом;
Таким образом, поиск продолжается до тех пор, пока открытый набор не станет пустым, или не будет достигнуто условие завершения, или не будет достигнуто максимальное количество поисков, после чего цикл завершится.
Выше приведен конкретный процесс реализации кода алгоритма A_star в проекте Prometheus.
Наконец, прилагается скриншот запуска моделирования A_star.
Вышеприведенное исследование кода лазерной функции предотвращения препятствий БПЛА серии P. Я полагаю, что после его прочтения у вас будет глубокое понимание кода. Вы также можете принять участие в оптимизации кода и отправке вопросов. Это это команда проекта Prometheus с открытым исходным кодом. Участники ценят поддержку и ценят вклад.
Благодаря быстрому развитию технологий Amu Lab будет идти в ногу с темпами развития технологий и продолжать рекомендовать всем новейшие технологии и оборудование в индустрии робототехники. Наблюдение за тем, как студенты, прошедшие обучение у нас, делают рывки в технологиях, является величайшей ценностью нашего обучения. Если вы работаете в индустрии роботов, обратите внимание на нашу публичную учетную запись, мы продолжим публиковать самую ценную информацию и технологии в индустрии роботов.
Amu Lab занимается обучением и интеллектуальным оборудованием передовых ИТ-технологий, делая исследования и разработки роботов более эффективными!