CH07위치 추정 — 파티클 필터와 AMCL
이미 있는 지도 위에서 '나는 어디에 있나' 를 확률로 푸는 파티클 필터를 시뮬레이터로 돌려 보며 AMCL 의 원리를 이해합니다.
학습 목표#
- 위치를 점 하나가 아니라 확률 분포(믿음, belief) 로 표현해야 하는 이유를 설명하고, 베이즈 필터의 예측(이동) · 측정 두 단계가 분포를 각각 "퍼뜨리고" · "뾰족하게" 만드는 것을 1D 복도 시뮬레이터와 numpy 코드로 확인할 수 있습니다.
- 파티클 필터의 한 스텝(예측 → 가중치 → 리샘플링)을 numpy 로 구현하고, 파티클 수 · 이동 노이즈 · 측정 노이즈가 정확도와 계산 시간에 주는 영향을 시뮬레이터 실험으로 비교할 수 있습니다.
- 전역 위치 추정이 어려운 이유(같은 모양 선반 열이 만드는 다봉 분포 · 오수렴)와 납치 로봇 문제를 설명하고, 무작위 파티클 주입(α_slow · α_fast)과 RViz 의 2D Pose Estimate 가 각각 어떤 문제를 푸는지 시뮬레이터로 확인할 수 있습니다.
- myAGV Pro 의
agvpro.yamlamcl 섹션(파티클 수 · alpha1 ~ 5 · update_min_d/a · 운동 모델)을 읽고, 전방향 로봇에 차동 운동 모델을 잘못 고르면 횡이동 때 위치가 튀는 이유를 설명할 수 있습니다. - AMCL 의 결과가
map → odom변환으로 TF 에 들어가는 흐름을 ch04 의 TF 트리와 연결해 설명할 수 있습니다.
이론#
ch06 에서는 지도를 만들었습니다. 이번 차시는 그 지도를 들고 다니며 "나는 지금 지도의 어디에 있나" 를 답합니다. 지도가 이미 있으니 SLAM 보다 쉬워 보이지만, 두 가지가 문제입니다. 바퀴 오도메트리는 갈수록 틀어지고(ch04), 라이다 스캔 한 장만으로는 똑같이 생긴 선반 통로를 구별하지 못합니다. 그래서 위치를 "여기" 라고 단정하지 않고 "여기일 확률이 높고, 저기일 수도 있다" 는 분포로 들고 다닙니다. Nav2 가 쓰는 AMCL(Adaptive Monte Carlo Localization) 은 이 분포를 수백 ~ 수천 개의 점(파티클)으로 표현하는 파티클 필터입니다.
| 항목 | 값 | 출처 |
|---|---|---|
| 위치 추정 노드 | nav2_amcl (Nav2 bringup 이 map_server 와 함께 띄움) | agv_pro_ros2 agv_pro_navigation2/launch/navigation2_active.launch.py |
| 파라미터 파일 | agv_pro_navigation2/param/agvpro.yaml 의 amcl: 섹션 | agv_pro_ros2 저장소 (humble 브랜치) |
| 파티클 수 | min_particles: 500 · max_particles: 2000 | agvpro.yaml |
| 운동 모델 | robot_model_type: "nav2_amcl::OmniMotionModel" (전방향) | agvpro.yaml |
| 운동 노이즈 | alpha1: 0.4 · alpha2: 0.3 · alpha3: 0.1 · alpha4: 0.1 · alpha5: 0.04 | agvpro.yaml |
| 측정 모델 | laser_model_type: "likelihood_field" · sigma_hit: 0.02 · max_beams: 60 | agvpro.yaml |
| 갱신 조건 | update_min_d: 0.025 m · update_min_a: 0.06 rad · resample_interval: 2 | agvpro.yaml |
| 무작위 주입 | recovery_alpha_slow: 0.0 · recovery_alpha_fast: 0.0 (꺼짐) | agvpro.yaml |
| 초기 위치 | set_initial_pose: True · initial_pose = (0, 0, 0) | agvpro.yaml |
- 시뮬레이터의 창고(10 m × 6 m) · 라이다 · 오도메트리 슬립은 교육용 모델 입니다. 가운데 세 통로는 일부러 똑같이 만들었습니다 (3교시의 대칭 문제를 보기 위해).
- 운동 모델(Omni · Differential)은 nav2_amcl 의 구조(어떤 노이즈가 어느 alpha 에 비례하는지)를 따랐지만, 표준편차를 √(…) 로 두는 등 세부 계산은 단순화했습니다. 정확한 식은 nav2_amcl 소스를 확인하세요 (확인 필요).
- 측정 모델은 AMCL 과 같은 likelihood field(빔 끝점에서 가장 가까운 장애물까지 거리)입니다. 다만 빔 확률을 그대로 곱하면 너무 뾰족해지므로 지수 0.3 을 걸어 완만하게 합쳤습니다. nav2_amcl 의 likelihood field 구현은 빔 확률을 곱하는 대신 세제곱을 더하는(p = 1 + Σ pz³) 방식으로 완만하게 합칩니다 (확인 필요).
- 라이다는 로봇 중심에 있다고 가정했습니다 (실제
laser_link는 base_link 앞 0.179 m, ch01). - 시간은 시뮬레이션 시간입니다. 로봇은 0.5 m/s 로 달리고, 0.1 s 가 한 틱입니다.
1교시 · 확률로 보는 위치#
왜 점이 아니라 분포인가#
ch04 의 오도메트리는 "로봇은 (2.70, 0.85) 에 있다" 처럼 점 하나를 냅니다. 그런데 그 점이 얼마나 믿을 만한지는 알려 주지 않습니다. 10 m 를 달린 뒤의 (2.70, 0.85) 와 출발 직후의 (2.70, 0.85) 는 같은 숫자지만 믿음의 크기가 전혀 다릅니다.
위치를 분포로 들고 다니면 세 가지가 생깁니다.
| 분포로 얻는 것 | 예 | 쓰는 곳 |
|---|---|---|
| 불확실성의 크기 | "x 는 ±5 cm, y 는 ±30 cm 정도 틀릴 수 있다" | /amcl_pose 의 공분산(covariance), Nav2 의 판단 |
| 여러 가설을 동시에 | "1번 통로 아니면 3번 통로" | 똑같이 생긴 선반 열 (3교시) |
| 증거를 합치는 규칙 | 새 스캔이 오면 맞는 가설은 키우고 틀린 가설은 줄임 | 베이즈 정리 |
베이즈 필터 — 두 단계의 반복#
위치에 대한 믿음 bel(x) 를 두 단계로 고쳐 갑니다.
| 단계 | 언제 | 식 | 분포의 변화 |
|---|---|---|---|
| 예측 (prediction, 이동) | 로봇이 움직였다는 소식(오도메트리 u)이 왔을 때 | bel⁻(x) = Σ P(x | x′, u) · bel(x′) | 이동만큼 밀리고, 이동 노이즈만큼 퍼짐 |
| 측정 (correction, 센서) | 센서 값 z 가 왔을 때 | bel(x) = η · P(z | x) · bel⁻(x) | 측정과 맞는 곳은 커지고 아닌 곳은 줄어 뾰족해짐 |
- P(x | x′, u) 는 운동 모델: "x′ 에서 u 만큼 가라고 했을 때 실제로 x 에 도착할 확률". 바퀴 슬립 같은 이동 노이즈가 여기에 들어갑니다.
- P(z | x) 는 측정 모델(우도, likelihood): "x 에 있다면 센서가 z 를 읽을 확률". 위치 x 마다 하나씩 계산합니다.
- η 는 곱한 결과의 합이 1 이 되게 나누는 정규화 상수 입니다. η = 1 / Σ P(z | x) · bel⁻(x).
예측하기: 문이 3개인 복도에서 문을 한 번 보면?#
20 칸짜리 복도에 문이 3 개(3 · 8 · 15 번 칸) 있습니다. 로봇은 자기가 어디 있는지 전혀 모릅니다(모든 칸 1/20 = 0.05). 센서는 문 앞에서 90 % 확률로 "문" 이라고 읽고, 벽 앞에서도 10 % 확률로 "문" 이라고 잘못 읽습니다. 센서가 "문" 이라고 읽었습니다.
시뮬레이터를 돌리기 전에 공책에 적어 보세요.
- 문 앞 칸(예: 3 번 칸) 하나의 확률은 0.05 보다 커질까요? 얼마쯤일까요? 1/3 일까요?
- 벽 앞 칸 하나의 확률은 0 이 될까요?
- 세 문 칸의 확률을 모두 더하면 1 이 될까요?
시뮬레이터 1 · 1D 복도 베이즈 필터#
막대 하나가 칸 하나의 믿음 bel(x) 입니다. 칸을 클릭하면 로봇의 실제 위치를 옮길 수 있습니다 (필터는 실제 위치를 모르고, 센서 값과 이동 명령만 받습니다). 점선은 바로 앞 단계의 분포입니다.
해 볼 것
- 예측 확인 — 처음 상태(문 3개, 0.9 / 0.1)에서 '문' 으로 측정 을 한 번 누릅니다. 문 칸 하나의 bel 과 벽 칸 하나의 bel, 문 칸들의 합을 읽고 공책의 예측과 비교합니다. 출력의 η 가 어떻게 계산됐는지 따라가 봅니다.
- 이어서 → 1칸 이동 을 누릅니다. 봉우리 세 개는 어디로 갔고, 높이는 어떻게 바뀌었나요? 엔트로피 H 는 커졌나요, 작아졌나요? 같은 이동을 측정 없이 5 번 더 누르면 분포는 어떤 모양으로 가나요?
- 모름 (균등) 으로 를 누르고, 실제 로봇을 3 번 칸에 둔 뒤 ▶ 이동 + 센서 × 10 을 누릅니다. 실제 칸의 bel 이 0.5 를 처음 넘는 것은 문을 몇 번째 지났을 때인가요? 왜 첫 번째 문만으로는 부족할까요?
- 문 배치를 문 4개 (등간격) 으로 바꿔 3 번을 반복합니다 (오탐 0, 이동 노이즈 0 으로 두고 '센서 읽기' 와 '이동' 을 번갈아 눌러도 됩니다). 몇 번을 돌아도 봉우리가 4 개 남는 이유를 설명해 봅니다.
- 오탐 P(문 | 벽 앞) 을 0 으로 두고, 실제 로봇이 벽 앞에 있을 때 '문' 으로 측정 을 누릅니다. 무슨 일이 생기나요? 오탐을 0.01 로 두고 같은 실험을 하면 어떻게 다른가요?
관찰 포인트 — 측정은 곱하기, 이동은 밀고 나눠 주기입니다. 문을 한 번 본 직후 문 칸 하나의 확률은 0.9 / (3 × 0.9 + 17 × 0.1) = 0.9 / 4.4 ≈ 0.205 로, 1/3 이 아닙니다. 벽 앞일 가능성(17 칸 × 0.1)이 아직 꽤 크기 때문입니다. 이동은 분포를 밀면서 퍼뜨리므로 엔트로피를 늘리고, 측정은 대개 엔트로피를 줄입니다. 문 사이 간격이 서로 다르면(5 · 7 · 8 칸) "문을 본 뒤 몇 칸 만에 다음 문이 나왔는가" 가 위치를 가려 주지만, 등간격이면 어떤 측정 순서로도 네 후보를 구별할 수 없습니다. 오탐을 0 으로 두면 "벽 앞에서는 절대 '문' 이 안 나온다" 고 믿게 되어, 실제로 오탐이 한 번 나면 실제 위치의 확률이 0 이 되고 다시는 살아나지 못합니다.
문 3개(3 · 8 · 15 번 칸)인 20 칸 복도, 균등 분포에서 시작합니다. 센서가 적중 0.9 · 오탐 0.1 일 때 '벽' 이라고 읽었다면, 벽 칸 하나의 확률은?
- 0.05 그대로 (벽은 정보가 없으므로)
- 0.9 / (17 × 0.9 + 3 × 0.1) ≈ 0.0577
- 1 / 17 ≈ 0.0588
- 0.9 × 0.05 = 0.045
정답 보기
② — P(벽 | 벽 앞) = 1 − 0.1 = 0.9, P(벽 | 문 앞) = 1 − 0.9 = 0.1 입니다. 곱한 합은 0.05 × (17 × 0.9 + 3 × 0.1) = 0.78, 벽 칸 하나는 0.05 × 0.9 / 0.78 ≈ 0.0577, 문 칸 하나는 0.05 × 0.1 / 0.78 ≈ 0.0064 입니다. '벽' 도 정보입니다(①). 다만 벽 칸이 많아서 벽 칸 하나는 조금만 커집니다. ③은 문 칸을 0 으로 지운 경우(적중 1.0)이고, ④는 정규화(η)를 빠뜨린 값입니다. 시뮬레이터의 '벽' 으로 측정 버튼으로 확인할 수 있습니다.시뮬레이터에서 문 4개(등간격, 2 · 7 · 12 · 17)인 순환 복도에 오탐 0 · 이동 노이즈 0 으로 이동과 센서를 50 번 반복했더니 봉우리 4 개가 각각 0.25 로 남았습니다. 가장 알맞은 해석은?
- 측정이 부족하다. 100 번 반복하면 하나로 모인다
- 복도가 5 칸마다 똑같이 생겨서, 어떤 위치든 5 칸 옮긴 위치와 측정 순서가 똑같다. 이 센서로는 구별할 수 없다
- 이동 노이즈가 0 이라서 분포가 움직이지 못한다
- 정규화를 잘못해서 합이 1 이 아니다
정답 보기
② — 5 칸 떨어진 네 후보는 앞으로 받을 모든 측정이 똑같아서 우도가 영원히 같습니다. 반복을 늘려도(①) 비율은 그대로입니다. 분포는 이동마다 잘 밀리고 있고(③), 0.25 × 4 = 1 이므로 정규화도 맞습니다(④). 이것이 3교시의 "똑같은 선반 열" 문제의 1D 판입니다. 해결은 센서 밖의 정보(초기 위치 알려 주기)나 대칭을 깨는 특징(문 하나를 다른 색으로)입니다.한 칸에 0.8 로 확신하고 있던 분포에서 측정 없이 → 1칸 이동 만 10 번 했습니다 (ε = 0.1, 순환). 엔트로피 H 는?
- 그대로 — 이동은 정보를 바꾸지 않는다
- 줄어든다 — 이동할수록 위치를 더 잘 안다
- 늘어난다 — 이동할 때마다 이동 노이즈만큼 퍼진다
- 0 이 된다
정답 보기
③ — 예측 단계는 확률을 옆 칸으로 나눠 주므로 봉우리가 낮고 넓어집니다. 측정 없이 오래 달리면 결국 균등 분포(4.32 bit)에 가까워집니다. ch04 에서 오도메트리만으로 달리면 오차가 쌓이던 것을 확률로 본 모습입니다. 그래서 AMCL 은 움직일 때마다 라이다 측정으로 다시 뾰족하게 만들어야 합니다.2교시 · 파티클 필터 동작 관찰#
칸 나누기의 한계 — 왜 파티클인가#
1교시의 막대그래프(히스토그램 필터)를 창고에 그대로 쓰면 어떻게 될까요? 10 m × 6 m 창고를 5 cm 칸으로 나누고 방향을 5° 씩 나누면 200 × 120 × 72 ≈ 173 만 칸 입니다. 측정이 올 때마다 173 만 칸 모두에 P(z | x) 를 계산해야 하는데, 그중 대부분은 확률이 거의 0 인 칸(선반 안, 반대쪽 끝)입니다.
파티클 필터(particle filter) 는 칸을 미리 나누지 않고, 확률이 있는 곳에만 점(파티클) 을 뿌립니다. 파티클이 많이 모인 곳 = 확률이 높은 곳입니다.
| 용어 | 뜻 | myAGV Pro (AMCL) |
|---|---|---|
| 파티클 (particle) | 위치 가설 하나 (x, y, θ) — "로봇이 여기 이 방향으로 서 있을지도" | 500 ~ 2000 개 |
| 가중치 (weight) w | 그 자리에서 라이다 스캔이 지도와 얼마나 잘 맞는지 | likelihood field 로 계산 |
| 추정 위치 | 가장 무거운 파티클 무리의 가중 평균 | /amcl_pose |
| 불확실성 | 파티클 구름이 퍼진 정도 | /amcl_pose 의 공분산 |
한 스텝 = 예측 · 가중치 · 리샘플링#
- 예측 — 오도메트리가 "앞으로 0.1 m, 옆으로 0, 회전 0" 이라고 하면, 파티클마다 자기 방향 기준으로 0.1 m 를 옮기고 운동 모델의 노이즈(alpha1 ~ 5 에 비례)를 따로따로 더합니다. 같은 명령인데도 파티클마다 조금씩 다르게 가므로 구름이 퍼집니다. 1교시의 "이동은 퍼뜨린다" 와 같습니다.
- 가중치 — 라이다 빔 하나하나의 끝점을 그 파티클 자리에서 지도에 찍어 봅니다. 끝점이 지도의 벽 · 선반 바로 위에 떨어지면 잘 맞는 것입니다. AMCL 의
likelihood_field모델은 끝점에서 가장 가까운 장애물까지 거리 d 를 미리 계산된 표에서 읽어q = z_hit · exp(−d² / 2σ²) + z_rand로 빔 하나의 확률을 정합니다. σ 가sigma_hit입니다. 빔들의 확률을 합쳐 가중치로 쓰고, 모두 더해 1 이 되게 나눕니다. - 리샘플링 — 가중치에 비례하는 확률로 파티클 N 개를 다시 뽑습니다. 무거운 파티클은 여러 번 뽑혀 복제되고, 가벼운 것은 사라집니다. 개수는 그대로 N, 가중치는 다시 모두 1/N 입니다. 흔히 저분산 리샘플링(low-variance resampling) — 간격이 1/N 인 빗 하나를 누적 가중치 위에 한 번 던져 N 개를 뽑는 방법 — 을 씁니다. 무작위를 한 번만 쓰므로 운에 덜 흔들립니다.
유효 파티클 수(effective sample size) N_eff = 1 / Σ wᵢ² 는 "실제로 쓸모 있는 파티클이 몇 개인가" 입니다. 모든 가중치가 같으면 N_eff = N, 하나만 1 이고 나머지가 0 이면 N_eff = 1 입니다. N_eff 가 작다는 것은 대부분의 파티클이 틀린 곳에 있다는 뜻입니다.
리샘플링은 무거운 파티클을 복제하므로, 할 때마다 서로 다른 가설의 수가 줄어듭니다(다양성 감소). 그래서 AMCL 은 resample_interval: 2 로 필터 갱신 2 번에 1 번만 리샘플링합니다. N_eff 가 N/2 아래로 떨어질 때만 리샘플링하는 방법도 널리 씁니다. 시뮬레이터 2 의 '리샘플링' 선택에서 비교해 보세요.
파티클 수와 계산 시간#
가중치 계산이 가장 무겁습니다. 파티클 N 개 × 빔 K 개마다 지도 표를 한 번씩 읽으므로 계산량은 대략 N × K 에 비례 합니다. 파티클이 많으면 진짜 위치 근처에 파티클이 있을 가능성이 커지지만, 그만큼 느려집니다. AMCL 은 이 둘을 자동으로 맞춥니다. 파티클 구름이 넓게 퍼져 있으면 max_particles(2000)까지 늘리고, 한곳에 모이면 min_particles(500)까지 줄입니다. 몇 개면 충분한지는 KLD 샘플링 이 pf_err: 0.05 · pf_z: 0.99(분포 근사 오차 한계와 신뢰도)로 정합니다. "Adaptive" MCL 의 A 가 이것입니다.
예측하기: 파티클 50 개 vs 2000 개#
출발점 근처 ±1 m · ±30° 어딘가에 있다는 것만 알고 시작합니다. 파티클 50 개로 시작하면 수렴할까요? 2000 개면 50 개보다 계산 시간이 몇 배가 될까요? 수렴한 뒤의 위치 오차도 40 배 작아질까요?
시뮬레이터 2 · 파티클 필터 한 스텝씩 보기#
초록 상자 = 실제 로봇, 파란 점 = 파티클(짧은 선 = 파티클의 방향), 빨간 원 ◎ = 추정 위치, 빨간 점 = 실제 로봇 자리에서 찍은 라이다 끝점입니다. ① ② ③ 을 차례로 누르며 한 스텝을 나눠 보고, ▶ 자동 주행 으로 순찰 경로(아래 통로 → 선반 사이 → 위 통로)를 돌려 봅니다.
해 볼 것
- 한 스텝 나눠 보기 — 처음 상태(N 500, 대략)에서 ① → ② → ③ 을 누릅니다. ① 뒤에 퍼짐 σ 는 커졌나요? ② 뒤에 점이 커진 파티클은 어디에 모여 있고, N_eff 는 500 중 몇인가요? ③ 뒤의 "서로 다른 파티클" 수와 N_eff 를 비교해 봅니다. 이것을 3 ~ 4 번 반복하며 구름이 실제 로봇(초록)으로 모이는지 봅니다.
- 예측 확인 — 파티클 수를 50 과 2000 으로 바꿔 가며 수렴 실험 (난수 5 개) 을 누릅니다. 성공 횟수와 "갱신 1 번 ms" 를 기록합니다. 계산 시간은 대략 몇 배가 되나요? 수렴한 뒤의 위치 오차(그래프 오른쪽 끝)도 그만큼 작아지나요?
- 초기 퍼짐 정확히 로 두고 측정 노이즈 σ 를 0.02 m(agvpro.yaml 의
sigma_hit) · 0.15 m · 0.4 m 로 바꿔 가며 자동 주행 중에 ② 가중치를 눌러 N_eff 를 읽습니다. 어느 값에서 N_eff 가 가장 작나요? 이어서 초기 퍼짐 넓게 에서 σ 0.02 · 0.15 · 0.4 의 수렴 실험 결과를 비교해 봅니다. - 이동 노이즈 배율을 0.1 과 5 로 바꿔 ▶ 자동 주행합니다. 모퉁이를 돌 때(제자리 회전) 구름 모양이 어떻게 다른가요? 0.1 일 때 구름이 아주 작아지는 것은 좋은 일일까요?
- 리샘플링을 끄기 로 두고 자동 주행한 뒤 N_eff 를 봅니다. 어떻게 되나요? 다시 2 번에 1 번 으로 돌려 비교합니다.
관찰 포인트 — 예측은 구름을 퍼뜨리고, 가중치는 "어느 파티클이 맞는지" 를 점 크기로 보여 주고, 리샘플링은 맞는 파티클을 복제해 구름을 모읍니다. 계산 시간은 N 에 거의 비례하지만, 수렴한 뒤의 정확도는 N 에 비례해 좋아지지 않습니다. 정확도는 라이다 · 지도의 품질이 정하고, N 은 주로 "처음에 진짜 위치 근처에 파티클이 하나라도 있는가" 를 정합니다. 측정 노이즈 σ 를 너무 작게 잡으면 가중치가 한두 파티클에 몰려(N_eff ≈ 1) 다양성이 사라지고, 너무 크게 잡으면 스캔이 위치를 잘 가려 주지 못합니다. 리샘플링을 끄면 파티클들은 계속 움직이지만 가중치가 점점 한 파티클에 몰려 N_eff 가 1 근처로 떨어집니다(퇴화, degeneracy).
시뮬레이터 2 에서 ② 가중치를 눌렀더니 N = 500, N_eff = 3 이 나왔습니다. 가장 알맞은 해석은?
- 파티클 3 개만 남고 497 개는 지워졌다
- 가중치가 거의 3 개 파티클에 몰려 있다. 나머지는 스캔과 잘 맞지 않는 가설이다
- 필터가 완전히 수렴해 위치를 정확히 알게 되었다
- 라이다 빔이 3 개만 지도와 맞았다
정답 보기
② — N_eff = 1 / Σ w² 는 "가중치가 고르게 퍼져 있다면 몇 개짜리와 같은가" 입니다. ② 단계에서는 아무것도 지워지지 않습니다(①). 리샘플링을 하면 그 몇 개가 복제되어 500 개가 됩니다. 몇 개에 몰렸다고 그것이 정답이라는 보장은 없습니다(③) — 진짜 위치 근처에 파티클이 없었다면 "덜 틀린" 가설이 뽑힌 것입니다. N_eff 는 빔 수가 아니라 파티클 가중치의 성질입니다(④). σ 가 너무 작을 때, 또는 처음 측정 때 이런 값이 흔합니다.초기 퍼짐 '대략' 에서 수렴 실험을 했더니 다음과 같았습니다. N 50: 성공 3 / 5 · 0.19 ms, N 500: 성공 5 / 5 · 1.4 ms, N 2000: 성공 5 / 5 · 5.6 ms. 성공한 경우의 마지막 위치 오차는 셋 다 1 ~ 2 cm 로 비슷했습니다. 옳은 설명은?
- N 을 늘리면 수렴 뒤 오차도 N 에 반비례해 줄어든다
- 이 조건에서는 500 이면 충분하다. 2000 은 같은 결과에 약 4 배 시간을 쓴다. 다만 더 넓게 모를 때를 대비한 여유가 된다
- 계산 시간은 N² 에 비례한다
- 50 개로 실패한 것은 측정 노이즈가 커서다
정답 보기
② — 수렴 뒤 정확도는 센서 · 지도 품질이 정하므로 N 을 늘려도 거의 같습니다(①). 시간은 N 에 비례합니다(0.19 → 1.4 → 5.6 ms, ③ 오답). 50 개로 실패한 것은 ±1 m · ±30° 를 덮기에 파티클이 너무 성겨서 처음에 실제 위치 근처에 파티클이 하나도 없었던 경우입니다(④). AMCL 이 min 500 · max 2000 사이에서 개수를 바꾸는 이유가 이것입니다. 모를 때는 많이, 알면 적게.3교시 · 전역 위치 추정과 납치 로봇 문제#
세 가지 위치 추정 문제#
| 문제 | 처음에 아는 것 | 어려운 점 | 예 |
|---|---|---|---|
| 위치 추적 (tracking) | 대략의 시작 위치 | 오도메트리 오차를 따라잡기 | 2D Pose Estimate 를 준 뒤의 평소 주행 |
| 전역 위치 추정 (global localization) | 아무것도 모름 (지도 안 어딘가) | 지도 전체를 덮을 만큼 파티클이 필요, 비슷한 장소끼리 헷갈림 | 전원을 켰는데 로봇이 어디 있는지 모름 |
| 납치 로봇 (kidnapped robot) | "안다" 고 믿는데 사실은 틀림 | 필터가 틀렸다는 것을 스스로 알아채야 함 | 사람이 로봇을 들어 다른 통로에 놓음, 바퀴가 크게 미끄러짐 |
전역 위치 추정 — 지도 전체에 뿌리기#
시작 위치를 모르면 파티클을 지도의 빈 공간 전체에 고르게 뿌리고(방향도 무작위) 같은 필터를 돌립니다. Nav2 의 AMCL 에서는 서비스 하나로 이렇게 할 수 있습니다.
ros2 service call /reinitialize_global_localization std_srvs/srv/Empty문제는 밀도입니다. 시뮬레이터 창고의 빈 공간은 약 35 m² 입니다. 파티클 2000 개를 뿌리면 1 m² 에 약 57 개, 방향까지 생각하면 "실제 위치에서 0.2 m · 15° 안" 에 들어가는 파티클은 평균 1 개도 안 됩니다. 그래서 전역 위치 추정은 파티클이 많이 필요하고, 그래도 자주 틀린 곳에 모입니다.
똑같이 생긴 선반 열 — 다봉 분포와 오수렴#
물류 창고는 같은 선반이 같은 간격으로 늘어선 곳이 많습니다. 시뮬레이터 창고의 가운데 세 통로(선반 A-B, B-C, C-D 사이)도 똑같습니다. 라이다가 통로 안에서 양옆 선반만 본다면, 세 통로 어디에 있든 스캔이 똑같아 우도도 똑같습니다. 그러면 파티클 구름은 봉우리가 여러 개인 분포(다봉 분포, multimodal) 가 됩니다 — 1교시의 "문 4개 등간격" 과 같은 상황입니다.
여기서 두 가지 함정이 생깁니다.
- 평균의 함정 — 봉우리 둘의 평균은 그 사이 선반 안 일 수도 있습니다. 그래서 AMCL 은 파티클을 무리(cluster)로 나눈 뒤 가장 무거운 무리 의 평균을 위치로 냅니다. 시뮬레이터의 ◎ 도 같은 방식입니다.
- 오수렴 — 리샘플링은 우연히 조금 더 무거운 봉우리를 복제합니다. 몇 번 반복하면 다른 봉우리의 파티클이 모두 사라지고, 틀린 통로 하나에 "확신" 하게 됩니다. 그 뒤에 통로 끝의 충전소 · 기둥 같은 구별되는 특징이 보여도, 그 근처에 파티클이 하나도 없으면 고칠 수 없습니다.
납치 로봇 — 무작위 파티클 주입#
필터가 한곳에 확신한 상태에서 로봇이 순간이동하면, 모든 파티클이 옛 위치 근처에 있으므로 어떤 파티클도 새 스캔과 맞지 않습니다. 가중치는 모두 낮지만 정규화하면 합은 다시 1 이 되므로, 필터는 "덜 틀린" 파티클을 복제하며 엉뚱한 곳에 머뭅니다. 이동 노이즈로 구름이 퍼지다가 운 좋게 진짜 위치 근처에 닿으면 돌아오기도 하지만, 언제 돌아올지 아무도 보장하지 못합니다.
해결책은 "스캔이 평소보다 지도와 안 맞는다" 를 감지해 무작위 파티클을 섞는 것 입니다(Augmented MCL). AMCL 은 측정이 맞는 정도의 평균 w_avg 를 두 속도로 따라갑니다.
w_slow ← w_slow + α_slow · (w_avg − w_slow) 느린 평균 (평소에 얼마나 잘 맞았나)
w_fast ← w_fast + α_fast · (w_avg − w_fast) 빠른 평균 (요즘 얼마나 잘 맞나)
주입 확률 = max(0, 1 − w_fast / w_slow) 요즘이 평소보다 나쁘면 > 0리샘플링할 때 각 파티클을 이 확률로 지도 전체의 무작위 파티클 로 바꿉니다. 그중 하나가 우연히 진짜 위치 근처에 떨어지면 그 파티클이 큰 가중치를 받아 복제되고, 필터가 새 위치로 옮겨 갑니다.
myAGV Pro 의 agvpro.yaml 은 recovery_alpha_slow: 0.0 · recovery_alpha_fast: 0.0 입니다. 0 이면 주입을 하지 않습니다. Nav2 문서는 켤 때의 값으로 recovery_alpha_slow: 0.001 · recovery_alpha_fast: 0.1 을 예로 듭니다. 켜면 납치에서 회복할 수 있지만, 평소에도 스캔이 잠깐 안 맞을 때(사람 · 팔레트가 라이다를 가릴 때) 무작위 파티클이 섞여 위치가 흔들릴 수 있습니다. 끄고 쓰는 대신 운용자가 초기 위치를 주는 것 이 기본 방법입니다.
RViz 의 2D Pose Estimate — 사람이 주는 초기 위치#
RViz 상단의 2D Pose Estimate 버튼을 누르고 지도 위에서 로봇 위치를 클릭한 채 방향으로 끌면, RViz 가 /initialpose 토픽(geometry_msgs/PoseWithCovarianceStamped)을 발행합니다. AMCL 은 이것을 받아 파티클을 그 위치 주변의 정규분포로 다시 뿌립니다. RViz 가 함께 보내는 공분산은 x · y 각 0.25 m²(표준편차 0.5 m), 방향 약 0.0685 rad²(표준편차 약 15°)입니다.
agvpro.yaml 은 set_initial_pose: True, initial_pose = (0, 0, 0) 이므로 AMCL 은 켜지자마자 "로봇은 지도의 원점에 정면을 보고 있다" 고 가정합니다. 로봇이 지도를 만들 때의 출발점(보통 map 의 원점)에서 켜졌다면 그대로 맞지만, 다른 곳에서 켰다면 틀린 곳에 확신한 상태(= 납치된 상태) 로 시작합니다. 무작위 주입도 꺼져 있으므로 스스로 고치지 못합니다. 그래서 내비게이션을 시작하기 전에 2D Pose Estimate 로 위치를 알려 주는 것이 공식 절차에 들어 있습니다.
예측하기: 로봇을 옆 통로로 옮기면?#
필터가 1번 통로(A-B 사이)에서 잘 수렴해 있습니다. 로봇을 들어 똑같이 생긴 3번 통로(C-D 사이)의 같은 높이 에 놓았습니다. 무작위 주입은 꺼져 있습니다(agvpro.yaml). 로봇이 통로 안에 있는 동안 추정 위치는 어디에 있을까요? 통로를 빠져나가면 저절로 고쳐질까요?
시뮬레이터 3 · 전역 위치 추정 · 납치 · 2D Pose Estimate#
로봇은 순찰 경로를 계속 돕니다. 버튼으로 로봇을 납치하거나 파티클을 지도 전체에 뿌릴 수 있고, 지도를 클릭한 채 끌면 RViz 의 2D Pose Estimate 처럼 그 위치 · 방향 주변에 파티클을 다시 뿌립니다 (클릭 = 위치, 끄는 방향 = 로봇 방향). 사건(납치 · 전역 초기화 · 2D Pose Estimate) 뒤 위치 오차가 0.3 m 안으로 들어와 2 s 동안 유지되면 "재수렴 시간" 을 기록합니다.
해 볼 것
- 예측 확인 — 주입 agvpro.yaml (0, 0) · 최대 거리 3.5 m 로 ▶ 주행합니다. 로봇이 통로 1 에 들어가면 납치: 같은 모양 통로로 를 누릅니다. 통로 안에 있는 동안 추정 ◎ 는 어디에 있고, w_avg 는 납치 전과 비교해 어떤가요? 통로를 빠져나온 뒤 w_avg 와 위치 오차가 어떻게 바뀌는지 재수렴 기록이 나올 때까지(또는 150 s) 지켜봅니다. 다른 난수 로 두세 번 더 해 봅니다.
- 같은 실험을 주입 Nav2 예시 (0.001, 0.1) 로 반복합니다. 납치 직후 w_fast · w_slow · 주입 확률은 어떻게 움직이나요? 재수렴까지 몇 초 걸렸나요? 민감하게 (0.01, 0.3) 와도 비교합니다.
- 주입을 끈 채로 전역 초기화 를 로봇이 통로 안에 있을 때 누릅니다. 무리 수가 2 ~ 3 개로 나뉘는 순간을 찾고, 추정 ◎ 가 어느 무리에 붙는지 봅니다. 최대 거리를 3.5 m 와 10 m 로 바꿔 각각 3 번씩 해 보고, 올바르게 수렴한 횟수를 셉니다.
- 납치당해 틀린 곳에 확신한 상태에서 지도를 클릭 · 끌어 실제 로봇 위치와 방향을 알려 줍니다(2D Pose Estimate). 재수렴까지 몇 초 걸리나요? 일부러 0.5 m · 30° 쯤 틀리게 알려 주면 어떻게 되나요? 2 m 틀리게(옆 통로) 알려 주면?
- 주입을 민감하게 (0.01, 0.3) 로 켠 채로 납치 없이 한참 주행합니다. 주입 확률 그래프가 0 이 아닌 순간이 있나요? 그때 위치 오차는 어떻게 되나요?
관찰 포인트 — 같은 모양 통로로 납치하면 통로 안에서는 틀린 가설도 스캔과 잘 맞아 w_avg 가 거의 떨어지지 않고, 필터는 틀린 통로에 확신한 채 달립니다. 통로를 빠져나와 스캔이 지도와 어긋나기 시작해야 w_avg 가 떨어집니다. 주입이 꺼져 있으면 그다음은 운입니다 — 모퉁이를 돌 때 이동 노이즈(agvpro 의 alpha1 = 0.4 는 회전할 때 위치 노이즈도 키웁니다)로 구름이 퍼지다가 진짜 위치에 닿으면 돌아오지만(이 시뮬레이터에서는 흔히 30 s 안팎), 난수에 따라 1 분 넘게 걸리거나 끝내 못 돌아옵니다. 주입을 켜면 w_fast 가 w_slow 보다 빨리 떨어지는 순간 무작위 파티클이 섞이고, 그중 하나가 진짜 위치 근처에 떨어지면 몇 스텝 만에 구름이 옮겨 갑니다(흔히 10 s 안팎, 운이 나쁘면 더 깁니다). 그래서 한 번이 아니라 여러 번 재서 비교해야 합니다. 최대 거리가 짧을수록(3.5 m) 통로 안의 스캔은 통로끼리 구별이 안 되어 다봉 분포가 오래 남고, 오수렴이 잦아집니다. 길면 통로 끝의 충전소 · 기둥 · 팔레트가 보여 대칭이 깨집니다. 2D Pose Estimate 는 σ 0.5 m · 15° 범위에 파티클을 뿌리므로 그 정도 오차는 스캔이 고쳐 주지만, 옆 통로(2 m)처럼 범위 밖으로 틀리게 주면 그 통로에 확신해 버립니다.
주입이 꺼진 상태(agvpro.yaml)에서 로봇을 통로 1 에서 똑같이 생긴 통로 3 으로 납치했습니다. 통로 안에 있는 동안 w_avg 는 납치 전과 거의 같았습니다. 그 이유로 가장 알맞은 것은?
- 라이다가 고장 나서 측정이 들어오지 않았다
- 통로 1 과 통로 3 은 스캔이 똑같이 보여서, 틀린 통로에 있는 파티클도 스캔과 잘 맞는다
- 정규화 때문에 w_avg 는 항상 같다
- 오도메트리가 납치를 알려 주었다
정답 보기
② — 같은 모양 통로에서는 틀린 가설도 스캔과 잘 맞으므로 "지도와 맞는 정도" 가 떨어지지 않습니다. 그래서 주입을 켜 두었더라도 통로 안에서는 w_fast 가 거의 떨어지지 않아 주입이 일어나지 않고, 통로를 빠져나와 충전소 · 기둥 · 다른 열의 끝이 보일 때 비로소 떨어집니다. w_avg 는 정규화 **전** 의 우도로 계산하므로 항상 같지 않습니다(③). 오도메트리는 바퀴가 돌지 않았으므로 납치를 모릅니다(④).agvpro.yaml 로 AMCL 을 켰는데 로봇은 지도의 원점이 아니라 5 m 떨어진 충전소에 있습니다. 무엇이 일어나고, 무엇을 해야 할까요?
- AMCL 이 전역 위치 추정을 해서 스스로 충전소를 찾는다
set_initial_pose: True·initial_pose(0, 0, 0) 때문에 원점에 확신한 상태로 시작하고, 무작위 주입도 꺼져 있어 스스로 못 고친다. RViz 의 2D Pose Estimate 로 실제 위치를 준다max_particles를 5000 으로 늘리면 저절로 고쳐진다- 오도메트리를 0 으로 리셋하면 된다
정답 보기
② — 시작부터 "납치된" 상태입니다. 파티클이 원점 주변에만 있으므로 개수를 늘려도(③) 충전소 근처에는 파티클이 없습니다. AMCL 은 스스로 전역 위치 추정을 시작하지 않습니다(①). 필요하면 `/reinitialize_global_localization` 서비스를 부를 수 있지만, 대칭인 창고에서는 오수렴할 수 있어 사람이 주는 초기 위치가 더 확실합니다. 오도메트리 리셋(④)은 odom → base 만 바꾸고 map 에서의 위치 문제는 그대로입니다.4교시 · AMCL 파라미터 읽기#
agvpro.yaml 의 amcl 섹션#
agv_pro_navigation2/param/agvpro.yaml 의 맨 앞이 amcl: 섹션입니다 (humble 브랜치 기준, 일부).
amcl:
ros__parameters:
alpha1: 0.4
alpha2: 0.3
alpha3: 0.1
alpha4: 0.1
alpha5: 0.04
base_frame_id: "base_footprint"
global_frame_id: "map"
odom_frame_id: "odom"
laser_likelihood_max_dist: 2.0
laser_max_range: 100.0
laser_min_range: -1.0
laser_model_type: "likelihood_field"
max_beams: 60
max_particles: 2000
min_particles: 500
pf_err: 0.05
pf_z: 0.99
recovery_alpha_fast: 0.0
recovery_alpha_slow: 0.0
resample_interval: 2
robot_model_type: "nav2_amcl::OmniMotionModel"
sigma_hit: 0.02
tf_broadcast: true
transform_tolerance: 0.3
update_min_a: 0.06
update_min_d: 0.025
z_hit: 0.7
z_rand: 0.059
set_initial_pose: True| 무리 | 파라미터 | 값 | 뜻 · 이 차시에서 본 곳 |
|---|---|---|---|
| 파티클 | min_particles · max_particles | 500 · 2000 | KLD 샘플링이 이 범위에서 개수를 정함 (2교시) |
pf_err · pf_z | 0.05 · 0.99 | KLD 샘플링의 근사 오차 한계 · 신뢰도 | |
resample_interval | 2 | 필터 갱신 2 번에 1 번 리샘플링 (시뮬레이터 2) | |
| 운동 모델 | robot_model_type | OmniMotionModel | 전방향 로봇용 운동 모델 (이번 교시) |
alpha1 ~ alpha5 | 0.4 · 0.3 · 0.1 · 0.1 · 0.04 | 오도메트리를 얼마나 못 믿는지 (아래 표) | |
| 측정 모델 | laser_model_type | likelihood_field | 빔 끝점 ↔ 가장 가까운 장애물 거리 (2교시) |
sigma_hit | 0.02 m | 그 거리를 얼마나 엄격하게 볼지 (시뮬레이터 2 의 σ) | |
z_hit · z_rand | 0.7 · 0.059 | 빔이 "지도와 맞을 몫" · "아무 데나 떨어질 몫" | |
laser_likelihood_max_dist | 2.0 m | 거리 표를 계산할 최대 거리 | |
max_beams | 60 | 스캔에서 골라 쓸 빔 수 (계산량 ∝ 파티클 × 빔) | |
laser_max_range · laser_min_range | 100.0 · −1.0 | 쓸 빔 거리 범위 (−1 이면 센서 값 사용) | |
| 갱신 조건 | update_min_d · update_min_a | 0.025 m · 0.06 rad (약 3.4°) | 이만큼 움직여야 필터를 한 번 돌림 |
| 회복 | recovery_alpha_slow · recovery_alpha_fast | 0.0 · 0.0 | 무작위 주입 꺼짐 (3교시) |
| 좌표계 | global_frame_id · odom_frame_id · base_frame_id | map · odom · base_footprint | map → odom 을 발행 (아래) |
alpha 의 뜻은 운동 모델마다 조금 다릅니다. Nav2 문서의 설명은 다음과 같습니다.
| 파라미터 | Nav2 문서의 뜻 | 커지면 |
|---|---|---|
alpha1 | 회전에서 생기는 회전 추정 노이즈 | 제자리 회전할 때 구름이 크게 퍼짐 |
alpha2 | 이동에서 생기는 회전 추정 노이즈 | 직진할 때 방향이 퍼짐 |
alpha3 | 이동에서 생기는 이동 추정 노이즈 | 직진할 때 진행 방향으로 퍼짐 |
alpha4 | 회전에서 생기는 이동 추정 노이즈 | 회전할 때 위치가 퍼짐 |
alpha5 | 이동 관련 노이즈 (omni 모델에서만 사용) | 옆방향(strafe)으로 퍼짐 |
alpha 가 크면 예측 단계에서 구름을 크게 퍼뜨리고 라이다에 더 기댑니다. 작으면 오도메트리를 믿고 구름을 작게 유지합니다. 바퀴가 잘 미끄러지는 바닥 · 메카넘 바퀴의 옆 이동처럼 오도메트리가 실제로 틀리는 만큼보다 alpha 를 작게 잡으면, 구름이 실제 위치를 덮지 못해 필터가 놓칩니다. 너무 크게 잡으면 구름이 쓸데없이 퍼져 위치가 떨리고, 같은 정확도에 파티클이 더 필요합니다.
전방향 운동 모델과 차동 운동 모델#
같은 오도메트리 Δ = (앞 dx, 옆 dy, 회전 dθ) 를 두 모델은 다르게 쪼갭니다.
차동 모델 (DifferentialMotionModel) | 전방향 모델 (OmniMotionModel) | |
|---|---|---|
| 쪼개는 방법 | rot1 = atan2(dy, dx), trans = √(dx² + dy²), rot2 = dθ − rot1 | trans = √(dx² + dy²) (방향 = atan2(dy, dx)), strafe = 0, rot = dθ |
| 회전 노이즈 크기 | α1 · rot1² + α2 · trans² (rot2 도 같은 꼴) | α4 · rot² + α2 · trans² |
| 진행 방향 노이즈 | α3 · trans² + α4 · (rot1² + rot2²) | α3 · trans² + α1 · rot² |
| 옆방향 노이즈 | (없음 — 바퀴가 옆으로 못 간다고 가정) | α1 · rot² + α5 · trans² |
| 맞는 로봇 | 두 바퀴 차동 구동 (옆으로 못 감) | 메카넘 · 옴니 휠 (myAGV Pro, ch02) |
(위 식은 nav2_amcl 의 두 모델 구조를 따라 정리한 것입니다. 실제 코드는 각 항을 표준편차 자리에 그대로 넣는 등 세부가 다르니 소스를 확인하세요 — 확인 필요.)
차동 모델로 옆 이동을 해석하면 rot1 = 90° 입니다. 뒤로 가는 경우를 빼 주는 보정(min(|rot1|, |π − rot1|))을 해도 90° 는 90° 그대로라, 아무리 짧게 옆으로 가도 회전 노이즈가 α1 · (π/2)² ≈ 0.99 rad² (agvpro 의 α1 = 0.4 기준)만큼 들어갑니다. 게다가 update_min_d 가 0.025 m 로 작아서 1 m 옆으로 가는 동안 이 큰 노이즈가 약 40 번 더해집니다. 평균 위치는 맞게 옮겨도, 구름의 방향이 사방으로 흩어져 라이다가 따라잡지 못하는 순간 추정 위치가 튑니다.
예측하기: 운동 모델을 잘못 고르면?#
robot_model_type 을 nav2_amcl::DifferentialMotionModel 로 잘못 적었습니다. 로봇이 방향을 그대로 둔 채 옆으로(횡이동) 3.4 m 를 갑니다. 직진만 하는 경로와 비교해 위치 오차는 어떻게 될까요? alpha 를 모두 아주 작게 하면 차동 모델로도 괜찮아질까요?
시뮬레이터 4 · 운동 모델 · alpha 조정과 map → odom#
두 필터가 같은 오도메트리 · 같은 스캔 을 받습니다. 파랑 = 고른 모델, 주황 = 비교용 다른 모델입니다. 회색 점선 = 오도메트리만 믿은 경로(odom 좌표를 그대로 map 에 그린 것), 초록 = 실제 로봇입니다. 경로를 한 바퀴 돌면 자동으로 멈추고 결과를 기록합니다. 오른쪽 위에는 파랑 필터가 발행하는 TF 값이 보입니다.
해 볼 것
- 예측 확인 — 기본값(Omni, 횡이동 경로, agvpro.yaml)으로 ▶ 한 바퀴 주행합니다. 파랑(Omni)과 주황(Diff)의 구름 · 추정 위치는 위아래(횡이동) 구간에서 어떻게 다른가요? 기록의 RMS · 최대 오차 · 튐 횟수를 비교합니다.
- 경로를 돌고 나서 직진 으로 바꿔 같은 실험을 합니다. 이번에는 두 모델의 차이가 있나요? 왜 그런지 그림 ch07-motion-models 로 설명해 봅니다.
- 운동 모델을 DifferentialMotionModel 로 두고(이제 파랑이 Diff), 횡이동 경로에서 모두 작게 (0.005) 를 누른 뒤 한 바퀴 돕니다. 차동 모델이 괜찮아졌나요? 그렇다면 그것은 모델이 맞아서일까요, 노이즈를 꺼서일까요? 이어서 Omni 로 돌아가 실제 횡이동 슬립을 30 % 로 올리고 agvpro.yaml · 모두 작게 · 모두 크게 세 가지의 RMS 를 비교합니다.
- Omni 에서
update_min_d를 0.3 m 로 키우고 한 바퀴 돕니다. 갱신 횟수 · RMS 는 어떻게 바뀌나요? 차동 모델(주황)의 튐 횟수와 RMS 는 각각 어떻게 바뀌었나요? "1 m 옆으로 가는 동안 90° 회전 노이즈가 몇 번 더해지는가" 로 설명해 봅니다. - 오른쪽 위 TF 값을 보며 한 바퀴 돕니다.
odom→base는 매끄럽게 변하고map→odom은 조금씩 바뀝니다. 회색 점선(오도메트리만)이 실제 경로에서 멀어질수록map→odom은 어떻게 되나요? 출력의 "TF 확인" 줄에서 두 변환을 이은 값이 추정 위치와 같은지 확인합니다.
관찰 포인트 — 횡이동 구간에서 차동 모델은 짧은 옆 이동마다 "90° 회전" 노이즈를 뿌리므로 구름의 방향이 흩어지고, 라이다가 방향을 다시 맞추기 전에 추정 위치가 튑니다 (이 시뮬레이터에서 RMS 수 m, 튐 200 번 안팎). 회전 후 직진 경로에서는 두 모델이 거의 같습니다(둘 다 RMS 1 ~ 2 cm). alpha 를 아주 작게 하면 차동 모델도 괜찮아 보이지만, 모델이 맞아서가 아니라 회전 노이즈를 꺼 버렸기 때문입니다. 이 시뮬레이터는 라이다가 2.5 cm 마다 고쳐 주므로 그래도 버티지만, 오도메트리가 실제로 크게 틀리는 순간(바퀴 헛돎 · 부딪힘)에는 구름이 실제 위치를 덮지 못합니다. 슬립 30 % 에서 Omni 의 RMS 는 agvpro.yaml 값이 가장 작고, 모두 작게 · 모두 크게는 조금(cm 단위) 커집니다 — alpha 는 "실제 오도메트리 오차만큼" 이 적당합니다. update_min_d 를 0.3 m 로 키우면 갱신이 약 1/7 로 줄어 계산이 가벼워지고, Omni 의 RMS 는 몇 cm 로 조금 커집니다. 차동 모델은 "90° 노이즈" 가 더해지는 횟수가 줄어 튐 횟수는 줄지만 RMS 는 여전히 m 단위입니다 — 잘못 고른 모델은 갱신 주기로 고쳐지지 않습니다. map→odom 은 오도메트리가 틀어진 만큼을 메우는 보정입니다. 오도메트리만의 오차가 커질수록 map→odom 의 크기도 커지고, 둘을 이은 값이 RViz 가 보는 로봇 위치입니다.
map → odom 보정이 TF 에 들어가는 흐름 (ch04 복습)#
/amcl_pose · /particle_cloud 와 함께 TF map → odom 을 발행합니다.ch04 에서 본 것처럼 base_footprint 의 부모는 하나뿐이어야 하므로, AMCL 은 "지도에서 로봇의 위치(map → base_footprint)" 를 직접 발행하지 않고 오도메트리와의 차이만 발행합니다.
T_map_odom = T_map_base(AMCL 추정) · inv(T_odom_base(오도메트리))
T_map_base = T_map_odom · T_odom_base ← RViz · Nav2 가 TF 로 조회하는 값odom → base_footprint는 바퀴 오도메트리가 매 주기(빠르게, 매끄럽게) 발행합니다. 로봇이 움직이면 이 값이 바로 따라갑니다.map → odom은 AMCL 이 필터를 돌릴 때만(update_min_d·update_min_a만큼 움직였을 때) 새로 계산합니다. 그 사이에는 마지막 값을 계속 다시 보냅니다 (transform_tolerance: 0.3s 만큼 미래 시각을 붙여 발행).- 그래서 RViz 에서 로봇은 오도메트리 덕분에 매끄럽게 움직이다가, AMCL 이 갱신할 때
map → odom이 바뀌면서 조금씩 점프 합니다. 오도메트리가 좋을수록 점프가 작습니다. - 로봇이 멈춰 있으면 AMCL 은 필터를 돌리지 않습니다. 멈춘 채로 위치를 다시 맞추고 싶으면
/request_nomotion_update서비스(std_srvs/srv/Empty)로 한 번 돌리게 할 수 있습니다.
3D 라이다로 만든 지도에서 AMCL 쓰기 (공식 문서 6.2.6 · 6.2.7)#
ch06 의 Point-LIO · FAST-LIO 는 3D 점군(.pcd)을 만들고 pcd2pgm 으로 2D 지도(.pgm + .yaml)를 뽑습니다. AMCL 은 2D 스캔만 받으므로, 3D 라이다 로봇은 점군을 한 높이의 가짜 2D 스캔으로 바꿔 넣습니다.
# 0) 지도 경로를 바꾼 뒤 그 패키지만 다시 빌드
# agv_pro_navigation2/launch/navigation2_active.launch.py 의 map_dir 기본값 (예: 'map_end.yaml')
cd ~/agv_pro_ros2 && colcon build --packages-select agv_pro_navigation2 && source install/setup.bash
# (문서에는 agv_pro_navigation 으로 적혀 있으나 저장소의 패키지 이름은 agv_pro_navigation2)
# 1) ~ 4) 터미널 네 개
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
ros2 launch unitree_lidar_ros2 launch.py # Unitree L2 (Point-LIO)
# 또는 ros2 launch livox_ros_driver2 rviz_MID360_launch.py # Livox MID-360 (FAST-LIO)
ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py # 3D 점군 -> /scan
ros2 launch agv_pro_navigation2 navigation2_active.launch.py| 단계 | 무엇이 바뀌나 | AMCL 입장 |
|---|---|---|
pointcloud_to_laserscan | 높이 범위 안의 점을 각도별 최소 거리로 접어 /scan 발행 | 2D 라이다와 똑같은 /scan 을 받음 |
pcd2pgm 지도 | 3D 점군을 위에서 눌러 만든 점유격자 | 2D 라이다 지도보다 낮은 · 높은 물체까지 벽처럼 들어 있음 |
먼저 예측하세요. 지도에는 들어 있는데 가짜 스캔의 높이 범위 밖이라 스캔에는 안 보이는 물체(예: 천장 배관)가 많으면, 2교시의 우도 계산에서 무슨 일이 생길까요? → 지도와 스캔이 어긋난 칸이 많아져 파티클 가중치가 고르게 낮아지고 수렴이 느려집니다. 지도를 만들 때와 위치 추정할 때 높이 범위를 맞추는 것이 핵심입니다 (값은 각 launch 파일의 설정 확인 필요).
녹화해 두고 다시 보기(6.2.6): ros2 bag record -a 로 모든 토픽을 기록하고, ros2 launch point_lio l2_mapping_rviz.launch.py 로 RViz 를 연 뒤 ros2 bag play <폴더> 로 재생합니다. 만든 점군은 pcl_viewer scans.pcd 로 엽니다 (없으면 sudo apt install pcl-tools).
시뮬레이터 4 의 횡이동 경로에서 한 바퀴 기록이 다음과 같았습니다. Omni RMS 0.010 m · 최대 0.03 m · 튐 0 번 | Diff RMS 3.954 m · 최대 8.26 m · 튐 195 번. 같은 오도메트리 · 같은 스캔인데 차이가 나는 가장 큰 이유는?
- 차동 모델은 파티클 수를 절반만 쓴다
- 차동 모델은 옆 이동을 "90° 회전 + 직진 + −90° 회전" 으로 쪼개, 짧은 옆 이동마다 α1 · (π/2)² 만큼 큰 회전 노이즈를 뿌린다
- 차동 모델은 라이다를 쓰지 않는다
- 차동 모델은 평균 이동을 계산하지 못해 옆으로 움직이지 않는다
정답 보기
② — 평균 이동(rot1 → trans → rot2)은 결과적으로 같은 자리에 도착하므로 옆으로도 잘 갑니다(④ 오답). 문제는 노이즈입니다. rot1 = 90° 라서 회전 노이즈가 이동량과 상관없이 크고, update_min_d = 0.025 m 마다 더해지므로 구름의 방향이 흩어집니다. 파티클 수 · 라이다는 두 필터가 똑같습니다(①, ③). myAGV Pro 처럼 옆으로 가는 로봇은 `nav2_amcl::OmniMotionModel` 을 써야 합니다.로봇이 오도메트리로는 (2.70, 0.85, 0°) 에 있고, AMCL 이 추정한 지도 위치는 (2.90, 0.85, 0°) 입니다. AMCL 이 발행하는 map → odom 은? (회전은 모두 0)
- (2.90, 0.85, 0°)
- (0.20, 0.00, 0°)
- (−0.20, 0.00, 0°)
- (2.70, 0.85, 0°)
정답 보기
② — 회전이 없으면 T_map_odom = T_map_base − T_odom_base = (2.90 − 2.70, 0.85 − 0.85) = (0.20, 0.00) 입니다. "오도메트리 좌표계 전체를 지도에서 0.2 m 오른쪽으로 옮겨 놓아야 맞는다" 는 뜻입니다. ① 은 map → base 를 그대로 발행하는 실수(부모가 둘이 됨), ③ 은 방향을 거꾸로 계산한 것입니다. 회전이 있으면 inv(T_odom_base) 를 곱해야 하므로 단순 빼기가 아닙니다 (시뮬레이터 4 의 "TF 확인" 줄).실습 (시뮬레이터)#
교시마다 Playground 에서 코드를 직접 돌립니다. starter 의 TODO 를 채워 실행한 뒤 풀이 와 비교하세요. 모든 코드는 표준 라이브러리 + numpy 만 쓰고, 창고 지도 · 운동 모델 · 측정 모델은 시뮬레이터 2 ~ 4 와 같습니다. 브라우저에서는 한 번 실행에 수 초 ~ 수십 초 걸릴 수 있습니다.
시뮬레이터 1 과 같은 20 칸 순환 복도(문 3 · 8 · 15)에서 베이즈 필터를 직접 만듭니다.
- TODO 1
sense()— 칸마다 우도 P(z | x) 를 곱하고 합이 1 이 되게 나눕니다. - TODO 2
move()—np.roll세 개로 "정확히 · 1칸 덜 · 1칸 더" 를 섞습니다.
예측 먼저 — 첫 줄 출력 after one 'door' 의 문 칸 · 벽 칸 값을 실행 전에 계산해 적어 둡니다 (1교시 예측하기).
import numpy as np
# 1D 복도: 칸 20 개, 문은 3 · 8 · 15 번 칸. 끝과 처음이 이어진 순환 복도
N_CELLS = 20
DOORS = [3, 8, 15]
is_door = np.array([i in DOORS for i in range(N_CELLS)])
P_HIT = 0.9 # 문 앞에서 '문' 이라고 읽을 확률 P(z=door | door)
P_FALSE = 0.1 # 벽 앞에서 '문' 이라고 읽을 확률 P(z=door | wall)
EPS = 0.1 # 이동 노이즈: 1 칸 덜 갈 확률 = 1 칸 더 갈 확률 = EPS
def sense(bel, z):
"""측정 갱신: bel(x) <- eta * P(z | x) * bel(x)"""
# TODO 1-a: 칸마다 우도 like = P(z | x) 를 만듭니다.
# z 가 "door" 이면 문 칸 P_HIT, 벽 칸 P_FALSE / "wall" 이면 문 칸 1-P_HIT, 벽 칸 1-P_FALSE
# 힌트: np.where(is_door, 값1, 값2)
# TODO 1-b: post = bel * like 를 계산하고, 합이 1 이 되게 나눠 돌려줍니다.
return bel
def move(bel, u):
"""예측 갱신: u 칸 이동. 정확히 u 칸 1-2*EPS, u-1 칸 EPS, u+1 칸 EPS (순환)"""
# TODO 2: np.roll(bel, k) 는 확률을 k 칸 오른쪽으로 민 배열입니다 (순환).
# u 칸 민 것에 (1 - 2*EPS), u-1 칸 민 것에 EPS, u+1 칸 민 것에 EPS 를 곱해 더합니다.
return bel
def entropy(bel):
p = bel[bel > 0]
return float(-(p * np.log2(p)).sum())
def show(bel, true_pos, title):
print(f"--- {title} (sum {bel.sum():.3f}, H {entropy(bel):.2f} bit, max {bel.max():.3f} @ {int(bel.argmax())})")
for i in range(N_CELLS):
mark = "D" if is_door[i] else "."
robot = "<- robot" if i == true_pos else ""
print(f"{i:2d} {mark} {bel[i]:.3f} " + "#" * int(round(bel[i] * 60)) + " " + robot)
# 1) 예측하기: 아무것도 모를 때 문을 한 번 보면?
bel = np.full(N_CELLS, 1 / N_CELLS)
bel = sense(bel, "door")
print(f"after one 'door': door cell {bel[3]:.4f}, wall cell {bel[0]:.4f}, all doors {bel[is_door].sum():.3f}")
# 2) 실제 로봇: 3 번 칸(문)에서 출발, 한 칸씩 오른쪽으로. 센서는 실제 칸을 그대로 읽는다고 가정
bel = np.full(N_CELLS, 1 / N_CELLS)
true_pos = 3
for step in range(8):
z = "door" if is_door[true_pos] else "wall"
bel = sense(bel, z)
print(f"step {step}: true {true_pos:2d} z={z:4s} P(true) {bel[true_pos]:.3f} best {int(bel.argmax()):2d} H {entropy(bel):.2f} bit")
bel = move(bel, 1)
true_pos = (true_pos + 1) % N_CELLS
show(bel, true_pos, "after 8 sense+move")
# 3) 측정 없이 이동만 10 번 하면 분포는?
for k in range(10):
bel = move(bel, 1)
true_pos = (true_pos + 1) % N_CELLS
show(bel, true_pos, "10 moves without sensing")해 볼 것
- 첫 줄의 값이 0.2045 · 0.0227 · 0.614 인지 확인합니다. 시뮬레이터 1 의 '문' 으로 측정 결과와 같은가요?
step 0 ~ 7에서 P(true) 가 처음으로 크게 뛰는 것은 몇 번째 단계인가요? 그때 센서는 무엇을 읽었나요?DOORS = [2, 7, 12, 17]로 바꿔 실행합니다. 8 단계 뒤 가장 큰 bel 은 얼마인가요? 확인 문제 1-2 와 연결해 설명합니다.EPS = 0.3으로 바꾸면 "10 moves without sensing" 의 엔트로피는 어떻게 되나요?EPS = 0이면?
import numpy as np
# 1D 복도: 칸 20 개, 문은 3 · 8 · 15 번 칸. 끝과 처음이 이어진 순환 복도
N_CELLS = 20
DOORS = [3, 8, 15]
is_door = np.array([i in DOORS for i in range(N_CELLS)])
P_HIT = 0.9 # 문 앞에서 '문' 이라고 읽을 확률 P(z=door | door)
P_FALSE = 0.1 # 벽 앞에서 '문' 이라고 읽을 확률 P(z=door | wall)
EPS = 0.1 # 이동 노이즈: 1 칸 덜 갈 확률 = 1 칸 더 갈 확률 = EPS
def sense(bel, z):
"""측정 갱신: bel(x) <- eta * P(z | x) * bel(x)"""
if z == "door":
like = np.where(is_door, P_HIT, P_FALSE)
else:
like = np.where(is_door, 1 - P_HIT, 1 - P_FALSE)
post = bel * like
return post / post.sum()
def move(bel, u):
"""예측 갱신: u 칸 이동. 정확히 u 칸 1-2*EPS, u-1 칸 EPS, u+1 칸 EPS (순환)"""
return ((1 - 2 * EPS) * np.roll(bel, u)
+ EPS * np.roll(bel, u - 1)
+ EPS * np.roll(bel, u + 1))
def entropy(bel):
p = bel[bel > 0]
return float(-(p * np.log2(p)).sum())
def show(bel, true_pos, title):
print(f"--- {title} (sum {bel.sum():.3f}, H {entropy(bel):.2f} bit, max {bel.max():.3f} @ {int(bel.argmax())})")
for i in range(N_CELLS):
mark = "D" if is_door[i] else "."
robot = "<- robot" if i == true_pos else ""
print(f"{i:2d} {mark} {bel[i]:.3f} " + "#" * int(round(bel[i] * 60)) + " " + robot)
# 1) 예측하기: 아무것도 모를 때 문을 한 번 보면?
bel = np.full(N_CELLS, 1 / N_CELLS)
bel = sense(bel, "door")
print(f"after one 'door': door cell {bel[3]:.4f}, wall cell {bel[0]:.4f}, all doors {bel[is_door].sum():.3f}")
# 2) 실제 로봇: 3 번 칸(문)에서 출발, 한 칸씩 오른쪽으로. 센서는 실제 칸을 그대로 읽는다고 가정
bel = np.full(N_CELLS, 1 / N_CELLS)
true_pos = 3
for step in range(8):
z = "door" if is_door[true_pos] else "wall"
bel = sense(bel, z)
print(f"step {step}: true {true_pos:2d} z={z:4s} P(true) {bel[true_pos]:.3f} best {int(bel.argmax()):2d} H {entropy(bel):.2f} bit")
bel = move(bel, 1)
true_pos = (true_pos + 1) % N_CELLS
show(bel, true_pos, "after 8 sense+move")
# 3) 측정 없이 이동만 10 번 하면 분포는?
for k in range(10):
bel = move(bel, 1)
true_pos = (true_pos + 1) % N_CELLS
show(bel, true_pos, "10 moves without sensing")- 첫 줄: 문 칸 0.2045 · 벽 칸 0.0227 · 문 칸 합 0.614 입니다 (0.9 / 4.4, 0.1 / 4.4).
- step 5 (두 번째 문, 8 번 칸)에서 P(true) 가 0.15 → 0.44 로 뜁니다. 3 → 8 의 간격 5 칸이 다른 간격(7 · 8 칸)과 달라서 "문 → 벽 4 번 → 문" 순서가 위치를 가려 줍니다.
- 측정 없이 10 번 이동하면 H 가 3.51 → 4.10 bit 로 늘고 최댓값이 0.30 → 0.13 으로 낮아집니다.
시뮬레이터 2 의 ① ② ③ 을 numpy 로 씁니다. 파티클 500 개를 출발점 주변 ±0.5 m 에 뿌리고, 오른쪽으로 2 m 간 뒤 방향은 그대로 옆으로 2 m 갑니다 (옆 이동 오도메트리에는 8 % 슬립).
- TODO 1
predict()— 전방향(omni) 운동 모델로 파티클마다 다른 노이즈를 더합니다. - TODO 2
weight()— 빔 끝점을 파티클 자리에서 지도에 찍고 likelihood field 로 가중치를 계산합니다. - TODO 3
resample()— 저분산 리샘플링.
예측 먼저 — starter 를 그대로 실행하면(노이즈 없음 · 가중치 균등 · 리샘플링 없음) 오차는 줄어들까요? Neff 는 몇일까요?
import time
import numpy as np
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
ALPHA = [0.4, 0.3, 0.1, 0.1, 0.04] # agvpro.yaml 의 alpha1 ~ alpha5
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1)
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def predict(P, dx, dy, dth):
"""전방향(omni) 운동 모델. P = (N, 3) 배열 [x, y, theta]. (dx, dy, dth) 는 로봇 좌표계 오도메트리"""
a1, a2, a3, a4, a5 = ALPHA
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx) # 파티클마다 자기 방향 기준으로 이동 방향
# TODO 1: 파티클마다 다른 노이즈를 더해 옮깁니다.
# t_hat = trans + 정규분포(0, s_trans) n 개 (진행 방향 이동량)
# st_hat = 정규분포(0, s_strafe) n 개 (옆방향 이동량)
# x += t_hat*cos(bearing) + st_hat*sin(bearing), y += t_hat*sin(bearing) - st_hat*cos(bearing)
# theta += dth + 정규분포(0, s_rot) n 개
# 지금은 노이즈 없이 평균 이동만 합니다.
P[:, 0] += trans * np.cos(bearing)
P[:, 1] += trans * np.sin(bearing)
P[:, 2] += dth
return P
def weight(P, ang, r):
"""빔 끝점을 각 파티클 자리에서 지도에 찍어 보고, 장애물과 가까울수록 큰 가중치"""
ok = r < RMAX # 최대 거리 빔은 쓰지 않음
a, rr = ang[ok], r[ok]
# TODO 2-a: 빔 끝점 ex, ey 를 (N, K') 배열로. ex = x + rr*cos(theta + a) (P[:, 0:1] 처럼 열로 잘라 브로드캐스트)
# TODO 2-b: d = dist_to_map(ex, ey), q = Z_HIT*exp(-d^2 / (2 SIGMA^2)) + (1 - Z_HIT)
# TODO 2-c: logw = BETA * log(q) 의 빔 합, w = exp(logw - logw.max()), 합이 1 이 되게 나눔
return np.full(len(P), 1 / len(P))
def resample(P, w):
"""저분산(low-variance) 리샘플링: 간격 1/N 의 빗 하나로 N 개를 뽑음"""
n = len(w)
# TODO 3: 빗의 이빨 위치 u = (rng.random() + [0, 1, ..., n-1]) / n
# idx = np.searchsorted(np.cumsum(w), u) -> P[np.minimum(idx, n - 1)].copy() 를 돌려줌
return P.copy()
def estimate(P, w):
x, y = (w * P[:, 0]).sum(), (w * P[:, 1]).sum()
th = np.arctan2((w * np.sin(P[:, 2])).sum(), (w * np.cos(P[:, 2])).sum())
return np.array([x, y, th])
# ---- 실행: 출발점 근처에 파티클 500 개, 오른쪽으로 2 m 간 뒤 옆으로 2 m (방향은 그대로)
N = 500
truth = np.array([1.0, 0.7, 0.0])
P = np.column_stack([truth[0] + rng.normal(0, 0.5, N), truth[1] + rng.normal(0, 0.5, N),
truth[2] + rng.normal(0, 0.3, N)])
moves = [(0.1, 0.0, 0.0)] * 20 + [(0.0, 0.1, 0.0)] * 20
print("step truth(x, y) estimate(x, y, deg) err(m) Neff ms")
for k, (dx, dy, dth) in enumerate(moves):
c, s = np.cos(truth[2]), np.sin(truth[2])
truth += (c * dx - s * dy, s * dx + c * dy, dth) # 참 이동
odo = (dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, 0.08)), dth) # 미끄러진 오도메트리
t0 = time.perf_counter()
P = predict(P, *odo)
ang, r = lidar(truth)
w = weight(P, ang, r)
est = estimate(P, w)
neff = 1 / (w**2).sum()
P = resample(P, w)
ms = (time.perf_counter() - t0) * 1000
if k % 4 == 3 or k < 3:
err = np.hypot(*(est[:2] - truth[:2]))
print(f"{k:4d} ({truth[0]:.2f}, {truth[1]:.2f}) ({est[0]:.2f}, {est[1]:.2f}, {np.degrees(est[2]):+6.1f}) {err:6.3f} {neff:5.0f} {ms:5.1f}")해 볼 것
- 풀이를 실행해 step 0 의 Neff 와 step 1 이후의 Neff 를 비교합니다. step 0 에서 Neff 가 작은 이유는?
SIGMA를 0.02(agvpro 의sigma_hit)와 0.5 로 바꿔 Neff · 오차를 비교합니다.N을 50 · 500 · 2000 으로 바꿔 ms 열을 기록합니다. N 에 비례하나요?predict()에서 노이즈를 모두 0 으로 만들면(s_trans = s_rot = s_strafe = 0) 몇 스텝 뒤 무슨 일이 생기나요? 서로 다른 파티클 수(len(np.unique(P[:, 0])))를 출력해 봅니다.
import time
import numpy as np
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
ALPHA = [0.4, 0.3, 0.1, 0.1, 0.04] # agvpro.yaml 의 alpha1 ~ alpha5
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1)
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def predict(P, dx, dy, dth):
"""전방향(omni) 운동 모델. P = (N, 3) 배열 [x, y, theta]. (dx, dy, dth) 는 로봇 좌표계 오도메트리"""
a1, a2, a3, a4, a5 = ALPHA
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx) # 파티클마다 자기 방향 기준으로 이동 방향
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def weight(P, ang, r):
"""빔 끝점을 각 파티클 자리에서 지도에 찍어 보고, 장애물과 가까울수록 큰 가중치"""
ok = r < RMAX # 최대 거리 빔은 쓰지 않음
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a) # (N, K') 끝점 x
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
logw = BETA * np.log(q).sum(axis=1)
w = np.exp(logw - logw.max()) # 큰 수끼리 빼서 넘침 방지
return w / w.sum()
def resample(P, w):
"""저분산(low-variance) 리샘플링: 간격 1/N 의 빗 하나로 N 개를 뽑음"""
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
def estimate(P, w):
x, y = (w * P[:, 0]).sum(), (w * P[:, 1]).sum()
th = np.arctan2((w * np.sin(P[:, 2])).sum(), (w * np.cos(P[:, 2])).sum())
return np.array([x, y, th])
# ---- 실행: 출발점 근처에 파티클 500 개, 오른쪽으로 2 m 간 뒤 옆으로 2 m (방향은 그대로)
N = 500
truth = np.array([1.0, 0.7, 0.0])
P = np.column_stack([truth[0] + rng.normal(0, 0.5, N), truth[1] + rng.normal(0, 0.5, N),
truth[2] + rng.normal(0, 0.3, N)])
moves = [(0.1, 0.0, 0.0)] * 20 + [(0.0, 0.1, 0.0)] * 20
print("step truth(x, y) estimate(x, y, deg) err(m) Neff ms")
for k, (dx, dy, dth) in enumerate(moves):
c, s = np.cos(truth[2]), np.sin(truth[2])
truth += (c * dx - s * dy, s * dx + c * dy, dth) # 참 이동
odo = (dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, 0.08)), dth) # 미끄러진 오도메트리
t0 = time.perf_counter()
P = predict(P, *odo)
ang, r = lidar(truth)
w = weight(P, ang, r)
est = estimate(P, w)
neff = 1 / (w**2).sum()
P = resample(P, w)
ms = (time.perf_counter() - t0) * 1000
if k % 4 == 3 or k < 3:
err = np.hypot(*(est[:2] - truth[:2]))
print(f"{k:4d} ({truth[0]:.2f}, {truth[1]:.2f}) ({est[0]:.2f}, {est[1]:.2f}, {np.degrees(est[2]):+6.1f}) {err:6.3f} {neff:5.0f} {ms:5.1f}")- step 0 의 Neff 는 약 15 입니다. 처음 ±0.5 m 로 퍼진 500 개 중 스캔과 잘 맞는 것이 몇 개뿐이라는 뜻이고, 리샘플링 뒤에는 330 ~ 440 으로 회복됩니다.
- 위치 오차는 1 ~ 3 cm 로 유지됩니다. 한 스텝 계산은 PC 에서 수 ms 이고, 브라우저(Pyodide)에서는 몇 배 느릴 수 있습니다.
계획의 실습 "수렴에 필요한 최소 파티클 수를 창고 지도에서 찾기" 입니다. 시뮬레이터 2 와 Playground 두 가지로 찾습니다.
시뮬레이터 2 로 — 초기 퍼짐 넓게, σ 0.15 m, 빔 36 개로 두고 N 을 50 → 100 → 200 → 300 → 500 → 1000 → 2000 순서로 바꿔 가며 수렴 실험 (난수 5 개) 을 누릅니다. "성공 5 / 5" 가 처음 나오는 N 과 그때의 "갱신 1 번 ms" 를 표로 적습니다. 빔을 12 개로 줄이거나 σ 를 0.4 m 로 바꾸면 최소 N 이 어떻게 바뀌는지도 적습니다.
| N | 성공 / 5 | 갱신 1 번 ms | 메모 |
|---|---|---|---|
| 50 | |||
| 200 | |||
| 500 | |||
| 2000 |
Playground 로 — 같은 실험을 numpy 로 합니다. 출발점 주변 표준편차 1.5 m · 1.0 rad 로 파티클을 뿌리고, 4 m 를 달린 뒤 오차 0.3 m 미만이면 성공입니다.
- TODO 1 성공 횟수 세기. TODO 2 처음으로 모두 성공한 N 기록.
import time
import numpy as np
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
ALPHA = [0.4, 0.3, 0.1, 0.1, 0.04] # agvpro.yaml 의 alpha1 ~ alpha5
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1) # run() 이 seed 마다 새로 만듦
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def predict(P, dx, dy, dth):
"""전방향(omni) 운동 모델. P = (N, 3) 배열 [x, y, theta]. (dx, dy, dth) 는 로봇 좌표계 오도메트리"""
a1, a2, a3, a4, a5 = ALPHA
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx) # 파티클마다 자기 방향 기준으로 이동 방향
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def weight(P, ang, r):
"""빔 끝점을 각 파티클 자리에서 지도에 찍어 보고, 장애물과 가까울수록 큰 가중치"""
ok = r < RMAX # 최대 거리 빔은 쓰지 않음
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a) # (N, K') 끝점 x
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
logw = BETA * np.log(q).sum(axis=1)
w = np.exp(logw - logw.max()) # 큰 수끼리 빼서 넘침 방지
return w / w.sum()
def resample(P, w):
"""저분산(low-variance) 리샘플링: 간격 1/N 의 빗 하나로 N 개를 뽑음"""
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
def estimate(P, w):
x, y = (w * P[:, 0]).sum(), (w * P[:, 1]).sum()
th = np.arctan2((w * np.sin(P[:, 2])).sum(), (w * np.cos(P[:, 2])).sum())
return np.array([x, y, th])
# ---- 실험: 출발점 주변 약 1.5 m / 57 도(표준편차) 안 어딘가라는 것만 알 때, 파티클 몇 개면 수렴할까?
MOVES = [(0.1, 0.0, 0.0)] * 20 + [(0.0, 0.1, 0.0)] * 20
def run(N, seed, spread=1.5, spread_th=1.0):
"""파티클 N 개로 한 번 달리고 (마지막 오차, 업데이트 1 번 평균 ms) 를 돌려줌"""
global rng
rng = np.random.default_rng(seed)
truth = np.array([1.0, 0.7, 0.0])
P = np.column_stack([truth[0] + rng.normal(0, spread, N), truth[1] + rng.normal(0, spread, N),
truth[2] + rng.normal(0, spread_th, N)])
total = 0.0
for dx, dy, dth in MOVES:
c, s = np.cos(truth[2]), np.sin(truth[2])
truth += (c * dx - s * dy, s * dx + c * dy, dth)
odo = (dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, 0.08)), dth)
t0 = time.perf_counter()
P = predict(P, *odo)
ang, r = lidar(truth)
w = weight(P, ang, r)
est = estimate(P, w)
P = resample(P, w)
total += time.perf_counter() - t0
return float(np.hypot(*(est[:2] - truth[:2]))), total / len(MOVES) * 1000
SEEDS = range(1, 6)
min_n = None
print(" N success mean err(m) ms/update")
for N in [10, 20, 50, 100, 200, 500, 1000]:
res = [run(N, s) for s in SEEDS]
ok = 0 # TODO 1: 마지막 오차 e 가 0.3 m 미만인 실행 수를 셉니다
mean_err = np.mean([e for e, _ in res])
ms = np.mean([m for _, m in res])
print(f"{N:5d} {ok}/{len(res)} {mean_err:7.3f} {ms:6.2f}")
# TODO 2: 처음으로 모든 난수에서 성공한 N 을 min_n 에 기록합니다
print("smallest N with 5/5 success:", min_n)해 볼 것
- 최소 N 을 찾은 뒤
spread를 0.5 · 1.0 · 2.0 m 로 바꿔 최소 N 이 어떻게 변하는지 표로 만듭니다. 퍼짐이 2 배면 최소 N 도 2 배일까요, 4 배일까요? (넓이를 생각해 보세요) SEEDS = range(1, 11)로 늘리면 결론이 바뀌나요? 몇 번 시험해야 "충분하다" 고 말할 수 있을까요?- AMCL 은
min_particles: 500·max_particles: 2000입니다. 여러분의 표로 이 두 값을 해석해 봅니다.
import time
import numpy as np
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
ALPHA = [0.4, 0.3, 0.1, 0.1, 0.04] # agvpro.yaml 의 alpha1 ~ alpha5
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1) # run() 이 seed 마다 새로 만듦
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def predict(P, dx, dy, dth):
"""전방향(omni) 운동 모델. P = (N, 3) 배열 [x, y, theta]. (dx, dy, dth) 는 로봇 좌표계 오도메트리"""
a1, a2, a3, a4, a5 = ALPHA
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx) # 파티클마다 자기 방향 기준으로 이동 방향
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def weight(P, ang, r):
"""빔 끝점을 각 파티클 자리에서 지도에 찍어 보고, 장애물과 가까울수록 큰 가중치"""
ok = r < RMAX # 최대 거리 빔은 쓰지 않음
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a) # (N, K') 끝점 x
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
logw = BETA * np.log(q).sum(axis=1)
w = np.exp(logw - logw.max()) # 큰 수끼리 빼서 넘침 방지
return w / w.sum()
def resample(P, w):
"""저분산(low-variance) 리샘플링: 간격 1/N 의 빗 하나로 N 개를 뽑음"""
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
def estimate(P, w):
x, y = (w * P[:, 0]).sum(), (w * P[:, 1]).sum()
th = np.arctan2((w * np.sin(P[:, 2])).sum(), (w * np.cos(P[:, 2])).sum())
return np.array([x, y, th])
# ---- 실험: 출발점 주변 약 1.5 m / 57 도(표준편차) 안 어딘가라는 것만 알 때, 파티클 몇 개면 수렴할까?
MOVES = [(0.1, 0.0, 0.0)] * 20 + [(0.0, 0.1, 0.0)] * 20
def run(N, seed, spread=1.5, spread_th=1.0):
"""파티클 N 개로 한 번 달리고 (마지막 오차, 업데이트 1 번 평균 ms) 를 돌려줌"""
global rng
rng = np.random.default_rng(seed)
truth = np.array([1.0, 0.7, 0.0])
P = np.column_stack([truth[0] + rng.normal(0, spread, N), truth[1] + rng.normal(0, spread, N),
truth[2] + rng.normal(0, spread_th, N)])
total = 0.0
for dx, dy, dth in MOVES:
c, s = np.cos(truth[2]), np.sin(truth[2])
truth += (c * dx - s * dy, s * dx + c * dy, dth)
odo = (dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, 0.08)), dth)
t0 = time.perf_counter()
P = predict(P, *odo)
ang, r = lidar(truth)
w = weight(P, ang, r)
est = estimate(P, w)
P = resample(P, w)
total += time.perf_counter() - t0
return float(np.hypot(*(est[:2] - truth[:2]))), total / len(MOVES) * 1000
SEEDS = range(1, 6)
min_n = None
print(" N success mean err(m) ms/update")
for N in [10, 20, 50, 100, 200, 500, 1000]:
res = [run(N, s) for s in SEEDS]
ok = sum(1 for e, _ in res if e < 0.3) # 성공 = 마지막 오차 0.3 m 미만
mean_err = np.mean([e for e, _ in res])
ms = np.mean([m for _, m in res])
print(f"{N:5d} {ok}/{len(res)} {mean_err:7.3f} {ms:6.2f}")
if min_n is None and ok == len(res):
min_n = N
print("smallest N with 5/5 success:", min_n)- 예시 결과(PC): N 10 · 20 은 2/5 · 1/5, 50 ~ 200 은 4/5, 500 부터 5/5 입니다. 계산 시간은 N 에 거의 비례합니다(500 에서 약 5 ms, 1000 에서 약 14 ms).
- 실패한 실행의 오차는 1 m 안팎입니다. 처음에 진짜 위치 근처에 파티클이 없어서, 비슷해 보이는 다른 자리로 모인 경우입니다. 수렴한 실행끼리는 N 이 커져도 오차가 크게 줄지 않습니다(0.010 → 0.008 m).
계획의 실습 "로봇을 다른 통로로 옮긴 뒤 다시 수렴하는 데 걸리는 시간 측정" 입니다.
시뮬레이터 3 으로 — 최대 거리 3.5 m · N 2000 에서, 로봇이 통로 1 에 있을 때 납치: 같은 모양 통로로 를 누르고 재수렴 기록을 읽습니다. 주입 세 가지(agvpro 0 · 0 / Nav2 예시 0.001 · 0.1 / 민감하게 0.01 · 0.3)마다 다른 난수 를 바꿔 가며 3 번씩 재서 표를 채웁니다. 최대 거리 10 m 에서도 한 줄 더 잽니다.
| 주입 (α_slow, α_fast) | 1 회 (s) | 2 회 (s) | 3 회 (s) | 중앙값 |
|---|---|---|---|---|
| 0, 0 (agvpro) | ||||
| 0.001, 0.1 | ||||
| 0.01, 0.3 | ||||
| 0.001, 0.1 · 최대 거리 10 m |
Playground 로 — Augmented MCL 의 주입 규칙을 직접 넣고, 납치(4 m 오른쪽 똑같은 통로) 뒤 몇 걸음(0.1 m/걸음) 만에 돌아오는지 잽니다.
- TODO 1 w_slow · w_fast 갱신. TODO 2 주입 확률. TODO 3 무작위 파티클로 바꿔 넣기.
import time
import numpy as np
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
ALPHA = [0.4, 0.3, 0.1, 0.1, 0.04] # agvpro.yaml 의 alpha1 ~ alpha5
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1)
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def predict(P, dx, dy, dth):
"""전방향(omni) 운동 모델. P = (N, 3) 배열 [x, y, theta]. (dx, dy, dth) 는 로봇 좌표계 오도메트리"""
a1, a2, a3, a4, a5 = ALPHA
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx) # 파티클마다 자기 방향 기준으로 이동 방향
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def weight(P, ang, r):
"""가중치 w 와 '지도와 맞는 정도' fit (빔 하나당 평균 확률, 0.05 ~ 1) 을 함께 돌려줌"""
ok = r < RMAX
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a)
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
ll = np.log(q).sum(axis=1)
fit = float(np.exp(ll / ok.sum()).mean()) # 파티클 평균 '빔당 확률'
w = np.exp(BETA * (ll - ll.max()))
return w / w.sum(), fit
def resample(P, w):
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
def random_particles(n):
"""지도 안 빈 공간에 고르게 뿌린 파티클 n 개 (방향도 무작위)"""
out = np.empty((0, 3))
while len(out) < n:
c = np.column_stack([rng.uniform(0.1, 9.9, 3 * n), rng.uniform(0.1, 5.9, 3 * n),
rng.uniform(-np.pi, np.pi, 3 * n)])
out = np.vstack([out, c[dist_to_map(c[:, 0], c[:, 1]) > 0.15]])
return out[:n]
def cluster_estimate(P):
"""리샘플링 뒤 파티클이 가장 많이 모인 0.5 m 칸 주변의 평균 (AMCL 처럼 '가장 무거운 무리')"""
i = np.clip((P[:, 0] / 0.5).astype(int), 0, 19)
j = np.clip((P[:, 1] / 0.5).astype(int), 0, 11)
H = np.zeros((12, 20))
np.add.at(H, (j, i), 1)
Hp = np.pad(H, 1)
S = sum(Hp[1 + a:13 + a, 1 + b:21 + b] for a in (-1, 0, 1) for b in (-1, 0, 1))
bj, bi = np.unravel_index(S.argmax(), S.shape)
m = (np.abs(i - bi) <= 1) & (np.abs(j - bj) <= 1)
return P[m, 0].mean(), P[m, 1].mean()
# 순찰 경로: 방향 0 을 유지한 채 0.1 m 씩 (전방향 로봇이라 옆으로도 감)
WP = [(1.0, 0.7), (3.1, 0.7), (3.1, 5.0), (5.1, 5.0), (5.1, 0.7), (7.1, 0.7), (7.1, 5.0), (1.0, 5.0), (1.0, 0.7)]
PATH = []
for (x0, y0), (x1, y1) in zip(WP[:-1], WP[1:]):
n = int(round(np.hypot(x1 - x0, y1 - y0) / 0.1))
PATH += [(x0 + (x1 - x0) * k / n, y0 + (y1 - y0) * k / n) for k in range(1, n + 1)]
def run(a_slow, a_fast, seed, N=500, kidnap_at=30, steps=200):
global rng
rng = np.random.default_rng(seed)
L = len(PATH)
P = np.column_stack([1.0 + rng.normal(0, 0.1, N), 0.7 + rng.normal(0, 0.1, N), rng.normal(0, 0.05, N)])
w_slow = w_fast = 0.0
off, ok_run, recovered, max_inj = 0, 0, None, 0.0
for k in range(steps):
if k == kidnap_at: # 납치! 4 m 오른쪽, 똑같이 생긴 통로의 같은 높이로 (오도메트리는 모름)
x, y = PATH[k % L]
off = min(range(L), key=lambda j: np.hypot(PATH[j][0] - x - 4.0, PATH[j][1] - y)) - k
i = (k + off) % L
truth = np.array([PATH[i][0], PATH[i][1], 0.0])
dx, dy = PATH[i][0] - PATH[i - 1][0], PATH[i][1] - PATH[i - 1][1] # 바퀴가 느낀 이동 (한 걸음)
P = predict(P, dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, 0.05)), 0.0)
ang, r = lidar(truth)
w, fit = weight(P, ang, r)
if w_slow == 0:
w_slow = w_fast = fit
else:
pass # TODO 1: w_slow, w_fast 를 각각 a_slow, a_fast 속도로 fit 쪽으로 따라가게 합니다
p_inj = 0.0 # TODO 2: a_slow > 0 이면 max(0, 1 - w_fast / w_slow), 아니면 0
max_inj = max(max_inj, p_inj)
P = resample(P, w)
# TODO 3: 파티클마다 확률 p_inj 로 random_particles() 의 무작위 파티클로 바꿔 넣습니다
# m = rng.random(N) < p_inj -> P[m] = random_particles(m.sum())
ex, ey = cluster_estimate(P)
err = np.hypot(ex - truth[0], ey - truth[1])
if k > kidnap_at:
ok_run = ok_run + 1 if err < 0.3 else 0
if recovered is None and ok_run >= 10:
recovered = k - kidnap_at - 9
return recovered, max_inj
print("alpha_slow alpha_fast seed steps to recover (0.1 m/step) max p_inject")
for a_slow, a_fast in [(0.0, 0.0), (0.001, 0.1), (0.01, 0.3)]:
for seed in (1, 2):
rec, mi = run(a_slow, a_fast, seed)
print(f" {a_slow:6.3f} {a_fast:5.2f} {seed} {'never' if rec is None else rec:>6} {mi:.3f}")해 볼 것
- 풀이의 표에서 주입이 꺼진 경우와 켠 경우를 비교합니다.
max p_inject는 납치 직후 바로 커지나요, 로봇이 통로를 빠져나온 뒤 커지나요? (k별로p_inj를 출력해 확인합니다) - 납치 거리를
4.0에서2.0(바로 옆 통로)으로 바꾸면 재수렴이 더 쉬울까요, 어려울까요? run()의kidnap_at을 아주 큰 값(납치 없음)으로 두고 주입을 켠 채 돌려, 납치가 없을 때도max p_inject가 0 보다 큰지 봅니다. agvpro.yaml 이 주입을 끈 이유와 연결해 봅니다.
import time
import numpy as np
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
ALPHA = [0.4, 0.3, 0.1, 0.1, 0.04] # agvpro.yaml 의 alpha1 ~ alpha5
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1)
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def predict(P, dx, dy, dth):
"""전방향(omni) 운동 모델. P = (N, 3) 배열 [x, y, theta]. (dx, dy, dth) 는 로봇 좌표계 오도메트리"""
a1, a2, a3, a4, a5 = ALPHA
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx) # 파티클마다 자기 방향 기준으로 이동 방향
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def weight(P, ang, r):
"""가중치 w 와 '지도와 맞는 정도' fit (빔 하나당 평균 확률, 0.05 ~ 1) 을 함께 돌려줌"""
ok = r < RMAX
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a)
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
ll = np.log(q).sum(axis=1)
fit = float(np.exp(ll / ok.sum()).mean()) # 파티클 평균 '빔당 확률'
w = np.exp(BETA * (ll - ll.max()))
return w / w.sum(), fit
def resample(P, w):
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
def random_particles(n):
"""지도 안 빈 공간에 고르게 뿌린 파티클 n 개 (방향도 무작위)"""
out = np.empty((0, 3))
while len(out) < n:
c = np.column_stack([rng.uniform(0.1, 9.9, 3 * n), rng.uniform(0.1, 5.9, 3 * n),
rng.uniform(-np.pi, np.pi, 3 * n)])
out = np.vstack([out, c[dist_to_map(c[:, 0], c[:, 1]) > 0.15]])
return out[:n]
def cluster_estimate(P):
"""리샘플링 뒤 파티클이 가장 많이 모인 0.5 m 칸 주변의 평균 (AMCL 처럼 '가장 무거운 무리')"""
i = np.clip((P[:, 0] / 0.5).astype(int), 0, 19)
j = np.clip((P[:, 1] / 0.5).astype(int), 0, 11)
H = np.zeros((12, 20))
np.add.at(H, (j, i), 1)
Hp = np.pad(H, 1)
S = sum(Hp[1 + a:13 + a, 1 + b:21 + b] for a in (-1, 0, 1) for b in (-1, 0, 1))
bj, bi = np.unravel_index(S.argmax(), S.shape)
m = (np.abs(i - bi) <= 1) & (np.abs(j - bj) <= 1)
return P[m, 0].mean(), P[m, 1].mean()
# 순찰 경로: 방향 0 을 유지한 채 0.1 m 씩 (전방향 로봇이라 옆으로도 감)
WP = [(1.0, 0.7), (3.1, 0.7), (3.1, 5.0), (5.1, 5.0), (5.1, 0.7), (7.1, 0.7), (7.1, 5.0), (1.0, 5.0), (1.0, 0.7)]
PATH = []
for (x0, y0), (x1, y1) in zip(WP[:-1], WP[1:]):
n = int(round(np.hypot(x1 - x0, y1 - y0) / 0.1))
PATH += [(x0 + (x1 - x0) * k / n, y0 + (y1 - y0) * k / n) for k in range(1, n + 1)]
def run(a_slow, a_fast, seed, N=500, kidnap_at=30, steps=200):
global rng
rng = np.random.default_rng(seed)
L = len(PATH)
P = np.column_stack([1.0 + rng.normal(0, 0.1, N), 0.7 + rng.normal(0, 0.1, N), rng.normal(0, 0.05, N)])
w_slow = w_fast = 0.0
off, ok_run, recovered, max_inj = 0, 0, None, 0.0
for k in range(steps):
if k == kidnap_at: # 납치! 4 m 오른쪽, 똑같이 생긴 통로의 같은 높이로 (오도메트리는 모름)
x, y = PATH[k % L]
off = min(range(L), key=lambda j: np.hypot(PATH[j][0] - x - 4.0, PATH[j][1] - y)) - k
i = (k + off) % L
truth = np.array([PATH[i][0], PATH[i][1], 0.0])
dx, dy = PATH[i][0] - PATH[i - 1][0], PATH[i][1] - PATH[i - 1][1] # 바퀴가 느낀 이동 (한 걸음)
P = predict(P, dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, 0.05)), 0.0)
ang, r = lidar(truth)
w, fit = weight(P, ang, r)
if w_slow == 0:
w_slow = w_fast = fit
else:
w_slow += a_slow * (fit - w_slow)
w_fast += a_fast * (fit - w_fast)
p_inj = max(0.0, 1 - w_fast / w_slow) if a_slow > 0 else 0.0
max_inj = max(max_inj, p_inj)
P = resample(P, w)
m = rng.random(N) < p_inj # 확률 p_inj 로 무작위 파티클로 바꿔 넣기
if m.any():
P[m] = random_particles(int(m.sum()))
ex, ey = cluster_estimate(P)
err = np.hypot(ex - truth[0], ey - truth[1])
if k > kidnap_at:
ok_run = ok_run + 1 if err < 0.3 else 0
if recovered is None and ok_run >= 10:
recovered = k - kidnap_at - 9
return recovered, max_inj
print("alpha_slow alpha_fast seed steps to recover (0.1 m/step) max p_inject")
for a_slow, a_fast in [(0.0, 0.0), (0.001, 0.1), (0.01, 0.3)]:
for seed in (1, 2):
rec, mi = run(a_slow, a_fast, seed)
print(f" {a_slow:6.3f} {a_fast:5.2f} {seed} {'never' if rec is None else rec:>6} {mi:.3f}")- 예시 결과: 주입이 꺼진 (0, 0) 은 두 난수 모두 170 걸음 안에 돌아오지 못했습니다(never). (0.001, 0.1) 은 117 걸음 · never, (0.01, 0.3) 은 38 · 30 걸음이었습니다.
- 같은 조건에서도 난수에 따라 크게 다르므로, 한 번 잰 값으로 결론을 내리지 않습니다. 시뮬레이터 3 의 표처럼 여러 번 재서 중앙값을 봅니다.
계획의 실습 "횡이동이 많은 경로에서 운동 모델 · alpha 값을 조정해 위치 오차를 최소화" 입니다.
시뮬레이터 4 로 — 경로 방향 고정 · 횡이동 많음, 슬립 8 % 에서 아래 조합으로 한 바퀴씩 돌려 기록을 표로 옮깁니다. 이어서 슬립을 30 % 로 올려 Omni 의 alpha 배율을 바꿔 가며 RMS 가 가장 작은 조합을 찾습니다(alpha 슬라이더를 직접 조정해도 됩니다).
| 모델 | alpha | update_min_d | RMS (m) | 최대 (m) | 튐 (번) |
|---|---|---|---|---|---|
| Omni | agvpro.yaml | 0.025 | |||
| Diff | agvpro.yaml | 0.025 | |||
| Diff | 모두 작게 | 0.025 | |||
| Omni | agvpro.yaml | 0.3 | |||
| Omni (슬립 30 %) | 내가 찾은 값: |
Playground 로 — agvpro.yaml 의 amcl 섹션을 문자열로 읽어 alpha 를 꺼내고, 두 운동 모델을 같은 경로 · 같은 난수로 비교합니다.
- TODO 1
sample_diff()의 rot1 · trans · rot2 분해. TODO 2 alpha 배율 4 가지 비교.
import time
import numpy as np
# agv_pro_navigation2/param/agvpro.yaml 의 amcl 섹션 일부 (humble 브랜치)
AMCL_YAML = """
amcl:
ros__parameters:
alpha1: 0.4
alpha2: 0.3
alpha3: 0.1
alpha4: 0.1
alpha5: 0.04
max_particles: 2000
min_particles: 500
robot_model_type: "nav2_amcl::OmniMotionModel"
update_min_a: 0.06
update_min_d: 0.025
"""
def read_params(text):
"""'키: 값' 줄만 골라 dict 로. 숫자는 float, 따옴표 문자열은 str"""
out = {}
for line in text.splitlines():
if ":" not in line or line.strip().endswith(":"):
continue
k, v = (s.strip() for s in line.split(":", 1))
try:
out[k] = float(v)
except ValueError:
out[k] = v.strip('"')
return out
PRM = read_params(AMCL_YAML)
ALPHA = [PRM[f"alpha{i}"] for i in range(1, 6)]
print("model:", PRM["robot_model_type"], " alpha:", ALPHA)
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1)
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def sample_omni(P, dx, dy, dth, a):
"""전방향 모델: 진행(trans) · 옆(strafe) · 회전(rot) 노이즈를 따로 (nav2_amcl OmniMotionModel 구조)"""
a1, a2, a3, a4, a5 = a
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx)
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def wrap(a):
return (a + np.pi) % (2 * np.pi) - np.pi
def sample_diff(P, dx, dy, dth, a):
"""차동 모델: 회전 rot1 -> 직진 trans -> 회전 rot2 (nav2_amcl DifferentialMotionModel 구조)"""
a1, a2, a3, a4, _ = a
n = len(P)
trans = np.hypot(dx, dy)
# TODO 1: Δ 를 rot1 -> trans -> rot2 로 쪼갭니다.
# rot1 = trans < 0.01 이면 0, 아니면 atan2(dy, dx) / rot2 = wrap(dth - rot1)
# n1 = min(|rot1|, |wrap(rot1 - pi)|), n2 도 같은 방식 (뒤로 가는 경우는 회전으로 치지 않음)
rot1, rot2, n1, n2 = 0.0, dth, 0.0, abs(dth)
r1 = rot1 - rng.normal(0, np.sqrt(a1 * n1**2 + a2 * trans**2), n)
tr = trans - rng.normal(0, np.sqrt(a3 * trans**2 + a4 * n1**2 + a4 * n2**2), n)
r2 = rot2 - rng.normal(0, np.sqrt(a1 * n2**2 + a2 * trans**2), n)
P[:, 0] += tr * np.cos(P[:, 2] + r1)
P[:, 1] += tr * np.sin(P[:, 2] + r1)
P[:, 2] = wrap(P[:, 2] + r1 + r2)
return P
def weight(P, ang, r):
ok = r < RMAX
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a)
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
logw = BETA * np.log(q).sum(axis=1)
w = np.exp(logw - logw.max())
return w / w.sum()
def resample(P, w):
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
# 횡이동이 많은 경로: 방향 0 을 유지한 채 오른쪽 2 m -> 위로(옆으로) 3.4 m -> 오른쪽 2 m -> 아래로(옆으로) 3.4 m
STEP = 0.05 # 필터 갱신 간격 (m). agvpro 는 update_min_d = 0.025
WP = [(1.0, 0.7), (3.1, 0.7), (3.1, 4.1), (5.1, 4.1), (5.1, 0.7)]
PATH = [WP[0]]
for (x0, y0), (x1, y1) in zip(WP[:-1], WP[1:]):
n = int(round(np.hypot(x1 - x0, y1 - y0) / STEP))
PATH += [(x0 + (x1 - x0) * k / n, y0 + (y1 - y0) * k / n) for k in range(1, n + 1)]
def run(model, alpha, seed=1, N=300, slip=0.08):
"""(RMS 오차 m, 최대 오차 m, 추정이 한 번에 0.15 m 넘게 튄 횟수)"""
global rng
rng = np.random.default_rng(seed)
sample = sample_omni if model == "omni" else sample_diff
P = np.column_stack([1.0 + rng.normal(0, 0.05, N), 0.7 + rng.normal(0, 0.05, N), rng.normal(0, 0.03, N)])
errs, jumps, prev = [], 0, None
for k in range(1, len(PATH)):
truth = np.array([*PATH[k], 0.0])
dx, dy = PATH[k][0] - PATH[k - 1][0], PATH[k][1] - PATH[k - 1][1]
odo = (dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, slip)), rng.normal(0, 0.002))
P = sample(P, *odo, alpha)
ang, r = lidar(truth)
w = weight(P, ang, r)
est = np.array([(w * P[:, 0]).sum(), (w * P[:, 1]).sum()])
P = resample(P, w)
errs.append(np.hypot(*(est - truth[:2])))
if prev is not None and np.hypot(*(est - prev)) > 0.15:
jumps += 1
prev = est
e = np.array(errs)
return np.sqrt((e**2).mean()), e.max(), jumps
print("model alpha x RMS(m) max(m) jumps")
for model in ["omni", "diff"]:
for scale in [1.0]: # TODO 2: 배율 0.01, 0.1, 1.0, 3.0 을 모두 비교합니다
a = [v * scale for v in ALPHA]
rms, mx, jumps = run(model, a)
print(f"{model:5s} {scale:6.2f} {rms:6.3f} {mx:6.3f} {jumps:4d}")해 볼 것
- 풀이 표에서 차동 모델의 RMS 가 alpha 배율에 따라 어떻게 바뀌나요? 전방향 모델은?
run()의slip을 0.3 으로,RMAX를 3.0,K를 12 로 바꿔(라이다가 약할 때) 다시 표를 만듭니다. Omni 에서 가장 좋은 배율은 어디인가요?STEP을 0.025 · 0.1 · 0.3 으로 바꿔 차동 모델의 jumps 가 어떻게 바뀌는지 봅니다. 4교시의 설명(90° 노이즈가 더해지는 횟수)과 맞나요?
import time
import numpy as np
# agv_pro_navigation2/param/agvpro.yaml 의 amcl 섹션 일부 (humble 브랜치)
AMCL_YAML = """
amcl:
ros__parameters:
alpha1: 0.4
alpha2: 0.3
alpha3: 0.1
alpha4: 0.1
alpha5: 0.04
max_particles: 2000
min_particles: 500
robot_model_type: "nav2_amcl::OmniMotionModel"
update_min_a: 0.06
update_min_d: 0.025
"""
def read_params(text):
"""'키: 값' 줄만 골라 dict 로. 숫자는 float, 따옴표 문자열은 str"""
out = {}
for line in text.splitlines():
if ":" not in line or line.strip().endswith(":"):
continue
k, v = (s.strip() for s in line.split(":", 1))
try:
out[k] = float(v)
except ValueError:
out[k] = v.strip('"')
return out
PRM = read_params(AMCL_YAML)
ALPHA = [PRM[f"alpha{i}"] for i in range(1, 6)]
print("model:", PRM["robot_model_type"], " alpha:", ALPHA)
# 창고 지도 10 m x 6 m (시뮬레이터와 같음). 상자 = (x0, y0, x1, y1)
BOXES = np.array([[0, 0, 10, 0.1], [0, 5.9, 10, 6], [0, 0, 0.1, 6], [9.9, 0, 10, 6],
[1.8, 1.3, 2.4, 4.1], [3.8, 1.3, 4.4, 4.1], [5.8, 1.3, 6.4, 4.1], [7.8, 1.3, 8.4, 4.1],
[0.1, 2.6, 0.5, 3.4], [9.1, 0.6, 9.4, 0.9], [8.9, 4.2, 9.9, 5.0]])
SIGMA, Z_HIT, BETA = 0.15, 0.95, 0.3 # 측정 노이즈(m), 빔이 지도와 맞을 몫, 완만하게 하는 지수
K, RMAX = 24, 8.0 # 빔 수, 라이다 최대 거리
rng = np.random.default_rng(1)
def raycast(px, py, ang):
"""(px, py) 에서 각도 배열 ang 방향으로 가장 가까운 상자까지 거리"""
c, s = np.cos(ang)[:, None], np.sin(ang)[:, None]
x0, y0, x1, y1 = (BOXES[:, k] - (px, py, px, py)[k] for k in range(4))
with np.errstate(divide="ignore", invalid="ignore"):
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))
return np.where((tmax >= tmin) & (tmin > 0), tmin, np.inf).min(axis=1)
def dist_to_map(x, y):
"""점 (x, y) 에서 가장 가까운 장애물까지 거리 (likelihood field). 최대 2 m 로 자름"""
b = BOXES
dx = np.maximum(np.maximum(b[:, 0] - x[..., None], 0), x[..., None] - b[:, 2])
dy = np.maximum(np.maximum(b[:, 1] - y[..., None], 0), y[..., None] - b[:, 3])
return np.minimum(np.hypot(dx, dy).min(axis=-1), 2.0)
def lidar(pose):
ang = np.linspace(-np.pi, np.pi, K, endpoint=False)
r = raycast(pose[0], pose[1], pose[2] + ang) + rng.normal(0, 0.02, K)
return ang, np.where(r < RMAX, r, RMAX)
def sample_omni(P, dx, dy, dth, a):
"""전방향 모델: 진행(trans) · 옆(strafe) · 회전(rot) 노이즈를 따로 (nav2_amcl OmniMotionModel 구조)"""
a1, a2, a3, a4, a5 = a
n = len(P)
trans = np.hypot(dx, dy)
s_trans = np.sqrt(a3 * trans**2 + a1 * dth**2)
s_rot = np.sqrt(a4 * dth**2 + a2 * trans**2)
s_strafe = np.sqrt(a1 * dth**2 + a5 * trans**2)
bearing = P[:, 2] + np.arctan2(dy, dx)
t_hat = trans + rng.normal(0, s_trans, n)
st_hat = rng.normal(0, s_strafe, n)
P[:, 0] += t_hat * np.cos(bearing) + st_hat * np.sin(bearing)
P[:, 1] += t_hat * np.sin(bearing) - st_hat * np.cos(bearing)
P[:, 2] += dth + rng.normal(0, s_rot, n)
return P
def wrap(a):
return (a + np.pi) % (2 * np.pi) - np.pi
def sample_diff(P, dx, dy, dth, a):
"""차동 모델: 회전 rot1 -> 직진 trans -> 회전 rot2 (nav2_amcl DifferentialMotionModel 구조)"""
a1, a2, a3, a4, _ = a
n = len(P)
trans = np.hypot(dx, dy)
rot1 = 0.0 if trans < 0.01 else np.arctan2(dy, dx)
rot2 = wrap(dth - rot1)
n1 = min(abs(rot1), abs(wrap(rot1 - np.pi))) # 뒤로 가는 경우는 회전으로 치지 않음
n2 = min(abs(rot2), abs(wrap(rot2 - np.pi)))
r1 = rot1 - rng.normal(0, np.sqrt(a1 * n1**2 + a2 * trans**2), n)
tr = trans - rng.normal(0, np.sqrt(a3 * trans**2 + a4 * n1**2 + a4 * n2**2), n)
r2 = rot2 - rng.normal(0, np.sqrt(a1 * n2**2 + a2 * trans**2), n)
P[:, 0] += tr * np.cos(P[:, 2] + r1)
P[:, 1] += tr * np.sin(P[:, 2] + r1)
P[:, 2] = wrap(P[:, 2] + r1 + r2)
return P
def weight(P, ang, r):
ok = r < RMAX
a, rr = ang[ok], r[ok]
ex = P[:, 0:1] + rr * np.cos(P[:, 2:3] + a)
ey = P[:, 1:2] + rr * np.sin(P[:, 2:3] + a)
d = dist_to_map(ex, ey)
q = Z_HIT * np.exp(-d**2 / (2 * SIGMA**2)) + (1 - Z_HIT)
logw = BETA * np.log(q).sum(axis=1)
w = np.exp(logw - logw.max())
return w / w.sum()
def resample(P, w):
n = len(w)
u = (rng.random() + np.arange(n)) / n
idx = np.searchsorted(np.cumsum(w), u)
return P[np.minimum(idx, n - 1)].copy()
# 횡이동이 많은 경로: 방향 0 을 유지한 채 오른쪽 2 m -> 위로(옆으로) 3.4 m -> 오른쪽 2 m -> 아래로(옆으로) 3.4 m
STEP = 0.05 # 필터 갱신 간격 (m). agvpro 는 update_min_d = 0.025
WP = [(1.0, 0.7), (3.1, 0.7), (3.1, 4.1), (5.1, 4.1), (5.1, 0.7)]
PATH = [WP[0]]
for (x0, y0), (x1, y1) in zip(WP[:-1], WP[1:]):
n = int(round(np.hypot(x1 - x0, y1 - y0) / STEP))
PATH += [(x0 + (x1 - x0) * k / n, y0 + (y1 - y0) * k / n) for k in range(1, n + 1)]
def run(model, alpha, seed=1, N=300, slip=0.08):
"""(RMS 오차 m, 최대 오차 m, 추정이 한 번에 0.15 m 넘게 튄 횟수)"""
global rng
rng = np.random.default_rng(seed)
sample = sample_omni if model == "omni" else sample_diff
P = np.column_stack([1.0 + rng.normal(0, 0.05, N), 0.7 + rng.normal(0, 0.05, N), rng.normal(0, 0.03, N)])
errs, jumps, prev = [], 0, None
for k in range(1, len(PATH)):
truth = np.array([*PATH[k], 0.0])
dx, dy = PATH[k][0] - PATH[k - 1][0], PATH[k][1] - PATH[k - 1][1]
odo = (dx * (1 + rng.normal(0, 0.02)), dy * (1 + rng.normal(0, slip)), rng.normal(0, 0.002))
P = sample(P, *odo, alpha)
ang, r = lidar(truth)
w = weight(P, ang, r)
est = np.array([(w * P[:, 0]).sum(), (w * P[:, 1]).sum()])
P = resample(P, w)
errs.append(np.hypot(*(est - truth[:2])))
if prev is not None and np.hypot(*(est - prev)) > 0.15:
jumps += 1
prev = est
e = np.array(errs)
return np.sqrt((e**2).mean()), e.max(), jumps
print("model alpha x RMS(m) max(m) jumps")
for model in ["omni", "diff"]:
for scale in [0.01, 0.1, 1.0, 3.0]:
a = [v * scale for v in ALPHA]
rms, mx, jumps = run(model, a)
print(f"{model:5s} {scale:6.2f} {rms:6.3f} {mx:6.3f} {jumps:4d}")- 예시 결과(slip 0.08): omni 는 배율과 관계없이 RMS 약 2 cm · jumps 0 입니다. diff 는 배율 0.01 · 0.1 에서 RMS 2 ~ 3 cm 이지만, 1.0(agvpro 값)에서 RMS 0.14 m · 최대 1.2 m · jumps 10, 3.0 에서 RMS 0.75 m · jumps 60 으로 무너집니다.
- 차동 모델의 회전 노이즈 α1 · (π/2)² 는 옆 이동 거리와 상관없이 매 갱신 더해지므로, alpha 를 키울수록 · 갱신이 잦을수록 나빠집니다.
창고 시뮬레이터로 확인하기#
Playground 의 타깃을 Simulator 로 두면 공식 URDF 로 만든 3D 창고 시뮬레이터에 from pymycobot import MyAGVPro 가 그대로 연결됩니다. agvsim 은 참값 위치 · 스캔 · 지도처럼 실물에는 없는 정보를 읽는 시뮬레이터 전용 모듈입니다 (실물에서는 ROS 2 토픽이 그 역할). 가속 한계 · 미끄러짐 · 보호 영역 거리 등은 교육용 가정입니다.
미끄러짐 10 % 로 앞 · 옆 · 회전을 한 번씩 한 뒤, agvsim.get_pose()(참값)를 "AMCL 이 라이다로 찾아낸 지도 위 위치" 라고 가정하고 map → odom 보정을 계산합니다.
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 compose(a, b): # a ∘ b (2D 변환 합성, ch04)
x, y, t = a
return (x + b[0] * math.cos(t) - b[1] * math.sin(t),
y + b[0] * math.sin(t) + b[1] * math.cos(t), t + b[2])
def inverse(a):
x, y, t = a
return (-x * math.cos(t) - y * math.sin(t), x * math.sin(t) - y * math.cos(t), -t)
agvsim.set_pose(0, 0, 0)
agvsim.set_slip(0.1)
for cmd in (agv.move_forward, agv.move_left_lateral, agv.turn_left):
cmd(0.4); time.sleep(2.0); agv.stop(); time.sleep(0.6)
agvsim.set_slip(0.0)
T_map_base = agvsim.get_pose() # AMCL 이 라이다로 찾아낸 "지도 위 위치" 라고 가정
T_odom_base = agvsim.get_odom() # 바퀴가 믿는 위치
# TODO: map -> odom 보정 = T_map_base ∘ (T_odom_base)^-1
T_map_odom = compose(T_map_base, inverse(T_odom_base))
print("map->base ", [round(v, 3) for v in T_map_base])
print("odom->base ", [round(v, 3) for v in T_odom_base])
print("map->odom ", [round(v, 3) for v in T_map_odom])
check = compose(T_map_odom, T_odom_base)
print("check map->odom->base", [round(v, 3) for v in check])확인할 것: 마지막 줄 check 가 map->base 와 같아야 합니다. 즉 오도메트리는 그대로 두고 보정 변환 하나만 덧붙이면 지도 위 위치가 맞습니다. 미끄러짐을 0 으로 두고 다시 실행하면 map->odom 이 (0, 0, 0) 에 가까워지는지도 확인하세요 — 오도메트리가 좋을수록 AMCL 의 "점프" 가 작다는 4교시 설명과 같습니다.
실습 (실물)#
ch06 에서 저장한 지도로 AMCL 을 띄우고, 파티클 구름이 줄어드는 모습 · 2D Pose Estimate · 납치 실험을 실물로 확인합니다. 로봇과 PC 의 연결 방법 · 작업 공간 경로 · 지도 파일 이름은 공식 문서와 로봇 이미지에 따라 다를 수 있으니 먼저 확인합니다 (확인 필요).
# 터미널 1: 로봇 기동 (라이다 · 오도메트리 · TF)
ros2 launch agv_pro_bringup agv_pro_bringup.launch.py
# 터미널 2: 지도 + AMCL + Nav2 + RViz (agvpro.yaml 을 읽음)
ros2 launch agv_pro_navigation2 navigation2_active.launch.py
# 다른 지도를 쓰려면 (경로는 예시)
ros2 launch agv_pro_navigation2 navigation2_active.launch.py map:=$HOME/agv_pro_ros2/src/agv_pro_navigation2/map/map.yaml
# 터미널 3: 키보드 조작 (ch06 과 같은 teleop)# 위치 추정 상태 확인
ros2 param get /amcl robot_model_type # nav2_amcl::OmniMotionModel 인가
ros2 param get /amcl max_particles
ros2 topic echo /amcl_pose --once # 위치 + 공분산
ros2 run tf2_ros tf2_echo map odom # AMCL 이 내는 보정
ros2 service call /reinitialize_global_localization std_srvs/srv/Empty # 전역 초기화 (실험용)
ros2 service call /request_nomotion_update std_srvs/srv/Empty # 멈춘 채 필터 한 번 돌리기RViz 에서 ParticleCloud 표시를 켜면 /particle_cloud 의 파티클이 화살표로 보입니다. 보이지 않으면 표시의 Reliability 를 Best Effort 로 바꿔 봅니다 (자주 나는 오류 2).
아래는 /amcl_pose 를 받아 위치와 표준편차(σx · σy · σθ) 를 출력하는 실물 전용 예시입니다. 공분산은 6 × 6 행렬(x, y, z, roll, pitch, yaw)을 한 줄로 편 36 개 값이므로, 0 · 7 · 35 번째가 x · y · yaw 의 분산입니다.
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy
from geometry_msgs.msg import PoseWithCovarianceStamped
class AmclWatcher(Node):
def __init__(self):
super().__init__("amcl_watcher")
# amcl_pose 는 마지막 값을 남겨 두는 QoS 로 발행되는 경우가 많음 (확인 필요)
qos = QoSProfile(depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL,
reliability=ReliabilityPolicy.RELIABLE)
self.create_subscription(PoseWithCovarianceStamped, "/amcl_pose", self.on_pose, qos)
def on_pose(self, msg):
p = msg.pose.pose
q = p.orientation
yaw = math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
c = msg.pose.covariance
sx, sy, sth = math.sqrt(c[0]), math.sqrt(c[7]), math.sqrt(c[35])
self.get_logger().info(
f"x {p.position.x:+.3f} y {p.position.y:+.3f} yaw {math.degrees(yaw):+6.1f} deg | "
f"sigma x {sx * 100:.1f} cm y {sy * 100:.1f} cm yaw {math.degrees(sth):.1f} deg")
def main():
rclpy.init()
node = AmclWatcher()
try:
rclpy.spin(node)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()실물에서 해 볼 것:
- 로봇을 지도 원점이 아닌 곳에 두고 AMCL 을 켭니다. RViz 에서 로봇 모델과 스캔(빨간 점)이 지도의 벽과 어긋나 있는지 봅니다. 2D Pose Estimate 로 위치를 준 뒤, 위 노드의 σ 가 어떻게 바뀌는지 기록합니다 (RViz 가 준 σ 0.5 m 근처에서 시작하는가).
- teleop 으로 천천히 1 ~ 2 m 움직이며 파티클 구름과 σ 가 줄어드는 것을 봅니다. 로봇을 멈추면 σ 가 계속 줄어드나요?
/request_nomotion_update를 몇 번 불러 비교합니다. - 방향을 그대로 둔 채 옆으로(횡이동) 1 m 를 갑니다.
robot_model_type이 Omni 인 지금은 구름이 어떻게 움직이나요? (Diff 로 바꿔 보는 실험은 파라미터 파일을 복사해 고친 뒤params_file:=로 넘겨야 하므로 강사와 함께 합니다.) - (전원 · 모터 정지 상태에서) 두 사람이 로봇을 들어 비슷하게 생긴 다른 통로로 옮깁니다. 다시 켠 뒤 천천히 움직이며 AMCL 이 스스로 고치는지 봅니다(agvpro.yaml 은 주입이 꺼져 있음). 고치지 못하면 2D Pose Estimate 로 바로잡습니다.
자주 나는 오류와 해결#
1) AMCL 이 위치를 내지 않고 RViz 에 로봇이 지도에 안 붙음#
[amcl]: AMCL cannot publish a pose or update the transform. Please set the initial pose...- 원인: AMCL 이 초기 위치를 받지 못해
map → odom을 발행하지 않습니다.set_initial_pose가 꺼진 파라미터 파일을 쓰거나,/initialpose가 아직 오지 않았을 때 나옵니다. 문구는 Nav2 버전에 따라 다를 수 있습니다 (확인 필요). - 해결: RViz 의 2D Pose Estimate 로 실제 위치와 방향을 줍니다. 매번 같은 곳에서 시작한다면
set_initial_pose: True와initial_pose.x/y/yaw를 그 위치로 맞춥니다. agvpro.yaml 의 기본값은 지도 원점 (0, 0, 0) 입니다.
2) RViz 에 파티클 구름(ParticleCloud)이 보이지 않음#
ParticleCloud Status: Warn No messages received- 원인:
/particle_cloud는 Best Effort 로 발행되는데 RViz 표시가 Reliable 로 구독하면 QoS 가 맞지 않아 메시지를 받지 못합니다 (Nav2 Humble 기준, 확인 필요). 표시의 토픽 이름이 다른 경우도 있습니다. - 해결: ParticleCloud 표시의 Topic 을
/particle_cloud, Reliability Policy 를Best Effort로 바꿉니다.ros2 topic info /particle_cloud -v로 발행 쪽 QoS 를 확인합니다.
3) 옆으로(횡이동) 갈 때만 위치가 튀거나 로봇 모델이 빙글 돎#
(RViz) 직진 · 회전은 괜찮은데, 옆으로 0.5 m 만 가도 로봇 위치가 수십 cm 씩 튀고 방향이 흔들림- 원인:
robot_model_type이nav2_amcl::DifferentialMotionModel로 되어 있어 옆 이동을 "90° 회전 + 직진" 으로 해석하고 큰 회전 노이즈를 뿌립니다 (4교시 · 시뮬레이터 4). 다른 로봇용 파라미터 파일을 복사해 쓸 때 흔합니다. - 해결:
robot_model_type: "nav2_amcl::OmniMotionModel"로 고칩니다. Humble 의 값은 플러그인 이름(nav2_amcl::...)이고, 옛 버전(Foxy 등)은"omnidirectional"·"differential"같은 문자열을 썼으므로 버전에 맞는 문서를 확인합니다.ros2 param get /amcl robot_model_type으로 실제 값을 확인합니다.
4) 옆 통로에 수렴해 버림 (지도와 스캔은 잘 맞아 보이는데 위치가 2 m 틀림)#
/amcl_pose: x +5.10 (실제 +3.10), sigma x 3.2 cm -- 확신하는데 틀림- 원인: 똑같이 생긴 선반 열에서 다봉 분포가 한쪽으로 오수렴했습니다 (3교시). 통로 안에서는 스캔이 그대로 맞으므로 σ 도 작게 나옵니다. 전역 초기화를 했거나, 2D Pose Estimate 를 옆 통로에 잘못 준 경우에 생깁니다.
- 해결: 2D Pose Estimate 를 실제 통로에 다시 줍니다. 통로 끝의 기둥 · 충전소처럼 구별되는 곳이 라이다에 보이는 자리에서 초기 위치를 주면 더 확실합니다. 자주 일어나면 통로 끝마다 서로 다른 표식(반사판 · 상자)을 두는 것도 방법입니다.
5) 스캔이 계속 버려진다는 경고#
[amcl]: Message Filter dropping message: frame 'laser_link' at time 1712.345 for reason 'discarding message because the queue is full'- 원인: AMCL 은 스캔의 시각에 맞는
odom → base_footprintTF 를 찾아야 하는데, 그 시각의 TF 가 아직 없거나 너무 늦게 옵니다. 오도메트리 노드가 꺼졌거나 느린 경우, 로봇과 PC 의 시계가 맞지 않는 경우, 시뮬레이션이 아닌데use_sim_time이 True 인 경우가 흔합니다. 프레임 이름 · 문구는 환경에 따라 다릅니다. - 해결:
ros2 run tf2_ros tf2_echo odom base_footprint로 TF 가 나오는지,ros2 topic hz /scan·/odom주기를 확인합니다. 여러 컴퓨터를 쓰면 시계를 맞추고(NTP 등), 실물에서는use_sim_time: False인지 확인합니다.
6) 로봇이 멈춰 있으면 위치가 안 고쳐짐#
(RViz) 2D Pose Estimate 를 대충 준 뒤 기다려도 파티클 구름이 그대로 넓음- 원인: AMCL 은
update_min_d(0.025 m) ·update_min_a(0.06 rad) 만큼 움직여야 필터를 돌립니다. 멈춰 있으면 새 스캔이 와도 갱신하지 않습니다 (4교시). - 해결: 로봇을 조금(몇 cm, 몇 도) 움직이거나
ros2 service call /request_nomotion_update std_srvs/srv/Empty를 몇 번 부릅니다. 제자리에서 천천히 한 바퀴 돌면 구름이 빨리 모입니다.
7) Playground 실습에서 가중치가 nan 이 됨#
RuntimeWarning: invalid value encountered in divide
step 0 ... estimate(nan, nan, nan)- 원인: 빔 확률을 곱하면(로그로는 더하면) 매우 작은 수가 되어
np.exp(logw)가 모두 0 이 되고, 0 / 0 으로 nan 이 생깁니다. 빔 수가 많거나 σ 가 작을수록 잘 생깁니다. - 해결: 실습 2-1 의 풀이처럼
np.exp(logw - logw.max())로 가장 큰 값을 빼고 지수를 취합니다. 가중치의 비율은 그대로이고 가장 큰 값이 1 이 되므로 넘치거나 0 이 되지 않습니다.
과제#
- 베이즈 필터 손계산: 문 3개 복도(3 · 8 · 15)에서 "문 → 1칸 이동 → 벽 → 1칸 이동 → 벽" 순서를 손으로 계산해(적중 0.9 · 오탐 0.1 · ε 0.1) 실습 1-1 의 코드 결과와 맞춰 봅니다. 확률이 가장 큰 칸 셋과 값을 적습니다.
- 파티클 수 설계 보고서: 실습 2-2 의 표를 초기 퍼짐 0.5 · 1.0 · 1.5 · 2.0 m 로 만들고, "퍼짐 대비 최소 N" 그래프(손그림 가능)와 함께 AMCL 의 min 500 · max 2000 이 이 창고에 알맞은지 한 문단으로 평가합니다.
- 납치 회복 비교: 실습 3-1 의 시뮬레이터 표(난수 3 번 × 4 조건)와 Playground 결과를 함께 정리하고, 주입을 켜는 것의 장단점(회복 시간 vs 평소의 흔들림)을 근거와 함께 씁니다. 여러분의 물류 현장이라면 켤지 끌지 정합니다.
- 파라미터 해설 카드:
agvpro.yamlamcl 섹션에서 5 개를 골라 "값 · 뜻 · 키우면 · 줄이면 · 이 차시에서 확인한 곳(시뮬레이터 번호)" 을 카드 한 장씩으로 만듭니다. 반드시robot_model_type·alpha1·update_min_d를 포함합니다.
참고자료#
- myAGV Pro 공식 문서 (내비게이션 · SLAM 절차): https://docs.elephantrobotics.com/docs/myAGV_Pro_en/
- myAGV Pro ROS 2 패키지 (
agv_pro_navigation2/param/agvpro.yaml·launch/navigation2_active.launch.py): https://github.com/elephantrobotics/agv_pro_ros2 - 제품 페이지: https://www.elephantrobotics.com/en/myagv-pro/
- 제품 소개 · 이미지 (현영이앤씨): https://www.hyenc.net/products/36
- Nav2 AMCL 파라미터 설명: https://docs.nav2.org/configuration/packages/configuring-amcl.html
- nav2_amcl 소스 (운동 모델 · likelihood field 구현): https://github.com/ros-navigation/navigation2/tree/humble/nav2_amcl
- RViz 2D Pose Estimate · Nav2 시작하기: https://docs.nav2.org/getting_started/index.html
- S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics (MIT Press, 2005) — 2장 베이즈 필터, 4장 파티클 필터, 5장 운동 모델, 6장 likelihood field, 8장 Monte Carlo 위치 추정 · Augmented MCL · KLD 샘플링
- D. Fox, "Adapting the Sample Size in Particle Filters Through KLD-Sampling", International Journal of Robotics Research, 2003
- 이 사이트의 도구: Playground · 물류창고 시뮬레이터 · 3D 뷰어