Journal Search Engine
Search Advanced Search Adode Reader(link)
Download PDF Export Citaion korean bibliography PMC previewer
ISSN : 1975-6291(Print)
ISSN : 2287-3961(Online)
Journal of Korea Robotics Society Vol.7 No.1 pp.29-35
DOI :

광범위 환경에서 EKF-SLAM의 일관성 향상을 위한 새로운 관찰모델

남 창 주1, 강 재 현2, 도 낙 주
고려대학교 전기전자전파, Texas A&M Computer Science & Engineering

A new Observation Model to Improve the Consistency of EKF-SLAM Algorithm in Large-scale Environments

Changjoo Nam1, Jaehyeon Kang2, Nakju Lett Doh

( Received : Nov. 4. 2011; Reviewed : Nov. 16. 2011; Accepted : Jan. 10. 2012 )

Abstract

This paper suggests a new observation model for Extended Kalman Filter basedSimultaneous Localization and Mapping (EKF-SLAM). Since the EKF framework linearizesnon-linear functions around the current estimate, the conventional line model has large linearizationerrors when a mobile robot locates faraway from its initial position. On the other hand, the model thatwe propose yields less linearization error with respect to the landmark position and thus suitable in alarge-scale environment. To achieve it, we build up a three-dimensional space by adding a virtual axisto the robot’s two-dimensional coordinate system and extract a plane by using a detected line on thetwo-dimensional space and the virtual axis. Since Jacobian matrix with respect to the landmarkposition has small value, we can estimate the position of landmarks better than the conventional linemodel. The simulation results verify that the new model yields less linearization errors than theconventional line model.

1. 서 론

SLAM(동시적인 위치인식과 지도 작성)은 지난 20년간 널리 연구되어 왔다. 그 중에서 SLAM문제에 가장 많이 사용 되는 것 중 하나는 확장된 칼만 필터(EKF)이다. 그러나 EKF는 테일러 전개를 이용하여 비선형 모델을 선형화하는 과정에서 선형화 에러가 발생하는 문제를 가지고 있다. 즉, EKF는 로봇과 랜드마크의 실제 위치가 아니라 현재 추정 위치의 주위에서 비선형함수를 선형화한다. 따라서 이러한 선형화 에러는 로봇의 주행 중에 계속 누적되게 된다. 이는 정확하지 않은 추정 값과 실제상태의 비 가우시안 분포에 의해 EKF-SLAM 알고리즘[1] 의 비일관성(inconsistency)을 유발한다. 반면 직선 특징(line feature)은 어느 곳에서도 추출 할 수 있고 작은 수의 특징으로도 긴 복도를 표현 할 수 있기 때문에 실내 SLAM에 매우 유용하게 사용된다[2]. 하지만 허프변환(hough transform)과 같은 파라미터를 사용하여 직선을 표현하는데 가장 많이 사용 되는 방법 중 하나인 M-model(Measurement Space-model)은[3] 모바일 로봇이 넓은 환경을 주행 할 때 큰 선형화 에러를 유발하게 된다. 이는 로봇이 작은 공간에 존재하게 되면 큰 문제가 되지 않는다. 하지만 시간이 지나면서 누적되는 에러에 의해 일관성을 잃게 되는 문제는 해결하지 못한다. 따라서 본 논문에서는 넓은 환경에서의 선형화 에러를 줄이기 위해 직선 특징의 새로운 관찰 모델을 제안한다. 이를위해, 로봇의 2차원 공간에 새로운 가상의 축을 더하여 3차원 공간을 만든다. 그리고 사영평면을 만들어 센서 데이터에서 추출된 직선을 사영평면에 사영한다. 이렇게 하면 축에 존재하는 점과 사영된 직선을 이용하여 SLAM의 랜드마크로 사용할 가상의 평면을 추출 할 수 있다. M-model의 랜드마크의 위치와 관련된 자코비안이 로봇의 출발점에서 멀어질수록 계속 커지는 것과 달리 제안된 모델의 자코비안 행렬은 넓은 공간에서도 매우 작은 값을 나타낸다. 이는 이와 같은 상황에서 제안된 모델이 M-model보다 랜드마크의 위치를 더 정확하게 추정 할 수 있음을 의미한다. 따라서 이는 넓은 환경에서 EKFSLAM이 가지는 비일관성의 문제를 줄여 줄 수 있다.

논문에서 사용할 표기법은 다음과 같다. 왼쪽 위의 첨자는 기준 좌표계를 의미하며, 오른쪽 아래의 첨자는 해당 좌표계를 통해 나타내는 값을 의미한다. 예를 들어 r Lf 의 경우 로봇 좌표계(r)를 기준으로 특징(f)을 나타내는 값을 의미한다. 단 기준 좌표계가 월드 좌표계(w)인 경우에는 w의 표기를 생략하기로 한다. 또한 추정 값은  와 같이 윗 첨자를 사용하여 표현한다. 

2. 문제 정의

2.1 Problem Definition

2차원 평면상에서 SLAM을 실시하는 경우 로봇이 넓은환경을 오랜 시간에 걸쳐 진행해 나감에 따라 오차가 지속적으로 쌓이게 되고 이로 인해 일관성을 잃게 되는 문제가 발생 한다. 본 논문에서는 거리 센서를 이용하여 직선을 추출해 낼 수 있는 2차원 실내 환경에서 SLAM을 실시한다는 가정 하에 이러한 일관성의 문제를 풀 것이다. 

2.2 M-model

 직선 기반의 EKF-SLAM 알고리즘의 상당수는 관찰된 직선을 표현하기 위해, 사용되는 변수가 작은 M-model을 사용 한다[4-6] . 이 모델은 그림 1 에서와 같이 절대 거리 값(ρ)을 가지는 법선 벡터와 벡터의 각도(α)를 통해 직선을 나타낸다. M-model의 측정 방정식은 다음과 같다.

 

그림 1. 절대거리 값(ρ)의 법선벡터와 각도(α)를 통해 직선을 나타내는 M-model

2.3 자코비안

 M-model의 자코비안 행렬은 다음과 같이 계산된다. 여기서 X는 현재의 로봇 상태와 측정값으로 이루어 져있는 벡터이고 h는 현재의 측정값이다. 또한  xr, yr, Ψr 은 각각 월드좌표계 기준으로 로봇의 x좌표, y좌표, 진행방향의 각도를 의미한다.

 

 Hr 행렬의 성분의 값은 삼각함수에 의해 –1 ~ 1 로 제한 되어있다. 반면에 HL(1,2) 값은 로봇의 위치 (xr·yr)관련되어 있다. 따라서 로봇이 시작지점에서 멀어질수록 값이 계속 증가하게 된다. (자코비안 행렬의 값을 측정하기위해 시뮬레이션을 실행한 결과는 4장에설명되어 있다.) HL(1,2) 의 값이 로봇의 위치에 따라 증가하기 때문에 로봇이 넓은 환경을 주행할 때 큰 선형화에러를 발생하게 된다. 이는 EKF-SLAM 알고리즘의 일관성에도 영향을 미친다. 따라서 선형화 에러를 낮추기 위한 새로운 직선 특징의 관찰 모델이 필요하다.

3. 제안된 관찰 모델

 그림 2는 관찰된 직선 특징의 길이와 각도를 표시한다. 월드 좌표계에서 직선 특징의 두 끝점은 다음과 같이 표현된다.

 

그림 2. 직선 특징의 관찰 모델

3.1 가상의 평면 추출

 제안된 모델의 관찰 모델을 정의하기 위해 로봇의 2차원 좌표계에 가상의 축을 더하여 3차원 좌표계에 존재하는 사영평면을 만들었다. 이때 사영평면은 동차좌표계(homogeneous coordinate)에서 마지막 좌표 값을 1로 두어 표현하는 것에 따라 w = 1의 평면상에 존재 한다. 그리고 그림 3 (a) 에서 보이듯이 관찰된 직선을 사영평면에 사영시킨다. 이를 통해 사영된 직선의 두 끝점과 로봇의 위치 Xr 로 구성되어 있는 새로운 평면을 만들 수 있다. 월드 좌표계에 대한 평면의 법선벡터는 다음과 같다.

 

그림 3. 좌표변환을 통해 생성된 가상 평면 : (a) xy평면에 ω axis를 더하고 관찰된 직선을 사영 평면에 사영 (b)Xm1, Xm2, Xm3, 세 점에서의 평면생성

(5)식을 (6)식에 대입하면 다음과 같은 식을 얻을 수 있다.

 

3.2 관찰모델

 우선 법선벡터를 xy 평면에 사영시켜 α를 정의 한다. 그림 4에서 나타내듯이 사영된 벡터와 x축사이의 각도가 α로 정의 된다. cm 은 (6)식의 cm과 같고 이 am과 cm을 관찰모델로 사용한다. 센서 값과 관찰 식은 다음과 같다.

 

그림 4. 각도 α의 정의

 r  에서 추정된 관찰 값과 월드 좌표계의 센서 값은 다음과 같다.

마지막으로 로봇 좌표계에서 등록된 특징의 기대 값은 다음과 같다. 

 

 

 

3.3 자코비안 행렬

 제안된 방법의 자코비안 행렬은 다음과 같다.

 

 도출된 자코비안 행렬 Hr의 결과는 다음과 같다.

 

 그러나 , 과  이 변수가 아니라 거리와 각도의 함수이기 때문에 HB 행렬은 수식적(analytical)으로 계산할 수 없다. 따라서 미분의 정의에 의하여 수치적(numerical)인 값을 찾을 수밖에 없다. 자코비안 행렬의 값은 다음장에 그래프로 나타내어져있다.

4. 시뮬레이션 결과

우선 모바일 로봇이 시작지점에서 멀리 떨어져 있는 여러 환경을 설정 하였다. 그림 5는 시뮬레이션의 두 가지 환경을 보여준다. 그림 5(a)는 로봇이 Xr(10,10)에 존재하는 경우를 나타내고 그림 5(b)는 로봇이 Xr(2000,2000)에 존재하는 경우를 나타낸다. 두 가지 경우 모두 로봇은 고정된 위치에서 ψr = 0° 에서 ψr = 180° 까지 회전한다. 관찰된 직선은 로봇의 정면에 5.66m의 거리에 위치하고 있다. r1과 r2 는 8m로 같고 b1과 b2 는 각각 -45°와 45°이다. 자코비안 행렬 HL과 HB의 값은 그림 6과 그림 7에서 나타내고 있다. M-model의 자코비안 값이 로봇의 위치가 멀어짐에 따라 증가하는 반면 제안된 모델의 자코비안 값은 로봇의 위치에 상관없이 변하지 않는다. 이러한 시뮬레이션들의 결과에서 제안된 모델의 HB행렬은 로봇의 위치와 관련이 없음을 확인 할 수 있다. 따라서 선형화 에러가 EKF-SLAM의 일관성에 미치는 영향이 줄어들게 된다. 단, 행렬을 수식적으로는 계산 할 수는 없다. 따라서 근사값으로 [1 0; 0 0]을 사용 하였다. 이는 작은 근사 에러를 만들지만 그 값이 전체과정동안 변하지 않으므로 사용이 가능하다. 

그림 5. 시뮬레이션 설정 : (a) 로봇 위치 - Xr(10,10) (b) 로봇 위치 - Xr(2000,2000). 로봇은 고정된 지점에서 ψr = 0º 에서 ψr = 180º까지 회전한다.

그림 6. M-model의 자코비안 행렬 HL (관찰모델에 대한 자코비안) (a) xr = 10, yr = 10, Ψ = 0˚에서 Ψ = 180˚까지. (b) xr = 2000, yr = 200, Ψ = 0˚에서 Ψ = 180˚까지. 각 그래 프의 위치는 자코비안 행렬 성분과 같은 순서로 배치되어 있다. 우측상단의 값이 로봇의 위치와 관련되어있기 때문에 값이 로봇의 위치에 따라 증가한다.

그림 7. M-model의 자코비안 행렬 HB (관찰모델에 대한 자코비안) (a) xr = 10, yr = 10, Ψ = 0˚에서 Ψ = 180˚까지. (b) xr = 2000, yr = 200, Ψ = 0˚에서 Ψ = 180˚까지. 각 그래 프의 위치는 자코비안 행렬 성분과 같은 순서로 배치되어 있다. 우측상단의 값이 로봇의 위치와 무관하므로 일정한 값을 나타낸다.

5. 결 론

 이 논문에서 EKF-SLAM 알고리즘의 새로운 관찰 모델을 제안하였다. 시뮬레이션의 결과, 기존 모델의 랜드마크 위치와 관련된 자코비안 행렬이 로봇의 위치에 영향을 받는 반면 제안된 모델의 자코비안 행렬은 로봇의 위치에 영향을 받지 않음을 확인 할 수 있었다. 이러한 부분에서, 제안된 모델은 기존의 M-model 보다 현격히 작은 선형화 에러를 가지게 된다. 이는 제안된 모델이 랜드마크의 위치를 더 정확히 추정하고 EKF-SLAM의 일관성 문제를 줄여줌을 의미한다. 그러나 제안된 모델의 HB 행렬을 수식적으로 계산할 수 없고 수치적으로만 계산할 수 있다는 문제점이 존재 한다. 따라서 근사 값을 사용 할 수밖에 없게 된다. 향후 연구에서는 사용한 근사값이 적절한 값임을 여러 시뮬레이션과 실제실험을 통해 보일 것이다.

 또한 실제 실험을 통해 일관성에 관한 증명을 하는 것이 마땅하나 비일관성의 문제는 관찰모델 뿐만 아니라, 모션모델, 상태 오그멘테이션(state augmentation) 등의 과정에서도 발생하며 각각의 요소들이 어느 정도의 영향을 주는지에 대한 분석은 어렵다고 알려져 있다[7] . 따라서 수정된 모델을 사용하여 실제 실험을 하더라도 다른 요소들에 의해 비일관성의 문제가 발생할 것으로 생각된다.

 더불어 역 거리(inverse distance)를 사용하는 방안도 비일관성의 문제를 줄일 수 있을 것으로 판단되나 이를 적용함에 따라 거리, 방향, 불확실도(covariance) 및 자코비안에 대하여 전반적으로 새로운 틀(framework)을 적용해야 하므로 이에 대한 분석은 차후 논문으로 대신할 것이다.        

Reference

1. T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, "Consistency of the EKF-SLAM algorithm," in Proc. of IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3562–3568, 2006.
2. N. L. Doh, K. Lee, and C. Nam, "Novel Line Representation with Consistent Second Order Statistics for SLAM Applications," The 5th International Conference on the Advanced Mechatronics, pp. 253–258, 2010.
3. J. Folkesson, P. Jensfelt, and H. I.Christensen, "The MSpace feature representation for SLAM," IEEE Trans. on Robotics, vol. 23, pp. 1024.1035, 2007
4. A. Garulli, A. Giannitrapani, A. Rossi and A.Vicino, "Simultaneous localization and map building using linear features," Proc. of the 2nd European Conference on Mobile Robots, pp.44-49, 2005.
5. C. Fulgenzia, G. Ippolitib, and S. Longhi, "Experimental validation of FastSLAM algorithm integrated with a linear features based map," Mechatronics, vol. 19, pp. 609-616, 2009. pp.44-49, 2005.
6. Y. Choi, T. Lee and S. Oh, "A line feature based SLAM with low grade range sensors using geometric constraints and active exploration for mobile robot," Autonomous Robot, vol. 24, pp.13-27, 2008
7. G P. Huang, A I . Mourikis and S I Roumeliotis, "Observability-based Rules for Designing Consistent EKF SLAM Estimators," International Journal of Robotics Research, vol. 29, pp. 502-528, 2009
오늘하루 팝업창 안보기 닫기