[SLAM] Occupancy Grid Maps

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

본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.

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

Feature

• EKF, EIF, SEIF는 feature 기반의 map을 사용한다.

• 센서 데이터를 그대로 사용하는 것이 아닌 compact한 표현 방법을 사용하여 map을 나타낸다.

• 많은 feature의 관측은 landmark의 위치 추정 정확도를 높인다.

Grid Map

• 공간을 cells로 나누어 표현한다.

• grid로 표현된 공간 구조는 고정이다.

• 각 cell은 occupied space와 free space로 구분된다.

• non-parametric model이다. 즉 센서데이터를 이용하여 landmark를 표현할 수 있는 parameter를 계산하지 않고 map을 표현한다.

• compact한 parameter로 map을 표현하지 않기 때문에 상당한 memory가 필요하다.

• feature detector에 의존하지 않는다.

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

Grid Map의 가정 1

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

• cell이 occupied: $p(m_i) = 1$

• cell이 free: $p(m_i) = 0$

• cell상태를 알수 없음: $p(m_i) = 0.5$

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 정리

• Occupancy Grid Map은 공간을 독립적인 cell로 분리하여 생각한다.

• 각 cell은 binary random variable이다.

• 정확한 로봇의 위치를 알고 있다면 mapping은 상대적으로 쉽다. 하지만 실제 모델에서는 motion noise 때문에 map이 일그러진다. 이러한 문제를 해결하기 위해서 센서 데이터간의 align을 통해 상대 위치를 계산해 나가는 scan-matching방법을 사용한다. Iterative Closest Point(ICP) 알고리즘이 scan-matching 방법 중에 하나이다.

• 계산의 속도를 높이기 위해 log odds model을 사용한다.

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