1. 들어가며
쇼핑몰 환경에서 3대의 Pinky 로봇을 운용하는 프로젝트를 진행 중이다. 로봇이 넓은 공간을 돌아다니다 보면 odometry 누적 오차(drift)가 생기고, 이걸 주기적으로 교정해줘야 AMCL 기반 위치 추정이 안정적으로 유지된다.
탑뷰 카메라가 있었다. 천장에서 로봇 위치를 내려다보며 교정하는 방식인데, 구현 자체는 어렵지 않다. 카메라로 로봇을 인식하고 좌표 뽑아서 넣어주면 끝이다. 너무 단순하게 느껴졌다.
그래서 AprilTag를 이용한 셀프 교정을 시도해봤다.
"태그가 맵 어디에 어떤 방향으로 붙어있는지 우리가 이미 안다. 로봇이 그 태그를 카메라로 인식하는 순간, 태그의 기울기를 역산하면 로봇의 절대 yaw를 구할 수 있다."
탑뷰 카메라 없이, 로봇 자신이 벽에 붙은 태그를 보는 것만으로 자기 방향을 교정하는 방식이다. 이론은 깔끔한데... 현실은 삽질의 연속이었다.
2. 핵심 아이디어
전체 흐름은 이렇다.
- 맵에 AprilTag를 붙이고, 각 태그의 절대 위치와 방향(
TAG_INFO)을 사전에 정의해둔다. - 로봇이 이동하다가 카메라로 태그를 인식하면,
pupil_apriltags로 태그의 R 행렬(회전 행렬)을 뽑는다. - R 행렬에서 태그 기준 yaw를 계산하고,
TAG_INFO의 절대 방향과 합산해서 로봇의 절대 yaw를 역산한다. - 역산한 yaw를
/initialpose토픽으로 AMCL에 주입해서 위치를 교정한다.
깔끔해 보이지만, 각 단계마다 함정이 있었다.
3. 삽질 #1 — TAG_INFO yaw 방향을 반대로 정의했다
문제
로봇이 태그 정면에 서있을 때 /initialpose의 yaw 값이 약 -2.94였다. 이걸 그대로 TAG_INFO에 넣었다.
시도: 로봇이 태그 정면에 섰을 때의 yaw(-2.94)를 TAG_INFO에 사용
결과: yaw 교정값이 계속 180도 가까이 틀어져 나옴
원인
이 값은 로봇이 바라보는 방향이다. 카메라에 찍힌 태그 이미지로 로봇 yaw를 역산하려면, TAG_INFO에는 태그가 map에서 바라보는 방향이 들어가야 한다. 로봇이 태그를 정면으로 바라보고 있을 때, 태그는 로봇의 반대 방향을 향하고 있다. 즉 두 값은 약 180도 차이가 난다.
해결
수식으로 고치려고 +π를 더하거나 빼거나 부호를 바꾸는 등 여러 번 시도했는데, 다른 오차들이 섞여서 정확히 안 잡혔다. 결국 가장 확실한 방법으로 바꿨다.
TAG_YAW = 0으로 놓고 계산 yaw 출력
→ 실제 AMCL yaw와 비교
→ 오프셋 = 실제 yaw - 계산 yaw
→ TAG_YAW = 오프셋좌표계나 π 계산을 직접 따질 필요 없이, 실측으로 오차를 한 번에 흡수하는 방식이다. 태그를 새로 추가할 때도 동일하게 적용할 수 있어서 재현성도 있다.
4. 삽질 #2 — yaw만 고치면 됐는데 x, y까지 덮어썼다
문제
태그 위치를 알고 있으니, 태그까지의 거리와 방향을 이용해서 로봇의 x, y 위치까지 계산하고 /initialpose로 x, y, yaw를 전부 덮어쓰는 방식으로 설계했다.
시도: /initialpose로 x, y, yaw 전부 덮어씀
결과: 로봇이 태그 위치로 텔레포트, AMCL 파티클 붕괴
원인
AMCL은 파티클 필터 기반이라, /initialpose로 위치를 덮어쓰면 파티클이 해당 위치 주변으로 재분포된다. x, y까지 같이 바꾸면 파티클이 현재 위치에서 완전히 다른 위치로 순간이동하는 셈이다. LiDAR 기반으로 x, y는 이미 어느 정도 맞게 잡혀있는 상황에서 굳이 건드릴 필요가 없었다.
해결
x, y는 /amcl_pose 토픽에서 현재값을 그대로 가져오고, yaw만 교정값으로 교체하는 방식으로 변경했다.
# 수정 전: x, y, yaw 전부 덮어쓰기
initial_pose.pose.pose.position.x = tag_x
initial_pose.pose.pose.position.y = tag_y
initial_pose.pose.pose.orientation = corrected_yaw_quat
# 수정 후: x, y는 현재 AMCL값 유지, yaw만 교정
current_pose = # /amcl_pose 토픽에서 가져옴
initial_pose.pose.pose.position.x = current_pose.pose.pose.position.x
initial_pose.pose.pose.position.y = current_pose.pose.pose.position.y
initial_pose.pose.pose.orientation = corrected_yaw_quat5. 삽질 #3 — 검증 기준 자체가 AMCL을 흔들었다
문제
교정이 제대로 되는지 확인하려고 RViz의 2D Pose Estimate 버튼으로 로봇의 실제 위치를 찍어서 교정 전후 yaw를 비교했다.
시도: 2D Pose Estimate로 실제 위치 찍어서 비교
결과: 비교할 때마다 결과가 달라짐. 교정이 잘 된 건지 아닌 건지 판단 불가
원인
2D Pose Estimate 자체가 /initialpose를 발행한다. 검증을 위해 위치를 찍는 행위가 AMCL의 파티클을 흩트려서, 비교 기준이 되는 AMCL pose 자체를 바꿔버리고 있었다. 측정 도구가 피측정 대상에 영향을 주는 상황이었다.
해결
두 가지로 바꿨다.
첫째, 비교 기준을 /amcl_pose 토픽 직접 구독으로 변경했다. 토픽을 읽는 건 AMCL에 아무 영향을 주지 않는다.
둘째, ROS2 없이 단순 파이썬 스크립트로 yaw 값만 빠르게 검증하는 방식을 추가했다. 시스템 전체를 돌리지 않아도 계산 로직이 맞는지 빠르게 확인할 수 있어서 디버깅 속도가 훨씬 빨라졌다.
6. 트러블슈팅 정리
문제 1: yaw 교정값이 계속 180도 가까이 틀어져 나온다
원인: TAG_INFO yaw를 로봇 시점으로 정의함
해결: TAG_YAW=0 기준으로 실측값과 비교해 오프셋 역산
문제 2: 교정 후 로봇이 순간이동한다
원인: /initialpose에 x, y, yaw를 전부 덮어씀
해결: x, y는 /amcl_pose에서 현재값 그대로 사용, yaw만 교정값으로 교체
문제 3: 검증할 때마다 결과가 달라진다
원인: 2D Pose Estimate가 내부적으로 /initialpose를 발행해 AMCL을 흔듦
해결: /amcl_pose 토픽 직접 구독으로 비교, ROS2 없는 독립 파이썬 스크립트로 계산 로직 단위 검증
7. 배운 점
1. 방향 정의는 "누구의 시점인가"를 먼저 확인하자
태그 yaw처럼 방향이 개입되는 값은 항상 기준 시점을 명확히 해야 한다. 머릿속에서 맞춰보려 하지 말고, 실측값 기준으로 역산하는 게 훨씬 안전하다.
2. 교정 범위는 필요한 것만
AMCL처럼 파티클 필터 기반 시스템은 값을 크게 바꿀수록 불안정해진다. x, y가 이미 맞다면 yaw만 건드리는 게 맞다. 욕심부리다 오히려 더 망가진다.
3. 측정 도구가 피측정 대상에 영향을 주는지 확인하자
2D Pose Estimate로 검증한다는 게 사실 AMCL을 계속 흔들고 있었다... 디버깅할 때 내가 쓰는 도구 자체가 시스템에 영향을 주는지 항상 의심해봐야 한다.
4. 단위 검증을 먼저
ROS2 전체 시스템을 띄우지 않아도 계산 로직만 따로 파이썬 스크립트로 검증할 수 있었다. 처음부터 이렇게 했으면 훨씬 빨리 끝났을 것이다.
8. 마무리
탑뷰 카메라가 있음에도 AprilTag 셀프 교정을 시도한 건 "더 어렵고 재미있어 보여서"였다. 결과적으로는 세 가지 삽질을 거쳐 yaw만 교정하는 깔끔한 구조가 완성됐다.
돌아보면 삽질의 공통점이 있었다. "이렇겠지"라고 가정하고 전체를 다 구현한 뒤 테스트했다는 점이다. 가장 단순한 케이스부터 하나씩 확인했으면 훨씬 빨리 끝났을 것이다. 로봇 소프트웨어는 가정이 틀렸을 때 증상이 엉뚱한 곳에서 나타나는 경우가 많다. 작은 단위로 쪼개서 검증하는 습관이 특히 중요하다는 걸 다시 한번 느꼈다.
