CH05라이다와 안전 센싱
2D 라이다가 거리를 재는 방식을 레이캐스팅 시뮬레이터로 체험하고, 장애물 감지 · 안전 영역 · 비상정지 로직을 만듭니다.
학습 목표#
- 2D 라이다가 거리를 재는 방식(비행시간 ToF · 삼각측량)을 설명하고,
sensor_msgs/LaserScan의angle_min·angle_increment·ranges로 "i 번째 값이 어느 방향의 거리인지" 계산할 수 있습니다. - 각 해상도 · 최대 거리 · 회전 주파수를 바꾸며 얇은 기둥이 보이는 조건
Δφ ≤ w / d를 시뮬레이터로 찾고, 숫자로 확인할 수 있습니다. - 스캔 배열에서 0 · inf 를 걸러 내고, 극좌표
(r, φ)를 직교좌표(x, y)로 바꾸고, 부채꼴 영역별 최소 거리를 구하는 함수를 만들 수 있습니다. 이동 평균 · 중앙값 필터가 노이즈와 얇은 장애물에 각각 어떤 영향을 주는지 비교할 수 있습니다. - 경고 영역(감속) · 보호 영역(정지)을 정지 거리
v·t_반응 + v²/(2a)로 설계하고, 전방향 로봇은 영역을 진행 방향 으로 돌려야 하는 이유를 시뮬레이터 결과로 설명할 수 있습니다. - 2D 라이다 · 3D 라이다 · 깊이 카메라가 각각 놓치는 물체(낮은 물체 · 돌출물 · 유리 · 검은 물체)를 정리하고, 창고에 필요한 최소 센서 구성을 근거와 함께 제안할 수 있습니다.
이론#
ch01 의 시뮬레이터 3 에서 "라이다는 바닥에서 0.229 m 높이의 한 수평면 만 본다" 는 것을 봤고, ch04 에서는 바퀴만으로 위치를 세면 오차가 쌓인다는 것을 확인했습니다. 이번 차시는 로봇에게 바깥을 보는 눈 을 달아 줍니다. 라이다(LiDAR, Light Detection And Ranging)는 뒤 차시의 지도 만들기 · 위치 추정의 재료이기도 하지만, 그보다 먼저 사람과 물건에 부딪히지 않게 하는 안전 장치 입니다. 이번 차시는 그 안전 쪽에 집중합니다.
| 항목 | 값 | 출처 |
|---|---|---|
| 2D 라이다 | N10 Plus · 360° · 0 ~ 15 m (반사율 70 %) · ±3 cm · 6 ~ 12 Hz | 공식 사양 (ch01) |
| 라이다 위치 | laser_link = base_link 앞 0.179 m, 바닥 기준 0.229 m | URDF lidar_joint (ch01) |
| 깊이 카메라 위치 | camera_link = base_link 앞 0.232 m, 바닥 기준 0.169 m | URDF camera_joint (ch01) |
| 차체 | 길이 0.53 m · 폭 0.36 m · 높이 0.245 m | ch01 |
| 속도 · 감속 | 앞뒤 0 ~ 1.5 m/s, 옆 0 ~ 1.0 m/s · 감속도 1.5 m/s² (가정값) | ch01 · ch03 |
- 시뮬레이터 속 라이다 · 카메라는 교육용 모델 입니다. N10 Plus 의 측정률(초당 측정 수), 3D 라이다 · 카메라의 수직 시야각, 검은 물체에서 줄어드는 거리, 유리의 투과 같은 값은 원리를 보이기 위한 가정이며, 실제 값은 각 제조사 사양서로 확인해야 합니다 (확인 필요 표시).
- Playground 코드는 표준 라이브러리 +
numpy만 씁니다. 실물 로봇 · ROS 2 가 필요한 부분은## 실습 (실물)에 따로 두었습니다. - 이 차시의 안전 영역 로직은 원리 학습용 입니다. 실제 산업 현장의 안전 기능은 인증된 안전 라이다 · 안전 PLC 같은 별도 장치가 맡으며, Python 코드 하나로 대신하지 않습니다.
강좌에 myAGV Pro 모델(assets/robot/robot.json)이 준비되면 3D 뷰어에서 laser_link 와 camera_link 를 골라 높이를 직접 확인해 봅니다. 위 표의 0.229 m · 0.169 m 가 차체 윗면(0.245 m)보다 낮다는 점 — 즉 라이다 평면이 차체 바로 아래를 지난다는 점 — 이 3교시 · 4교시 사각지대 이야기의 출발점입니다. 모델이 없을 때는 위 표의 숫자로 같은 계산을 합니다.
1교시 · 2D 라이다 스캔의 원리#
빛으로 거리를 재는 두 가지 방법#
라이다는 레이저 빛을 쏘고 되돌아온 빛으로 거리를 잽니다. 방법은 크게 둘입니다.
- 비행시간(ToF, Time of Flight): 펄스를 쏜 시각과 반사광이 돌아온 시각의 차이를 잽니다.
d = c · Δt / 2(빛의 속도c ≈ 3×10⁸ m/s, 왕복이라 2 로 나눔). 10 m 의 왕복 시간은 약 66.7 ns 이고, 1 cm 차이는 약 67 ps 입니다. 아주 빠른 시계가 필요하지만 먼 거리에서도 오차가 크게 늘지 않습니다. - 삼각측량(triangulation): 레이저와 수신 소자를 조금 떨어뜨려 두고, 반사점이 수신 소자의 어디에 맺히는지로 거리를 계산합니다. 값싸고 가까운 거리에서 정확하지만, 멀어질수록 오차가 빠르게 커집니다.
라이다 헤드는 이 측정을 하면서 빙글빙글 돕니다. 한 바퀴를 돌며 일정한 각도마다 잰 거리를 순서대로 모은 것이 스캔(scan) 한 장입니다.
LaserScan 메시지 읽기#
ROS 2 에서 2D 라이다 드라이버는 스캔 한 장을 sensor_msgs/LaserScan 메시지로 발행합니다. 꼭 알아야 할 필드는 다음과 같습니다.
| 필드 | 뜻 | 예 (1° 해상도, 360 개) |
|---|---|---|
angle_min · angle_max | 첫 · 마지막 측정의 각도 (rad) | -π · π - 0.01745 |
angle_increment | 이웃한 측정 사이의 각도 (rad) | 0.01745 (= 1°) |
scan_time | 한 바퀴 도는 데 걸린 시간 (s) | 0.1 (10 Hz) |
range_min · range_max | 믿을 수 있는 거리 범위 (m) | 0.05 · 15.0 |
ranges[] | 거리 배열 (m). 못 재면 inf 또는 0 등 범위 밖 값 | [3.21, 3.20, inf, …] |
intensities[] | 반사광 세기 (장비마다 의미가 다름) | [812, 790, 0, …] |
i 번째 측정의 각도 φᵢ = angle_min + i · angle_increment
각도 φ 의 인덱스 i = round((φ - angle_min) / angle_increment)
측정 개수 N = 360° / Δφ (Δφ = angle_increment)angle_min = -π 인 라이다라면 **ranges[0] 은 로봇 뒤쪽** 이고, 앞(0 rad)은 배열의 가운데(N/2)입니다. 이것을 모르고 ranges[0] 을 "앞" 으로 쓰는 실수가 가장 흔합니다 (자주 나는 오류 2). 라이다마다 angle_min 이 0 이거나 회전 방향이 반대일 수 있으니, 실물에서는 메시지의 값을 먼저 읽고 계산합니다.
해상도 · 회전 주파수 · 측정률#
스핀형 라이다는 1 초에 잴 수 있는 횟수(측정률, samples/s)가 거의 정해져 있습니다. 그래서 빨리 돌수록 한 바퀴의 측정 수가 줄어듭니다.
Δφ (도) = 360° × f / 측정률 f = 회전 주파수 (Hz)
이웃 광선 사이 간격 ≈ d · Δφ(rad) d = 거리 (m)
지름 w 인 기둥이 차지하는 각 ≈ w / d (rad)
→ 기둥을 반드시 한 번 이상 맞히려면 Δφ ≤ w / d- N10 Plus 의 회전 주파수는 6 ~ 12 Hz 입니다. 측정률은 이 강좌에서 4500 회/s 로 가정 합니다 (확인 필요). 그러면 10 Hz 에서 Δφ = 360 × 10 / 4500 = 0.8° 입니다.
- 회전이 빠르면 스캔이 자주 들어와 반응이 빨라지지만, 각 해상도가 거칠어집니다. 1.5 m/s 로 달리는 로봇은 6 Hz 스캔 한 장 사이에 0.25 m 를 갑니다.
- 거리 오차 ±3 cm 는 시뮬레이터에서 표준편차 σ = 1.5 cm 의 정규 노이즈로 흉내 냅니다 (±3 cm ≈ 2σ 로 가정).
시뮬레이터 1 · 창고 레이캐스팅 라이다#
시뮬레이터는 라이다 원점에서 각도마다 광선(ray)을 쏘고, 가장 먼저 닿는 물체까지의 거리를 계산합니다(레이캐스팅). 창고 선반은 라이다 높이(0.229 m)에 걸리는 부분, 즉 기둥(upright) 과 맨 아래 칸의 상자 만 그렸습니다. 빨간 원은 지름 5 cm 의 얇은 기둥이며, 끌어서 옮길 수 있습니다.
먼저 예측하세요. 각 해상도를 1° 에서 5° 로 바꾸면, 10 m 앞의 지름 5 cm 기둥은 보일까요? 해상도가 1° 일 때는 늘 보일까요?
해 볼 것
- Δφ = 1° 를 누르고 기둥 x 거리를 10 m 에 둡니다. 출력의 "이웃 광선 간격 d·Δφ" 는 몇 cm 인가요? 지름 5 cm 보다 큰가요, 작은가요? 스캔 시작 위상 슬라이더를 0 → 0.99 로 천천히 밀며 기둥이 보였다 사라졌다 하는지 봅니다.
- ▶ 연속 스캔 을 눌러 50 장 이상 모은 뒤 감지율을 읽습니다. 이론값
(w/d)/Δφ와 비슷한가요? 이번에는 Δφ = 5° 로 바꾸고 같은 실험을 합니다. 예측과 비교하세요. - 연속 스캔을 켠 채 기둥을 끌어 로봇 쪽(2 m)으로 가져옵니다. 감지율이 어떻게 바뀌나요? "가까운 얇은 물체" 와 "먼 얇은 물체" 중 무엇이 더 위험한지 말해 봅니다.
- N10 Plus 재현 을 누릅니다(측정률 고정). 회전 주파수를 6 → 12 Hz 로 올리면 Δφ 와 스캔 주기는 각각 어떻게 바뀌나요? 반응 속도와 해상도 중 무엇을 얻고 무엇을 잃나요?
- 최대 거리를 15 → 5 m 로 줄여 봅니다. 창고 안쪽 선반 기둥들은 어떻게 보이나요?
ranges의 그 방향 값은 무엇이 되나요?
관찰 포인트 — 얇은 물체가 늘 보이려면 그 물체가 차지하는 각 w/d 보다 광선 간격 Δφ 가 작아야 합니다. 그보다 크면 보일 확률이 (w/d)/Δφ 로 떨어지고, 스캔마다 위상이 바뀌어 보였다 안 보였다 합니다. 10 m 에서 5 cm 기둥은 w/d = 0.005 rad ≈ 0.29° 를 차지하므로, Δφ = 1° 에서는 약 29 %, 5° 에서는 약 6 % 의 스캔에서만 보입니다. 같은 측정률이면 회전을 빠르게 할수록 Δφ 가 커지는 반응 속도 ↔ 해상도 의 교환도 확인했습니다.
시뮬레이터 1 에서 Δφ = 1°, 기둥 지름 5 cm, 거리 10 m 로 두고 연속 스캔을 100 장 모았습니다. 감지율로 가장 그럴듯한 것은?
- 100 % — 1° 는 충분히 촘촘하므로
- 약 29 % — 기둥이 차지하는 각 0.29° 가 광선 간격 1° 의 약 29 % 이므로
- 약 5 % — 지름 5 cm 이므로
- 0 % — 10 m 는 너무 멀어서
정답 보기
② — 기둥이 차지하는 각은 w/d = 0.05/10 = 0.005 rad ≈ 0.286° 이고, 광선은 1° 마다 하나이므로 위상이 무작위라면 약 0.286 / 1 ≈ 29 % 의 스캔에서 광선 하나가 기둥에 닿습니다. 10 m 는 최대 거리 15 m 안이므로 ④ 는 틀렸고, ① 은 "간격 = 각도" 와 "물체 크기" 를 비교하지 않은 답입니다.angle_min = -3.1416, angle_increment = 0.0174533 (1°), len(ranges) = 360 인 스캔이 있습니다. 로봇 바로 왼쪽(+90°) 의 거리는 어느 인덱스에 있나요?
ranges[90]ranges[180]ranges[270]ranges[0]
정답 보기
③ — i = (φ − angle_min) / angle_increment = (π/2 + π) / (π/180) = 270 입니다. ① 은 angle_min 을 0 으로 착각한 경우이고, ② 는 앞(0 rad), ④ 는 뒤(−π) 입니다. 시뮬레이터 1 아래의 LaserScan 줄에서 "앞쪽 ranges[...]" 인덱스가 N/2 근처인 것도 확인해 보세요.2교시 · 스캔 데이터 다루기#
극좌표 → 직교좌표#
ranges[i] 하나는 "방향 φᵢ 로 rᵢ m 떨어진 곳에 무언가 있다" 는 뜻입니다. 이것을 라이다 기준 점 (x, y) 로 바꾸는 것이 거의 모든 스캔 처리의 첫 단계입니다.
φᵢ = angle_min + i · angle_increment
xᵢ = rᵢ · cos φᵢ (앞 +)
yᵢ = rᵢ · sin φᵢ (왼쪽 +)
로봇 기준으로는 laser_link 오프셋을 더함: x_base = xᵢ + 0.179 (ch01)믿을 수 없는 값 먼저 걸러내기#
ranges 에는 거리가 아닌 값이 섞여 들어옵니다. 계산하기 전에 걸러야 합니다.
| 값 | 뜻 | 그대로 쓰면 |
|---|---|---|
inf | 최대 거리 안에서 아무것도 맞지 않음 (열린 공간) | min() 은 괜찮지만 평균 · 좌표 변환이 inf · nan 이 됨 |
0.0 (또는 range_min 미만) | 측정 실패 (반사광 없음, 너무 가까움, 드라이버에 따라 다름) | "0 m 앞에 장애물" 로 읽혀 로봇이 계속 비상정지 |
nan | 측정 실패 (드라이버에 따라) | np.min 결과가 nan 이 되어 비교가 모두 False |
| 아주 작은 값 0.05 ~ 0.35 m 가 하나만 튐 | 먼지 · 반사 노이즈 | 유효 범위 안이라 걸러지지 않음 → 필터가 필요 |
규칙은 간단합니다: **range_min ≤ r ≤ range_max 인 값만 유효** 로 보고, 나머지는 "모름(NaN)" 으로 바꿔 계산에서 뺍니다.
부채꼴 영역별 최소 거리#
"앞에 무언가 가까이 있나?" 는 앞쪽 부채꼴(예: ±30°) 안의 유효 값 중 최솟값 으로 판단합니다. 같은 방식으로 앞 · 왼쪽 · 뒤 · 오른쪽 네 영역을 보면 로봇 주변 상황을 숫자 네 개로 요약할 수 있습니다. 각도 차이를 비교할 때는 ch04 에서 배운 대로 atan2(sin(φ - c), cos(φ - c)) 로 -π ~ π 로 감아야 뒤쪽(±180°) 영역이 끊기지 않습니다.
노이즈를 줄이는 두 필터#
이웃한 몇 개(창 크기 k)의 값을 모아 대표값으로 바꿉니다.
- 이동 평균(moving average): 창 안의 값을 평균. 정규 노이즈를 √k 배쯤 줄이지만, 튀는 값 하나가 평균을 끌어당기고 물체 경계를 흐립니다.
- 중앙값(median): 창 안의 값을 정렬해 가운데 값. 튀는 값 하나 · 둘은 통째로 없애지만, 창의 절반보다 적은 광선에만 걸린 얇은 물체도 없애 버립니다.
어느 쪽이 "좋은" 필터일까요? 시뮬레이터로 직접 판단해 봅니다.
시뮬레이터 2 · 스캔 배열과 필터 (1° 해상도, 360 개)#
왼쪽은 로봇 주변을 위에서 본 점(회색 = 받은 값, 파랑 = 필터 결과), 오른쪽은 같은 데이터를 각도 - 거리 그래프 로 펼친 것입니다. 빨간 부채꼴은 앞 영역, 빨간 점선은 정지 기준 거리입니다.
먼저 예측하세요. 로봇 앞 0.6 m 에 지름 3 cm 의 얇은 기둥이 있습니다. 노이즈를 없애려고 중앙값 필터의 창을 7 로 키우면, 앞 영역의 최소 거리는 0.6 m 근처로 나올까요?
해 볼 것
- 기본값에서 0 · inf 걸러내기 를 끕니다. 앞 영역의 "걸러낸 뒤" 값과 판정이 어떻게 바뀌나요? 주황 점(0 값)이 왼쪽 그림의 어디에 찍히는지 봅니다. 다시 켜서 비교하세요.
- 먼지 + 필터 없음 → ▶ 100 스캔 실험. 헛정지는 몇 회인가요? 이어서 먼지 + 평균 5, 먼지 + 중앙값 5 로 같은 실험을 합니다. 어느 필터가 튀는 값에 강한가요?
- 기둥 + 중앙값 7 을 누르고 판정을 읽습니다. 예측과 같았나요? 100 스캔 실험의 "놓침" 횟수를 확인하고, 기둥 + 중앙값 3 · 기둥 + 평균 7 과 비교합니다.
- 필터 없음으로 두고 노이즈 σ 를 0 → 10 cm 로 올리며 앞 영역 최소 거리의 평균 |오차| 를 100 스캔 실험으로 잽니다. 같은 σ 에서 이동 평균 k = 5 로 바꾸면 오차가 줄어드나요?
- 영역 반폭을 10° → 45° 로 바꿔 봅니다. 기둥(-20°)이 앞 영역에 들어오는 반폭은 몇 도부터인가요?
관찰 포인트 — 유효 범위 검사(0 · inf 걸러내기)는 필터보다 먼저 해야 합니다. 걸러 내지 않은 0 값 하나가 최솟값을 0 으로 만들어 헛정지를 일으킵니다. 중앙값 필터는 먼지처럼 혼자 튀는 값 을 없애는 데 강하지만, 창 크기의 절반보다 적은 광선에 걸린 얇은 물체도 지웁니다. 0.6 m 에서 3 cm 기둥은 약 2.9° 로 광선 2 ~ 3 개에만 걸리므로 k = 7 이면 사라지고, 이동 평균은 기둥을 뒤 배경과 섞어 실제보다 멀리 보이게 합니다. 안전 판단에 쓰는 값은 "덜 지우는" 쪽(작은 창, 또는 여러 스캔 연속 확인)이 원칙입니다. 노이즈가 클수록 "최솟값" 은 참값보다 작아지는 쪽으로 치우칩니다 — 여러 값 중 가장 작은 것을 고르면 음의 노이즈가 뽑히기 쉽기 때문입니다.
시뮬레이터 2 의 먼지 + 필터 없음 에서 100 스캔 실험 결과 헛정지가 수십 회 나왔습니다. 헛정지를 크게 줄이면서 앞 0.6 m 의 얇은 기둥(3 cm)도 놓치지 않을 방법으로 알맞은 것은?
- 중앙값 필터 k = 9
- 이동 평균 k = 9
- 중앙값 필터 k = 3
- 0 · inf 걸러내기를 끈다
정답 보기
③ — 먼지 튐은 대부분 광선 1 개짜리이므로 창 3 의 중앙값으로도 사라집니다. 기둥은 광선 2 ~ 3 개에 걸리므로 창 3 에서는 대부분 살아남습니다. ① 은 기둥까지 지우고, ② 는 기둥을 배경과 섞어 멀리 보이게 하며, ④ 는 0 값이 그대로 최솟값이 되어 헛정지가 더 늘어납니다. 시뮬레이터에서 기둥을 켜고 세 조건을 각각 100 스캔 실험으로 확인해 보세요.angle_min = -π, 1° 해상도의 스캔에서 ranges[200] = 1.5 입니다. 라이다 기준 직교좌표 (x, y) 로 가장 가까운 것은?
- (1.41, 0.51)
- (1.48, 0.26)
- (-1.41, -0.51)
- (0.51, 1.41)
정답 보기
① — φ = −180° + 200 × 1° = 20° 이므로 x = 1.5·cos 20° ≈ 1.41, y = 1.5·sin 20° ≈ 0.51 입니다 (앞쪽 약간 왼쪽). ③ 은 angle_min 을 0 으로 보고 200° 로 계산한 경우, ② 는 10° 로 잘못 계산한 경우, ④ 는 cos · sin 을 바꾼 경우입니다.3교시 · 보호 영역과 감속 · 정지 로직#
경고 영역과 보호 영역#
산업용 AGV 는 라이다로 로봇 둘레에 보이지 않는 울타리 를 둡니다. 울타리 안에 무언가 들어오면 미리 정한 동작을 합니다.
| 영역 | 들어오면 | 크기를 정하는 기준 |
|---|---|---|
| 경고 영역(warning field) | 감속 (예: 0.3 m/s) | 사람이 놀라지 않고, 감속이 끝날 만큼 여유 있게 |
| 보호 영역(protective field) | 정지 | 정지 거리 이상 — 이 안에서 멈추지 못하면 영역이 의미 없음 |
정지 거리는 ch01 에서 본 것처럼 "알아차리는 동안 가는 거리" 와 "브레이크를 밟은 뒤 미끄러지는 거리" 의 합입니다.
d_stop = v · t_반응 + v² / (2a)
t_반응 = 스캔 주기(1/f) + 처리 · 통신 지연 예: 0.1 + 0.2 = 0.3 s (가정)
a = 감속도 이 강좌의 가정값 1.5 m/s² (빈 차)
보호 영역 길이 Lp ≥ d_stop + 여유(0.1 m)v² 항 때문에 속도를 2 배로 올리면 제동 거리는 4 배가 됩니다. 그래서 고정된 보호 영역 하나로는 "느릴 때는 너무 크고(헛정지), 빠를 때는 너무 작은" 문제가 생기고, 실제 안전 라이다는 속도에 따라 영역 세트를 바꿔 끼웁니다.
전방향 로봇은 영역도 돌아야 합니다#
차동 구동 로봇은 앞뒤로만 가므로 앞(과 뒤)에만 영역을 두면 됩니다. 메카넘 휠의 myAGV Pro 는 옆 · 대각선으로도 갑니다 (ch02). 앞쪽에만 고정된 영역을 두고 move_right_lateral 로 가면, 진행 방향에는 울타리가 없는 상태로 달리게 됩니다.
- 진행 방향 =
atan2(vy, vx)(로봇 기준). 영역 사각형을 이 각도만큼 돌립니다. - 앞으로 갈 때 영역 폭은 차체 폭 0.36 m + 여유, 옆으로 갈 때는 차체 길이 0.53 m + 여유입니다.
- 제자리 회전(
ω만 있을 때)은 차체 대각선 반지름√(0.265² + 0.18²) ≈ 0.32 m둘레 전체를 봐야 합니다.
센서 사각지대#
라이다가 아무리 촘촘해도 스캔 평면 밖 과 빛이 돌아오지 않는 물체 는 못 봅니다. 이 내용은 4교시 시뮬레이터에서 직접 확인합니다.
- 높이: 바닥에서 0.229 m 의 한 평면만 봅니다. 그보다 낮은 물체(빈 팔레트, 지게차 포크, 바닥의 케이블)와, 그보다 높은 곳에서 튀어나온 물체(선반 판, 테이블 상판)는 안 보입니다 (ch01 시뮬레이터 3).
- 유리 · 거울: 빛이 통과하거나 다른 방향으로 반사되어 돌아오지 않습니다. 유리문은 "열린 공간" 으로 보일 수 있습니다.
- 검은 물체: 반사율이 낮아 최대 거리가 크게 줄어듭니다. N10 Plus 의 15 m 는 반사율 70 % 기준입니다.
- 가림(occlusion): 선반 뒤의 사람은 선반 끝에서 나타나는 순간까지 보이지 않습니다. 아래 시뮬레이터의 "갑자기 나타나는 사람" 이 바로 이 경우입니다.
시뮬레이터 3 · 보호 영역 설계와 갑자기 나타나는 사람#
창고 통로(폭 2.6 m)를 따라 로봇이 13 m 를 갑니다. 선반 사이 통로에서 사람이 1.2 m/s 로 걸어 나와 로봇의 길 한가운데에 2 초 서 있다가 지나갑니다. 사람은 선반 뒤에 있는 동안 보이지 않습니다. 100회 실험 은 사람이 나타나는 위치 · 시각을 무작위로 바꿔 100 번 돌리고, 충돌 없이 끝난 비율(성공률)과 평균 주행 시간을 셉니다.
감속도 1.5 m/s² · 가속도 0.8 m/s² · 경고 영역 속도 0.3 m/s · 사람 반지름 0.25 m · 여유 0.1 m. 경고 영역은 보호 영역을 감싸며 좌우로 0.5 m 씩 더 넓습니다. 라이다는 360° 를 가림 없이 본다고 두었고(선반 뒤만 가림), 반응 시간 동안은 직전 명령이 그대로 유지됩니다. 충돌은 "로봇이 0.05 m/s 이상으로 움직이는 중에 사람과 겹침" 으로 셉니다.
먼저 예측하세요. 1.0 m/s 로 달릴 때 보호 영역을 0.3 m 에서 1.0 m 로 늘리면 성공률이 100 % 가 될까요? 같은 영역으로 옆으로 달리면 어떻게 될까요?
해 볼 것
- 기본값(앞으로 1.0 m/s, Lp 0.3 m)에서 ▶ 100회 실험 을 누릅니다. 출력 첫 줄의 정지 거리와 Lp 를 비교하고, 충돌 장면 찾아 보기 로 충돌이 어떻게 일어나는지 봅니다.
- Lp 를 0.3 → 0.5 → 0.7 → 1.0 → 2.0 m 로 늘리며 매번 100회 실험을 합니다. 성공률과 평균 주행 시간을 표로 적으세요. 성공률이 크게 오르는 Lp 는 정지 거리와 어떤 관계인가요? Lp 를 키워도 100 % 가 안 되는 이유는 무엇일까요?
- 경고 영역 Lw 를 2 m 로 두고 다시 실험합니다. 같은 Lp 에서 성공률과 주행 시간은 어떻게 바뀌나요?
- 속도 비례 보호 영역 을 켜고 속도를 0.3 → 1.5 m/s 로 바꾸며 실험합니다. Lp 가 어떻게 따라 바뀌나요? 반응 시간을 0.3 → 0.6 s 로 늘리면?
- 주행 방식을 옆으로 바꿉니다(Lp 는 1.0 m). 영역 방향이 "로봇 앞쪽에 고정" 일 때와 "진행 방향을 따라감" 일 때의 성공률을 비교하고, 한 번 보기로 빨간 영역이 어디를 덮는지 확인합니다.
관찰 포인트 — 보호 영역이 정지 거리보다 짧으면, 영역에 들어온 것을 알아차려도 멈추기 전에 닿습니다. Lp ≥ v·t_반응 + v²/2a 를 만족시키는 순간 성공률이 크게 오르고, 그 뒤로는 영역을 키워도 "정지 거리 안쪽에서 갑자기 튀어나오는" 경우만 남습니다. 이 남은 위험은 영역이 아니라 속도를 줄여서(경고 영역, 사각 구간 서행) 줄입니다. 영역이 클수록 안전하지만 헛정지 · 주행 시간이 늘어나는 안전 ↔ 효율 의 교환이 있고, 속도 비례 영역은 이 둘을 동시에 맞추는 방법입니다. 옆으로 달릴 때 앞쪽 고정 영역은 진행 방향을 전혀 덮지 않아, 영역이 아무리 커도 성공률이 오르지 않습니다.
시뮬레이터 3 에서 v = 1.2 m/s, t_반응 = 0.3 s, 감속도 1.5 m/s² 입니다. 보호 영역 길이 Lp 의 최소 기준(여유 제외)으로 알맞은 것은?
- 0.36 m
- 0.48 m
- 0.84 m
- 1.32 m
정답 보기
③ — d_stop = v·t_반응 + v²/(2a) = 1.2 × 0.3 + 1.44 / 3 = 0.36 + 0.48 = 0.84 m 입니다. ① 은 반응 거리만, ② 는 제동 거리만 계산한 값이고, ④ 는 v²/a 로 2 를 빠뜨린 값(0.36 + 0.96)입니다. 시뮬레이터에서 속도 비례 영역을 켜고 v = 1.2 로 두면 Lp ≈ 0.95 m (여유 0.1 포함)가 나옵니다.시뮬레이터 3 에서 옆으로 주행하며 "로봇 앞쪽에 고정" 영역의 Lp 를 0.5 m → 2.5 m 로 키웠는데 성공률이 거의 그대로였습니다. 가장 알맞은 설명은?
- 옆으로 갈 때는 감속도가 작아서
- 영역이 로봇의 앞(위쪽)을 덮을 뿐, 진행 방향(오른쪽)을 덮지 않아서
- 사람이 선반 뒤에 있어 어떤 영역으로도 볼 수 없어서
- 옆 이동 최대 속도 1.0 m/s 가 너무 느려서
정답 보기
② — 영역을 아무리 키워도 진행 방향이 아닌 곳을 덮으면 앞에 선 사람을 알아차리지 못합니다. 영역 방향을 "진행 방향을 따라감" 으로 바꾸면 같은 Lp 에서 성공률이 앞으로 주행할 때 수준으로 오릅니다. ③ 은 사람이 통로로 나온 뒤에는 보이므로 틀렸고, 이 시뮬레이터의 감속도는 방향과 관계없이 같습니다.4교시 · 3D 라이다와 깊이 카메라#
2D 라이다의 한계를 메우는 센서들#
3교시까지 본 사각지대(낮은 물체 · 튀어나온 물체 · 유리 · 검은 물체)는 2D 라이다를 아무리 좋은 것으로 바꿔도 남습니다. 스캔이 한 평면 이기 때문입니다. myAGV Pro 는 옵션으로 3D 라이다와 깊이 카메라를 달 수 있습니다 (ch01).
| 센서 | 시야 (수평 × 수직) | 거리 | 출력 | 이 강좌의 시뮬레이터 값 |
|---|---|---|---|---|
| 2D 라이다 N10 Plus | 360° × 한 평면 | 0 ~ 15 m (반사율 70 %) | LaserScan (거리 배열) | 바닥 0.229 m 의 수평선 1 개 |
| 3D 라이다 Livox MID-360 | 360° × -7° ~ 52° | 40 m (반사율 10 %), 최소 0.1 m | PointCloud2 (점 구름) | 1° 간격 광선 60 개 (근사) |
| 3D 라이다 Unitree L2 | 360° × 약 90° (반구형) | 30 m (90 %) / 15 m (10 %) | PointCloud2 | -5° ~ 90° 근사 (확인 필요) |
| 깊이 카메라 Orbbec Gemini 2 | 약 91° × 66° (확인 필요) | 0.15 ~ 10 m | 깊이 영상 → 점 구름 | 바닥 0.169 m, ±33° |
- 점 구름(point cloud): 3D 라이다는 여러 높이의 광선을 동시에 돌려
(x, y, z)점 수만 개를 냅니다. 바닥의 낮은 물체와 머리 위의 돌출물을 함께 봅니다. 다만 아래쪽 시야각이 작으면(MID-360 은 -7°) 로봇 바로 앞 바닥 근처 는 여전히 못 봅니다. - 깊이 카메라(depth camera): 적외선 패턴을 비춰 두 카메라의 시차로 화소마다 거리를 계산합니다. 근거리(0.15 m ~)를 촘촘하게 보지만, 앞쪽 약 90° 만 보고, 유리 · 검은 물체 · 강한 햇빛에 약합니다.
- 3D 라이다 · 카메라 데이터를 장애물 판단에 쓸 때는 점 구름을 로봇 높이 범위(바닥 위 ~ 로봇 맨 위)로 잘라 2D 로 눌러 담는 방식이 흔합니다. 이렇게 하면 2교시의 영역별 최소 거리 로직을 그대로 쓸 수 있습니다.
센서 융합 — "각자 놓치는 것" 을 겹쳐 지우기#
하나의 센서로 모든 물체를 볼 수는 없습니다. 센서 융합(sensor fusion)의 출발점은 각 센서가 무엇을 놓치는지 표로 정리 하고, 놓치는 칸이 겹치지 않도록 조합을 고르는 것입니다. 안전 판단에서는 보통 "어느 센서든 가까운 장애물을 보면 멈춘다(OR)" 로 합칩니다. 유리처럼 빛 센서가 모두 놓치는 물체는 범퍼(접촉 스위치) · 초음파 센서 · 바닥 표식 같은 다른 원리의 센서나 운영 규칙 으로 메웁니다.
시뮬레이터 4 · 센서 비교 모드 (옆에서 본 그림)#
로봇(왼쪽)과 창고 장애물을 옆에서 봅니다. 체크한 센서의 광선이 그려지고, 아래 표에 센서별 감지 결과가 나옵니다. 위험 부분 은 로봇 높이(적재물 포함) 아래에 있어 부딪힐 수 있는 부분입니다.
최대 거리는 반사율의 제곱근에 비례한다고 근사했습니다 (예: N10 Plus 는 반사율 5 % 의 검은 상자를 약 4 m 까지). 깊이 카메라는 검은 물체를 3 m 까지만 본다고 두었고, 유리는 모든 빛 센서가 투과한다고 두었습니다. 실제 센서는 표면 · 각도 · 조명에 따라 다르므로 현장에서 반드시 시험합니다.
먼저 예측하세요. 지게차 포크(높이 5 cm)가 로봇 앞 1.5 m 에 있습니다. 2D 라이다 · MID-360 · 깊이 카메라 중 포크 끝을 보는 센서는 무엇일까요?
해 볼 것
- 기본값(지게차 포크, 1.5 m)에서 표를 읽습니다. 예측과 같았나요? 거리를 0.2 m → 6 m 로 천천히 밀며 MID-360 이 포크를 보기 시작하는 거리를 찾습니다. 그 거리는
장착 높이 / tan 7°와 어떤 관계인가요? - 3D 라이다 기울임 을 0° → 20° 로 올립니다. 1.5 m 의 포크가 보이기 시작하나요? 그림에서 위쪽 시야 끝(52° - 20° = 32°)이 어떻게 내려왔는지 보고, 너무 많이 기울이면 무엇을 잃을지(먼 곳의 높은 물체, 지도 만들기에 쓰는 수평 방향 점) 말해 봅니다.
- 검은 상자 로 바꾸고 거리를 1 m → 12 m 로 늘립니다. 2D 라이다 · 깊이 카메라 · MID-360 이 각각 놓치기 시작하는 거리를 적습니다.
- 유리 칸막이 를 골라 모든 센서를 켭니다. 융합 결과는? 이 칸을 메우려면 어떤 장치 · 규칙이 필요할지 적어 봅니다.
- 사람 을 고르고 방향을 0° → 60° 로 돌립니다. 깊이 카메라가 놓치기 시작하는 각도는? 로봇 높이를 0.25 m(적재물 없음)로 낮추고 선반 판 돌출 · 테이블 상판 을 다시 봅니다.
관찰 포인트 — 2D 라이다는 스캔 평면(0.229 m)을 지나는 물체만, 깊이 카메라는 앞쪽 약 90° 와 10 m(검은 물체는 더 짧게) 안만, 3D 라이다는 아래 시야각 경계가 바닥에 닿는 거리(h / tan θ_아래, 0.45 m · 7° 면 약 3.7 m)보다 먼 낮은 물체만 봅니다. 그래서 어느 하나도 다섯 종류를 모두 보지 못하고, 놓치는 칸이 서로 다른 센서를 겹쳐야 빈칸이 줄어듭니다. 유리처럼 빛 센서가 모두 놓치는 물체는 다른 원리(범퍼 · 초음파)나 운영 규칙(유리 앞 서행 구역)으로 메웁니다. 같은 센서라도 장착 높이 · 기울기 를 바꾸면 보는 것과 못 보는 것이 맞바뀝니다.
시뮬레이터 4 에서 MID-360 을 바닥 0.45 m 높이에 기울이지 않고 달았습니다(아래 시야 -7°). 높이 5 cm 의 지게차 포크 끝을 처음 볼 수 있는 거리(라이다 기준)에 가장 가까운 것은?
- 약 0.45 m
- 약 1.2 m
- 약 3.3 m
- 어떤 거리에서도 볼 수 없다
정답 보기
③ — 가장 아래 광선(-7°)이 높이 0.05 m 까지 내려오는 거리는 (0.45 − 0.05) / tan 7° ≈ 0.40 / 0.123 ≈ 3.3 m 입니다. 그보다 가까운 포크는 광선이 포크 위로 지나갑니다. 시뮬레이터에서 거리 슬라이더를 밀며 "못 봄 → 감지" 로 바뀌는 지점을 확인하고, 라이다가 차체 중심(로봇 앞 끝에서 0.265 m 뒤)에 있다는 것도 고려해 보세요.시뮬레이터 4 로 정리했을 때, 테이블 상판(0.70 m, 로봇 높이 0.8 m) · 빈 팔레트(0.14 m) · 로봇 옆 60° 방향의 사람 을 모두 감지하는 가장 작은 조합은? (3D 라이다는 기울이지 않고 0.45 m 에 장착, 장애물은 로봇 앞 1 ~ 1.5 m)
- 2D 라이다만
- 깊이 카메라만
- 2D 라이다 + 깊이 카메라
- 2D 라이다 + MID-360
정답 보기
③ — 테이블 상판과 빈 팔레트는 2D 라이다 평면 밖이라 2D 라이다로는 못 보지만, 깊이 카메라(±33°, 근거리 0.15 m ~)는 정면의 두 물체를 모두 봅니다. 60° 방향의 사람은 카메라 수평 시야(약 ±45°) 밖이지만 360° 2D 라이다가 봅니다. ② 는 옆의 사람을 놓치고, ④ 는 1 ~ 1.5 m 의 낮은 팔레트를 MID-360 의 -7° 시야로 제때 보지 못합니다 (못 봄 또는 늦게 감지). 시뮬레이터에서 세 경우를 차례로 확인해 보세요.실습 (시뮬레이터)#
교시마다 Playground 에서 코드를 직접 돌립니다. starter 의 TODO 를 채워 실행한 뒤, 풀이 와 비교하세요. 모든 코드는 표준 라이브러리 + numpy 만 씁니다.
계획의 실습 질문 "거리 10 m 에서 지름 5 cm 기둥을 놓치지 않으려면 필요한 각 해상도는?" 을 계산하고, 위상을 무작위로 바꾼 스캔 200 장으로 감지율을 확인합니다. 시뮬레이터 1 의 연속 스캔 통계와 같은 실험입니다.
import numpy as np
def required_resolution_deg(w, d):
# TODO 1: 지름 w 인 기둥이 거리 d 에서 차지하는 각 w/d (rad) 을 도(deg) 로 바꿔 돌려주기
return 1.0
def make_scan(res_deg, phase, pillar=(10.0, 0.0), dia=0.05, rmax=15.0):
"""기둥 하나만 있는 공간의 LaserScan 흉내 -> (angle_min, angle_increment, ranges)"""
n = int(round(360 / res_deg))
inc = 2 * np.pi / n
a0 = -np.pi + phase * inc
ang = a0 + inc * np.arange(n)
cx, cy = pillar
r = dia / 2
b = np.cos(ang) * cx + np.sin(ang) * cy
disc = b * b - (cx * cx + cy * cy - r * r)
t = np.where(disc >= 0, b - np.sqrt(np.maximum(disc, 0)), np.inf)
t = np.where((t > 0) & (t <= rmax), t, np.inf)
return a0, inc, t
def hits(ranges):
# TODO 2: ranges 중 inf 가 아닌 값(기둥에 맞은 광선)의 개수
return 0
need = required_resolution_deg(0.05, 10.0)
print(f"need <= {need:.3f} deg") # 기대: 약 0.286
rng = np.random.default_rng(0)
for res in (5.0, 1.0, 0.5, need * 0.99, need / 2):
# TODO 3: 위상 rng.random() 으로 make_scan 을 200 번 돌려 hits > 0 인 비율(%) 구하기
rate = 0.0
theory = 100 * min(1.0, need / res)
print(f"res {res:6.3f} deg -> detect {rate:5.1f} % (theory {theory:5.1f} %)")import numpy as np
def required_resolution_deg(w, d):
return np.degrees(w / d)
def make_scan(res_deg, phase, pillar=(10.0, 0.0), dia=0.05, rmax=15.0):
"""기둥 하나만 있는 공간의 LaserScan 흉내 -> (angle_min, angle_increment, ranges)"""
n = int(round(360 / res_deg))
inc = 2 * np.pi / n
a0 = -np.pi + phase * inc
ang = a0 + inc * np.arange(n)
cx, cy = pillar
r = dia / 2
b = np.cos(ang) * cx + np.sin(ang) * cy
disc = b * b - (cx * cx + cy * cy - r * r)
t = np.where(disc >= 0, b - np.sqrt(np.maximum(disc, 0)), np.inf)
t = np.where((t > 0) & (t <= rmax), t, np.inf)
return a0, inc, t
def hits(ranges):
return int(np.isfinite(ranges).sum())
need = required_resolution_deg(0.05, 10.0)
print(f"need <= {need:.3f} deg")
rng = np.random.default_rng(0)
for res in (5.0, 1.0, 0.5, need * 0.99, need / 2):
k = [hits(make_scan(res, rng.random())[2]) for _ in range(200)]
rate = 100 * np.mean(np.array(k) > 0)
theory = 100 * min(1.0, need / res)
print(f"res {res:6.3f} deg -> detect {rate:5.1f} % (theory {theory:5.1f} %, max hits {max(k)})")Δφ ≤ w/d ≈ 0.286° 를 만족하면(아래 need * 0.99) 감지율이 100 % 가 됩니다. 그 절반 해상도에서는 광선이 늘 2 개 이상 맞아, 노이즈로 한 점을 잃어도 버팁니다. 실제 설계에서는 "최소 2 ~ 3 점" 을 기준으로 잡는 경우가 많습니다. N10 Plus(가정 0.8°)로는 10 m 의 5 cm 기둥을 늘 보장할 수 없다는 것도 숫자로 확인됩니다.
비행시간으로 거리를 구하고, angle_min = -π 인 스캔에서 앞 · 왼쪽 · 뒤 · 오른쪽의 인덱스를 찾습니다. 측정률을 고정했을 때 회전 주파수와 해상도의 관계도 표로 만듭니다.
import math
C = 299_792_458.0 # 빛의 속도 (m/s)
def tof_distance(dt_s):
# TODO 1: 왕복 시간 dt_s (s) -> 거리 (m)
return 0.0
def angle_of(i, angle_min, inc):
# TODO 2: i 번째 측정의 각도 (rad)
return 0.0
def index_of(phi, angle_min, inc, n):
# TODO 3: 각도 phi 에 가장 가까운 인덱스. 0 ~ n-1 로 감기 (% n)
return 0
def points_per_rev(f_hz, rate=4500):
# TODO 4: 측정률 rate 회/s 로 f_hz 로 돌 때 (한 바퀴 측정 수, 각 해상도 deg)
return 1, 360.0
print(f"66.7 ns -> {tof_distance(66.7e-9):.3f} m") # 기대: 약 10 m
amin, inc, n = -math.pi, math.radians(1.0), 360
for name, deg in [("front", 0), ("left", 90), ("back", 180), ("right", -90), ("front-right 30", -30)]:
i = index_of(math.radians(deg), amin, inc, n)
print(f"{name:15s} index {i:3d} angle {math.degrees(angle_of(i, amin, inc)):7.1f} deg")
for f in (6, 10, 12):
pts, res = points_per_rev(f)
print(f"{f:2d} Hz -> {pts} points/rev, {res:.2f} deg, scan period {1000 / f:.0f} ms")import math
C = 299_792_458.0 # 빛의 속도 (m/s)
def tof_distance(dt_s):
return C * dt_s / 2
def angle_of(i, angle_min, inc):
return angle_min + i * inc
def index_of(phi, angle_min, inc, n):
return int(round((phi - angle_min) / inc)) % n
def points_per_rev(f_hz, rate=4500):
n = int(rate // f_hz)
return n, 360 / n
print(f"66.7 ns -> {tof_distance(66.7e-9):.3f} m")
print(f"1 cm -> {2 * 0.01 / C * 1e12:.1f} ps round trip")
amin, inc, n = -math.pi, math.radians(1.0), 360
for name, deg in [("front", 0), ("left", 90), ("back", 180), ("right", -90), ("front-right 30", -30)]:
i = index_of(math.radians(deg), amin, inc, n)
print(f"{name:15s} index {i:3d} angle {math.degrees(angle_of(i, amin, inc)):7.1f} deg")
for f in (6, 10, 12):
pts, res = points_per_rev(f)
print(f"{f:2d} Hz -> {pts} points/rev, {res:.2f} deg, scan period {1000 / f:.0f} ms")back 은 (π + π)/inc = 360 이 되므로 % n 으로 감아 0 이 됩니다. 감아 주지 않으면 ranges[360] 을 읽어 IndexError 가 납니다. 측정률이 고정이면 6 Hz → 0.48°, 12 Hz → 0.96° 로 해상도와 반응 속도가 맞바뀝니다.
fake_scan() 은 시뮬레이터 2 와 같은 장면의 스캔(노이즈 · 0 값 포함)을 만듭니다. 유효값만 남기고, 점을 (x, y) 로 바꾸고, 가장 가까운 장애물의 거리 · 방향을 찾습니다.
import numpy as np
def fake_scan(seed=0, sigma=0.02, drop=0.02, dust=0.0, pole=None):
"""시뮬레이터 2 와 같은 장면의 1 도 스캔 -> (angle_min, angle_increment, ranges, truth)"""
rng = np.random.default_rng(seed)
n, inc, amin = 360, np.radians(1.0), -np.pi
ang = amin + inc * np.arange(n)
c, s = np.cos(ang), np.sin(ang)
boxes = [(-3, -1.0, 6, -0.9), (-2.6, -3, -2.5, 3), (5, -1, 5.1, 2.5), (0.5, 2.4, 5.1, 2.5), (1.2, 0.05, 1.6, 0.65)]
t = np.full(n, np.inf)
with np.errstate(divide="ignore", invalid="ignore"):
for x0, y0, x1, y1 in boxes:
tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
t = np.where((tmax >= tmin) & (tmin > 0) & (tmin < t), tmin, t)
if pole is not None:
d, deg = pole
cx, cy, r = d * np.cos(np.radians(deg)), d * np.sin(np.radians(deg)), 0.015
b = c * cx + s * cy
disc = b * b - (cx * cx + cy * cy - r * r)
tp = np.where(disc >= 0, b - np.sqrt(np.maximum(disc, 0)), np.inf)
t = np.where((tp > 0) & (tp < t), tp, t)
truth = np.where(t > 8.0, np.inf, t)
meas = truth + rng.normal(0, sigma, n)
u = rng.random(n)
meas = np.where(u < drop, 0.0, meas)
meas = np.where((u >= drop) & (u < drop + dust), 0.05 + 0.3 * rng.random(n), meas)
return amin, inc, meas, truth
def clean(ranges, rmin=0.05, rmax=8.0):
r = np.asarray(ranges, dtype=float)
# TODO 1: rmin <= r <= rmax 인 값만 남기고 나머지(0, inf)는 np.nan 으로 (np.where)
return r
def to_xy(amin, inc, ranges):
ang = amin + inc * np.arange(len(ranges))
ok = np.isfinite(ranges)
# TODO 2: ok 인 값만 x = r cos(phi), y = r sin(phi) 로 바꿔 (x, y) 돌려주기
return np.zeros(1), np.zeros(1)
def nearest(amin, inc, ranges):
# TODO 3: 모두 nan 이면 None. 아니면 np.nanargmin 으로 (거리, 각도 deg, 인덱스)
return 0.0, 0.0, 0
amin, inc, raw, truth = fake_scan(seed=1)
print("raw min :", np.min(raw), " <- 0 (측정 실패) 가 최솟값이 됨")
r = clean(raw)
print("valid points :", int(np.sum(~np.isnan(r))), "/", len(r))
x, y = to_xy(amin, inc, r)
print("x range :", round(float(x.min()), 2), "~", round(float(x.max()), 2))
print("nearest :", nearest(amin, inc, r))
print("all invalid :", nearest(amin, inc, clean(np.zeros(360)))) # 기대: Noneimport numpy as np
def fake_scan(seed=0, sigma=0.02, drop=0.02, dust=0.0, pole=None):
"""시뮬레이터 2 와 같은 장면의 1 도 스캔 -> (angle_min, angle_increment, ranges, truth)"""
rng = np.random.default_rng(seed)
n, inc, amin = 360, np.radians(1.0), -np.pi
ang = amin + inc * np.arange(n)
c, s = np.cos(ang), np.sin(ang)
boxes = [(-3, -1.0, 6, -0.9), (-2.6, -3, -2.5, 3), (5, -1, 5.1, 2.5), (0.5, 2.4, 5.1, 2.5), (1.2, 0.05, 1.6, 0.65)]
t = np.full(n, np.inf)
with np.errstate(divide="ignore", invalid="ignore"):
for x0, y0, x1, y1 in boxes:
tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
t = np.where((tmax >= tmin) & (tmin > 0) & (tmin < t), tmin, t)
if pole is not None:
d, deg = pole
cx, cy, r = d * np.cos(np.radians(deg)), d * np.sin(np.radians(deg)), 0.015
b = c * cx + s * cy
disc = b * b - (cx * cx + cy * cy - r * r)
tp = np.where(disc >= 0, b - np.sqrt(np.maximum(disc, 0)), np.inf)
t = np.where((tp > 0) & (tp < t), tp, t)
truth = np.where(t > 8.0, np.inf, t)
meas = truth + rng.normal(0, sigma, n)
u = rng.random(n)
meas = np.where(u < drop, 0.0, meas)
meas = np.where((u >= drop) & (u < drop + dust), 0.05 + 0.3 * rng.random(n), meas)
return amin, inc, meas, truth
def clean(ranges, rmin=0.05, rmax=8.0):
r = np.asarray(ranges, dtype=float)
return np.where((r >= rmin) & (r <= rmax), r, np.nan)
def to_xy(amin, inc, ranges):
ang = amin + inc * np.arange(len(ranges))
ok = np.isfinite(ranges)
return ranges[ok] * np.cos(ang[ok]), ranges[ok] * np.sin(ang[ok])
def nearest(amin, inc, ranges):
if np.all(np.isnan(ranges)):
return None
i = int(np.nanargmin(ranges))
return float(ranges[i]), float(np.degrees(amin + i * inc)), i
amin, inc, raw, truth = fake_scan(seed=1)
print("raw min :", np.min(raw), " <- 0 (측정 실패) 가 최솟값이 됨")
r = clean(raw)
print("valid points :", int(np.sum(~np.isnan(r))), "/", len(r))
x, y = to_xy(amin, inc, r)
print("x range :", round(float(x.min()), 2), "~", round(float(x.max()), 2))
d, deg, i = nearest(amin, inc, r)
print(f"nearest : {d:.3f} m at {deg:.0f} deg (index {i}) -> x {d * np.cos(np.radians(deg)):.2f}, y {d * np.sin(np.radians(deg)):.2f}")
print("all invalid :", nearest(amin, inc, clean(np.zeros(360))))np.min(raw) 가 0.0 인 것은 측정 실패값 때문입니다. clean() 뒤에는 np.nanargmin 이 nan 을 건너뛰므로 진짜 최솟값(오른쪽 벽 약 0.86 m, -85° 근처)이 나옵니다. 모든 값이 무효이면 np.nanargmin 이 ValueError 를 내므로 먼저 None 을 돌려줍니다.
계획의 실습 "앞쪽 ±30° 최소 거리를 출력하는 함수를 만들고 필터 유무 결과 비교" 입니다. 먼지 튐이 있는 경우와 0.6 m 에 얇은 기둥이 있는 경우를 각각 200 스캔씩 돌려 틀린 판정 수 를 셉니다.
import numpy as np
import warnings
def fake_scan(seed=0, sigma=0.02, drop=0.02, dust=0.0, pole=None):
rng = np.random.default_rng(seed)
n, inc, amin = 360, np.radians(1.0), -np.pi
ang = amin + inc * np.arange(n)
c, s = np.cos(ang), np.sin(ang)
boxes = [(-3, -1.0, 6, -0.9), (-2.6, -3, -2.5, 3), (5, -1, 5.1, 2.5), (0.5, 2.4, 5.1, 2.5), (1.2, 0.05, 1.6, 0.65)]
t = np.full(n, np.inf)
with np.errstate(divide="ignore", invalid="ignore"):
for x0, y0, x1, y1 in boxes:
tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
t = np.where((tmax >= tmin) & (tmin > 0) & (tmin < t), tmin, t)
if pole is not None:
d, deg = pole
cx, cy, r = d * np.cos(np.radians(deg)), d * np.sin(np.radians(deg)), 0.015
b = c * cx + s * cy
disc = b * b - (cx * cx + cy * cy - r * r)
tp = np.where(disc >= 0, b - np.sqrt(np.maximum(disc, 0)), np.inf)
t = np.where((tp > 0) & (tp < t), tp, t)
truth = np.where(t > 8.0, np.inf, t)
meas = truth + rng.normal(0, sigma, n)
u = rng.random(n)
meas = np.where(u < drop, 0.0, meas)
meas = np.where((u >= drop) & (u < drop + dust), 0.05 + 0.3 * rng.random(n), meas)
return amin, inc, meas, truth
def clean(r, rmin=0.05, rmax=8.0):
return np.where((r >= rmin) & (r <= rmax), r, np.nan)
def sector_min(amin, inc, ranges, center_deg=0.0, half_deg=30.0):
ang = amin + inc * np.arange(len(ranges))
# TODO 1: 각 광선과 center 의 각도 차를 atan2(sin, cos) 로 -pi ~ pi 로 감고,
# |차| <= half_deg 인 값 중 nan 이 아닌 것의 최솟값 (없으면 np.inf)
return np.inf
def windows(r, k):
h = k // 2
return np.lib.stride_tricks.sliding_window_view(np.pad(r, h, mode="wrap"), k)
def moving_average(r, k=5):
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
# TODO 2: windows(r, k) 의 각 행을 nan 을 빼고 평균 (np.nanmean, axis=1)
return r
def median_filter(r, k=5):
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
# TODO 3: windows(r, k) 의 각 행을 nan 을 빼고 중앙값 (np.nanmedian, axis=1)
return r
def front_min(r, amin, inc, filt=None, k=5):
r = clean(r)
if filt is not None:
r = filt(r, k)
return sector_min(amin, inc, r, 0.0, 30.0)
amin, inc, raw, truth = fake_scan(seed=3)
print("front/left/back/right (truth):", [round(sector_min(amin, inc, truth, c), 3) for c in (0, 90, 180, -90)])
# 기대: [1.202, 2.454, 1.8, 0.9]
STOP = 0.8
for title, kw in [("dust 1.5 %", dict(dust=0.015, drop=0.0)), ("pole 0.6 m", dict(pole=(0.6, -20)))]:
print("==", title, "(stop if front < 0.8 m)")
for name, f, k in [("none", None, 0), ("mavg k=5", moving_average, 5), ("median k=3", median_filter, 3), ("median k=7", median_filter, 7)]:
wrong, err = 0, []
for seed in range(200):
amin, inc, raw, truth = fake_scan(seed=seed, **kw)
m, tm = front_min(raw, amin, inc, f, k), sector_min(amin, inc, truth)
wrong += (m < STOP) != (tm < STOP)
err.append(abs(m - tm) if np.isfinite(m - tm) else 0.0)
print(f" {name:11s} wrong decisions {wrong:3d}/200 mean |err| {100 * np.mean(err):5.1f} cm")import numpy as np
import warnings
def fake_scan(seed=0, sigma=0.02, drop=0.02, dust=0.0, pole=None):
rng = np.random.default_rng(seed)
n, inc, amin = 360, np.radians(1.0), -np.pi
ang = amin + inc * np.arange(n)
c, s = np.cos(ang), np.sin(ang)
boxes = [(-3, -1.0, 6, -0.9), (-2.6, -3, -2.5, 3), (5, -1, 5.1, 2.5), (0.5, 2.4, 5.1, 2.5), (1.2, 0.05, 1.6, 0.65)]
t = np.full(n, np.inf)
with np.errstate(divide="ignore", invalid="ignore"):
for x0, y0, x1, y1 in boxes:
tmin = np.maximum(np.minimum(x0 / c, x1 / c), np.minimum(y0 / s, y1 / s))
tmax = np.minimum(np.maximum(x0 / c, x1 / c), np.maximum(y0 / s, y1 / s))
t = np.where((tmax >= tmin) & (tmin > 0) & (tmin < t), tmin, t)
if pole is not None:
d, deg = pole
cx, cy, r = d * np.cos(np.radians(deg)), d * np.sin(np.radians(deg)), 0.015
b = c * cx + s * cy
disc = b * b - (cx * cx + cy * cy - r * r)
tp = np.where(disc >= 0, b - np.sqrt(np.maximum(disc, 0)), np.inf)
t = np.where((tp > 0) & (tp < t), tp, t)
truth = np.where(t > 8.0, np.inf, t)
meas = truth + rng.normal(0, sigma, n)
u = rng.random(n)
meas = np.where(u < drop, 0.0, meas)
meas = np.where((u >= drop) & (u < drop + dust), 0.05 + 0.3 * rng.random(n), meas)
return amin, inc, meas, truth
def clean(r, rmin=0.05, rmax=8.0):
return np.where((r >= rmin) & (r <= rmax), r, np.nan)
def sector_min(amin, inc, ranges, center_deg=0.0, half_deg=30.0):
ang = amin + inc * np.arange(len(ranges))
diff = np.arctan2(np.sin(ang - np.radians(center_deg)), np.cos(ang - np.radians(center_deg)))
sel = ranges[np.abs(diff) <= np.radians(half_deg) + 1e-9]
sel = sel[~np.isnan(sel)]
return float(sel.min()) if sel.size else np.inf
def windows(r, k):
h = k // 2
return np.lib.stride_tricks.sliding_window_view(np.pad(r, h, mode="wrap"), k)
def moving_average(r, k=5):
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
return np.nanmean(windows(r, k), axis=1)
def median_filter(r, k=5):
with warnings.catch_warnings():
warnings.simplefilter("ignore", RuntimeWarning)
return np.nanmedian(windows(r, k), axis=1)
def front_min(r, amin, inc, filt=None, k=5):
r = clean(r)
if filt is not None:
r = filt(r, k)
return sector_min(amin, inc, r, 0.0, 30.0)
amin, inc, raw, truth = fake_scan(seed=3)
print("front/left/back/right (truth):", [round(sector_min(amin, inc, truth, c), 3) for c in (0, 90, 180, -90)])
STOP = 0.8
for title, kw in [("dust 1.5 %", dict(dust=0.015, drop=0.0)), ("pole 0.6 m", dict(pole=(0.6, -20)))]:
print("==", title, "(stop if front < 0.8 m)")
for name, f, k in [("none", None, 0), ("mavg k=5", moving_average, 5), ("median k=3", median_filter, 3), ("median k=7", median_filter, 7)]:
wrong, err = 0, []
for seed in range(200):
amin, inc, raw, truth = fake_scan(seed=seed, **kw)
m, tm = front_min(raw, amin, inc, f, k), sector_min(amin, inc, truth)
wrong += (m < STOP) != (tm < STOP)
err.append(abs(m - tm) if np.isfinite(m - tm) else 0.0)
print(f" {name:11s} wrong decisions {wrong:3d}/200 mean |err| {100 * np.mean(err):5.1f} cm")먼지에는 필터가 강하지만(130 → 0 ~ 5 회), 얇은 기둥에는 이동 평균 k = 5 와 중앙값 k = 7 이 거의 모든 스캔에서 기둥을 지워 정지해야 할 때 통과 시킵니다. 중앙값 k = 3 만 두 경우 모두 잘 맞습니다. 안전 판단용 필터는 "막아야 할 가장 얇은 물체가 몇 개의 광선에 걸리는가" 로 창 크기를 정합니다.
계획의 실습 "속도에 비례해 보호 영역이 커지는 규칙을 만들고 정지 거리 기준을 만족하는지 검증" 입니다. 공식으로 구한 정지 거리를 작은 Δt 의 수치 적분으로 다시 확인하고, 고정 0.5 m 영역과 속도 비례 영역을 비교합니다.
import numpy as np
A = 1.5 # 감속도 m/s^2 (가정값)
TR = 0.3 # 반응 시간 s
MARGIN = 0.1 # 여유 m
def stop_distance(v, tr=TR, a=A):
# TODO 1: 반응 거리 v*tr + 제동 거리 v^2 / (2a)
return 0.0
def zone_length(v, tr=TR, a=A, margin=MARGIN):
# TODO 2: 속도 비례 보호 영역 = 정지 거리 + 여유
return 0.5
def simulate_stop(v, tr=TR, a=A, dt=0.001):
"""장애물을 본 순간부터 멈출 때까지 실제로 간 거리 (수치 적분)"""
x, t, vel = 0.0, 0.0, v
while vel > 0:
# TODO 3: t >= tr 이 되면 vel 을 a*dt 씩 줄이기 (0 아래로 내려가지 않게)
vel = 0.0
x += vel * dt
t += dt
return x
FIXED = 0.5
print(" v d_stop sim fixed 0.5 adaptive Lp")
for v in np.arange(0.2, 1.51, 0.1):
ds, sim, lp = stop_distance(v), simulate_stop(v), zone_length(v)
ok_f = "OK " if sim <= FIXED else "FAIL"
ok_a = "OK" if sim <= lp else "FAIL"
print(f"{v:4.1f} {ds:6.3f} {sim:6.3f} {ok_f} {lp:5.2f} {ok_a}")
# TODO 4: 고정 0.5 m 영역이 안전한(stop_distance <= 0.5) 가장 큰 속도를 0.01 m/s 단위로 찾기import numpy as np
A = 1.5 # 감속도 m/s^2 (가정값)
TR = 0.3 # 반응 시간 s
MARGIN = 0.1 # 여유 m
def stop_distance(v, tr=TR, a=A):
return v * tr + v * v / (2 * a)
def zone_length(v, tr=TR, a=A, margin=MARGIN):
return stop_distance(v, tr, a) + margin
def simulate_stop(v, tr=TR, a=A, dt=0.001):
"""장애물을 본 순간부터 멈출 때까지 실제로 간 거리 (수치 적분)"""
x, t, vel = 0.0, 0.0, v
while vel > 0:
if t >= tr:
vel = max(0.0, vel - a * dt)
x += vel * dt
t += dt
return x
FIXED = 0.5
print(" v d_stop sim fixed 0.5 adaptive Lp")
for v in np.arange(0.2, 1.51, 0.1):
ds, sim, lp = stop_distance(v), simulate_stop(v), zone_length(v)
ok_f = "OK " if sim <= FIXED else "FAIL"
ok_a = "OK" if sim <= lp else "FAIL"
print(f"{v:4.1f} {ds:6.3f} {sim:6.3f} {ok_f} {lp:5.2f} {ok_a}")
v_max_fixed = max(v for v in np.arange(0.05, 1.51, 0.01) if stop_distance(v) <= FIXED)
print(f"fixed 0.5 m zone is safe up to v = {v_max_fixed:.2f} m/s")공식과 수치 적분이 1 mm 안에서 맞습니다. 고정 0.5 m 영역은 약 0.85 m/s 까지만 안전하고, 그보다 빠르면 영역 안에서 멈추지 못합니다. 속도 비례 영역은 모든 속도에서 기준을 만족하지만 1.5 m/s 에서 1.3 m 로 커져 헛정지가 늘어납니다 — 시뮬레이터 3 의 "안전 ↔ 효율" 교환입니다.
로봇 기준 점(라이다 점)들을 받아 stop / slow / go 를 돌려주는 함수를 만듭니다. 영역을 로봇 앞에 고정했을 때와 속도 벡터 방향으로 돌렸을 때를 비교합니다.
import numpy as np
HX, HY = 0.265, 0.18 # 차체 반길이 (앞뒤), 반폭 (좌우)
MARGIN = 0.1
def zone_check(points, vx, vy, Lp, Lw, follow=True):
"""점(로봇 기준 x 앞, y 왼쪽)들을 보고 'stop' / 'slow' / 'go' 를 돌려준다."""
sp = np.hypot(vx, vy)
if sp < 1e-6:
return "go"
# TODO 1: follow 면 진행 방향 단위벡터 u = (vx, vy)/sp, 아니면 로봇 앞 (1, 0)
ux, uy = 1.0, 0.0
nx, ny = -uy, ux # u 에 수직 (왼쪽)
# TODO 2: 진행 방향으로 본 차체 반길이 ext 와 영역 반폭 half_w (차체 반폭 + MARGIN)
# 힌트: ext = |ux|*HX + |uy|*HY, 반폭 = |ux|*HY + |uy|*HX
ext, half_w = HX, HY + MARGIN
state = "go"
for px, py in points:
along, side = px * ux + py * uy, px * nx + py * ny
# TODO 3: |side| <= half_w 이고 ext <= along <= ext + Lp 이면 "stop"
# |side| <= half_w + 0.5 이고 ext <= along <= ext + Lp + Lw 이면 state = "slow"
pass
return state
person_right = [(0.0, -0.8)] # 로봇 오른쪽 0.8 m
person_front = [(0.8, 0.1)]
cases = [
("forward, person front", 0.5, 0.0, person_front),
("right lat, person right", 0.0, -0.5, person_right),
("right lat, person front", 0.0, -0.5, person_front),
("diagonal, person right", 0.35, -0.35, person_right),
]
for name, vx, vy, pts in cases:
a = zone_check(pts, vx, vy, Lp=0.7, Lw=1.0, follow=False)
b = zone_check(pts, vx, vy, Lp=0.7, Lw=1.0, follow=True)
print(f"{name:25s} fixed-front: {a:5s} follow-velocity: {b}")import numpy as np
HX, HY = 0.265, 0.18 # 차체 반길이 (앞뒤), 반폭 (좌우)
MARGIN = 0.1
def zone_check(points, vx, vy, Lp, Lw, follow=True):
"""점(로봇 기준 x 앞, y 왼쪽)들을 보고 'stop' / 'slow' / 'go' 를 돌려준다."""
sp = np.hypot(vx, vy)
if sp < 1e-6:
return "go"
ux, uy = (vx / sp, vy / sp) if follow else (1.0, 0.0)
nx, ny = -uy, ux
ext = abs(ux) * HX + abs(uy) * HY # 진행 방향으로 본 차체 반길이
half_w = abs(ux) * HY + abs(uy) * HX + MARGIN
state = "go"
for px, py in points:
along, side = px * ux + py * uy, px * nx + py * ny
if abs(side) <= half_w and ext <= along <= ext + Lp:
return "stop"
if abs(side) <= half_w + 0.5 and ext <= along <= ext + Lp + Lw:
state = "slow"
return state
person_right = [(0.0, -0.8)] # 로봇 오른쪽 0.8 m
person_front = [(0.8, 0.1)]
cases = [
("forward, person front", 0.5, 0.0, person_front),
("right lat, person right", 0.0, -0.5, person_right),
("right lat, person front", 0.0, -0.5, person_front),
("diagonal, person right", 0.35, -0.35, person_right),
]
for name, vx, vy, pts in cases:
a = zone_check(pts, vx, vy, Lp=0.7, Lw=1.0, follow=False)
b = zone_check(pts, vx, vy, Lp=0.7, Lw=1.0, follow=True)
print(f"{name:25s} fixed-front: {a:5s} follow-velocity: {b}")오른쪽으로 가는데 오른쪽 0.8 m 에 사람이 있으면 고정 영역은 go(위험), 따라가는 영역은 stop 입니다. 반대로 고정 영역은 오른쪽으로 가는 중에 앞에 있는 사람 때문에 stop 하는 헛정지를 냅니다. 대각선 이동의 반폭은 근사(지지 함수)이므로 실제 설계에서는 차체 사각형을 그대로 쓸어 낸(sweep) 다각형을 씁니다.
계획의 실습 "창고 장애물 5종을 센서별로 감지 가능/불가 표로 만들고 최소 센서 구성 제안" 입니다. 시뮬레이터 4 의 판정 규칙을 간단한 식으로 옮겼고, 유리 대응을 위해 초음파 센서(가정 사양)를 하나 더 넣었습니다.
import itertools
import math
# 장애물: 이름, 아래 z0, 위 z1 (m), 로봇 앞 끝에서 거리 d (m), 방향 (deg), 반사율, 유리?
OBJECTS = [
("forklift fork", 0.00, 0.05, 1.5, 0, 0.5, False),
("table top", 0.70, 0.75, 1.2, 0, 0.5, False),
("glass panel", 0.00, 2.00, 2.0, 0, 0.5, True),
("black box", 0.00, 0.50, 5.0, 0, 0.05, False),
("person at 60", 0.00, 1.70, 1.0, 60, 0.5, False),
]
FRONT = 0.265
def lidar2d(z0, z1, d, bearing, rho, glass, h=0.229, x=0.179):
r = d + FRONT - x
# TODO 1: 유리가 아니고, 스캔 높이 h 가 [z0, z1] 안이고, r <= 15*sqrt(rho/0.70)
return False
def lidar3d(z0, z1, d, bearing, rho, glass, h=0.45, low=-7, high=52):
r = d + FRONT
lo = h + r * math.tan(math.radians(low)) # 그 거리에서 가장 낮은 광선의 높이
hi = h + r * math.tan(math.radians(high))
# TODO 2: 유리가 아니고, [z0, z1] 과 [lo, hi] 가 겹치고, r <= min(70, 40*sqrt(rho/0.10))
return False
def depth_cam(z0, z1, d, bearing, rho, glass, h=0.169, x=0.232, vfov=33, hfov=45.5):
r = d + FRONT - x
rmax = 3.0 if rho < 0.1 else 10.0
lo, hi = h - r * math.tan(math.radians(vfov)), h + r * math.tan(math.radians(vfov))
# TODO 3: 유리가 아니고, |bearing| <= hfov, 0.15 <= r <= rmax, [z0, z1] 과 [lo, hi] 가 겹침
return False
def ultrasonic(z0, z1, d, bearing, rho, glass):
# 가정: 앞 +-15 deg, 0.03 ~ 2.5 m, 높이 0.10 ~ 0.60 m 를 봄. 소리라서 유리 · 검은색과 무관
r = d + FRONT
return abs(bearing) <= 15 and 0.03 <= r <= 2.5 and z1 >= 0.10 and z0 <= 0.60
SENSORS = {"2D lidar": lidar2d, "MID-360": lidar3d, "depth cam": depth_cam, "ultrasonic": ultrasonic}
print(f"{'object':15s}" + "".join(f"{k:>11s}" for k in SENSORS))
table = {}
for o in OBJECTS:
row = {k: f(*o[1:]) for k, f in SENSORS.items()}
table[o[0]] = row
print(f"{o[0]:15s}" + "".join(f"{('O' if v else '-'):>11s}" for v in row.values()))
# TODO 4: itertools.combinations 로 센서 1 개, 2 개, ... 조합을 차례로 보며
# 모든 장애물을 (하나 이상의 센서로) 감지하는 가장 작은 조합을 찾아 출력import itertools
import math
# 장애물: 이름, 아래 z0, 위 z1 (m), 로봇 앞 끝에서 거리 d (m), 방향 (deg), 반사율, 유리?
OBJECTS = [
("forklift fork", 0.00, 0.05, 1.5, 0, 0.5, False),
("table top", 0.70, 0.75, 1.2, 0, 0.5, False),
("glass panel", 0.00, 2.00, 2.0, 0, 0.5, True),
("black box", 0.00, 0.50, 5.0, 0, 0.05, False),
("person at 60", 0.00, 1.70, 1.0, 60, 0.5, False),
]
FRONT = 0.265
def lidar2d(z0, z1, d, bearing, rho, glass, h=0.229, x=0.179):
r = d + FRONT - x
return (not glass) and z0 <= h <= z1 and r <= 15 * math.sqrt(rho / 0.70)
def lidar3d(z0, z1, d, bearing, rho, glass, h=0.45, low=-7, high=52):
r = d + FRONT
lo = h + r * math.tan(math.radians(low)) # 그 거리에서 가장 낮은 광선의 높이
hi = h + r * math.tan(math.radians(high))
return (not glass) and z1 >= lo and z0 <= hi and r <= min(70, 40 * math.sqrt(rho / 0.10))
def depth_cam(z0, z1, d, bearing, rho, glass, h=0.169, x=0.232, vfov=33, hfov=45.5):
r = d + FRONT - x
rmax = 3.0 if rho < 0.1 else 10.0
lo, hi = h - r * math.tan(math.radians(vfov)), h + r * math.tan(math.radians(vfov))
return (not glass) and abs(bearing) <= hfov and 0.15 <= r <= rmax and z1 >= lo and z0 <= hi
def ultrasonic(z0, z1, d, bearing, rho, glass):
# 가정: 앞 +-15 deg, 0.03 ~ 2.5 m, 높이 0.10 ~ 0.60 m 를 봄. 소리라서 유리 · 검은색과 무관
r = d + FRONT
return abs(bearing) <= 15 and 0.03 <= r <= 2.5 and z1 >= 0.10 and z0 <= 0.60
SENSORS = {"2D lidar": lidar2d, "MID-360": lidar3d, "depth cam": depth_cam, "ultrasonic": ultrasonic}
print(f"{'object':15s}" + "".join(f"{k:>11s}" for k in SENSORS))
table = {}
for o in OBJECTS:
row = {k: f(*o[1:]) for k, f in SENSORS.items()}
table[o[0]] = row
print(f"{o[0]:15s}" + "".join(f"{('O' if v else '-'):>11s}" for v in row.values()))
light = ["2D lidar", "MID-360", "depth cam"]
best = None
for n in range(1, len(SENSORS) + 1):
for combo in itertools.combinations(SENSORS, n):
if all(any(table[o][s] for s in combo) for o in table):
best = combo
break
if best:
break
print("smallest set covering all :", best)
for n in range(1, 4):
for combo in itertools.combinations(light, n):
miss = [o for o in table if not any(table[o][s] for s in combo)]
print(f"{' + '.join(combo):32s} misses: {miss}")빛 센서 세 개를 모두 달아도 유리는 남고, 초음파를 더하면 MID-360 + depth cam + ultrasonic 이 다섯 종을 모두 덮는 가장 작은 조합입니다. 2D 라이다는 이 표에서는 빠지지만 360° 를 가볍게 보고 지도 만들기에도 쓰이므로 실제로는 함께 두는 경우가 많습니다. 제안할 때는 "표의 어느 칸을 어떤 센서가 메우는가" 를 근거로 적습니다.
창고 시뮬레이터로 확인하기#
Playground 의 타깃을 Simulator 로 두면 공식 URDF 로 만든 3D 창고 시뮬레이터에 from pymycobot import MyAGVPro 가 그대로 연결됩니다. agvsim 은 참값 위치 · 스캔 · 지도처럼 실물에는 없는 정보를 읽는 시뮬레이터 전용 모듈입니다 (실물에서는 ROS 2 토픽이 그 역할). 가속 한계 · 미끄러짐 · 보호 영역 거리 등은 교육용 가정입니다. 시뮬레이터의 2D 라이다는 URDF 의 laser_link 높이(바닥에서 0.229 m)에서 수평으로 360 개(1° 간격) 광선을 쏩니다. 그보다 낮은 물체는 보지 못합니다 — 미니맵에서 점선으로 그린 상자가 그런 물체입니다.
Playground (시뮬레이터 · 장애물 코스) 열기
통로 오른쪽 차선(y = −0.5 m)에서 0.3 m/s 로 직진합니다. 보호 영역(0.35 m 정지 · 0.8 m 감속)은 켜져 있습니다.
먼저 예측하세요. 앞 2.6 m 지점에 높이 0.14 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()
def front_min(scan, half_deg=5):
"""정면 +-half_deg 안에서 가장 가까운 거리 (None = 아무것도 안 맞음)"""
best = None
for i, r in enumerate(scan["ranges"]):
a = scan["angle_min"] + i * scan["angle_increment"]
if r is not None and abs(math.degrees(a)) <= half_deg:
best = r if best is None else min(best, r)
return best
agvsim.set_safety(True)
agvsim.set_pose(0.0, -0.5, 0.0) # 통로 오른쪽 차선
time.sleep(0.3)
print("front min at start:", front_min(agvsim.get_scan()), "m")
agv.move_forward(0.3)
t0 = time.time()
while time.time() - t0 < 12:
time.sleep(0.5)
machine = agv.get_robot_status()
x, y, _ = agvsim.get_pose()
if machine[2] or machine[3]:
print(f"BUMPER at x={x:.2f} m status={machine}")
break
agv.stop()
print("map boxes lower than lidar:",
[b for b in agvsim.get_map()["boxes"] if b["h"] < 0.229])관찰 · 해설
출발할 때 정면 ±5° 의 최소 거리는 수 m(멀리 있는 벽)로 나옵니다 — 팔레트가 스캔에 **없습니다**. 로봇은 감속 없이 달리다 x ≈ 2.0 m 에서 범퍼 비트(`get_robot_status()[2]`)가 1 이 되며 멈춥니다. 2D 라이다 보호 영역만으로는 지게차 포크 · 팔레트 · 낮은 상자를 막을 수 없다는 뜻이고, 4교시의 3D 라이다 · 깊이 카메라가 필요한 이유입니다. 실물 현장에서는 이런 물체를 지도에 금지 구역(keepout)으로 넣거나 범퍼 · 3D 센서를 함께 씁니다.같은 장면에서 scan = agvsim.get_scan() 을 받아 2교시의 극좌표 → 직교좌표 변환으로 점을 만들고, 정면(±30°) · 왼쪽 · 오른쪽 구역의 최소 거리를 출력하세요. 시뮬레이터 화면에서 로봇을 키보드로 옮겨 가며(새 탭) 숫자가 미니맵의 빨간 점과 맞는지 확인합니다. ranges 의 None 은 12 m 안에서 아무것도 맞지 않은 방향입니다 (실물 드라이버에서는 inf 로 옵니다).
실습 (실물)#
ROS 2 드라이버를 띄운 뒤(방법은 공식 문서 · ch09) 스캔 토픽을 확인합니다. 토픽 이름 · 프레임 이름 · angle_min 은 드라이버 버전에 따라 다를 수 있으니 먼저 목록과 메시지를 읽고 코드에 반영합니다 (확인 필요).
ros2 topic list | grep -i scan # 스캔 토픽 이름 찾기 (예: /scan)
ros2 topic hz /scan # 회전 주파수 (6 ~ 12 Hz 범위인가?)
ros2 topic echo /scan --once --field angle_min # 시작 각도
ros2 topic echo /scan --once --field angle_increment
ros2 topic info /scan --verbose # QoS (RELIABLE / BEST_EFFORT) 확인아래는 2 · 3교시의 로직을 ROS 2 노드로 옮긴 실물 전용 예시입니다. 앞 ±30° 의 유효 최솟값이 보호 영역 안이면 cmd_vel 에 0 을 보냅니다. 실제 운용에서는 Nav2 의 안전 기능(ch09)이나 인증된 안전 장치와 함께 써야 합니다.
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
A, TR, MARGIN = 1.5, 0.3, 0.1 # 감속도 · 반응 시간 · 여유 (가정값 - 실측으로 바꿀 것)
class SafetyStop(Node):
def __init__(self):
super().__init__("safety_stop")
self.v_cmd = 0.2 # 시험 속도 (m/s)
self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
# 라이다 드라이버는 보통 BEST_EFFORT 로 발행 -> sensor_data QoS 로 구독
self.create_subscription(LaserScan, "/scan", self.on_scan, qos_profile_sensor_data)
def front_min(self, msg, half=math.radians(30)):
best = math.inf
for i, r in enumerate(msg.ranges):
if not (msg.range_min <= r <= msg.range_max):
continue # 0 · inf · nan 걸러내기
a = msg.angle_min + i * msg.angle_increment
if abs(math.atan2(math.sin(a), math.cos(a))) <= half:
best = min(best, r)
return best
def on_scan(self, msg):
lp = self.v_cmd * TR + self.v_cmd ** 2 / (2 * A) + MARGIN
d = self.front_min(msg)
tw = Twist()
tw.linear.x = 0.0 if d < lp else self.v_cmd
self.pub.publish(tw)
self.get_logger().info(f"front {d:.2f} m, Lp {lp:.2f} m -> v {tw.linear.x:.2f}")
def main():
rclpy.init()
node = SafetyStop()
try:
rclpy.spin(node)
finally:
node.pub.publish(Twist()) # 어떤 경우에도 정지 명령을 남기고 끝냄 (ch03)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()실물에서 해 볼 것:
- 로봇을 멈춘 상태에서
ros2 topic echo /scan --once로angle_min·angle_increment·ranges길이를 읽고, 실습 1-2 의index_of()로 "앞" 인덱스를 계산합니다. 상자를 로봇 정면 1 m 에 두고 그 인덱스의 값이 약 1 m 인지 확인합니다 (라이다 위치 0.179 m 를 생각하면 몇 m 여야 할까요?). - 지름이 다른 막대(2 · 5 · 10 cm)를 3 m · 6 m 에 세우고
ranges에서 몇 개의 점이 걸리는지 셉니다. 실습 1-1 의 예측과 비교합니다. - 낮은 상자(15 cm) · 유리판 · 검은 천을 씌운 상자를 차례로 두고 스캔에 보이는지 기록해 4교시 표의 "2D 라이다" 열과 비교합니다.
- 0.2 m/s 로 상자를 향해 가며
SafetyStop이 멈춘 위치를 줄자로 잽니다. 상자와의 남은 거리가Lp - d_stop정도인지 확인하고, 차이가 나면 반응 시간 · 감속도 가정을 고칩니다.
자주 나는 오류와 해결#
1) 아무것도 없는데 로봇이 계속 비상정지함#
front min = 0.000 m -> STOP (repeats every scan)- 원인: 측정 실패값
0.0(또는range_min미만 값)을 걸러 내지 않고min(ranges)를 썼습니다. - 해결:
range_min ≤ r ≤ range_max인 값만 남깁니다 (실습 2-1clean()). 걸러 낸 뒤 필터를 겁니다. 순서가 바뀌면 0 값이 필터 안에서 이웃 값까지 끌어내립니다.
2) "앞" 장애물 거리가 엉뚱하게 뒤쪽 값으로 나옴#
ranges[0] = 0.41 m # but the box is 1.2 m in FRONT, wall is 0.4 m BEHIND- 원인:
ranges[0]을 앞이라고 가정했습니다.angle_min = -π이면ranges[0]은 뒤쪽이고, 앞은N/2근처입니다. - 해결: 인덱스는 항상
i = round((φ - angle_min) / angle_increment) % N으로 계산합니다 (실습 1-2). 라이다가 거꾸로 달렸거나 회전 방향이 반대인 경우도 있으니, 실물에서 상자를 앞 · 왼쪽에 두고 인덱스를 직접 확인합니다.
3) 평균 · 좌표 변환 결과가 nan 이나 inf 로 나옴#
RuntimeWarning: invalid value encountered in multiply
mean range = nan- 원인:
inf에cos(φ)를 곱하면inf·nan이 나오고, 하나만 섞여도np.mean이 무너집니다. 모두 무효인 영역에서np.nanargmin은ValueError: All-NaN slice encountered를 냅니다. - 해결:
np.isfinite로 고른 값만 변환하고, 평균 · 최솟값은np.nanmean·np.nanmin을 쓰되 "모두 무효" 인 경우를 먼저 검사합니다 (실습 2-1nearest()).
4) 필터를 넣었더니 얇은 기둥 · 의자 다리에 부딪힘#
raw front min 0.58 m (STOP) -> median(k=7) front min 1.20 m (GO)- 원인: 창 크기의 절반보다 적은 광선에 걸리는 물체는 중앙값 필터가 지우고, 이동 평균은 배경과 섞어 멀리 보이게 합니다 (시뮬레이터 2).
- 해결: 막아야 할 가장 얇은 물체가 몇 개의 광선에 걸리는지(
(w/d)/Δφ) 계산해 창 크기를 그보다 작게 둡니다. 튀는 값은 필터 대신 "연속 2 스캔 이상에서 보이면 정지" 처럼 시간 방향 으로 확인하는 방법도 있습니다.
5) 옆으로 갈 때만 장애물에 닿을 뻔함#
cmd: move_right_lateral(0.5) front zone: clear -> contact on the right side- 원인: 보호 영역을 로봇 앞쪽(+x)에만 두었습니다. 전방향 로봇은 옆 · 대각선으로도 갑니다.
- 해결: 영역을 속도 벡터
(vx, vy)방향으로 돌리고, 폭은 그 방향에서 본 차체 폭으로 잡습니다 (실습 3-2). 회전 중에는 차체 둘레 전체를 봅니다.
6) 스캔 구독 노드가 메시지를 하나도 받지 못함#
[WARN] [safety_stop]: New publisher discovered on topic '/scan', offering incompatible QoS. No messages will be received from it. Last incompatible policy: RELIABILITY- 원인: 라이다 드라이버는
BEST_EFFORT로 발행하는데 구독 쪽이 기본값RELIABLE을 요구했습니다. - 해결:
qos_profile_sensor_data(BEST_EFFORT) 로 구독합니다.ros2 topic info /scan --verbose로 발행자의 QoS 를 먼저 확인합니다.
과제#
- 해상도 설계표: 실습 1-1 을 확장해 거리 2 · 5 · 10 · 15 m 와 물체 지름 2 · 5 · 10 cm 의 조합마다 "항상 2 점 이상 맞는" 최대 Δφ 를 표로 만들고, N10 Plus(가정 0.8°)로 보장되는 칸에 표시합니다.
- 필터 선택 보고서: 실습 2-2 를 노이즈 σ = 1 · 3 · 5 cm, 먼지 비율 0.5 · 1.5 · 3 % 로 돌려 "필터 없음 · 중앙값 3 · 중앙값 3 + 연속 2 스캔 확인" 의 틀린 판정 수를 비교하고, 이 강좌 로봇에 쓸 방식을 한 문단으로 고릅니다.
- 영역 세트 설계: 속도 0.3 · 0.6 · 1.0 · 1.5 m/s 각각의 보호 · 경고 영역 길이와 폭을 앞 · 옆 주행에 대해 정하고(실습 3-1 · 3-2), 시뮬레이터 3 의 100회 실험 성공률 · 주행 시간으로 근거를 붙입니다.
3 ~ 4 명이 한 팀이 되어, 가상의 창고(또는 실습실)를 하나 정해 다음을 만듭니다.
- 사각지대 조사표: 공간에서 낮은 물체 · 돌출물 · 유리 · 검은 물체 · 가려진 출입구를 찾아 위치와 높이를 적고, 시뮬레이터 4 로 센서별 감지 여부를 표시합니다 (실습 4-1 의 표 형식).
- 센서 구성 제안: 표의 빈칸을 메우는 최소 구성과, 메우지 못하는 칸의 운영 규칙(서행 구역 · 바닥 표식 · 출입구 감속)을 제안합니다.
- 영역 · 속도 규칙: ch04 의 오도메트리 오차와 이번 차시의 정지 거리를 함께 고려해, 구역별 최고 속도와 보호 영역 크기를 정합니다. 시뮬레이터 3 의 성공률을 근거로 씁니다.
- 발표 (5 분): "우리 로봇이 가장 못 보는 것 3 가지와 대책" 을 시뮬레이터 화면과 함께 발표합니다.
참고자료#
- myAGV Pro 공식 문서 (센서 옵션 · ROS 2): https://docs.elephantrobotics.com/docs/myAGV_Pro_en/
- myAGV Pro ROS 2 패키지 (URDF 의
laser_link· 드라이버): https://github.com/elephantrobotics/agv_pro_ros2 - 제품 페이지: https://www.elephantrobotics.com/en/myagv-pro/
- 제품 소개 · 이미지 (현영이앤씨): https://www.hyenc.net/products/36
sensor_msgs/LaserScan메시지 정의: https://docs.ros.org/en/humble/p/sensor_msgs/interfaces/msg/LaserScan.html- REP 103 — 단위 · 좌표 관례 (각도 반시계 +): https://www.ros.org/reps/rep-0103.html
- ROS 2 QoS 설정 (BEST_EFFORT · RELIABLE): https://docs.ros.org/en/humble/Concepts/Intermediate/About-Quality-of-Service-Settings.html
- Livox MID-360: https://www.livoxtech.com/mid-360
- Unitree L2 (4D LiDAR): https://www.unitree.com/LiDAR/
- Orbbec Gemini 2: https://www.orbbec.com/products/stereo-vision-camera/gemini-2/
- 이 사이트의 도구: Playground · 물류창고 시뮬레이터 · 3D 뷰어