CH08경로 계획과 경로 추종
코스트맵 위에서 A* 로 전역 경로를 찾고, 지역 제어기로 그 경로를 따라가며 장애물을 피하는 과정을 직접 튜닝합니다.
학습 목표#
- 전역 · 지역 코스트맵이 정적 · 장애물 · 팽창 레이어를 겹쳐 만들어진다는 것을 설명하고, myAGV Pro 의 footprint(0.52 × 0.36 m)에서 내접 반경 0.18 m 와
inflation_radius·cost_scaling_factor가 셀 비용(0 ~ 254)을 어떻게 정하는지 시뮬레이터로 확인할 수 있습니다. - 격자 지도에서 Dijkstra 와 A* 가 탐색하는 셀의 수와 경로 길이를 비교하고, 휴리스틱 종류(맨해튼 · 유클리드 · 옥타일)와 가중치 w 가 "빠름 vs 최단" 을 어떻게 맞바꾸는지 numpy + heapq 로 구현해 설명할 수 있습니다.
- 경로 추종에서 전방 주시 거리(lookahead)가 짧으면 진동하고 길면 코너를 깎는 이유를 설명하고, 전방향 로봇이 진행 방향과 머리 방향을 따로 제어해 옆으로 붙는 도킹을 할 수 있음을 (vx, vy, ω) P 제어기로 확인할 수 있습니다.
- DWB 지역 계획기가 속도를 샘플링해 평가 함수(경로 근접 · 목표 · 장애물)로 고르는 과정을 설명하고, 가중치를 바꿔 "빠르지만 위험한" 설정과 "느리지만 안전한" 설정을 수치(도착 시간 · 최소 여유 거리)로 비교할 수 있습니다.
- Nav2 의 Planner · Controller · Behavior 서버가 나눠 맡는 일을 말하고, "경로 없음 · 진동 · 목표 근처 맴돌기" 증상을 원인 파라미터와 연결할 수 있습니다.
이론#
ch06 에서 지도를 만들었고, ch07 에서 그 지도 위의 내 위치를 알게 되었습니다. 이제 남은 질문은 "어떻게 가는가" 입니다. 물류 로봇은 이 질문을 두 단계로 나눠 풉니다.
- 전역 경로 계획(global planning) — 지도 전체를 보고 출발점에서 목표까지의 길을 한 번에 찾습니다. 느려도 되지만 막다른 길에 빠지지 않아야 합니다.
- 지역 계획 · 경로 추종(local planning, control) — 그 길을 따라가며 1 초에 수십 번 속도 명령
(vx, vy, ω)을 냅니다. 지도에 없던 사람 · 지게차를 피하는 것도 이 단계의 몫입니다.
두 단계 모두 같은 바탕 위에서 일합니다. 지도의 각 칸에 "여기를 지나가면 얼마나 위험한가" 를 숫자로 적어 둔 코스트맵(costmap) 입니다. 아래는 myAGV Pro 의 ROS 2 패키지(agv_pro_ros2, humble 브랜치)에 들어 있는 Nav2 설정 agv_pro_navigation2/param/agvpro.yaml 에서 이 차시와 관련된 값입니다.
| 항목 | agvpro.yaml 의 값 | 이 차시에서 |
|---|---|---|
| footprint | [[0.26, 0.18], [0.26, -0.18], [-0.26, -0.18], [-0.26, 0.18]] (0.52 × 0.36 m) | 1교시 |
| 코스트맵 해상도 | resolution: 0.05 (셀 하나 5 cm) | 1 · 2교시 |
| global_costmap 레이어 | static_layer · obstacle_layer · inflation_layer | 1교시 |
| local_costmap | voxel_layer · inflation_layer, 3 × 3 m, rolling_window: true | 1 · 4교시 |
| 팽창 | inflation_radius: 0.30 · cost_scaling_factor: 4.0 | 1교시 |
| 전역 계획기 | nav2_navfn_planner/NavfnPlanner, use_astar: false, tolerance: 2.0, allow_unknown: true | 2교시 |
| 지역 계획기 | dwb_core::DWBLocalPlanner, controller_frequency: 20.0 | 3 · 4교시 |
| 속도 한계 | max_vel_x: 0.26 · max_vel_y: 0.0 · max_vel_theta: 0.5 · acc_lim_x: 2.5 | 3 · 4교시 |
| DWB 샘플 | vx_samples: 20 · vy_samples: 5 · vtheta_samples: 40 · sim_time: 1.7 | 4교시 |
| DWB 평가 함수 | PathDist 32 · GoalDist 24 · PathAlign 23 · GoalAlign 18 · RotateToGoal 32 · BaseObstacle 0.02 | 4교시 |
| 목표 판정 | xy_goal_tolerance: 0.25 · yaw_goal_tolerance: 0.25 | 4교시 |
| 복구 동작 | spin · backup · wait (recoveries_server) | 4교시 |
- 위 값은 공개 저장소의 파일을 읽은 것입니다. 로봇에 실제로 설치된 버전 · 브랜치에 따라 다를 수 있으니, 실물에서는
ros2 param get으로 확인합니다 (ch09). - 눈여겨볼 점: myAGV Pro 는 전방향(메카넘) 로봇인데, 이 파일의 DWB 는
max_vel_y: 0.0·acc_lim_y: 0.0이라 옆으로 가는 속도를 내지 않습니다. 최고 속도 0.26 m/s 와 가속도 2.5 m/s² 도 TurtleBot3 예제의 값과 같습니다(파일 주석에도 turtlebot 3 이야기가 있음). 그대로 쓰기보다 이 로봇에 맞게 튜닝할 출발점 으로 보는 것이 맞습니다. 이 차시의 시뮬레이터에서 직접 바꿔 봅니다. - 시뮬레이터 1 ~ 4 는 Nav2 의 계산 방식(팽창 비용 식, Navfn 의 셀 비용, DWB 의 샘플링 · 평가)을 따라 만든 교육용 모델 입니다. 세부 구현(경계 처리, 평가 함수의 정규화 등)은 단순화했습니다.
- 차시의 실제 Nav2 기동 · RViz 조작 · 파라미터 적용은 다음 차시(ch09)에서 다룹니다. 이 차시는 원리와 튜닝 감각에 집중합니다.
1교시 · 코스트맵과 장애물 팽창#
지도에서 코스트맵으로#
ch06 의 점유격자 지도는 셀마다 "비었다 · 찼다 · 모른다" 세 가지만 알려 줍니다. 경로를 계획하려면 한 가지가 더 필요합니다. "벽에서 얼마나 가까운가" 입니다. 벽에 딱 붙은 빈 셀은 지도로는 "비었다" 이지만, 로봇 중심이 거기에 오면 차체가 벽에 부딪힙니다.
Nav2 의 코스트맵은 여러 레이어(layer) 를 차례로 겹쳐 셀마다 0 ~ 254 의 비용을 만듭니다.
| 비용 값 | 이름 | 뜻 |
|---|---|---|
| 254 | LETHAL (치명) | 장애물 그 자체인 셀 |
| 253 | INSCRIBED (내접) | 로봇 중심 이 여기 오면 차체가 반드시 장애물에 닿는 셀 |
| 1 ~ 252 | 팽창 비용 | 닿지는 않지만 가까울수록 큰 값. 계획기는 이 값을 "멀리 돌아가는 비용" 과 저울질 |
| 0 | FREE | 장애물에서 inflation_radius 보다 먼 셀 |
| 255 | NO_INFORMATION | 모르는 셀 (track_unknown_space: true 일 때) |
두 종류의 코스트맵이 있습니다.
| global_costmap | local_costmap | |
|---|---|---|
| 좌표계 | map (지도 전체) | odom (로봇 둘레 3 × 3 m 가 로봇을 따라 움직임) |
| 레이어 (agvpro.yaml) | static + obstacle + inflation | voxel + inflation (정적 지도는 쓰지 않음) |
| 갱신 | 1 Hz | 5 Hz |
| 쓰는 곳 | 전역 계획기 (2교시) | 지역 계획기 · 컨트롤러 (3 · 4교시) |
ch07 에서 보았듯 AMCL 이 map → odom 을 고칠 때 map 좌표의 로봇 위치는 가끔 "툭" 뜁니다. 1 초에 20 번 속도를 내는 컨트롤러가 그 점프를 보면 핸들을 꺾습니다. 그래서 가까운 장애물 회피는 매끄러운 odom 좌표의 지역 코스트맵에서, 먼 길 찾기는 지도와 맞는 map 좌표의 전역 코스트맵에서 합니다 (ch04 의 TF 구조).
로봇을 점으로 만드는 방법 — 내접 반경과 팽창#
경로 계획기는 로봇을 점 으로 보고 길을 찾습니다. 크기가 있는 로봇을 점으로 바꾸려면, 반대로 장애물을 로봇 크기만큼 부풀리면 됩니다. 이것이 팽창(inflation)입니다.
agvpro.yaml 의 footprint 는 중심에서 앞뒤 0.26 m, 좌우 0.18 m 인 사각형(0.52 × 0.36 m, 실제 차체 530 × 360 mm, ch01)입니다. 여기서 두 반경이 나옵니다.
- 내접 반경(inscribed radius) = 중심에서 가장 가까운 변까지 = 0.18 m. 장애물이 이보다 가까우면 어느 방향을 보든 반드시 부딪힙니다 → 비용 253.
- 외접 반경(circumscribed radius) = 중심에서 모서리까지 = √(0.26² + 0.18²) ≈ 0.316 m. 장애물이 이보다 멀면 어느 방향을 보든 절대 안 부딪힙니다. 그 사이는 "방향에 따라 다름" 입니다.
내접 반경 바깥은 아래 식으로 비용이 줄어듭니다 (Nav2 inflation_layer).
비용(d) = 252 · e^(−k · (d − 0.18)), d ≤ inflation_radius 일 때. 그 밖은 0. (k =
cost_scaling_factor)
agvpro.yaml 의 값(k = 4.0, inflation_radius = 0.30 m)으로 계산하면 다음과 같습니다. 셀이 5 cm 이므로 거리도 5 cm 단위로 끊깁니다.
| 장애물까지 거리 d | 0 | 0.05 ~ 0.15 | 0.20 | 0.25 | 0.30 | 0.35 이상 |
|---|---|---|---|---|---|---|
| 비용 | 254 | 253 | 232 | 190 | 155 | 0 |
inflation_radius 는 비용이 붙는 범위 를, cost_scaling_factor 는 비용이 줄어드는 빠르기 를 정합니다. k 가 크면 벽 바로 옆에서만 비싸고 금방 0 에 가까워지며, k 가 작으면 멀리까지 비쌉니다.
Nav2 는 footprint(다각형) 대신 robot_radius(원)로도 로봇 크기를 적을 수 있습니다. 원으로 적으면 내접 반경 = 그 반지름입니다. 0.53 × 0.36 m 로봇을 "다 덮는 원" 으로 적으면 반지름이 약 0.32 m 가 되어, 옆 폭은 0.36 m 인데도 폭 0.64 m 의 로봇처럼 취급됩니다. 길쭉한 로봇일수록 이 차이가 큽니다.
예측하기: 폭 0.6 m 통로에 들어갈 수 있을까?#
선반 사이에 폭 0.6 m 의 통로가 있습니다. myAGV Pro 의 폭은 0.36 m 이니 양옆에 0.12 m 씩 남습니다. 시뮬레이터를 열기 전에 공책에 적어 보세요.
- agvpro.yaml 그대로(사각 footprint, inflation_radius 0.30, k 4.0)라면 계획기가 이 통로를 쓸까요? 통로 한가운데 셀의 비용은 얼마일까요? (위 표를 보면 됩니다.)
- footprint 대신
robot_radius: 0.32를 쓰면? - inflation_radius 를 0.30 → 0.50 으로 늘리면, 통로가 "막히는" 걸까요, "비싸지는" 걸까요?
시뮬레이터 1 · 코스트맵 팽창과 좁은 통로#
왼쪽은 6 × 4.4 m 창고 바닥(셀 5 cm)입니다. 가운데 벽에 좁은 통로(폭 조절) 와 아래 넓은 통로(1.0 m)가 있고, 왼쪽 방에 기둥이 하나 있습니다. 로봇은 왼쪽(초록)에서 오른쪽(주황)으로 갑니다. 계획기는 Navfn 처럼 셀 비용을 거리에 얹어(이동 비용 = 거리 × (1 + 0.016 × 셀 비용)) 가장 싼 길을 찾습니다. 오른쪽 그래프는 장애물까지 거리 → 비용 곡선입니다.
해 볼 것
- 예측 확인 — 처음 값(폭 0.6 m, agvpro.yaml)에서 경로가 좁은 통로로 가는지, 출력의 "좁은 통로 안 가장 싼 셀 비용" 이 얼마인지 읽고 공책의 예측과 비교합니다. 오른쪽 그래프에서 초록 점선(통로 중앙)이 곡선의 어디에 걸리는지 봅니다.
- 로봇 크기를 robot_radius 0.32 (원) 로 바꿉니다. 경로는 어떻게 되나요? 좁은 통로 폭을 몇 m 까지 넓혀야 다시 좁은 통로를 쓰나요?
- 다시 사각 footprint 로 두고 inflation_radius 를 0.30 → 0.35 → 0.50 으로 올립니다. 경로가 넓은 통로로 바뀌는 순간, 좁은 통로 안의 셀 비용은 253 인가요, 그보다 작은가요? 즉 통로가 "막힌" 것인가요, "비싸진" 것인가요?
- inflation_radius 를 0.30 에 두고 k 를 4 → 8 → 1 로 바꿔 봅니다. 기둥 옆 여유와 경로 모양이 어떻게 변하나요? k = 8 일 때 inflation_radius 0.50 이면 어떤가요?
- inflation_radius 를 0.10 으로 내립니다. 빨간 경고와 "경로 전체 최소 여유" 를 확인합니다. 이어서 오른쪽 방에 상자 놓기 를 켜고, 보기를 "정적 레이어만" · "팽창 비용만" 으로 바꿔 레이어가 어떻게 겹치는지 봅니다.
관찰 포인트 — 폭 0.6 m 통로의 한가운데는 양쪽 벽에서 6 셀(0.30 m) 떨어져 있습니다. 사각 footprint 의 내접 반경 0.18 m 보다 머니 253 이 아니고, inflation_radius 가 0.30 이면 비용 155 의 "비싼 길" 입니다. 넓은 통로로 돌아가는 길이 훨씬 길어서 계획기는 비싼 좁은 통로를 고릅니다. inflation_radius 를 조금만 늘려도(0.35) 통로 앞뒤로 비용이 붙는 구간이 길어져, 셀 비용은 253 미만인데도 계획기가 스스로 피하는 순간이 옵니다. 반대로 원형 robot_radius 0.32 는 내접 반경 자체가 0.32 라서 통로 중앙(0.30)도 253 이 되어 정말로 막힙니다. inflation_radius 를 내접 반경보다 작게 하면 계획기가 벽에 붙은 셀까지 "자유" 로 보게 되어, 경로는 짧아지지만 실제 차체가 벽과 겹칩니다.
시뮬레이터 1 을 agvpro.yaml 값(사각 footprint, inflation_radius 0.30, k 4.0)으로 두고 좁은 통로 폭을 0.6 m 로 했습니다. 좁은 통로 한가운데 셀의 비용과 그 의미로 옳은 것은?
- 253 — 로봇 중심이 여기 오면 반드시 부딪히므로 계획기는 통로를 쓰지 못한다
- 155 — 부딪히지는 않지만 비싼 셀이다. 돌아가는 길이 충분히 길면 계획기는 이 통로를 쓴다
- 0 — 통로 중앙은 벽에서 inflation_radius 보다 멀다
- 254 — 통로 폭이 차체 폭의 두 배가 안 되므로 장애물로 표시된다
정답 보기
② — 중앙은 벽에서 6 셀 = 0.30 m 입니다. 내접 반경 0.18 m 보다 멀므로 253 이 아니고(①), inflation_radius 0.30 이하이므로 0 도 아닙니다(③). 252 · e^(−4 × 0.12) = 155.9 → 155 입니다. 254 는 장애물 셀 자체에만 붙습니다(④). Navfn 은 이 비용을 거리에 얹어 비교하므로 넓은 통로로 돌아가는 1.5 m 남짓의 추가 거리보다 싸면 좁은 통로를 고릅니다.같은 0.6 m 통로에서 inflation_radius 를 0.35 로 올렸더니 경로가 넓은 통로로 바뀌었습니다. 출력의 "좁은 통로 안 가장 싼 셀 비용" 은 155 그대로입니다. 이 상황을 가장 잘 설명한 것은?
- 통로가 막혔다. 0.35 > 0.30 이므로 통로 중앙이 253 이 되었다
- 통로는 열려 있다. 통로 앞뒤로 비용이 붙는 구간이 늘어 좁은 통로 길 전체의 비용이 우회 비용보다 커졌다
- 계획기 오류다. 비용이 같으면 경로도 같아야 한다
- inflation_radius 가 외접 반경(0.316)을 넘었으므로 Nav2 가 통로를 자동으로 닫는다
정답 보기
② — 중앙 셀은 그대로 155 이므로 막힌 것이 아닙니다(①). 계획기는 셀 하나가 아니라 길 전체의 비용 합을 비교합니다. inflation_radius 가 늘면 벽 끝(통로 입구 · 출구) 둘레의 0 이던 셀에도 비용이 붙어 좁은 길의 합이 커집니다(③ 오답). ④ 같은 규칙은 없습니다. "막힘" 은 통로 안 모든 셀이 253 이상일 때뿐이며, 그 경우 출력에 "막힘" 이 표시됩니다(원형 robot_radius 0.32 로 확인).2교시 · 전역 경로 계획: Dijkstra 와 A*#
격자 지도는 그래프입니다#
코스트맵의 셀 하나를 노드(node), 이웃 셀로 가는 한 걸음을 간선(edge) 으로 보면 경로 계획은 그래프의 최단 경로 문제가 됩니다.
- 4방향 이동: 상하좌우만. 한 걸음 길이 1 셀.
- 8방향 이동: 대각선 포함. 대각선 한 걸음은 √2 ≈ 1.414 셀. 벽 모서리를 대각선으로 "긁고" 지나가지 않도록, 옆 두 칸 중 하나라도 벽이면 대각선을 막습니다.
- 간선 비용: 거리 × (1 + 셀 비용에 비례하는 값). 1교시의 팽창 비용이 여기서 쓰입니다.
Dijkstra — 가까운 곳부터 고르게#
Dijkstra 알고리즘은 "출발점에서 지금까지 온 비용 g" 가 가장 작은 셀부터 하나씩 꺼내(우선순위 큐, heapq) 이웃의 g 를 갱신합니다. 물결이 퍼지듯 모든 방향으로 고르게 넓혀 가다가 목표에 닿으면 멈춥니다. 최단 경로를 보장하지만, 목표 반대쪽까지 열심히 탐색합니다.
A* — 목표 쪽으로 기울여서#
A* 는 꺼내는 기준을 f = g + w · h 로 바꿉니다. h(휴리스틱) 는 "여기서 목표까지 적어도 이만큼은 남았다" 는 추정입니다.
| 휴리스틱 | 식 (dx, dy = 목표까지 칸 수 차이) | 성질 |
|---|---|---|
| 맨해튼 | dx + dy | 4방향 이동의 정확한 거리. 8방향에서는 과대평가 (대각선이 더 짧으므로) |
| 유클리드 | √(dx² + dy²) | 어떤 이동에서도 과대평가하지 않음. 다만 8방향 격자에서는 다소 작게 잡음 |
| 옥타일(octile) | max(dx, dy) + (√2 − 1) · min(dx, dy) | 8방향 이동의 정확한 거리 (벽이 없을 때) |
h 가 실제 남은 비용을 넘지 않으면(허용적, admissible) A* 도 최단 경로를 보장합니다. 가중치 w 를 1 보다 크게 하면(Weighted A*) 목표 쪽으로 더 서둘러 탐색 셀이 크게 줄지만, 경로가 최단보다 길어질 수 있습니다. 경로 길이는 최단의 w 배를 넘지 않습니다.
NavfnPlanner 의 use_astar: false 는 Dijkstra 로 탐색한다는 뜻입니다. 창고 하나 크기(수만 셀)에서는 Dijkstra 도 충분히 빠르고, 1 Hz 정도로만 다시 계획하기 때문입니다. 지도가 아주 크거나 계획을 자주 해야 하면 use_astar: true 나 다른 계획기(Smac Planner 등)를 고려합니다. tolerance: 2.0 은 목표 셀이 막혀 있으면 반경 2 m 안의 가장 가까운 빈 셀로 대신 계획한다는 뜻입니다.
예측하기: 가중치 w 를 1.0 → 2.0 으로 올리면?#
시뮬레이터 2 의 창고 지도(10 × 6 m, 셀 0.25 m)에서 왼쪽 아래 → 오른쪽 위로 갑니다. A*(옥타일, w = 1.0)의 탐색 셀 수를 100 이라고 하면, w = 2.0 일 때는 대략 몇 개일까요? 경로 길이는 늘까요, 그대로일까요? 공책에 적고 시작합니다.
시뮬레이터 2 · Dijkstra 와 A* 의 탐색량 비교#
창고 지도(40 × 24 셀, 셀 하나 0.25 m)입니다. 파랑으로 칠해진 셀이 탐색한(큐에서 꺼낸) 셀이고, 진할수록 나중에 꺼낸 셀입니다. 초록 S · 주황 G 를 끌어 옮길 수 있고, 빈 곳을 클릭 · 드래그하면 벽을 그리거나 지웁니다.
해 볼 것
- 예측 확인 — 창고 지도에서 네 가지 한 번에 비교 를 누릅니다. Dijkstra · A*(w = 1) · A*(w = 2) · 탐욕의 탐색 셀 수와 경로 길이를 표로 옮기고 예측과 비교합니다.
- 알고리즘을 A* 로 두고 w 를 1.0 → 1.2 → 1.5 → 2.0 → 3.0 으로 올리며 "탐색한 셀" 과 "경로 비용(최적 대비 %)" 을 기록합니다. 탐색량이 가장 크게 줄어드는 구간은 어디인가요?
- w = 1.0 에서 휴리스틱을 옥타일 → 유클리드 → 맨해튼 으로 바꿉니다. 8방향 이동에서 맨해튼을 쓰면 왜 w 를 올린 것과 비슷한 결과가 나오나요? 이동을 4방향으로 바꾸면 어떤가요?
- U 자 함정 지도(출발점이 목표 반대쪽으로 열린 컵 안에 있음)에서 ▶ 탐색 과정 보기 를 Dijkstra · A* 로 봅니다. 이어서 네 가지 한 번에 비교 를 누릅니다. 창고 지도에서는 w = 2 가 탐색을 수 분의 일로 줄였는데, 여기서는 얼마나 줄이나요? 왜 그럴까요?
- 창고 지도에서 코스트 반영 과 경로 매끄럽게 를 켭니다. 경로가 선반에서 얼마나 떨어지나요? 매끄럽게 한 경로의 꼭짓점 수와 길이를 원래 경로와 비교합니다.
관찰 포인트 — Dijkstra 는 h = 0 인 A* 입니다. h 가 실제 남은 거리를 넘지 않으면(옥타일 · 유클리드, w = 1) A* 는 Dijkstra 와 같은 최적 경로를 더 적은 탐색으로 찾습니다. w 를 올리면 탐색은 몇 분의 일로 줄지만 경로가 최적보다 몇 % 길어질 수 있고, 그런데 목표 쪽이 벽으로 막힌 U 자 컵 안에서는 h 가 "벽 너머 목표" 를 가리키므로 A* 든 탐욕 탐색이든 컵 안을 거의 다 채운 뒤에야 빠져나옵니다. 이때는 w 를 올려도 탐색이 거의 줄지 않습니다. 휴리스틱은 장애물을 모르는 추정이기 때문입니다. 8방향 격자 경로는 45° 단위로만 꺾여서 같은 길이의 경로가 여러 개이고 지그재그가 생깁니다. 매끄럽게 하기(smoothing)는 "직선으로 이어도 장애물에 안 닿는 꼭짓점" 을 건너뛰어 길이와 꺾임을 줄입니다. 단, 코스트를 무시하고 매끄럽게 하면 선반 모서리를 스치는 경로가 됩니다.
시뮬레이터 2 의 창고 지도(8방향, 옥타일)에서 A* 의 w 를 1.0 에서 2.0 으로 올렸더니 탐색 셀이 300 개 남짓에서 50 개 안팎으로 줄고, 경로 길이는 11.64 m 에서 12.14 m 로 늘었습니다. 바르게 해석한 것은?
- w = 2.0 의 경로가 틀렸다. A* 는 항상 최단 경로를 찾는다
- w · h 가 실제 남은 거리를 과대평가해 최적성 보장이 깨졌다. 대신 목표 쪽으로 서둘러 탐색이 크게 줄었다
- 탐색 셀 수가 줄어든 것은 지도가 단순해졌기 때문이다
- w 를 더 올리면 경로가 다시 짧아진다
정답 보기
② — A* 의 최적 보장은 h 가 허용적일 때만 성립합니다(①). w = 2 이면 2 × 옥타일 거리는 대부분의 셀에서 실제 남은 거리보다 커서 "목표에 가까워 보이는" 셀을 먼저 꺼냅니다. 그 결과 탐색이 수 배 줄고 경로는 약 4 % 길어졌습니다. 길이는 최적의 w 배를 넘지 않는다는 것만 보장됩니다. 지도는 같습니다(③). w 를 더 올려도 짧아지지 않고 탐욕 탐색에 가까워집니다(④).8방향 이동에서 휴리스틱을 옥타일 대신 맨해튼으로, w = 1.0 그대로 두었더니 탐색 셀이 크게 줄고 경로가 최적보다 조금 길어졌습니다. 이유로 옳은 것은?
- 맨해튼 거리는 항상 옥타일 거리보다 작아서 탐색이 줄었다
- 8방향에서는 대각선이 √2 라서 맨해튼(dx + dy)이 실제 거리보다 크다. w 를 올린 것처럼 과대평가가 되었다
- 맨해튼 거리는 4방향에서만 계산되므로 대각선 이동이 꺼졌다
- 맨해튼 거리는 코스트를 반영하기 때문이다
정답 보기
② — 대각선 한 걸음은 실제 √2 ≈ 1.41 이지만 맨해튼은 2 로 셉니다. dx = dy 인 방향에서는 최대 약 1.41 배 과대평가하므로 w ≈ 1.4 의 Weighted A* 와 비슷하게 동작합니다. 맨해튼은 옥타일 이상입니다(①). 이동 방식은 휴리스틱과 무관하게 그대로 8방향입니다(③). 휴리스틱은 코스트를 모릅니다(④). 4방향 이동이라면 맨해튼이 정확한 거리라서 최적을 유지합니다.3교시 · 경로 추종: Pure Pursuit 와 전방향 제어#
경로를 "따라간다" 는 것#
전역 경로는 5 cm 간격 점들의 목록일 뿐입니다. 로봇은 매 제어 주기(agvpro.yaml 은 20 Hz = 0.05 s)마다 "지금 이 점들을 보고 어떤 속도를 낼까" 를 정해야 합니다. 가장 단순하고 널리 쓰이는 방법이 전방 주시(lookahead) 입니다.
- 경로 위에서 로봇에 가장 가까운 점을 찾습니다.
- 거기서 경로를 따라 L (lookahead 거리) 만큼 앞의 점을 목표점으로 잡습니다.
- 그 목표점을 향하는 속도를 냅니다.
사람이 자전거를 탈 때 바로 앞바퀴 밑이 아니라 몇 m 앞을 보는 것과 같습니다. 성능은 L 하나로 크게 달라집니다.
| L 이 짧으면 | L 이 길면 |
|---|---|
| 경로에 바짝 붙으려고 급하게 방향을 꺾음 | 먼 점을 보며 부드럽게 감 |
| 제어 지연 · 가속도 한계가 있으면 지나쳤다 돌아오기를 반복 (진동) | 코너에서 목표점이 이미 코너 너머라 안쪽으로 깎아 지름 (코너 깎기) |
| 직선에서 오차가 빨리 줄어듦 | 직선에서는 안정적이지만 오차가 천천히 줄어듦 |
차동 구동의 Pure Pursuit vs 전방향 로봇#
일반 차동 구동 로봇(TurtleBot 등)은 옆으로 못 가므로, 목표점을 지나는 원호 를 따라 돌면서 갑니다. 이것이 Pure Pursuit 입니다. 로봇 좌표계에서 목표점의 옆 거리를 y 라 하면 곡률 κ = 2y / L², 각속도 ω = v · κ 입니다. 머리 방향이 곧 진행 방향이라 경로가 꺾이면 머리를 먼저 돌려야 합니다.
myAGV Pro 는 메카넘 휠의 전방향 로봇입니다(ch02). (vx, vy) 로 아무 방향으로나 갈 수 있고, ω 는 그와 따로 정할 수 있습니다. 그래서 추종기를 두 부분으로 나눌 수 있습니다.
- 위치: 목표점 방향으로 속도 벡터 (vx, vy) = v · (목표점 − 로봇) / |목표점 − 로봇| (월드 좌표, 로봇 좌표로 돌려서 보냄)
- 머리 방향: ω = K_θ · (원하는 방향 − 현재 방향) (P 제어)
"원하는 방향" 을 진행 방향으로 두면 차동 로봇처럼 앞을 보며 가고, 고정값으로 두면 머리는 그대로인 채 옆으로 · 비스듬히 갑니다. 컨베이어나 선반 옆면에 차체 긴 변을 붙이는 도킹이 이렇게 가능합니다.
Nav2 에는 Pure Pursuit 를 개선한 Regulated Pure Pursuit 컨트롤러(곡률이 크거나 장애물이 가까우면 속도를 줄임)와, 전방향 로봇의 횡이동까지 샘플링하는 DWB · MPPI 컨트롤러가 있습니다. agvpro.yaml 은 DWB 를 쓰지만 max_vel_y: 0.0 이라 지금 설정으로는 옆으로 가지 않습니다(4교시). 3교시의 추종기는 원리를 보기 위한 단순한 P 제어기입니다.
예측하기: lookahead 를 0.6 m → 0.05 m 로 줄이면?#
시뮬레이터 3 의 S 자 통로(반지름 0.6 m 의 곡선 두 개)를 최대 속도 0.5 m/s, 제어 지연 0.2 s 로 달립니다. L = 0.6 m 일 때 최대 횡방향 오차가 약 13 cm 였다면, L = 0.05 m 로 줄이면 오차는 줄어들까요? 0.2 m 로 하면? 결과를 그래프 모양(코너에서 한 번 크게 · 계속 흔들림)과 함께 예측해 적어 둡니다.
시뮬레이터 3 · lookahead · 속도 · 가속도로 추종 오차 관찰#
위: 경로(회색 띠 = 통로, 초록 띠 = ±5 cm), 로봇의 실제 궤적(초록 = 오차 5 cm 이내, 빨강 = 초과), 주황 원 = lookahead 원과 목표점. 아래: 시간에 따른 횡방향 오차 e (cm) 그래프. 슬라이더를 움직이면 즉시 다시 계산합니다. ▶ 재생 으로 로봇의 움직임을 볼 수 있습니다.
해 볼 것
- 예측 확인 — 처음 값(S 자, L 0.6, v 0.5, 가속도 2.5, 지연 0.2)에서 최대 |e| 와 그래프 모양을 봅니다. 이어서 L 을 0.05 · 0.1 · 0.2 · 0.3 · 0.4 로 바꿔 가며 최대 |e| 와 "흔들림" 횟수를 표로 적습니다. 오차가 가장 작은 L 은 어디인가요?
- L 을 0.05 에 두고 제어 지연을 0.2 → 0.05 → 0 으로 줄입니다. 흔들림이 사라지나요? 진동의 원인이 "L 이 짧은 것" 하나뿐인지 생각해 봅니다.
- L = 0.2 에서 가속도 한계를 2.5 → 1.0 → 0.5 로 낮춥니다. 같은 속도 0.5 m/s 인데 왜 오차가 커지나요? (힌트: 반지름 0.6 m 곡선을 0.5 m/s 로 돌려면 v²/R ≈ 0.42 m/s² 의 구심 가속도가 필요합니다.)
- 목표: S 자 통로를 최대 |e| ≤ 5 cm 로 통과하면서 가장 빨리 도착하는 (L, v_max, 가속도) 찾기 (지연 0.2 s 고정). 찾은 값과 도착 시간을 적습니다.
- 경로를 컨베이어 옆면 도킹 으로 바꾸고 제어 방식 세 가지를 비교합니다 (L 0.2). 차체 긴 변을 컨베이어에 붙일 수 있는 방식은 어느 것인가요? 다른 두 방식에서는 무슨 일이 생기나요? 머리 고정 방식으로 최대 |e| ≤ 5 cm 까지 맞추려면 속도를 얼마로 낮춰야 하나요?
관찰 포인트 — L 이 길면 목표점이 코너 너머에 있어 안쪽으로 깎고(그래프에서 코너마다 한 번 크게 벗어남), L 이 짧으면 목표점이 코앞이라 방향을 급하게 바꾸는데, 0.2 s 늦게 반영되므로 지나쳤다 돌아오기를 반복합니다(그래프가 계속 + − 로 흔들림). 지연이 0 이면 짧은 L 도 흔들리지 않습니다. 진동은 짧은 L × 지연 · 가속도 한계 의 조합입니다. 속도를 줄이면 둘 다 줄어서, 같은 L 로도 오차가 작아집니다. 차동 Pure Pursuit 는 머리를 돌려야 방향이 바뀌므로 짧은 L 에서 더 크게 흔들립니다. 도킹에서는 "머리 고정" 만이 머리 방향 0° 를 유지한 채 옆으로 내려와 긴 변을 붙입니다. 나머지는 머리가 −90° 가 되어 차체 앞쪽(0.265 m)이 컨베이어를 향하므로 같은 위치에서 부딪힙니다.
시뮬레이터 3 의 S 자 통로(v 0.5, 가속도 2.5, 지연 0.2)에서 L 을 바꿨더니 최대 |e| 가 L 0.05 → 약 14 cm (흔들림 8 번), L 0.2 → 약 1 cm, L 0.6 → 약 13 cm (흔들림 1 번) 였습니다. 가장 알맞은 해석은?
- L 은 클수록 좋다. 0.6 의 오차는 측정 오류다
- L 0.05 는 지연 때문에 지나쳤다 돌아오는 진동, L 0.6 은 코너 안쪽을 깎는 오차다. 둘 사이에 최적값이 있다
- 두 경우 모두 코너 깎기다
- 두 경우 모두 가속도 한계 때문이며 L 과는 관계없다
정답 보기
② — 흔들림 횟수가 단서입니다. 짧은 L 에서는 오차 부호가 여러 번 바뀌는 진동, 긴 L 에서는 코너마다 한 번 크게 벗어나는 깎기입니다(③ 오답). 오차가 L 에 따라 U 자로 변하므로 L 과 관계가 있고(④), 0.6 이 가장 좋지도 않습니다(①). 지연을 0 으로 하면 짧은 L 의 진동이 사라지는 것으로 원인을 확인할 수 있습니다.컨베이어 옆면 도킹 경로에서 "전방향 · 머리 = 진행 방향" 으로 달렸더니 경로 오차는 작은데 마지막에 "충돌!" 이 표시되었습니다. 이유로 옳은 것은?
- 전방향 로봇은 아래로 내려갈 때 반드시 머리를 돌려야 한다
- 진행 방향(−y)을 보며 내려왔으므로 마지막 머리 방향이 −90° 가 되어, 차체 앞쪽 끝(0.265 m)이 옆면(0.18 m)보다 컨베이어에 더 가까워졌다
- lookahead 가 너무 길어 코너를 깎았다
- 가속도 한계 때문에 목표점을 지나쳤다
정답 보기
② — 경로 끝(y = 0.7 m)은 같아도 차체의 어느 면이 컨베이어(y = 0.47 m)를 보느냐에 따라 끝까지의 거리가 다릅니다. 머리 0° 면 아래 끝은 0.7 − 0.18 = 0.52 (간격 약 5 cm), −90° 면 0.7 − 0.265 = 0.435 (약 −3.5 cm, 겹침)입니다. 시뮬레이터 값은 마지막 정지 오차 때문에 1 cm 안팎 다를 수 있습니다. 전방향 로봇은 머리를 돌리지 않고 옆으로 내려갈 수 있습니다(①). 오차가 작으므로 ③ · ④ 가 원인이 아닙니다. 이것이 메카넘 로봇이 좁은 곳에서 옆으로 붙는 도킹에 유리한 이유입니다.4교시 · 지역 계획과 동적 장애물 회피#
전역 경로만으로는 부족한 이유#
전역 경로는 1 초에 한 번 정도, 저장된 지도와 그때까지 본 장애물로 계산됩니다. 그 사이에 사람이 걸어오거나 지게차가 통로를 가로지르면, 경로는 그대로인데 길이 막힙니다. 그래서 컨트롤러는 경로를 "따라가되" 지역 코스트맵(로봇 둘레 3 × 3 m, 5 Hz 갱신)을 보며 지금 당장 안전한 속도 를 골라야 합니다.
DWA / DWB — 속도를 뿌려 보고 고르기#
DWA(Dynamic Window Approach) 와 Nav2 의 개선판 DWB 는 이렇게 동작합니다 (agvpro.yaml: 20 Hz).
- 동적 창(dynamic window): 현재 속도에서 가속도 한계(
acc_lim_x: 2.5등)로 한 주기 안에 낼 수 있는 속도 범위를 정합니다. - 샘플링: 그 범위에서 vx · vy · ω 를 고르게 뽑습니다 (
vx_samples: 20,vy_samples: 5,vtheta_samples: 40). - 모의 주행: 각 속도를
sim_time: 1.7초 동안 유지했을 때의 궤적을 그려 봅니다. - 평가(critic): 궤적마다 여러 평가 함수의 점수에 가중치(scale)를 곱해 더합니다. 점수가 가장 낮은 궤적의 속도를 냅니다. 장애물에 닿는(253 이상) 궤적은 탈락입니다.
| 평가 함수 (critic) | 보는 것 | agvpro.yaml scale |
|---|---|---|
PathDist | 궤적 끝이 전역 경로에서 얼마나 먼가 | 32.0 |
GoalDist | 궤적 끝에서 목표까지 경로를 따라 얼마나 남았나 | 24.0 |
PathAlign · GoalAlign | 로봇 앞 0.1 m 지점이 경로 · 목표 방향과 맞는가 | 23.0 · 18.0 |
BaseObstacle | 궤적이 지나는 셀들의 코스트 합 | 0.02 |
RotateToGoal | 목표 근처에서 제자리 회전으로 방향 맞추기 | 32.0 |
Oscillation | 앞뒤 · 좌우로 왔다 갔다 하는 궤적에 벌점 | (기본값) |
가중치의 균형이 성격을 정합니다. PathDist 가 크면 경로에 딱 붙어서 가고, BaseObstacle 이 크면 장애물에서 멀찍이 떨어집니다. agvpro.yaml 은 BaseObstacle 이 0.02 로 작아서(셀 비용 합이 수천 단위라 곱하면 수십) 경로를 고집하는 쪽 입니다. sim_time 이 길면 멀리 내다보아 일찍 피하지만, 좁은 곳에서는 대부분의 궤적이 "언젠가 닿음" 으로 탈락해 움직이지 못합니다.
Nav2 의 세 서버#
recoveries_server 에 spin · backup · wait 를 둡니다.| 서버 | 하는 일 | 실패하면 |
|---|---|---|
| Planner | 전역 코스트맵에서 경로 계산 (Navfn, 2교시) | "경로 없음" → 코스트맵 지우기 · 대기 후 재시도 |
| Controller | 경로를 따라 속도 계산 (DWB, 3 · 4교시) | 유효한 궤적 없음 · 진행 없음 → 복구 동작 |
| Behavior | spin(제자리 회전) · backup(후진) · wait(대기) | 정해진 횟수 실패 시 목표 포기 |
진행 감시는 progress_checker 가 합니다. agvpro.yaml 은 **8 초(movement_time_allowance) 동안 0.15 m(required_movement_radius) 이상 움직이지 못하면** 컨트롤러 실패로 보고 복구 동작으로 넘어갑니다.
막혔을 때의 세 가지 전략#
| 전략 | 동작 | 좋은 상황 | 나쁜 상황 |
|---|---|---|---|
| 대기 | 경로 위에 장애물이 있으면 멈추고 기다림 | 사람 · 지게차처럼 곧 비켜 줄 때. 가장 예측 가능 | 영영 안 비키는 팔레트 |
| 지역 우회 | 전역 경로는 그대로, DWB 가 옆으로 비켜 감 | 통로가 넓고 장애물이 작을 때 | 통로가 막혔을 때 (경로를 고집하다 멈춤) |
| 재계획 | 본 장애물을 넣어 전역 경로를 다시 계산 (1 Hz) | 통로가 완전히 막혔을 때 다른 통로로 | 잠깐 지나가는 장애물에 먼 길로 돌아감 |
예측하기: 사람이 마주 걸어올 때#
폭 1.8 m 통로에서 사람이 0.5 m/s 로 마주 걸어옵니다. agvpro.yaml 설정(최대 0.26 m/s, 옆 이동 없음, PathDist 32 · BaseObstacle 0.02)이라면 로봇은 ① 사람을 옆으로 비켜 지나갈까요, ② 멈춰서 기다릴까요? 옆 이동(vy)을 허용하면 달라질까요?
시뮬레이터 4 · 동적 장애물과 평가 함수 가중치#
10 × 5 m 창고입니다. 아래 통로는 폭 1.8 m, 위 통로는 1.5 m 이고, 선반 두 블록 사이에 폭 0.8 m 의 가로 통로가 있습니다. 로봇은 왼쪽 아래(0.8, 1.0)에서 오른쪽 아래(9.2, 1.0)로 갑니다. 라이다는 2.5 m 안의 장애물만 지역 코스트맵에 넣습니다(obstacle_max_range: 2.5). 회색 가는 선 = DWB 가 모의 주행한 궤적, 빨강 = 탈락한 궤적, 초록 굵은 선 = 고른 궤적, 파랑 = 전역 경로입니다. 시간은 0.1 s 단위의 시뮬레이션 시간입니다.
해 볼 것
- 예측 확인 — agvpro.yaml 값 · 사람이 마주 걸어옴 · 지역 우회로 ▶ 실행 합니다. 로봇은 비켜 갔나요, 멈췄나요? 사람은 어떻게 했나요? 이어서 옆 이동 vy 샘플 허용 을 켜고 같은 실험을 합니다. 도착 시간이 어떻게 달라지나요?
- 빠르지만 위험 · 느리지만 안전 예시값을 사람 · 지게차 상황(지역 우회)에서 각각 끝까지 계산 하고, 도착 시간 · 동적 장애물까지 최소 차체 여유를 표로 적습니다. 두 설정은 어떤 값이 다른가요?
- agvpro.yaml 값으로 돌아가 사람 상황에서 sim_time 을 0.5 · 1.0 · 3.0 으로 바꿔 봅니다. 0.5 에서는 무슨 일이 생기나요? 이어서 BaseObstacle 을 0 → 1.0 으로 올려 봅니다. 최소 여유가 거의 그대로라면, inflation_radius 를 0.6 으로 함께 올려 다시 봅니다. 왜 그럴까요?
- 팔레트가 통로를 막음 에서 막혔을 때 전략 세 가지를 비교합니다. 성공하는 전략은 무엇이고, 나머지는 "진행 없음" 이 몇 번 나오나요? 같은 상황을 느리지만 안전 설정으로 재계획하면 어떻게 되나요?
- 지게차가 가로 통로로 내려옴 을 빠르지만 위험 + 대기 로 돌려 봅니다. 시간 초과가 났다면 로봇과 지게차가 어디에 서 있는지 보고 원인을 설명해 봅니다.
관찰 포인트 — DWB 는 "경로에서 멀어지는 것(PathDist)" 과 "목표에서 멀어지는 것(GoalDist)" 을 무겁게 보므로, agvpro.yaml 설정에서는 사람을 비켜 가기보다 멈춰서 기다리는 쪽을 고릅니다. 옆 이동(vy)을 허용하면 머리를 돌리지 않고 옆으로 비켜 가서 훨씬 빨리 지나갑니다 (전방향 로봇의 장점을 설정이 막고 있었던 것). sim_time 이 너무 짧으면 멀리 보지 못해 장애물 코앞까지 가서 꼼짝 못 하고, 길면 일찍 속도를 줄입니다. BaseObstacle 은 코스트맵에 적힌 비용 을 더하는 것이므로, inflation_radius 가 0.30 이면(내접 0.18 밖 12 cm 만 비용) 가중치를 올려도 거의 달라지지 않습니다. 1교시의 팽창 설정과 4교시의 평가 함수는 한 묶음입니다. 막힌 통로는 재계획만이 풀지만, 팽창과 장애물 가중치를 크게 한 "안전한" 설정은 폭 0.8 m 가로 통로 입구에서 멈춰 버립니다. 대기 전략은 어디에서 기다리느냐 도 중요합니다. 지게차가 내려올 자리 앞에서 기다리면, 사람이 모는 지게차도 로봇 앞에서 서서 서로 기다리는 교착(deadlock)이 됩니다.
| 사람 상황 · 지역 우회 (예시 결과) | 도착 시간 | 사람까지 최소 차체 여유 |
|---|---|---|
| agvpro.yaml (0.26 m/s, sim_time 1.7) | 약 35 s | 약 13 cm |
| agvpro.yaml + vy 허용 | 약 27 s | 약 13 cm |
| 빠르지만 위험 (1.0 m/s, sim_time 0.6, BaseObstacle 0) | 약 11 s | 약 5 cm |
| 느리지만 안전 (0.4 m/s, sim_time 2.5, BaseObstacle 0.5, 팽창 0.8) | 약 29 s | 약 20 cm |
시뮬레이터 4 에서 agvpro.yaml 값(inflation_radius 0.30)으로 BaseObstacle scale 을 0.02 → 1.0 으로 50 배 올렸는데, 사람까지 최소 여유가 13 cm → 15 cm 로 거의 그대로였습니다. 가장 알맞은 설명은?
- BaseObstacle 은 동적 장애물을 보지 않는다
- BaseObstacle 은 궤적이 지나는 셀의 코스트를 더하는데, inflation_radius 0.30 이면 장애물 둘레 아주 좁은 띠에만 비용이 있어 더할 것이 거의 없다. 팽창 범위를 함께 넓혀야 효과가 난다
- 가중치는 1.0 이 최댓값이라 효과가 없다
- sim_time 이 길어서 장애물 비용이 무시된다
정답 보기
② — 평가 함수는 코스트맵을 읽을 뿐입니다. 내접 반경 0.18 m 와 inflation_radius 0.30 m 사이의 12 cm 띠 밖은 비용이 0 이라, 그 바깥에서 지나가는 궤적끼리는 장애물 점수 차이가 없습니다. inflation_radius 를 0.6 으로 넓히면 같은 가중치로 여유가 20 cm 이상으로 늘어납니다. 사람도 라이다 2.5 m 안에 들어오면 지역 코스트맵에 들어가므로 ① 은 틀립니다. 가중치에 상한은 없고(③), sim_time 은 오히려 장애물을 더 일찍 보게 합니다(④).팔레트가 아래 통로를 완전히 막은 상황에서 "지역 우회 (경로 고정)" 로 달렸더니 로봇이 팔레트 앞에서 멈추고 "진행 없음" 이 여러 번 기록되었습니다. Nav2 에서 이 상황을 풀어 주는 구성 요소는?
- Controller Server 의 PathDist 가중치를 0 으로 낮춘다
- Planner Server 가 장애물 레이어에 들어간 팔레트를 반영해 전역 경로를 다시 계산한다 (가로 통로 → 위 통로)
- velocity_smoother 의 최대 가속도를 올린다
- AMCL 의 파티클 수를 늘린다
정답 보기
② — 길 자체가 막혔으므로 속도를 아무리 잘 골라도(①, ③) 목표에 갈 수 없습니다. 지역 계획기는 전역 경로 둘레만 보므로, 장애물을 반영한 새 전역 경로가 필요합니다. Nav2 에서는 BT Navigator 가 주기적으로(또는 실패 시) Planner Server 를 다시 부르고, 그래도 안 되면 Behavior Server 의 복구 동작(spin · backup · wait)을 시도합니다. 위치 추정(④)은 이 문제와 관계없습니다.실습 (시뮬레이터)#
교시마다 Playground 에서 코드를 직접 돌립니다. starter 의 TODO 를 채워 실행한 뒤 풀이 와 비교하세요. 모든 코드는 표준 라이브러리 + numpy 만 쓰며, 지도 · 모델은 시뮬레이터 1 ~ 4 와 같거나 그것을 단순화한 것입니다. starter 는 TODO 를 채우지 않아도 오류 없이 실행되지만 결과가 틀리게 나옵니다.
시뮬레이터 1 과 같은 창고(폭 0.6 m 좁은 통로 · 기둥)에서 Nav2 의 팽창 비용을 직접 만들고, inflation_radius 를 바꿔 가며 경로를 계획합니다.
- TODO 1 · 2
inflate()— 팽창이 닿는 셀 수 Rc 를 구하고, 그 안의 셀에 253(내접) 또는 252 · e^(−k(d − 0.18)) 를 줍니다. - 합격 조건: ① 좁은 통로로 통과(우회하지 않음) ② 기둥 옆 실제 차체 여유 ≥ 10 cm.
예측 먼저 — 첫 줄 center cost of 0.6 m gap 의 값(1교시 표)과, 합격하는 R 이 몇 개일지 적어 둡니다.
import numpy as np
import heapq
# 시뮬레이터 1 과 같은 창고: 6.0 x 4.4 m, 셀 0.05 m
RES = 0.05
NX, NY = 120, 88
R_INS = 0.18 # 사각 footprint 0.52 x 0.36 m 의 내접 반경
START, GOAL = (0.6, 1.2), (5.4, 1.2)
def make_map(gap=0.6):
"""True = 장애물 셀. 가운데 벽에 좁은 통로(폭 gap)와 넓은 통로(1.0 m), 왼쪽 방에 기둥"""
xs = (np.arange(NX) + 0.5) * RES
ys = (np.arange(NY) + 0.5) * RES
X, Y = np.meshgrid(xs, ys)
occ = np.zeros((NY, NX), bool)
occ[0, :] = occ[-1, :] = occ[:, 0] = occ[:, -1] = True
wall = (X >= 2.8) & (X < 3.2)
narrow = (Y >= 1.2 - gap / 2) & (Y < 1.2 + gap / 2)
wide = (Y >= 3.0) & (Y < 4.0)
occ |= wall & ~narrow & ~wide
occ |= (X >= 1.5) & (X < 1.9) & (Y >= 0.85) & (Y < 1.35)
return occ
def distance_cells(occ, maxc=20):
"""셀마다 가장 가까운 장애물 셀까지 거리 (셀 단위). maxc 셀보다 멀면 inf"""
d = np.full(occ.shape, np.inf)
pad = np.pad(occ, maxc)
for dy in range(-maxc, maxc + 1):
for dx in range(-maxc, maxc + 1):
r = np.hypot(dx, dy)
if r > maxc:
continue
shifted = pad[maxc + dy:maxc + dy + NY, maxc + dx:maxc + dx + NX]
d = np.where(shifted & (r < d), r, d)
return d
def inflate(dist_c, inflation_radius, k):
"""Nav2 inflation_layer 처럼 셀 비용(0 ~ 254)을 만듭니다."""
cost = np.zeros(dist_c.shape)
# TODO 1: 팽창이 닿는 셀 수 Rc = ceil(inflation_radius / RES) 를 구합니다 (부동소수 오차: - 1e-9)
# TODO 2: dist_c <= Rc 인 셀에만 비용을 줍니다. (지금은 장애물 셀만 254 라 경로가 벽에 붙습니다)
# - 거리(m) = dist_c * RES 가 R_INS 이하면 253
# - 그 밖은 floor(252 * exp(-k * (거리 - R_INS)))
cost[dist_c == 0] = 254 # 장애물 셀 (주어짐)
return cost
def plan(cost):
"""Navfn 과 비슷한 Dijkstra: 한 걸음 비용 = 거리 x (1 + 0.016 x 셀 비용), 253 이상은 통행 불가"""
sx, sy = int(START[0] / RES), int(START[1] / RES)
gx, gy = int(GOAL[0] / RES), int(GOAL[1] / RES)
if cost[sy, sx] >= 253 or cost[gy, gx] >= 253:
return None
step = 1 + 0.016 * cost
g = np.full(cost.shape, np.inf)
g[sy, sx] = 0.0
parent = {}
pq = [(0.0, sy, sx)]
nbrs = [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
(1, 1, 2 ** 0.5), (1, -1, 2 ** 0.5), (-1, 1, 2 ** 0.5), (-1, -1, 2 ** 0.5)]
while pq:
c, y, x = heapq.heappop(pq)
if c > g[y, x]:
continue
if (y, x) == (gy, gx):
break
for dy, dx, L in nbrs:
ny, nx = y + dy, x + dx
if cost[ny, nx] >= 253:
continue
nc = c + L * RES * 0.5 * (step[y, x] + step[ny, nx])
if nc < g[ny, nx]:
g[ny, nx] = nc
parent[(ny, nx)] = (y, x)
heapq.heappush(pq, (nc, ny, nx))
if not np.isfinite(g[gy, gx]):
return None
path = [(gy, gx)]
while path[-1] != (sy, sx):
path.append(parent[path[-1]])
return path[::-1]
def evaluate(path, dist_c):
"""(경로, 길이 m, 기둥 옆 차체 여유 m)"""
length = sum(np.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])) * RES
route = "narrow" if max(y for y, x in path) * RES < 2.5 else "detour"
pillar = min(dist_c[y, x] for y, x in path if x * RES < 2.5) * RES - R_INS
return route, length, pillar
occ = make_map(0.6)
dist_c = distance_cells(occ)
# 확인: agvpro.yaml 값에서 통로 중앙 셀의 비용 (예측: 155)
c = inflate(dist_c, 0.30, 4.0)
print("center cost of 0.6 m gap:", int(c[int(1.2 / RES), int(3.0 / RES)]))
print(" R(m) route length(m) pillar clearance(cm) OK?")
for R in [0.15, 0.20, 0.25, 0.30, 0.35, 0.40, 0.50]:
path = plan(inflate(dist_c, R, 4.0))
if path is None:
print(f"{R:5.2f} no path")
continue
route, length, pillar = evaluate(path, dist_c)
ok = route == "narrow" and pillar >= 0.10
print(f"{R:5.2f} {route:7s} {length:8.2f} {pillar * 100:12.1f} {'OK' if ok else '-'}")해 볼 것
- 첫 줄이 155 인지, 표가 시뮬레이터 1 의 결과(경로 길이 · 기둥 옆 여유)와 같은지 확인합니다.
- 합격하는 R 은 몇 개인가요? agvpro.yaml 의 0.30 은 그 범위의 어디에 있나요?
make_map(0.7)로 통로를 넓히면 합격 범위가 어떻게 바뀌나요?- k(cost_scaling_factor)를 8.0 으로 바꿔 같은 표를 만듭니다. "R 을 키워도 우회하지 않는" 이유를 비용 곡선으로 설명합니다.
import numpy as np
import heapq
# 시뮬레이터 1 과 같은 창고: 6.0 x 4.4 m, 셀 0.05 m
RES = 0.05
NX, NY = 120, 88
R_INS = 0.18 # 사각 footprint 0.52 x 0.36 m 의 내접 반경
START, GOAL = (0.6, 1.2), (5.4, 1.2)
def make_map(gap=0.6):
"""True = 장애물 셀. 가운데 벽에 좁은 통로(폭 gap)와 넓은 통로(1.0 m), 왼쪽 방에 기둥"""
xs = (np.arange(NX) + 0.5) * RES
ys = (np.arange(NY) + 0.5) * RES
X, Y = np.meshgrid(xs, ys)
occ = np.zeros((NY, NX), bool)
occ[0, :] = occ[-1, :] = occ[:, 0] = occ[:, -1] = True
wall = (X >= 2.8) & (X < 3.2)
narrow = (Y >= 1.2 - gap / 2) & (Y < 1.2 + gap / 2)
wide = (Y >= 3.0) & (Y < 4.0)
occ |= wall & ~narrow & ~wide
occ |= (X >= 1.5) & (X < 1.9) & (Y >= 0.85) & (Y < 1.35)
return occ
def distance_cells(occ, maxc=20):
"""셀마다 가장 가까운 장애물 셀까지 거리 (셀 단위). maxc 셀보다 멀면 inf"""
d = np.full(occ.shape, np.inf)
pad = np.pad(occ, maxc)
for dy in range(-maxc, maxc + 1):
for dx in range(-maxc, maxc + 1):
r = np.hypot(dx, dy)
if r > maxc:
continue
shifted = pad[maxc + dy:maxc + dy + NY, maxc + dx:maxc + dx + NX]
d = np.where(shifted & (r < d), r, d)
return d
def inflate(dist_c, inflation_radius, k):
"""Nav2 inflation_layer 처럼 셀 비용(0 ~ 254)을 만듭니다."""
cost = np.zeros(dist_c.shape)
Rc = np.ceil(inflation_radius / RES - 1e-9) # 팽창이 닿는 셀 수
inside = dist_c <= Rc + 1e-9
d_m = dist_c * RES
cost[inside] = np.floor(252 * np.exp(-k * (d_m[inside] - R_INS)))
cost[inside & (d_m <= R_INS + 1e-9)] = 253 # 중심이 여기면 반드시 충돌
cost[dist_c == 0] = 254 # 장애물 셀
return cost
def plan(cost):
"""Navfn 과 비슷한 Dijkstra: 한 걸음 비용 = 거리 x (1 + 0.016 x 셀 비용), 253 이상은 통행 불가"""
sx, sy = int(START[0] / RES), int(START[1] / RES)
gx, gy = int(GOAL[0] / RES), int(GOAL[1] / RES)
if cost[sy, sx] >= 253 or cost[gy, gx] >= 253:
return None
step = 1 + 0.016 * cost
g = np.full(cost.shape, np.inf)
g[sy, sx] = 0.0
parent = {}
pq = [(0.0, sy, sx)]
nbrs = [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
(1, 1, 2 ** 0.5), (1, -1, 2 ** 0.5), (-1, 1, 2 ** 0.5), (-1, -1, 2 ** 0.5)]
while pq:
c, y, x = heapq.heappop(pq)
if c > g[y, x]:
continue
if (y, x) == (gy, gx):
break
for dy, dx, L in nbrs:
ny, nx = y + dy, x + dx
if cost[ny, nx] >= 253:
continue
nc = c + L * RES * 0.5 * (step[y, x] + step[ny, nx])
if nc < g[ny, nx]:
g[ny, nx] = nc
parent[(ny, nx)] = (y, x)
heapq.heappush(pq, (nc, ny, nx))
if not np.isfinite(g[gy, gx]):
return None
path = [(gy, gx)]
while path[-1] != (sy, sx):
path.append(parent[path[-1]])
return path[::-1]
def evaluate(path, dist_c):
"""(경로, 길이 m, 기둥 옆 차체 여유 m)"""
length = sum(np.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])) * RES
route = "narrow" if max(y for y, x in path) * RES < 2.5 else "detour"
pillar = min(dist_c[y, x] for y, x in path if x * RES < 2.5) * RES - R_INS
return route, length, pillar
occ = make_map(0.6)
dist_c = distance_cells(occ)
# 확인: agvpro.yaml 값에서 통로 중앙 셀의 비용 (예측: 155)
c = inflate(dist_c, 0.30, 4.0)
print("center cost of 0.6 m gap:", int(c[int(1.2 / RES), int(3.0 / RES)]))
print(" R(m) route length(m) pillar clearance(cm) OK?")
for R in [0.15, 0.20, 0.25, 0.30, 0.35, 0.40, 0.50]:
path = plan(inflate(dist_c, R, 4.0))
if path is None:
print(f"{R:5.2f} no path")
continue
route, length, pillar = evaluate(path, dist_c)
ok = route == "narrow" and pillar >= 0.10
print(f"{R:5.2f} {route:7s} {length:8.2f} {pillar * 100:12.1f} {'OK' if ok else '-'}")
# 덤: k = 8 이면 창이 넓어지는가?
print("k = 8.0")
for R in [0.30, 0.40, 0.50]:
route, length, pillar = evaluate(plan(inflate(dist_c, R, 8.0)), dist_c)
print(f"{R:5.2f} {route:7s} {length:8.2f} {pillar * 100:12.1f}")- 예시 결과(통로 0.6 m, k = 4): R 0.15 는 기둥 옆 여유가 −2.2 cm (내접 반경보다 작아 충돌), 0.20 · 0.25 는 2.6 · 7.5 cm 로 부족, 0.30 만 합격(12.4 cm, 길이 5.26 m), 0.35 부터는 넓은 통로로 우회(6.73 m 이상)합니다. agvpro.yaml 의 값이 이 지도에서는 합격 범위의 유일한 값, 즉 경계에 있습니다.
- k = 8 이면 비용이 벽 가까이에서만 크고 빨리 줄어서 R 0.40 · 0.50 에서도 좁은 통로를 쓰고 기둥 옆 여유는 22 cm 로 늡니다. "멀리까지 조금" 이 아니라 "가까이만 많이" 비싸게 만드는 것입니다.
시뮬레이터 2 의 창고 지도에서 A* 를 직접 구현하고, 휴리스틱 가중치 w = 1.0 과 2.0 의 탐색 셀 수 · 경로 길이를 비교합니다.
- TODO 1
heuristic()— 옥타일 · 유클리드 · 맨해튼 거리를 돌려줍니다. - TODO 2 이웃 갱신 — 더 싼 길을 찾으면
g·parent를 고치고(f, g, (c, r))를 큐에 넣습니다. 튜플의 두 번째 값 g 는 f 가 같을 때 꺼내는 순서를 정합니다.
예측 먼저 — w = 2.0 의 탐색 셀 수는 w = 1.0 의 몇 분의 일일까요? 경로는 몇 % 길어질까요?
import heapq
import math
import numpy as np
# 시뮬레이터 2 의 창고 지도: 40 x 24 셀, 셀 하나 0.25 m. '#' = 선반 / 벽
CELL = 0.25
ROWS, COLS = 24, 40
grid = np.zeros((ROWS, COLS), bool)
grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = True
for r0, c0, c1 in [(4, 3, 30), (9, 9, 17), (9, 21, 36), (14, 3, 30), (19, 9, 17), (19, 21, 36)]:
grid[r0:r0 + 2, c0:c1 + 1] = True
S, G = (2, 21), (36, 2) # (열 c, 행 r)
# 선반 옆 코스트: 벽에 붙은 셀 0.6, 한 칸 떨어진 셀 0.2 (시뮬레이터 2 의 '코스트 반영')
near1 = np.zeros_like(grid)
near2 = np.zeros_like(grid)
for dr in range(-2, 3):
for dc in range(-2, 3):
sh = np.roll(np.roll(grid, dr, 0), dc, 1)
if max(abs(dr), abs(dc)) == 1:
near1 |= sh
elif max(abs(dr), abs(dc)) == 2:
near2 |= sh
pen = np.where(grid, 0.0, np.where(near1, 0.6, np.where(near2, 0.2, 0.0)))
SQ2 = math.sqrt(2)
NBRS = [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
(1, 1, SQ2), (1, -1, SQ2), (-1, 1, SQ2), (-1, -1, SQ2)]
def heuristic(c, r, kind):
dx, dy = abs(c - G[0]), abs(r - G[1])
# TODO 1: kind 에 따라 남은 거리 추정값을 돌려줍니다.
# "octile" : max(dx, dy) + (sqrt(2) - 1) * min(dx, dy)
# "euclid" : sqrt(dx^2 + dy^2)
# "manhattan" : dx + dy
return 0.0 # 지금은 h = 0 -> Dijkstra 와 같음
def astar(w=1.0, kind="octile", use_cost=False):
"""(경로 셀 목록, 길이 m, 비용, 탐색한 셀 수)"""
g = {S: 0.0}
parent = {}
closed = set()
pq = [(w * heuristic(*S, kind), 0.0, S)]
while pq:
f, gc, node = heapq.heappop(pq)
if node in closed:
continue
closed.add(node)
if node == G:
break
c, r = node
for dc, dr, L in NBRS:
nc, nr = c + dc, r + dr
if grid[nr, nc]:
continue
if dc and dr and (grid[r, nc] or grid[nr, c]): # 모서리 긁기 금지
continue
step = L * (1 + (pen[r, c] + pen[nr, nc]) / 2) if use_cost else L
# TODO 2: 새 비용 ng = gc + step 이 기존 g 보다 작으면 g · parent 를 고치고
# (ng + w * h, ng, (nc, nr)) 를 pq 에 넣습니다 (heapq.heappush).
pass
if G not in g:
return None, 0.0, math.inf, len(closed)
path = [G]
while path[-1] != S:
path.append(parent[path[-1]])
path.reverse()
length = sum(math.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])) * CELL
return path, length, g[G] * CELL, len(closed)
def show(path):
rows = [["#" if grid[r, c] else "." for c in range(COLS)] for r in range(ROWS)]
for c, r in path:
rows[r][c] = "*"
rows[S[1]][S[0]], rows[G[1]][G[0]] = "S", "G"
print("\n".join("".join(row) for row in rows))
print("algorithm expanded length(m) cost")
for label, w, kind, use_cost in [("Dijkstra (w=0)", 0.0, "octile", False),
("A* octile w=1.0", 1.0, "octile", False),
("A* octile w=2.0", 2.0, "octile", False),
("A* manhattan w=1.0", 1.0, "manhattan", False),
("A* octile w=1.0 cost", 1.0, "octile", True),
("A* octile w=2.0 cost", 2.0, "octile", True)]:
path, length, cost, n = astar(w, kind, use_cost)
if path is None:
print(f"{label:22s} {n:6d} no path")
else:
print(f"{label:22s} {n:6d} {length:7.2f} {cost:7.2f}")
path, *_ = astar(2.0, "octile")
if path:
show(path)해 볼 것
- 표의 숫자가 시뮬레이터 2 의 네 가지 한 번에 비교 결과와 같은지 확인합니다 (Dijkstra 616 · A* 313 · w=2 48).
- w 를 1.0 · 1.2 · 1.5 · 2.0 · 3.0 으로 바꾼 표를 만들고, "탐색 셀 수 vs 최적 대비 경로 길이" 를 한 줄로 정리합니다.
- 코스트를 반영(
use_cost=True)했을 때 같은 w = 2.0 의 비용 증가율은 코스트가 없을 때보다 큰가요, 작은가요? S,G를 바꿔(예:S = (35, 21),G = (4, 2)) 같은 비교를 합니다.
import heapq
import math
import numpy as np
# 시뮬레이터 2 의 창고 지도: 40 x 24 셀, 셀 하나 0.25 m. '#' = 선반 / 벽
CELL = 0.25
ROWS, COLS = 24, 40
grid = np.zeros((ROWS, COLS), bool)
grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = True
for r0, c0, c1 in [(4, 3, 30), (9, 9, 17), (9, 21, 36), (14, 3, 30), (19, 9, 17), (19, 21, 36)]:
grid[r0:r0 + 2, c0:c1 + 1] = True
S, G = (2, 21), (36, 2) # (열 c, 행 r)
# 선반 옆 코스트: 벽에 붙은 셀 0.6, 한 칸 떨어진 셀 0.2 (시뮬레이터 2 의 '코스트 반영')
near1 = np.zeros_like(grid)
near2 = np.zeros_like(grid)
for dr in range(-2, 3):
for dc in range(-2, 3):
sh = np.roll(np.roll(grid, dr, 0), dc, 1)
if max(abs(dr), abs(dc)) == 1:
near1 |= sh
elif max(abs(dr), abs(dc)) == 2:
near2 |= sh
pen = np.where(grid, 0.0, np.where(near1, 0.6, np.where(near2, 0.2, 0.0)))
SQ2 = math.sqrt(2)
NBRS = [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
(1, 1, SQ2), (1, -1, SQ2), (-1, 1, SQ2), (-1, -1, SQ2)]
def heuristic(c, r, kind):
dx, dy = abs(c - G[0]), abs(r - G[1])
if kind == "manhattan":
return dx + dy
if kind == "euclid":
return math.hypot(dx, dy)
return max(dx, dy) + (SQ2 - 1) * min(dx, dy) # octile
def astar(w=1.0, kind="octile", use_cost=False):
"""(경로 셀 목록, 길이 m, 비용, 탐색한 셀 수)"""
g = {S: 0.0}
parent = {}
closed = set()
pq = [(w * heuristic(*S, kind), 0.0, S)]
while pq:
f, gc, node = heapq.heappop(pq)
if node in closed:
continue
closed.add(node)
if node == G:
break
c, r = node
for dc, dr, L in NBRS:
nc, nr = c + dc, r + dr
if grid[nr, nc]:
continue
if dc and dr and (grid[r, nc] or grid[nr, c]): # 모서리 긁기 금지
continue
step = L * (1 + (pen[r, c] + pen[nr, nc]) / 2) if use_cost else L
ng = gc + step
if ng < g.get((nc, nr), math.inf):
g[(nc, nr)] = ng
parent[(nc, nr)] = node
heapq.heappush(pq, (ng + w * heuristic(nc, nr, kind), ng, (nc, nr)))
if G not in g:
return None, 0.0, math.inf, len(closed)
path = [G]
while path[-1] != S:
path.append(parent[path[-1]])
path.reverse()
length = sum(math.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])) * CELL
return path, length, g[G] * CELL, len(closed)
def show(path):
rows = [["#" if grid[r, c] else "." for c in range(COLS)] for r in range(ROWS)]
for c, r in path:
rows[r][c] = "*"
rows[S[1]][S[0]], rows[G[1]][G[0]] = "S", "G"
print("\n".join("".join(row) for row in rows))
print("algorithm expanded length(m) cost")
for label, w, kind, use_cost in [("Dijkstra (w=0)", 0.0, "octile", False),
("A* octile w=1.0", 1.0, "octile", False),
("A* octile w=2.0", 2.0, "octile", False),
("A* manhattan w=1.0", 1.0, "manhattan", False),
("A* octile w=1.0 cost", 1.0, "octile", True),
("A* octile w=2.0 cost", 2.0, "octile", True)]:
path, length, cost, n = astar(w, kind, use_cost)
if path is None:
print(f"{label:22s} {n:6d} no path")
else:
print(f"{label:22s} {n:6d} {length:7.2f} {cost:7.2f}")
path, *_ = astar(2.0, "octile")
if path:
show(path)- 예시 결과: Dijkstra 616 셀 · 11.64 m, A*(w = 1) 313 셀 · 11.64 m (같은 최단), A*(w = 2) 48 셀 · 12.14 m (+4.3 %), 맨해튼(w = 1) 57 셀 · 12.14 m. 코스트를 반영하면 A*(w = 1) 401 셀 · 비용 13.96, A*(w = 2) 70 셀 · 비용 16.39 (+17 %)로, 코스트가 있을 때 w 를 올리면 더 많이 손해 봅니다. 휴리스틱이 코스트를 모르기 때문입니다.
- 큐에 넣는 튜플에 numpy 배열을 넣으면 f 가 같을 때 배열끼리 비교하다 오류가 납니다(자주 나는 오류 7). 좌표는 파이썬 정수 튜플로 넣습니다.
8방향 격자 경로는 45° 단위로 꺾여 지그재그가 생깁니다. "직선으로 이어도 안 막히는 꼭짓점은 건너뛰는" string pulling 으로 매끄럽게 합니다.
- TODO 1
line_of_sight()— 두 셀 사이 직선을 0.1 셀 간격으로 나눠 모든 점이free()인지 봅니다. - TODO 2
smooth()— 지금 꼭짓점에서 보이는 가장 먼 꼭짓점으로 건너뜁니다.
import heapq
import math
import numpy as np
# 실습 2-1 과 같은 창고 지도 (40 x 24 셀, 0.25 m). A* 는 완성본을 씁니다.
CELL = 0.25
ROWS, COLS = 24, 40
grid = np.zeros((ROWS, COLS), bool)
grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = True
for r0, c0, c1 in [(4, 3, 30), (9, 9, 17), (9, 21, 36), (14, 3, 30), (19, 9, 17), (19, 21, 36)]:
grid[r0:r0 + 2, c0:c1 + 1] = True
S, G = (2, 21), (36, 2)
near1 = np.zeros_like(grid)
for dr in (-1, 0, 1):
for dc in (-1, 0, 1):
near1 |= np.roll(np.roll(grid, dr, 0), dc, 1)
near1 &= ~grid # 선반에 붙은 빈 셀
SQ2 = math.sqrt(2)
NBRS = [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
(1, 1, SQ2), (1, -1, SQ2), (-1, 1, SQ2), (-1, -1, SQ2)]
def astar(nbrs):
def h(c, r):
dx, dy = abs(c - G[0]), abs(r - G[1])
return max(dx, dy) + (SQ2 - 1) * min(dx, dy) if len(nbrs) == 8 else dx + dy
g, parent, closed, pq = {S: 0.0}, {}, set(), [(h(*S), 0.0, S)]
while pq:
f, gc, node = heapq.heappop(pq)
if node in closed:
continue
closed.add(node)
if node == G:
break
c, r = node
for dc, dr, L in nbrs:
nc, nr = c + dc, r + dr
if grid[nr, nc] or (dc and dr and (grid[r, nc] or grid[nr, c])):
continue
if gc + L < g.get((nc, nr), math.inf):
g[(nc, nr)] = gc + L
parent[(nc, nr)] = node
heapq.heappush(pq, (gc + L + h(nc, nr), gc + L, (nc, nr)))
path = [G]
while path[-1] != S:
path.append(parent[path[-1]])
return path[::-1]
def length(path):
return sum(math.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])) * CELL
def turns(path):
d = [(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])]
return sum(1 for u, v in zip(d, d[1:]) if u != v)
def free(x, y, avoid_near):
"""점 (x, y) (셀 좌표, 실수) 둘레 +-0.35 셀이 모두 빈 셀인가. avoid_near 이면 선반에 붙은 셀도 피함"""
for ox in (-0.35, 0.35):
for oy in (-0.35, 0.35):
c, r = int(round(x + ox)), int(round(y + oy))
if grid[r, c] or (avoid_near and near1[r, c]):
return False
return True
def line_of_sight(a, b, avoid_near):
"""셀 a 에서 b 까지 직선이 막히지 않았는가 (0.1 셀 간격으로 검사)"""
# TODO 1: n = ceil(거리 * 10) 개 점으로 나눠 free() 가 모두 True 이면 True
return False
def smooth(path, avoid_near=False):
"""string pulling: 지금 꼭짓점에서 '직선으로 보이는 가장 먼 꼭짓점' 으로 건너뜀"""
out = [path[0]]
i = 0
# TODO 2: i 가 마지막 전까지 반복
# j 를 마지막 꼭짓점부터 줄여 가며 line_of_sight(path[i], path[j]) 인 가장 큰 j 를 찾고
# (최소 j = i + 1), out 에 path[j] 를 넣고 i = j
return path # 지금은 그대로 돌려줌
for name, nbrs in [("4-connected", NBRS[:4]), ("8-connected", NBRS)]:
p = astar(nbrs)
print(f"{name}: vertices {len(p):3d} turns {turns(p):2d} length {length(p):6.2f} m")
p8 = astar(NBRS)
for avoid in (False, True):
sp = smooth(p8, avoid)
print(f"smoothed (avoid shelf-side cells={avoid}): vertices {len(sp):3d} length {length(sp):6.2f} m")
print(" corners:", sp)해 볼 것
- 4방향 · 8방향 경로의 꼭짓점 수 · 꺾임 수 · 길이를 비교합니다. 4방향이 꺾임은 적은데 왜 더 긴가요?
- 매끄럽게 한 경로(선반 옆 셀 허용)의 꼭짓점 좌표를 시뮬레이터 2 에서 경로 매끄럽게 를 켰을 때의 초록 선과 비교합니다.
avoid_near=True로 하면 꼭짓점이 왜 거의 줄지 않나요? A* 경로가 처음부터 어디를 지나가는지 보고 설명합니다.- (도전) 실습 2-1 의
use_cost=TrueA* 경로를 가져와avoid_near=True로 매끄럽게 해 봅니다.
import heapq
import math
import numpy as np
# 실습 2-1 과 같은 창고 지도 (40 x 24 셀, 0.25 m). A* 는 완성본을 씁니다.
CELL = 0.25
ROWS, COLS = 24, 40
grid = np.zeros((ROWS, COLS), bool)
grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = True
for r0, c0, c1 in [(4, 3, 30), (9, 9, 17), (9, 21, 36), (14, 3, 30), (19, 9, 17), (19, 21, 36)]:
grid[r0:r0 + 2, c0:c1 + 1] = True
S, G = (2, 21), (36, 2)
near1 = np.zeros_like(grid)
for dr in (-1, 0, 1):
for dc in (-1, 0, 1):
near1 |= np.roll(np.roll(grid, dr, 0), dc, 1)
near1 &= ~grid # 선반에 붙은 빈 셀
SQ2 = math.sqrt(2)
NBRS = [(1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
(1, 1, SQ2), (1, -1, SQ2), (-1, 1, SQ2), (-1, -1, SQ2)]
def astar(nbrs):
def h(c, r):
dx, dy = abs(c - G[0]), abs(r - G[1])
return max(dx, dy) + (SQ2 - 1) * min(dx, dy) if len(nbrs) == 8 else dx + dy
g, parent, closed, pq = {S: 0.0}, {}, set(), [(h(*S), 0.0, S)]
while pq:
f, gc, node = heapq.heappop(pq)
if node in closed:
continue
closed.add(node)
if node == G:
break
c, r = node
for dc, dr, L in nbrs:
nc, nr = c + dc, r + dr
if grid[nr, nc] or (dc and dr and (grid[r, nc] or grid[nr, c])):
continue
if gc + L < g.get((nc, nr), math.inf):
g[(nc, nr)] = gc + L
parent[(nc, nr)] = node
heapq.heappush(pq, (gc + L + h(nc, nr), gc + L, (nc, nr)))
path = [G]
while path[-1] != S:
path.append(parent[path[-1]])
return path[::-1]
def length(path):
return sum(math.hypot(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])) * CELL
def turns(path):
d = [(b[0] - a[0], b[1] - a[1]) for a, b in zip(path, path[1:])]
return sum(1 for u, v in zip(d, d[1:]) if u != v)
def free(x, y, avoid_near):
"""점 (x, y) (셀 좌표, 실수) 둘레 +-0.35 셀이 모두 빈 셀인가. avoid_near 이면 선반에 붙은 셀도 피함"""
for ox in (-0.35, 0.35):
for oy in (-0.35, 0.35):
c, r = int(round(x + ox)), int(round(y + oy))
if grid[r, c] or (avoid_near and near1[r, c]):
return False
return True
def line_of_sight(a, b, avoid_near):
"""셀 a 에서 b 까지 직선이 막히지 않았는가 (0.1 셀 간격으로 검사)"""
n = max(1, math.ceil(math.hypot(b[0] - a[0], b[1] - a[1]) * 10))
return all(free(a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n, avoid_near)
for k in range(n + 1))
def smooth(path, avoid_near=False):
"""string pulling: 지금 꼭짓점에서 '직선으로 보이는 가장 먼 꼭짓점' 으로 건너뜀"""
out = [path[0]]
i = 0
while i < len(path) - 1:
j = len(path) - 1
while j > i + 1 and not line_of_sight(path[i], path[j], avoid_near):
j -= 1
out.append(path[j])
i = j
return out
for name, nbrs in [("4-connected", NBRS[:4]), ("8-connected", NBRS)]:
p = astar(nbrs)
print(f"{name}: vertices {len(p):3d} turns {turns(p):2d} length {length(p):6.2f} m")
p8 = astar(NBRS)
for avoid in (False, True):
sp = smooth(p8, avoid)
print(f"smoothed (avoid shelf-side cells={avoid}): vertices {len(sp):3d} length {length(sp):6.2f} m")
print(" corners:", sp)- 예시 결과: 4방향 54 개 꼭짓점 · 13.25 m, 8방향 43 개 · 11.64 m. 매끄럽게 하면 5 개 · 11.33 m 로 짧아지고 꺾임도 3 번뿐입니다.
- 선반 옆 셀을 피하게 하면 34 개 · 11.52 m 로 거의 줄지 않습니다. 코스트를 모르는 A* 경로가 처음부터 선반에 딱 붙어 지나가기 때문입니다. 코스트를 반영해 계획한 뒤 매끄럽게 해야 안전한 매끄러운 경로가 나옵니다. Nav2 의 smoother_server(
SimpleSmoother)도 계획기가 낸 경로를 다듬는 별도 단계입니다.
시뮬레이터 3 과 같은 S 자 통로 · 지연 0.2 s 모델에서 전방향 추종기를 완성하고, 최대 횡방향 오차 5 cm 이내로 가장 빨리 도착하는 (L, v_max, 가속도)를 찾습니다.
- TODO 1 목표점 쪽 속도 벡터 (vx, vy)
- TODO 2 가속도 한계 (속도 변화량의 크기를 acc · DT 로 자름)
- TODO 3 머리 방향 P 제어 ω (고정 0° 또는 경로 방향)
import math
import numpy as np
# 시뮬레이터 3 과 같은 모델: 제어 주기 0.05 s, 명령은 DELAY 주기 늦게 반영, 가속도 한계
DT = 0.05
W_MAX, K_TH = 1.0, 2.0
def make_s_path(ds=0.01):
"""S 자 통로: 직선 -> 왼쪽 90도(R 0.6) -> 오른쪽 90도(R 0.6) -> 직선"""
pts = []
def line(a, b):
n = max(1, round(math.hypot(b[0] - a[0], b[1] - a[1]) / ds))
pts.extend((a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n))
def arc(c, r, a0, a1):
n = max(1, round(abs(a1 - a0) * r / ds))
pts.extend((c[0] + r * math.cos(a0 + (a1 - a0) * k / n), c[1] + r * math.sin(a0 + (a1 - a0) * k / n)) for k in range(n))
line((0.5, 0.8), (1.8, 0.8))
arc((1.8, 1.4), 0.6, -math.pi / 2, 0)
arc((3.0, 1.4), 0.6, math.pi, math.pi / 2)
line((3.0, 2.0), (5.5, 2.0))
pts.append((5.5, 2.0))
return np.array(pts)
PATH = make_s_path()
SEG = np.diff(PATH, axis=0)
SLEN = np.hypot(SEG[:, 0], SEG[:, 1])
S_AT = np.concatenate([[0.0], np.cumsum(SLEN)])
TOTAL = S_AT[-1]
def wrap(a):
return math.atan2(math.sin(a), math.cos(a))
def closest(x, y, i0):
"""(가장 가까운 선분 번호, 선분 안 비율 t, 부호 있는 횡방향 오차 e) e > 0 이면 경로 왼쪽"""
lo, hi = max(0, i0 - 20), min(len(SEG), i0 + 80) # 지난번 근처만 찾음
A, D, L2 = PATH[lo:hi], SEG[lo:hi], SLEN[lo:hi] ** 2
t = np.clip(((x - A[:, 0]) * D[:, 0] + (y - A[:, 1]) * D[:, 1]) / L2, 0.0, 1.0)
d = np.hypot(x - A[:, 0] - t * D[:, 0], y - A[:, 1] - t * D[:, 1])
k = int(np.argmin(d))
i = lo + k
e = (SEG[i][0] * (y - PATH[i][1]) - SEG[i][1] * (x - PATH[i][0])) / SLEN[i]
return i, float(t[k]), float(e)
def point_at(s):
"""경로를 따라 거리 s 인 점"""
s = min(s, TOTAL)
i = min(int(np.searchsorted(S_AT, s, side="right")) - 1, len(SEG) - 1)
t = (s - S_AT[i]) / SLEN[i]
return PATH[i] + t * SEG[i]
def follow(L, v_max, acc, delay=0.2, heading="fixed", t_max=60.0):
"""전방향 추종기. (최대 |e| m, 흔들림 횟수, 도착 시간 s) 를 돌려줌"""
x, y = PATH[0]
th = vx = vy = 0.0
queue = [(0.0, 0.0, 0.0)] * round(delay / DT)
idx, t, errs = 0, 0.0, []
while t < t_max:
idx, tt, e = closest(x, y, idx)
s_here = S_AT[idx] + tt * SLEN[idx]
remain = TOTAL - s_here
dist_end = math.hypot(x - PATH[-1][0], y - PATH[-1][1])
v_now = math.hypot(vx, vy)
errs.append(e)
if v_now < 0.02 and dist_end < 0.03:
break
target = point_at(s_here + L)
# 끝에서 멈출 수 있는 속도 (지연 동안 더 가는 거리까지 고려)
speed = min(v_max, math.sqrt(2 * acc * max(remain - v_now * delay, 0.0)))
if remain < 0.03:
speed = min(v_max, 1.5 * dist_end)
# TODO 1: 목표점 방향 단위벡터 (ux, uy) 를 구하고, 원하는 속도 (tvx, tvy) = speed * (ux, uy)
# TODO 2: 가속도 한계 - 속도 변화 (tvx - vx, tvy - vy) 의 크기가 acc * DT 를 넘으면 그 크기로 줄여
# 명령 (cvx, cvy) = (vx, vy) + 변화
# TODO 3: 머리 방향 - heading == "fixed" 이면 0, 아니면 경로 방향 atan2(SEG[idx][1], SEG[idx][0])
# cw = K_TH * wrap(원하는 방향 - th) 를 +-W_MAX 로 자름
cvx, cvy, cw = 0.0, 0.0, 0.0
queue.append((cvx, cvy, cw))
vx, vy, w = queue.pop(0) # 지연된 명령이 실제로 반영됨
x += vx * DT
y += vy * DT
th = wrap(th + w * DT)
t += DT
e = np.array(errs)
big = np.sign(e[np.abs(e) > 0.01])
wobble = int(np.sum(big[1:] != big[:-1])) if len(big) > 1 else 0
return float(np.abs(e).max()), wobble, t
# 1) lookahead 만 바꿔 보기 (v 0.5, 가속도 2.5, 지연 0.2)
print(" L(m) max|e|(cm) wobble time(s)")
for L in [0.05, 0.1, 0.2, 0.3, 0.4, 0.6]:
mx, wob, t = follow(L, 0.5, 2.5)
print(f"{L:5.2f} {mx * 100:8.1f} {wob:5d} {t:6.1f}")
# 2) 목표: max|e| <= 5 cm 이면서 가장 빨리 도착하는 (L, v_max, acc)
best = None
for acc in [2.5, 3.0]:
for v in [0.4, 0.5, 0.6]:
for L in [0.15, 0.2, 0.3]:
mx, wob, t = follow(L, v, acc)
if mx <= 0.05 and t < 59 and (best is None or t < best[0]):
best = (t, L, v, acc, mx)
if best:
t, L, v, acc, mx = best
print(f"best: L={L} v_max={v} acc={acc} -> time {t:.1f} s, max|e| {mx * 100:.1f} cm")
else:
print("no setting passed")해 볼 것
- 첫 표(L 만 바꿈)가 시뮬레이터 3 의 값(L 0.05 → 14.3 cm 흔들림 8 번, L 0.2 → 0.7 cm, L 0.6 → 12.8 cm)과 같은지 확인합니다.
follow(..., heading="path")로 바꿔도 오차가 같은 이유를 설명합니다 (위치와 머리 방향이 분리되어 있음).delay=0.0·0.3으로 같은 탐색을 해 봅니다. 지연이 커지면 가장 좋은 L 은 길어지나요, 짧아지나요?- 찾은 설정을 시뮬레이터 3 에 넣고 ▶ 재생으로 확인합니다.
import math
import numpy as np
# 시뮬레이터 3 과 같은 모델: 제어 주기 0.05 s, 명령은 DELAY 주기 늦게 반영, 가속도 한계
DT = 0.05
W_MAX, K_TH = 1.0, 2.0
def make_s_path(ds=0.01):
"""S 자 통로: 직선 -> 왼쪽 90도(R 0.6) -> 오른쪽 90도(R 0.6) -> 직선"""
pts = []
def line(a, b):
n = max(1, round(math.hypot(b[0] - a[0], b[1] - a[1]) / ds))
pts.extend((a[0] + (b[0] - a[0]) * k / n, a[1] + (b[1] - a[1]) * k / n) for k in range(n))
def arc(c, r, a0, a1):
n = max(1, round(abs(a1 - a0) * r / ds))
pts.extend((c[0] + r * math.cos(a0 + (a1 - a0) * k / n), c[1] + r * math.sin(a0 + (a1 - a0) * k / n)) for k in range(n))
line((0.5, 0.8), (1.8, 0.8))
arc((1.8, 1.4), 0.6, -math.pi / 2, 0)
arc((3.0, 1.4), 0.6, math.pi, math.pi / 2)
line((3.0, 2.0), (5.5, 2.0))
pts.append((5.5, 2.0))
return np.array(pts)
PATH = make_s_path()
SEG = np.diff(PATH, axis=0)
SLEN = np.hypot(SEG[:, 0], SEG[:, 1])
S_AT = np.concatenate([[0.0], np.cumsum(SLEN)])
TOTAL = S_AT[-1]
def wrap(a):
return math.atan2(math.sin(a), math.cos(a))
def closest(x, y, i0):
"""(가장 가까운 선분 번호, 선분 안 비율 t, 부호 있는 횡방향 오차 e) e > 0 이면 경로 왼쪽"""
lo, hi = max(0, i0 - 20), min(len(SEG), i0 + 80) # 지난번 근처만 찾음
A, D, L2 = PATH[lo:hi], SEG[lo:hi], SLEN[lo:hi] ** 2
t = np.clip(((x - A[:, 0]) * D[:, 0] + (y - A[:, 1]) * D[:, 1]) / L2, 0.0, 1.0)
d = np.hypot(x - A[:, 0] - t * D[:, 0], y - A[:, 1] - t * D[:, 1])
k = int(np.argmin(d))
i = lo + k
e = (SEG[i][0] * (y - PATH[i][1]) - SEG[i][1] * (x - PATH[i][0])) / SLEN[i]
return i, float(t[k]), float(e)
def point_at(s):
"""경로를 따라 거리 s 인 점"""
s = min(s, TOTAL)
i = min(int(np.searchsorted(S_AT, s, side="right")) - 1, len(SEG) - 1)
t = (s - S_AT[i]) / SLEN[i]
return PATH[i] + t * SEG[i]
def follow(L, v_max, acc, delay=0.2, heading="fixed", t_max=60.0):
"""전방향 추종기. (최대 |e| m, 흔들림 횟수, 도착 시간 s) 를 돌려줌"""
x, y = PATH[0]
th = vx = vy = 0.0
queue = [(0.0, 0.0, 0.0)] * round(delay / DT)
idx, t, errs = 0, 0.0, []
while t < t_max:
idx, tt, e = closest(x, y, idx)
s_here = S_AT[idx] + tt * SLEN[idx]
remain = TOTAL - s_here
dist_end = math.hypot(x - PATH[-1][0], y - PATH[-1][1])
v_now = math.hypot(vx, vy)
errs.append(e)
if v_now < 0.02 and dist_end < 0.03:
break
target = point_at(s_here + L)
# 끝에서 멈출 수 있는 속도 (지연 동안 더 가는 거리까지 고려)
speed = min(v_max, math.sqrt(2 * acc * max(remain - v_now * delay, 0.0)))
if remain < 0.03:
speed = min(v_max, 1.5 * dist_end)
dx, dy = target[0] - x, target[1] - y
n = math.hypot(dx, dy) or 1.0
tvx, tvy = speed * dx / n, speed * dy / n # 목표점 쪽 속도 벡터
ax, ay = tvx - vx, tvy - vy
an, lim = math.hypot(ax, ay), acc * DT
if an > lim: # 가속도 한계
ax, ay = ax * lim / an, ay * lim / an
cvx, cvy = vx + ax, vy + ay
th_des = 0.0 if heading == "fixed" else math.atan2(SEG[idx][1], SEG[idx][0])
cw = max(-W_MAX, min(W_MAX, K_TH * wrap(th_des - th))) # 머리 방향은 따로 P 제어
queue.append((cvx, cvy, cw))
vx, vy, w = queue.pop(0) # 지연된 명령이 실제로 반영됨
x += vx * DT
y += vy * DT
th = wrap(th + w * DT)
t += DT
e = np.array(errs)
big = np.sign(e[np.abs(e) > 0.01])
wobble = int(np.sum(big[1:] != big[:-1])) if len(big) > 1 else 0
return float(np.abs(e).max()), wobble, t
# 1) lookahead 만 바꿔 보기 (v 0.5, 가속도 2.5, 지연 0.2)
print(" L(m) max|e|(cm) wobble time(s)")
for L in [0.05, 0.1, 0.2, 0.3, 0.4, 0.6]:
mx, wob, t = follow(L, 0.5, 2.5)
print(f"{L:5.2f} {mx * 100:8.1f} {wob:5d} {t:6.1f}")
# 2) 목표: max|e| <= 5 cm 이면서 가장 빨리 도착하는 (L, v_max, acc)
best = None
for acc in [2.5, 3.0]:
for v in [0.4, 0.5, 0.6]:
for L in [0.15, 0.2, 0.3]:
mx, wob, t = follow(L, v, acc)
if mx <= 0.05 and t < 59 and (best is None or t < best[0]):
best = (t, L, v, acc, mx)
if best:
t, L, v, acc, mx = best
print(f"best: L={L} v_max={v} acc={acc} -> time {t:.1f} s, max|e| {mx * 100:.1f} cm")
else:
print("no setting passed")- 예시 결과: L 만 바꾸면 0.2 m 근처가 가장 좋습니다(0.7 cm). 속도까지 함께 찾으면 L 0.3 · v_max 0.6 · 가속도 3.0 → 12.5 s, 최대 1.2 cm 가 이 탐색 범위에서 가장 빠릅니다. v_max 0.6 에서 L 0.2 는 7 cm 로 실패하는데, 빠를수록 지연 동안 더 많이 가므로 L 을 조금 늘려야 진동이 줄어듭니다. 같은 이유로 Regulated Pure Pursuit 는 속도에 비례해 lookahead 를 늘리는 옵션(
use_velocity_scaled_lookahead_dist)을 둡니다. - 머리 방향을 경로 방향으로 바꿔도 위치 오차는 똑같습니다. 전방향 로봇은 (vx, vy) 와 ω 가 독립이기 때문입니다. 차동 로봇에서는 그렇지 않습니다(시뮬레이터 3 의 차동 모드).
시뮬레이터 4 의 "사람이 마주 걸어옴 · 지역 우회" 를 단순화한 미니 DWB 입니다. 속도 54 개(vx 6 × ω 9)를 샘플링해 모의 주행하고, 평가 점수가 가장 낮은 것을 고릅니다.
- TODO
dwb_step()의 평가 점수 — PathDist · GoalDist · PathAlign(23 고정) · BaseObstacle 을 가중합합니다. 거리 critic 은 셀(0.05 m) 단위입니다.
예측 먼저 — 세 설정의 도착 시간과 사람까지 최소 여유의 순서를 적어 둡니다.
import math
import numpy as np
# 시뮬레이터 4 의 "사람이 마주 걸어옴 + 지역 우회" 를 단순화한 미니 DWB
DT, R_INS, RES = 0.1, 0.18, 0.05
ACC, ACC_W, W_MAX, SENSE = 2.5, 3.2, 0.5, 2.5
BOXES = np.array([[0, 0, 10, 0.1], [0, 4.9, 10, 5], [0, 0, 0.1, 5], [9.9, 0, 10, 5],
[2.0, 1.9, 4.6, 3.4], [5.4, 1.9, 8.0, 3.4]]) # 벽 · 선반
GOAL = np.array([9.2, 1.0])
PATH = np.column_stack([np.arange(0.8, 9.2001, 0.1), np.full(85, 1.0)]) # 전역 경로 (직선)
PL = np.concatenate([[0], np.cumsum(np.hypot(*np.diff(PATH, axis=0).T))])
FOOT = np.array([[0.265, 0.18], [0.265, -0.18], [-0.265, -0.18], [-0.265, 0.18], [0, 0.18], [0, -0.18],
[0.265, 0], [-0.265, 0], [0.13, 0.18], [0.13, -0.18], [-0.13, 0.18], [-0.13, -0.18]])
def dist_to_obstacles(px, py, person):
"""점들(px, py 같은 모양 배열)에서 가장 가까운 장애물(벽 · 선반 · 사람 원)까지 거리"""
dx = np.maximum(np.maximum(BOXES[:, 0] - px[..., None], 0), px[..., None] - BOXES[:, 2])
dy = np.maximum(np.maximum(BOXES[:, 1] - py[..., None], 0), py[..., None] - BOXES[:, 3])
d = np.hypot(dx, dy).min(axis=-1)
if person is not None:
d = np.minimum(d, np.hypot(px - person[0], py - person[1]) - 0.25)
return d
def body_clearance(x, y, th, person):
"""차체 둘레 12 점에서 잰 최소 거리 (0 이하 = 충돌)"""
x, y, th = np.atleast_1d(x), np.atleast_1d(y), np.atleast_1d(th)
c, s = np.cos(th)[..., None], np.sin(th)[..., None]
px = x[..., None] + FOOT[:, 0] * c - FOOT[:, 1] * s
py = y[..., None] + FOOT[:, 0] * s + FOOT[:, 1] * c
return dist_to_obstacles(px, py, person).min(axis=-1)
def cell_cost(d, inflation):
"""코스트맵 비용: 내접 반경 안 254, 팽창 범위 밖 0, 그 사이 252*exp(-4(d-0.18))"""
c = 252 * np.exp(-4.0 * (d - R_INS))
return np.where(d <= R_INS, 254.0, np.where(d > inflation, 0.0, c))
def nearest_on_path(px, py):
d = np.hypot(px[:, None] - PATH[:, 0], py[:, None] - PATH[:, 1])
j = d.argmin(axis=1)
return j, d[np.arange(len(px)), j]
def dwb_step(robot, person_seen, cfg):
"""속도 샘플 -> 모의 주행 -> 평가. 고른 (vx, w) 또는 None (유효한 궤적 없음)"""
x0, y0, th0, vx0, w0 = robot
vxs = np.linspace(max(0, vx0 - ACC * DT), min(cfg["vmax"], vx0 + ACC * DT), 6)
ws = np.linspace(max(-W_MAX, w0 - ACC_W * DT), min(W_MAX, w0 + ACC_W * DT), 9)
VX, WW = [a.ravel() for a in np.meshgrid(vxs, ws)]
n = max(1, round(cfg["sim_time"] / DT))
k = np.arange(1, n + 1)
TH = th0 + WW[:, None] * DT * k # (궤적 수, 단계 수)
X = x0 + np.cumsum(VX[:, None] * np.cos(TH) * DT, axis=1)
Y = y0 + np.cumsum(VX[:, None] * np.sin(TH) * DT, axis=1)
d = dist_to_obstacles(X, Y, person_seen)
lethal = d <= R_INS
near = (d < 0.32) & ~lethal # 모서리가 닿는지 차체 모양으로 확인
if near.any():
lethal[near] = body_clearance(X[near], Y[near], TH[near], person_seen) <= 0.01
valid = ~lethal.any(axis=1)
if not valid.any():
return None
obstacle = cell_cost(d, cfg["inflation"]).sum(axis=1) # BaseObstacle
j, path_d = nearest_on_path(X[:, -1], Y[:, -1]) # PathDist 용
goal_d = PL[-1] - PL[j] + path_d # GoalDist 용
_, align_d = nearest_on_path(X[:, -1] + 0.1 * np.cos(TH[:, -1]), # PathAlign 용 (scale 23 고정)
Y[:, -1] + 0.1 * np.sin(TH[:, -1]))
# TODO: 평가 점수 = PathDist scale * path_d/RES + GoalDist scale * goal_d/RES
# + 23 * align_d/RES + BaseObstacle scale * obstacle
# (cfg["path"], cfg["goal"], cfg["obstacle"] 를 씁니다)
score = np.zeros(len(VX))
score[~valid] = np.inf
b = int(np.argmin(score))
return VX[b], WW[b]
def run(cfg, t_max=60.0):
x, y, th, vx, w = 0.8, 1.0, 0.0, 0.0, 0.0
person = {"x": 8.8, "y": 1.0, "stop": 0.0, "side": False, "ty": None}
t, min_clear, no_valid = 0.0, 9.9, 0
while t < t_max:
p = (person["x"], person["y"]) if person["x"] > 0.4 else None
seen = p if p is not None and math.hypot(p[0] - x, p[1] - y) - 0.25 < SENSE else None
cmd = dwb_step((x, y, th, vx, w), seen, cfg)
if cmd is None:
no_valid += 1
cmd = (max(0.0, vx - ACC * DT), 0.0)
vx, w = cmd
th += w * DT
x += vx * math.cos(th) * DT
y += vx * math.sin(th) * DT
t = round(t + DT, 1)
if p is not None: # 사람: 가까우면 멈추고, 2 s 뒤 옆으로 비켜 지나감
clr = float(body_clearance(x, y, th, p)[0])
if not person["side"]:
if clr < 0.4 and person["x"] > x - 0.3:
person["stop"] += DT
if person["stop"] > 2:
person["side"], person["ty"] = True, (0.4 if y > person["y"] else 1.6)
else:
person["x"] -= 0.5 * DT
elif abs(person["y"] - person["ty"]) > 1e-6:
oy = person["y"]
person["y"] += max(-0.4 * DT, min(0.4 * DT, person["ty"] - oy))
if body_clearance(x, y, th, (person["x"], person["y"]))[0] < 0.02:
person["y"] = oy
elif clr > 0.05 or person["x"] < x - 0.3:
person["x"] -= 0.5 * DT
min_clear = min(min_clear, float(body_clearance(x, y, th, (person["x"], person["y"]))[0]))
if math.hypot(x - GOAL[0], y - GOAL[1]) < 0.25:
return t, min_clear, no_valid
return None, min_clear, no_valid
CONFIGS = {
"agvpro.yaml": dict(vmax=0.26, sim_time=1.7, path=32, goal=24, obstacle=0.02, inflation=0.30),
"fast but risky": dict(vmax=1.0, sim_time=0.6, path=32, goal=48, obstacle=0.0, inflation=0.30),
"slow but safe": dict(vmax=0.4, sim_time=2.5, path=24, goal=24, obstacle=0.5, inflation=0.80),
}
print("config arrival(s) min clearance to person(cm) no-valid")
for name, cfg in CONFIGS.items():
t, clr, nv = run(cfg)
arrival = f"{t:6.1f}" if t is not None else " fail"
print(f"{name:16s} {arrival} {clr * 100:8.1f} {nv:4d}")해 볼 것
- 결과를 시뮬레이터 4 의 예시 결과 표와 비교합니다 (모델이 단순해서 1 ~ 3 cm 정도 다를 수 있음).
- "slow but safe" 에서
inflation만 0.30 으로 낮춰 봅니다. 최소 여유는 어떻게 되나요?obstacle을 0.5 → 2.0 으로 올리면? - "fast but risky" 의
sim_time을 0.6 → 1.5 로만 늘려 봅니다. 빠르기를 크게 잃지 않고 안전해지나요? - 나만의 설정 을 하나 만들어 "도착 20 s 이내 · 최소 여유 15 cm 이상" 을 만족시켜 봅니다.
import math
import numpy as np
# 시뮬레이터 4 의 "사람이 마주 걸어옴 + 지역 우회" 를 단순화한 미니 DWB
DT, R_INS, RES = 0.1, 0.18, 0.05
ACC, ACC_W, W_MAX, SENSE = 2.5, 3.2, 0.5, 2.5
BOXES = np.array([[0, 0, 10, 0.1], [0, 4.9, 10, 5], [0, 0, 0.1, 5], [9.9, 0, 10, 5],
[2.0, 1.9, 4.6, 3.4], [5.4, 1.9, 8.0, 3.4]]) # 벽 · 선반
GOAL = np.array([9.2, 1.0])
PATH = np.column_stack([np.arange(0.8, 9.2001, 0.1), np.full(85, 1.0)]) # 전역 경로 (직선)
PL = np.concatenate([[0], np.cumsum(np.hypot(*np.diff(PATH, axis=0).T))])
FOOT = np.array([[0.265, 0.18], [0.265, -0.18], [-0.265, -0.18], [-0.265, 0.18], [0, 0.18], [0, -0.18],
[0.265, 0], [-0.265, 0], [0.13, 0.18], [0.13, -0.18], [-0.13, 0.18], [-0.13, -0.18]])
def dist_to_obstacles(px, py, person):
"""점들(px, py 같은 모양 배열)에서 가장 가까운 장애물(벽 · 선반 · 사람 원)까지 거리"""
dx = np.maximum(np.maximum(BOXES[:, 0] - px[..., None], 0), px[..., None] - BOXES[:, 2])
dy = np.maximum(np.maximum(BOXES[:, 1] - py[..., None], 0), py[..., None] - BOXES[:, 3])
d = np.hypot(dx, dy).min(axis=-1)
if person is not None:
d = np.minimum(d, np.hypot(px - person[0], py - person[1]) - 0.25)
return d
def body_clearance(x, y, th, person):
"""차체 둘레 12 점에서 잰 최소 거리 (0 이하 = 충돌)"""
x, y, th = np.atleast_1d(x), np.atleast_1d(y), np.atleast_1d(th)
c, s = np.cos(th)[..., None], np.sin(th)[..., None]
px = x[..., None] + FOOT[:, 0] * c - FOOT[:, 1] * s
py = y[..., None] + FOOT[:, 0] * s + FOOT[:, 1] * c
return dist_to_obstacles(px, py, person).min(axis=-1)
def cell_cost(d, inflation):
"""코스트맵 비용: 내접 반경 안 254, 팽창 범위 밖 0, 그 사이 252*exp(-4(d-0.18))"""
c = 252 * np.exp(-4.0 * (d - R_INS))
return np.where(d <= R_INS, 254.0, np.where(d > inflation, 0.0, c))
def nearest_on_path(px, py):
d = np.hypot(px[:, None] - PATH[:, 0], py[:, None] - PATH[:, 1])
j = d.argmin(axis=1)
return j, d[np.arange(len(px)), j]
def dwb_step(robot, person_seen, cfg):
"""속도 샘플 -> 모의 주행 -> 평가. 고른 (vx, w) 또는 None (유효한 궤적 없음)"""
x0, y0, th0, vx0, w0 = robot
vxs = np.linspace(max(0, vx0 - ACC * DT), min(cfg["vmax"], vx0 + ACC * DT), 6)
ws = np.linspace(max(-W_MAX, w0 - ACC_W * DT), min(W_MAX, w0 + ACC_W * DT), 9)
VX, WW = [a.ravel() for a in np.meshgrid(vxs, ws)]
n = max(1, round(cfg["sim_time"] / DT))
k = np.arange(1, n + 1)
TH = th0 + WW[:, None] * DT * k # (궤적 수, 단계 수)
X = x0 + np.cumsum(VX[:, None] * np.cos(TH) * DT, axis=1)
Y = y0 + np.cumsum(VX[:, None] * np.sin(TH) * DT, axis=1)
d = dist_to_obstacles(X, Y, person_seen)
lethal = d <= R_INS
near = (d < 0.32) & ~lethal # 모서리가 닿는지 차체 모양으로 확인
if near.any():
lethal[near] = body_clearance(X[near], Y[near], TH[near], person_seen) <= 0.01
valid = ~lethal.any(axis=1)
if not valid.any():
return None
obstacle = cell_cost(d, cfg["inflation"]).sum(axis=1) # BaseObstacle
j, path_d = nearest_on_path(X[:, -1], Y[:, -1]) # PathDist 용
goal_d = PL[-1] - PL[j] + path_d # GoalDist 용
_, align_d = nearest_on_path(X[:, -1] + 0.1 * np.cos(TH[:, -1]), # PathAlign 용 (scale 23 고정)
Y[:, -1] + 0.1 * np.sin(TH[:, -1]))
score = (cfg["path"] * path_d / RES + cfg["goal"] * goal_d / RES
+ 23 * align_d / RES + cfg["obstacle"] * obstacle)
score[~valid] = np.inf
b = int(np.argmin(score))
return VX[b], WW[b]
def run(cfg, t_max=60.0):
x, y, th, vx, w = 0.8, 1.0, 0.0, 0.0, 0.0
person = {"x": 8.8, "y": 1.0, "stop": 0.0, "side": False, "ty": None}
t, min_clear, no_valid = 0.0, 9.9, 0
while t < t_max:
p = (person["x"], person["y"]) if person["x"] > 0.4 else None
seen = p if p is not None and math.hypot(p[0] - x, p[1] - y) - 0.25 < SENSE else None
cmd = dwb_step((x, y, th, vx, w), seen, cfg)
if cmd is None:
no_valid += 1
cmd = (max(0.0, vx - ACC * DT), 0.0)
vx, w = cmd
th += w * DT
x += vx * math.cos(th) * DT
y += vx * math.sin(th) * DT
t = round(t + DT, 1)
if p is not None: # 사람: 가까우면 멈추고, 2 s 뒤 옆으로 비켜 지나감
clr = float(body_clearance(x, y, th, p)[0])
if not person["side"]:
if clr < 0.4 and person["x"] > x - 0.3:
person["stop"] += DT
if person["stop"] > 2:
person["side"], person["ty"] = True, (0.4 if y > person["y"] else 1.6)
else:
person["x"] -= 0.5 * DT
elif abs(person["y"] - person["ty"]) > 1e-6:
oy = person["y"]
person["y"] += max(-0.4 * DT, min(0.4 * DT, person["ty"] - oy))
if body_clearance(x, y, th, (person["x"], person["y"]))[0] < 0.02:
person["y"] = oy
elif clr > 0.05 or person["x"] < x - 0.3:
person["x"] -= 0.5 * DT
min_clear = min(min_clear, float(body_clearance(x, y, th, (person["x"], person["y"]))[0]))
if math.hypot(x - GOAL[0], y - GOAL[1]) < 0.25:
return t, min_clear, no_valid
return None, min_clear, no_valid
CONFIGS = {
"agvpro.yaml": dict(vmax=0.26, sim_time=1.7, path=32, goal=24, obstacle=0.02, inflation=0.30),
"fast but risky": dict(vmax=1.0, sim_time=0.6, path=32, goal=48, obstacle=0.0, inflation=0.30),
"slow but safe": dict(vmax=0.4, sim_time=2.5, path=24, goal=24, obstacle=0.5, inflation=0.80),
}
print("config arrival(s) min clearance to person(cm) no-valid")
for name, cfg in CONFIGS.items():
t, clr, nv = run(cfg)
arrival = f"{t:6.1f}" if t is not None else " fail"
print(f"{name:16s} {arrival} {clr * 100:8.1f} {nv:4d}")- 예시 결과: agvpro.yaml 34.7 s · 11.4 cm, fast but risky 11.4 s · 2.1 cm, slow but safe 28.9 s · 17.0 cm. "빠르지만 위험" 은 속도가 4 배인 데다 sim_time 0.6 s 로 0.6 m 앞까지만 보고 BaseObstacle 이 0 이라, 사람과 2 cm 를 두고 스칩니다. 실제 현장이라면 안전 영역(ch05)이 먼저 로봇을 세웠을 거리입니다.
- "느리지만 안전" 의 여유는 BaseObstacle 0.5 와 팽창 0.8 이 함께 만든 것입니다. 팽창을 0.30 으로 낮추면 장애물 비용이 거의 0 이 되어 가중치가 의미를 잃습니다(확인 문제 4-1).
창고 시뮬레이터로 확인하기#
Playground 의 타깃을 Simulator 로 두면 공식 URDF 로 만든 3D 창고 시뮬레이터에 from pymycobot import MyAGVPro 가 그대로 연결됩니다. agvsim 은 참값 위치 · 스캔 · 지도처럼 실물에는 없는 정보를 읽는 시뮬레이터 전용 모듈입니다 (실물에서는 ROS 2 토픽이 그 역할). 가속 한계 · 미끄러짐 · 보호 영역 거리 등은 교육용 가정입니다. 실물의 Nav2 는 속도 명령을 /cmd_vel 로 내므로, 여기서는 같은 뜻의 agvsim.cmd_vel(vx, vy, ω) 를 씁니다.
작업장 출발점에서 기둥 왼쪽을 돌아 스테이션 A 까지, 미리 정한 경로를 lookahead 0.5 m 로 따라갑니다. 차동 로봇과 달리 헤딩은 0° 로 고정하고 옆으로 미끄러지듯 갑니다.
먼저 예측하세요. 꺾이는 모서리(2.5, 0) 에서 로봇은 모서리를 정확히 지날까요, 안쪽으로 질러갈까요? lookahead 를 1.0 m 로 늘리면?
from pymycobot import MyAGVPro
import agvsim, time, math
agv = MyAGVPro("/dev/agvpro_controller", baudrate=1000000)
if agv.is_power_on() != 1:
agv.power_on()
# 작업장(room): 출발점 -> 기둥 왼쪽을 돌아 스테이션 A (5.5, 2.6)
PATH = [(0.0, 0.0), (2.5, 0.0), (2.5, 2.0), (5.5, 2.0), (5.5, 2.6)]
LOOKAHEAD, VMAX, HEADING = 0.5, 0.4, 0.0 # 헤딩은 0 으로 고정 - 옆으로 미끄러져 감
def carrot(x, y):
"""경로 위에서 로봇과 가장 가까운 점보다 LOOKAHEAD 만큼 앞의 점"""
best, best_s, s0 = 1e9, 0.0, 0.0
for (ax, ay), (bx, by) in zip(PATH, PATH[1:]):
L = math.hypot(bx - ax, by - ay)
u = max(0.0, min(1.0, ((x - ax) * (bx - ax) + (y - ay) * (by - ay)) / (L * L)))
d = math.hypot(ax + u * (bx - ax) - x, ay + u * (by - ay) - y)
if d < best:
best, best_s = d, s0 + u * L
s0 += L
s = best_s + LOOKAHEAD
for (ax, ay), (bx, by) in zip(PATH, PATH[1:]):
L = math.hypot(bx - ax, by - ay)
if s <= L:
return ax + (bx - ax) * s / L, ay + (by - ay) * s / L
s -= L
return PATH[-1]
agvsim.set_pose(0, 0, 0)
t0, last = time.time(), -1
while time.time() - t0 < 45:
x, y, th = agvsim.get_pose() # 위치 추정(AMCL) 대신 참값 사용
gx, gy = PATH[-1]
if math.hypot(gx - x, gy - y) < 0.05:
break
cx, cy = carrot(x, y)
ex, ey = cx - x, cy - y # 지도 좌표의 오차
# TODO: 지도 좌표 오차 (ex, ey) 를 로봇 몸체 좌표 (bx, by) 로 돌리기 (-th 회전)
bx = ex * math.cos(th) + ey * math.sin(th)
by = -ex * math.sin(th) + ey * math.cos(th)
d = math.hypot(bx, by)
v = min(VMAX, 1.2 * math.hypot(gx - x, gy - y)) # 목표 근처에서 감속
w = 1.5 * math.atan2(math.sin(HEADING - th), math.cos(HEADING - th))
agvsim.cmd_vel(v * bx / d, v * by / d, w)
if int(time.time() - t0) != last:
last = int(time.time() - t0)
print(f"t={last:2d}s pose=({x:+.2f},{y:+.2f},{math.degrees(th):+5.1f}deg) carrot=({cx:.2f},{cy:.2f})")
time.sleep(0.05)
agvsim.stop()
x, y, th = agvsim.get_pose()
print(f"arrived ({x:.2f}, {y:.2f}) in {time.time() - t0:.1f} s")관찰 · 해설
모서리 근처에서 carrot 이 이미 다음 구간에 올라가 있으므로 로봇은 **안쪽으로 질러갑니다** (출력에서 세로 구간 초반의 x 가 2.5 를 조금 넘었다 돌아오는 부분). lookahead 를 키우면 더 크게 질러가 기둥 · 선반에 가까워지고, 줄이면 모서리를 정확히 돌지만 흔들림이 커집니다 — 3교시의 lookahead 트레이드오프입니다. 헤딩 고정 덕분에 회전 없이 옆으로 가므로, 같은 경로를 차동 로봇으로 갈 때보다 시간이 짧습니다. 목표 근처의 감속은 보호 영역(벽까지 0.8 m)의 감속과 겹쳐 보일 수 있습니다.시뮬레이터를 새 탭으로 열고 같은 코드를 실행하는 동안, 로봇 앞을 가로막도록 다른 경로를 계획해 보세요: 경로의 경유점을 상자(4.6, −0.3) 쪽으로 지나게 바꾸고 실행합니다. 보호 영역이 켜져 있으면 감속 · 정지하고, 패널에서 보호 영역 을 끄면 범퍼에 부딪힙니다. 지역 계획기(4교시)가 없으면 경로 추종기는 "막혀도 계속 밀기만" 한다는 것을 확인하고, 막혔을 때 다시 계획하는 조건(예: 1 s 이상 속도 0)을 코드에 넣어 보세요.
자주 나는 오류와 해결#
1) 전역 경로가 만들어지지 않음 (Failed to create plan)#
[planner_server]: GridBased plugin failed to plan calculation to (5.40, 1.20): "Failed to create plan with tolerance of: 2.000000"- 원인: 출발 · 목표 셀이 253 이상이거나, 그 사이의 모든 통로가 253 이상으로 막혔습니다(1교시). 원형
robot_radius를 크게 잡았거나footprint_padding을 늘린 경우, 지도에 남은 잡음 점(ch06)이 통로를 막은 경우가 흔합니다. 문구는 Nav2 버전에 따라 다를 수 있습니다. - 해결: RViz 에서 global_costmap 을 켜고 통로가 보라(253)로 막혔는지 봅니다. footprint 를 실제 사각형으로 적었는지, 내접 반경이 통로 폭의 절반보다 작은지 계산합니다. 막힌 원인이 지나간 장애물이면
clear_entirely_global_costmap서비스로 지우고 다시 계획합니다.
2) inflation_radius 가 내접 반경보다 작다는 경고#
[global_costmap.global_costmap]: The configured inflation radius (0.100) is smaller than the computed inscribed radius (0.180) of your footprint, it is highly recommended to set inflation radius to be at least as big as the inscribed radius to avoid collisions- 원인: 팽창 범위가 차체 반폭(0.18 m)보다 작아, 차체가 벽에 닿는 셀까지 "자유" 로 보입니다 (시뮬레이터 1 의 해 볼 것 5).
- 해결:
inflation_radius를 내접 반경 이상으로 올립니다. 벽에 너무 붙는 것이 걱정이면 외접 반경(0.316 m) 이상을 권합니다. 통로가 좁아 우회한다면 반경 대신cost_scaling_factor를 키워 "가까이만 비싸게" 만듭니다 (실습 1-1).
3) 경로를 따라가며 좌우로 흔들림 (진동)#
(RViz) 직선 통로인데 로봇이 경로를 중심으로 지그재그로 흔들리며 감. /cmd_vel 의 angular.z 가 + - 로 계속 바뀜- 원인: 추종기가 경로에 너무 급하게 붙으려 합니다. Pure Pursuit 계열은 lookahead 가 짧을 때, DWB 는 PathDist · PathAlign 가중치가 크고 sim_time 이 짧을 때 잘 생깁니다. 위치 추정의 지연 · 튐(ch07)이나 낮은 controller_frequency 가 함께 원인인 경우가 많습니다 (시뮬레이터 3 의 지연 슬라이더).
- 해결: lookahead 를 늘리거나 속도를 낮춥니다. DWB 는 sim_time 을 늘리고 PathDist 를 조금 낮추며,
Oscillationcritic 이 켜져 있는지 확인합니다. TF · AMCL 의 지연을 먼저 줄이는 것도 효과가 큽니다.
4) 목표 근처에서 맴돌거나 제자리 회전만 반복#
[controller_server]: Failed to make progress
[bt_navigator]: Goal failed- 원인: 위치는 거의 도착했는데 방향(
yaw_goal_tolerance: 0.25rad ≈ 14°)을 못 맞추거나,xy_goal_tolerance가 로봇이 실제로 낼 수 있는 정밀도보다 작습니다. 목표 근처에서 RotateToGoal 과 경로 critic 이 서로 다른 방향을 원하면 맴돌고, 8 초 동안 0.15 m 를 못 움직이면 progress_checker 가 실패로 봅니다. - 해결: 목표 허용 오차를 현실적으로(위치 오차의 2 ~ 3 배) 둡니다. 도킹처럼 정밀해야 하면 Nav2 로 근처까지 간 뒤 별도의 정밀 접근 제어(3교시의 머리 고정 P 제어 같은)로 마무리하는 구성이 흔합니다.
5) 유효한 궤적이 없다며 멈춤#
[controller_server]: No valid trajectories out of 1000!
[controller_server]: Controller patience exceeded- 원인: DWB 가 뿌린 모든 궤적이 장애물(253 이상)에 닿았습니다. 좁은 통로에서 팽창이 크거나 sim_time 이 길 때, 로봇이 이미 벽에 너무 붙어 있을 때, 지역 코스트맵에 지나간 장애물의 흔적이 남았을 때 생깁니다 (시뮬레이터 4 의 "느리지만 안전" + 팔레트). 숫자와 문구는 샘플 수 · 버전에 따라 다릅니다.
- 해결: 지역 코스트맵의 팽창 · sim_time 을 줄이거나, 복구 동작(backup · spin)으로 벽에서 떨어지게 합니다. 장애물 흔적이면
clear_entirely_local_costmap을 부르고, 라이다의raytrace_max_range(3.0 m)가 지우기에 충분한지 확인합니다.
6) 전방향 로봇인데 옆으로 가지 않음#
(RViz) 옆 통로로 옮겨 가야 하는데 로봇이 매번 제자리에서 90° 돌고 나서 감. /cmd_vel 의 linear.y 가 항상 0- 원인: agvpro.yaml 의 DWB 가
max_vel_y: 0.0·acc_lim_y: 0.0이고, velocity_smoother 의max_velocity도[0.26, 0.0, 0.5]라서 vy 를 0 으로 자릅니다 (4교시 시뮬레이터의 vy 체크). - 해결: DWB 의
min_vel_y·max_vel_y·acc_lim_y·decel_lim_y·vy_samples와 velocity_smoother 의 y 한계를 함께 고칩니다. 두 곳 중 한 곳만 고치면 여전히 0 이 됩니다. 실제 값과 적용 방법은 ch09 에서 실물로 확인합니다 (확인 필요).
7) Playground 의 A* 에서 비교 오류#
ValueError: The truth value of an array with more than one element is ambiguous. Use a.any() or a.all()- 원인:
heapq.heappush(pq, (f, np.array([c, r])))처럼 numpy 배열을 큐에 넣었습니다. f 가 같은 항목이 생기면 heapq 가 두 번째 값(배열)끼리 비교하다 오류가 납니다. - 해결: 좌표는
(int(c), int(r))같은 파이썬 정수 튜플로 넣고, 실습 2-1 처럼(f, g, (c, r))로 비교 순서를 분명히 합니다.
과제#
- 팽창 설계표: 실습 1-1 을 통로 폭 0.5 · 0.6 · 0.7 · 0.8 m 와 k = 2 · 4 · 8 로 확장해, 조건(좁은 통로 통과 + 기둥 옆 여유 ≥ 10 cm)을 만족하는 inflation_radius 범위를 표로 만들고 "우리 창고의 가장 좁은 통로" 에 맞는 값을 고릅니다.
- 탐색 비교 보고서: 실습 2-1 로 창고 · U 자 · 빈 바닥 세 지도에서 Dijkstra · A*(w = 1, 1.5, 2) · 맨해튼의 탐색 셀 수와 경로 길이를 표로 만들고, "agvpro.yaml 이 Dijkstra(
use_astar: false)를 써도 되는 이유" 를 한 문단으로 씁니다. - 추종기 튜닝 카드: 실습 3-1 로 지연 0 · 0.1 · 0.2 · 0.3 s 마다 5 cm 이내 · 가장 빠른 (L, v_max)를 찾아 표로 만들고, 지연과 최적 L 의 관계를 한 줄로 정리합니다.
- DWB 설정 두 장: 시뮬레이터 4 의 세 상황 × 세 전략을 "빠르지만 위험" · "느리지만 안전" · 나만의 설정으로 돌려 도착 시간 · 최소 여유 · 실패를 표로 만들고, 물류 현장에 쓸 설정 하나를 근거와 함께 고릅니다.
3 ~ 4 명이 한 팀이 되어, ch06 ~ ch08 에서 다룬 것을 하나의 설계서로 묶습니다.
- 지도 (ch06): 가상의 창고(또는 실습실) 평면을 정하고, 가장 좁은 통로 · 사람 통행 구역 · 지게차 교차로 · 도킹 위치를 표시합니다.
- 위치 (ch07): 똑같이 생긴 선반 열처럼 위치 추정이 헷갈릴 곳을 찾고, 초기 위치 · 표식 배치 대책을 적습니다.
- 길 (ch08): 팽창(inflation_radius · cost_scaling_factor)을 가장 좁은 통로에 맞춰 정하고(실습 1-1), 구역별 최고 속도 · lookahead · DWB 가중치를 정합니다. 사람 구역 · 지게차 교차로 · 팔레트 적치 구역에서 쓸 전략(대기 · 우회 · 재계획)을 시뮬레이터 4 결과를 근거로 고릅니다.
- 발표 (5 분): "우리 로봇이 가장 위험한 장면 하나와 그 장면을 위한 파라미터" 를 시뮬레이터 화면과 함께 발표합니다. 실제 Nav2 에 적용하는 것은 ch09 에서 합니다.
참고자료#
- myAGV Pro 공식 문서 (내비게이션 · ROS 2): https://docs.elephantrobotics.com/docs/myAGV_Pro_en/
- myAGV Pro ROS 2 패키지 (
agv_pro_navigation2/param/agvpro.yaml, humble 브랜치): https://github.com/elephantrobotics/agv_pro_ros2 - 제품 페이지: https://www.elephantrobotics.com/en/myagv-pro/
- 제품 소개 · 이미지 (현영이앤씨): https://www.hyenc.net/products/36
- Nav2 Inflation Layer 파라미터: https://docs.nav2.org/configuration/packages/costmap-plugins/inflation.html
- Nav2 Costmap 2D 설정 (footprint · robot_radius · 레이어): https://docs.nav2.org/configuration/packages/configuring-costmaps.html
- Nav2 NavFn Planner: https://docs.nav2.org/configuration/packages/configuring-navfn.html
- Nav2 DWB Controller 와 critic: https://docs.nav2.org/configuration/packages/configuring-dwb-controller.html
- Nav2 Regulated Pure Pursuit: https://docs.nav2.org/configuration/packages/configuring-regulated-pp.html
- Nav2 Behavior Server (복구 동작): https://docs.nav2.org/configuration/packages/configuring-behavior-server.html
- Nav2 튜닝 가이드 (팽창 · footprint · 컨트롤러 고르기): https://docs.nav2.org/tuning/index.html
- P. E. Hart, N. J. Nilsson, B. Raphael, "A Formal Basis for the Heuristic Determination of Minimum Cost Paths", IEEE Trans. SSC, 1968 (A*)
- D. Fox, W. Burgard, S. Thrun, "The Dynamic Window Approach to Collision Avoidance", IEEE Robotics & Automation Magazine, 1997
- R. C. Coulter, "Implementation of the Pure Pursuit Path Tracking Algorithm", CMU-RI-TR-92-01, 1992
- S. Macenski et al., "The Marathon 2: A Navigation System", IROS 2020 (Nav2)
- 이 사이트의 도구: Playground · 물류창고 시뮬레이터 · 3D 뷰어