[SLAM] TF, Localization, AMCL

📕 참고: 표윤석 박사님 SLAM과 네이게이션 강의 자료

TF(Transform Frame)

---

TF는 로봇 좌표계(frame) 간의 변환 관계를 정의하고 관리하는 라이브러리이다. 로봇에서 각각의 센서 데이터는 자신의 기준 좌표계를 통해 데이터를 생성한다. TF는 이러한 좌표계간의 변환(회전, 이동)을 추적하고 계산하여, 하나의 좌표계에서 다른 좌표계로 데이터를 변환해주는 역할을 한다.

그럼 왜 TF을 사용해야 할까?

LiDAR 센서가 base_laser 프레임에 부착되었다고 가정한다.
이 센서가 base_laser 기준으로 (0.3, 0.0, 0.0) 위치에 장애물을 감지했다고 할 때, 이 좌표는 base_laser 기준의 값이다.

base_link에서 감지한 장애물 위치를 생각해보자.
base_laser는 base_link 기준으로 z축으로 +0.2m, x축으로 +0.1m 떨어져 있다.
따라서, base_link 기준에서는 장애물이 (0.4, 0.0, 0.2) 위치에 있다고 계산된다.

이처럼 로봇이 이동 할 때 각 센서 프레임의 위치와 방향도 변하게 된다. 이때, 고정된 기준 프레임(base_link)를 사용하지 않으면 로봇의 움직임에 따라 모든 센서 데이터를 계속 계산하고 변환해야 하는 번거로움이 생긴다.

TF 라이브러리는 모든 센서 프레임 데이터를 자동으로 base_link 기준으로 변환해준다. 이렇게 변환된 데이터는 base_link 기준으로 통합되므로, 각 센서 데이터를 다른 기준으로 변환할 필요 없이 바로 사용할 수 있다. 따라서, 로봇이 장애물과의 상대적 위치를 계산하거나 이동 방향을 결정하는 작업이 훨씬 간단해진다.

---

주요 프레임은 다음과 같다.

📌 map

• 로봇이 동작하는 전체 환경에 대한 지도의 기준을 나타내는 프레임
• 모든 로봇의 위치와 방향은 map 프레임 기준으로 표현

📌 odom

• 로봇의 처음 시작 위치를 기준으로 한 로봇의 상대적 위치를 나타내는 프레임
• 시간에 따른 누적 오차가 발생하여 위치 보정 필요

📌 base footprint

• 로봇의 바닥을 나타내는 프레임
• 로봇이 물리적으로 어떻게 지면 위에 위치하고 있는지 정의

📌 base link

• 로봇 본체 중심을 나타내는 프레임
• 다른 센서 및 프레임과 연결
• 모든 센서는 이 프레임을 기준으로 변환

프레임 구조를 TF Tree로 표현하면 map → odom → base_footprint → base_link 로 움직이는 걸 확인할 수 있다.

특히, map → odom 간의 관계가 중요하다
odometry 정보는 주로 엔코더 이동 정보로부터 파생되므로 시간에 따른 누적 오차(drift)가 발생한다. 그래서 이 부분을 Localization이 보정하여 로봇이 map 기준 자신의 위치를 정확히 알 수 있게 된다.

Localization

---

Localizaion은 로봇이 주변 환경을 인식하고, 지도에서 자신이 어디 있는지 주변 정보와 센서 데이터를 활용해 위치를 추정하는 과정을 의미한다.

MCL(Monte Carlo Localzation)

MCL은 입자 필터(Particle Filter) 기반의 알고리즘으로, 로봇의 위치를 확률 분포로 표현하며 예측을 통해 임의로 생성된 입자를 업데이트하면서 점진적으로 로봇의 위치를 추정한다.

입자는 로봇의 위치와 방향을 나타내는 추측 값이다. 그래서 여러 개의 입자는 로봇이 맵 상 어디에 있을지에 대한 다양한 가설을 나타낸다.

MCL은 다음의 과정을 거친다.

1. 초기화(initialization)

• 지도 상에 랜덤한 입자들을 생성

2. 예측(prediction)

• 로봇의 odometry 데이터를 기반으로 입자들의 위치 이동

3. 보정(update)

• 로봇의 센서 데이터를 사용해 각 입자가 현재 환경과 얼마나 일치하는지 계산
• 일치 정도에 따라 입자 가중치 업데이트

4. 위치 추정(pose estimation) & 재추출(resampling)

• 가중치가 높은 입자들은 복제하고 낮은 입자들은 제거
• 가장 가중치 높은 입자를 기준으로 로봇 위치와 방향 추정

결과적으로 입자 분포가 로봇의 실제 위치 주변으로 밀집된다.

AMCL(Adaptive Monte Carlo Localization)

AMCL은 MCL(Monte Carlo Localization)의 확장된 버전으로 입자 수를 동적으로 조정하여 MCL의 효율성과 정확성을 개선하였다.

특성	MCL (Monte Carlo Localization)	AMCL (Adaptive Monte Carlo Localization)
입자 수	고정된 개수의 입자를 사용	불확실성에 따라 필요한 만큼 입자를 사용
정확성	초기 입자 수에 따라 정확성이 크게 좌우	입자 수를 조정하며 위치 추정을 점진적으로 정확히 수행
특징	위치 불확실성이 클 때도 일정한 입자 수를 유지	불확실성이 클 때는 입자 수를 늘려 더 많은 가능성을 탐색

MCL은 고정된 입자 수를 사용하는 반면,
AMCL은 위치의 불확실성이 커지면 많은 입자를 사용하고, 위치가 명확해지면 적은 수의 입자를 사용하여 불필요한 계산을 줄이고 위치 추정의 안정성을 확보한다.

📌 Rviz에서 2D Pose Estimate

Rviz에서 2D Pose Estimate를 수행하는 것은 AMCL에 초기 위치와 초기 방향을 알려주는 과정이다.

초기 위치가 없으면 어떤 문제가 있을까?

1. 센서데이터와 입자데이터 불일치
AMCL은 센서 데이터와 입자 분포를 비교하여 로봇의 위치를 추정하는데, 초기 위치가 없으면 로봇이 어디에 있는지 전혀 알 수 없다.

2. 입자 분포의 퍼짐
초기 위치를 지정하지 않으면 입자들이 맵 전체에 고르게 분포된다. 이렇게 되면 로봇 위치와 상관없이 많은 입자가 생성되어 실제 로봇의 위치 데이터를 파악하기 어렵다.

하지만 초기 위치를 지정하면
AMCL이 입자들을 올바른 영역에 집중시킬 수 있어 위치 추정을 빠르고 정확하기 할 수 있다! 따라서, 2D Pose Estimate는 Localization을 시작하는데 필수적인 단계이다.

rviz2를 실행하면 처음에 /map 프레임이 존재하지 않는다고 뜬다. AMCL이 초기 위치를 모르면 맵과 로봇의 관계가 설정되지 않아 /map → /odom 트랜스폼도 생성되지 않는다.
따라서 /map 토픽을 필요로 하는 Navigation2의 다른 노드들도 동작하지 않게 되기 때문이다.

2D Pose Estimate를 해주면 /map 프레임과 Nav2가 잘 동작하는 걸 확인할 수 있다.