СИСТЕМА ДЛЯ АВТОНОМНОГО ПОСТРОЕНИЯ КАРТ РОБОТАМИ В КРУПНОМАСШТАБНЫХ СРЕДАХ

​A SYSTEM FOR AUTONOMOUS MAPPING OF ROBOT IN LARGE-SCALE SCENES

Jianhan Zhou

master's student, Department of Robotics Systems and Mechatronics, Bauman Moscow State Technical University,

Russia, Moscow

АННОТАЦИЯ

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

Сначала мы исследуем технику одновременной локализации и построения карт на основе оптимизации графов (Karto_SLAM), представляем алгоритмический процесс и принцип ее работы, а также проводим сравнительные эксперименты, чтобы убедиться в преимуществе этого алгоритма при построении карт в крупномасштабных средах. После этого мы изучаем метод определения положения движения робота на основе стратегии границ и, наконец, завершаем моделирование робота и среды, а также симуляция процесса автономного построения карты в Gazebo и Rviz.

ABSTRACT

With the rapid development of robotics technology, the working environment of robots has gradually become complex, which puts forward higher requirements on the intelligence and autonomy of robots. In this paper, we propose a system for robots to autonomously construct 2D environment grid maps in large-scale environments.

We first study the simultaneous localization and map construction technique based on graph optimization (Karto_SLAM), introduce its algorithmic process and working principle, and verify the advantages of this algorithm in constructing maps in large-scale environments through comparative experiments. After that, we study a method of determining the robot's motion position based on boundary strategy, and finally, we model the robot and the environment in Gazebo and Rviz, and then simulate the autonomous map construction process.

Ключевые слова: SLAM; автономно построенные карты; стратегия границ; мобильные роботы; крупномасштабные сцены.

Keywords: SLAM; autonomously constructed maps; boundary strategies; mobile robots; large-scale scenes.

В данной работе мы хотим исследовать систему, позволяющую роботам автономно строить карты окружающей среды в крупномасштабных средах. Поэтому алгоритм построения карты является одним из основных направлений исследования. Во многих случаях мы не можем заранее узнать положение робота, поэтому требованиям нашей задачи отвечает метод одновременной локализации и построения карты (SLAM). Учитывая особенности рабочей сцены робота с большим радиусом действия, в качестве алгоритма для локализации и построения карт мы выбрали Karto_SLAM, основанный на оптимизации графа [1, с. 16].

Программная структура Karto_SLAM

Техника SLAM (т. е. Задача одновременной локализации и построения карты) — это ориентация робота в неизвестной среде с помощью сенсорной информации, которая постепенно строит карту окружения в процессе движения и таким образом завершает точную оценку собственной позы.

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

Процесс Karto_SLAM можно разделить на два процесса, передний и задний, а блок-схема показана на Рисунке 1. Передний конец используется в основном для корреляции данных датчиков, т. е. для сопоставления сканов и обнаружения петель. Поскольку данные датчиков зашумлены, результаты, полученные на переднем конце, будут иметь некоторые ошибки, поэтому для их устранения требуется оптимизация графа на заднем конце. Задний конец не обрабатывает данные датчиков и только оптимизирует карты поз, созданные передним концом, для достижения максимальной вероятности позы.

Рисунок 1. Общая структура Karto_SLAM

Общая процедурная структура Karto_SLAM показана на Рисунке 2:

Рисунок 2. Общая процедурная структура Karto_SLAM

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

Сопоставление сканов (scan-matcher)

Сопоставление сканов — это, по сути, процесс сопоставления данных LiDAR текущего кадра с локальной картой с целью повышения точности локализации робота. Локальная карта — это карта ячеек в пределах определенного расстояния от робота (как показано на Рисунке 3), на которой записана информация о препятствиях в окружении робота. Под сопоставлением мы понимаем оценку соответствия между текущими лазерными данными и информацией о препятствиях, записанной в локальной карте.

Рисунок 3. Локальная карта

Чтобы улучшить локализацию робота, мы строим пространство перемещений и пространство углов поворота на основе позы робота, полученной с помощью одометра, и ищем оптимальную позу робота в этих двух пространствах. При оптимальном положении робота совпадение данных LiDAR текущего кадра с локальной картой будет иметь максимальное значение [2, с. 16].

Схема алгоритма сопоставления сканов показана на Рисунке 4.

Рисунок 4. Схема алгоритма сопоставления сканов

После определения оптимальной позы робота мы строим граф поз.

Построение карты поз

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

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

Обнаружение петель и оптимизация SPA

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

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

Рисунок 5. Обнаружение петель

Где,

 - вершина

 - относительная позиция перед обнаружением петли

 - ограничение, то есть наблюдения за относительной позицией

 – ошибка

В задаче целью оптимизации является:

Где  - информационная матрица(ковариационная матрица)

Меньшее  означает лучшую оптимизацию.

С математической точки зрения, оптимизация SPA（Sparse Pose Adjustment） — это, по сути, алгоритм L-M оптимизации.

Где,

H - Гессиан Матрица;

J - Матрица Якоби;

 - множитель Лагранжа;

x -  вектор вершины

Алгоритм L-M является продуктом комбинации алгоритма Гаусса-Ньютона и метода градиентного спуска. Основная идея заключается в том, чтобы с помощью параметра решить, какому итерационному методу отдать предпочтение - алгоритму Гаусса-Ньютона или методу градиентного спуска, чтобы гарантировать, что каждая итерация будет убывающей [3-4, с. 16].

Симуляция построения карт в крупномасштабной среде

Нам необходимо проверить эффективность алгоритма Karto_SLAM при построении карт в крупномасштабной среде. Построение крупномасштабной среды моделирования в Gazebo является обременительным, поэтому для проверки эффективности алгоритма SLAM для построения карт мы использовали наборы данных LIDAR из Интернета.

С сайта http://ais.informatik.uni-freiburg.de/slamevaluation/datasets.php загрузите набор данных под названием Dataset: ACES Building (Austin).

Преобразование наборов лазерных данных в файлы rosbag для последующего моделирования.

В качестве сравнения мы используем Gmapping, классический алгоритм SLAM, основанный на фильтрации частиц.

Количество частиц установлено на 30 и 50 соответственно, а карты показаны на Рисунке 6.

Рисунок 6. Карты алгоритма Gmapping для 30(слева) и 50(справа) частиц

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

Карта, построенная с помощью алгоритма Karto_SLAM, показана на Рисунке 7.

Рисунок 7. Карта создаётся с помощью алгоритма Karto_SLAM

Сравнивая Рисунок 6 и 7, можно увидеть, что алгоритм Karto_SLAM более точен при построении карт для крупномасштабных сцен.

Автономное построение карт на основе стратегий границ

Суть задачи автономного построения карт заключается в том, чтобы автономно планировать переднее положение робота в соответствии с определенными условиями и зацикливать этот процесс, чтобы робот мог быстро построить полную карту окружения с помощью методов SLAM [5, с. 16]. Для этого мы вводим понятие "границы" и метод автономного построения карт на основе стратегий границ.

Метод поиска границ

Чтобы извлечь границу, необходимо построить карту стоимости, используемую в алгоритме автоматической разведки, на основе карты ячеек, полученной алгоритмом SLAM. Ячейки на карте ячеек, полученной с помощью алгоритма SLAM, могут находиться только в трех состояниях: неизвестном, занятом или пустом [6, с. 16].

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

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

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

• текущий состояние ячейки - "неизвестно"
• хотя бы одна из четырех ячеек, соседних с текущей, имеет состояние "пусто"

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

Поиск по ширине (BFS: Breadth First Search) — это стратегия обхода, которая фактически представляет собой иерархический процесс поиска. Начиная с ячейки, в которой находится центр робота, обходятся 8 окружающих ячеек. После этого он обходит эти 8 клеток в качестве начальной точки, пока не найдет все клетки на карте стоимости, которые удовлетворяют граничным условиям. Соседние ячейки, удовлетворяющие граничным условиям, образуют одну границу [7, с. 16].

Структура алгоритма автономной разведки показана на Рисунке 8.

Рисунок 8. Алгоритм для автономной разведки

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

Функция стоимости

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

Мы считаем, что чем ближе граница к текущему положению робота и чем длиннее граница, тем более целесообразно ее исследовать, чтобы повысить эффективность исследования и получить максимальное поле зрения. Граница с наименьшей стоимостью принимается за оптимальную границу. Функция стоимости  для границы и стоимость  для оптимальной границы определяются следующим образом[6-7, с. 16]:

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

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

Моделирование автономно построенных карт

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

Сначала робот моделируется с помощью компонента URDF.

Создана модель четырехколесного цилиндрического робота с четырьмя колесами, состоящими из двух ведущих и двух опорных колес с карданным шарниром, приводимого в движение дифференциальным приводом и оснащенного однолинейным LIDAR. Окончательная модель робота показана на Рисунке 9.

Рисунок 9. Модель робота

Основные соответствующие параметры LIDAR следующие:

Частота обновления: 40 Hz

Количество выборок: 1081

Диапазон выборки: -135 ~ 135

Диапазон выборки радара: 0.1м ~ 30м

Точность выборки: 0,01 м

Создавайте среду моделирования в Gazebo.

Окончательный построенный сценарий показан на Рисунке 10: прямоугольная комната длиной 13 м и шириной 8 м с препятствиями, расположенными внутри.

Рисунок 10. Среда моделирования

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

Рисунок 11. Автономная разведка

Рисунок 12. Создание карты

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

Рисунок 13. Карта, созданная автономно

Рисунок 14. Карта, построенная роботами, управляемыми оператором

Результаты

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

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

Границы извлекаются из карты, построенной алгоритмом Karto_SLAM, и порядок, в котором робот исследует границы, сортируется в соответствии с функцией стоимости, и наконец, моделирование автономного построения карты на основе стратегий границ осуществляется в Gazebo и Rviz.

Список литературы:

1. Чжан Тяньи. Разработка и реализация SLAM и автономной навигации для деформируемого робота-спасателя угольной шахты [D]. Ляонинский университет инженерии и технологии, 2021 год.
2. Лу Гуокунь. SLAM и автономная разведка робота-спасателя [D]. Гуандунский технологический университет, 2020 г. DOI:10.27029/d.cnki.ggdgu.2020.001382.
3. Olson, E. B. “Real-time correlative scan matching.” Robotics and Automation, 2009. ICRA '09. IEEE International Conference on IEEE, 2009.
4. Konolige, Kurt, et al. “Efficient sparse pose adjustment for 2D mapping.” 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems IEEE, 2010.
5. Kalyanasundaram B, Pruhs K R .Constructing competitive tours from local information[J].Theoretical Computer Science, 1994, 130(1):125-138.DOI:10.1016/0304-3975(94)90155-4.
6. Thrun S. Exploration in Active Learning[J].The handbook of brain theory and neural networks, 1995.
7. A. Mobarhani, S. Nazari, A. H. Tamjidi, and H. D. Taghirad, “Histogram based frontier exploration,” in Ieee/rsj International Conference on Intelligent Robots and Systems, pp. 1128–1133