[SLAM] 5일차: Particle Filter SLAM 이론과 실습

Particle Filter SLAM 이론과 실습

이번 단계에서는 Particle Filter SLAM의 기본 이론을 배우고, 이를 적용한 간단한 실습을 진행하겠습니다.

1. Particle Filter SLAM 이론

Particle Filter SLAM은 로봇의 위치 추정을 위해 많은 수의 입자(particles)를 사용하는 방법입니다. 각 입자는 로봇의 가능한 위치를 나타내며, 입자 필터는 이 입자들의 가중치를 업데이트하여 위치를 추정합니다.

Particle Filter SLAM의 주요 단계:

1. 초기화 (Initialization): 많은 수의 입자를 초기 상태로 설정
2. 예측 단계 (Prediction Step): 각 입자를 로봇의 모델에 따라 이동
3. 갱신 단계 (Update Step): 센서 데이터를 사용하여 각 입자의 가중치를 업데이트
4. 재샘플링 (Resampling): 낮은 가중치의 입자를 제거하고 높은 가중치의 입자를 복제

예측 단계 수식:

• 각 입자 iii의 예측된 상태:
  xt[i]=f(xt−1[i],ut)+ϵt[i]x_t^{[i]} = f(x_{t-1}^{[i]}, u_t) + \epsilon_t^{[i]}xt[i]​=f(xt−1[i]​,ut​)+ϵt[i]​
  여기서 xt[i]x_t^{[i]}xt[i]​는 입자 iii의 예측된 상태, fff는 로봇의 운동 모델, utu_tut​는 제어 입력, ϵt[i]\epsilon_t^{[i]}ϵt[i]​는 잡음입니다.

갱신 단계 수식:

• 각 입자 iii의 가중치 업데이트:
  wt[i]=P(zt∣xt[i])w_t^{[i]} = P(z_t | x_t^{[i]})wt[i]​=P(zt​∣xt[i]​)
  여기서 wt[i]w_t^{[i]}wt[i]​는 입자 iii의 가중치, P(zt∣xt[i])P(z_t | x_t^{[i]})P(zt​∣xt[i]​)는 측정 모델입니다.

재샘플링 단계:

• 높은 가중치의 입자를 복제하고 낮은 가중치의 입자를 제거하여 새로운 입자 집합 생성

2. 실습: 간단한 Particle Filter SLAM 구현

Python을 이용하여 Particle Filter SLAM을 간단하게 구현해보겠습니다.

 

1. 필요한 라이브러리 설치

pip install numpy scipy matplotlib

 

2. Particle Filter SLAM 구현 코드

import numpy as np
import matplotlib.pyplot as plt

def initialize_particles(num_particles, bounds):
    particles = np.empty((num_particles, 3))
    particles[:, 0] = np.random.uniform(bounds['x'][0], bounds['x'][1], size=num_particles)
    particles[:, 1] = np.random.uniform(bounds['y'][0], bounds['y'][1], size=num_particles)
    particles[:, 2] = np.random.uniform(bounds['theta'][0], bounds['theta'][1], size=num_particles)
    return particles

def predict(particles, u, std, dt=1.0):
    particles[:, 0] += u[0] * np.cos(particles[:, 2]) * dt + np.random.normal(0, std[0], size=len(particles))
    particles[:, 1] += u[0] * np.sin(particles[:, 2]) * dt + np.random.normal(0, std[1], size=len(particles))
    particles[:, 2] += u[1] * dt + np.random.normal(0, std[2], size=len(particles))
    particles[:, 2] %= 2 * np.pi
    return particles

def update(particles, weights, z, R, landmarks):
    for i, landmark in enumerate(landmarks):
        dx = landmark[0] - particles[:, 0]
        dy = landmark[1] - particles[:, 1]
        expected_z = np.sqrt(dx**2 + dy**2)
        weights *= scipy.stats.norm(expected_z, R).pdf(z[i])
    weights += 1.e-300  # avoid round-off to zero
    weights /= sum(weights)
    return weights

def resample(particles, weights):
    indices = np.random.choice(len(particles), size=len(particles), p=weights)
    particles = particles[indices]
    weights.fill(1.0 / len(weights))
    return particles, weights

num_particles = 1000
bounds = {'x': [0, 10], 'y': [0, 10], 'theta': [0, 2*np.pi]}
particles = initialize_particles(num_particles, bounds)
weights = np.ones(num_particles) / num_particles

u = np.array([1.0, 0.1])
std = [0.2, 0.2, 0.05]
z = [2.0, 2.0]
R = 0.5
landmarks = np.array([[2, 2], [8, 8]])

particles = predict(particles, u, std)
weights = update(particles, weights, z, R, landmarks)
particles, weights = resample(particles, weights)

print("추정된 상태:", np.mean(particles, axis=0))

 

3. 결과 확인

위 코드를 실행하면 입자 필터를 통해 추정된 로봇의 상태가 출력됩니다. 입자 필터의 기본 동작 원리를 이해하는 데 도움이 됩니다.

 

Particle Filter SLAM의 기본 이론과 간단한 구현을 통해 SLAM의 또 다른 접근 방식을 이해할 수 있습니다. 이제 더 복잡한 예제를 통해 SLAM을 심화 학습할 준비가 되었습니다.