[ SLAM ] Graph SLAM
0. 머리말
Graph SLAM에서는 Node를 로봇의 위치, Edge를 로봇의 위치 변화로 하는 Graph를 만든다.
그리고, Node와 Edge로 이루어진 Graph를 최적화하여 가장 적합한 Pose의 Node들을 계산한다.
SLAM(Simultaneous Localization and Mapping)이란 자신의 위치를 추정(Localization)하고, 동시에 주변의 환경 지도를 작성(Mapping)하는 기술을 말한다. 자율 주행 자동차, 로봇 분야에서 다양하게 사용되고 있으며, 다방면으로 개발되고 있는 기술이다.
SLAM에는 여러 종류가 있으나, 오늘은 Graph SLAM에 관하여 얘기해보도록 하겠다.
1. Graph SLAM
Graph SLAM은 수학적 개념인 Graph를 이용하여 SLAM을 수행하기 때문에 이름이 Graph SLAM이다. 위키피디아를 참고하면, Graph는 Node와 Node들을 연결하는 Edge로 이루어진 것을 말한다. 그림으로 표현하면 다음과 같다.
Graph SLAM에서는 Node를 로봇의 위치(=Pose), 그리고 Edge를 로봇의 위치 변화(=Constraint)로 하는 Graph를 만든다. 그리고, 이렇게 Node와 Edge로 이루어진 Graph를 만든 후, 그래프 최적화를 통해 최적화된 Pose를 가진 Node들을 구하게 된다.
Pose = Node , Constraint = Edge
Graph SLAM에서 Graph를 만들기 위해 제일 먼저 해야하는 일은 Node를 만드는 일이다. 대부분의 Graph SLAM에서는 로봇이 일정 거리를 움직이거나 일정 각도를 회전하였을 때, 새로운 Node를 추가하도록 되어있다. 그리고 새로운 Node가 추가되면, 우리는 Node들 사이의 Edge인 Constraint를 구하게 된다. 여기서, 우리는 Constraint를 구하기 위해 이전 위치에서의 데이터와 현재 위치에서의 데이터를 이용하게 하여 이전 위치에 대하여 현재 얼마나 이동, 또는 회전했는지 알아낼 수 있다. 예를 들어 LiDAR 센서를 이용하면, 이전 위치에서의 스캔 데이터와 현재 위치에서의 스캔 데이터를 정합하여 얻어낸 변환(Transformation)을 통해 두 Pose 사이의 Constraint를 구할 수 있다. 이렇게 하면, Node와 Edge를 구했으므로 Graph를 만들 수 있게 된다. 따라서, 여기까지의 과정만으로도 로봇의 위치 추정과 지도 작성을 할 수 있으나, 우리는 더 정밀한 결과를 위해 Graph의 최적화를 시도하게 된다. 즉, Graph의 최적화를 위해 기준이 될 수 있는 특정한 조건을 찾게 되며, 대게는 특정한 조건으로 로봇이 동일한 위치에 온 것을 판단하는 Loop Closure를 찾는다. 이를 Loop Closure Detection이라고 부르며, 여기까지가 Graph SLAM의 front-end에 해당하는 부분이다. 정리하면, Front-end에서는 (1) Node 생성, (2) Edge 계산, (3) Loop Closure Detection을 맡게 된다.
이후에 Graph SLAM의 back-end에서는, 앞서 찾은 Loop Closure를 기준으로 Graph의 최적화를 수행하게 된다. 여기서, 우리는 Constraint를 기준으로 하여 가장 적합한 Node(=Pose)들을 찾는 것을 목적으로 한다. 즉, Back-end에서는 Graph optimization을 맡게 된다.
2. Front-end
2-1. Node Generation
실제 로봇의 움직임은 연속적이지만, 센서의 측정 주파수는 유한하므로 우리는 로봇의 위치를 완전히 연속적으로 기록할 수 없다. 하지만 다행히도, 우리는 로봇의 위치를 적당한 간격을 두어 추정해도 괜찮은 지도 결과를 얻을 수 있다. (물론, 로봇의 움직임을 정밀하게 제어하기 위해서는 위치를 추정하는 주기가 짧을수록 좋다.) 오히려 추정된 모든 로봇의 위치를 그대로 사용하여 그래프의 Node의 갯수가 많아지게 되면, 실제 로봇에서는 메모리 관리 측면에서 매우 비효율적인 동작이 된다. 따라서, 대부분의 SLAM에서는 일정한 거리/각도를 로봇이 움직였을 때, Node를 생성하도록 설계되어있다.
2-2. Edge Calculation
로봇이 한 위치에서 Node를 만들고, 일정한 거리/각도를 움직였다고 판단되어 새로운 Node를 생성하게 되는 경우, 우리는 두 Node 사이의 Edge를 계산해주어야 한다. 다양한 방법으로 두 Node에서의 관측된 Observation을 비교하여 Edge를 계산할 수 있는다. 현재, 가장 일반적인 방법은 3D LiDAR 센서를 기반으로 하여 두 Node에서의 3D Scan을 매칭하면 두 Node 사이의 Edge를 알아낼 수 있다. 이때, Scan Data인 3D Point Cloud를 정합하기 위해서 ICP(Iterative Closest Point)나 NDT(Normal Distribution Transform)와 같은 스캔 정합 알고리즘이 보편적으로 사용된다.
2-3. Loop Closure Detection
로봇이 같은 자리로 돌아온 것을 인지하는 것을 Loop Closure Detection이라고 하며, 대부분의 SLAM에서 이 조건을 Graph Optimization의 조건으로 사용한다. 우리는 최적화를 통해서 더 정밀한 위치 추정 결과를 얻어낼 수 있기 때문에, 제자리로 돌아왔음을 잘 인지하는 것은 매우 중요하다. 만약, Loop Closure를 잘못 판단하게 되면 이전에 추정해놓은 위치 결과들이 모두 뒤틀릴 수 있으므로, 정확히 판단하는 것이 중요하다. Loop Closure를 판단하기 위해 이전에 추정해온 위치와 현재 추정된 위치를 비교하거나, Global하게 정해진 마커(marker)를 이용한다던지, GPS를 이용하는 방법도 있다.
3. Back-end
3-1. Loop Closure Optimization
Front-end에서는 Node Generation, Edge Calculation, 그리고 Loop Closure Detection을 수행하였다. Back-end에서는 앞서 검출된 Loop Closure를 바탕으로 최적화 문제를 계산하는 과정을 도맡는다. 오로지, 수학적으로 최적화 문제의 답을 계산하는 영역인 것이다. 특히, 여기서 다루는 Node와 Edge로 이루어진 Graph를 최적화하는 것을 Graph Opitmization이라 하고, 다행히도 많은 오픈 소스들이 개발되어 쉽게 Graph Optimization을 해결할 수 있다. 이를 구현해놓은 오픈 소스로는 g2o, ceres solver, gtsam, iSAM 등이 있다. 해당 소스들은 구글에 검색하면 쉽게 찾을 수 있으며, 많은 용법들이 공개되어 있기에 유용하게 사용할 수 있다.
사실, back-end는 이름에서도 알 수 있듯이 front-end 뒤에 붙는 부수적인 영역이다. 이미 front-end가 Graph SLAM의 핵심이자 그래프 생성을 모두 맡고 있는 영역이기에, 실험 측정값과 추정된 위치의 신뢰도가 충분한 상황이라면 back-end가 필요없는 경우도 있을 것이다. 그럼에도 back-end의 loop closure optimization이 필요한 이유는 front-end에서는 시간이 경과하면서 새로이 추가되는 node들의 불확실성(covariance)가 지속적으로 높아지기 때문이다. 시간이 지남에 따라 추정되는 위치가 점점 불확실해지는 것은 지역적으로 발생하는 오차가 누적되면서 자연스럽게 생기는 현상이다. 여기서 지역적으로 국소적으로 발생하는 오차를 drift error라고 부른다. loop clousre optimization은 각 node의 pose와 covariance를 모두 고려하여 최적화를 하기에 지역적으로 누적된 drift error를 보정한다고 볼 수 있다.
4. References
[1] jinyongjeong.github.io/2017/02/26/lec13_Least_square_SLAM/
[2] www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
[3] ais.informatik.uni-freiburg.de/teaching/ws13/mapping/