LECTURE 03

VSLAM · 내비게이션

미지 환경에서의 위치 추정과 경로 계획

10/2CRASH LAB M1

01
카메라와 위치 추정오도메트리·드리프트
02
VSLAM 원리프론트엔드·백엔드
03
지도와 코스트맵특징점·점유 격자
04
내비게이션 연동TF·Nav2
05
구현 사례시행착오와 측정

QUESTION

카메라로 위치를 찾았다면, 바로 목적지까지 갈 수 있을까?

위치 추정경로 계획제어기

SLAM은 위치 추정과 지도 생성을, Navigation은 경로 계획과 제어기 실행을 맡는다.

VSLAM이 주는 것

로봇의 현재 자세를 추정한다.
관측한 특징점으로 지도를 만든다.
같은 장소를 다시 인식해 누적 오차를 줄인다.

Navigation이 더 필요한 것

이동 가능한 공간과 장애물 위치
로봇 크기와 속도·가속도 한계
목적지까지의 경로와 추종 제어기

QUESTION

외부 관측 없이 걸으면 몇 걸음 만에 위치를 잃을까?

위치 추정경로 계획제어기

바퀴가 돈 양으로 이동 거리를 계산한다.
이를 적분해 현재 위치를 추정한다.
슬립은 추정에 반영되지 않는다.

BUILD 1 / 2

작은 오차도 적분 과정에서 누적된다.

BUILD 2 / 2

백 걸음이면 방 하나 크기만큼 벌어진다.

QUESTION

카메라 한 대로 물체까지의 거리를 알 수 있을까?

위치 추정경로 계획제어기

단안 Vision-only SLAM은 절대 스케일을 관측할 수 없다.

정류된 평행 스테레오에서는 Z ≈ fₓB/d로 뎁스를 근사한다.
fₓ와 d는 픽셀 단위, B와 Z는 같은 길이 단위다.
원거리에서는 시차가 작아져 뎁스 오차가 커진다.
정류·캘리브레이션이 어긋나면 대응점과 뎁스가 틀어진다.

유리·반사체·검은 물체에서는 뎁스 값이 누락될 수 있다.

IMU는 각속도와 비력(specific force)을 높은 주기로 측정한다.
자세·중력 방향·속도·바이어스는 카메라와 IMU를 융합해 추정한다.
IMU는 빠른 회전 구간에서 카메라 프레임 사이 움직임을 보완한다.
카메라는 IMU 적분 오차를 억제하지만 전역 드리프트를 없애지는 못한다.

정확한 외부 파라미터와 시간 동기화가 위치 추정의 기준을 맞춘다.

QUESTION

연속된 두 영상으로 로봇의 이동량을 계산할 수 있을까?

위치 추정경로 계획제어기

픽셀 기반 방식은 특징점 대신 선택한 픽셀의 밝기 오차를 사용한다.

K는 초점거리와 주점을 담은 내부 파라미터다.
[R|t]는 월드 좌표를 카메라 좌표로 바꾸는 변환이다.
카메라가 움직이면 같은 점이 다른 픽셀에 맺힌다.
픽셀 위치의 변화를 이용해 카메라 이동량을 추정한다.

여러 프레임에서 같은 키포인트를 추적해 카메라 이동을 계산한다.

특징점 기반

키포인트와 기술자로 프레임 간 대응을 만든다.
명시적인 대응과 장소 인식 구성에 유리하다.
저텍스처·반복 무늬·모션 블러에 약할 수 있다.

픽셀 기반

선택한 픽셀의 밝기 오차를 직접 줄인다.
특징 검출기가 버리는 영상 정보도 사용한다.
노출·조명 변화와 초기값에 민감하다.

모든 프레임을 지도에 넣으면 연산량이 빠르게 증가한다.
이동량·회전량·새 관측량을 기준으로 일부 프레임만 선택한다.
키프레임을 중심으로 지도와 카메라 궤적을 관리한다.

시선 교차각이 충분해야 3차원 점의 뎁스를 안정적으로 추정할 수 있다.

지도점을 영상에 다시 투영해 실제 관측과 비교한다.
예측 픽셀과 관측 픽셀의 차이가 재투영 오차다.
여러 카메라 자세와 지도점을 함께 조정해 오차를 줄인다.

같은 장소를 다시 인식하면 Pose Graph나 전역 BA로 누적 오차를 지도 전체에 보정한다.

비슷한 장면은 기하학적 검증을 통과해야 같은 장소로 인정한다.

루프 클로저

추적이 유지된 상태에서 같은 장소를 다시 인식한다.
목적은 누적된 지도 오차를 보정하는 것이다.
Pose Graph를 보정하고 필요하면 전역 BA를 수행한다.

위치 재추정

추적을 잃은 뒤 현재 위치를 다시 계산한다.
저장된 지도와 현재 영상을 대조한다.
성공하면 기존 지도 좌표계에서 추적을 다시 시작한다.

Visual SLAM

카메라 영상의 특징이나 픽셀 밝기를 사용한다.
색상·텍스처 등 풍부한 장면 정보를 얻는다.
조명 변화와 모션 블러에 민감할 수 있다.

LiDAR SLAM

레이저 거리와 주변 형상을 사용한다.
조명 변화의 영향을 거의 받지 않는다.
유리·반사면과 반복 형상에서 오차가 생길 수 있다.

QUESTION

VSLAM이 만든 지도를 그대로 내비게이션에 쓸 수 있을까?

위치 추정경로 계획제어기

특징점 지도는 위치 추정용이며, 장애물 지도는 아니다.

Dense Point Cloud표면 형상을 자세히 표현한다.

Occupancy Grid자유 공간과 장애물을 셀로 표현한다.

Sparse map은 위치 추정용이고, Navigation에는 충돌을 판단할 지도 표현이 필요하다.

지도 작성은 환경 지도를 만들고, 위치 추정은 저장된 지도에서 현재 자세를 계산한다.

QUESTION

지도가 생겼다. 이제 경로를 어떻게 정할까?

위치 추정경로 계획제어기

RGB-D와 Stereo 뎁스는 보통 PointCloud2 또는 LaserScan으로 변환해 입력한다.

비용이 낮은 칸을 이으면 안전한 경로가 된다.

너무 작으면

로봇이 벽에 바짝 붙어 지나간다.
작은 위치 추정 오차에도 모서리를 긁는다.
부풀림 반경은 로봇 반경보다 커야 한다.

너무 크면

좁은 통로가 통째로 막힌 것으로 보인다.
지나갈 수 있는데도 경로를 못 찾는다.
footprint와 통로 폭을 함께 재야 한다.

전역 플래너는 지도 전체를 탐색한다.
A*는 후보 경로의 비용을 비교한다.
최소 비용 경로를 선택한다.

지역 플래너는 실시간 장애물에 즉시 대응한다.

DWB	속도 후보를 평가해 짧은 시간 구간의 이동 명령을 선택한다.
MPPI	여러 제어 궤적을 샘플링하고 비용으로 가중해 명령을 계산한다.
Regulated Pure Pursuit	경로 곡률과 장애물 거리에 맞춰 추종 속도를 조절한다.
TEB	시간이 포함된 경로를 최적화하는 외부 플러그인 방식이다.

Behavior Tree는 계획·추종·복구의 실행 순서를 정의한다.
주행이 실패하면 원인에 맞는 복구 행동을 수행한다.
복구 뒤 다시 계획하거나, 재시도 한도를 넘으면 종료한다.

QUESTION

루프 클로저 때 연속으로 유지할 TF와 전역 보정을 반영할 TF는 무엇일까?

위치 추정경로 계획제어기

map→odom과 odom→base_link의 최종 발행 주체는 각각 하나만 둔다.

루프 클로저는 전역 위치 추정값을 보정한다.
전역 보정은 map→odom 변환에 반영한다.
odom→base_link는 제어기에 필요한 연속성을 유지한다.
따라서 map 기준 로봇 위치는 보정 순간 달라질 수 있다.

Nav2에는 전역 자세·TF와 충돌 지도를 역할에 맞게 연결한다.

위치 추정 모듈이 로봇의 현재 자세를 계산한다.
Planner가 목표까지의 경로를 생성한다.
Controller가 경로를 따라갈 속도 명령을 계산한다.
평면 Nav2는 주로 x, y, yaw 상태를 사용한다.

차동구동 로봇은 주로 linear.x와 angular.z를 사용한다.
홀로노믹 로봇은 linear.y 명령도 사용할 수 있다.
메시지 형식은 Nav2 배포판과 enable_stamped_cmd_vel 설정에 따른다.

저텍스처사용 가능한 특징점이 급격히 줄어든다.

거짓 루프 클로저반복 장면을 같은 장소로 오인하면 지도가 왜곡된다.

실패 원인은 장면·센서 설정·알고리즘 가정으로 나누어 진단한다.

바닥 오인식	기울어진 뎁스 포인트클라우드가 바닥을 장애물로 등록한다.
센서 사각	유리·검은 물체·낮은 장애물은 뎁스 값이 누락될 수 있다.
잔상 장애물	지나간 물체가 Costmap에 남아 이동 가능한 통로를 막는다.
전역 위치 보정	보정량이 크면 재계획이 필요하며, 로컬 제어기는 연속적인 odom을 사용한다.

QUESTION

토픽 흐름만 보고 직접 구현하면 무엇이 먼저 문제가 될까?

위치 추정경로 계획제어기

기성 SLAM 패키지 없이 토픽으로 공개된 값만 입력으로 사용한다.

슬라이드 11의 오차식으로 뎁스 상한을 4.2 m로 정했다.
ORB는 멀고 대비가 큰 구조물에 주로 반응한다.
회전 구간에서는 상한 안쪽 특징점이 0.3~4.7 %만 남았다.
3.5 m 루프에서 추정 위치가 7.4 m 벗어났다.

같은 주행 기록을 고정 컷과 적응형 컷으로 각각 처리한 궤적 비교 — **왼쪽 고정 컷 · 오른쪽 적응형 컷 + 스텝 게이트**같은 기록을 두 설정으로 다시 처리한 결과다. 추적 실패는 81회에서 0회로, 드리프트는 50 %에서 2.6 %로 줄었다. 상한을 6 m·8 m로 넓히면 추적 실패 없이 6673 % 드리프트가 통과하므로, 물리적으로 불가능한 이동량은 추적 실패로 처리한다.

오차식은 스테레오 매칭을 전제하며, 시뮬레이터 뎁스는 렌더링된 Z-버퍼다.

추적이 거부한 프레임을 키프레임으로 표시했다.
오도메트리 엣지를 보정된 노드와 raw VO 자세로 만들었다.
첫 루프 클로저 뒤 chi2가 104에서 1416까지 커졌다.
엣지는 두 키프레임의 추적 자세로만 계산한다.

궤적, 키프레임 클라우드, 점유 격자 세 장으로 본 SLAM 결과 — **3.52 m 사각 루프 · 키프레임 65개 · 루프 클로저 32개**주황 점선은 VO 단독, 빨강은 그래프 최적화 결과다. ATE는 7.00 cm에서 3.60 cm로, 출발점 복귀 오차는 6.56 cm에서 0.43 cm로 줄었다.

오른쪽 점유 격자에서는 자유 공간이 부챗살 모양으로만 채워진다.

장애물 높이 반사점만 쓰면 부챗살만 남는다.
바닥 반사점까지 쏘면 광선이 박스 위를 지난다.
임계값을 올리면 장애물 셀의 94~99 %가 지워진다.
가상 2D 스캔은 방위마다 도달 거리를 하나만 남긴다.

추적은 늘 같은 구간에서 실패한다.
그 구간에 선 지게차는 움직이지 않는다.
통로를 향하면 6.3 m 안쪽에 무늬가 없다.
깊이가 한 겹이면 회전과 이동을 구분하지 못한다.

시도	기대한 효과	측정 결과
odom 시그마를 실측값으로 조정	구속이 강하면 잘못된 클로저가 드러난다	0.20 m → 1.8 m
클로저 일관성 게이트	그래프와 어긋난 클로저를 제외한다	110개 중 105개 기각
Cauchy 로버스트 커널	이상치의 가중치만 낮춘다	2.8 m 벗어남
퇴화 기하 프레임 거부	추정이 불안정한 방향을 제외한다	ATE 6.7 → 30.4 cm
PnP 공분산으로 엣지 가중	엣지마다 측정 품질을 반영한다	ATE 6.7 → 12.4 cm

VO 오차는 잡음보다 편향에 가까워 한 걸음의 정밀도로 누적 결과를 예측할 수 없다. 드리프트가 미터 단위면 올바른 클로저의 잔차도 미터 단위여서 잔차 기준으로는 걸러지지 않는다. 다섯 가지는 모두 코드에서 제거했고, 프론트엔드 처리 시간은 26 % 줄었다.

같은 코드에서 리플레이 chi2는 32, 라이브는 11106이며 네 가지를 고치면 327이 된다.

같은 주행을 두 번 재현할 수 없어 rosbag에 기록한다.
프론트엔드가 사용하는 토픽만 녹화한다.
설정마다 같은 1934 프레임을 다시 처리한다.
루프 후보를 늘렸을 때 오차가 가장 크게 줄었다.

원인추적 실패를 이유별로 집계하면 90 %가 한 방향에서 발생한다. 증상이 아니라 원인을 기준으로 처리한다.
비교라이브 비교는 입력이 달라 측정이 되지 않는다. 같은 기록에 설정만 바꿔 비교한다.
남은 것위치 재추정 미구현, 완전 탐색 루프 탐색, 회전 중 yaw 드리프트.

실습 1

주행 스택을 띄우고 /aeirobot/vslam_left_camera_info의 K에서 fx·cx를 읽어 화각을 계산한다.

주행 스택을 기동한다 — SLAM Manager(8090)에서 전체 시작을 눌러도 같다aeirobot up driving --sim
카메라 토픽 세 개(image · depth · camera_info)를 찾는다ros2 topic list | grep vslam
K · D · width · height를 기록한다ros2 topic echo /aeirobot/vslam_left_camera_info --once
K에서 fx 586.7 · cx 640을 뽑아 수평 화각 2·atan(1280 / (2·fx)) ≈ 95°를 검산한다
실제 발행 주기를 잰다 — 30 Hz가 아니라 6 Hz 근처다ros2 topic hz /aeirobot/vslam_left_image

확인 cx·cy가 640·360이고 D가 전부 0이면 이미 rectify된 영상이다. frame_id는 zed_left_camera_optical_frame.

실습 2

base_link에서 zed_left_camera_optical_frame까지의 변환을 뽑아 축이 어떻게 돌아가는지 확인한다.

frames.pdf로 트리를 뽑는다 — map → odom → base_footprint → base_link 순이다ros2 run tf2_tools view_frames
변환을 직접 읽는다ros2 run tf2_ros tf2_echo base_link zed_left_camera_optical_frame
평행이동이 (−0.009, 0.060, 1.372) — base_link 기준 카메라 높이다
RPY이 (−110°, 0°, −90°)다. optical 기본값 (−90°, 0°, −90°)에서 20° 숙인 값이다
RViz TF에서 축 색(X 빨강 · Y 초록 · Z 파랑)으로 그 20°를 눈으로 확인한다

확인 쿼터니언은 교과서의 (−0.5, 0.5, −0.5, 0.5)가 아니다. RTAB-Map·Nav2의 기준 프레임은 base_footprint이고 base_link와 z로 0.127 m 어긋나 있다.

실습 3

같은 자리로 되돌아왔을 때 휠 오도메트리와 EKF 융합 결과가 각각 얼마나 벌어지는지 숫자로 잰다.

휠 오도메트리 시작값을 적는다 — isaac_wheel_odom · 20 Hzros2 topic echo /aeirobot/odometry --once
EKF 융합값도 같이 적는다 — ekf_filter_node · 28 Hzros2 topic echo /odometry/filtered --once
사각형 경로를 한 바퀴 돌아 같은 자리로 되돌아온 뒤 두 값을 다시 읽는다
시작값과의 차이 = 드리프트. 휠 단독과 EKF 융합 중 어느 쪽이 작은지 비교한다
나중에 재생할 수 있게 기록한다ros2 bag record /aeirobot/odometry /odometry/filtered /tf /tf_static

확인 둘 다 odom 기준이라 드리프트는 누적되기만 한다. 이걸 지우는 건 map→odom을 보정하는 SLAM·AMCL뿐이다 — 실습 4·5.

실습 4

Gazebo TurtleBot3를 slam_toolbox로 돌려 맵을 만들고, 파일로 저장해 실습 5의 입력으로 쓴다.

시뮬레이터를 SLAM 모드로 띄운다ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True headless:=False
라이다가 들어오는지 먼저 본다 — /scan이 없으면 SLAM은 한 발도 못 나간다ros2 topic hz /scan
RViz에 /map을 올리고 키보드로 천천히 몰며 맵을 채운다ros2 run teleop_twist_keyboard teleop_twist_keyboard
출발한 곳으로 되돌아와 루프 클로저로 벽이 한 줄로 정렬되는지 본다
맵을 파일로 저장한다ros2 run nav2_map_server map_saver_cli -f my_map

확인 벽이 두 겹으로 번져 있으면 루프가 닫히지 않은 것이다. my_map.yaml의 resolution 0.05와 origin을 적어 실습 5에서 그대로 쓴다.

실습 5

저장한 맵으로 목표점까지 주행한 뒤, 위치를 일부러 틀어 무엇이 무너지는지 본다.

저장한 맵으로 Nav2를 띄운다ros2 launch nav2_bringup tb3_simulation_launch.py map:=my_map.yaml headless:=False
RViz에서 2D Pose Estimate로 초기 위치를 잡고 Nav2 Goal로 목표를 찍는다
주행 중 2D Pose Estimate로 엉뚱한 곳을 찍어 AMCL을 일부러 틀어놓는다
Costmap과 /plan이 어떻게 튀는지, 복구 행동이 뜨는지 관찰한다
입자가 다시 모이며 공분산이 줄어드는지 본다ros2 topic echo /amcl_pose --field pose.covariance

확인 위치가 틀리면 Planner를 손봐도 소용없다 — 슬라이드 47의 진단 순서. /scan → TF → /amcl_pose → /plan 순으로 짚는다.

실습 6

/aeirobot/vslam_depth의 거리값을 TF에서 잰 기선 B로 되짚어 disparity를 검산한다.

이 로봇에는 disparity 토픽이 없다 — 뎁스로 나온다ros2 topic list -t | grep vslam
기선 B를 TF에서 직접 잰다 — y = −0.120 mros2 run tf2_ros tf2_echo zed_left_camera_frame zed_right_camera_frame
뎁스는 32FC1 · 단위는 m다. 1 m와 3 m 물체의 Z를 화면에서 읽는다ros2 run rqt_image_view rqt_image_view
d = fx·B / Z로 disparity를 되짚는다 — 1 m면 70.4 px, 3 m면 23.5 px
무늬 없는 벽 · 반복 무늬 · 유리 앞에서 뎁스가 비는 것을 확인한다

확인 거리가 2배면 오차는 4배 — 슬라이드 11의 오차식과 맞는지 본다. 멀수록 disparity 1 px 차이가 거리로 크게 벌어진다.