Introduction to Visual SLAM - SLAM 개요 (2)

Xiang Gao, Tao Zhang 저자의 'Introduction to Visual SLAM from Theory to Practice' 책을 통해 SLAM에 대해 공부한 내용을 기록하고자 한다.

책의 내용은 주로 'SLAM의 기초 이론', 'system architecture', 'mainstream module'을 다룬다.

 

본 내용은 아래 내용에 이어서 정리한 내용이다.

https://jjuke-brain.tistory.com/entry/Introduction-to-Visual-SLAM-SLAM-%EA%B0%9C%EC%9A%94

Introduction to Visual SLAM - SLAM 개요 (1)

 

포스팅에는 기초 이론만 다룰 것이며, 실습을 해보고 싶다면 다음 github를 참조하자.

 

https://github.com/gaoxiang12/slambook2

GitHub - gaoxiang12/slambook2: edition 2 of the slambook

 

 

 

목차

• Mathematical Formulation of SLAM Problems
  • Motion
  • Observation
  • Parameterization
  • Noise를 포함한 Modeling

 

 

 

Mathematical Formulation of SLAM Problems

 

SLAM 문제에 대한 수식을 살펴보자. (아마도 이 포스팅 내용 중에서 가장 중요하고, 앞으로 SLAM에 대한 다양한 내용의 근간이 되는 내용일 것이다.)

 

몇몇 센서가 장착된 로봇이 새로운 환경 (unknown environment)에서 움직인다고 가정해보자.

각 parameter가 의미하는 바는 아래와 같다.

• 연속적인 time step을 $1,\cdots ,k,$와 같이 이산적인 time point로 쪼갠다.
• $\mathrm{x}$ : 로봇의 위치를 나타낸다.
  • 각 time step에서의 로봇의 위치는 ${\mathrm{x}}_1,\cdots ,{\mathrm{x}}_k$로 나타내며, 이것이 모여 로봇의 trajectory를 이룬다.
• $N$ : map을 이루는 landmark들의 개수이다.
  • 각 landmark를 ${\mathrm{y}}_1,\cdots ,{\mathrm{y}}_N$으로 나타낸다.

 

이제, 로봇이 움직임에 따라 아래와 같은 두 가지 개념을 수식으로 표현해볼 수 있다.

1. motion이 무엇인가? 즉, time step $k-1$에서 $k$까지 $\mathrm{x}$이 어떻게 변하는가?
2. 센서가 관찰하는 것은 무엇인가? 예를 들어, 로봇이 특정 landmark ${\mathrm{y}}_j$를 ${\mathrm{x}}_k$에서 감지했다면, 수학적으로 이를 어떻게 표현할 것인가?

 

 

 

 

Motion

 

먼저 motion을 살펴보자.

 

'반시계방향으로 15도 돌아라'라는 명령을 내리고 싶다고 가정하자. 이때 controller에 따라 다양한 방법으로 움직임을 취할 수 있을 것이다.

하지만 어떤 controller로 제어를 하든 다음과 같은 수식으로 로봇의 움직임을 수학적으로 모델링할 수 있다.

 

${\mathrm{x}}_k=f({\mathrm{x}}_{k-1},{\mathrm{u}}_k,{\mathrm{w}}_k)$

 

위 식을 motion equation이라 한다.

여기서 ${\mathrm{u}}_k$는 input commands이고, ${\mathrm{w}}_k$는 noise이다. 또한 함수 $f$를 특정한 형태가 아닌 일반적인 형태로 나타냈음을 주목하자. 이로 인해 위 함수는 특정 motion input에 상관 없이 일반적으로 표현할 수 있다.

 

그리고 noise term을 포함함으로써 위 모델은 stochastic model이 된다. (이때 stochastic model이란, 확률적 모델을 말하며, deterministic model와 반대된다.) 왜냐하면 노이즈는 항상 랜덤으로 발생하기 때문이다. (노이즈가 어떻게 발생하는지 결정적으로(deterministically) 알 수 있다면 추정을 할 필요가 없을 것이다.)

만약 noise를 무시한다면, 로봇의 위치는 명령어 입력에 따라 정확한 값으로 결정될 것이다.

 

 

 

 

Observation

 

motion equation에 이어, 로봇이 landmark를 관찰함으로써 얻게 되는 데이터를 수식화하는 observation equation을 알아보자.

 

${\mathrm{z}}_{k,j}=h({\mathrm{y}}_j,{\mathrm{x}}_k,{\mathrm{v}}_{k,j})$

 

motion equation과 마찬가지로 특정되지 않은 함수 $h(\cdot )$로 정의되었다.

앞서 언급했듯이, 로봇이 landmark point ${\mathrm{y}}_j$를 ${\mathrm{x}}_k$ 위치에서 관찰했을 때, noise term ${\mathrm{v}}_{k,j}$과 함께 함수를 거쳐 얻은 결과물이 바로 데이터 ${\mathrm{z}}_{k,j}$이다.

 

 

 

 

Parameterization

 

실제 움직임과 사용한 센서 종류에 따라 여러 parameterization 방법이 있을 수 있다.

여기서 parameterization이란, 주어진 표현식을 다른 parameter로 표현하는 것을 말한다.

위에서 주어진 식은 $f,h$를 사용하여 표현했지만, motion과 observation이 구체적으로 어떻게 이루어지는지 알기는 힘들다.

하지만, parameterization을 통해 식을 구체화할 수 있다.

 

만약 로봇이 $x,y$축과 각도로 표현되는 2D plane 위를 움직인다고 가정해보자.

이에 따라 time step $k$에서의 위치를 나타내는 변수 ${\mathrm{x}}_k$는 각 축에서의 위치를 나타내는 $x_1,x_2$와 각도 $\theta$를 사용하면 다음과 같이 나타낼 수 있다.

${\mathrm{x}}_k={[x_1,x_2,\theta ]}_k^T$

 

그리고 input command는 일정 시간 간격 사이의 세 parameter의 변화로, 다음과 같이 나타낼 수 있다.

${\mathrm{u}}_k={[\mathrm{\Delta }{x}_1,\mathrm{\Delta }{x}_2,\mathrm{\Delta }\theta ]}_k^T$

 

따라서 최종적인 motion equation은 다음과 같은 형태가 될 것이다.

 

${\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_k={\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_{k-1}+{\left[\begin{array}{c}\mathrm{\Delta }{x}_1\\ \mathrm{\Delta }{x}_2\\ \mathrm{\Delta }\theta \end{array}\right]}_k+{\mathrm{w}}_k$

 

이 motion equation은 간단한 linear 관계에서 사용 가능한 수식이다. 하지만, 모든 input command가 위와 같이 위치와 각도만으로 변화를 주지는 않을 것이므로, 처음에 소개한 $f,h$로 표현한 수식처럼 일반적으로 표현한 것이다.

 

이번에는 2차원 laser sensor를 활용한 경우에 수식을 parameterize해보자.

laser는 로봇과 landmark point 사이의 거리 $r$과 각도 $\varphi$ 두 가지 변수를 사용하여 2D landmark를 제공한다.

landmark point는 ${\mathrm{y}}_j={[y_1,y_2]}_j^T$에, pose는 ${\mathrm{x}}_k={[x_1,x_2]}_k^T$, 관찰된 데이터는 ${\mathrm{z}}_{k,j}={[r_{k,j},{\varphi }_{k,j}]}^T$라 하면, 아래 observation equation을 만족한다.

 

$\left[\begin{array}{c}{r}_{k,j}\\ {\varphi }_{k,j}\end{array}\right]=\left[\begin{array}{c}\sqrt{{(y_{1,j}-x_{1,k})}^2+{(y_{2,j}-x_{2,k})}^2}\\ \arctan \frac{y_{2,j}-x_{2,k}}{y_{1,j}-x_{1,k}}\end{array}\right]+{\mathrm{v}}_{k,j}$

 

위 예시에서 알 수 있듯이, 두 식은 서로 다른 센서에 따라 다르게 구체화된다.

 

noisy control input $\mathrm{u}$와 data $\mathrm{z}$를 읽어들이는 센서를 사용하여  $\mathrm{x}$를 추정하는 것은 localization problem, $\mathrm{y}$를 추정하는 것은 mapping problem과 연관이 있다.

최종적으로 SLAM Process의 기본 식인 motion equation과 observation equation을 정리해보자면 다음과 같다.

 

$\{\begin{array}{l}{\mathrm{x}}_k=f({\mathrm{x}}_{k-1},{\mathrm{u}}_k,{\mathrm{w}}_k),k=1,\cdots ,K\\ {\mathrm{z}}_{k,j}=h({\mathrm{y}}_j,{\mathrm{x}}_k,{\mathrm{v}}_{k,j}),(k,j)\in \mathcal{O}\end{array}$

 

$\mathcal{O}$는 landmark가 관찰된 pose에 대한 정보를 갖는 집합이다. landmark가 보이는 시점은 $k$, 어떤 landmark인지를 $j$를 통해 알 수 있다.

 

이를 통해 우리는 SLAM problem을 state estimation problem으로 모델링하였다.

state estimation problem이란, noisy한 측정 데이터로부터 내부에 숨어있는 state variable들을 어떻게 추정할 것인가에 대한 문제이다.

 

 

 

 

Noise를 포함한 Modeling

 

state estimation problem을 해결하기 위해서는 구체화된 두 수식에 추가적으로 noise probability distribution이 필요하다.

예를 들어, motion 및 observation equation이 linear인지, noise가 Gaussian distribution을 따르는지에 따라 state estimation problem은 linear/nonlinear & Gaussian/non-Gaussian system으로 나뉜다.

 

Linear Gaussian (LG) system은 가장 간단한 형태로, Kalman Filter(KF)를 통해 unbiase된 최적의 추정을 할 수 있다.

 

Nonlinear non-Gaussian (NLNG) system의 경우 매우 복잡한데, 기본적으로 아래와 같은 두 방법을 사용한다.

• Extended Kalman Filter (EKF)
• Nonlinear Optimization

 

21세기까지도 EKF기반 filter method는 SLAM 분야에서 매우 많이 사용된다. 이를 통해 우리는 predict-update라는 두 단계를 통해  system을 선형화할 수 있다. (정확한 방법은 추후에 다루게 될 것이다.) 그리고, 최근 real-time Visual SLAM system 또한 EKF를 기반으로 개발되었다.

EKF의 단점을 극복하기 위해 사람들은 particle filters, nonlinear optimization 등 새로운 방법을 사용하기 시작했다. 요즘에는 visual SLAM에서 주로 'graph optimization'이라는 최신 기술을 사용한다.

 

 

이제 일반적인 SLAM의 수학적 모델은 살펴보았다. 하지만 모호한 표현인 pose $\mathrm{x}$가 정확히 어떤 것을 의미하는지, 복잡한 pose를 어떻게 표현할 것이며, 어떻게 최적화할 것인지, 또한 observation에서 scene이 어떻게 image plane에 projection되는지 등에 대해 앞으로 배워볼 것이다.