이전 글에 이어 EKF-SLAM을 3D 환경으로 옮겨 보았습니다. 이전 글에서는 로봇이 랜드마크를 인식하였을 때 평면상의 방향 정보만을 사용하였습니다. 여기서는 평면상의 방향정보와 수직으로 방향정보를 사용합니다. 이러한 사실은 카메라에서 벽에 붙은 마커를 인식하였을 경우를 가정한 것입니다. 카메라로 받아들인 영상에서 마커를 인식한 경우, 영상에서 마커의 위치(x,y)는 쉽게 알 수 있지만 마커와의 거리(z)는 알아내기가 어렵기 때문입니다. 영상에서 마커의 위치를 알 경우 카메라의 화각을 안다면 마커의 수직방향과 수평방향의 각도는 쉽게 계산가능합니다.
EKF-SLAM의 수행에 대하여는 다음 문서를 참조하시기 바랍니다.
아래는 EKF-SLAM을 구현한 예제 코드입니다.
상기 소스코드를 컴파일 하여 실행하기 위해서는 이동로봇을 시뮬레이션 하는 시뮬레이터가 필요합니다. 이동로봇 시뮬레이터는 다음 링크를 참조하시기 바랍니다.
xxx - Mobile Robot Simulation 프로그램
다음 동영상을 보시면, 로봇은 회색 박스로 표시되며 로봇 바로 위의 매쉬 형태의 타원체가 로봇의 위치와 방향에 대한 공분산 행렬을 시각적으로 표시한 것입니다. 그리고 공간상의 갈색 구는 랜드마크들입니다. 로봇이 이를 인식하게 되면 붉은색으로 바뀌면서 주변에 랜드마크의 위치(x,y,z)에 대한 공분산 행렬을 매쉬 형태의 타원체로 표시합니다.