ФАЙЛ «MATH.PY»

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

ИНИЦИАЛИЗАЦИЯ БИБЛИОТЕК

import math
import heapq

math: Библиотека math предоставляет множество математических функций и констант, которые могут быть полезны для выполнения математических операций.

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

В данном случае устанавливать дополнительные библиотеки не нужно.

ИНИЦИАЛИЗАЦИЯ КЛАССА

def __init__(self, aruco_map):
    self.aruco_map = aruco_map

Здесь aruco_map — это словарь, который содержит данные о положении маркеров ArUco. Этот словарь передается при создании объекта класса, чтобы в дальнейшем ровер мог использовать эту информацию для своих вычислений, поиска путей и других задач, связанных с навигацией и локализацией.

МЕТОД ПОСТРОЕНИЯ ГРАФОВ

def build_graph(self):
        """
        Построение графа смежности на основе дистанций между маркерами.

        Возвращает:
        - dict: Граф смежности, где ключами являются идентификаторы маркеров, а значениями - словари соседних маркеров и расстояний до них.

        Описание:
        Соединяет маркеры, расположенные на расстоянии менее одного метра друг от друга.
        """

        graph = {}

        for id1, marker1 in self.aruco_map.items():
            neighbors = {}
            for id2, marker2 in self.aruco_map.items():
                dist = math.sqrt((marker2['x'] - marker1['x'])**2 + (marker2['y'] - marker1['y'])**2)
                if dist < 1.0:  # Соединяем маркеры, если они находятся на расстоянии менее 1 метра
                    neighbors[id2] = dist
            graph[id1] = neighbors
        return graph

Метод build_graph создаёт граф, где узлы — это маркеры, а рёбра — связи между маркерами, расположенными на расстоянии менее 1 метра друг от друга.

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

Алгоритм построения графа

Циклы перебора маркеров:

Сначала выбирается первый маркер id1, затем метод сравнивает его с каждым другим маркером id2 в aruco_map.

Расчёт расстояния:

Для каждого сравниваемого маркера рассчитывается расстояние с использованием теоремы Пифагора:

dist=(marker2[′x′]−marker1[′x′])2+(marker2[′y′]−marker1[′y′])2dist=(marker2[′x′]−marker1[′x′])2+(marker2[′y′]−marker1[′y′])2

Добавление соседей в граф:

Если расстояние меньше 1 метра, то id2 добавляется в список соседей для id1, и расстояние сохраняется как вес ребра.

Возврат графа:

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

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

МЕТОД НАХОЖДЕНИЯ БЛИЖАЙШЕГО МАРКЕРА (ID)


 def find_closest_marker_id(self):
        """
        Нахождение ближайшего маркера к текущей позиции робота.

        Возвращает:
        - int: Идентификатор ближайшего маркера.

        Описание:
        Рассчитывает расстояние до каждого маркера в aruco_map и возвращает идентификатор маркера с минимальным расстоянием.
        """
        return min(self.aruco_map, key=lambda id: math.hypot(self.aruco_map[id]['x'] - self.robot_state.x, self.aruco_map[id]['y'] - self.robot_state.y))

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

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

Алгоритм нахождения ближайшего маркера

Метод find_closest_marker_id работает следующим образом:

Определение текущей позиции робота:

Позиция ровера определяется через атрибуты self.robot_state.x и self.robot_state.y.

Расчёт расстояний:

math.hypot используется для вычисления евклидова расстояния от текущей позиции ровера до каждого маркера:distance=(xmarker−xrobot)2+(ymarker−yrobot)2distance=(xmarker−xrobot)2+(ymarker−yrobot)2

Поиск минимального расстояния:

Метод возвращает идентификатор маркера, для которого расстояние минимально.

МЕТОД АЛГОРИТМА А*

  async def astar_algorithm(self, start, goal):
        """
        Реализация алгоритма A* для поиска пути.

        Параметры:
        - start (int): Идентификатор начального маркера.
        - goal (int): Идентификатор целевого маркера.

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

        Описание:
        Использует алгоритм A* для нахождения кратчайшего пути на графе маркеров. 
        Возвращает путь в виде списка идентификаторов маркеров.
        """
        open_set = []
        heapq.heappush(open_set, (0, start))
        came_from = {}
        g_score = {node: float('inf') for node in self.aruco_map}
        g_score[start] = 0
        f_score = {node: float('inf') for node in self.aruco_map}
        f_score[start] = self.heuristic(start, goal)

        while open_set:
            current = heapq.heappop(open_set)[1]

            if current == goal:
                path = []
                while current in came_from:
                    path.append(current)
                    current = came_from[current]
                return path[::-1]  # Return reversed path

            for neighbor, weight in self.graph[current].items():
                tentative_g_score = g_score[current] + weight
                if tentative_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g_score
                    f_score[neighbor] = tentative_g_score + self.heuristic(neighbor, goal)
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))
        return []

Этот метод реализует алгоритм A* для поиска кратчайшего пути между двумя маркерами на графе. Он асинхронный (async), что позволяет выполнять его в асинхронном контексте.

Алгоритм A* — один из самых эффективных алгоритмов для поиска кратчайшего пути в графе, и он часто используется в задачах построения пути ровера для планирования маршрута.

Алгоритм A* работает следующим образом

Инициализация:

Создаётся открытый набор (open_set), который содержит начальный узел (маркер) с начальной стоимостью 0.
Инициализируются словари came_from, g_score и f_score для отслеживания путей и стоимости перемещения по графу.

Поиск пути:

Пока в open_set есть элементы, выбирается узел с наименьшей стоимостью f_score.
Если узел совпадает с целью (goal), строится и возвращается путь.

Обновление соседей:

Для каждого соседа текущего узла рассчитывается временная стоимость tentative_g_score.
Если эта стоимость меньше текущей, обновляется came_from, g_score и f_score для соседа, и он добавляется в open_set для дальнейшего рассмотрения.

Возврат пути:

Если удалось достичь цели, метод возвращает список идентификаторов маркеров, составляющих кратчайший путь от start до goal.

МЕТОД ЭВРИСТИЧЕСКОЙ ФУНКЦИИ

def heuristic(self, a, b):
        """
        Эвристическая функция для оценки расстояния между двумя маркерами.

        Параметры:
        - a (int): Идентификатор первого маркера.
        - b (int): Идентификатор второго маркера.

        Возвращает:
        - float: Расстояние между маркерами в метрах.

        Описание:
        Рассчитывает евклидово расстояние между двумя маркерами на основе их координат.
        """
        return math.sqrt((self.aruco_map[b]['x'] - self.aruco_map[a]['x'])**2 + (self.aruco_map[b]['y'] - self.aruco_map[a]['y'])**2)

Этот метод используется в алгоритме A* для оценки расстояния между двумя маркерами. Здесь рассчитывается евклидово расстояние между координатами двух маркеров.

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

Процесс подготовки кода:

Создайте файл и назовите его Math.py. Обязательно называйте файлы так, как написано здесь, иначе связка кодов работать не будет.

Примечание: папка test названа для удобства ведения статьи, ваша папка может называться по другому. Название папки ни на что не влияет.

Поэтапно копируйте части кода в ваш файл, в итоге получится такое окно программирования:

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

PreviousОписание управляющей программы NextФАЙЛ «ARUCO.PY»

Last updated 7 days ago