[SLAM] Occupancy Grid Maps

Landmark 기반이 아닌 volumetric 기반의 grid map을 소개한다.


본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다. 개인적인 의견을 포함하여 작성되기 때문에 틀린 내용이 있을 수도 있습니다. 틀린 부분은 지적해주시면 확인 후 수정하겠습니다.

이번 글에서는 EKF와 EIF등과 같이 landmark나 feature를 기반의 map표현 방법이 아닌, 센서의 raw 데이터를 이용하여 volumetric하게 map을 표현하는 방법에 대해서 설명하도록 한다.

Feature

Grid Map

위 그림은 grid map을 보여준다.

Grid map의 가정 1

첫번째 가정: cell을 구성하는 영역은 완전히 binary random variable로 occupied이거나 free이다.

Grid map의 가정 2

두번째 가정: map은 움직이지 않는다. 즉 static world이다. 대부분의 mapping system은 이러한 가정을 한다.

Grid map의 가정 3

세번째 가정: map을 이루는 각 cell은 서로 독립적(independent)이다. 따라서 전체 map의 확률은 각 cell 확률의 곱으로 계산되어진다.

p(m) = \prod_i p(m_t)

위 식에서 m은 map전체를 의미하며, m_i는 각 cell을 의미한다.

Data를 이용한 Map의 추정

주어진 센서 데이터 z_{1:t}와 로봇의 위치 x_{1:t}를 이용하여 map을 추정하면 다음과 같다.

p(m \mid z_{1:t}, x_{1:t}) = \prod_i p(m_i \mid z_{1:t},x_{1:t})

여기서 각 cell을 의미하는 m_i는 binary random variable이다. 각 cell의 확률을 계산하면 다음과 같다.

위 식의 유도과정에는 bayes rule과 markov assumption이 사용된다. 위에서 계산된 p(m_i \mid z_{1:t},x_{1:t})는 cell이 occupied일 확률이며, cell이 free space일 확률을 구하면 다음과 같다.

이제 위에서 구한 식을 이용하여 cell이 occupied확률을 계산해보자. 아래 식의 binary random variable의 특징을 보자.

\begin{aligned}
\frac{p(x)}{1-p(x)} &= Y \\
p(x) &= Y - Yp(x)\\
p(x) (1+Y) &= Y\\
p(x) &= \frac{Y}{1+Y}\\
p(x) &= \frac{1}{1+\frac{1}{Y}}
\end{aligned}

즉 binary random variable의 두 확률의 ratio를 이용하면 probability를 계산할 수 있다. 이러한 특징을 이용하여 cell의 occupied확률을 계산해 보자.

\frac{p(m_i \mid z_{1:t}, x_{1:t})}{p(\lnot m_i \mid z_{1:t}, x_{1:t})} = \frac{p(m_i \mid z_{1:t}, x_{1:t})}{1-p(m_i \mid z_{1:t}, x_{1:t})}

즉 probability를 정리하면 3개의 항으로 생각할 수 있다. 가운데 항은 recursive term으로 볼 수 있으며, 오른쪽 항은 맵에대한 prior term으로 볼 수 있다.

여기서 우리가 계산해야 하는 cell의 확률 p(m_i \mid z_{1:t},x_{1:t})을 효율적으로 계산하기 위해 Log odds notation을 이용한다.

Log odds notation

log odds notation은 다음과 같이 정의된다.

l(m_i \mid z_{1:t},x_{1:t}) = log\big( \frac{p(m_i \mid z_{1:t},x_{1:t})}{1 - p(m_i \mid z_{1:t},x_{1:t})}\big)

log odds notation을 이용하여 식을 정리하면 다음과 같다.

l(m_i \mid z_{1:t},x_{1:t}) = l(m_i \mid z_t,x_t) + l(m_i \mid z_{1:t-1},x_{1:t-1}) - l(m_i)

여기서 l(m_i \mid z_{1:t-1},x_{1:t-1})은 recursive하게 계산되는 항이며, l(m_i)은 prior 확률이다. 따라서 inverse sensor model인 l(m_i \mid z_t,x_t)가 계산된다면 효율적으로 단순한 더하기로 l(m_i \mid z_{1:t},x_{1:t})를 계산할 수 있다. 이렇게 계산된 log odds notation으로 부터 cell의 확률을 아래와 같이 계산할 수 있다.

p(m_i \mid z_{1:t},x_{1:t}) = 1- \frac{1}{1+ exp(l(m_i \mid z_{1:t},x_{1:t}))}

전체적인 grid map의 algithm은 다음과 같다.

Inverse sensor model

앞에서 grid map의 update algorithm에 대해서 살펴보았으며, inverse sensor model인 p(m_i \mid z_t, x_t)를 알아야 grid map을 생성할 수 있다. p(m_i \mid z_t, x_t)은 로봇의 위치와 센서 관측값이 있을 때 각 cell의 occupied 되어 있을 확률이다. 여기서는 2가지 모델을 예를 보여준다.

1. Sonar sensor model

첫번째 sensor는 sonar 센서이다. sonar센서는 상대적으로 거리 측정값에 대한 noise가 크다. 아래 그림에서 z는 센서의 측정값이다. z-d_1보다 가까운 구간은 free space로 생각하며, z 부터 z+d_3구간은 object가 있는 영역으로 생각한다. z+d_3보다 먼 영역은 알수 없는 영역이므로 확률은 0.5이다. sonar센서는 측정값의 noise가 크기 때문에 d가 상대적으로 크다.

아래 그림은 sonar센서를 이용하여 생성한 grid map이다.

2. Laser Range Finder(LiDAR sensor)

두번째 센서는 Lidar sensor이다. Lidar는 sonar에 비해 거리측정 오차가 매우 적다. 따라서 occupied로 생각하는 영역이 sonar모델에 비해 매우 좁다.

아래 그림은 LiDAR센서를 이용하여 생성한 Grid map이다. Sonar를 이용한 map에 비해 상대적으로 매우 정확한 것을 알 수 있다.

Occupancy Grid Map 정리

본 글을 참조하실 때에는 출처 명시 부탁드립니다.