Задача локализации. Пример.

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

Пример локализации робота в помещении с использование лидара:

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

def simulate_lidar_data_in_room():
    # Параметры комнаты
    room_size = 10  # Размер комнаты в метрах (квадрат 10м x 10м)

    # Параметры объектов
    center_square_size = 2
    circle_center = (8, 8)
    circle_radius = 1

    # Инициализация данных лидара
    lidar_data = []

    # Стены комнаты (четыре стороны)
    for x in np.linspace(0, room_size, 100):
        lidar_data.append((x, 0))  # Нижняя стена
        lidar_data.append((x, room_size))  # Верхняя стена
    for y in np.linspace(0, room_size, 100):
        lidar_data.append((0, y))  # Левая стена
        lidar_data.append((room_size, y))  # Правая стена

    # Квадратный объект в центре
    for x in np.linspace((room_size - center_square_size) / 2, (room_size + center_square_size) / 2, 50):
        lidar_data.append((x, (room_size - center_square_size) / 2))  # Нижняя грань
        lidar_data.append((x, (room_size + center_square_size) / 2))  # Верхняя грань
    for y in np.linspace((room_size - center_square_size) / 2, (room_size + center_square_size) / 2, 50):
        lidar_data.append(((room_size - center_square_size) / 2, y))  # Левая грань
        lidar_data.append(((room_size + center_square_size) / 2, y))  # Правая грань

    # Круглый объект в углу
    angles = np.linspace(0, 2*np.pi, 100)
    for angle in angles:
        x = circle_center[0] + circle_radius * np.cos(angle)
        y = circle_center[1] + circle_radius * np.sin(angle)
        lidar_data.append((x, y))

    return np.array(lidar_data)

def plot_lidar_data_with_gaussian_seaborn(lidar_data, robot_position, sigma):
    plt.figure(figsize=(10, 10))
    plt.scatter(lidar_data[:, 0], lidar_data[:, 1], s=5)

    # Генерация данных для нормального распределения
    x = np.random.normal(robot_position[0], sigma, 1000)
    y = np.random.normal(robot_position[1], sigma, 1000)

    # Использование Seaborn для создания контурного графика
    sns.kdeplot(x=x, y=y, levels=10, fill=True)


    plt.scatter(robot_position[0], robot_position[1], s=200, color="red", label="Robot")



    plt.xlabel('X [m]')
    plt.ylabel('Y [m]')
    plt.title('Simulated Lidar Data with Gaussian Distribution (Seaborn)')
    plt.grid(True)
    plt.axis('equal')
    plt.legend()
    plt.show()

# Генерация и визуализация данных лидара для комнаты
lidar_data = simulate_lidar_data_in_room()
robot_position = [2, 8]  # Координаты робота
sigma = 0.4  # Дисперсия нормального распределения
plot_lidar_data_with_gaussian_seaborn(lidar_data, robot_position, sigma)

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