FastSLAM_개념과 실습

경희대학교 황효석 교수님의 2021-2 로봇 프로그래밍 수업을 듣고 공부한 것입니다.

FastSLAM은 Particle Filter를 기반으로 동작합니다. FastSLAM은 랜드마크의 수가 수천개 이상 되었을 때 EKF-SLAM과 비교하여 장점이 있습니다. EKF-SLAM은 랜드마크 수의 제곱에 비례하여 계산 량이 증가하기 때문에 랜드마크의 수가 수백개 정도 되면 계산량이 상당히 많습니다.

하지만 FastSLAM에서는 계산량이 랜드마크 수와 선형으로 비례하여 증가하기 때문에 EKF-SLAM보다 그래도 좀 더 나은 편입니다.

Exercise

more off;

clear all;
close all;
addpath('tools');

landmarks = read_world('../data/world.dat');
data = read_data('../data/sensor_data.dat');

world.dat 에 들어 있는 것은 1~9 까지 실제 랜드마크 위치의 좌표가 들어있다.

sensor.dat 에는 odometry 의 rotation, translation, encoder 값이 들어 있고,
sensor 에는 (range bearing sensor) 받은 랜드마크의 거리와 각도가 들어 있다.

N = size(landmarks,2);
noise = [0.01, 0.02, 0.01];

noise 는 motion 에 대한 노이즈 값이다. motion 값의 order 는 rotation 1, translation, rotation 2 인데,
각각의 값에 대한 variance 를 나타낸다.

numParticles = 200;

particle 는 100 개를 사용할 것이다.

particles = struct;

particle 는 struct 이다.

particles = struct;
for i = 1:numParticles
  particles(i).weight = 1. / numParticles;
  particles(i).pose = zeros(3,1);
  particles(i).history = cell();
  for l = 1:N % initialize the landmarks aka the map
    particles(i).landmarks(l).observed = false;
    particles(i).landmarks(l).mu = zeros(2,1);    % 2D position of the landmark
    particles(i).landmarks(l).sigma = zeros(2,2); % covariance of the landmark
  end
end

particle 을 초기화 해 주는데, particle 은 weight 과 pose 와 history 가 있다.

weight 은 처음에는 particle 갯수만큼 나눠져서 모두가 같은 weight 을 가지게 된다. 이후에 prediction 한 랜드마크의 위치와 observed 된 랜드마크의 위치를 가지고 particle 들에 대해서 weight 을 별도로 부과해주게 된다.

particle 의 위치는 zeros 를 통해 모두 0,0 으로 초기화해주었다.

history 는 로봇의 pose 를 계속 쌓아두는 역할을 한다. 나중에 이어주면 로봇의 trajectory (궤적) 이 된다.

모든 랜드마크들은 서로 다른 랜드마크들에 대해서 독립적이라고 가정한다. 따라서 extended kalman filter 와 달리 랜드마크들 사이의 covariance 가 필요하지 않다. 즉, 랜드마크는 자기 자신의 covariance 값 (2x2) 과 자기 자신의 위치만 가지고 있으면 된다. (그래서 속도가 빠르다. FAST)

showGui = true;  

실행되는 동안 볼 수 있다.

for t = 1:size(data.timestep, 2)
%for t = 1:50
    printf('timestep = %d\n', t);

    % Perform the prediction step of the particle filter
    particles = prediction_step(particles, data.timestep(t).odometry, noise);

    % Perform the correction step of the particle filter
    particles = correction_step(particles, data.timestep(t).sensor);

    % Generate visualization plots of the current state of the filter
    plot_state(particles, landmarks, t, data.timestep(t).sensor, showGui);

    % Resample the particle set
    particles = resample(particles);
end

prediction - correction - resample 순서대로 iterative 하게 동작한다.

prediction step

resample 된 particle 이 있을 때, particle 을 command, 혹은 encoder 값에 따라서 실제로 움직여 주는 것을 prediction 이라고 한다.

encoder 값 + noise 값 만큼 pose 에 반영해준다.
noise 값은 일정 범위 내에서 랜덤으로 정해진다. 따라서 동일한 위치에서 동일한 encoder 값을 가진다 하더라도 실제로 100개의 particle 이 prediction 을 하고 나면 조금씩 위치와 방향이 틀어진다.

correction step

prediction 에서는 odometry 값을 넣어 줬다면 correction 에서는 sensor 값을 넣어 준다.

numParticles = length(particles);

파티클 갯수를 받아온다.

m = size(z, 2);

m : 지금 센서로 찾고 data association 을 해서 랜드마크라고 판단이 된 것이 몇 개인가?

랜드마크들에 대한 update 는 칼만 필터와 동일한 방식이다.

Q_t = 0.05*eye(2);

Q_t : measurement (range bearing observation model: 거리와 translation) 에 대한 variance 를 0.05로 둘 것이다.

for i = 1:numParticles
  robot = particles(i).pose;

particle 들은 현재 robot pose 를 어떻게 predict 했는가?

  for j = 1:m
      l = z(j).id;
      if (particles(i).landmarks(l).observed == false)
         particles(i).landmarks(l).mu = [robot(1)+z(j).range*cos(z(j).bearing + robot(3)); robot(2) + z(j).range*sin(z(j).bearing + robot(3))];
         [h, H] = measurement_model(particles(i), z(j));

m (관측한 랜드마크 개수) 만큼 반복문을 도는데, 만약 내가 보는 랜드마크가 처음 보는 랜드마크라면
prediction 한 위치와 observed 된 위치를 곱해서 새로운 위치를 얻고 weight 을 얻을 필요 없이 그냥 그 위치로 초기화 해주면

(평균과 covariance 를 정해줌. 평균은 그냥 로봇의 위치 + 관측된 위치 (거리와 방향이기 때문에 cos 과 sin 으로 표현됨. )
covariance 는 taylor expansion 을 사용해서 선형화 해 줌. )

particles(i).landmarks(l).sigma = H\Q_t*inv(H)';
particles(i).landmarks(l).observed = true;

구한 covariance 값을 이용해서 sigma 를 초기화해주고, observed 를 true 로 바꿔준다.

else
	[expectedZ, H] = measurement_model(particles(i), z(j));
    Q = H*particles(i).landmarks(l).sigma*H' + Q_t;
    K = particles(i).landmarks(l).sigma*H'/Q;

이미 찾은 랜드마크라고 하면, 나의 위치로부터 예측한 값과 내가 실제 관측한 값을 통해서 weight 을 정하고 랜드마크 위치를 조정해줘야 한다.
extended Kalman Filter 에서 했던 내용과 같다.

measurement model 을 통해 Q 를 구하고, 구한 Q 로부터 K 를 구하고
update 해준다.

	z_diff = [z(j).range;z(j).bearing] - expectedZ;
	z_diff(2) = normalize_angle(z_diff(2));

z_diff 는 내가 예측한 부분과 실제 관측한 것의 차이가 얼마나 되는지를 나타내는 값이다. (weight 을 구할 때 사용됨)

normalize_angle 은 값이 특정한 범위 안으로 들어오도록 한다.

	particles(i).landmarks(l).mu = particles(i).landmarks(l).mu + K*z_diff;
	particles(i).landmarks(l).sigma = (eye(2) - K*H)*particles(i).landmarks(l).sigma;

다시 landmark 의 평균과 covariance 를 update 해 주고 weight 을 구한다.
update 된 가우시안 분포에서 내가 예측한 값과 관측값이 동일하면 가장 높은 weight 값을 가지고, 차이가 크면 weight 값이 작아지게 된다.
(weight 값이 작다 -> 내 위치가 잘못되었다.)
weight 값이 작은 particle 은 궁극적으로 사라진다.

% resample the set of particles.
% A particle has a probability proportional to its weight to get
% selected. A good option for such a resampling method is the so-called low
% variance sampling, Probabilistic Robotics pg. 109
function newParticles = resample(particles)

numParticles = length(particles);

w = [particles.weight];

% normalize the weight
w = w / sum(w);

% consider number of effective particles, to decide whether to resample or not
useNeff = false;
%useNeff = true;
if useNeff
  neff = 1. / sum(w.^2);
  neff
  if neff > 0.5*numParticles
    newParticles = particles;
    for i = 1:numParticles
      newParticles(i).weight = w(i);
    end
    return;
  end
end

newParticles = struct;

% TODO: implement the low variance re-sampling

% the cummulative sum
cs = cumsum(w);
weightSum = cs(length(cs));

% initialize the step and the current position on the roulette wheel
step = weightSum / numParticles;
position = unifrnd(0, weightSum);
idx = 1;

% walk along the wheel to select the particles
for i = 1:numParticles
  position += step;
  if (position > weightSum)
    position -= weightSum;
    idx = 1;
  end
  while (position > cs(idx))
    idx++;
  end
  newParticles(i) = particles(idx);
  newParticles(i).weight = 1/numParticles;
end

end