2.13내비게이션 실습

2.11장에서는 지도를 만들었고 2.12장에서는 Nav2의 설정을 읽었습니다. 이 장에서는 반대로 이미 만들어진 지도가 있다고 가정하고, 로봇이 그 지도를 보며 목표까지 가는 과정을 직접 짭니다. Nav2가 하는 일의 뼈대(지도 읽기, 코스트맵, 전역 경로 계획, 경로 따라가기, 장애물 재계획)를 한 파일에 넣은 최소 구현입니다.

위치 추정은 하지 않습니다. 에뮬레이터의 /odom은 시작 자세가 원점이므로 지도 좌표와 같다고 보고 그대로 씁니다. 실제 Nav2에서 이 자리를 AMCL이 맡는다는 점은 아래 표에서 다시 짚습니다.

실습 공간: office 월드

office 월드는 8 m x 6 m 공간이 벽으로 나뉜 방 두 개입니다. 로봇은 왼쪽 방의 (0, 0)에서 출발하고, 오른쪽 방으로 가려면 벽의 문(폭 1 m, y 1.2~2.2)을 지나야 합니다. 방마다 원통이 하나씩 있습니다. 목표 기본값은 오른쪽 방의 (6.0, 2.0)입니다.

코드 맨 위의 WALLS와 CYLINDERS가 로봇이 아는 지도입니다. 실제로는 map_server가 .pgm과 .yaml 파일에서 지도를 읽지만, 여기서는 파일 대신 도면 목록으로 격자를 그립니다. 같은 격자를 /map으로 발행하므로 결과 화면의 지도 탭에서 볼 수 있습니다.

지도에서 코스트맵으로

점유 격자는 "장애물이 있는 칸"만 알려 줍니다. 로봇은 크기가 있어서 장애물 칸을 피하는 것만으로는 부족하고 벽에 스치지 않는 간격이 필요합니다. 그래서 칸마다 가장 가까운 장애물까지의 거리 d를 구해 비용을 매깁니다.

inflation은 파라미터입니다. 값을 키우면 위험하다고 보는 폭이 넓어지고, 줄이면 장애물 바로 옆까지 비용이 0이 됩니다. Nav2의 inflation_layer도 같은 구조인데, 비용이 거리에 따라 지수로 줄어든다는 점이 다릅니다(2.12장의 표). 결과 화면의 코스트맵 체크박스를 켜면 이 비용이 주황색으로 보입니다. 붉은 칸은 지나갈 수 없는 칸입니다.

예제의 ROBOT_R = 0.15는 에뮬레이터 로봇의 충돌 반지름(0.12 m)에 여유를 둔 값입니다. 실제 TIKI-MINI는 URDF 형상을 감싸는 가장 작은 원이 약 0.20 m이므로(2.5장) 같은 방법으로 쓰려면 더 크게 잡아야 합니다.

전역 경로 계획: A*

A*는 출발 칸에서 시작해 이웃 칸으로 퍼져 나가며 목표 칸까지의 최소 비용 경로를 찾습니다. 이 장의 plan()은 다음 규칙을 씁니다.

• 이웃은 위, 아래, 왼쪽, 오른쪽과 대각선 4칸까지 8방향입니다. 대각선 이동은 길이가 sqrt(2) 배입니다.
• 칸 하나를 지나는 비용은 이동 거리 x (1 + 8 x step_cost)입니다. 장애물에 가까운 칸일수록 돌아가는 편이 싸집니다.
• 휴리스틱은 목표까지의 직선거리입니다. 실제 비용보다 크지 않아서 최소 비용 경로가 보장됩니다.
• 찾은 칸 경로는 shortcut()으로 꺾이는 점만 남깁니다. 단, 원래 경로보다 위험한 칸을 지나게 되는 직선은 쓰지 않습니다. 이 조건이 없으면 문 모서리를 깎아 지나가서 inflation을 키워도 경로가 달라지지 않습니다.
• 남은 점 사이를 0.1 m 간격으로 채워 /plan(nav_msgs/Path)으로 발행합니다.

경로 따라가기: 순수 추종

경로를 받은 로봇은 지금 위치에서 0.4 m 앞의 경로 점을 목표로 삼고 그쪽을 향해 갑니다. 이것을 순수 추종이라고 부릅니다. 목표 방향과 로봇 방향의 차이가 0.5 rad보다 크면 제자리에서 돌고, 그보다 작으면 0.2 m/s로 전진하면서 차이에 비례해 조향합니다. 경로가 꺾이는 곳에서 목표 점이 갑자기 바뀌지 않도록 idx로 로봇이 경로의 어디까지 왔는지 기억합니다.

실행해 보기

지도에 없는 장애물

지도는 만든 순간의 모습이어서 시간이 지나면 현실과 달라집니다. 사람이 상자를 놓아 두었다면 지도에는 없는 장애물이 경로를 막습니다. 이때는 /scan이 필요합니다.

on_scan()은 라이다가 본 점을 칸으로 바꿔서 원래 지도(self.static)의 점유 칸 근처(±2칸)에 없으면 새 장애물로 보고 그 칸을 점유로 표시합니다. 표시하는 순간 주변 거리 값이 갱신되어 코스트맵이 달라지고, need_replan이 켜져서 0.5초 안에 A*를 다시 돌립니다. 로봇은 새 경로가 나올 때까지 이전 경로를 따라갑니다.

office_blocked 월드는 office와 같은 공간에 오른쪽 방 문 앞(4.3, 1.9)에 지도에 없는 원통이 하나 더 놓여 있습니다. 같은 코드를 돌려 봅니다.

이 코드와 Nav2

slam_toolbox는 이 표에 나오지 않습니다. slam_toolbox는 지도를 만들고 map → odom 위치를 내놓을 뿐 경로는 계획하지 않으며, 경로 계획과 추종은 모두 Nav2의 서버들이 합니다. tiki_navigation의 bringup.launch.py가 slam:=True일 때 SLAM과 Nav2를 함께 띄우는 것입니다(2.12장). 각 구성 요소의 설정 항목은 2.12장에서 읽었습니다. 이 장의 코드는 "각 칸이 무엇을 하는지"를 확인하는 용도이고, 실제 로봇에서는 Nav2를 씁니다. 4.7장에서 tiki_navigation을 실행합니다.

DWB: 속도 후보를 점수로 고르기

이 장의 follow()는 경로의 0.4 m 앞 점 하나를 향해 조향하는 규칙 하나입니다. 실제 tiki_navigation은 2.12장에서 읽은 DWBLocalPlanner를 씁니다. DWB는 "어느 점으로 갈까"가 아니라 "지금 어떤 속도로 달리는 것이 가장 좋을까"를 고르는 방식이고, 컨트롤러가 0.1초마다 아래 순서를 반복합니다.

1. 속도 후보를 만듭니다. vx_samples 20개와 vtheta_samples 20개를 곱해 400개 안팎입니다. 후보는 현재 속도에서 한 주기 동안 가속 한계(acc_lim_x 2.5, acc_lim_theta 3.2)로 도달할 수 있는 범위 안에서만 뽑습니다.
2. 후보마다 그 속도를 sim_time 1.7초 동안 유지한다고 보고 앞으로의 궤적을 계산합니다.
3. 궤적마다 critic 점수를 매깁니다. tiki 설정에서는 PathDist와 PathAlign이 전역 경로에서 얼마나 벗어나는지, GoalDist와 GoalAlign이 목표까지 얼마나 남았는지를 보고 BaseObstacle이 코스트맵 값을 봅니다. 로봇이 닿는 궤적은 이 단계에서 탈락합니다.
4. critic 점수에 가중치(scale)를 곱해 합친 값이 가장 낮은 후보의 속도를 /cmd_vel로 보냅니다.

아래 모형은 이 구조를 단순하게 옮긴 것입니다. 실제 DWB의 critic 식은 더 복잡해서(예를 들어 코스트맵 값을 궤적을 따라 모읍니다) 숫자가 같지는 않고, RotateToGoal, Oscillation, GoalAlign은 넣지 않았습니다. 후보를 만들고, 궤적을 그리고, 가중합으로 고른다는 흐름을 확인하는 용도입니다. 속도 상한과 가속 한계, 1.7초는 tiki 설정 값입니다.

직접 바꿔 보기

• 장애물을 멀리 치우면 400개 후보 중 충돌이 0개이고, 직진 속도는 상한 0.26 m/s, 회전은 0.017 rad/s가 선택됩니다. 경로를 따라 최대 속도로 달립니다.
• 장애물을 (0.4, 0.0), 즉 경로 위 로봇 앞에 두면 충돌 후보가 232개로 늘고 선택은 직진 0.096 m/s, 회전 0.017 rad/s가 됩니다. 장애물을 돌아가지 않고 속도를 줄입니다. 1.7초 안에 닿지 않는 후보 중에서 경로에 가장 가까운 것을 고르기 때문입니다. 이렇게 큰 우회는 지역 계획기가 아니라 앞 절의 전역 경로 재계획이 맡습니다.
• BaseObstacle 가중치를 0.02에서 1로 올리면 같은 장면에서 직진 속도가 0이 되어 멈춥니다. 코스트가 붙는 구간을 한 번이라도 지나는 후보가 너무 비싸지기 때문입니다.
• GoalDist 가중치를 0으로 두면(경로 점수만 남기면) 장애물이 앞에 있을 때 직진 0, 회전 -0.017이 선택되고 총점이 3.59로 가장 낮습니다. 경로에서 벗어나지 않으려면 가만히 서 있는 것이 가장 좋은 점수라서, 목표 쪽으로 전진하게 만드는 것은 GoalDist입니다.
• 가속 한계 안의 후보만 쓰기를 끄면 후보가 속도 범위 전체로 넓어져 같은 장면의 충돌 후보가 232개에서 172개로 줄고 선택은 직진 0.096, 회전 0.053이 됩니다. 현재 속도에서 갑자기 바꿀 수 없는 후보까지 섞여서 실제로는 낼 수 없는 속도를 고를 수 있습니다.
• 현재 속도를 0.26 m/s로 올리고 장애물을 (0.25, 0.0)에 두면 400개 중 380개가 충돌하고 고른 직진 속도는 0.01 m/s입니다. 빠르게 달리는 중에는 한 주기에 줄일 수 있는 속도가 작아서 고를 수 있는 후보가 거의 사라집니다.

모든 후보가 충돌하면 DWB는 쓸 수 있는 속도를 찾지 못하고 실패를 알립니다. Nav2에서는 이 경우 행동 트리가 복구 동작(제자리 회전, 후진 등)으로 넘어갑니다.

DWB로 직접 주행하기

같은 코드에서 follow()만 DWB 방식으로 바꾼 노드입니다. 지도, 코스트맵, A* 계획, 지도에 없는 장애물 처리는 그대로이고, 순수 추종 대신 매 0.1초마다 rollout()으로 후보 150개(직진 10개 x 회전 15개)를 1.7초 앞까지 계산해서 score()의 가중합이 가장 낮은 속도를 고릅니다. 직진 0.2 m/s와 회전 0.85 rad/s 한계는 diff_controller의 값이고, 가중치 32, 24, 0.02는 nav2_params.yaml의 값입니다. 지역 계획기는 전역 경로 가운데 앞쪽 1.5 m만 봅니다(지역 코스트맵 크기에 맞춘 것). 목표까지의 거리도 그 구간의 끝 점을 기준으로 잽니다. 세 critic의 가중치(path_scale, goal_scale, obstacle_scale)는 파라미터로 열어 두었으니 입력 칸에서 바꿔 가며 결과를 비교하세요.

순수 추종과 DWB 겹쳐 보기

위의 실행 2(순수 추종)와 실행 3(DWB)을 둘 다 실행하면 아래에 두 결과가 한 화면에 겹쳐집니다. 같은 지도와 같은 A* 경로를 쓰므로 차이는 모두 follow()에서 나옵니다.

읽는 법

• 직진 속도 그래프에서 순수 추종은 출발 직후부터 0.2 m/s에 붙어 있고(평균 0.200 m/s) DWB는 0.2에서 자주 내려옵니다(평균 0.187 m/s). 순수 추종은 속도를 정하지 않고 늘 최대로 달리고, DWB는 후보 궤적의 점수에 따라 속도도 고르기 때문입니다.
• 회전 속도 그래프에서 두 선이 같은 곳에서 크게 흔들리면 지도에 없는 원통 때문에 경로가 바뀐 구간입니다. 이 장면에서 회전 속도 변화량의 합은 순수 추종이 9.4 rad/s, DWB가 6.2 rad/s입니다. DWB 쪽이 덜 흔들렸습니다.
• 벽과의 거리 그래프에서는 두 방식이 비슷하게 0.3 m 안팎을 유지합니다(최솟값 0.31 m와 0.32 m). 이 장면은 A* 코스트맵이 이미 벽에서 떨어진 경로를 주기 때문에, 추종 방식이 달라도 거리는 크게 달라지지 않습니다.
• 표의 도착 시각은 33.4초와 36.0초입니다. DWB가 2.6초 늦는 대신 덜 흔들렸습니다.

A*와 DWB는 무엇이 다른가

실행 2와 실행 3은 초록 경로가 똑같습니다. 둘 다 같은 A가 계획했기 때문입니다. 바뀐 것은 A가 아니라 그 경로를 따라가는 방식(순수 추종에서 DWB로)입니다. 둘은 서로 대신할 수 있는 같은 종류가 아니라 앞뒤로 이어지는 다른 일을 합니다.

실습의 횟수를 보면 차이가 분명합니다. office에서 A는 경로를 1번만 계획했고(점 66개, 6.6 m), DWB는 34.8초에 도착하기까지 0.1초마다 약 350번 속도를 골랐습니다. 지도에 없는 원통이 있는 office_blocked에서는 A가 새 장애물이 보일 때마다 14번 다시 계획했습니다. A*의 결과는 "어디로"이고 DWB의 결과는 "지금 어떤 속도로"입니다.

출발할 때 지도 전체를 보고 길을 정하는 일에는 A의 최소 비용 보장이 잘 맞고, 달리는 동안 장애물에 반응하고 로봇이 낼 수 있는 속도 안에서 움직이는 일에는 DWB가 맞습니다. 앞의 멈춤 실험도 이 분업에서 나옵니다. A가 로봇이 따라가기 어려운 급한 꺾임을 내놓았고, DWB가 그것을 따라갈 수 없을 때 점수의 균형에 따라 서 버린 것입니다.

한눈에 정리

1. 코스트맵은 점유 격자에서 칸마다 가장 가까운 장애물까지의 거리를 구해, 로봇 반지름 이하는 지나갈 수 없는 칸, 반지름에서 inflation까지는 위험한 칸으로 만든다.
2. A*는 이동 거리에 칸 비용을 얹은 합이 가장 작은 경로를 찾는다. inflation이 클수록 벽에서 멀리 돌고 경로는 길어진다. 에뮬레이터에서 0.2일 때 가장 가까웠던 거리가 0.34 m였고 0.0일 때는 0.16 m였다.
3. 꺾이는 점만 남기는 단축은 원래 경로보다 위험한 칸을 지나지 않는 조건이 있어야 inflation이 경로에 반영된다.
4. 순수 추종은 경로의 0.4 m 앞 점을 향해 조향한다. 경로에서 지나온 위치를 기억하지 않으면 급하게 꺾이는 곳에서 목표 점이 바뀌며 제자리에서 맴돌 수 있다.
5. 목표가 지나갈 수 없는 칸이면 계획을 포기하고 멈춘다.
6. 지도에 없는 장애물은 /scan으로 발견해 코스트맵에 더하고 경로를 다시 짠다. office_blocked에서 경로를 14번 계획했고 충돌 없이 33.4초에 도착했다.
7. 이 코드는 Nav2의 map_server, AMCL(여기서는 odom 신뢰), 코스트맵, 전역 플래너, 컨트롤러의 역할을 최소한으로 합친 것이다.
8. DWB는 속도 후보 400개 안팎을 1.7초 앞까지 시뮬레이션해 critic 점수의 가중합이 가장 낮은 속도를 고른다. 장애물이 앞에 있으면 돌아가기보다 속도를 줄이고, 큰 우회는 전역 경로 재계획이 맡는다.
9. 경로가 급하게 꺾이는 곳에서 DWB가 멈추는 것은 충돌이 아니라 경로 critic과 목표 critic의 가중치 균형 때문이다. 에뮬레이터에서 goal_scale을 24에서 48로 올리면 멈추던 세 목표가 모두 도착했다.

확인 문제

다음 장

2.14장에서는 지금까지 배운 센서 읽기, 지도, 주행을 합쳐 course 월드의 미니 프로젝트를 풉니다.