FastSLAM - 3.3 FastSLAM 1.0 알고리즘

 

3.3 FastSLAM 1.0 알고리즘

 식 (3.3)과 같은 사후확률 분해는 SLAM 문제에 있어서 중요한 구조를 보여주고 있습니다. 이 구조는 적절한 조건이 주어짐에 따라 랜드마크들사이에 상관관계가 존재하지않음을 알려주고 있습니다. FastSLAM은 M + 1개의 필터를 사용하여 분해된 표현을 사용하고 있으며, 모든 N + 1개들은 저차원이 됩니다.

 

 FastSLAM은 식 (3.3)의 첫번째 항인 로봇의 경로 사후확률을 파티클 필터를 사용하여 추정합니다. 나머지 N개의 조건부 랜드마크 사후확률은 EKF를 사용하여 추정됩니다. 각 EKF 추적기는 단일 랜드마크의 위치를 추적하며, 그러므로 고정된 크기의 저차원이 됩니다. 이 랜드마크 EKF는 파티클필터에서의 파티클이며, EKF에서 처리되는 파티클인 로봇의 경로에 조건부가 됩니다.

 

그림 3.3 파티클 필터에서의 M개의 파티클이 있으며, 각 파티클은 N개의 독립적인 EKF들을 가지고 있습니다. 랜드마크 추정치들 사이에 아무런 상관관계를 갖지 않습니다.

 

 

 총 N * M 개의 EKF가 존재하며, M개는 파티클필터에서 전체 파티클의 개수이고, 파티클 필터는 그림 3.3에서 시각적으로 보여주고 있습니다. 각 FastSLAM의 파티클들은 다음의 형태를 띄고 있습니다.

 꺽쇄 표기인 [m]은 파티클의 인덱스를 나타내며 $s^{t, [m]}$은 m번째 파티클의 경로 추정, $\mu_{n, t}^{[m]}$, $\Sigma_{n, t}^{[m]}$은 경로 $s^{t, [m]}$가 주어질때 n번째 특징의 위치를 나타내는 가우시안의 평균과 공분산이 됩니다.

 

 이 값들은 모두 m번째 파티클 $S_{t}^{[m]}$에 속하며, FastSLAM 사후확률에는 총 M개가 있습니다. 시간 t-1로부터 시간 t에 대한 사후확률을 계산하는 필터링은 $S_{t-1}$에서 새 파티클 집합인 $S_{t}$를 생성하는 과정도 포함됩니다. 새로운 파티클 집합은 최신 제어 입력 $u_t$와 관측 $z_t$(그리고 대응관계 데이터 연관 $n_t$)를 반영합니다.

 

 여기서 갱신 과정은 4가지 단계로 수행됩니다. 첫번째로 새로운 로봇의 자세가 최신 제어 입력을 반영하여 각 파티클로 그려집니다. 각 자세들은 적절한 로봇 경로 추정기 $s^{t-1, [m]}$에 추가됩니다. 다음으로 관측된 랜드마크에 대응하는 랜드마크 EKF들이 새로운 관측으로 갱신됩니다. 로봇의 경로 파티크이 실제 경로 사후확률로부터 그려지지 않았기 때문에 각 파티클들은 이 차이를 반영하기위해 중요도 가중치를 받습니다. 새로운 파티클 집합 $S_t$가 중요도 리샘플링을 사용하여 가중화된 파티클 집합으로부터 생성됩니다. 이 중요도 리셈플링 단계는 파티클들이 실제 사후확률을 따라 분포되도록 하기위해 필수적입니다. 이 FastSLAM의 기본적인 4가지 단계는 표 3.1에서 보여주고 있으며, 아래의 섹션에서 자세히 살펴보겠습니다.

표 3.1 기본 FastSLAM 알고리즘 개요