Задача построения 2D карты. Примеры.¶

Задача¶

Имеется набор измерений \(d={u_1,z_1,u_2,z_2,\dots,u_t,z_t}\) И необходимо построить карту \(m^\star=argmax_{m}P(m|d)\) То есть самую вероятную карту с учетом данных измерений.

Примеры¶

Pasted image 20240526200024.png

Точно известны различные позиции робота и все измерения, которые в них сделаны. Как из этих измерений получений карту?

Pasted image 20240526200149.png Вот ещё пример, где робот перемещался по парку в Австралии и строит 2D карту.

Pasted image 20240526200255.png Продолжим развивать задачу построения карты с вероятностями.

Вероятностная карта робота¶

В начале каждая точка карты (ячейка) имеет вероятность \(0.5\), что означает неопределенность: ячейка может быть как занята, так и свободна. По мере того как робот собирает данные, он обновляет вероятности ячеек на основе полученной информации.

Обновление вероятностей¶

Для каждой ячейки\(m_i\)вероятности обновляются следующим образом:

Если ячейка свободна (робот обнаружил, что путь свободен), вероятность уменьшается, скажем, до значения меньше \(0.5\) (например, \(0.3\)).
Если ячейка занята (робот обнаружил препятствие), вероятность увеличивается, скажем, до значения больше \(0.5\) (например, \(0.8\)).

Модель независимости ячеек¶

Каждая ячейка предполагается независимой от других, поэтому общая вероятность карты\(p(m)\)может быть представлена как произведение вероятностей всех ячеек\(m_i\):

\(p(m) = \prod_{i} p(m_i)\)

где\(p(m_i)\)— вероятность для ячейки\(m_i\).

Пример карты¶

Вот пример вероятностной карты:

Серый цвет (\(p = 0.5\)) — неизвестные зоны.
Белый цвет (\(p < 0.5\)) — свободные зоны.
Черный цвет (\(p > 0.5\)) — занятые зоны.

Обновление карты с использованием байесовской фильтрации¶

Для более точного обновления вероятностей можно использовать байесовский подход. Пусть \(z\)— это наблюдение, а \(m_i\)— состояние ячейки. Тогда обновление вероятности с использованием теоремы Байеса выглядит следующим образом:

\(p(m_i | z) = \dfrac{p(z | m_i) \cdot p(m_i)}{p(z)}\)

где:

\(p(m_i | z)\) — апостериорная вероятность ячейки \(m_i\) с учетом наблюдения\(z\).
\(p(z | m_i)\) — вероятность наблюдения\(z\), если ячейка \(m_i\) занята или свободна.
\(p(m_i)\) — априорная вероятность ячейки\(m_i\).
\(p(z)\) — нормализующая константа.

Пример использования байесовской фильтрации¶

Изначальная вероятность:
\(p(m_i) = 0.5\)
Наблюдение:
Если робот обнаружил препятствие: \(z = \text{"обнаружено препятствие"}\)
Если робот не обнаружил препятствие: \(z = \text{"свободно"}\)
Обновление вероятности:
Если\(z = \text{"обнаружено препятствие"}\):
- \(p(z | m_i = \text{занято}) = 0.9\)
- \(p(z | m_i = \text{свободно}) = 0.1\)
Если\(z = \text{"свободно"}\):
- \(p(z | m_i = \text{занято}) = 0.1\)
- \(p(z | m_i = \text{свободно}) = 0.9\)

Пример расчета¶

Предположим, что робот обнаружил препятствие в ячейке\(m_i\):

\(p(m_i = \text{занято} | z = \text{"обнаружено препятствие"}) = \dfrac{p(z = \text{"обнаружено препятствие"} | m_i = \text{занято}) \cdot p(m_i = \text{занято})}{p(z = \text{"обнаружено препятствие"})}\)

Где:

\(p(z = \text{"обнаружено препятствие"}) = p(z = \text{"обнаружено препятствие"} | m_i = \text{занято}) \cdot p(m_i = \text{занято}) + p(z = \text{"обнаружено препятствие"} | m_i = \text{свободно}) \cdot p(m_i = \text{свободно})\)

Подставляем значения:

\(p(z = \text{"обнаружено препятствие"}) = 0.9 \cdot 0.5 + 0.1 \cdot 0.5 = 0.5\)

Теперь можно вычислить апостериорную вероятность:

\(p(m_i = \text{занято} | z = \text{"обнаружено препятствие"}) = \dfrac{0.9 \cdot 0.5}{0.5} = 0.9\)

Таким образом, вероятность того, что ячейка \(m_i\) занята, увеличивается до 0.9.

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

Python¶

import numpy as np
import seaborn as sns
import matplotlib.pyplot as plt

class ProbabilityMap:
    def __init__(self, size):
        self.size = size
        self.grid = np.full((size, size), 0.5)  # Изначально все ячейки неопределены

    def display(self):
        """Отображение карты"""
        sns.heatmap(self.grid, cmap='coolwarm', annot=True, cbar=False, square=True, linewidths=0.5)
        plt.title("Probability Map")
        plt.xlabel("X")
        plt.ylabel("Y")
        plt.show()

    def update(self, observation):
        """Обновление карты на основе наблюдения"""
        for i in range(self.size):
            for j in range(self.size):
                self.grid[i, j] = self._bayesian_update(self.grid[i, j], observation)

    @staticmethod
    def _bayesian_update(prior_prob, observation):
        """Байесовское обновление вероятности"""
        if observation == "occupied":
            likelihood = 0.9 if prior_prob > 0.5 else 0.1
        elif observation == "free":
            likelihood = 0.1 if prior_prob > 0.5 else 0.9
        else:
            raise ValueError("Invalid observation")

        evidence = (likelihood * prior_prob) + ((1 - likelihood) * (1 - prior_prob))
        posterior_prob = (likelihood * prior_prob) / evidence
        return posterior_prob


# Пример использования
if __name__ == "__main__":
    map_size = 5
    my_map = ProbabilityMap(map_size)

    print("Initial Map:")
    my_map.display()

    print("\nAfter Observation 'occupied':")
    my_map.update("occupied")
    my_map.display()

    print("\nAfter Observation 'free':")
    my_map.update("free")
    my_map.display()