Ref. Open: SLAM(Simultaneous localization and mapping)과 칼만 필터

2014년 8월 21일 목요일

SLAM(Simultaneous localization and mapping)과 칼만 필터

SLAM(Simultaneous localization and mapping) - Kalman Filter

동시적 위치추정 및 지도작성과 칼만 필터

국문으로 번역된 이름이 마음에 들지 않지만, 마땅히 다르게 번역할 방법이 없어서 기존에 사용되던 것들 중에서 차용하였습니다.

제목에서 느낄 수 있는 것처럼 이동 로봇이 미지의 세계를 방문할 때 자신의 위치추정과 지도작성을 동시에 수행하는 것을 말한다. 동시적이라고 하면 시간의 흐름상 완전한 동시성을 의미하는 것처럼 느껴지기 때문에 사실은 일관된, 연관된 정도로 번역하는 것이 자연스럽다고 생각된다.

칼만 필터를 이용한 이동 로봇의 SLAM에 대해서 먼저 기술하고 다음으로 다른 종류의 필터(EKF, UKF, Particle Filter)에 대해 적어볼 예정입니다.

위치 추정과 지도 작성은 동시에 진행하는 것이 타당하다. 위치를 인식하기 위해서는 지도가 정확해야 하는데, 지도의 정확성은 위치의 정확성에 의존하기 때문이다. SLAM이 어려운 이유는 그림에서 보는 것처럼 예측할 수 없는 요소들이 많기 때문이다.

왜 SLAM은 어려운 문제인가?

세상엔 정확한 센서는 존재하지 않기 때문에 항상 노이즈를 포함하고 있다. 정확한 센서 하나만 있었더라도 이렇게 힘들게 고민할 이유가 없었을 것이다.

위의 그림을 조건부 확률식으로 적어보면 아래와 같다.

조건부 확률식으로 전개한 SLAM 문제

상태 벡터 \( \mathbf{x}_{k-1} \)에서 \( \mathbf{x}_{k} \)로 이동하는 로봇이 \( \mathbf{m}_{i}, \mathbf{m}_{j}\) 랜드 마크를 관측한 값이 측정치 \( \mathbf{z}_{k,j}\) 로 입력된다. \( \mathbf{u}_k \)는 현재 로봇의 조종 명령이다.

조건부 확률 식을 말로 풀이하면 " \( \mathbf{z}_{1:t}, \mathbf{u}_{1:t} \)가 만족 되는 경우에 즉, 최초 시점 1에서 현재 시점 \( t \)까지의 관측 값과 조종 명령이 주어질 때, 현재 위치를 의미하는 상태 벡터 \( \mathbf{x}_t \)와 지도를 의미하는 \( \mathbf{m} \)이 어떤 확률 분포를 가지는가?" 로 서술할 수 있다.

확률은 그 합이 '1'이 되어야 하기 때문에 조건이 주어지지 않은 경우의 현재 상태와 지도에 대한 확률과 합하면 조건부 확률 값은 '1'이 된다. 위와 같은 조건부 확률 식으로 전개해 두고 나면 앞서 기술한 베이지안 추정 방법이나 마르코프 위치 인식 방법을 사용할 수 있게 되므로 매우 유리한 점이 생긴다.

상태 천이 행렬이 선형(Linearity: Homogeneity와 superposition을 만족)인 경우에 적용할 수 있다. 선형 상태 천이를 한다는 것은 상태 천이 함수가 선형이라는 말이며 등속, 등가속 운동처럼 이후의 운동의 예측할 수 있는 경우를 말한다.

그림에서 상태 천이 함수는 \( A \) 행렬이다. 등속 모델인 경우엔 \( \mathbf{A}=\left[\begin{array}{cccc}1 & 0 & 1 & 0 \\ 0 & 1 & 0 & 1 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right] \), 상태 벡터는 \(\mathbf{x}=\left[\begin{array}{c}x \\ y \\ \dot{x} \\ \dot{y} \end{array}\right] \) 로 설정하면 2차원 좌표에서 등속으로 운동하는 물체에 대한 칼만 필터의 추정식이 완성된다. 실제 시스템 모델이 맞도록 공정 잡음과 측정 잡음을 설정하면 된다.

간단하게 몇 줄의 코드 만으로도 칼만 필터는 훌륭하게 동작합니다. 아래에 2차원에서 움직이는 물체에 대한 칼만 필터 예제를 게시합니다.

kalmandemo

칼만 필터 예제

% 2D Kalman filter example
% 2014. 8. 21
% refopen.blogspot.kr

2차원에서 움직이는 참 값 생성

true = [0:0.03:pi/2; sin(0:0.03:pi/2)];
close all;
plot(true(1, :), true(2, :), 'b*-');

파라미터 초기화

x = [0; 0; 0; 0];
A = [1 0 1 0;
    0 1 0 1;
    0 0 1 0;
    0 0 0 1];
sigmax = 0.01;
sigmay = 0.01;
sigmaxdot = 0.01;
sigmaydot = 0.01;
Q = [sigmax.^2 0 0 0;
    0 sigmay.^2 0 0;
    0 0 sigmaxdot.^2 0;
    0 0 0 sigmaydot.^2];
H = [1 0 0 0; 0 1 0 0];
R = [0.05 0;
    0 0.05];

xhatk = x;
Phatk = Q;

kfest = zeros(size(true));
measure = zeros(size(true));

예측과 갱신 반복

for n=1:size(true, 2)   %% 참 값으로 만든 횟수 만큼

    % 측정값 생성
    % 표준 편차가 0.03인 가상의 노이즈를 더한 가상의 측정값
    zk = true(:, n) + randn(2, 1)*0.03;
    measure(:, n) = zk;

    % 예측 과정
    xbark = A*xhatk;
    Pbark = A*Phatk*A' + Q;

    % 갱신 과정
    K = (Pbark*H'*(H*Pbark*H'+R)^-1);
    xhatk = xbark + K*(zk - H*xbark);
    Phatk = (eye(4) - K*H)*Pbark;

    kfest(:, n) = xhatk(1:2);

end

hold on;
plot(measure(1, :), measure(2, :), 'k*-');
plot(kfest(1, :), kfest(2, :), 'r*-');

legend('true', 'measure', 'kf est.');

Ref. Open