A General Framework for Lifelong Localization and Mapping in Changing Environment

이 논문은 싱가폴 Min zhao라는 분이 회사에서 쓴 논문 같습니다.

---

Introduction

슈퍼 마켓이나 주차장 등 정적이지 못한 환경에서 로봇이 Localization을 해야할 경우가 많습니다.
이렇게 변화하는 환경을 map에 업데이트 하면서 SLAM하는 것을 이 논문에선 Lifelong SLAM이라고 정의합니다.

SLAM은 보통 프론트엔드와 백엔드로 구성되어 있습니다.
프론트엔드: 센서 데이터를 수집 + 연속된 데이터 프레임 간 변환을 계산
백엔드: Loop Closure를 수행하여 프론트 엔드 추정 드리프트를 보정

이 논문에서는 맵 업데이트 모듈을 도입하여 새로운 환경을 업데이트 할 수 있도록 합니다.

맵 업데이트 모듈
1. 센서 데이터를 수집하고 동적 장면 기록
2. 과거 맵과 실시간 업데이트된 맵 사이 차이를 탐지
3. 실시간 맵으로 과거 맵을 트리밍하여 변화하는 환경에 맞는 최신 맵 유지
4. 계산 복잡도를 일정하게 유지하면서 효율적으로 작동

주요 Contribution
1. Lifelong SLAM의 프레임 워크 제안
2. 서브맵 기반 그래프 최소화
3. 공개 데이터 셋을 공개

---

System Overview

• System Structure

  이 그림과 같이 Framework는 3개의 하위 시스템으로 구성됩니다.

  1. Local LiDAR Odometry(LLO): 스캔을 기반으로 연속적인 서브맵을 생성
  2. Global LiDAR Matching(GLM): 새로운 스캔 데이터와 글로벌 서브맵 간 상대 constraint 생성
  3. Pose Graph Refinement(PGR): LLO와 GLM에서 전달받은 데이터를 활용하여 전역 맵 유지

• Notation
  주요 변수
  $X$: 노드의 위치를 나타냅니다.
  $M$: 서브맵의 위치를 나타냅니다.
  $Z$: 상대적인 Constraint을 나타냅니다.

  $z^{mix}_{ij}$: node submap constratint
  $z^{node}_{ij}$: node node constratint
  $z^{map}_{ij}$: submap submap constratint

  node, submap 간의 관계와 공분산 행렬을 통해 constraint를 부여합니다.

  $z^{odom}_{ij}$: 인접한 node간의 제약은 odometry constraint라고 따로 부여

  Pose 표현
  이 논문은 평탄한 지형을 가정합니다. 2D 환경에서 작동하고 3D환경을 다루는 SE(3)로 쉽게 확장가능하다고 설명합니다.

• Problem Definition

  포즈와 constraint가 주어졌을 때, graph 최적화 문제를 MAP 문제로 정의합니다.
  MAP 문제는 다음과 같은 Least Squares Problem으로 변홥됩니다.
  $X^*,M^* = argmin_{X,M}J(X,M,Z)$

  Cost function J는 다음과 같이 정의됩니다.

  $J(X,M,Z) = ||x_0^0||_{\Omega_0}+||m_0^0||_{\Omega_0}+\Sigma_{s=0}^{S-1}(\Sigma_{i,j}||x_i^s⊖m_j^{s'}⊖z^{mix}_{i^sj^{s'}}||^2_{\Omega^{mix}_{i^sj^{s'}}}+\Sigma_{i,j}||x_i^s⊖x_j^{s'}⊖z^{odom}_{i^sj^{s'}}||^2_{\Omega^{mix}_{i^sj^{s'}}}+\Sigma_{i,j}||x_i^s⊖x_j^{s'}⊖z^{node}_{i^sj^{s'}}||^2_{\Omega^{mix}_{i^sj^{s'}}}+\Sigma_{i,j}||m_i^s⊖m_j^{s'}⊖z^{map}_{i^sj^{s'}}||^2_{\Omega^{mix}_{i^sj^{s'}}})$

  ---

  Pose Graph Refinement

• Multi-Session Localization

  

  • 초기 맵핑 단계에서는 새로운 환경에서 로봇이 LiDAR,IMU, 휠 인코더 등의 센서 데이터를 수집하여 환경을 표현하는 맵을 생성합니다. 맵은 여러 Occupancy grid submap으로 구성되어 있고 각 서브맵은 고정된 개수의 LiDAR 스캔과 해당 스캔의 위치를 포함합니다.

  • Localization 단계에서는 초기 맵을 기반으로 Localization 작업을 수행하며 새로운 서브맵을 생성합니다. 생성된 서브맵은 Pose Graph Refinement로 전달됩니다.

  • Global LiDAR Matching 단계에서 새로 수집된 스캔 데이터와 글로벌 서브맵 간의 Relative Constraint를 계산합니다. 계산된 Constraint는 PGR 모듈로 전달됩니다.

  • Pose Graph Refinement 단계에서는 Map update 과정을 수행합니다.

    Pose Graph Refinement는 3가지 하위 시스템으로 구성되어 있습니다.

    1. Submap Trimming: 오래된 서브맵을 제거하여, 최신 서브맵만 유지

    2. Pose Graph Sparsification: 중요도가 낮은 노드와 제약을 제거하여, 그래프를 간결하게 유지

    3. Pose Graph Optimization: 새로운 Submap과 Constraint를 통합하여 그래프를 최적화

    PGR에서 처리된 Submap은 Global Submap Database로 전송합니다.
    이 데이터 베이스를 통해 Localization을 진행합니다.

• Pose Graph Refinement

  • Submap Trimming
    로봇이 이전에 방문한 지역을 재진입할 때, 오래된 서브맵을 새로운 서브맵으로 대체하는 module입니다.
    Submap의 수를 제한하여 계산 복잡도를 제어합니다.

    기존의 방식은 맵과 최신맵을 cell단위로 비교하여 변화여부를 탐지하는 것이지만 이것은 계산량이 많습니다.
    이를 해결하기 위해 Overlapping ratio 계산 방식을 도입하였습니다.

    Overlapping은 Cell 하나하나에 대한 정보를 계산하는 것이 아니라 Submap대 Submap으로 겹치는 비율을 계산하여 submap을 유지할지 말지를 정합니다.

    threshold보다 낮으면 같은 위치의 현재 submap과 과거 submap의 차이가 크다는 것이기 때문에 submap을 global 계산에 남겨두고 threshold보다 크면 별 차이 없다는 것이기 때문에 trimming으로 표시하고 삭제합니다.

    새로운 서브맵들은 항상 Pose graph에 추가되어 계산에 활용됩니다.
    이전 Submap들을 삭제하는 방식으로 메모리를 관리합니다.

  • Pose Graph Sparsification

    서브맵을 삭제할 때 관련된 노드와 constraint도 제거하면 그래프의 중요한 정보가 손실될 수 있습니다. 정보의 손실을 줄이기 위해 marginalization을 도입하였습니다.

    Marginalization: $p(x) = \int p(x,y)dy$

    이렇게 지우는 과정에서 주변 node와의 새로운 제약 정보를 추가해줍니다.
    이렇게 되면 제거된 노드의 모든 이웃 노드 간에 새로운 constraint가 추가되기 때문에 Graph가 매우 빽빽해지는 문제가 발생합니다.

    이를 해결하기 위해 Chow-Liu Tree 를 사용하여 graph를 조금 더 Sparse하게 만듭니다.

    다음은 Graph를 Spasification하는 것을 묘사한 그림입니다.
    

    (a)-(b)-(c)과정은 Marginalization을 통해 node를 삭제하여 constraint가 생기는 과정입니다. (d)에서 graph Sparsification과정이 발생합니다.

    이 과정에서 새롭게 constraint가 생기는 node들을 locally approximation합니다.
    
    이는 markov assumption기반 approximation으로 이전 하나의 node에만 영향을 받는다는 식으로 근사를 합니다.
    

    KL divergence를 통해 node간의 중요도를 판단하고 중요한 constraint만 남겨서 sparsification을 진행합니다.

    ---

    환경은 항상 변하는데 그것을 고려하는 SLAM이 나온 것 같습니다.
    현재는 예상치 못한 급격한 환경 변화를 충분히 처리하지 못하고
    새로운 스캔데이터와 기존 맵 사이에 유효한 Constraint가 없는 경우 drift가 발생한다는 한계가 있습니다.
    그것을 고려하더라도 좋은 접근 방식이라고 생각했습니다.