Задача одновременной локализации и построения карты. EKF SLAM.¶

EKF SLAM (Extended Kalman Filter Simultaneous Localization and Mapping) — это метод, который используется в робототехнике для одновременного построения карты неизвестного пространства и определения местоположения робота в этой карте. Чтобы понять, как работает EKF SLAM, давай разобьем его на более простые части.

SLAM (Simultaneous Localization and Mapping)¶

SLAM — это задача, где робот: 1. Создает карту окружающей среды. 2. Определяет свое местоположение в этой карте.

Kalman Filter (Фильтр Калмана)¶

Фильтр Калмана — это математический инструмент, который используется для оценивания состояния системы, когда есть шумные измерения. В контексте роботов это может быть: - Состояние системы: Положение и ориентация робота. - Измерения: Данные с сенсоров, такие как данные с камер, лидаров или других датчиков.

Фильтр Калмана помогает предсказывать будущее состояние системы, а затем обновлять это предсказание на основе новых данных.

Extended Kalman Filter (Расширенный фильтр Калмана)¶

Расширенный фильтр Калмана (EKF) — это версия фильтра Калмана, которая работает с нелинейными системами. В реальной жизни большинство систем, включая роботов, нелинейны, и EKF позволяет обрабатывать такие системы, используя приближенные (линейные) модели.

Как работает EKF SLAM¶

Предсказание (Prediction):
Робот использует свою модель движения (например, скорость и направление), чтобы предсказать, где он окажется через небольшой промежуток времени.
Используя эту информацию, фильтр обновляет предполагаемое положение робота и карту.
Обновление (Update):
Робот собирает данные с сенсоров (например, видит препятствия или ориентиры).
Эти данные используются для корректировки предположений о положении робота и обновления карты.

Пример¶

Представь, что у тебя есть робот, который едет по комнате с несколькими ориентирующими точками (например, стульями или столами): 1. Робот начинает с некой начальной позиции и знает, что он может двигаться вперед или поворачивать. 2. По мере движения он предсказывает, где окажется через секунду, используя свои датчики движения. 3. Затем он сканирует окружающую среду и замечает стул в определенном месте. 4. Робот использует данные о стуле, чтобы откорректировать свое предположение о собственном положении и обновить карту комнаты.

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

Python¶

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

class EKF_SLAM:
    def __init__(self, num_landmarks, initial_pose, motion_noise, measurement_noise):
        self.num_landmarks = num_landmarks
        self.pose = initial_pose
        self.motion_noise = motion_noise
        self.measurement_noise = measurement_noise
        self.landmarks = self.generate_landmarks()
        self.covariance = np.zeros((3 + 2 * num_landmarks, 3 + 2 * num_landmarks))

    def generate_landmarks(self):
        return np.random.uniform(-10, 10, size=(self.num_landmarks, 2))

    def motion_model(self, u):
        # Простая модель движения
        v = np.random.normal(u, self.motion_noise)
        return v

    def measurement_model(self):
        # Модель измерения
        measured_landmarks = []
        for landmark in self.landmarks:
            dx = landmark[0] - self.pose[0]
            dy = landmark[1] - self.pose[1]
            d = np.sqrt(dx ** 2 + dy ** 2)
            bearing = np.arctan2(dy, dx) - self.pose[2]
            measured_landmarks.append([d, bearing])
        return np.array(measured_landmarks)

    def predict(self, u):
        # Предсказание
        v = self.motion_model(u)
        self.pose[0] += v * np.cos(self.pose[2])
        self.pose[1] += v * np.sin(self.pose[2])
        self.pose[2] += np.random.normal(0, self.motion_noise)

        G = np.eye(3 + 2 * self.num_landmarks)
        G[0, 2] = -v * np.sin(self.pose[2])
        G[1, 2] = v * np.cos(self.pose[2])

        self.covariance = G @ self.covariance @ G.T

    def update(self, z):
        # Обновление
        H = np.zeros((2, 3 + 2 * self.num_landmarks))
        for i, landmark in enumerate(self.landmarks):
            dx = landmark[0] - self.pose[0]
            dy = landmark[1] - self.pose[1]
            q = dx ** 2 + dy ** 2
            sqrt_q = np.sqrt(q)

            H[0, 0] = -sqrt_q * dx
            H[0, 1] = -sqrt_q * dy
            H[0, 2] = 0
            H[0, 3 + 2 * i] = sqrt_q * dx
            H[0, 3 + 2 * i + 1] = sqrt_q * dy

            H[1, 0] = dy
            H[1, 1] = -dx
            H[1, 2] = -q
            H[1, 3 + 2 * i] = -dy
            H[1, 3 + 2 * i + 1] = dx

            z_hat = np.array([
                sqrt_q,
                np.arctan2(dy, dx) - self.pose[2]
            ])

            Y = z - z_hat
            Y[1] = np.arctan2(np.sin(Y[1]), np.cos(Y[1]))  # Нормализация угла

            H = H @ self.covariance @ H.T + np.eye(2) * self.measurement_noise
            K = self.covariance @ H.T @ np.linalg.inv(H)

            self.pose += K @ Y
            self.covariance = (np.eye(3 + 2 * self.num_landmarks) - K @ H) @ self.covariance

    def plot(self):
        sns.set()
        plt.scatter(self.pose[0], self.pose[1], color='r', marker='o', label='Robot Pose')
        plt.scatter(self.landmarks[:, 0], self.landmarks[:, 1], color='b', marker='*', label='Landmarks')
        plt.legend()
        plt.xlabel('X')
        plt.ylabel('Y')
        plt.title('EKF SLAM')
        plt.grid(True)
        plt.show()

if __name__ == "__main__":
    # Пример использования
    num_landmarks = 5
    initial_pose = np.array([0, 0, 0])
    motion_noise = 0.1
    measurement_noise = 0.1

    ekf_slam = EKF_SLAM(num_landmarks, initial_pose, motion_noise, measurement_noise)

    for _ in range(10):
        u = np.random.uniform(0, 1)
        ekf_slam.predict(u)
        z = ekf_slam.measurement_model()
        ekf_slam.update(z)

    ekf_slam.plot()