2륜 이동로봇과 칼만 필터 EKF-SLAM

① 로봇이 자신의 위치를 어떻게 알까?

두 바퀴의 회전량을 적분하면 이동 위치를 예측할 수 있지만, 바퀴 미끄러짐과 엔코더 오차가 누적됩니다. EKF-SLAM은 로봇 위치와 주변 랜드마크의 위치를 하나의 상태벡터로 관리하고, 거리·방위 관측으로 함께 보정합니다.

구현 범위: 알려진 랜드마크 ID를 사용하는 2D EKF-SLAM 교육용 모델입니다. 랜드마크의 실제 위치는 필터에 제공하지 않으며, 최초 관측으로 지도에 추가합니다. 카메라 특징점 매칭, 데이터 연관, 루프 클로저 최적화, 점유격자 지도는 포함하지 않습니다. 최초 로봇 자세를 원점 기준으로 고정하므로 지도 역시 이 좌표계에 대해 추정합니다.

② 실시간 지도와 위치 추정

③ 엔코더 적분과 EKF-SLAM 비교

동일한 실제 궤적을 기준으로 오도메트리만 사용한 위치와 랜드마크 관측으로 보정한 EKF 위치를 비교합니다. 첫 번째 탭에서 시뮬레이션을 실행하면 아래 그래프가 갱신됩니다.

「엔코더 위치를 실제 위치로 보정」 버튼은 시뮬레이터만 알고 있는 정답 위치를 오도메트리에 복사하는 교육용 기능입니다. 실제 로봇에서는 GPS, 비전, 기준 마커 등 별도의 절대 위치 정보가 필요합니다. SLAM은 초기 기준 좌표계의 전역 이동·회전 자유도를 관측만으로 없앨 수 없습니다. 여기서는 초기 위치와 방향을 알고 있다고 가정합니다. 관측이 적거나 필터의 잡음 설정이 틀리면 EKF가 항상 더 정확한 것은 아닙니다.

④ 펀월드 수식 설명 — 차동구동 로봇

좌우 바퀴 선속도 vL, vR과 바퀴 간격 b를 이용해 로봇의 병진·회전 속도를 계산합니다.

엔코더의 4체배 카운트를 사용하면 바퀴 이동거리는 다음과 같습니다.

⑤ 펀월드 수식 설명 — EKF 예측

상태벡터는 로봇 자세와 현재 발견한 랜드마크 좌표를 함께 포함합니다.

F는 상태 전이 Jacobian, G는 좌우 바퀴 이동량에 대한 Jacobian입니다. Q는 바퀴 이동 오차의 공분산입니다. 예측 단계에서는 지도 랜드마크 좌표를 그대로 유지하고, 로봇 자세 및 교차 공분산을 갱신합니다.

⑥ 펀월드 수식 설명 — 랜드마크 관측 보정

ν는 혁신(Innovation), K는 칼만 게인, H는 관측 Jacobian, R은 거리·방위 측정 잡음 공분산입니다. 공분산은 수치 안정성이 좋은 Joseph 형식으로 업데이트합니다. 각도 잔차는 −π~π로 정규화합니다.

처음 관측한 랜드마크의 지도 등록

기존 상태와 새 랜드마크의 상관관계를 Jacobian으로 전파하여 공분산 행과 열을 추가합니다. 지도 좌표를 실제 정답으로 초기화하지 않습니다.

⑦ 추천 실험

1. 기본 설정에서 원형 주행으로 30초 동안 EKF와 오도메트리 오차를 비교합니다.
2. 휠 슬립을 증가시키고, 랜드마크 보정이 오차를 얼마나 줄이는지 확인합니다.
3. 랜드마크 관측 범위를 줄이거나 관측 주기를 늘려 보정 빈도를 줄입니다.
4. 거리·방위 센서 노이즈를 증가시켜 추정 결과와 공분산 변화를 비교합니다.
5. 필터의 잡음 가정(Q/R)을 실제 노이즈와 다르게 설정하면 추정 결과가 어떻게 바뀌는지 관찰합니다.