본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이번 글에서는 이전에 설명했던 Graph based SLAM이 outlier(잘못된 정보)에 Robust하게 만드는 방법에 대해서 설명한다.
Graph-based SLAM은 least square방법을 사용하여 로봇과 landmark의 위치를 최적화 시킨다. 즉, 현재의 graph상의 로봇의 위치에서 얻어질 것으로 예상되는 측정값과 실제 측정값과의 차이를 최소화 시키는 방향으로 graph가 최적화 된다.
만약 그래프가 최적화 되는 과정에서 아래 그림과 같이 잘못된 연결(edge)이 생성되면 어떻게 될까?
위 그림과 같이 전혀 다른 두 위치를 같은 위치로 인식하여 edge를 발생시켰을 경우 아래와 같이 그래프 전체에 왜곡이 발생하게 된다.
그리고 이렇게 잘못 발생된 edge가 많아질 수록 왜곡은 심해지게 된다. 이러한 문제는 다음과 같은 상황에서 주로 발생한다.
특징이 없는 환경(아무런 특징이 없는 복도 등)
같은 빌딩내의 비슷한 환경의 방
GPS의 multi-path(GPS 신호가 다른 건물에 반사된 후 수신되어 오차가 발생하는 현상)
따라서 실제 graph-based SLAM을 수행할 때 위의 상황 뿐만아니라 다양한 상황에서 왜곡이 발생하게 된다. 이러한 잘못된 정보를 outlier라고 하며, 이러한 outlier에 덜 영향을 받는 최적화 방법이 필요하다. 이번 글에서는 최적화 과정을 Robust하게 만드는 방법에 대해서 설명하도록 한다.
Robust M-estimator
이전 글인 Least Square관련 글에서는 noise를 Gaussina 분포로 가정하고, least square방법으로 최적해를 구하는 방법에 대해서 설명하였다. 또한 least square와 Gaussian 분포의 관계에 대해서도 언급하였었다. Least square방법은 M-estimator의 한 종류이며, Gaussian noise를 가정한 model이다. M-estimator는 noise의 형태를 Gaussian으로 가정하지 않는다. M-estimator의 PDF(Probability density function)은 아래와 같이 정의된다. \[p(e)= exp(-\rho(e))\]
그렇다면 least square에서 최적해를 계산하는 것과 같이 negative log likelihood form으로 표기하면 PDF의 최대값을 찾는 문제는 log likelihood의 최소값을 찾는 문제가 된다. \[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_i \rho(e_i(\mathbf{x}))\]
즉 위의 log likelihood식에서 $\rho(e)$가 $\rho(e) = e^2$ 처럼 제곱의 형태인 것이 Least square문제가 되는 것이다. 그렇다면 $\rho$ fucntion에는 어떤 종류들이 있는지 살펴보도록 하자.
Gaussian: $\rho(e) = e^2$
L1 norm: $\rho(e) = \mid e \mid$
Huber M-estimator: $\rho(e) = \begin{cases} \frac{e^2}{2} & if \mid e \mid < c \ c(\mid e \mid -\frac{c}{2}) & otherwise \end{cases}$
Others: Tukey, Cauchy …
위 그림은 각 함수의 형태를 보여준다. 최적화를 하기 위한 cost function의 형태에 변화를 줌으로써 outlier에 강인한 특성을 주기 위한 노력들이다. 이렇게 cost function의 형태를 바꿈으로써 outlier에 Robust한 특성을 어떻게 갖게 되는 것인지 살펴보자. 최적화는 error들의 합을 최소화 하는 방향으로 입력을 변화시키며 해를 찾아가는 방법이다. 이러한 과정에서 error가 매우 큰(Outlier라고 생각 할 수 있는) term들이 최적화 과정에서 큰 weight로 작용하게 된다(영향을 크게 준다). 따라서 위의 함수들은 ourlier라고 생각 할 수 있는, 즉 error가 매우 큰 term에 대해서 weight를 다소 줄이는 방향으로 cost function의 형태를 구성한 것이다. 다양한 형태의 function중에서 가장 많이 사용되는 형태는 Huber M-estimator이다.
앞으로 설명할 Max-mixture와 dynamic covariance scaling 방법도 m-estimator와 비슷한 개념을 이용하여 최적화의 robust함을 높이려는 방법이다.
Max-mixture
Max-mixture는 M-estimator와 마찬가지로 graph-based optimization을 outlier에 robust하게 만들기 위한 방법중에 하나이다. Max-mixture는 이름에서 알 수 있듯이 여러개의 Gaussian 분포를 합하는 과정에서 수학적인 이점을 얻기 위해서 각 Gaussian 분포의 최대값을 이용한다. 왜 이러한 방법을 사용하는지 아래 수식을 보도록 하자.
세상에 존재하는 다앙햔 분포를 1개의 Gaussian만을 이용하여 표현하기엔 부족한 경우가 많이 발생한다. 따라서 다양한 분포를 표현하기 위해서 여러개의 Gaussian을 더하는 형태로 분포를 표현할 수 있다. \[p(\mathbf{z}\mid\mathbf{x}) = \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\]
위의 분포를 최적화 시키기 위한 cost function을 얻기 위해 negative log likelihood을 취하면 다음과 같은 형태가 된다. \[-log p(\mathbf{z}\mid\mathbf{x}) = -log \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\]
1개의 Gaussian분포일 경우에는 sum($\sum$)이 없기 떄문에 log와 exponential이 계산되어 우리가 앞에서 계산한 cost function의 형태로 계산되어 최적화 과정을 진행할 수 있게 된다. 하지만 Gaussian mixture 모델의 경우 Sum의 형태이기 때문에 log가 sum의 안으로 들어갈 수 없다. 따라서 mixture 모델을 최적화 시키기 위해서 최대값을 이용하는 max-mixture approximation 방법을 이용한다. \[\begin{aligned} p(\mathbf{z}\mid\mathbf{x}) &= \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\\ &\approx max_k \ \ w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k \end{aligned}\]
즉 위의 근사화과정을 그림으로 표현하면 아래와 같다.
즉 두개의 분포의 값을 더하는 것 대신 최대값만을 추출하여 분포를 표현하는 것이다. 이는 두 분포의 평균값이 어느정도 거리로 떨어져 있을 경우에는 오차가 작지만 두 분포의 평균값이 매우 가까운 경우에는 큰 오차를 발생시킨다. 근사화된 식의 negative log likelihood는 다음과 같다. \[\begin{aligned} -log p(\mathbf{z}\mid\mathbf{x}) &= -log \ \ max_k \ \ w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\\ &= min_k (\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k - log(w_k \eta_k) \end{aligned}\]
즉 max-mixture는 Gaussian 분포의 max값을 취하는 형태로 근사화 함으로써 우리가 풀 수 있는 최적화의 Cost function을 구할 수 있게 되었다.
위 그림에서 빨간색 그래프는 같은 mean값을 갖는(다른 variance) 두 gaussian의 합을 max-mixture방법을 통해 얻은 분포의 cost-function을 보여준다. 앞에서 설명한 M-estimator의 cost function들과 유사함을 알 수 있다. 따라서 여러개의 Gaussian 분포를 더함으로써 graph optimization과정을 Robust하게 만들 수 있다.
Dynamic Covariance Scaling(DCS)
Graph의 최적화를 Robust하게 만드는 다른 방법은 DCS(Dynamic Covariance Scaling)방법이다. DCS는 각 Error값에 해당하는 information matrix의 크기를 조절함으로써 robust하게 만든다. 원래 graph optimization식은 다음과 같다. \[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}(\mathbf{x})^T \Omega_{ij}\mathbf{e}_{ij}(\mathbf{x})\]
DCS는 cost function의 information matrix의 크기를 조절하는 scaling factor($s_{ij}^2$)를 추가한다. \[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}(\mathbf{x})^T (s_{ij}^2 \Omega_{ij})\mathbf{e}_{ij}(\mathbf{x})\]
scaling factor인 $s_{ij}^2$는 다음과 같이 정의된다. \[s_{ij} = min(1, \frac{2\Phi}{\Phi+\chi_{ij}^2})\]
즉 두개의 파라미터 $\Phi$와 $\chi$에 의해 scaling factor가 조절되며, 이 두 파라미터에 의해서 cost function이 변하게 된다.
위 그림은 DCS의 원래 cost function(검정색)과 scaling factor(파란색)그리고 최종 scaling이 된 cost function(빨간색)을 보여준다. Error가 적은 영역에서 scaling factor는 1이지만 error가 큰 영역으로 갈 수록 scaling factor가 작아지며, error가 큰 영역의 함수 출력값을 줄여준다. 이는 m-estimator가 error가 큰 영역의 값을 줄이는 것과 비슷한 효과로 볼 수 있다. 원래 DCS가 이러한 개념을 처음 사용한 것은 아니다. DCS가 나오기 전인 2012년 IROS에 error가 큰 term의 영향을 없애는 “Switchable Constraint for Robust Pose Graph SLAM”이 발표되었다. 이는 획기적으로 graph 최적화를 robust하게 만들었으나 과정이 매우 복잡한 단점이 있었다. 그 이후에 “Switchable Constraint” 논문을 위와같이 close form으로 정리하여 ICRA 2013년도에 발표한 논문이 “Robust Map Optimization using Dynamic Covariance Scaling”이다.
이 글에서 DCS에 대해서 너무 깊이 논하지는 않을 것이다. 조금 더 자세히 내용이 필요한 경우 위 논문을 찾아보기 바란다.
정리
이번 글에서는 graph-based SLAM을 outlier에 robust하게 만드는 방법에 대해서 살펴보았다. 이러한 목적으로 여러가지 방법이 사용되고 있으나 결과적으로는 최적화의 cost function에서 error가 큰 영역의 출력값을 줄여주는 방법이라는 점에서는 유사함을 알 수 있다.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
graph-based SLAM 포스트에서는 landmark가 없는 환경에서 로봇의 위치 간의 관계 만을 이용하여 graph를 최적화 시키는 방법에 대해서 설명하였다. 이번 글에서는 landmark가 있는 환경에서 이 landmark들을 이용하여 로봇 간의 위치 정보를 알 수 있을 때 graph-based SLAM이 어떻게 달라지는지 설명할 것이다.
Graph-based SLAM with Landmark
아래 그림은 Global 좌표계에서 로봇의 위치와 landmark의 위치를 보여주는 그림이다.
Landmark가 존재하는 실제 환경은 아래와 같다. 아래 그림과 같은 공원의 경우 공원에 있는 나무들이 landmark가 되고 나무들의 위치정보를 이용하여 SLAM을 적용한다.
로봇의 위치와 landmark와의 관계를 도식적으로 그려보면 다음과 같다.
이제 graph에서 node는 로봇의 위치뿐만 아니라 landmark의 위치도 포함하게 된다. 이때 landmark는 방향없이 x,y좌표로만 구성된다.
Node
로봇의 위치
landmark의 위치
Edge
로봇의 Odometry measurement
로봇의 Landmark observation
위의 정보들로 구성된 graph의 error를 최소화 하는 landmark와 로봇의 위치를 계산함으로써 로봇의 위치를 구할 수 있다. Landmark가 없는 환경에서 “virtual measurement”라는 것을 언급했었다. “virtual measurement”는 non-successive한 로봇의 위치에서 얻어진 센서 데이터를 이용하여 계산된 두 로봇간의 상대 위치(relative pose)를 의미한다. 이렇게 계산된 “virtual measurement”는 non-successive한 노드 사이의 constraint가 된다. Landmark가 있는 환경에서는 위 그림과 같이 non-succesive한 로봇간의 direct한 edge는 발생하지 않으며, landmark를 통해서만 연결된다.
Rank of Information Matrix
Landmark를 통한 graph SLAM의 경우 센서 특성에 따른 information matrix의 rank를 살펴보아야 한다. 여기서는 bearing only observation 센서모델로 로봇의 위치에서 landmark까지의 각도만을 측정할 수 있는 센서의 경우를 생각한다.
Bearing observation의 경우 observation function은 다음과 같다.
이 observation function을 이용한 error term은 다음과 같다.
위 error term으로 구성되는 information matrix의 rank는 어떻게 될까? \[\mathbf{H}_{ij} = \mathbf{J}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\]
위 식에서 $\mathbf{J}{ij}$는 error term을 편미분한 행렬인데, bearing only 센서의 경우 error term의 dimension은 1이므로 Jacobian의 rank는 1이다. 따라서 $\mathbf{H}{ij}$의 rank도 1이 된다.
rank가 1이라는 것은 무엇을 의미할까? rank가 1이라는 것은 위 그림과 같이 어떠한 landmark가 있을 때 로봇은 x-y plane에 어디든 존재할 수 있으며 단지 그 x-y좌표에 해당하는 heading 각도만 한정된다는 의미이다. 따라서 bearing only sensor의 경우 로봇의 위치를 정확히 알기 위해서는 3개 이상의 observation이 필요하다. 이러한 시스템을 “under-determined” 시스템이라고 부른다.
따라서 Information matrix의 rank는 로봇의 위치 중(x,y,heading) 몇가지의 정보를 한정할 수 있는지를 의미하며, 로봇의 위치에 대한 unique solusion을 계산하기 위해서는 full lank가 되어야 한다.
Under-determined System
이러한 under-determined system, 즉 information matrix의 rank가 full lank가 아닌 경우 이러한 상황을 해결하기 위한 방법이 “Levenberge Marquardt” method 이다. 즉 “damping factor”를 더함으로써 full rank의 matrix를 만들어 unique한 해를 찾는다. 원래의 식은 아래와 같다. \[\mathbf{H} \triangle \mathbf{x} = -\mathbf{b}\]
damping factor가 추가된 식은 아래와 같다. \[(\mathbf{H} + \lambda \mathbf{I})\triangle \mathbf{x} = -\mathbf{b}\]
damping factor는 error의 증감에 따라 크기를 변화시킨다. 전체적인 알고리즘은 다음과 같다.
정리
이 글에서는 Landmark가 있는 환경에서의 Graph-based SLAM에 대해서 알아보았다. Landmark가 없는 환경에서는 센서 데이터를 이용하여 non-successive node간의 edge정보를 계산하였다. 이러한 edge는 node간의 직접적인 연결이다. 반면, landmark가 있는 환경에서는 각 node에서 바라보는 landmark의 위치가 observation 정보가 되며, non-successive한 node간의 연결은 landmark를 통해서 이루어진다. 이때 센서의 종류에 따라 information matrix의 rank가 결정되고 under-determined system이 되므로 이를 풀기위한 기법이 필요하게 된다(LM method).
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이 글에서는 Least square를 이용하여 실제로 어떻게 SLAM에 적용이 되는지 알아볼 것이다. 아직 least squre에 대해서 익숙하지 않다면 이전 글을 참고하기 바란다. 이번 글에서의 표기법은 이전 글의 표기법을 그대로 사용한다.
Graph-based SLAM
Graph-based SLAM은 로봇의 위치와 움직임을 graph처럼 node와 edge로 표현함을 의미한다.
Node: Graph의 node는 로봇의 pose를 의미한다.
Edge: 두 node사이의 edge는 로봇의 위치 사이의 odometry정보이며 constraint라고 한다.
로봇이 움직이며 odometry 정보를 이용하여 node와 constraint를 생성한다.
로봇이 위 그림과 같이 이전에 방문했던 지역을 다시 방문할 경우 주변 환경에 대한 정보를 이용하여 같은 위치임을 인식하고 연속적이지 않은(non-successive) node사이에 constraint를 추가하고, graph를 최적화 함으로써 measurement에 최적화된 로봇의 위치를 계산할 수 있다.
아래의 youtube 동영상은 pose graph SLAM을 이용한 SLAM의 예이다.
동영상에서 차량이 움직이는 경로의 실선은 constraint이며, 실선 중간에 생성되는 네모는 graph의 node이다. 위 application은 도로 상의 marking정보를 이용하여 장소를 인식하고 non-successive node사이의 constraint를 추가함으로써 odometry의 error를 보정한다. 동영상에서 이러한 constraint를 추가함으로써 계속적으로 graph가 최적화 되는 모습을 볼 수 있다.
Overall SLAM System
Graph SLAM은 아래 그림과 같이 크게 Front-end와 Back-end로 나눌 수 있다.
위의 동영상을 예로 들면, Front-end는 camera로 부터 얻어지는 이미지로 부터 데이터 처리를 통해 sub-map을 만들고, 생성된 sub-map을 통해서 다시 방문한 장소를 인식함으로써 각 node사이의 constraint를 생성하는 과정을 말한다. 이렇게 생성된 constraint들과 node들의 정보를 이용하여 최적화된 node의 위치를 계산하는 과정은 graph SLAM의 back-end에서 수행한다. 최근 많이 사용되는 graph SLAM의 back-end로는 iSAM과 g2o가 있다.
Graph
이 글에서 로봇이 이동하는 환경은 2-dimension 으로 가정하며, 로봇의 위치는 다음과 같이 3 DOF로 표현한다. \[\mathbf{x}_i = (x_i, y_i, \theta_i)\]
Graph는 이러한 로봇의 위치를 표현하는 $\mathbf{x}_i$로 구성된 vector이다. \[\mathbf{x} = \mathbf{x}_{1:n}\]
Graph를 구성하는 노드 $\mathbf{x}i$와 $\mathbf{x}{i+1}$ 사이에는 odometry 센서로부터 얻어진 measurement가 constraint/edge로 생성된다.
만약 로봇이 같은 장소를 다시 방문했을 때 두 로봇의 위치에서 얻은 measurement를 이용하여 두 노드 사이의 상대적인 위치(relative pose)를 계산할 수 있다. 아래 그림은 다른 위치에서 획득한 같은 물체의 센서 데이터를 보여준다.
두 센서데이터의 매칭을 통해서 두 node사이의 상대위치를 계산할 수 있고, 이 상대위치가 두 node사이의 constraint가 된다. 이때 직접적으로 node사이의 관계를 구한 것이 아닌 센서 measurement로부터 계산하였기 때문에 virtual measurement라고 부른다.
Pose Graph
아래 그림은 Pose graph SLAM에서 measurement에 대한 error를 그림으로 표현하여 보여준다.
$\mathbf{x}i$와 $\mathbf{x}_j$는 현재 graph에서 두 node의 위치이다. $<\mathbf{z}{ij},\Omega_{ij}>$는 $\mathbf{x}i$의 위치에서 센서를 이용하여 측정한 $\mathbf{x}_j$의 위치이다. 센서로 측정한 node의 위치와 현재 graph상의 위치의 차이를 $\mathbf{e}{ij}$로 표현한다. Graph optimization은 이러한 error를 최소화 시키는 graph를 계산하는 것이다. 여기서 $\Omega_{ij}$가 센서 measurement의 information matrix임을 기억하자.
Error function ($\mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j)$)을 homogeneous coordinate로 표현하면 다음과 같이 표현할 수 있다. \[\mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j) = \text{t2v}(\mathbf{Z}_{ij}^{-1}(\mathbf{X}_i^{-1}\mathbf{X}_j))\]
homogeneous coordinate은 로봇의 translation과 rotation을 하나의 matrix로 표현하는 방법이며, 이러한 표현방법에 대해서는 이글을 참고하라.
위 식에서 $\mathbf{Z}_{ij}$는 i에서 바라본 j의 measurement이며, $\mathbf{X}_i^{-1}\mathbf{X}_j$는 현재 graph에서 i를 기준으로 j의 위치를 의미한다. t2v함수는 homogeneous coordinate를 vector form으로 바꾸는 transform 함수이다.
Graph optimization식을 전체 state를 표현하는 $\mathbf{x}$를 이용하여 표현하면 다음과 같다. \[\begin{aligned} x^* &=\text{argmin}_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}^T(\mathbf{x}_i,\mathbf{x}_j) \mathbf{\Omega}_{ij} \mathbf{e}_{ij}(\mathbf{x}_i,\mathbf{x}_j)\\ &=\text{argmin}_{\mathbf{x}} \sum_k \mathbf{e}_k^T(\mathbf{x}) \mathbf{\Omega}_k \mathbf{e}_k(\mathbf{x}) \end{aligned}\]
여기서 state vector $\mathbf{x}$는 graph의 node가 각각의 block을 구성하는 vector이다. \[\mathbf{x}^T = \begin{pmatrix}\mathbf{x}_1^T & \mathbf{x}_2^T & \cdots & \mathbf{x}_n^T \end{pmatrix}\]
error function을 선형화하면 Jacobian으로 표현할 수 있다. \[\begin{aligned} \mathbf{e}_{ij}(\mathbf{x}+\triangle\mathbf{x}) &\approx \mathbf{e}_{ij}(\mathbf{x}) + \mathbf{J}_{ij}(\mathbf{x})\triangle \mathbf{x} \end{aligned}\]
이전 글에서 optimization 과정을 통해서 최적화된 $\mathbf{x}^*$를 계산하였다. 최적화 식을 만족하는 state를 계산하기 위해서는 $\mathbf{b}^T$와 $\mathbf{H}$를 계산해야 한다. \[\begin{aligned} \mathbf{b}^T &= \sum_{ij} \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\\ \mathbf{H} &= \sum_{ij} \mathbf{J}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij} \end{aligned}\]
Jacobian matrix $\mathbf{J}_{ij}$가 sparse matrix이기 때문에 information matrix인 $\mathbf{H}$도 sparse matrix가 된다. Jacobian matrix의 sparse함이 각 파라미터에 어떻게 영향을 미치는지 그림으로 보자.
위에서 설명한 과정을 통해서 최적의 $\mathbf{x}$를 계산하는 과정을 정리하면 다음과 같다.
위에서 설명한 과정을 통해 모든 measurement에 대한 information matrix($\mathbf{H}$)와 $\mathbf{b}$를 계산하고, 두 값을 이용하여 state의 변화량인 $\triangle \mathbf{x}$를 계산한다($\triangle \mathbf{x} = -\mathbf{H}^{-1}\mathbf{b}$). $\mathbf{H}$는 크기가 큰 matrix이므로 inverse과정이 높은 계산량을 요구하므로, 이를 쉽게 계산하기 위해서 Cholesky factorization 방법을 사용한다. 이제 계산된 변화량을 이용하여 state를 업데이트하고($\mathbf{x} = \mathbf{x}+\triangle \mathbf{x}$) state가 수렴할 때 까지 반복하여 최적화된 state를 계산한다.
Example of Pose Graph
이해를 돕기위해 graph의 optimization과정을 통한 state update과정의 예를 통해 설명할 것이다.
위 그림과 같이 현재 2개의 state가 있으며, 센서를 통해 측정한 두 node 사이의 odometry정보는 1이다. 처음에는 모든 state의 값을 모르기 때문에 모두 0으로 설정한다. \[\begin{aligned} \mathbf{x} &= \begin{pmatrix} 0 & 0 \end{pmatrix}^T\\ \mathbf{z}_{12} &= 1\\ \mathbf{\Omega} &= 2\\ \mathbf{e}_{12} &= \mathbf{z}_{12} -(x_2 - x_1) = 1-(0-0) = 1\\ \mathbf{J}_{12} &= \begin{pmatrix}1 -1\end{pmatrix}^T\\ \mathbf{b}_{12} &= \mathbf{e}_{12}^T \mathbf{\Omega}_{12} \mathbf{J}_{12} = \begin{pmatrix} 2 & -2 \end{pmatrix}\\ \mathbf{H}_{12} &= \mathbf{J}_{12}^T \mathbf{\Omega} \mathbf{J}_{12} = \begin{pmatrix} 2 & -2 \\ -2 & 2\end{pmatrix}\\ \triangle \mathbf{x} &= -\mathbf{H}_{12}^{-1} b_{12} \end{aligned}\]
센서 measurement의 information은 2로 설정하였다. state update를 위해 \(\mathbf{b}, \mathbf{H}\)를 계산하였다. 이때 \(\triangle\mathbf{x}\)를 계산하기 위해서는 \(\mathbf{H}\)의 inverse를 계산해야 하지만 현재 \(det(\mathbf{H})\)는 0이므로 계산할 수 없다. 이것은 모든 node의 uncertainty가 같아 graph가 floating상태이기 때문이다. 이를 해결하기 위해서 첫번째 node를 고정(fix)시켜 줘야한다. 이를 위해 첫번째 node의 uncertainty를 낮춰준다(즉, information을 더해서 크게 해준다). \[\begin{aligned} \mathbf{H} &= \begin{pmatrix} 2 & -2 \\ -2 & 2 \end{pmatrix} + \begin{pmatrix} 1 & 0 \\ 0 & 0 \end{pmatrix}\\ \triangle \mathbf{x} &= -\mathbf{H}^{-1} b_{12}\\ \triangle \mathbf{x} &= \begin{pmatrix} 0 & 1 \end{pmatrix}^T \end{aligned}\]
따라서 error를 최소화 하는 state의 변화량은 $\begin{pmatrix} 0 & 1 \end{pmatrix}^T$가 되며 state를 업데이트하면 최종적으로 $\mathbf{x} = \mathbf{x} + \triangle \mathbf{x} = \begin{pmatrix} 0 & 1 \end{pmatrix}^T$가 된다.
Hierarchical Approach to Least Square SLAM
Hierarchical Approach는 Graph SLAM의 속도를 향상시키기 위한 한가지 방법이다.
맨 왼쪽 그림은 원래의 pose graph를 보여주고 있다. Hierarchical Approach는 이름에서 알 수 있듯이 여러개의 계층구조로 구성되어 있으며 아래의 layer일수록 많은 node를, 위로 갈수록 적은 수의 node를 갖는 graph로 구성되어 있다. Optimization 문제는 node의 갯수가 많을 수록 계산시간이 오래 걸리기 때문에 이러한 계층구조를 이용하여, 적은 수의 node의 graph에서(sampling된 node로 이루어진 graph) 최적의 node위치를 계산 후, 계산된 node를 기준으로 주변 node의 위치를 다시 계산하는 방법을 이용한다. Hierarchical Approach에 대해서는 자세히 언급하지 않겠다. 이 부분에 대해서 자세히 알고 싶으면 Robot Mapping 강의 16강을 참고하기 바란다.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
SLAM은 로봇의 위치를 추정함과 동시에 주변 환경에 대한 맵도 같이 생성하는 기술이다. SLAM의 방법은 크게 3가지로 나눌 수 있다.
Kalman Filter
Particle Filter
Graph-based
이번 글에서 부터는 Graph-based SLAM에 대해서 설명할 것이며, 이 글에서는 Graph-based SLAM의 접근 방법인 Least square에 대해서 설명한다.
Least Square
Least square는 “overdetermined system”의 해를 구하기 위한 방법이다. overdetermined system이란 미지수의 갯수보다 식의 수가 더 많기 때문에, 모든 식을 만족하는 해가 존재하지 않는 시스템을 말한다.
Least square는 에러의 제곱합(sum of the squared error)을 최소화 하는 방법으로 해를 구한다.
Problem of Least Square
$\mathbf{x}$는 state vector
$\mathbf{z}_i$는 실제 measurement(실제 센서 측정값)
$\hat{\mathbf{z}}_i=f_i(x)$는 현재 state vector $\mathbf{x}$에서 예상되는 measurement값
목표: 센서로부터 실제 얻어진 measurement($\mathbf{z}_i$)에 가장 적합한 $\mathbf{x}$ 추정
위 그림은 least square를 그림으로 표현하였다. Least square는 센서로부터 측정된 measurement에 가장 적합한 로봇의 state를 계산하는 방법이다. Sensor observation model처럼 로봇의 상태를 알고 있을 때 예상되는 measurement를 예상할 수 있고($\mathbf{\hat{z}}_i$) 실제 measurement와 예상되는 measurement와의 차이를 최소화 함으로써 최적화된 로봇의 state를 계산할 수 있다.
Error Function
Error($\mathbf{e}_i$)는 실제 measurement와 예상되는 measurement와의 차이를 나타내며, measurement와 같은 dimension을 갖는 vector이다. \[\mathbf{e}_i(\mathbf{x}) = \mathbf{z}_i - f_i(\mathbf{x})\]
위 식에서 measurement의 noise는 평균이 0이며, Gaussian 분포를 따른다고 가정한다.
Information matrix($\Omega_i$)는 measurement의 covariance matrix의 inverse이며, measurement($\mathbf{z}_i$)와 dimension의 크기가 같은 matrix이다.
squared error($e_i(\mathbf{x})$)는 아래와 같으며, vector가 아닌 scalar값을 갖는다. \[e_i(\mathbf{x}) = \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x})\]
Find Minimum of Error Function
로봇의 최적화된 state를 계산하는 문제는, 주어진 모든 센서 measurement를 이용한 error function의 합이 최소화 되는 state를 찾는 문제이다. 식으로 표현하면 다음과 같다. \[\begin{aligned} x* &= \text{argmin}_{\mathbf{x}} F(\mathbf{x})\\ &=\text{argmin}_{\mathbf{x}} \sum_i e_i(\mathbf{x})\\ &=\text{argmin}_{\mathbf{x}} \sum_i \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x}) \end{aligned}\]
위 식에서 각 element들에 대해서는 error function에서 설명했다. 이 식을 보고 넘어갈 때 각 항들의 dimension이 어떻게 되는지 확인하고 넘어가도록 하자. 만약 센서 measurement의 dimension이 n일 때 $\mathbf{e}_i$는 $n\times1$, $\mathbf{\Omega}_i$는 $n\times n$의 dimension을 갖는다.
위와 같은 optimization 문제는 복잡하며, closed form(미지수의 해가 수식으로 정리되는 형태)이 아니기 때문에 수학적인 접근방법으로 해를 구한다.
Optimization 방법으로 최적의 로봇 state($\mathbf{x}^*$)를 구하는 순서는 다음과 같다.
현재의 state를 기준으로 error term($\mathbf{e(x)}$)의 선형화
선형화된 error term으로 계산되는 squared error function($e_i(\mathbf{x})$)의 1차 미분계산
squared error function은 quadratic form이므로 1차 미분값이 0이 되는 점이 최소값이므로 미분값이 0인 solution을 계산
계산된 solution을 이용하여 state를 update
위의 과정을 수렴할 때까지 반복
위의 과정을 통해 iterative하게 optimal한 state를 계산할 수 있다. 각 단계의 과정을 자세히 살펴보도록 하자.
1. Error Term의 선형화
vector의 형태를 갖는 error term은 현재 measurement와 예상되는 measurement의 차이이다. \[\mathbf{e}_i(\mathbf{x}) = \mathbf{z}_i - f_i(\mathbf{x})\]
이때 $f_i(\mathbf{x})$는 observation model로 많은 경우에 비선형형태를 갖는다. observation model에 대해 익숙하지 않다면 Motion Model과 Observation Model에 대한 이전 글을 참고하기 바란다. 따라서 식을 풀기 위해서는 현재 state기준으로 선형화를 수행한다. 선형화 방법으로는 EKF와 마찬가지로 Tayor expansion방법을 사용한다. \[\begin{aligned} \mathbf{e}_i(\mathbf{x}+\triangle\mathbf{x}) &\approx \mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})((\mathbf{x}+\triangle \mathbf{x})-\mathbf{x})\\ &= \mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})\triangle \mathbf{x} \end{aligned}\]
계산된 squared error는 위와 같으며, 마지막 식에서 $c_i, \mathbf{b}_i, \mathbf{H}_i$는 다음과 같다. \[\begin{aligned} c_i &= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{e}_i\\ \mathbf{b}_i^T &= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i\\ \mathbf{H}_i &= \mathbf{J}_i^T \mathbf{\Omega}_i \mathbf{J}_i \end{aligned}\]
$\mathbf{H}_i$는 선형화 후의 information matrix이다.
위에서 계산된 $e_i(\mathbf{x}+\triangle\mathbf{x})$는 1개의 measurement에 대한 error function이므로 모든 measurement에 대한 error function은 다음과 같다. \[\begin{aligned} F(\mathbf{x}+\triangle \mathbf{x}) &\approx \sum_i (c_i + 2 \mathbf{b}_i^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H}_i)\triangle \mathbf{x}\\ &= \sum_i c_i + 2 (\sum_i \mathbf{b}_i^T) \triangle \mathbf{x} + \triangle \mathbf{x}^T (\sum_i \mathbf{H}_i) \triangle \mathbf{x}\\ &= c + 2 \mathbf{b}^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H} \triangle \mathbf{x} \end{aligned}\] \[\begin{aligned} \mathbf{b}^T &= \sum_i \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i\\ \mathbf{H} &= \sum_i \mathbf{J}_i^T \mathbf{\Omega}_i \mathbf{J}_i \end{aligned}\]
그렇다면 이제 squared error function의 마지막 식을 살펴보자. \[\begin{aligned} F(\mathbf{x}+\triangle \mathbf{x}) &\approx c + 2 \mathbf{b}^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H} \triangle \mathbf{x} \end{aligned}\]
위의 식은 $\triangle \mathbf{x}$에 대해서 quadratic한 형태를 띄고 있다. quadratic form은 일반 2차방정식의 형태로 생각할 수 있다. 즉, 함수의 극대 혹은 극소값은 함수의 미분이 0이 되는 지점을 계산함으로써 계산할 수 있다.
그럼 이제 위의 식의 1차 미분을 계산한다. 일반적으로 quadratic form의 미분은 다음과 같다(matrix cookbook, section 2.2.4참고). \[\begin{aligned} f(x) &= \mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{b}^T\mathbf{x}\\ \frac{\partial f}{\partial x} &= (\mathbf{H} + \mathbf{H}^T)\mathbf{x} +\mathbf{b} \end{aligned}\]
따라서 함수의 1차 미분은 다음과 같다. \[\frac{\partial F(\mathbf{x}+\triangle \mathbf{x})}{\partial \triangle \mathbf{x}} = 2 \mathbf{b} + 2 \mathbf{H}\triangle \mathbf{x}\]
3. Global Error Function의 최소값 계산
2번단계에서 global error function의 1차 미분값을 계산하였다. global error function은 quadratic form이므로 미분값이 0인 지점이 함수의 극대 혹은 극소값이다. 우리가 계산하고 있는 least square의 목적은 error를 최소로 만드는 state를 계산하는 것이므로 계속해서 error function의 최소값을 찾아 나간다. 최소값을 찾기 위하여 미분값을 0으로 만드는 $\mathbf{x}$를 계산한다. \[\frac{\partial F(\mathbf{x}+\triangle \mathbf{x})}{\partial \triangle \mathbf{x}} = 2 \mathbf{b} + 2 \mathbf{H}\triangle \mathbf{x} = 0\]
따라서 optimal한 state는 다음과 같다. \[\triangle \mathbf{x}^* = -\mathbf{H}^{-1}\mathbf{b}\]
이때 $\mathbf{H}$는 크기가 큰 matrix이므로 inverse를 계산 시 많은 계산이 소요된다. 이러한 문제를 해결하기 위해서 Cholesky factorization, 혹은 QR decomposition과 같은 방법을 사용한다. Cholesky factorization은 matrix $\mathbf{H}$를 하 삼각행렬(lower triangular matrix)의 곱으로 표현하는 방법이다($\mathbf{H}= \mathbf{L}\mathbf{L}^T$). Triangular matrix는 일반 matrix에 비해 inverse의 계산이 간단하므로 이렇게 분해함으로써 inverse를 더 빠르게 계산할 수 있다. \[\begin{aligned} \triangle \mathbf{x}^* &= -\mathbf{H}^{-1}\mathbf{b}\\ &= -\mathbf{L}^{-T}\mathbf{L}^{-1}\mathbf{b} \end{aligned}\]
4. State Update
3번 과정에서 global error function을 최소로 만드는 state인 $\triangle \mathbf{x}$를 계산하였다. 이제 이 결과를 이용하여 state를 update한다. \[\mathbf{x} = \mathbf{x} + \triangle \mathbf{x}\]
업데이트 완료되면 업데이트된 state를 기준으로 다시 선형화를 하여 위 과정을 반복한다. 여러번 반복함으로써 계속 최적화된 state로 업데이트하고, state가 수렴하면 계산을 중지하고 optimization과정을 종료한다.
위와 같이 quadratic form을 계산함으로써 optimization 문제를 해결하는 방법을 Gauss-Newtom optimization 이라고 한다.
Least Square와 Gaussian의 관계
위에서 least square의 계산과정은 모두 설명하였다. Least square문제는 아래의 식을 품으로써 최적의 state를 계산하였다. \[\begin{aligned} x* &=\text{argmin}_{\mathbf{x}} \sum_i \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x}) \end{aligned}\]
하지만 왜 이런 형태의 식을 만들어서 풀게 되었을까? 이것은 Gaussian 분포와 관계가 있다. Bayes filter의 posterior의 식은 다음과 같다. \[p(x_{0:t} \mid z_{1:t}, u_{1:t}) = \eta p(x_0) \prod_t [p(x_t \mid x_{t-1}, u_t)p(z_t \mid x_t)]\]
여기서 prior와, motion model 그리고 observation model은 모두 gaussian 분포를 따른다. 따라서 log likelihood를 계산하면 다음과 같다. \[log p(x_{0:t} \mid z_{1:t}, u_{1:t}) = \text{const.} + log p(x_0) + \sum_t [log p(x_t \mid x_{t-1}, u_t)+ log p(z_t \mid x_t)]\]
여기서 모든 확률은 Gaussian 분포를 따르는데, Gaussian 분포의 log는 다음과 같다. \[log \mathcal{N}(x,\mu,\Sigma) = \text{const.} -\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\]
계산된 Gaussian의 log형태가 매우 익숙하다. 위의 형태는 우리가 최적화에 사용하였던 함수와 같은 형태임을 알수 있다. 따라서 posterior를 error function으로 표현하면 다음과 같다. \[\begin{aligned} log p(x_{0:t} \mid z_{1:t}, u_{1:t}) &= \text{const.} + log p(x_0) + \sum_t [log p(x_t \mid x_{t-1}, u_t)+ log p(z_t \mid x_t)]\\ &= \text{const.} - \frac{1}{2} e_p(\mathbf{x}) -\frac{1}{2} \sum_t [e_{u_t}(\mathbf{x})+e_{z_t}(\mathbf{x})] \end{aligned}\]
따라서, posterior의 최대값을 찾는 문제는 error function의 최소값을 찾는 문제가 된다. \[\begin{aligned} argmax \ \ log p(x_{0:t} \mid z_{1:t}, u_{1:t}) &= argmin \ \ e_p(\mathbf{x})+ \sum_t [e_{u_t}(\mathbf{x})+e_{z_t}(\mathbf{x})] \end{aligned}\]
이러한 Gaussian과의 관계에 의해서 위에서와 같이 error function이 정의됨을 기억하자.
본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었습니다.
이번 글에서는 Particle filter를 이용한 localization방법에 대해서 설명한다. EKF나 EIF와 같은 filter들은 Gaussian 분포만을 이용하여 시스템을 모델링하였다. Particle filter의 목적은 아래 그림과 같이 Gaussian이 아닌 임의의 분포를 다루기 위한 접근 방법이다.
Particle Selection
가중치를 갖는 particle들의 set은 다음과 같이 정의된다. \[\chi = \{<x^{[j]},w^{[j]}>\}_{j=1,2,\cdots, J}\]
$x^{[j]}$는 particle의 위치, $w^{[j]}$는 particle의 weight를 의미한다. Posterior는 다음과 같이 표현된다. \[p(x) = \sum_{j=1}^{J} w^{[j]}\delta_{x^{[j]}}(x)\]
위 식에서 $\delta$는 dirac delta 함수로 함수의 인자로 주어진 값 이외의 출력은 0이며 전체 합(Integral)은 1인 함수이다.
왼쪽 그림은 Gaussian 분포를, 오른쪽 그림은 임의의 분포를 보여준다.
이러한 분포에서 sample은 어떻게 추출될까?
Gaussian 분포의 경우는 위와 같이 $-\sigma$와 $\sigma$사이에서 12개의 임의의 값을 뽑고 합을 구함으로써 Gaussian 분포에 맞는 sample을 추출할 수 있다. 하지만 임의의 분포의 경우에는 위의 방법으로 sample을 추출할 수 없다.
위 분포는 Gaussian 분포가 아닐 때 sampling 하는 방법을 설명한다. proposal(x)는 실제 분포인 target(x)를 Gaussian 분포로 근사한 함수이다. 근사된 Gaussian 분포인 proposal(x) 함수에서 sample을 추출하고, 각 sample에 weight를 할당함으로써 실제 분포와 유사한 의미를 부여할 수 있다. 이때 할당되는 weight 는 \[w = \frac{f}{g}\]
이며, f는 target, g는 proposal의 함수값이다. 위 그림에서와 같이 실제 sample의 수는 Gaussian 분포를 따르지만 weight가 실제 함수의 확률을 반영한다.
Particle Filter Algorithm and Monte Carlo Localization
위의 sampling 방법을 적용한 particle filter 알고리즘은 다음과 같다.
이제 particle filter를 localization에 적용시켜보자. Monte Carlo 방법은 난수를 이용하여 함수의 값을 확률적으로 계산하는 알고리즘이다. 계산하려는 값이 close form(정확한 수식으로 계산되는 형태)이 아니거나 너무 복잡하여 근사적으로 구해야 할 때 주로 사용된다. particle filter는 이러한 Monte Carlo 방법이다. 이제 particle filter를 이용한 localization 알고리즘은 알아보자.
달라진 부분은 sample을 구하는 3번과, weight를 계산하는 4번이다. sample을 구하는 3번식은 로봇의 motion model이 되며, 각 particle의 weight는 센서의 observation model이다. 7번부터 10번까지는 resampling 과정이다.
Resampling
Resamping 과정은 확률이 작은 particle은 제거하고, 확률이 큰 파티클은 여러개로 나눈다. Resampling을 하는 이유는 우리가 사용할 수 있는 point의 갯수가 한정적이기 때문이며(computation문제와 memory문제 때문에) resampling 전과 후의 particle 갯수는 같아야 한다. 또한 resampling 전에 particle들의 weight는 다르지만, resampling 후에는 모두 같아진다. 아래 그림은 resampling을 하는 두가지 방법을 보여준다.
우선 노란색 원은 각 particle의 weight에 맞게 영역을 할당하여 원으로 표현한 것으로 생각할 수 있다. 즉 particle의 weight가 클수록 차지하는 영역이 크므로 랜덤하게 선택될 가능성이 크다.
Roulette wheel 방법의 경우 한번의 1개씩 random하게 particle을 추출한다. Stochastic universal sampling 방법은 random하게 추출함과 동시에 일정한 간격으로 particle을 추출한다. stochastic universal sampling 방법의 장점으로는 계산시간이 선형적이며, 모든 particle의 weight가 같은 경우 이전과 같은 형태로 particle이 resampling 된다는 점이다.
Particle Filter Localization Example
particle filter localization에서 초기상태는 아래 그림과 같다. 모든 particle은 균일하게 분포되어 있으며 동일한 weight를 갖는다.
control input에 의해서 모든 particle은 motion update가 진행된다.
로봇의 센서로부터 주변 환경에 대한 데이터를 수집한다.
센서 정보를 이용하여 각 particle의 weight를 update한다.
계산된 weight를 기준으로 resampling과정을 거친다.
resampling된 particle들을 이용하여 다시 motion update를 한다.
로봇 센서로부터 주변환경에 대한 데이터를 수집한다.
데이터로 부터 각 particle의 weight를 update한다.
update된 weight를 기준으로 다시 resampling을 한다.
resampling된 particle을 기준으로 motion이 update된다.
위 그림과 같이 Particle filter는 motion update, measurement, weight update, resampling과정을 반복하며 로봇의 위치를 추정해 나간다.
